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

#include "Aanderaa_O2.h"
#include "Aanderaa_O2IF.h"

#include "data/SimSlate.h"
#include "data/Slate.h"
#include "data/StrValue.h"
#include "data/UniversalDataWriter.h"
#include "units/Units.h"

const unsigned Aanderaa_O2::O2_MOLECULAR_MASS( 32 ); // g/mol

Aanderaa_O2::Aanderaa_O2( const Module* module )
    : SyncSensorComponent( Aanderaa_O2IF::NAME, module ),
      loadControl_( Aanderaa_O2IF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      uart_( Aanderaa_O2IF::UART, Aanderaa_O2IF::BAUD, 0.010, logger_, 4095 ),
      startTime_(),
      timeout_( 10.0 ),
      period_( 1.0 ),
      model_( Str::EMPTY_STR ),
      debug_( false ),
      startCommanded_( false ),
      temperature_( -1 ),
      o2Concentration_( -1 ),
      airSaturation_( -1 )
{
    // Read from Config file
    StrValue tempStr( Str::EMPTY_STR );
    Slate::ReadOnce( Aanderaa_O2IF::MODEL, tempStr, logger_ );
    model_ = tempStr.asString();

    // initialize universal output with specified accuracy
    o2ConcentrationWriter_ = newUniversalWriter( UniversalURI::MASS_CONCENTRATION_OF_OXYGEN_IN_SEA_WATER, Units::MICROGRAM_PER_LITER, 8.0 / 0.03125 );

    // initialize component outputs
    temperatureWriter_ = newDataWriter( Aanderaa_O2IF::TEMPERATURE );
    airSaturationWriter_ = newDataWriter( Aanderaa_O2IF::AIRSATURATION );

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

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

Aanderaa_O2::~Aanderaa_O2()
{
}

void Aanderaa_O2::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 Aanderaa_O2::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;
    }

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

    // Open the uart
    uart_.open();

    startCommanded_ = false;
    startTime_ = Timestamp::Now();

    return STARTING;
}

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

    if( !startCommanded_ )
    {
        uart_ << "do start\r\n";
        startCommanded_ = true;
        uart_.flush();
    }
    else if( 0 < uart_.canReadUntil( "\n" ) )
    {
        if( uart_.readUntil( deviceResponse_, MAX_DEVICE_RESPONSE, "\n", 1 ).hasError() )
        {
            logger_.syslog( "Uart error: ", uart_.errorString(), Syslog::ERROR );
            this->setFailure( FailureMode::COMMUNICATIONS );
            return STOP;
        }
        else if( ( 48 < uart_.bytesRead() ) && parse() )
        {
            // OK to run in the next cycle
            startTime_ = Timestamp::Now();
            return RUNNABLE;
        }
    }

    if( startTime_.elapsed() > timeout_ )
    {
        logger_.syslog( "Failed to initialize within timeout.", Syslog::FAULT );
        this->setFailure( FailureMode::COMMUNICATIONS );
        return STOP;
    }

    return STARTING;
}

/// Pause for a short period (indicated by pauseTime)
Component::RunState Aanderaa_O2::pause()
{
    if( debug_ ) logger_.syslog( "Pause", Syslog::INFO );

    temperature_ = nanf( "" );
    airSaturation_ = nanf( "" );
    o2Concentration_ = nanf( "" );
    writeData();
    if( !simulateHardware() )
    {
        if( !loadControl_.powerDown() )
        {
            logger_.syslog( "Failed to power down", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }
    }
    uart_.close();

    return PAUSED;
}

/// Should eventually follow a PAUSE request: should set continueTime
Component::RunState Aanderaa_O2::paused()
{
    if( debug_ ) logger_.syslog( "Paused", Syslog::INFO );
    /// Pause if we don't want data
    if( isDataRequested() )
    {
        return START;
    }
    return STOP;
}

Component::RunState Aanderaa_O2::resume()
{
    // This state not currently used
    if( debug_ ) logger_.syslog( "Resume", Syslog::INFO );
    return resuming();
}

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

Component::RunState Aanderaa_O2::runnable()
{
    // if( debug_ ) logger_.syslog( "Runnable", Syslog::INFO );
    bool dataToWrite( false );
    bool processPacket( false );
    unsigned short readCount( 0 );

    /// Stop if we don't want data
    if( !isDataRequested() )
    {
        return STOP;
    }
    // If data is available, parse it
    if( !simulateHardware() )
    {
        // Log voltage and current and check for any faults
        loadControl_.requestVoltageAndCurrent();
        if( loadControl_.hasError() )
        {
            // Put anything that isn't an actual fault first
            if( loadControl_.errorString().find( "Software Overcurrent" ) )
            {
                // TODO: Run a short mission to determine the offending subsystems.
                if( debug_ )logger_.syslog( "LCB error:" + loadControl_.errorString(), Syslog::ERROR ); // TODO: LCB firmware should be updated with software overcurrent values
            }
            // And things that set failures second
            else
            {
                logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
                this->setFailure( FailureMode::HARDWARE );
                return STOP;
            }
        }

        // Read most recent device data
        while( ( 0 < uart_.canReadUntil( "\n" ) ) && ( readCount < MAX_DEVICE_MSG_QUEUE_SIZE ) )
        {
            if( uart_.readUntil( deviceResponse_, MAX_DEVICE_RESPONSE, "\n", 1 ).hasError() )
            {
                logger_.syslog( "Uart error: ", uart_.errorString(), Syslog::ERROR );
                this->setFailure( FailureMode::COMMUNICATIONS );
                setRunState( STOP );

                processPacket = false;
                break;
            }
            else if( ( 88 < uart_.bytesRead() ) && ( uart_.bytesRead() < 95 ) )
            {
                // Valid device response
                if( debug_ ) logger_.syslog( "Got valid response. Skip count: " + Str( readCount ) + ".", Syslog::INFO );
                processPacket = true;
            }

            ++readCount;
        }

        // Process device data
        if( processPacket && ( readCount < MAX_DEVICE_MSG_QUEUE_SIZE ) )
        {
            dataToWrite = parse();
            if( !dataToWrite )
            {
                if( debug_ ) logger_.syslog( "Error parsing device response", Syslog::ERROR );
            }
            // If we have data to write, write it now and reset the timer
            if( dataToWrite )
            {
                writeData();
                startTime_ = Timestamp::Now();
                this->resetFailCount();
            }
        }
        else if( MAX_DEVICE_MSG_QUEUE_SIZE <= readCount )
        {
            // Flush the buffer in case we're out of step
            logger_.syslog( "Faild to get valid response or reached max message skip count.", Syslog::ERROR );
            uart_.flush();
        }

    }
    else
    {
        writeData();
        startTime_ = Timestamp::Now();
    }

    // TODO SimSlate stuff here, once we have SimSlate entries for this stuff

    // Re-init if we've expired
    if( startTime_.elapsed() > timeout_ )
    {
        logger_.syslog( Str( "No communication! Re-starting" ), Syslog::ERROR );
        this->setFailure( FailureMode::COMMUNICATIONS );
        return STOP;
    }

    return RUNNABLE;
}

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

    temperature_ = nanf( "" );
    airSaturation_ = nanf( "" );
    o2Concentration_ = nanf( "" );
    writeData();

    if( !simulateHardware() )
    {
        uninitialize(); // First power down then query for faults next cycle
    }
    return STOPPING;
}


Component::RunState Aanderaa_O2::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() )
        {
            logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }

    }
    return STOPPED;
}


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


bool Aanderaa_O2::parse()
{
    char model[50];

    // coverity[secure_coding] // indicate that the code below is indeed safe
    if( 4 == sscanf( deviceResponse_, "%s %*d %f %f %f %*f %*f %*f %*f %*f %*f %*f",
                     model, &o2Concentration_, &airSaturation_, &temperature_ ) )
    {
        return true;
    }
    if( debug_ ) logger_.syslog( "Error parsing data: " + Str( deviceResponse_ ), Syslog::ERROR );
    return false;
}

void Aanderaa_O2::writeData()
{
    temperatureWriter_->write( Units::CELSIUS, temperature_ );
    airSaturationWriter_->write( Units::PERCENT, airSaturation_ );
    if( SimSlate::Read( SimSlate::MASS_CONCENTRATION_OF_OXYGEN_UG_PER_L, o2Concentration_ ) )
    {
        o2Concentration_ /= O2_MOLECULAR_MASS;
    }
    o2ConcentrationWriter_->write( Units::MICROGRAM_PER_LITER, o2Concentration_ * O2_MOLECULAR_MASS );
}

bool Aanderaa_O2::isDataRequested()
{
    return o2ConcentrationWriter_->isDataRequested()
           || airSaturationWriter_->isDataRequested()
           || temperatureWriter_->isDataRequested();
}

void Aanderaa_O2::uninitialize()
{
    if( !simulateHardware() )
    {
        uart_.close();

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

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