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

#include "CANONSampler.h"
#include "CANONSamplerIF.h"

#include "data/BlobWriter.h"
#include "data/ConfigReader.h"
#include "data/SimSlate.h"
#include "data/Slate.h"
#include "data/StrValue.h"
#include "data/UniversalDataWriter.h"
#include "data/UniversalDataReader.h"
#include "units/Units.h"
#include "missionScript/SettingReader.h"
#include <stdlib.h>


CANONSampler::CANONSampler( const Module* module )
    : SyncSensorComponent( CANONSamplerIF::NAME, module ),
      uart_( CANONSamplerIF::UART, CANONSamplerIF::BAUD, 0.110, logger_, 4095 ),
      loadControl_( CANONSamplerIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      debug_( false ),
      sampleTimeout_( 285 ),
      poTimeout_( 60 ),
      poRetry_( false ),
      samplingActive_( false ),
      rotateOnly_( false ),
      sampleNumber_( 0 ),
      sampleState_( DONE )
{

    samplingActiveWriter_ = newDataWriter( CANONSamplerIF::SAMPLING_ACTIVE_STATE );
    sampleNumberWriter_ = newDataWriter( CANONSamplerIF::SAMPLE_NUMBER );

    rotateOnlyReader_ = newDataReader( CANONSamplerIF::ROTATE_ONLY );

    sampleTimeoutCfgReader_ = newConfigReader( CANONSamplerIF::SAMPLE_TIMEOUT_CFG );

    readConfig();

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

    this->setFailureMissionCritical( false );
    this->setAllowableFailures( 3 );
    this->setRetryTimeout( 250 );
}

CANONSampler::~CANONSampler()
{
}

void CANONSampler::run()
{
    //logger_.syslog("Run", Syslog::INFO);
}

/// Do what needs to be done to run
/// Similar to initialize, in old init/run/uninit sequence
Component::RunState CANONSampler::start()
{
    if( debug_ ) logger_.syslog( "Start", Syslog::INFO );
    if( simulateHardware() )
    {
        return STARTING;
    }

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

    // Open the uart
    uart_.enableUART();
    uart_.open();
    if( uart_.hasError() )
    {
        logger_.syslog( "Error opening port: ", uart_.errorString() );
        return START;
    }

    logger_.syslog( "Powering up", Syslog::INFO );
    if( !simulateHardware() )
    {
        // The discrete may already be asserted from the stopped state, but will need to be activated here if we're coming out of a failed state
        if( !loadControl_.powerUp() )
        {
            logger_.syslog( "Failed to power up", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOPPED;
        }
        poRetry_ = false; //set retry for power on prompt to false
    }
    return STARTING;
}


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

    if( simulateHardware() )
    {
        return RUNNABLE;
    }

    // We're operating on real hardware from here down

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

    return RUNNABLE;
}


// Pause for a short period (indicated by pauseTime)
Component::RunState CANONSampler::pause()
{
    // This state is not currently used
    if( debug_ ) logger_.syslog( "Pause", Syslog::INFO );
    return STOP;
}


// Should eventually follow a PAUSE request: should set continueTime
Component::RunState CANONSampler::paused()
{
    if( debug_ ) logger_.syslog( "Paused", Syslog::INFO );
    // This state is not currently used
    return STOP;
}


Component::RunState CANONSampler::resume()
{
    if( debug_ ) logger_.syslog( "Resume", Syslog::INFO );

    return START;
}


Component::RunState CANONSampler::resuming()
{
    if( debug_ ) logger_.syslog( "Resuming", Syslog::INFO );
    // This state is not currently used
    return START;
}


Component::RunState CANONSampler::runnable()
{
    //if( debug_ ) logger_.syslog( "Runnable", Syslog::INFO );

    readConfig();

    if( simulateHardware() )
    {
        if( getSimulatedData() )
        {
            writeData();
            this->resetFailCount();
            return RUNNABLE;
        }
        else if( samplingActive_ )
        {
            return RUNNABLE;
        }
        else
        {
            return STOP;
        }
    }

    /********************************************************************************************************
    Our simple API is as follows:
    Power on displays two bytes after about 5 seconds
    S>

    You send 3 bytes to start sampling:
    S<cr/lf>

    Sampler sends async status data.
    Look for "err_code" and sample number which confirms a sample has finished (i.e. we won't return to that sample number again
    All lines except the final S> prompt are terminated with cr/lf

    MO=0
    ELMO will go to position 1
    Initial cnts = 0
    HOMING DONE with MO=0
    Pumping  DONE
    Next
    MO=1, PX=437
    MO=1, PX=1173
    MO=0, PX=1497
    DONE with MO=0, PX=1497
    Saving.... DONE
    Pumping  DONE
    HOMING DONE with MO=0
    Sample 1, err_code=0
    S>

    A single capital "C" will reset UI[1] to 0.

    A single capital "E" will allow direct comms with the Elmo motor controller
    *********************************************************************************************************/


    // See if we want data
    if( isDataRequested() && sampleState_ == DONE )
    {
        samplingActive_ = true;
        sampleTime_ = Timestamp::Now();
        sampleState_ = BEGIN;
        return RUNNABLE;
    }
    else if( sampleState_ == DONE ) // Nothing else needed. There's no data requested and any previous sample has been taken.
    {
        return STOP;
    }

    // Grab the voltage/current info from the LCB
    if( !logVoltageAndCurrent() )
    {
        return STOP;
    }

    switch( sampleState_ )
    {
    case BEGIN:
        if( debug_ ) logger_.syslog( "PO TIMEOUT", Syslog::INFO );
        if( sampleTime_.elapsed() > poTimeout_ )
        {
            // Check for the boot prompt
            if( uart_.canReadUntil( 'S' ) )
            {
                if( debug_ ) logger_.syslog( "Got Prompt", Syslog::INFO );

                // Check here to see if we just want to rotate (cmd R) or actually sample.
                if( rotateOnlyReader_->asInt( Units::BOOL ) )
                {
                    rotateOnly_ = true;
                    logger_.syslog( "Rotation Only!", Syslog::IMPORTANT );
                    uart_ << "R\r\n"; // Command the rotation sequence to begin
                    sampleTime_ = Timestamp::Now();
                    sampleState_ = SAMPLING;
                }
                else
                {

                    uart_ << "S\r\n"; // Command the sample sequence to begin
                    sampleTime_ = Timestamp::Now();
                    sampleState_ = SAMPLING;

                    // Technically the sampling has begun, so write the active writer state.
                    samplingActiveWriter_->write( Units::BOOL, samplingActive_, sampleTime_ );
                }
            }
            // We didn't receive a prompt
            else
            {
                //if ~prompt retried issue /r and reset sampleTime_now
                if( !poRetry_ )
                {
                    poRetry_ = true;
                    uart_ << "\r\n"; // see if we can get it to respond with prompt
                    sampleTime_ = Timestamp::Now(); //wait another PO TIMEOUT
                    sampleState_ = BEGIN;
                    logger_.syslog( "Failed to receive prompt, trying again", Syslog::FAULT );
                    return RUNNABLE;
                }
                else
                {
                    logger_.syslog( "Failed to receive prompt, done trying", Syslog::FAULT );
                    while( uart_.canReadUntil( '\n' ) )
                    {
                        uart_.readUntil( deviceResponse_, sizeof( deviceResponse_ ), '\n' );
                        // Log the response from the sampler
                        logger_.syslog( Str( deviceResponse_ ), Syslog::IMPORTANT );
                    }
                    this->setFailure( FailureMode::HARDWARE );
                    return STOP;
                }
            }
        }
        return RUNNABLE;
        break;
    case SAMPLING:
        //if( debug_ ) logger_.syslog( "SAMPLING", Syslog::INFO );

        // Check the timeout
        if( sampleTime_.elapsed() > sampleTimeout_ )
        {
            logger_.syslog( "Failed to complete sampling within timeout", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }

        // See if we're done yet
        if( sampleStatus() )
        {
            if( debug_ ) logger_.syslog( "Sample Completed Sucessfully", Syslog::INFO );
            sampleTime_ = Timestamp::Now();
            sampleState_ = DONE;
            samplingActive_ = false;
            writeData();
        }
        return RUNNABLE;
        break;
    case DONE:
        return RUNNABLE;
        break;
    default:
        logger_.syslog( "Invalid sampling state: ", sampleState_, Syslog::ERROR );
        break;
    }

    return RUNNABLE;
}


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

    logger_.syslog( "Powering down", Syslog::INFO );
    samplingActive_ = false;
    rotateOnly_ = false;
    sampleState_ = DONE;

    if( !simulateHardware() )
    {
        if( !loadControl_.powerDown() )  // First power down then query for faults next cycle
        {
            logger_.syslog( "Failed to power down", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOPPING;
        }
        uart_.close();
    }

    sampleTime_ = Timestamp::Now(); // start the clock for power off reset.
    return STOPPING;
}


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

    if( sampleTime_.elapsed() < poTimeout_ )
    {
        return STOPPING;
    }

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


Component::RunState CANONSampler::stopped()
{
    if( debug_ ) logger_.syslog( "Stopped", Syslog::INFO );
    if( isDataRequested() )
    {
        return START;
    }
    return STOPPED;
}


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


bool CANONSampler::isDataRequested()
{
    return samplingActiveWriter_->isDataRequested();
}


// This duplicated stop() for now so that quit will call it
void CANONSampler::uninitialize()
{
    if( !simulateHardware() )
    {
        if( !loadControl_.powerDown() )
        {
            logger_.syslog( "Failed to power down", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }
        uart_.close();
    }
}

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


bool CANONSampler::sampleStatus()
{
    while( uart_.canReadUntil( '\n' ) )
    {
        uart_.readUntil( deviceResponse_, sizeof( deviceResponse_ ), '\n' );
        // Log the response from the sampler
        logger_.syslog( Str( deviceResponse_ ), Syslog::INFO );

        // Check for end of sample
        if( strstr( deviceResponse_, "err_code" ) )
        {
            // Check here to see if it was just a rotation command (cmd R) or actually a sampling command.
            if( rotateOnly_ )
            {
                // if we got err_code we're good. Return
                rotateOnly_ = false;
                return true;
            }
            if( debug_ ) logger_.syslog( "FOUND SAMPLE", Syslog::INFO );
            if( sscanf( deviceResponse_, "Sample %d, err_code=%*d", &sampleNumber_ ) == 1 ) // grab the sample number and go
            {
                logger_.syslog( Str( deviceResponse_ ), Syslog::IMPORTANT ); // Log the response from the sampler
                return true;
            }
        }
    }
    return false;
}


bool CANONSampler::readConfig()
{
    bool ok( true );
    ok &= sampleTimeoutCfgReader_->read( sampleTimeout_ );
    return ok;
}


bool CANONSampler::getSimulatedData()
{
    if( !samplingActive_ )
    {
        sampleTime_ = Timestamp::Now();
        samplingActive_ = true;
        return true;
    }
    else if( sampleNumber_ > 14 ) // we only have 15 samples
    {
        logger_.syslog( "lightweight sim tried to take sample ", sampleNumber_, Syslog::ERROR );
        return false;
    }
    else if( sampleTime_.elapsed() > sampleTimeout_ - Timespan( 2 ) ) // simulate completion a couple seconds before configured timeout
    {
        sampleTime_ = Timestamp::Now();
        samplingActive_ = false;
        sampleNumber_ += 1;
        return true;
    }
    return false;
}


void CANONSampler::writeData()
{
    samplingActiveWriter_->write( Units::BOOL, samplingActive_, sampleTime_ );
    sampleNumberWriter_->write( Units::COUNT, sampleNumber_, sampleTime_ );
}