/** \file
 *
 *  Contains the Sonardyne_Nano class implementation.
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#include "Sonardyne_Nano.h"
#include "Sonardyne_NanoIF.h"

#include "data/ConfigReader.h"
#include "data/Location.h"
#include "data/SimSlate.h"
#include "data/Slate.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"
#include "units/Units.h"


/*
 * NOTES:
 *
*/

Sonardyne_Nano::Sonardyne_Nano( const Module* module )
    : SyncSensorComponent( Sonardyne_NanoIF::NAME, module ),
      loadControl_( Sonardyne_NanoIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      uart_( Sonardyne_NanoIF::UART, Sonardyne_NanoIF::BAUD, 0.01, logger_ ),
      poTimeout_( 1.5 ),
      requestTimeout_( 60 * 10 ), // Request data from unit on this interval (sec)
      dataTimeout_( 60 * 60 * 60 ), // Valid data from the unit should be expected within this time (sec).
      startTime_( Timestamp::NOT_SET_TIME ),
      statusTimestamp_( Timestamp::NOT_SET_TIME ),
      chargeMax_( 80 ),
      chargeMin_( 50 ),
      lastCharge_( 0 ),
      uuid_( 0 ),
      selfTestPassed_( false ),
      charging_( false ),
      debug_( false )
{
    // Slate output
    chargeWriter_ = newDataWriter( Sonardyne_NanoIF::CHARGE_PERCENT );

    // config readers
    chargeMaxCfgReader_ = newConfigReader( Sonardyne_NanoIF::CHARGE_MAX_CFG );
    chargeMinCfgReader_ = newConfigReader( Sonardyne_NanoIF::CHARGE_MIN_CFG );

    setAllowableFailures( 5 );
    setRetryTimeout( 30 );

    setRunState( START );
}

Sonardyne_Nano::~Sonardyne_Nano()
{}

bool Sonardyne_Nano::readConfig()
{
    bool ok( true );
    ok &= chargeMaxCfgReader_->read( Units::PERCENT, chargeMax_ );
    ok &= chargeMinCfgReader_->read( Units::PERCENT, chargeMin_ );
    return ok;
}

/// Similar to initialize, in old init/run/uninit sequence
Component::RunState Sonardyne_Nano::start()
{
    logger_.syslog( "Initializing.", Syslog::INFO );

    if( !simulateHardware() )
    {
        deviceResponse_[0] = '\0';

        if( !loadControl_.powerUp() )
        {
            logger_.syslog( "Load controller failed to power up.", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return START;
        }

        // Open the uart
        uart_.open();
        if( uart_.hasError() )
        {
            logger_.syslog( "Error opening port: ", uart_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::COMMUNICATIONS );
            return STOP;
        }
        uart_.flush();
    }

    startTime_ = Timestamp::Now();
    return STARTING;
}

/// Might follow a STOP...START sequence
Component::RunState Sonardyne_Nano::starting()
{
    if( debug_ ) logger_.syslog( "Starting", Syslog::INFO );

    // Allow time for the system to come up
    if( startTime_.elapsed() > poTimeout_ )
    {
        readConfig();

        if( !simulateHardware() )
        {
            // Command mode
            uart_ << "+++\r\n";
        }

        startTime_ = Timestamp::Now();
        return RUNNABLE;
    }

    return STARTING;
}

/// Pause for a short period
Component::RunState Sonardyne_Nano::pause()
{
    if( debug_ ) logger_.syslog( "Pause", Syslog::INFO );
    return STOP;
}

/// Should eventually follow a PAUSE request
Component::RunState Sonardyne_Nano::paused()
{
    if( debug_ ) logger_.syslog( "Paused", Syslog::INFO );
    return STOP;
}

Component::RunState Sonardyne_Nano::resume()
{
    if( debug_ ) logger_.syslog( "Resume", Syslog::INFO );
    return START;
}

Component::RunState Sonardyne_Nano::resuming()
{
    if( debug_ ) logger_.syslog( "Resuming", Syslog::INFO );
    return START;
}

Component::RunState Sonardyne_Nano::runnable()
{
    readConfig();

    if( simulateHardware() )
    {
        return RUNNABLE;
    }

    logVoltageAndCurrent();

    bool readData = readAndParseData();

    if( getNextRequest() )
    {
        if( debug_ )
        {
            logger_.syslog( "Command indicated, sending ", nextRequest_, Syslog::INFO );
        }
        uart_.write( nextRequest_, strlen( nextRequest_ ) );
        uart_ <<  "\r\n";
    }

    // Write last charge data to slate
    if( readData )
    {
        chargeWriter_->write( Units::PERCENT, float( lastCharge_ ), statusTimestamp_ );
        startTime_ = Timestamp::Now();
        this->resetFailCount();
    }

    if( startTime_.elapsed() > dataTimeout_ )
    {
        logger_.syslog( "Failed to receive valid data within the specified timeout.", Syslog::FAULT );
        this->setFailure( FailureMode::COMMUNICATIONS );
        return STOP;
    }

    return RUNNABLE;
}

Component::RunState Sonardyne_Nano::stop()
{
    if( debug_ ) logger_.syslog( "Stop", Syslog::INFO );
    // Power down and disable the uart.
    uninitialize();

    return STOPPING;
}

Component::RunState Sonardyne_Nano::stopping()
{
    if( debug_ ) logger_.syslog( "Stopping", Syslog::INFO );
    if( !simulateHardware() )
    {
        // See if anything went wrong that may have caused this request for uninitialize
        loadControl_.readFaults();
        if( loadControl_.hasError() )
        {
            logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }

    }

    return STOPPED;
}

Component::RunState Sonardyne_Nano::stopped()
{
    if( debug_ ) logger_.syslog( "Stopped", Syslog::INFO );

    // Power cycle complete. Back to work.
    return START;
}

void Sonardyne_Nano::uninitialize()
{
    if( debug_ ) logger_.syslog( "Uninitializing.", Syslog::INFO );

    if( !simulateHardware() )
    {
        logger_.syslog( "Powering down", Syslog::INFO );
        if( !loadControl_.powerDown() )
        {
            logger_.syslog( "Failed to power down", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }
        uart_.close();
    }
}


bool Sonardyne_Nano::readAndParseData()
{
    unsigned short readCount( 0 );
    bool gotResp( false );
    while( uart_.dataAvailable() && ( readCount < MAX_DEVICE_MSG_QUEUE_SIZE ) )
    {
        if( uart_.readUntil( deviceResponse_, MAX_DEVICE_RESP, '\n' ).hasError() )
        {
            if( uart_.getError() != UartStream::TIMEOUT )
            {
                logger_.syslog( "UART read error:", uart_.errorString(), Syslog::ERROR );
                if( uart_.bytesRead() > 0 )
                {
                    logger_.syslog( "Receieved", deviceResponse_, Syslog::INFO );
                }
            }
            break;
        }
        else
        {
            gotResp = true;
        }
        readCount++;
    }
    if( gotResp && ( readCount < MAX_DEVICE_MSG_QUEUE_SIZE ) )
    {
        if( debug_ )
        {
            logger_.syslog( "Received response:", deviceResponse_, Syslog::INFO );
        }

        int charging = 0;
        int acoustic_id = 0;
        // Volatile status
        if( strstr( deviceResponse_, ">VS:" ) )
        {
            parseVolatile();
        }
        // Fixed status
        else if( sscanf( deviceResponse_, ">FS:%d,U%6x", &acoustic_id, &uuid_ ) )
        {
            logger_.syslog( "Found beacon with acoustic ID " + Str( acoustic_id ), Syslog::IMPORTANT );
            if( debug_ )
            {
                logger_.syslog( "Read UUID: %x", uuid_, Syslog::INFO );
            }
        }
        // Secure configuration (reponse to setting/clearing charge)
        else if( sscanf( deviceResponse_, ">SC:%*d,U%*x,HPR%*d,BT%*d;DIS%*d;CHG%d", &charging ) )
        {
            charging_ = charging;
            if( debug_ )
            {
                logger_.syslog( "Received command set request", Syslog::INFO );
            }
        }
        else if( strstr( deviceResponse_, ">CKHW" ) )
        {
            if( debug_ )
            {
                logger_.syslog( "Received self-test result", Syslog::INFO );
            }

            if( strstr( deviceResponse_, "PASS" ) )
            {
                selfTestPassed_ = true;
            }
            else
            {
                logger_.syslog( "Failed hardware self-test:", deviceResponse_, Syslog::FAULT );
                this->setFailure( FailureMode::HARDWARE );
            }
            return false;
        }
        else
        {
            logger_.syslog( "Failed to get valid response or reached max queue size, flushing UART", Syslog::ERROR );
            uart_.flush();
            return false;
        }
        return true;
    }
    else
    {
        return false;
    }
}


void Sonardyne_Nano::parseVolatile()
{
    // Check for external power flag, should always be present if LCB is powered/component is running
    if( !strstr( deviceResponse_, "EXT" ) )
    {
        logger_.syslog( "External power not detected", Syslog::ERROR );
    }

    // Update charge status
    charging_ = false;
    if( strstr( deviceResponse_, "CHG" ) )
    {
        charging_ = true;
    }

    // The volatile status response has a variable number of fields, so it's a pain to parse
    // Fortunately, other than external power and charge status above, which are present/absent,
    // we really only care about charge percentage, which is always present, and occurs after the
    // only forward slash in the string. So, skip trying to parse the rest, just look for the
    // forward slash and pull out charge percentage.
    bool success( false );
    char* chgPtr = strstr( deviceResponse_, "/" );
    if( chgPtr != NULL )
    {
        success = sscanf( chgPtr + 1, "%d", &lastCharge_ );
    }
    if( !success )
    {
        logger_.syslog( "Failed to parse response: ", deviceResponse_, Syslog::ERROR );
    }
}

bool Sonardyne_Nano::getNextRequest()
{
    // Don't continue until self-test passes
    if( !selfTestPassed_ )
    {
        strcpy( nextRequest_, "CKHW" );
        return true;
    }
    // Get UUID if we haven't yet
    else if( uuid_ == 0 )
    {
        strcpy( nextRequest_, "FS" );
        return true;
    }
    // Get charge status, if it needs an update
    else if( statusTimestamp_.elapsed() > requestTimeout_ )
    {
        statusTimestamp_ = Timestamp::Now();
        strcpy( nextRequest_, "VS" );
        return true;
    }
    else if( lastCharge_ < chargeMin_ )
    {
        if( charging_ )
        {
            return false;
        }
        else // Below minimum, enable charging
        {
            logger_.syslog( "Battery below minimum, enabling charging", Syslog::INFO );
            sprintf( nextRequest_, "SC:U%x,CHG1", uuid_ );
            return true;
        }
    }
    else if( lastCharge_ >= chargeMax_ )
    {
        if( charging_ ) // Reached maximum, disable charging
        {
            logger_.syslog( "Battery at maximum, disabling charging", Syslog::INFO );
            sprintf( nextRequest_, "SC:U%x,CHG0", uuid_ );
            return true;
        }
        else
        {
            return false;
        }
    }
    return false;
}

// Log voltage and current and check for any faults
void Sonardyne_Nano::logVoltageAndCurrent()
{
    loadControl_.requestVoltageAndCurrent();
    if( loadControl_.hasError() )
    {
        logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
        stop();
    }
}

/// Should return [myNamespace]::SIMULATE_HARDWARE, or [myNamespace]::POWER, etc
ConfigURI Sonardyne_Nano::getConfigURI( ConfigOption configOption ) const
{
    switch( configOption )
    {
    case CONFIG_POWER:
        return Sonardyne_NanoIF::POWER;
    case CONFIG_SIMULATE_HARDWARE:
        return Sonardyne_NanoIF::SIMULATE_HARDWARE;
    default:
        return ConfigURI::NO_CONFIG_URI;
    }
}