/** \file
 *
 *  Contains the Waterlinked class implementation.
 *
 *  Copyright (c) 2023, MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 *
 */

/*

The DVL needs to be configured prior to use. PD6 serial mode is expected. Mounting is assumed to be 180 degrees (facing backwards). Sign flips applied upon ingest below.

*/

#include "Waterlinked.h"
#include "WaterlinkedIF.h"
#include "Power24vConverterIF.h"

#include <cstdlib>

#include "data/BlobWriter.h"
#include "data/ConfigReader.h"
#include "data/SimSlate.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"
#include "units/Units.h"
#include "utils/AuvMath.h"
#include "controlModule/VerticalControlIF.h"

#define DVL_M_ACCURACY (0.001)

Waterlinked::Waterlinked( const Module* module )
    : SyncSensorComponent( WaterlinkedIF::NAME, module ),
      loadControl_( WaterlinkedIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      uart_( WaterlinkedIF::UART, WaterlinkedIF::BAUD, 0.010, logger_, UART_BUFSIZE ),
      hasPD6_( false ),
      dvlTimeout_( 10.0 ),
      dvlFailTimeout_( 180 ),
      powerOnTimeout_( 28 ), // Boot time is ~25 sec with a static ip and ~100 seconds with DHCP enabled.
      dvlTimeoutArmed_( false ),
      veloInst_( 0 ),
      veloInstX_( 0 ),
      veloInstY_( 0 ),
      veloInstZ_( 0 ),
      veloError_( -32000 ),
      veloInstFlag_( 0 ),
      altEstimate_( -1 ),
      surfaceThreshold_( 1.0 ),
      debug_( false )
{

    surfaceThresholdCfgReader_ = newConfigReader( VerticalControlIF::SURFACE_THRESHOLD_CFG );
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );
    power24vConverterDataReader_ = newDataReader( Power24vConverterIF::POWER_24V_CONVERTER );

    altitudeWriter_ = newUniversalWriter( UniversalURI::HEIGHT_ABOVE_SEA_FLOOR, Units::METER, DVL_M_ACCURACY );

    velocityWrtGroundWriter_ = newUniversalBlobWriter( UniversalURI::PLATFORM_VELOCITY_WRT_GROUND, Units::METER_PER_SECOND, DVL_M_ACCURACY );
    xVelWrtGroundWriter_ = newUniversalWriter( UniversalURI::PLATFORM_X_VELOCITY_WRT_GROUND, Units::METER_PER_SECOND, DVL_M_ACCURACY );
    yVelWrtGroundWriter_ = newUniversalWriter( UniversalURI::PLATFORM_Y_VELOCITY_WRT_GROUND, Units::METER_PER_SECOND, DVL_M_ACCURACY );
    zVelWrtGroundWriter_ = newUniversalWriter( UniversalURI::PLATFORM_Z_VELOCITY_WRT_GROUND, Units::METER_PER_SECOND, DVL_M_ACCURACY );
    veloInstFlagWriter_  = newDataWriter( WaterlinkedIF::BOTTOM_VELOCITY_FLAG_READING );

    this->setAllowableFailures( 3 );
    this->setRetryTimeout( 180 );
    // This configures the advanced run modes.
    setRunState( START );
}

Waterlinked::~Waterlinked()
{
}

void Waterlinked::run()
{
}


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

    // Start the clock for PO timeout
    startTime_ = Timestamp::Now();

    // And un-arm the data timeout
    dvlTimeoutArmed_ = false;

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

    // Request 24V power
    power24vConverterDataReader_->requestData( true );

    if( simulateHardware() )
    {
        return STARTING;
    }

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

    // Open the uart
    deviceResponse_[0] = '\0';
    uart_.open();
    uart_.enableUART();
    if( uart_.hasError() )
    {
        logger_.syslog( "Error opening uart: ", uart_.errorString(), Syslog::ERROR );
        this->setFailure( FailureMode::COMMUNICATIONS );
        return STOP;
    }
    uart_.flush();

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

    return STARTING;
}


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

    if( startTime_.elapsed() < powerOnTimeout_ )
    {
        if( uart_.canReadUntil( "wrw,DVL" ) ) // Check for the boot banner
        {
            if( debug_ ) logger_.syslog( "Initialized", Syslog::INFO );
            // Throw away that line
            uart_.flushCRLF().readLines( deviceResponse_, sizeof( deviceResponse_ ), 1 );
            startTime_ = Timestamp::Now(); // Reset the clocks
            bottomLockTime_ = Timestamp::Now();
            return RUNNABLE;
        }
        return STARTING; // give it some more time to power up
    }
    else
    {
        logger_.syslog( Str( "Could not initialize." ), Syslog::FAULT );
        this->setFailure( FailureMode::COMMUNICATIONS );
        // We're ready to go. Reset the clock and head for runnable.
        return STOP;
    }
}


Component::RunState Waterlinked::pause()
{
    if( debug_ ) logger_.syslog( "Pause", Syslog::INFO );
    return PAUSED;
}

// Not using pause/paused
Component::RunState Waterlinked::paused()
{
    if( debug_ ) logger_.syslog( "Paused", Syslog::INFO );
    return STOP;
}


Component::RunState Waterlinked::resume()
{
    if( debug_ ) logger_.syslog( "Resume", Syslog::INFO );
    return STOP;
}


// Stop
Component::RunState Waterlinked::resuming()
{
    if( debug_ ) logger_.syslog( "Resuming", Syslog::INFO );
    return STOP;
}

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

    if( !simulateHardware() ) logVoltageAndCurrent();

    hasPD6_ = simPD6(); // Always attempt to read simulated values, for vehicle in the loop sims

    if( !simulateHardware() )
    {
        hasPD6_ = readPD6();
    }

    if( hasPD6_ )
    {
        processPD6();
        writeData();
        this->resetFailCount();
    }

    // Run checks on data timeouts, etc.
    if( !checkTimeouts() )
    {
        return STOP;
    }

    // Stop if we don't want data or if we might be in air
    // Note that this call is placed after the initial parsing of a potentially good DVL message
    // so we don't have to wait for a dive to confirm okayToPowerOn
    if( !isDataRequested() || !okayToPowerOn() )
    {
        return STOP;
    }

    return RUNNABLE;
}

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

    // Set the writers invalid
    setGroundVelWritersInvalid( true );
    altitudeWriter_->setInvalid( true );
    uninitialize(); // First power down then query for faults next cycle
    return STOPPING;
}

Component::RunState Waterlinked::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() )
        {
            // Put anything that isn't an actual fault first
            if( loadControl_.errorString().find( "Software" ) )
            {
                if( debug_ )logger_.syslog( "LCB error:" + loadControl_.errorString(), Syslog::ERROR );
            }
            // And things that set failures second
            else
            {
                logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
                this->setFailure( FailureMode::HARDWARE );
            }
        }

    }
    return STOPPED;
}


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

    if( isDataRequested() && okayToPowerOn() )
    {
        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 Waterlinked::uninitialize()
{
    if( !simulateHardware() )
    {
        uart_.close();
        uart_.disableUART();
    }

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

    // 24V power is no longer needed
    power24vConverterDataReader_->requestData( false );
}


// Log voltage and current and check for any faults
void Waterlinked::logVoltageAndCurrent() // TODO: Elevate to superclass
{
    loadControl_.requestVoltageAndCurrent();
    if( loadControl_.hasError() )
    {
        // Put anything that isn't an actual fault first
        if( loadControl_.errorString().find( "Software" ) )
        {
            if( debug_ )logger_.syslog( "LCB error:" + loadControl_.errorString(), Syslog::ERROR );
        }
        // And things that set failures second
        else
        {
            logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            stop();
        }
    }
}


ConfigURI Waterlinked::getConfigURI( ConfigOption configOption ) const
{
    switch( configOption )
    {
    case CONFIG_POWER:
        return WaterlinkedIF::POWER;
    case CONFIG_SIMULATE_HARDWARE:
        return WaterlinkedIF::SIMULATE_HARDWARE;
    default:
        return ConfigURI::NO_CONFIG_URI;
    }
}

bool Waterlinked::readConfig()
{
    bool ok( false );
    ok = surfaceThresholdCfgReader_->read( Units::METER, surfaceThreshold_ );
    return ok;
}


bool Waterlinked::checkTimeouts()
{
    // Is non-simulated data available in time since the last ping?
    if( !simulateHardware() &&  startTime_.elapsed() > dvlTimeout_ )
    {
        logger_.syslog( Str( "No DVL communication! Re-initializing" ), Syslog::ERROR );
        this->setFailure( FailureMode::COMMUNICATIONS );
        return false;
    }
    // Next, check overall valid data timeout
    if( !dvlTimeoutArmed_ )
    {
        dvlFailTime_ = Timestamp::Now();
        dvlTimeoutArmed_ = true;
    }

    if( dvlFailTime_.elapsed() > dvlFailTimeout_ )
    {
        logger_.syslog( "DVL failed to acquire valid data within timeout.", Syslog::FAULT );
        this->setFailure( FailureMode::DATA );
        dvlTimeoutArmed_ = false;
        return false;
    }
    return true;
}


bool Waterlinked::simPD6()
{
    bool goodSim = false;

    SimDvlStruct dvlSimData;

    // Get the latests simulated DVL data
    SimSlate::Read( dvlSimData );

    // WRT Ground including altitude
    if( dvlSimData.valid )
    {
        // Convert values from meters/s to mm/s
        veloInstX_ = dvlSimData.velocityWrtBottom.getU() * 1000.0;
        veloInstY_ = dvlSimData.velocityWrtBottom.getV() * 1000.0;
        veloInstZ_ = dvlSimData.velocityWrtBottom.getW() * 1000.0;
        altEstimate_ = dvlSimData.bottomRange;

        // We have valid simulated data
        veloInstFlag_ = !dvlSimData.velocityWrtBottom.isNan() ? 1 : 0;
        setGroundVelWritersInvalid( dvlSimData.velocityWrtBottom.isNan() );

        startTime_ = dvlSimData.timestamp;  // roughly set the time for the incoming data here
        dvlTimeoutArmed_ = false;
        goodSim = true;
    }
    else
    {
        goodSim = false;
    }

    return goodSim;
}


// Used when usePD6 config variable is set to true
bool Waterlinked::readPD6()
{
    // See if an entire message has made it in
    // If so, we want to parse the lines: :TS, :BI, and  :BD
    if( uart_.canReadUntil( ":SA" ) && uart_.canReadUntil( ":BD" ) )
    {
        if( debug_ ) logger_.syslog( "Message in queue", Syslog::INFO );
        // Parse the response
        if( !parsePD6() )
        {
            if( debug_ ) logger_.syslog( "Failed to parse:", deviceResponse_, Syslog::ERROR );
            setGroundVelWritersInvalid( true );
            uart_.flush();
            return false;
        }
        else // Parsing was sucessfull
        {
            setGroundVelWritersInvalid( false );
            startTime_ = Timestamp::Now();  // roughly set the time for the incoming data here
            return true;
        }
    }
    else // There isn't a PD6 string in the UART yet.
    {
        return false;
    }
}


// Parse the actual PD6 message. Only TS, BI, and BD are populated.
bool Waterlinked::parsePD6()
{
    char *ptr;
    int scanRes = 0;
    // Line :SA is a throwaway
    uart_.flushCRLF().readLine( deviceResponse_, sizeof( deviceResponse_ ) );

    // Line :TS
    uart_.flushCRLF().readLine( deviceResponse_, sizeof( deviceResponse_ ) );
    if( uart_.hasError() )
    {
        deviceResponse_[0] = '\0';
        logger_.syslog( "DVL uart error: ", uart_.errorString(), Syslog::ERROR );
        this->setFailure( FailureMode::COMMUNICATIONS );
        setRunState( STOP );
        return false;
    }
    // Here we just want the BIT error if any is present
    if( ( ptr = strstr( deviceResponse_, ":TS" ) ) )
    {
        bitError_ = 1; // reinitialize value
        scanRes = sscanf( ptr, ":TS,%*d,%*f,%*f,%*f,%*f,%d", &bitError_ );
        if( debug_ ) logger_.syslog( "BIT Error is:", bitError_, Syslog::INFO );
        if( scanRes != 1 )
        {
            logger_.syslog( "only read " + Str( scanRes ) + " of 1 data item for BIT error", Syslog::ERROR );
            return false;
        }
        if( bitError_ != 0 )
        {
            logger_.syslog( "DVL BIT error. See manual. Result code: ", bitError_, Syslog::ERROR );
        }
    }
    else
    {
        return false; // Expecting to see :TS and didn't
    }

    // Line :WI
    uart_.flushCRLF().readLines( deviceResponse_, sizeof( deviceResponse_ ), 1 );

    // Line :WS
    // This sensor doesn't have water mass data
    uart_.flushCRLF().readLines( deviceResponse_, sizeof( deviceResponse_ ), 1 );

    // Line :WE
    uart_.flushCRLF().readLines( deviceResponse_, sizeof( deviceResponse_ ), 1 );

    // Line :WD
    uart_.flushCRLF().readLines( deviceResponse_, sizeof( deviceResponse_ ), 1 );

    // Line :BI
    uart_.flushCRLF().readLine( deviceResponse_, sizeof( deviceResponse_ ) );
    if( uart_.hasError() )
    {
        deviceResponse_[0] = '\0';
        logger_.syslog( "DVL uart error: ", uart_.errorString(), Syslog::ERROR );
        this->setFailure( FailureMode::COMMUNICATIONS );
        setRunState( STOP );
        return false;
    }
    // Here we want the bottom track data
    if( ( ptr = strstr( deviceResponse_, ":BI" ) ) )
    {
        veloInstX_ = veloInstY_ = veloInstZ_ = 0; // Initialize for now
        char flag = 'F';

        scanRes = sscanf( ptr, ":BI,%lf,%lf,%lf,%lf,%c", &veloInstX_, &veloInstY_, &veloInstZ_, &veloError_, &flag );
        if( flag == 'A' )
        {
            veloInstFlag_ = 1;
        }
        else
        {
            veloInstFlag_ = 0;
        }

        // Standard mounting is 180 (facing backwards) so sign flip x and y here
        veloInstX_ *= -1.0;
        veloInstY_ *= -1.0;

        if( debug_ ) logger_.syslog( "Instrument Velocity X:", veloInstX_, Syslog::INFO );
        if( debug_ ) logger_.syslog( "Instrument Velocity Y:", veloInstY_, Syslog::INFO );
        if( debug_ ) logger_.syslog( "Instrument Velocity Z:", veloInstZ_, Syslog::INFO );
        if( debug_ ) logger_.syslog( "Velocity Error:", veloError_, Syslog::INFO );
        if( debug_ ) logger_.syslog( "Flag:" + Str( flag ), Syslog::INFO );
        if( scanRes != 5 )
        {
            logger_.syslog( "only read " + Str( scanRes ) + " of 5 data items", Syslog::ERROR );
            return false;
        }
    }
    else
    {
        return false; // Expecting to see :BS and didn't
    }

    // Line :BS
    uart_.flushCRLF().readLines( deviceResponse_, sizeof( deviceResponse_ ), 1 );

    // Line :BE is throwaway
    uart_.flushCRLF().readLines( deviceResponse_, sizeof( deviceResponse_ ), 1 );

    // Line :BD
    uart_.flushCRLF().readLine( deviceResponse_, sizeof( deviceResponse_ ) );
    if( uart_.hasError() )
    {
        deviceResponse_[0] = '\0';
        logger_.syslog( "DVL uart error: ", uart_.errorString(), Syslog::ERROR );
        this->setFailure( FailureMode::COMMUNICATIONS );
        setRunState( STOP );
        return false;
    }
    // Here we want the altitude data
    if( ( ptr = strstr( deviceResponse_, ":BD" ) ) )
    {
        altEstimate_ = -1;
        scanRes = sscanf( ptr, ":BD,%*f,%*f,%*f,%lf,%*f", &altEstimate_ );

        if( debug_ ) logger_.syslog( "Altitude:", altEstimate_, Syslog::INFO );
        if( scanRes != 1 )
        {
            logger_.syslog( "only read " + Str( scanRes ) + " of 1 data item for altitude", Syslog::ERROR );
            return false;
        }
    }
    else
    {
        return false; // Expecting to see :BD and didn't
    }

    // The data is valid. Reset the clock next time through
    dvlTimeoutArmed_ = false;

    uart_.flush();
    return true;
}


void Waterlinked::processPD6()
{
    // populate the vectors for writers
    veloInst_.setU( veloInstX_ );
    veloInst_.setV( veloInstY_ );
    veloInst_.setW( veloInstZ_ );

    // finally, perform validity checks for quality control
    if( veloInstFlag_ == 1 ) // Ground speed is valid
    {
        setGroundVelWritersInvalid( false );
    }
    else // ground speed is not valid
    {
        setGroundVelWritersInvalid( true );
    }

    if( altEstimate_ > 0 ) // Altitude has to be greater than 0 (at least by a little) to be valid
    {
        altitudeWriter_->setInvalid( false );
        bottomLockTime_ = Timestamp::Now(); // Good bottom lock if we have altitude so we'll reset the timer.
    }
    else
    {
        altitudeWriter_->setInvalid( true );
    }
}


void Waterlinked::writeData()
{
    // Vehicle referenced bottom-track data
    velocityWrtGroundWriter_->setAccuracy( Units::METER_PER_SECOND, DVL_M_ACCURACY );
    velocityWrtGroundWriter_->write1DClass( Units::MILLIMETER_PER_SECOND, veloInst_, startTime_ );

    xVelWrtGroundWriter_->write( Units::MILLIMETER_PER_SECOND, veloInstX_, startTime_ );
    yVelWrtGroundWriter_->write( Units::MILLIMETER_PER_SECOND, veloInstY_, startTime_ );
    zVelWrtGroundWriter_->write( Units::MILLIMETER_PER_SECOND, veloInstZ_, startTime_ );

    veloInstFlagWriter_->write( Units::COUNT, veloInstFlag_, startTime_ );
    altitudeWriter_->write( Units::METER, altEstimate_, startTime_ );
}

void Waterlinked::setGroundVelWritersInvalid( bool invalid )
{
    xVelWrtGroundWriter_->setInvalid( invalid );
    yVelWrtGroundWriter_->setInvalid( invalid );
    zVelWrtGroundWriter_->setInvalid( invalid );
    velocityWrtGroundWriter_->setInvalid( invalid );
}


// The DVL will get overheated if left powered out of the water.
bool Waterlinked::okayToPowerOn()
{
    float depth( 0.0 );

    // If we're underwater no worries.
    if( depthReader_->isActive() && depthReader_->read( Units::METER, depth ) && ( ( depth > surfaceThreshold_ ) ) )
    {
        return true;
    }
    // And if we're getting a good altitude we can also remain powered. We'll wait for a little to be sure.
    else if( altEstimate_ > 0 || bottomLockTime_.elapsed() < dvlTimeout_ )
    {
        return true;
    }
    return false;
}


bool Waterlinked::isDataRequested()
{
    return altitudeWriter_->isDataRequested()
           || velocityWrtGroundWriter_->isDataRequested()
           || xVelWrtGroundWriter_->isDataRequested()
           || yVelWrtGroundWriter_->isDataRequested()
           || zVelWrtGroundWriter_->isDataRequested();
}