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

#include "WetLabsSeaOWL_UV_A.h"
#include "WetLabsSeaOWL_UV_AIF.h"

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

WetLabsSeaOWL_UV_A::WetLabsSeaOWL_UV_A( const Module* module )
#ifdef __arm__
    : AsyncComponent( WetLabsSeaOWL_UV_AIF::NAME, module ),
#else
    :
    SyncSensorComponent( WetLabsSeaOWL_UV_AIF::NAME, module ),
#endif
      uart_( WetLabsSeaOWL_UV_AIF::UART,  WetLabsSeaOWL_UV_AIF::BAUD, 0.40, logger_, 4095, true ),
      loadControl_( WetLabsSeaOWL_UV_AIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      startTime_(),
      timeout_( 5.0 ),
#ifdef __arm__
      pausePeriod_( !simulateHardware() ? 0.4 : 0.1 ),
#endif
      dataTime_( Timestamp::NOT_SET_TIME ),
      serial_( Str::EMPTY_STR ),
      debug_( false ),
      output700_( -1 ),
      outputFDOM_( -1 ),
      outputOil_( -1 ),
      outputChl_( -1 ),
      volScat700_( nanf( "" ) ),
      backScat700_( nanf( "" ) ),
      fdomConc_( nanf( "" ) ),
      oilConc_( nanf( "" ) ),
      chlConc_( nanf( "" ) )
{

    timeoutCfgReader_ = newConfigReader( WetLabsSeaOWL_UV_AIF::TIMEOUT );
    periodCfgReader_ = newConfigReader( WetLabsSeaOWL_UV_AIF::PERIOD );
    serialCfgReader_ = newConfigReader( WetLabsSeaOWL_UV_AIF::SERIAL );
    scaleFactor700CfgReader_ = newConfigReader( WetLabsSeaOWL_UV_AIF::SCALE_FACTOR_700 );
    darkCounts700CfgReader_ = newConfigReader( WetLabsSeaOWL_UV_AIF::DARK_COUNTS_700 );
    scaleFactorFDOMCfgReader_ = newConfigReader( WetLabsSeaOWL_UV_AIF::SCALE_FACTOR_FDOM );
    darkCountsFDOMCfgReader_ = newConfigReader( WetLabsSeaOWL_UV_AIF::DARK_COUNTS_FDOM );
    scaleFactorOilCfgReader_ = newConfigReader( WetLabsSeaOWL_UV_AIF::SCALE_FACTOR_OIL );
    scaleFactorChlCfgReader_ = newConfigReader( WetLabsSeaOWL_UV_AIF::SCALE_FACTOR_CHL );
    darkCountsOilCfgReader_ = newConfigReader( WetLabsSeaOWL_UV_AIF::DARK_COUNTS_OIL );
    darkCountsChlCfgReader_ = newConfigReader( WetLabsSeaOWL_UV_AIF::DARK_COUNTS_CHL );
    fdomAccuracyCfgReader_ = newConfigReader( WetLabsSeaOWL_UV_AIF::FDOM_ACCURACY );
    oilAccuracyCfgReader_ = newConfigReader( WetLabsSeaOWL_UV_AIF::OIL_ACCURACY );
    chlAccuracyCfgReader_ = newConfigReader( WetLabsSeaOWL_UV_AIF::CHL_ACCURACY );

    salinityReader_ = newUniversalReader( UniversalURI::SEA_WATER_SALINITY );

    output700Writer_ = newDataWriter( WetLabsSeaOWL_UV_AIF::OUTPUT_700 );
    outputFDOMWriter_ = newDataWriter( WetLabsSeaOWL_UV_AIF::OUTPUT_FDOM );
    outputOilWriter_ = newDataWriter( WetLabsSeaOWL_UV_AIF::OUTPUT_OIL );
    outputChlWriter_ = newDataWriter( WetLabsSeaOWL_UV_AIF::OUTPUT_CHL );

    volScat700Writer_ = newDataWriter( WetLabsSeaOWL_UV_AIF::VOLUME_SCAT_COEFF_117DEG_700NM );

    backScat700Writer_ = newDataWriter( WetLabsSeaOWL_UV_AIF::BACKSCATTERING_COEFF_700NM );

    fdomConcWriter_ = newUniversalWriter( UniversalURI::CONCENTRATION_OF_COLORED_DISSOLVED_ORGANIC_MATTER_IN_SEA_WATER );
    oilConcWriter_ = newUniversalWriter( UniversalURI::MASS_CONCENTRATION_OF_PETROLEUM_HYDROCARBONS_IN_SEA_WATER );
    chlConcWriter_ = newUniversalWriter( UniversalURI::MASS_CONCENTRATION_OF_CHLOROPHYLL_IN_SEA_WATER );

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

    this->setAllowableFailures( 5 );
    this->setRetryTimeout( 150 );
    this->setFailureMissionCritical( false );

}

WetLabsSeaOWL_UV_A::~WetLabsSeaOWL_UV_A()
{
}

bool WetLabsSeaOWL_UV_A::readConfig()
{
    bool ok = true;
    float tempSecs = 0;
    float oilAccuracy = DataElement::NO_ACCURACY;
    float chlAccuracy = DataElement::NO_ACCURACY;
    float fdomAccuracy = DataElement::NO_ACCURACY;
    StrValue tempStr( Str::EMPTY_STR );
    timeoutCfgReader_->read( Units::SECOND, tempSecs );
    serialCfgReader_->read( tempStr );
    serial_ = tempStr.asString();

    ok &= ( scaleFactor700CfgReader_->read( Units::RECIPROCAL_METER_PER_STERADIAN_PER_COUNT, scaleFactor700_ ) && !isnan( scaleFactor700_ ) );
    ok &= darkCounts700CfgReader_->read( Units::COUNT, darkCounts700_ );

    ok &= ( scaleFactorFDOMCfgReader_->read( Units::PART_PER_BILLION_PER_COUNT, scaleFactorFDOM_ ) && !isnan( scaleFactorFDOM_ ) );
    ok &= darkCountsFDOMCfgReader_->read( Units::COUNT, darkCountsFDOM_ );

    ok &= ( scaleFactorChlCfgReader_->read( Units::MICROGRAM_PER_LITER_PER_COUNT, scaleFactorChl_ ) && !isnan( scaleFactorChl_ ) );
    ok &= darkCountsChlCfgReader_->read( Units::COUNT, darkCountsChl_ );

    ok &= ( scaleFactorOilCfgReader_->read( Units::PART_PER_BILLION_PER_COUNT, scaleFactorOil_ ) && !isnan( scaleFactorOil_ ) );
    ok &= darkCountsOilCfgReader_->read( Units::COUNT, darkCountsOil_ );

    if( oilAccuracyCfgReader_->read( Units::PART_PER_BILLION, oilAccuracy ) && !isnan( oilAccuracy ) )
    {
        oilConcWriter_->setAccuracy( Units::PART_PER_BILLION, oilAccuracy );
    }
    if( chlAccuracyCfgReader_->read( Units::MICROGRAM_PER_LITER, chlAccuracy ) && !isnan( chlAccuracy ) )
    {
        chlConcWriter_->setAccuracy( Units::MICROGRAM_PER_LITER, chlAccuracy );
    }
    if( fdomAccuracyCfgReader_->read( Units::PART_PER_BILLION, fdomAccuracy ) && !isnan( fdomAccuracy ) )
    {
        fdomConcWriter_->setAccuracy( Units::PART_PER_BILLION, fdomAccuracy );
    }

    return ok;
}

void WetLabsSeaOWL_UV_A::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 WetLabsSeaOWL_UV_A::start()
{
    if( debug_ ) logger_.syslog( "Start", Syslog::INFO );

    if( !readConfig() )
    {
        logger_.syslog( "Failed to load configuration parameters.", Syslog::CRITICAL );
        this->setFailure( FailureMode::DATA );
        return STOP;
    }

    if( simulateHardware() )
    {
        return STARTING;
    }


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

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

    startTime_ = Timestamp::Now();
    if( !simulateHardware() )
    {
        logger_.syslog( "Powering up", Syslog::INFO );
        if( !loadControl_.powerUp() )
        {
            logger_.syslog( "Failed to power up", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }
    }

    return STARTING;
}

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

    // Look for output
    if( uart_.readUntil( deviceResponse_, MAX_DEVICE_RESPONSE_SIZE, "\n", 1 ).hasError() )
    {
        if( debug_ ) logger_.syslog( "In starting, uart error:", uart_.errorString(), Syslog::INFO );
        if( uart_.getError() != UartStream::TIMEOUT )
        {
            logger_.syslog( "Uart error: ", uart_.errorString(), Syslog::ERROR );
            this->setFailure( FailureMode::COMMUNICATIONS );
            return STOP;
        }
        // See if the timeout has expired
        else if( startTime_.elapsed() > timeout_ )
        {
            this->setFailure( FailureMode::COMMUNICATIONS );
            return STOP;
        }
    }
    else
    {
        unsigned int bytesRead = uart_.bytesRead();
        if( debug_ ) logger_.syslog( "In starting, bytesRead=", ( int )bytesRead, Syslog::INFO );
        deviceResponse_[AuvMath::Min( ( unsigned )MAX_DEVICE_RESPONSE_SIZE, bytesRead )] = '\0';
        char* trimmedDeviceResponse = strstr( deviceResponse_, serial_.cStr() );
        if( trimmedDeviceResponse )
        {
            if( !parse( trimmedDeviceResponse ) )
            {
                this->setFailure( FailureMode::DATA );
                return STOP;
            }
            startTime_ = Timestamp::Now();
            return RUNNABLE;
        }
    }

    // coverity[secure_coding] // indicate that the code below is indeed safe
    return STARTING;
}

/// Pause for a short period (indicated by pauseTime)
Component::RunState WetLabsSeaOWL_UV_A::pause()
{
    if( debug_ ) logger_.syslog( "Pause", Syslog::INFO );
    return STOPPED; // State not used at this time
}

/// Should eventually follow a PAUSE request: should set continueTime
Component::RunState WetLabsSeaOWL_UV_A::paused()
{
    if( debug_ ) logger_.syslog( "Paused", Syslog::INFO );
    pausePeriod_.sleepFor();
    return STOPPED; // State not used at this time

}

Component::RunState WetLabsSeaOWL_UV_A::resume()
{
    if( debug_ ) logger_.syslog( "Resume", Syslog::INFO );
    return STOPPED; // State not used at this time
}

Component::RunState WetLabsSeaOWL_UV_A::resuming()
{
    return STOPPED; // State not used at this time
}

Component::RunState WetLabsSeaOWL_UV_A::runnable()
{
    // if( debug_ ) logger_.syslog( "Runnable", Syslog::INFO );
    runTime_ = Timestamp::Now();
    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() )
        {
            logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }

        // Read most recent device data
        while( uart_.dataAvailable() && ( readCount < MAX_DEVICE_MSG_QUEUE_SIZE ) )
        {
            // The uart explicitly blocks here so we don't run in a fast loop
            if( uart_.readUntil( deviceResponse_, MAX_DEVICE_RESPONSE_SIZE, "\n", 1 ).hasError() )
            {
                if( uart_.getError() != UartStream::TIMEOUT )
                {
                    logger_.syslog( "Uart error: ", uart_.errorString(), Syslog::ERROR );
                    this->setFailure( FailureMode::COMMUNICATIONS );
                    setRunState( STOP );
                }
                processPacket = false;
                break;
            }
            else
            {
                // 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 ) )
        {
            unsigned bytesRead = ( unsigned )uart_.bytesRead();
            deviceResponse_[AuvMath::Min( ( unsigned )MAX_DEVICE_RESPONSE_SIZE, bytesRead )] = '\0';
            char* trimmedDeviceResponse = strstr( deviceResponse_, serial_.cStr() );
            if( trimmedDeviceResponse )
            {
                dataToWrite = parse( trimmedDeviceResponse );
                if( !dataToWrite )
                {
                    if( debug_ ) logger_.syslog( "Error parsing device response", Syslog::INFO );
                }
                else
                {
                    dataTime_ = Timestamp::Now();
                }
            }
        }
        else if( readCount >= MAX_DEVICE_MSG_QUEUE_SIZE )
        {
            // 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
    {
        if( SimSlate::ReadScience( UniversalURI::MASS_CONCENTRATION_OF_PETROLEUM_HYDROCARBONS_IN_SEA_WATER, Units::MICROGRAM_PER_LITER, oilConc_ ) )
        {
            float counts = oilConc_ / scaleFactorOil_ + darkCountsOil_;
            fdomConc_ = ( counts - darkCountsFDOM_ ) * scaleFactorFDOM_;
            if( debug_ ) logger_.syslog( "From ReadScience got oilConc_=" + Str( oilConc_ ), Syslog::INFO );
        }
        SimSlate::Read( SimSlate::MASS_CONCENTRATION_OF_CHLOROPHYLL_UG_PER_L, chlConc_ );

        SimSlate::Read( SimSlate::MASS_CONCENTRATION_OF_PETROLEUM_HYDROCARBON_KG_PER_CUBIC_METER, oilConc_ );
        /*debug
        printf("Here\\");
        logger_.syslog( "Str( oilConc_ )=" + Str( oilConc_ ), Syslog::INFO );
        */

        SimSlate::Read( SimSlate::MASS_CONCENTRATION_OF_CHLOROPHYLL_UG_PER_L, volScat700_ ); // TODO: Use the correct SimSlate entry for volScat700_
        dataToWrite = true; // assume the sim always has data to write
    }

    if( dataToWrite ) // write the data
    {
        startTime_ = Timestamp::Now();
        writeData();
        this->resetFailCount();
    }

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

    runPeriod_ = runTime_.elapsed();
    if( runPeriod_ < pausePeriod_ )
    {
        Timespan::Seconds( ( pausePeriod_ - runPeriod_ ).asDouble() ).sleepFor();
    }

    return RUNNABLE;
}

Component::RunState WetLabsSeaOWL_UV_A::stop()
{
    if( debug_ ) logger_.syslog( "Stop", Syslog::INFO );
    if( !simulateHardware() )
    {
        uninitialize(); // First power down then query for faults next cycle
    }
    output700_ = -1;
    outputFDOM_ = -1;
    outputOil_ = -1;
    outputChl_ = -1;
    volScat700_ = nanf( "" );
    backScat700_ = nanf( "" );
    fdomConc_ = nanf( "" );
    oilConc_ = nanf( "" );
    chlConc_ = nanf( "" );

    writeData(); // TODO: why do we write all the NaNs? shouldn't this be treated consistently across all sensors?

    pausePeriod_.sleepFor();
    return STOPPING;
}


Component::RunState WetLabsSeaOWL_UV_A::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 );
        }

    }
    // skipping pasusePeriod here as we can run straight through to stopped
    return STOPPED;
}


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

    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();
        }
    }

    pausePeriod_.sleepFor();
    return STOPPED;
}

bool WetLabsSeaOWL_UV_A::parse( char* trimmedDeviceResponse )
{
    if( NULL != trimmedDeviceResponse )
    {
        int scanned = sscanf( trimmedDeviceResponse, "%*s %*d %*d %*d %d %*d %*d %*d %d %*d %*d %*d %*d %d",
                              &outputChl_, &output700_, &outputFDOM_ );
        if( debug_ ) logger_.syslog( Str( "scanned=" ) + scanned + ", outputChl_=" + outputChl_ + ", output700_=" + output700_ + ", outputFDOM_=" + outputFDOM_, Syslog::INFO );
        if( 3 == scanned )
        {
            if( outputChl_ < 160000 && outputChl_ >= darkCountsChl_ )
            {
                chlConc_ = scaleFactorChl_ * ( outputChl_ - darkCountsChl_ );
                outputChlWriter_->setInvalid( false );
                chlConcWriter_->setInvalid( false );
            }
            else
            {
                outputChlWriter_->setInvalid( true );
                chlConcWriter_->setInvalid( true );
            }

            if( output700_ < 160000 && output700_ >= darkCounts700_ )
            {
                volScat700_ = scaleFactor700_ * ( output700_ - darkCounts700_ );
                output700Writer_->setInvalid( false );
                volScat700Writer_->setInvalid( false );
            }
            else
            {
                output700Writer_->setInvalid( true );
                volScat700Writer_->setInvalid( true );
            }

            if( outputFDOM_ < 160000 && outputFDOM_ >= darkCountsFDOM_ )
            {
                fdomConc_ = scaleFactorFDOM_ * ( outputFDOM_ - darkCountsFDOM_ );
                outputFDOMWriter_->setInvalid( false );
                fdomConcWriter_->setInvalid( false );
            }
            else
            {
                outputFDOMWriter_->setInvalid( true );
                fdomConcWriter_->setInvalid( true );
            }

            outputOil_ = outputFDOM_;
            if( outputOil_ < 160000 && outputOil_ >= darkCountsOil_ )
            {
                oilConc_ = scaleFactorOil_ * ( outputOil_ - darkCountsOil_ );
                outputOilWriter_->setInvalid( false );
                oilConcWriter_->setInvalid( false );
            }
            else
            {
                outputOilWriter_->setInvalid( true );
                oilConcWriter_->setInvalid( true );
            }
            if( debug_ ) logger_.syslog( Str( "parsed, chlConc_=" ) + chlConc_ + ", volScat700_=" + volScat700_ + ", fdomConc_=" + fdomConc_ + ", oilConc_=" + oilConc_, Syslog::INFO );
            return true;
        }
    }
    if( debug_ ) logger_.syslog( "Error parsing data: ", deviceResponse_, Syslog::ERROR );
    return false;
}

void WetLabsSeaOWL_UV_A::writeData()
{
    output700Writer_->write( Units::COUNT, output700_, dataTime_ );
    outputFDOMWriter_->write( Units::COUNT, outputFDOM_, dataTime_ );
    outputOilWriter_->write( Units::COUNT, outputOil_, dataTime_ );
    outputChlWriter_->write( Units::COUNT, outputChl_, dataTime_ );
    volScat700Writer_->write( Units::RECIPROCAL_METER_PER_STERADIAN, volScat700_, dataTime_ );
    fdomConcWriter_->write( Units::PART_PER_BILLION, fdomConc_, dataTime_ );
    oilConcWriter_->write( Units::MICROGRAM_PER_LITER, oilConc_, dataTime_ );
    chlConcWriter_->write( Units::MICROGRAM_PER_LITER, chlConc_, dataTime_ );

    // TODO: this section below seems more like data preprocessing. It shouldn't be in the writeData method
    if( !volScat700Writer_->isInvalid() )
    {
        float salinity( nanf( "" ) );
        float theta = D2R( 117.0 );
        float d = 0.09;
        bool haveSalinity = salinityReader_->wasTouchedSinceLastRun( this )
                            && salinityReader_->read( Units::PRACTICAL_SALINITY_UNIT, salinity )
                            && !isnan( salinity );
        if( haveSalinity && !volScat700Writer_->isInvalid() )
        {
            // These calculations are from the Triplet Puck User’s Guide, Revision H
            float Bw_117_700 = 1.38 * pow( 700.0 / 500.0, -4.32 ) * ( 1 + 0.3 * salinity / 37 ) * 1e-4 * ( 1 + pow( cos( theta ), 2 ) * ( 1 - d ) / ( 1 + d ) );
            float Bp_117_700 = volScat700_ - Bw_117_700;
            float bw_700;
            if( salinity < 35 )
            {
                bw_700 = 0.0022533 * pow( 700.0 / 500.0, -4.23 ) * 1e-4;
            }
            else
            {
                bw_700 = 0.0029308 * pow( 700.0 / 500.0, -4.24 ) * 1e-4;
            }
            float bbw_700 = bw_700 / 2;
            float bbp_700 = M_2PI * 1.1 * Bp_117_700;
            backScat700_ = bbp_700 + bbw_700;
            backScat700Writer_->write( Units::RECIPROCAL_METER, backScat700_, dataTime_ );
            backScat700Writer_->setInvalid( false );
        }
        else
        {
            backScat700Writer_->setInvalid( true );
        }
    }
    else
    {
        backScat700Writer_->setInvalid( true );
    }

}

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

bool WetLabsSeaOWL_UV_A::isDataRequested()
{
    return oilConcWriter_->isDataRequested()
           || chlConcWriter_->isDataRequested()
           || volScat700Writer_->isDataRequested()
           || fdomConcWriter_->isDataRequested();
}

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