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

#include "CTD_NeilBrown.h"
#include "CTD_NeilBrownIF.h"

#include "data/ConfigReader.h"
#include "data/SimSlate.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"
#include "units/Units.h"
#include "bitModule/CBITIF.h"

#include <stdlib.h>
#include <cmath>        // std::pow

#define CTD_ACCURACY_COND (0.001)    // (S/m). Per spec
#define CTD_ACCURACY_TEMP (0.005)   // (C) per spec
#define CTD_ACCURACY_DENSITY (0.01)  // per calc
#define CTD_ACCURACY_SALINITY (0.01) // per calc
#define CTD_ACCURACY_DEPTH (0.04) // per Brett
#define CTD_ACCURACY_SPEED (0.15)    // per calc
#define CTD_ACCURACY_PRESSURE CTD_ACCURACY_DEPTH
#define CTD_DATA_ITEMS (5)
// #define CTD_DATA_WIDTH (32)
#define CTD_BIN_SIZE (45) // XXX hard-coded bin size to start

// 5Hz data streaming from sensor
const Timespan CTD_NeilBrown::PERIOD( 0.2 );

CTD_NeilBrown::CTD_NeilBrown( const Module* module )
    : AsyncComponent( CTD_NeilBrownIF::NAME, module, PERIOD ),
      bufferOffset_( 0 ),
      debug_( false ),
      loadControl_( CTD_NeilBrownIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      pressBoundingError_( false ),
      startTime_( Timestamp::NOT_SET_TIME ),
      ctdTimeout_( 7.0 ),
      uart_( CTD_NeilBrownIF::UART, CTD_NeilBrownIF::BAUD, 0.40, logger_, 4095, true ),
      latitude_( nanf( "" ) ),
      conductivity_( 0 ),
      temperature_( 0 ),
      pressureDB_( 0 ),
      depth_( 0 ),
      salinity_( 0 ),
      density_( 0 ),
      soundSpeed_( 0 ),
      badConductivity_( true ),
      badTemperature_( true ),
      badPressure_( true ),
      badSalinity_( true ),
      pressureOffset_( 0 ),
      maxPressBound_( 499 ),
      minPressBound_( -9 ),
      maxSalinityBound_( 38 ),
      minSalinityBound_( 28 ),
      temperatureBin_( CTD_BIN_SIZE ),
      salinityBin_( CTD_BIN_SIZE ),
      nextLineQuestionable_( true )

{
    // Configuration inputs
    maxPressBoundCfgReader_    = newConfigReader( CTD_NeilBrownIF::MAX_PRESS_BOUND );
    minPressBoundCfgReader_    = newConfigReader( CTD_NeilBrownIF::MIN_PRESS_BOUND );
    offsetCfgReader_           = newConfigReader( CTD_NeilBrownIF::OFFSET );
    maxSalinityBoundCfgReader_ = newConfigReader( CTD_NeilBrownIF::MAX_SALINITY_BOUND );
    minSalinityBoundCfgReader_ = newConfigReader( CTD_NeilBrownIF::MIN_SALINITY_BOUND );

    // Slate inputs
    latitudeReader_     = newUniversalReader( UniversalURI::LATITUDE );
    gfScanActiveReader_ = newDataReader( CBITIF::GF_ACTIVE_STATE );

    // Universal Slate outputs with specified accuracy
    conductivityWriter_ = newUniversalWriter( UniversalURI::SEA_WATER_ELECTRICAL_CONDUCTIVITY, Units::MILLIMHO_PER_CENTIMETER, CTD_ACCURACY_COND );
    densityWriter_      = newUniversalWriter( UniversalURI::SEA_WATER_DENSITY, Units::KILOGRAM_PER_CUBIC_METER, CTD_ACCURACY_DENSITY );
    depthWriter_        = newUniversalWriter( UniversalURI::DEPTH, Units::METER, CTD_ACCURACY_DEPTH );
    pressureWriter_     = newUniversalWriter( UniversalURI::SEA_WATER_PRESSURE, Units::DECIBAR, CTD_ACCURACY_PRESSURE );
    salinityWriter_     = newUniversalWriter( UniversalURI::SEA_WATER_SALINITY, Units::PRACTICAL_SALINITY_UNIT, CTD_ACCURACY_SALINITY );
    soundSpeedWriter_   = newUniversalWriter( UniversalURI::SPEED_OF_SOUND_IN_SEA_WATER, Units::METER_PER_SECOND, CTD_ACCURACY_SPEED );
    temperatureWriter_  = newUniversalWriter( UniversalURI::SEA_WATER_TEMPERATURE, Units::CELSIUS, CTD_ACCURACY_TEMP );

    Str binChannelName = getName() + "_BINNED";
    binMedianTemperatureWriter_             = newDataWriter( binChannelName, CTD_NeilBrownIF::BIN_MEDIAN_SEA_WATER_TEMPERATURE );
    binMeanTemperatureWriter_               = newDataWriter( binChannelName, CTD_NeilBrownIF::BIN_MEAN_SEA_WATER_TEMPERATURE );
    binStandardDeviationTemperatureWriter_  = newDataWriter( binChannelName, CTD_NeilBrownIF::BIN_STANDARD_DEVIATION_SEA_WATER_TEMPERATURE );
    binMedianSalinityWriter_                = newDataWriter( binChannelName, CTD_NeilBrownIF::BIN_MEDIAN_SEA_WATER_SALINITY );
    binMeanSalinityWriter_                  = newDataWriter( binChannelName, CTD_NeilBrownIF::BIN_MEAN_SEA_WATER_SALINITY );
    binStandardDeviationSalinityWriter_     = newDataWriter( binChannelName, CTD_NeilBrownIF::BIN_STANDARD_DEVIATION_SEA_WATER_SALINITY );

    this->setFailureMissionCritical( false );

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

CTD_NeilBrown::~CTD_NeilBrown()
{
}

void CTD_NeilBrown::run()
{
}

bool CTD_NeilBrown::parseResponse( char *data )
{
    char* checkedData;
    char* strTokPtr;
    char* ckPtr;
    int tokens = 0;
    int cksum = 0xFF;
    int cksum_calculated;

    if( data == NULL )
    {
        logger_.syslog( "CTD sample equals NULL.", Syslog::ERROR );
        return false;
    }

    // Separate data from checksum
    checkedData = strtok_r( data, "*", &ckPtr );

    cksum_calculated = calcChecksum( data );
    if( ( 1 == sscanf( ckPtr, "%02X", &cksum ) ) && ( cksum == cksum_calculated ) )
    {
        // Checksum match. Parse the comma delimited data packet.
        tokens++;

        // Pressure
        strtok_r( checkedData, ",", &strTokPtr );
        if( checkedData != NULL )
        {
            pressureDB_ = atof( checkedData );
            pressureDB_ += pressureOffset_;
            tokens++;
        }

        // Temperature
        char* response = strtok_r( NULL, ",", &strTokPtr );
        if( response != NULL )
        {
            temperature_ = atof( response );
            tokens++;
        }

        // Conductivity
        response = strtok_r( NULL, ",", &strTokPtr );
        if( response != NULL )
        {
            conductivity_ = atof( response );
            tokens++;
        }

        // Salinity
        response = strtok_r( NULL, "*", &strTokPtr );
        if( response != NULL )
        {
            salinity_ = atof( response );
            tokens++;
        }
    }
    else
    {
        logger_.syslog( "Failed to match checksum. Expected: " + Str( cksum ) + " got: " + Str( cksum_calculated ), Syslog::ERROR );
    }

    return ( tokens == CTD_DATA_ITEMS );
}

int CTD_NeilBrown::calcChecksum( const char* response )
{
    int checksum( 0 );
    while( 0 != *response && '*' != *response )
    {
        if( *response == '$' )
        {
            checksum = 0;
        }
        else
        {
            checksum ^= ( unsigned char ) * response;
        }
        ++response;
    }
    return checksum;
}

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


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

    if( simulateHardware() )
    {
        startTime_ = Timestamp::Now();
        return STARTING;
    }

    readConfig();

    deviceResponse_[0] = '\0';
    this->setAllowableFailures( 3 );
    this->setRetryTimeout( 150 );
    if( !loadControl_.powerUp() )
    {
        logger_.syslog( "Error: CTD_NeilBrown load controller failed to power up.", Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
        return START;
    }

    // Open the uart
    uart_.open( );
    if( uart_.hasError() )
    {
        logger_.syslog( "Error opening port: ", uart_.errorString(), Syslog::ERROR );
        this->setFailure( FailureMode::COMMUNICATIONS );
        return STOP;
    }
    temperatureBin_.clear();
    salinityBin_.clear();
    startTime_ = Timestamp::Now();
    return STARTING;
}


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

    // Run command to start just in case asynchronous startup doesn't work
    go();
    return RUNNABLE;
}


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

    setWritersInvalid();

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

    return PAUSED;
}


/// Should eventually follow a PAUSE request: should set continueTime
Component::RunState CTD_NeilBrown::paused()
{
    // if( debug_ ) logger_.syslog( "Paused", Syslog::INFO );
    if( isDataRequested() )
    {
        return resume();
    }

    return PAUSED;
}


Component::RunState CTD_NeilBrown::resume()
{
    if( debug_ ) logger_.syslog( "Resume", Syslog::INFO );
    if( !simulateHardware() )
    {
        // Open the uart
        uart_.open();
        if( uart_.hasError() )
        {
            logger_.syslog( "Error opening port on resume: ", uart_.errorString(), Syslog::ERROR );
            this->setFailure( FailureMode::COMMUNICATIONS );
            return STOP;
        }

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


Component::RunState CTD_NeilBrown::resuming()
{
    if( debug_ ) logger_.syslog( "Resuming", Syslog::INFO );

    nextLineQuestionable_ = true;
    // Run command to start just in case asynchronous startup doesn't work
    go();
    startTime_ = Timestamp::Now();
    return RUNNABLE;
}


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

    // Pause if we don't want data
    if( !isDataRequested() ) return PAUSE;

    readConfig();

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


    // get a latitude (needed for the depth conversion, whether in simulation or IRL)
    if( !latitudeReader_->isActive() || !latitudeReader_->read( Units::RADIAN, latitude_ ) || isnan( latitude_ ) )
    {
        Slate::ReadOnce( "Config/workSite", "initLat", Units::RADIAN, latitude_, logger_ ); // TODO: used to be reading as angular_degree, but using same as latitude read as radians -- check this
    }

    // Always include simulated values, even if hardware exists.
    // If we are in the water, there will be no simulated values available.
    // (Trying to be consistent with this, but to me it seems it would be better to be explicit about using sim or hardware without allowing a potentially confusing mix.)
    if( getSimulatedData() || receive() ) // use simulated data or try to receive data from the UART (blocking w/ timeout)
    {
        startTime_ = Timestamp::Now(); // Reset the time
        preprocessData();
        writeData();
        updateTemporalBin();
        this->resetFailCount();
    }
    else if( startTime_.elapsed() > ctdTimeout_ )
    {
        logger_.syslog( "Failed to acquire real or simulated CTD data within timeout.", Syslog::FAULT );
        this->setFailure( FailureMode::COMMUNICATIONS );
        setRunState( STOP );
    }

    return RUNNABLE;
}


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

    setWritersInvalid();

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

    return STOPPING;
}


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

    return STOPPED;
}


void CTD_NeilBrown::readConfig()
{
    offsetCfgReader_->read( Units::DECIBAR, pressureOffset_ );
    maxPressBoundCfgReader_->read( Units::DECIBAR, maxPressBound_ );
    minPressBoundCfgReader_->read( Units::DECIBAR, minPressBound_ );
    maxSalinityBoundCfgReader_->read( Units::PRACTICAL_SALINITY_UNIT, maxSalinityBound_ );
    minSalinityBoundCfgReader_->read( Units::PRACTICAL_SALINITY_UNIT, minSalinityBound_ );
}


bool CTD_NeilBrown::isDataRequested()
{
    return densityWriter_->isDataRequested()
           || depthWriter_->isDataRequested()
           || pressureWriter_->isDataRequested()
           || salinityWriter_->isDataRequested()
           || temperatureWriter_->isDataRequested()
           || conductivityWriter_->isDataRequested()
           || soundSpeedWriter_->isDataRequested();
}

void CTD_NeilBrown::setWritersInvalid()
{
    salinityWriter_->setInvalid( false );
    temperatureWriter_->setInvalid( false );
    depthWriter_->setInvalid( false );
    pressureWriter_->setInvalid( false );
    conductivityWriter_->setInvalid( false );
    soundSpeedWriter_->setInvalid( false );
}

void CTD_NeilBrown::go()
{
    if( !simulateHardware() )
    {
        uart_ << "g\r\n";
    }
}


bool CTD_NeilBrown::receive()
{
    int msgCount( 0 );
    bool processPacket( false );
    bool gotGoodResponse( false );

    if( !simulateHardware() )
    {
        // Read most recent device data
        while( uart_.dataAvailable() && ( msgCount < 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( msgCount ) + ".", Syslog::INFO );
                processPacket = true;
            }

            ++msgCount;
        }

        if( processPacket && ( msgCount < MAX_DEVICE_MSG_QUEUE_SIZE ) )
        {
            // No error. We have a complete line.
            if( !parseResponse( deviceResponse_ ) )
            {
                logger_.syslog( "Bad response: ", deviceResponse_, Syslog::ERROR );
                gotGoodResponse = false;
            }
            else
            {
                gotGoodResponse = true;
            }
            uart_.flushCRLF();
        }
        else if( msgCount >= MAX_DEVICE_MSG_QUEUE_SIZE )
        {
            // Flush the buffer in case we're out of lockstep
            logger_.syslog( "Faild to get valid response or reached max message skip count.", Syslog::ERROR );
            uart_.flush();
        }
    }

    return gotGoodResponse;
}


bool CTD_NeilBrown::getSimulatedData()
{
    if( SimSlate::Read( SimSlate::DEPTH_METER, depth_ )
            && SimSlate::Read( SimSlate::SALINITY_PART_PER_THOUSAND, salinity_ ) // Looks like we're reading the simulated salinity in PPT, but using it as PSU
            && SimSlate::Read( SimSlate::TEMPERATURE_DEGREE_CELSIUS, temperature_ ) )
    {
        if( salinity_ < minSalinityBound_ ) salinity_ = minSalinityBound_; // truncate out-of-bounds salinity for sim
        if( salinity_ > maxSalinityBound_ ) salinity_ = maxSalinityBound_; // truncate out-of-bounds salinity for sim
        startTime_ = Timestamp::Now(); // Reset the time (for the host)
        pressureDB_ = AuvMath::OceanPressure( depth_, latitude_ ) / 10000;
        density_ = AuvMath::Density( salinity_, temperature_ + 273.15, pressureDB_ * 10000 );
        soundSpeed_ = AuvMath::SoundSpeed( salinity_, temperature_, pressureDB_ );
        conductivity_ = 4.0; // hack to make sure there is something to operate on
        return true;
    }
    else
    {
        return false;
    }
}

void CTD_NeilBrown::preprocessData()
{
    bool gfScanActive( false );
    float latitude( nanf( "" ) );

    badConductivity_ = badTemperature_ = badPressure_ = badSalinity_ = false;

    if( ( salinity_ < minSalinityBound_ ) || ( salinity_ > maxSalinityBound_ ) )
    {
        logger_.syslog( "Salinity reading out of range: " + Str( salinity_ ) + " psu", Syslog::ERROR );
        badSalinity_ = true;
    }

    if( ( pressureDB_ < minPressBound_ ) || ( pressureDB_ > maxPressBound_ ) )
    {
        logger_.syslog( "Pressure reading out of range: " + Str( pressureDB_ ) + " decibar", Syslog::ERROR );
        badPressure_ = true;
    }
    else
    {
        depth_ = AuvMath::OceanDepth( pressureDB_ * 10000.0, latitude );
        density_ = AuvMath::Density( salinity_, temperature_ + 273.15, pressureDB_ * 10000 );
        soundSpeed_ = AuvMath::SoundSpeed( salinity_, temperature_, pressureDB_ );
    }

    if( gfScanActiveReader_->read( gfScanActive ) && gfScanActive )
    {
        logger_.syslog( "Ground Fault scan is active; will mark data as invalid.", Syslog::INFO );
        badConductivity_ = true;
        badTemperature_ = true;
        badSalinity_ = true;
    }

}

void CTD_NeilBrown::writeData()
{
    conductivityWriter_->setInvalid( badConductivity_ );
    temperatureWriter_->setInvalid( badTemperature_ );
    pressureWriter_->setInvalid( badPressure_ );
    depthWriter_->setInvalid( badPressure_ );
    salinityWriter_->setInvalid( badSalinity_ );
    densityWriter_->setInvalid( badTemperature_ || badPressure_ || badSalinity_ );
    soundSpeedWriter_->setInvalid( densityWriter_->isInvalid() );

    conductivityWriter_->write( Units::MILLIMHO_PER_CENTIMETER, conductivity_, startTime_ );
    temperatureWriter_->write( Units::CELSIUS, temperature_, startTime_ );
    pressureWriter_->write( Units::DECIBAR, pressureDB_, startTime_ );
    depthWriter_->write( Units::METER, depth_, startTime_ );
    salinityWriter_->write( Units::PRACTICAL_SALINITY_UNIT, salinity_, startTime_ );
    densityWriter_->write( Units::KILOGRAM_PER_CUBIC_METER, density_, startTime_ );
    soundSpeedWriter_->write( Units::METER_PER_SECOND, soundSpeed_, startTime_ );
}

void CTD_NeilBrown::updateTemporalBin()
{
    if( badConductivity_ || badTemperature_ || badSalinity_ )
    {
        logger_.syslog( "some bad data, not updating bins", Syslog::INFO );
    }
    else
    {
        temperatureBin_.add( temperature_, startTime_ );
        salinityBin_.add( salinity_, startTime_ );
        Timestamp binTime;
        if( temperatureBin_.window_refreshed() )
        {
            binTime = temperatureBin_.median_time();
            binMedianTemperatureWriter_->write( Units::CELSIUS, temperatureBin_.median(), binTime );
            binMeanTemperatureWriter_->write( Units::CELSIUS, temperatureBin_.mean(), binTime );
            binStandardDeviationTemperatureWriter_->write( Units::CELSIUS, std::sqrt( temperatureBin_.variance() ), binTime );

        }
        if( salinityBin_.window_refreshed() )
        {
            binTime = salinityBin_.median_time();
            binMedianSalinityWriter_->write( Units::PRACTICAL_SALINITY_UNIT, salinityBin_.median(), binTime );
            binMeanSalinityWriter_->write( Units::PRACTICAL_SALINITY_UNIT, salinityBin_.mean(), binTime );
            binStandardDeviationSalinityWriter_->write( Units::PRACTICAL_SALINITY_UNIT, std::sqrt( salinityBin_.variance() ), binTime );
        }
    }
}

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