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

/*

The DVL micro needs to be configured prior to use. This component currently expects bottom track
capability output in PD6 message format as shown below. Note that this does not match typical PD6 outputs
but the vendor refers to this version of firmware as PD6.

:SA,-00.41,+00.52,099.8
:TS,00000000000000,35.0,+19.6,0000.0,1489.0,000
:WI,-00032,-00188,+00289,+00000,A
:BI,-00032,-00188,+00289,+00000,I
:BD,+00000000.00,+00000000.00,+00000000.00,9999.99,000.00
:RD,+9999.99,+9999.99,+9999.99,+9999.99
:SA,-00.37,+00.52,099.8
:TS,00000000000000,35.0,+19.6,0000.0,1489.0,000
:WI,-02118,+00225,+00716,+00000,A
:BI,-02118,+00225,+00716,+00000,I
:BD,+00000000.00,+00000000.00,+00000000.00,9999.99,000.00
:RD,+9999.99,+9999.99,+9999.99,+9999.99


Additionally, the unit should be configured for power on continuous mode operation such that it begins pinging once power is applied.

*/

#include "DVL_micro.h"
#include "DVL_microIF.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"

#define DVL_M_ACCURACY (0.001)

DVL_micro::DVL_micro( const Module* module )
    : SyncSensorComponent( DVL_microIF::NAME, module ),
      hasPD6_( false ),
      dvlTimeout_( 10.0 ),
      dvlFailTimeout_( 180 ),
      powerOnTimeout_( 7 ),
      debug_( false ),
      dvlTimeoutArmed_( false ),
      uart_( DVL_microIF::UART, DVL_microIF::BAUD, 0.010, logger_, UART_BUFSIZE ),
      loadControl_( DVL_microIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      veloInst_( 0 ),
      veloInstX_( 0 ),
      veloInstY_( 0 ),
      veloInstZ_( 0 ),
      veloError_( -32000 ),
      veloInstFlag_( 0 ),
      altEstimate_( -1 ),
      beam1Range_( -1 ),
      beam2Range_( -1 ),
      beam3Range_( -1 ),
      beam4Range_( -1 ),
      beam1Good_( 0 ),
      beam2Good_( 0 ),
      beam3Good_( 0 ),
      beam4Good_( 0 ),
      rollOffset_( 0.0 ),
      pitchOffset_( 0.0 ),
      rollDeg_( 0 ),
      pitchDeg_( 0 ),
      compassHeadingDeg_( 0 ),
      magneticHeadingDeg_( 0 ),
      headingDeg_( 0 ),
      startup_( NQ_START )
{
    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( DVL_microIF::BOTTOM_VELOCITY_FLAG_READING );

    beam1RangeWriter_  = newDataWriter( DVL_microIF::BEAM1RANGE );
    beam2RangeWriter_  = newDataWriter( DVL_microIF::BEAM2RANGE );
    beam3RangeWriter_  = newDataWriter( DVL_microIF::BEAM3RANGE );
    beam4RangeWriter_  = newDataWriter( DVL_microIF::BEAM4RANGE );

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

DVL_micro::~DVL_micro()
{
}

void DVL_micro::run()
{
}


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

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

    // 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 DVL_micro::starting()
{
    if( debug_ ) logger_.syslog( "Starting", Syslog::INFO );
    if( simulateHardware() )
    {
        return RUNNABLE;
    }

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

    switch( startup_ )
    {
    case NQ_START:
        // Command continuous mode
        uart_ << "#&!LQNQ.COMD2525";
        startTime_ = Timestamp::Now();
        startup_ = VEL_ON;
        break;
    case VEL_ON:
        // Start to work in continuous mode if command was successful
        if( uart_.canReadUntil( "AUTO_VEL_ON" ) )
        {
            if( uart_.readLine( deviceResponse_, UART_BUFSIZE ).hasError() )
            {
                logger_.syslog( "Uart error: ", uart_.errorString(), Syslog::ERROR );
                this->setFailure( FailureMode::COMMUNICATIONS );
                return STOP;
            }
            else
            {
                // OK to run in the next cycle
                startTime_ = Timestamp::Now();
                return RUNNABLE;
            }
        }
    default:
        break;
    }

    if( startTime_.elapsed() > dvlTimeout_ )
    {
        logger_.syslog( "Failed to initialize within timeout.", Syslog::FAULT );
        this->setFailure( FailureMode::COMMUNICATIONS );
        return STOP;
    }

    return STARTING;
}


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

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


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


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

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

    if( !simulateHardware() ) logVoltageAndCurrent();

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

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

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

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

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

    return RUNNABLE;
}

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

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

Component::RunState DVL_micro::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 DVL_micro::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 DVL_micro::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 DVL_micro::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 DVL_micro::getConfigURI( ConfigOption configOption ) const
{
    switch( configOption )
    {
    case CONFIG_POWER:
        return DVL_microIF::POWER;
    case CONFIG_SIMULATE_HARDWARE:
        return DVL_microIF::SIMULATE_HARDWARE;
    default:
        return ConfigURI::NO_CONFIG_URI;
    }
}
void DVL_micro::readConfig()
{

}


bool DVL_micro::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 DVL_micro::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 (and flip sign so it
        // is the velocity of the ground relative to the instrument)
        veloInstX_ = dvlSimData.velocityWrtBottom.getU() * 1000.0;
        veloInstY_ = dvlSimData.velocityWrtBottom.getV() * 1000.0;
        veloInstZ_ = dvlSimData.velocityWrtBottom.getW() * 1000.0;

        beam1Range_ =
            !isnan( dvlSimData.beamRanges[0] ) ? dvlSimData.beamRanges[0] : -1;
        beam2Range_ =
            !isnan( dvlSimData.beamRanges[1] ) ? dvlSimData.beamRanges[1] : -1;
        beam3Range_ =
            !isnan( dvlSimData.beamRanges[2] ) ? dvlSimData.beamRanges[2] : -1;
        beam4Range_ =
            !isnan( dvlSimData.beamRanges[3] ) ? dvlSimData.beamRanges[3] : -1;
        altEstimate_ = dvlSimData.bottomRange;

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

        startTime_ = dvlSimData.timestamp;
        dvlTimeoutArmed_ = false;
        goodSim = true;
    }
    else
    {
        goodSim = false;
    }

    return goodSim;
}


bool DVL_micro::readPD6()
{
    // See if an entire message has made it in
    if( uart_.canReadUntil( ":SA" ) && uart_.canReadUntil( ":RD" ) )
    {
        if( debug_ ) logger_.syslog( "Message in queue", Syslog::INFO );
        // Parse the response
        if( !parsePD6() )
        {
            logger_.syslog( "Failed to parse:", deviceResponse_, Syslog::ERROR );
            setAcousticWritersInvalid( true );
            uart_.flush();
            return false;
        }
        else // Parsing was sucessfull
        {
            setAcousticWritersInvalid( 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;
    }
}


bool DVL_micro::parsePD6()
{
    char *ptr;
    int scanRes = 0;
    // Line :SA is a throwaway
    uart_.readLine( deviceResponse_, UART_BUFSIZE );

    // Line :TS
    uart_.readLine( deviceResponse_, UART_BUFSIZE );
    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. Device response is:" + Str( ptr ), Syslog::ERROR );
            return false;
        }
        if( bitError_ != 0 )
        {
            logger_.syslog( "DVL BIT error. See manual. Result code: " + Str( bitError_ ) + "\nDevice respsone is:" + Str( ptr ), Syslog::ERROR );
        }
    }
    else
    {
        return false; // Expecting to see :TS and didn't
    }

    // Line :WI is throwaway for now.
    uart_.readLines( deviceResponse_, UART_BUFSIZE, 1 );

    // Line :BI for instrument referenced data
    uart_.readLine( deviceResponse_, UART_BUFSIZE );
    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';
        // Tranverse then longitudinal so Y,X
        scanRes = sscanf( ptr, ":BI,%lf,%lf,%lf,%*f,%c", &veloInstY_, &veloInstX_, &veloInstZ_, &flag );
        if( flag == 'A' )
        {
            veloInstFlag_ = 1;
        }
        else
        {
            veloInstFlag_ = 0;
        }

        if( debug_ ) logger_.syslog( "Velocity X:", veloInstX_, Syslog::INFO );
        if( debug_ ) logger_.syslog( "Velocity Y:", veloInstY_, Syslog::INFO );
        if( debug_ ) logger_.syslog( "Velocity Z:", veloInstZ_, Syslog::INFO );
        if( debug_ ) logger_.syslog( "Flag:" + Str( flag ), Syslog::INFO );
        if( scanRes != 4 )
        {
            logger_.syslog( "only read " + Str( scanRes ) + " of 4 data items for bottom velocity. Device response is:" + Str( ptr ), Syslog::ERROR );
            return false;
        }
    }
    else
    {
        return false; // Expecting to see :BI and didn't
    }

    // Line :BD is throwaway
    uart_.readLines( deviceResponse_, UART_BUFSIZE, 1 );

    // Line :RD
    uart_.readLine( deviceResponse_, UART_BUFSIZE );
    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 range to bottom data
    if( ( ptr = strstr( deviceResponse_, ":RD" ) ) )
    {
        beam1Range_ = beam2Range_ = beam3Range_ = beam4Range_ = altEstimate_ = -1; // Initialize for now
        beam1Good_ = beam2Good_ = beam3Good_ = beam4Good_ = 0;

        scanRes = sscanf( ptr, ":RD,%lf,%lf,%lf,%lf", &beam1Range_, &beam2Range_, &beam3Range_, &beam4Range_ );

        if( debug_ ) logger_.syslog( "Beam 1 Range:", beam1Range_, Syslog::INFO );
        if( debug_ ) logger_.syslog( "Beam 2 Range:", beam2Range_, Syslog::INFO );
        if( debug_ ) logger_.syslog( "Beam 3 Range:", beam3Range_, Syslog::INFO );
        if( debug_ ) logger_.syslog( "Beam 4 Range:", beam4Range_, Syslog::INFO );

        if( scanRes != 4 )
        {
            logger_.syslog( "only read " + Str( scanRes ) + " of 4 data items for beam range. Device response is:" + Str( ptr ), Syslog::ERROR );
            return false;
        }

        // We have ranges, check for validity. 9999.99 means invalid.
        // Per e-mail exchange with Ning at linkquest 09/10/2019 this unit will report beam ranges of
        // 0.3m when they should be reporting 9999.99. We'll check for both of those instances here
        // using 1000 m as a high end instead of 9999.99
        // We'll use a single beam altitude in this case if it's available.
        if( beam1Range_ > 0.4 && beam1Range_ < 1000 )
        {
            beam1Good_ = 1;
            altEstimate_ = beam1Range_;
        }
        if( beam2Range_ > 0.4 && beam2Range_ < 1000 )
        {
            beam2Good_ = 1;
            if( ( beam2Range_ < altEstimate_ ) || ( altEstimate_ < 0 ) ) altEstimate_ = beam2Range_;
        }
        if( beam3Range_ > 0.4 && beam3Range_ < 1000 )
        {
            beam3Good_ = 1;
            if( ( beam3Range_ < altEstimate_ ) || ( altEstimate_ < 0 ) ) altEstimate_ = beam3Range_;
        }

        if( beam4Range_ > 0.4 && beam4Range_ < 1000 )
        {
            beam4Good_ = 1;
            if( ( beam4Range_ < altEstimate_ ) || ( altEstimate_ < 0 ) ) altEstimate_ = beam4Range_;
        }
        // We'd like at least a 2 beam solution.
        if( beam1Good_ + beam2Good_ + beam3Good_ + beam4Good_ < 2 ) altEstimate_ = -1;

    }
    else
    {
        return false; // Expecting to see :RD and didn't
    }

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

    uart_.flush();
    return true;
}


void DVL_micro::processPD6()
{

    // populate the vectors for writers using ship referenced data (navigation frame)
    veloInst_.setU( veloInstX_ );
    veloInst_.setV( veloInstY_ );
    veloInst_.setW( veloInstZ_ );

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

    if( altEstimate_ > 0 ) // Altitude has to be greater than 0 (at least by a little) to be valid
    {
        altitudeWriter_->setInvalid( false );
    }
    else
    {
        altitudeWriter_->setInvalid( true );
    }
}


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

    velocityWrtGroundWriter_->setAccuracy( Units::METER_PER_SECOND, DVL_M_ACCURACY ); // since writeWithAccuracy does not exist for blob writers
    velocityWrtGroundWriter_->write1DClass( Units::MILLIMETER_PER_SECOND, veloInst_, startTime_ );

    altitudeWriter_->write( Units::METER, altEstimate_, startTime_ );

    veloInstFlagWriter_->write( Units::COUNT, veloInstFlag_, startTime_ );

    beam1RangeWriter_->write( Units::METER, beam1Range_, startTime_ );
    beam2RangeWriter_->write( Units::METER, beam2Range_, startTime_ );
    beam3RangeWriter_->write( Units::METER, beam3Range_, startTime_ );
    beam4RangeWriter_->write( Units::METER, beam4Range_, 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_ );
}

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

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