/** \file
 *
 *  Contains the AMEcho class implementation.
 *
 *  Copyright (c) 2020, MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 *
 */


#include "AMEcho.h"
#include "AMEchoIF.h"
#include "Power24vConverterIF.h"

#include "data/ConfigReader.h"
#include "data/SimSlate.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"

#define AMECHO_M_ACCURACY (0.01)

AMEcho::AMEcho( const Module* module )
    : SyncSensorComponent( AMEchoIF::NAME, module ),
      commsTimeout_( 10.0 ),
      powerOnTimeout_( 2.0 ),
      altitude_( -1.0 ),
      enabled_( true ),
      debug_( false ),
      depthThreshold_( 0.0 ),
      uart_( AMEchoIF::UART, AMEchoIF::BAUD, 0.010, logger_, ECHO_UART_BUFSIZE ),
      loadControl_( AMEchoIF::LOAD_CONTROL, !simulateHardware(), logger_, this )
{
    altitudeWriter_ = newUniversalWriter( UniversalURI::HEIGHT_ABOVE_SEA_FLOOR, Units::METER, AMECHO_M_ACCURACY );

    depthReader_ = newUniversalReader( UniversalURI::DEPTH );
    power24vConverterDataReader_ = newDataReader( Power24vConverterIF::POWER_24V_CONVERTER );

    enabledCfgReader_ = newConfigReader( AMEchoIF::ENABLED );
    depthThresholdCfgReader_ = newConfigReader( AMEchoIF::DEPTH_THRESHOLD );

    // This configures the advanced run modes.
    setRunState( START );
}

AMEcho::~AMEcho()
{
}

void AMEcho::run()
{
}

bool AMEcho::belowDepthThreshold( void )
{
    float depth;
    return depthReader_->isActive() && depthReader_->read( Units::METER, depth ) && ( depth > depthThreshold_ );
}

Component::RunState AMEcho::start()
{
    if( debug_ ) logger_.syslog( "Start", Syslog::INFO );

    logger_.syslog( "Powering up.", Syslog::INFO );

    // Request 24V power
    power24vConverterDataReader_->requestData( true );

    // Start the clock for PO timeout
    startTime_ = Timestamp::Now();

    if( simulateHardware() )
    {
        return STARTING;
    }

    deviceResponse_[0] = '\0';
    this->setAllowableFailures( 3 );
    this->setRetryTimeout( 180 );

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

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

    if( !isDataRequested() )
    {
        return STOP;
    }

    return STARTING;
}


Component::RunState AMEcho::starting()
{
    if( debug_ ) logger_.syslog( "Starting", Syslog::INFO );

    if( startTime_.elapsed() < powerOnTimeout_ )
    {
        return STARTING; // give it some more time to power up
    }
    else if( !simulateHardware() )
    {
        // Write settings
        // disable all sentences
        uart_ << "$PAMTC,EN,ALL,0\r\n";
        // enable DepthBelowTranducer sentence at 4/10s intervals
        uart_ << "$PAMTC,EN,DBT,1,4\r\n";
        // We're ready to go. Reset the clock and head for runnable.
        startTime_ = Timestamp::Now();
        uart_.flush();
    }

    return RUNNABLE;
}


Component::RunState AMEcho::pause()
{
    if( debug_ ) logger_.syslog( "Pause", Syslog::INFO );
    return PAUSED;
}

// Not using pause/paused
Component::RunState AMEcho::paused()
{
    if( debug_ ) logger_.syslog( "Paused", Syslog::INFO );
    return STOP;
}

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

// Stop
Component::RunState AMEcho::resuming()
{
    if( debug_ ) logger_.syslog( "Resuming", Syslog::INFO );
    return STOP;
}

Component::RunState AMEcho::runnable()
{
    bool hasAlt = false;
    if( debug_ ) logger_.syslog( "Runnable", Syslog::INFO );

    readConfig();

    if( !simulateHardware() ) logVoltageAndCurrent();

    // Pause if we don't want data
    if( !isDataRequested() )
    {
        return STOP;
    }

    // Run checks on data timeouts, etc.
    if( !checkTimeouts() )
    {
        return STOP;
    }

    hasAlt = getSimAltitude(); // Always attempt to read simulated values, for vehicle in the loop sims

    if( !simulateHardware() )
    {
        hasAlt = readAltitude();
    }

    if( hasAlt )
    {
        if( !belowDepthThreshold() )
        {
            // Altitude reading is valid only when the vehicle is under the specified depth threshold.
            altitudeWriter_->setInvalid( true );
        }

        altitudeWriter_->write( Units::METER, altitude_, startTime_ );
        this->resetFailCount();
    }

    return RUNNABLE;
}

Component::RunState AMEcho::stop()
{
    if( debug_ ) logger_.syslog( "Stop", Syslog::INFO );

    altitudeWriter_->setInvalid( true );
    uninitialize(); // First power down then query for faults next cycle
    return STOPPING;
}

Component::RunState AMEcho::stopping()
{
    if( debug_ ) logger_.syslog( "Stopping", Syslog::INFO );

    if( !simulateHardware() )
    {
        loadControl_.readFaults(); // See if anything went wrong that may have caused this request for uninitialize
        if( loadControl_.hasError() )
        {
            // Put anything that isn't an actual fault first
            if( loadControl_.errorString().find( "Software" ) )
            {
                if( debug_ )logger_.syslog( "LCB error:" + loadControl_.errorString(), Syslog::ERROR );
            }
            // And things that set failures second
            else
            {
                logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
                this->setFailure( FailureMode::HARDWARE );
            }
        }

    }
    return STOPPED;
}

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

    readConfig();

    if( isDataRequested() )
    {
        return START;
    }

    if( !simulateHardware() )
    {
        if( ( loadControl_.getPowerState() != LoadControl::OFF ) && ( loadControl_.getPowerState() != LoadControl::POWER_DOWN ) )
        {
            return stop();
        }

        // Close if the uart if it is open
        if( uart_.isReadable() )
        {
            uart_.close();
        }
    }
    return STOPPED;
}

void AMEcho::uninitialize()
{
    if( !simulateHardware() )
    {
        uart_.close();
        uart_.disableUART();
    }

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

    // 24V power is no longer needed
    power24vConverterDataReader_->requestData( false );
}

// Log voltage and current and check for any faults
void AMEcho::logVoltageAndCurrent() // TODO: Elevate to superclass
{
    loadControl_.requestVoltageAndCurrent();
    if( loadControl_.hasError() )
    {
        // Put anything that isn't an actual fault first
        if( loadControl_.errorString().find( "Software" ) )
        {
            if( debug_ )logger_.syslog( "LCB error:" + loadControl_.errorString(), Syslog::ERROR );
        }
        // And things that set failures second
        else
        {
            logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            stop();
        }
    }
}

ConfigURI AMEcho::getConfigURI( ConfigOption configOption ) const
{
    switch( configOption )
    {
    case CONFIG_POWER:
        return AMEchoIF::POWER;
    case CONFIG_SIMULATE_HARDWARE:
        return AMEchoIF::SIMULATE_HARDWARE;
    default:
        return ConfigURI::NO_CONFIG_URI;
    }
}

void AMEcho::readConfig()
{
    bool enabled;
    float depthThreshold;

    if( enabledCfgReader_->read( enabled ) )
    {
        if( enabled != enabled_ )
            logger_.syslog( Str( "Setting 'enabled' to " + Str( enabled ) ), Syslog::IMPORTANT );
        enabled_ = enabled;
    }
    if( depthThresholdCfgReader_->read( Units::METER, depthThreshold ) )
    {
        if( depthThreshold != depthThreshold_ )
            logger_.syslog( Str( "Setting 'depthThreshold' to " + Str( depthThreshold ) ), Syslog::IMPORTANT );
        depthThreshold_ = depthThreshold;
    }
}

bool AMEcho::isDataRequested()
{
    return altitudeWriter_->isDataRequested() && enabled_;
}

bool AMEcho::checkTimeouts()
{
    // Is non-simulated data available in time since the last ping?
    if( !simulateHardware() &&  startTime_.elapsed() > commsTimeout_ )
    {
        logger_.syslog( Str( "No EchoSounder communication! Re-initializing" ), Syslog::ERROR );
        this->setFailure( FailureMode::COMMUNICATIONS );
        return false;
    }

    return true;
}

bool AMEcho::getSimAltitude()
{
    bool goodSim = false;

    // WRT Ground including altitude
    if( SimSlate::Read( SimSlate::ALTITUDE_METER, altitude_ ) )
    {
        // We have valid simulated data
        goodSim = true;
        startTime_ = Timestamp::Now();  // roughly set the time for the incoming data here
    }

    return goodSim;
}

bool AMEcho::verifyChecksum( char *msg, char sum )
{
    size_t len = strlen( msg );
    char csum = 0;
    bool begin = false;

    for( size_t i = 0; i < len; i++ )
    {
        if( msg[i] == '*' ) break;
        if( begin ) csum ^= msg[i];
        if( msg[i] == '$' ) begin = true;
    }
    return( sum == csum );
}

bool AMEcho::readAltitude()
{
    int i, scanRes = 0;
    unsigned int csum;
    bool gotMessage = false;

    /*
     * There could be more than just 1 message in buffer, pull them all out
     * but only evaluate the last one since that is the only one for which the
     * time stamp will be close enough. Limit to 10 messages so we don't hold
     * up things for too long.
     */
    for( i = 0; i < 10; i++ )
    {
        // See if an entire message has made it in
        if( !uart_.canReadUntil( "\n" ) ) break;
        /*
         * uart_.readline() only reads to the \r so the following \n ends up in the
         * next line, use readUntil() instead)
         */
        uart_.readUntil( deviceResponse_, sizeof( deviceResponse_ ), '\n' );
        if( debug_ ) logger_.syslog( Str( deviceResponse_, uart_.bytesRead() ), Syslog::INFO );

        if( uart_.hasError() )
        {
            deviceResponse_[0] = '\0';
            logger_.syslog( "uart error: ", uart_.errorString(), Syslog::ERROR );
            this->setFailure( FailureMode::COMMUNICATIONS );
            setRunState( STOP );
            return false;
        }
        gotMessage = true;
    }

    if( !gotMessage )
    {
        return false;
    }

    if( i == 10 )
    {
        logger_.syslog( "Unqueued 10 messages, instrument reporting frequency is too high", Syslog::ERROR );
        uart_.flush();
    }

    /*
     * Output examples:
     * $SDDBT,22.80,f,6.95,M,3.80,F*0F
     * $SDDBT,36.58,f,11.15,M,6.10,F*3D
     */
    altitude_ = -1;
    scanRes = sscanf( deviceResponse_, "$SDDBT,%*f,f,%f,M,%*f,F*%X", &altitude_, &csum );
    if( scanRes != 2 )
    {
        altitudeWriter_->setInvalid( true );
        // if we have no bottom lock but instrument is still sending valid
        // sentences do not trigger comms timeout
        if( uart_.bytesRead() >= 15 && strncmp( deviceResponse_, "$SDDBT,,,,,,*45", 15 ) == 0 )
        {
            startTime_ = Timestamp::Now();
            return true;
        }
        return false;
    }

    //logger_.syslog( "Alt: " + Str( altitude_ ) + ",csum: " + Str( csum ), Syslog::INFO );

    if( !verifyChecksum( deviceResponse_, ( char )( csum & 0xff ) ) )
    {
        logger_.syslog( "checksum error in received NMEA msg", Syslog::ERROR );
        altitudeWriter_->setInvalid( true );
        return false;
    }

    altitudeWriter_->setInvalid( false );
    startTime_ = Timestamp::Now();
    return true;
}