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

#include "CTD_SeabirdLCM.h"
#include "CTD_SeabirdLCMIF.h"

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


#include <stdio.h>
#include <lcm/lcm-cpp.hpp>
#include <algorithm>    // std::min
#include "lrauv-lcmtypes/marine-sensors/marine_sensors/bottom_track.hpp"

#define CTD_ACCURACY_COND (0.001)    // Accuracy of 0.0001 S/m in mmhos/cm. Per spec
#define CTD_ACCURACY_TEMP (0.0005)   // per spec
#define CTD_ACCURACY_DENSITY (0.01)  // per calc
#define CTD_ACCURACY_SALINITY (0.01) // per calc
#define CTD_ACCURACY_DEPTH (0.04)
#define CTD_ACCURACY_SPEED (0.15)    // per calc
#define CTD_ACCURACY_PRESSURE CTD_ACCURACY_DEPTH
#define CTD_BIN_SIZE (5) // XXX hard-coded bin size to start

CTD_SeabirdLCM::CTD_SeabirdLCM( const Module* module )
    : AsyncComponent( CTD_SeabirdLCMIF::NAME, module ),
      debug_( false ),
      newCTDData_( false ),
      depthNeeded_( false ),
      loadControl_( CTD_SeabirdLCMIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      powerOnTimeout_( 5.0 ),
      pausePeriod_( 0.4 ),
      dataTimestamp_( Timestamp::NOT_SET_TIME ),
      sampleTime_( 35.0 ),
      conductivity_( 0 ),
      temperature_( 0 ),
      pressureDB_( 0 ),
      depth_( 0 ),
      salinity_( 0 ),
      density_( 0 ),
      oxygenFreq_( 0 ),
      soundSpeed_( 0 ),
      badConductivity_( true ),
      badTemperature_( true ),
      badPressure_( true ),
      badSalinity_( true ),
      badOxygenFreq_( true ),
      maxPressBound_( 499 ),
      minPressBound_( -9 ),
      maxSalinityBound_( 38 ),
      minSalinityBound_( 28 ),
      temporal_bin_count_( 0 ),
      temporal_bin_conductivity_( CTD_BIN_SIZE, nanf( "" ) ),
      temporal_bin_temperature_( CTD_BIN_SIZE, nanf( "" ) ),
      temporal_bin_salinity_( CTD_BIN_SIZE, nanf( "" ) ),
      binTime_( Timestamp::NOT_SET_TIME )
{
    // Universal outputs
    // initialize universal writers with specified accuracy
    conductivityWriter_ = newUniversalWriter( UniversalURI::SEA_WATER_ELECTRICAL_CONDUCTIVITY, Units::MILLIMHO_PER_CENTIMETER, CTD_ACCURACY_COND );
    temperatureWriter_ = newUniversalWriter( UniversalURI::SEA_WATER_TEMPERATURE, Units::CELSIUS, CTD_ACCURACY_TEMP );
    salinityWriter_ = newUniversalWriter( UniversalURI::SEA_WATER_SALINITY, Units::PRACTICAL_SALINITY_UNIT, CTD_ACCURACY_SALINITY );
    densityWriter_ = newUniversalWriter( UniversalURI::SEA_WATER_DENSITY, Units::KILOGRAM_PER_CUBIC_METER, CTD_ACCURACY_DENSITY );
    soundSpeedWriter_ = newUniversalWriter( UniversalURI::SPEED_OF_SOUND_IN_SEA_WATER, Units::METER_PER_SECOND, CTD_ACCURACY_SPEED );
    depthWriter_ = newUniversalWriter( UniversalURI::DEPTH, Units::METER, CTD_ACCURACY_DEPTH );
    pressureWriter_ = newUniversalWriter( UniversalURI::SEA_WATER_PRESSURE, Units::DECIBAR, CTD_ACCURACY_PRESSURE );

    binMedianConductivityWriter_ = newDataWriter( CTD_SeabirdLCMIF::BIN_MEDIAN_SEA_WATER_ELECTRICAL_CONDUCTIVITY );
    binMedianTemperatureWriter_ = newDataWriter( CTD_SeabirdLCMIF::BIN_MEDIAN_SEA_WATER_TEMPERATURE );
    binMedianSalinityWriter_ = newDataWriter( CTD_SeabirdLCMIF::BIN_MEDIAN_SEA_WATER_SALINITY );

    oxygenFreqWriter_ = newDataWriter( CTD_SeabirdLCMIF::OXYGEN_FREQUENCY );

    maxPressBoundCfgReader_ = newConfigReader( CTD_SeabirdLCMIF::MAX_PRESS_BOUND );
    minPressBoundCfgReader_ = newConfigReader( CTD_SeabirdLCMIF::MIN_PRESS_BOUND );
    maxSalinityBoundCfgReader_ = newConfigReader( CTD_SeabirdLCMIF::MAX_SALINITY_BOUND );
    minSalinityBoundCfgReader_ = newConfigReader( CTD_SeabirdLCMIF::MIN_SALINITY_BOUND );

    depthReader_ = newUniversalReader( UniversalURI::DEPTH );
    latitudeReader_ = newUniversalReader( UniversalURI::LATITUDE );
    gfScanActiveReader_ = newDataReader( CBITIF::GF_ACTIVE_STATE );

    // Configuration inputs
    lcmApplicationCfgReader_ = newConfigReader( CTD_SeabirdLCMIF::LCM_APP_NAME_CFG );

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

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


CTD_SeabirdLCM::~CTD_SeabirdLCM()
{}


Component::RunState CTD_SeabirdLCM::start()
{
    if( debug_ ) logger_.syslog( "start", Syslog::INFO );
    logger_.syslog( "Initializing", Syslog::INFO );
    startTime_ = Timestamp::Now(); // Set the time

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

    if( debug_ ) logger_.syslog( "Checking LCM", Syslog::INFO );
    if( !LcmInstance::IsValid() )
    {
        logger_.syslog( "LCM not connected.", Syslog::FAULT );
        this->setFailure( FailureMode::SOFTWARE );
        return START;
    }
    else if( debug_ )
    {
        logger_.syslog( "LCM OK", Syslog::INFO );
    }

    dataTimestamp_ = 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 START;
        }
    }
    pausePeriod_.sleepFor();
    return STARTING;
}

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

    if( !simulateHardware() && ( startTime_.elapsed() < powerOnTimeout_ ) )
    {
        pausePeriod_.sleepFor();
        return STARTING; // give it some more time to power up
    }

    // Stop any previous instance of the Seabird interface. Intentionally ignoring return value.
    system( "killall gpctd" );
    if( debug_ ) logger_.syslog( "Stopping potential previous instance(s) of CTD_SeabirdLCM LCM interface", Syslog::INFO );

    // Start the stand alone LCM interface application and pipe any output to null
    Str appCmd = lcmApplication_.asString() + " " + uartName_.asString() + " -b " + Str( baudInt_ ) + " >& /dev/null &";
    if( system( appCmd.cStr() ) == 0 )
    {
        if( debug_ ) logger_.syslog( "Started Seabird LCM interface with command:" + appCmd, Syslog::INFO );

        char *channelName = LcmUtils::channelName( marine_sensors::seabird_gpctd_t::getTypeName(), SEABIRD_GPCTD, NULL );
        if( channelName )
        {
            LcmInstance::GetInstance()->subscribe( channelName, &CTD_SeabirdLCM::handleCTDMessage, this );
            logger_.syslog( "LCM subscribed to channel:" + Str( channelName ), Syslog::INFO );
        }
        else
        {
            logger_.syslog( "LcmUtils::channelName returned NULL\n", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }
    }
    else
    {
        logger_.syslog( "Failed to start Seabird LCM interface. Issued command:" + appCmd, Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
        return STOP;
    }

    // Check to see if we need CTD to provide vehicle depth
    isDepthNeeded();

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

    pausePeriod_.sleepFor();
    return RUNNABLE;
}


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


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


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


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


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

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

    // If not simulating, check the load control board.
    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;
            }
        }
    }
    else
    {
        if( getSimulatedData() ) // use simulated data
        {
            newCTDData_ = true;
        }
    }

    // grab messages (if any) here.
    if( LcmInstance::IsValid() ) LcmInstance::GetInstance()->handleTimeout( pausePeriod_.asMillis() );

    if( newCTDData_ )
    {
        // Got data; reset flag
        newCTDData_ = false;

        // Assume valid data to start
        setWritersInvalid( false );
        // Run QC and preprocess.
        preprocessData();
        writeData();
        updateTemporalBin();
        this->resetFailCount();
    }

    // As long as data is flowing we're okay. If not, take action.
    if( dataTimestamp_.elapsed() > sampleTime_ )
    {
        setWritersInvalid( true );
        logger_.syslog( "Did not receive valid device response within the specified allowable sample time.", Syslog::FAULT );
        this->setFailure( FailureMode::COMMUNICATIONS ); // You could argue that this is a data failure.
        return STOP;
    }
    // Do nothing. You are still within the allowable time for a valid response.
    pausePeriod_.sleepFor();
    return RUNNABLE;
}


Component::RunState CTD_SeabirdLCM::stop()
{
    if( debug_ ) logger_.syslog( "stop", Syslog::INFO );
    uninitialize();
    pausePeriod_.sleepFor();
    return STOPPING;
}


Component::RunState CTD_SeabirdLCM::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 CTD_SeabirdLCM::stopped()
{
    if( debug_ ) logger_.syslog( "Stopped", Syslog::INFO );
    if( isDataRequested() )
    {
        if( debug_ )logger_.syslog( "Data requested. STOPPED ==> START", Syslog::INFO );
        return START;
    }
    pausePeriod_.sleepFor();
    return STOPPED;
}


bool CTD_SeabirdLCM::readConfig( void )
{
    if( debug_ ) logger_.syslog( "in readConfig", Syslog::INFO );
    bool ok = true;
    ok &= lcmApplicationCfgReader_->read( lcmApplication_ );
    ok &= Slate::ReadOnce( CTD_SeabirdLCMIF::UART, uartName_, logger_ );
    ok &= Slate::ReadOnce( CTD_SeabirdLCMIF::BAUD, Units::BIT_PER_SECOND, baudInt_, logger_ );
    ok &= maxPressBoundCfgReader_->read( Units::DECIBAR, maxPressBound_ );
    ok &= minPressBoundCfgReader_->read( Units::DECIBAR, minPressBound_ );
    ok &= maxSalinityBoundCfgReader_->read( Units::PRACTICAL_SALINITY_UNIT, maxSalinityBound_ );
    ok &= minSalinityBoundCfgReader_->read( Units::PRACTICAL_SALINITY_UNIT, minSalinityBound_ );
    return ok;
}


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

    // Stop any previous instance of the Seabird interface. Intentionally ignoring return value.
    system( "killall gpctd" );
    logger_.syslog( "Stopping potential previous instance(s) of CTD_SeabirdLCM LCM interface", Syslog::INFO );

    if( !simulateHardware() )
    {
        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 CTD_SeabirdLCM::getConfigURI( ConfigOption configOption ) const
{
    return configOption == CONFIG_SIMULATE_HARDWARE ? CTD_SeabirdLCMIF::SIMULATE_HARDWARE : ConfigURI::NO_CONFIG_URI;
}


bool CTD_SeabirdLCM::isDataRequested()
{
    return densityWriter_->isDataRequested()
           || soundSpeedWriter_->isDataRequested()
           || salinityWriter_->isDataRequested()
           || temperatureWriter_->isDataRequested()
           || pressureWriter_->isDataRequested()
           || conductivityWriter_->isDataRequested()
           || oxygenFreqWriter_->isDataRequested()
           || depthNeeded_;
}


bool CTD_SeabirdLCM::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_ ) )
    {

        // get a latitude (needed for the depth conversion)
        float latitude( nanf( "" ) );
        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
        }

        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::Density( salinity_, temperature_, pressureDB_ );
        conductivity_ = 4.0; // hack to make sure there is something to operate on
        return true;
    }
    else
    {
        return false;
    }
}


void CTD_SeabirdLCM::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 // Good pressure so let's calculate depth
    {
        bool haveLocation = latitudeReader_->isActive()
                            && latitudeReader_->read( Units::RADIAN, latitude );

        // Just to double check that latitude isn't nan
        if( haveLocation && isnan( latitude ) )
        {
            haveLocation = false;
            logger_.syslog( "Latitude is nan. Will use worksite latitude.", Syslog::ERROR );
        }

        if( haveLocation )
        {
            depth_ = AuvMath::OceanDepth( pressureDB_ * 10000.0, latitude );
        }
        else // No location so use defaults
        {
            float initLat;
            Slate::ReadOnce( "Config/workSite", "initLat", Units::RADIAN, initLat, logger_ );
            depth_ = AuvMath::OceanDepth( pressureDB_ * 10000.0, initLat );
        }
        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_SeabirdLCM::updateTemporalBin()
{
    if( badConductivity_ || badTemperature_ || badSalinity_ )
    {
        logger_.syslog( "some bad data, not updating bins", Syslog::INFO );
    }
    else if( temporal_bin_count_ < temporal_bin_conductivity_.size() )
    {
        temporal_bin_conductivity_[temporal_bin_count_] = conductivity_; // add new sample to the bin
        temporal_bin_temperature_[temporal_bin_count_] = temperature_; // add new sample to the bin
        temporal_bin_salinity_[temporal_bin_count_] = salinity_; // add new sample to the bin
        temporal_bin_count_ += 1; // increment the counter
        if( temporal_bin_count_ == temporal_bin_conductivity_.size() / 2 ) binTime_ = dataTimestamp_ ;
        else if( temporal_bin_count_ == temporal_bin_conductivity_.size() )
        {
            calculateAndReportTemporalBinStatistics();
        }
    }
    else
    {
        logger_.syslog( "bin count larger than size of bin:", temporal_bin_conductivity_.size(), Syslog::ERROR );
    }
}


void CTD_SeabirdLCM::calculateAndReportTemporalBinStatistics()
{
    float median_conductivity( 0 );
    float median_temperature( 0 );
    float median_salinity( 0 );

    std::stable_sort( temporal_bin_conductivity_.begin(), temporal_bin_conductivity_.end() ); // consider using regular sort
    median_conductivity = temporal_bin_conductivity_[( temporal_bin_conductivity_.size() / 2 ) ];  // take the middle element

    std::stable_sort( temporal_bin_temperature_.begin(), temporal_bin_temperature_.end() ); // consider using regular sort
    median_temperature = temporal_bin_temperature_[( temporal_bin_temperature_.size() / 2 ) ];  // take the middle element

    std::stable_sort( temporal_bin_salinity_.begin(), temporal_bin_salinity_.end() ); // consider using regular sort
    median_salinity = temporal_bin_salinity_[( temporal_bin_salinity_.size() / 2 ) ];  // take the middle element

    binMedianConductivityWriter_->write( Units::MILLIMHO_PER_CENTIMETER, median_conductivity, binTime_ );
    binMedianTemperatureWriter_->write( Units::CELSIUS, median_temperature, binTime_ );
    binMedianSalinityWriter_->write( Units::PRACTICAL_SALINITY_UNIT, median_salinity, binTime_ );
    temporal_bin_count_ = 0;
}


void CTD_SeabirdLCM::writeData( void )
{

// Set validity of specific writers following QC.
    conductivityWriter_->setInvalid( badConductivity_ );
    binMedianConductivityWriter_->setInvalid( badConductivity_ );
    temperatureWriter_->setInvalid( badTemperature_ );
    binMedianTemperatureWriter_->setInvalid( badTemperature_ );
    pressureWriter_->setInvalid( badPressure_ );
    depthWriter_->setInvalid( badPressure_ );
    salinityWriter_->setInvalid( badSalinity_ );
    binMedianSalinityWriter_->setInvalid( badSalinity_ );
    oxygenFreqWriter_->setInvalid( badOxygenFreq_ );
    densityWriter_->setInvalid( badTemperature_ || badPressure_ || badSalinity_ );
    soundSpeedWriter_->setInvalid( badTemperature_ || badPressure_ || badSalinity_ );

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


void CTD_SeabirdLCM::handleCTDMessage( const lcm::ReceiveBuffer* rbuf,
                                       const std::string& chan,
                                       const marine_sensors::seabird_gpctd_t* msg )
{
    conductivity_ = msg->sea_water_electrical_conductivity;
    temperature_ = msg->sea_water_temperature;
    pressureDB_ = msg->sea_water_pressure;
    salinity_ = msg->sea_water_salinity;
    oxygenFreq_ = msg->dissolved_oxygen_frequency;
    dataTimestamp_ = msg->epoch_usec / 1e6;
    newCTDData_ = true;
}


void CTD_SeabirdLCM::setWritersInvalid( bool validity )
{
    conductivityWriter_->setInvalid( validity );
    temperatureWriter_->setInvalid( validity );
    salinityWriter_->setInvalid( validity );
    densityWriter_->setInvalid( validity );
    soundSpeedWriter_->setInvalid( validity );
    pressureWriter_->setInvalid( validity );
    binMedianConductivityWriter_->setInvalid( validity );
    binMedianTemperatureWriter_->setInvalid( validity );
    binMedianSalinityWriter_->setInvalid( validity );
    oxygenFreqWriter_->setInvalid( validity );
}


void CTD_SeabirdLCM::isDepthNeeded( void )
{
    if( !depthReader_->isActive() && !depthReader_->wasTouchedSinceLastRun( this ) )
        depthNeeded_ = true;
}