/** \file
 *
 *  Contains the WetLabsUBAT class implementation.
 *
 *  Copyright (c) 2014 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#include "WetLabsUBAT.h"
#include "WetLabsUBATIF.h"
#include "data/ConfigReader.h"
#include "data/BlobWriter.h"
#include "data/SimSlate.h"
#include "units/Units.h"
#include <string.h>

/**
 * UBAT Hardware User’s Guide, Revision C, 9 July 2015
 *
 * UBAT Headers:
 * ------------------------------------------------------------------------
 * | Col   | Output   | Header                              | Units       |
 * ------------------------------------------------------------------------
 * | 1     | UBAT0051 | UBAT S/N                            | NA          |
 * | 2     | 00001    | Record Number                       | NA          |
 * | 3     | 1.46E9   | Calibration coefficient for HV step | photons s-1 |
 * | 4     | 0.000E00 | Average Bioluminescence             | photons s-1 |
 * | 5     | 1193     | Pump RPM                            | RPM         |
 * | 6     | 11.894   | System V oltage                     | Volt        |
 * | 7     | 736      | Flow RPM                            | RPM         |
 * | 8     | 0.364    | HV step                             | Volt        |
 * | 9-10  | 39, 0.00 | Reserved                            | NA          |
 * | 11-70 | 0,12,... | 60 Hz digitized raw A/D counts      | NA          |
 * ------------------------------------------------------------------------
 *
 * Example of UBAT output:
 * -------------------------------------------------------------------------
 * UBAT0051,00001,1.46E9,0.000E00,1193,11.894,736,0.364,39,0.00,0,12,102,62,8,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,4,1,0,1,0,0,0,0,0,0,0,1,1,0,0,0
 *
 */

using namespace WetLabsUBATIF;

const Timespan WetLabsUBAT::PERIOD( 0.4 );

const unsigned short WetLabsUBAT::MAX_DEVICE_MSG_QUEUE_SIZE( 5 );

// NOTE: actual msg should have 70 headers, but setting it here to 71 ends
//       up working out better when parsing.
const unsigned short WetLabsUBAT::DEVICE_MSG_HEADER_NUM( 70 + 1 );

const unsigned short WetLabsUBAT::RECORD_NUM_OFFSET( 2 );
const unsigned short WetLabsUBAT::HV_STEP_CAL_OFFSET( 3 );
const unsigned short WetLabsUBAT::AVG_BIOLUM_OFFSET( 4 );
const unsigned short WetLabsUBAT::FLOW_RPM_OFFSET( 7 );
const unsigned short WetLabsUBAT::RAW_AD_DATA_OFFSET( 11 );

WetLabsUBAT::WetLabsUBAT( const Module* module )
    : AsyncComponent( WetLabsUBATIF::NAME, module, PERIOD ),
      uart_( WetLabsUBATIF::UART,  WetLabsUBATIF::BAUD, 0.4, logger_, 4095, true ),
      loadControl_( WetLabsUBATIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      powerOnTimeout_( 5.0 ),
      dataTimestamp_( Timestamp::NOT_SET_TIME ),
      startTime_( Timestamp::NOT_SET_TIME ),
      sampleTimeout_( 30.0 ),
      avgBiolum_( nanf( "" ) ),
      recordNum_( 0 ),
      hvStepCalCoeff_( nanf( "" ) ),
      flowrateCalibCoeff_( 0 ),
      minFlowrateThreshold_( 0.05 ),
      flowRate_( 0 ),
      flowRPM_( 0 ),
      loadAtStartup_( true ),
      debug_( false )
{
    memset( &rawADCounts_, 0x00, RAW_AD_DATA_SIZE );

    // Confiuration inputs
    flowrateCalCoeffCfgReader_ = newConfigReader( WetLabsUBATIF::FLOWRATE_CAL_COEFF );
    minFlowrateCfgReader_ = newConfigReader( WetLabsUBATIF::MIN_FLOWRATE_THRESHOLD );
    serialCfgReader_ = newConfigReader( WetLabsUBATIF::SERIALNO );

    // Slate outputs
    avgBiolumWriter_       = newDataWriter( WetLabsUBATIF::AVG_BIOLUM );
    flowRateWriter_        = newDataWriter( WetLabsUBATIF::FLOW_RATE );
    hvStepCalCoeffWriter_  = newDataWriter( WetLabsUBATIF::HV_STEP_CAL_COEFF );
    recordNumberWriter_    = newDataWriter( WetLabsUBATIF::UBAT_RECORD_NUM );

    rawADCountsWriter_     = newBlobWriter( WetLabsUBATIF::RAW_AD_COUNT );

    // loadAtStartup bypass
    enableUbatReader_ = newDataReader( WetLabsUBATIF::UBAT_ENABLE_MODE );
    enableUbatWriter_ = newDataWriter( WetLabsUBATIF::UBAT_ENABLE_MODE );
    enableUbatWriter_->write( Units::COUNT, ( int )WetLabsUBATIF::UBAT_ENABLE );

    this->setFailureMissionCritical( true );
    this->setAllowableFailures( 5 );
    this->setRetryTimeout( 600 );

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

WetLabsUBAT::~WetLabsUBAT()
{
}

bool WetLabsUBAT::readConfig()
{
    bool ok( true );

    StrValue tempStr( Str::EMPTY_STR );
    ok &= serialCfgReader_->read( tempStr );
    if( ok ) deviceSerialNumber_ = tempStr.asString();

    ok &= flowrateCalCoeffCfgReader_->read( Units::NONE, flowrateCalibCoeff_ );
    ok &= minFlowrateCfgReader_->read( Units::LITER_PER_SECOND, minFlowrateThreshold_ );

    return ok;
}

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

    readConfig();

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

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

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


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

    if( startTime_.elapsed() < powerOnTimeout_ )
    {
        // Allow time for the device to power up
        return STARTING;
    }

    if( simulateHardware() )
    {
        return RUNNABLE;
    }

    // Look for device output
    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 );
            return STOP;
        }
    }
    else
    {
        char* strTokPtr;
        int tokens = 0;

        // Parse on comma
        char* response = strtok_r( deviceResponse_, ",", &strTokPtr );
        tokens++;

        // First header holds the device serial number
        if( response != NULL && checkSerial( response ) )
        {
            // Count the number of headers in the device message...
            while( response != NULL && tokens < DEVICE_MSG_HEADER_NUM )
            {
                response = strtok_r( NULL, ",", &strTokPtr );
                tokens++;
            }

            // ...and make sure all headers are accounted for
            if( tokens != DEVICE_MSG_HEADER_NUM )
            {
                logger_.syslog( "Failed to acquire vaid data. Device message size is undefined.", Syslog::FAULT );
                this->setFailure( FailureMode::DATA );
                return STOP;
            }

            startTime_ = Timestamp::Now();
            this->resetFailCount();
            return RUNNABLE;
        }
    }

    // See if the timeout has expired
    if( startTime_.elapsed() > sampleTimeout_ )
    {
        logger_.syslog( "Failed to acquire valid data within specified timeout upon startup.", Syslog::FAULT );
        this->setFailure( FailureMode::COMMUNICATIONS );
        return STOP;
    }

    return STARTING;
}

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

// Should eventually follow a PAUSE request: should set continueTime
Component::RunState WetLabsUBAT::paused()
{
    if( debug_ ) logger_.syslog( "paused", Syslog::INFO );
    return stop(); // State not used
}

Component::RunState WetLabsUBAT::resume()
{
    if( debug_ ) logger_.syslog( "resume", Syslog::INFO );
    return stop(); // State not used
}

Component::RunState WetLabsUBAT::resuming()
{
    if( debug_ ) logger_.syslog( "resuming", Syslog::INFO );
    return stop(); // State not used
}

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

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

    if( simulateHardware() )
    {
        if( getSimulatedData() )
        {
            // TODO: deal with sim data
        }
        return RUNNABLE;
    }

    // Check load control board
    // Log voltage and current and check for any faults
    if( !simulateHardware() )
    {
        loadControl_.requestVoltageAndCurrent();
        if( loadControl_.hasError() )
        {
            logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }
    }

    // Get data
    if( readRecord() )
    {
        // Process and write the data
        processRecord();
        writeData();

        // Report flow rate issues
        if( flowTime_.elapsed() > sampleTimeout_ )
        {
            logger_.syslog( "UBAT flow rate is below the specified threshold of " + Str( minFlowrateThreshold_, 2 ) + " l/s.", Syslog::FAULT );
            flowTime_ = Timestamp::Now();
        }

        startTime_ = Timestamp::Now();
        this->resetFailCount();
    }

    // Re-init if we've expired
    if( startTime_.elapsed() > sampleTimeout_ )
    {
        logger_.syslog( "Failed to acquire valid data within specified timeout.", Syslog::FAULT );
        this->setFailure( FailureMode::COMMUNICATIONS );
        return STOP;
    }

    return RUNNABLE;
}


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

    // First power down then query for faults next cycle
    uninitialize();

    return STOPPING;
}


Component::RunState WetLabsUBAT::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 WetLabsUBAT::stopped()
{
    //    if( debug_ ) logger_.syslog( "Stopped", Syslog::INFO );

    if( isDataRequested() )
    {
        if( debug_ ) logger_.syslog( "Data requested.", Syslog::INFO );
        return START;
    }
    return STOPPED;
}


bool WetLabsUBAT::readRecord()
{
    bool processRecord( false );
    unsigned short readCount( 0 );

    // Read most recent device data
    while( uart_.dataAvailable() && readCount < MAX_DEVICE_MSG_QUEUE_SIZE )
    {
        // The uart explicitly blocks here
        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 );
            }
            processRecord = false;
            break;
        }
        else
        {
            // Got device response
            if( debug_ ) logger_.syslog( "Got device response. Skip count: " + Str( readCount ) + ".", Syslog::INFO );
            processRecord = readCount < MAX_DEVICE_MSG_QUEUE_SIZE;
            dataTimestamp_ = Timestamp::Now();
        }

        ++readCount;
    }

    // Process device data
    if( processRecord )
    {
        if( parseRecord() )
        {
            return true;
        }
    }
    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. Reached max message skip count.", Syslog::ERROR );
        uart_.flush();
    }

    return false;
}

bool WetLabsUBAT::parseRecord()
{
    /**
     *
     * Example of UBAT output headers:
     * -------------------------------------------------------------------------
     * UBAT0051,00001,1.46E9,0.000E00,1193,11.894,736,0.364,39,0.00,0,12,102,62,8,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,4,1,0,1,0,0,0,0,0,0,0,1,1,0,0,0
     *
     */
    char* strTokPtr;
    int tokens = 0;
    memset( &rawADCounts_, 0x00, RAW_AD_DATA_SIZE );
    hvStepCalCoeff_ = nanf( "" );
    avgBiolum_ = nanf( "" );
    flowRPM_ = 0;

    // Parse on comma, first header is the device serial number (toss)
    char* response = strtok_r( deviceResponse_, ",", &strTokPtr );
    tokens++;

    // Extract the HV step cal coeff, avg biolum, and flow RPM
    while( response != NULL && tokens < RAW_AD_DATA_OFFSET )
    {
        switch( tokens )
        {
        case RECORD_NUM_OFFSET:
            recordNum_ = atof( response );
            break;

        case HV_STEP_CAL_OFFSET:
            hvStepCalCoeff_ = atof( response );
            break;

        case AVG_BIOLUM_OFFSET:
            avgBiolum_ = atof( response );
            break;

        case FLOW_RPM_OFFSET:
            flowRPM_ = atoi( response );
            break;

        default:
            break;
        }

        response = strtok_r( NULL, ",", &strTokPtr );
        tokens++;
    }

    // Extract 60 Hz digitized raw A/D counts
    int i = 0;
    while( response != NULL && i < RAW_AD_DATA_SIZE )
    {
        rawADCounts_[i] = atoi( response );

        response = strtok_r( NULL, ",", &strTokPtr );
        tokens++;
        i++;
    }

    // Confirm we got the correct number of headers from the message
    if( tokens != DEVICE_MSG_HEADER_NUM )
    {
        logger_.syslog( "Failed to parse incomplete device message.", Syslog::ERROR );
        return false;
    }

    return true;
}

void WetLabsUBAT::processRecord()
{
    flowRate_ = 0.0;
    // Convert RPMs to flow rate (l s-1)
    // Flow rate (l s-1) = Flow (RPM) * Flow rate calibration coefficient
    flowRate_ = float( flowRPM_ ) * flowrateCalibCoeff_;

    if( flowRate_ > minFlowrateThreshold_ )
    {
        flowTime_ = Timestamp::Now();
    }
}

void WetLabsUBAT::writeData()
{
    avgBiolumWriter_->write( Units::PHOTON_PER_SECOND, avgBiolum_, dataTimestamp_ );
    flowRateWriter_->write( Units::LITER_PER_SECOND, flowRate_, dataTimestamp_ );
    hvStepCalCoeffWriter_->write( Units::NONE, hvStepCalCoeff_, dataTimestamp_ );
    recordNumberWriter_->write( Units::COUNT, recordNum_, dataTimestamp_ );

    rawADCountsWriter_->write1DArray( Units::COUNT, rawADCounts_, dataTimestamp_ );
}

bool WetLabsUBAT::checkSerial( const char* serial )
{
    if( deviceSerialNumber_ != serial )
    {
        logger_.syslog( "Device serial number mismatch, expected " + deviceSerialNumber_ + " but got " + Str( serial ) + ".", Syslog::ERROR );
        return false;
    }

    return true;
}

bool WetLabsUBAT::isDataRequested()
{
    bool dataRequested = avgBiolumWriter_->isDataRequested()
                         || hvStepCalCoeffWriter_->isDataRequested()
                         || flowRateWriter_->isDataRequested();

    if( dataRequested )
    {
        WetLabsUBATIF::UBATMode mode( WetLabsUBATIF::UBAT_ENABLE );
        mode = ( WetLabsUBATIF::UBATMode )enableUbatReader_->asInt( Units::COUNT );
        dataRequested &= ( mode == WetLabsUBATIF::UBAT_ENABLE );
    }

    return dataRequested;
}

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

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

bool WetLabsUBAT::getSimulatedData()
{
    avgBiolum_ = 1.0;
    flowRate_ = 0.3;
    return true;
}

// Should return [myNamespace]::SIMULATE_HARDWARE, or [myNamespace]::POWER, etc
ConfigURI WetLabsUBAT::getConfigURI( ConfigOption configOption ) const
{
    return configOption == CONFIG_SIMULATE_HARDWARE ? WetLabsUBATIF::SIMULATE_HARDWARE : ConfigURI::NO_CONFIG_URI;
}