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

/*

The RDI Pathfinder needs to be configured prior to use. This component currently expects bottom track
capability output in PD13 message format as shown below.

:SA,   +0.00,   +0.00,  0.00
:TS,21032213560806,35.0, +19.6,   0.0,1520.5,  0
:RA,  0.00,   0.00,   0.00,   0.00,   0.00
:WI,-32768,-32768,-32768,-32768,V
:WS,-32768,-32768,-32768,V
:WE,-32768,-32768,-32768,V
:WD,        +0.00,        +0.00,        +0.00,  20.00,176.00
:BI,-32768,-32768,-32768,-32768,V
:BS,-32768,-32768,-32768,V
:BE,-32768,-32768,-32768,V
:BD,        +0.00,        +0.00,        +0.00,   0.00,176.00

Additionally, the turnkey operation should be set to true (CT1) so the unit starts pinging within 10 seconds of PO reset.
The unit must be also be configured to reflect the appropriate rotation offset as needed using the EA command.
The time between pings variable is usually set to 1 Hz for power savings. However this frequency can be increased as desired up to 2.5 Hz.

Those configuration commands are as follows:
EA+0000
TP00:01.00

Enable water mass data set BK1.

This interface was written against firmware version 67.19

The config variable usePD6 will expect PD6 messages from the instrument instead of the default PD13 messages.

*/

#include "RDI_Pathfinder.h"
#include "RDI_PathfinderIF.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)

RDI_Pathfinder::RDI_Pathfinder( const Module* module )
    : SyncSensorComponent( RDI_PathfinderIF::NAME, module ),
      loadControl_( RDI_PathfinderIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      uart_( RDI_PathfinderIF::UART, RDI_PathfinderIF::BAUD, 0.010, logger_, UART_BUFSIZE ),
      hasPD13_( false ),
      hasPD6_( false ),
      dvlTimeout_( 10.0 ),
      dvlFailTimeout_( 180 ),
      powerOnTimeout_( 12 ),
      dvlTimeoutArmed_( false ),
      usePD6_( false ),
      waterVeloInst_( 0 ),
      waterVeloInstX_( 0 ),
      waterVeloInstY_( 0 ),
      waterVeloInstZ_( 0 ),
      waterVeloError_( -32000 ),
      waterVeloInstFlag_( 0 ),
      waterMassRange_( -1 ),
      veloInst_( 0 ),
      veloInstX_( 0 ),
      veloInstY_( 0 ),
      veloInstZ_( 0 ),
      veloError_( -32000 ),
      veloInstFlag_( 0 ),
      altEstimate_( -1 ),
      beam1Range_( -1 ),
      beam2Range_( -1 ),
      beam3Range_( -1 ),
      beam4Range_( -1 ),
      debug_( false )
{

    power24vConverterDataReader_ = newDataReader( Power24vConverterIF::POWER_24V_CONVERTER );

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

    velocityWrtWaterWriter_ = newUniversalBlobWriter( UniversalURI::PLATFORM_VELOCITY_WRT_SEA_WATER, Units::METER_PER_SECOND, DVL_M_ACCURACY );
    xVelWrtWaterWriter_ = newUniversalWriter( UniversalURI::PLATFORM_X_VELOCITY_WRT_SEA_WATER, Units::METER_PER_SECOND, DVL_M_ACCURACY );
    yVelWrtWaterWriter_ = newUniversalWriter( UniversalURI::PLATFORM_Y_VELOCITY_WRT_SEA_WATER, Units::METER_PER_SECOND, DVL_M_ACCURACY );
    zVelWrtWaterWriter_ = newUniversalWriter( UniversalURI::PLATFORM_Z_VELOCITY_WRT_SEA_WATER, Units::METER_PER_SECOND, DVL_M_ACCURACY );
    waterVeloInstFlagWriter_ = newDataWriter( RDI_PathfinderIF::WATER_MASS_VELOCITY_FLAG );
    waterMassRangeWriter_ = newDataWriter( RDI_PathfinderIF::WATER_MASS_CENTER_RANGE );

    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( RDI_PathfinderIF::BOTTOM_VELOCITY_FLAG_READING );

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

    // Configuration inputs
    readPD6CfgReader_ = newConfigReader( RDI_PathfinderIF::USE_PD6_CFG );

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

RDI_Pathfinder::~RDI_Pathfinder()
{
}

void RDI_Pathfinder::run()
{
}


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

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

    if( !readConfig() )
    {
        logger_.syslog( Str( "Failed to read configuration." ), Syslog::FAULT );
        this->setFailure( FailureMode::DATA );
        return 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 RDI_Pathfinder::starting()
{
    if( debug_ ) logger_.syslog( "Starting", Syslog::INFO );

    if( startTime_.elapsed() < powerOnTimeout_ )
    {
        return STARTING; // give it some more time to power up
    }
    else
    {
        // We're ready to go. Reset the clock and head for runnable.
        startTime_ = Timestamp::Now();
        return RUNNABLE;
    }
}


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

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


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


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

Component::RunState RDI_Pathfinder::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;
    }

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

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

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

    }
    else // Default to PD13
    {
        hasPD13_ = simPD13(); // Always attempt to read simulated values, for vehicle in the loop sims

        if( !simulateHardware() )
        {
            hasPD13_ = readPD13();
        }

        if( hasPD13_ )
        {
            processPD13();
            writeData();
            this->resetFailCount();
        }
    }



    return RUNNABLE;
}

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

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

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

bool RDI_Pathfinder::readConfig()
{
    bool ok( true );
    ok &= readPD6CfgReader_->read( Units::BOOL, usePD6_ );
    if( usePD6_ )
    {
        logger_.syslog( "Expecting PD6 message format", Syslog::IMPORTANT );
    }
    else
    {
        logger_.syslog( "Expecting PD13 message format", Syslog::IMPORTANT );
    }

    return ok;
}


bool RDI_Pathfinder::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 RDI_Pathfinder::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;

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


bool RDI_Pathfinder::simPD13()
{
    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;

        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 simulated data
        veloInstFlag_ = !dvlSimData.velocityWrtBottom.isNan() ? 1 : 0;
        setGroundVelWritersInvalid( dvlSimData.velocityWrtBottom.isNan() );

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

    return goodSim;
}

// Used when usePD6 config variable is set to true
bool RDI_Pathfinder::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;
    }
}


bool RDI_Pathfinder::readPD13()
{
    // See if an entire message has made it in
    // If so, we want to parse the lines: :TS, :RA, :BI, and :BD
    if( uart_.canReadUntil( ":SA" ) && uart_.canReadUntil( ":BD" ) )
    {
        if( debug_ ) logger_.syslog( "Message in queue", Syslog::INFO );
        // Parse the response
        if( !parsePD13() )
        {
            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 PD13 string in the UART yet.
    {
        return false;
    }
}


// Used when usePD6 config variable is set to true
bool RDI_Pathfinder::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. 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
    uart_.flushCRLF().readLines( deviceResponse_, sizeof( deviceResponse_ ), 1 );

    // Line :WS
    uart_.flushCRLF().readLine( deviceResponse_, sizeof( deviceResponse_ ) );
    if( debug_ ) logger_.syslog( "Line :WS:", deviceResponse_, Syslog::INFO );
    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 water mass data
    if( ( ptr = strstr( deviceResponse_, ":WS" ) ) )
    {
        waterVeloInstX_ = waterVeloInstY_ = waterVeloInstZ_ = 0; // Initialize for now
        char flag = 'F';
        // Note that the RDI orients +Y forward (beam 3) so X navigation frame actually corresponds to Y in the RDI spec. Thus the flip below
        // Here we read in Y,X,Z even because the RDI calls X Port->Starboard (beam 1 -> beam 2)
        scanRes = sscanf( ptr, ":WS,%lf,%lf,%lf,%c", &waterVeloInstY_, &waterVeloInstX_, &waterVeloInstZ_, &flag );
        if( flag == 'A' )
        {
            waterVeloInstFlag_ = 1;
        }
        else
        {
            waterVeloInstFlag_ = 0;
        }

        if( debug_ ) logger_.syslog( "Water-Mass Ship Velocity X:", waterVeloInstX_, Syslog::INFO );
        if( debug_ ) logger_.syslog( "Water-Mass Ship Velocity Y:", waterVeloInstY_, Syslog::INFO );
        if( debug_ ) logger_.syslog( "Water-Mass Ship Velocity Z:", waterVeloInstZ_, 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 water velocity. Device response is:" + Str( ptr ), Syslog::ERROR );
            return false;
        }
    }
    else
    {
        return false; // Expecting to see :WS and didn't
    }

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

    // Line :WD
    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 water mass range estimate
    if( ( ptr = strstr( deviceResponse_, ":WD" ) ) )
    {
        waterMassRange_ = -1;
        scanRes = sscanf( ptr, ":WD,%*f,%*f,%*f,%lf,%*f", &waterMassRange_ );

        if( debug_ ) logger_.syslog( "Range to water-mass:", waterMassRange_, Syslog::INFO );
        if( scanRes != 1 )
        {
            logger_.syslog( "only read " + Str( scanRes ) + " of 1 data item for water mass range. Device response is:" + Str( ptr ), Syslog::ERROR );
            return false;
        }
    }
    else
    {
        return false; // Expecting to see :WD and didn't
    }

    // Line :BI is throwaway. We'll actually use the :BS ship referenced data since our RDI doesn't have AHRS. This will allow for the units to be interchanged between vehicles no matter what their
    // manufacturer beam orientation is.
    uart_.flushCRLF().readLines( deviceResponse_, sizeof( deviceResponse_ ), 1 );

    // Line :BS
    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_, ":BS" ) ) )
    {
        veloInstX_ = veloInstY_ = veloInstZ_ = 0; // Initialize for now
        char flag = 'F';
        // Note that the RDI orients +Y forward (beam 3) so X navigation frame actually corresponds to Y in the RDI spec. Thus the flip below
        // Here we read in Y,X,Z even because the RDI calls X Port->Starboard (beam 1 -> beam 2)
        scanRes = sscanf( ptr, ":BS,%lf,%lf,%lf,%c", &veloInstY_, &veloInstX_, &veloInstZ_, &flag );
        if( flag == 'A' )
        {
            veloInstFlag_ = 1;
        }
        else
        {
            veloInstFlag_ = 0;
        }

        if( debug_ ) logger_.syslog( "Ship Velocity X:", veloInstX_, Syslog::INFO );
        if( debug_ ) logger_.syslog( "Ship Velocity Y:", veloInstY_, Syslog::INFO );
        if( debug_ ) logger_.syslog( "Ship 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 :BS and didn't
    }



    // 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. Device response is:" + Str( ptr ), Syslog::ERROR );
            return false;
        }
    }
    else
    {
        return false; // Expecting to see :BD and didn't
    }

    // Line :HM Health status
    /*    uart_.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 leak detection data
        if( ( ptr = strstr( deviceResponse_, ":HM" ) ) )
        {
            char leakA, leakB = ' ';
            scanRes = sscanf( ptr, ":HM,%c,%c,%*x,%*x,%*f,%*f,%*f", &leakA, &leakB );

            if( scanRes != 2 )
            {
                logger_.syslog( "only read " + Str( scanRes ) + " of 1 data item for altitude", Syslog::ERROR );
                return false;
            }

            if( leakA == 'G' && ( leakB == 'G' || leakB == 'D' ) )
            {
                // No leaks detected
                if( debug_ ) logger_.syslog( "No leaks", Syslog::INFO );
            }
            else if( leakA == 'L' || leakB == 'L' )
            {
                // A leak has been detected
                logger_.syslog( "DVL leak detected. System Health output is:", deviceResponse_, Syslog::CRITICAL );
                this->setFailure( FailureMode::HARDWARE );
            }
            else
            {
                logger_.syslog( "Unknown system health message", Syslog::ERROR );
                this->setFailure( FailureMode::HARDWARE );
            }
        }
        else
        {
            return false; // Expecting to see :HM and didn't
        }

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

    uart_.flush();
    return true;
}


bool RDI_Pathfinder::parsePD13()
{
    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. 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 :RA
    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 range to bottom data
    if( ( ptr = strstr( deviceResponse_, ":RA" ) ) )
    {
        beam1Range_ = beam2Range_ = beam3Range_ = beam4Range_ = -1; // Initialize for now
        scanRes = sscanf( ptr, ":RA,%*f,%lf,%lf,%lf,%lf", &beam1Range_, &beam2Range_, &beam3Range_, &beam4Range_ );

        // Convert from decimeters to meters
        beam1Range_ = beam1Range_ / 10;
        beam2Range_ = beam2Range_ / 10;
        beam3Range_ = beam3Range_ / 10;
        beam4Range_ = beam4Range_ / 10;

        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;
        }
    }
    else
    {
        return false; // Expecting to see :RA and didn't
    }

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

    // Line :WS
    uart_.flushCRLF().readLine( deviceResponse_, sizeof( deviceResponse_ ) );
    if( debug_ ) logger_.syslog( "Line :WS:", deviceResponse_, Syslog::INFO );
    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 water mass data
    if( ( ptr = strstr( deviceResponse_, ":WS" ) ) )
    {
        waterVeloInstX_ = waterVeloInstY_ = waterVeloInstZ_ = 0; // Initialize for now
        char flag = 'F';
        // Note that the RDI orients +Y forward (beam 3) so X navigation frame actually corresponds to Y in the RDI spec. Thus the flip below
        // Here we read in Y,X,Z even because the RDI calls X Port->Starboard (beam 1 -> beam 2)
        scanRes = sscanf( ptr, ":WS,%lf,%lf,%lf,%c", &waterVeloInstY_, &waterVeloInstX_, &waterVeloInstZ_, &flag );
        if( flag == 'A' )
        {
            waterVeloInstFlag_ = 1;
        }
        else
        {
            waterVeloInstFlag_ = 0;
        }

        if( debug_ ) logger_.syslog( "Water-Mass Ship Velocity X:", waterVeloInstX_, Syslog::INFO );
        if( debug_ ) logger_.syslog( "Water-Mass Ship Velocity Y:", waterVeloInstY_, Syslog::INFO );
        if( debug_ ) logger_.syslog( "Water-Mass Ship Velocity Z:", waterVeloInstZ_, 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 water velocity. Device response is:" + Str( ptr ), Syslog::ERROR );
            return false;
        }
    }
    else
    {
        return false; // Expecting to see :WS and didn't
    }

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

    // Line :WD
    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 water mass range estimate
    if( ( ptr = strstr( deviceResponse_, ":WD" ) ) )
    {
        waterMassRange_ = -1;
        scanRes = sscanf( ptr, ":WD,%*f,%*f,%*f,%lf,%*f", &waterMassRange_ );

        if( debug_ ) logger_.syslog( "Range to water-mass:", waterMassRange_, Syslog::INFO );
        if( scanRes != 1 )
        {
            logger_.syslog( "only read " + Str( scanRes ) + " of 1 data item for water mass range. Device response is:" + Str( ptr ), Syslog::ERROR );
            return false;
        }
    }
    else
    {
        return false; // Expecting to see :WD and didn't
    }

    // Line :BI is throwaway. We'll actually use the :BS ship referenced data since our RDI doesn't have AHRS. This will allow for the units to be interchanged between vehicles no matter what their
    // manufacturer beam orientation is.
    uart_.flushCRLF().readLines( deviceResponse_, sizeof( deviceResponse_ ), 1 );

    // Line :BS
    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_, ":BS" ) ) )
    {
        veloInstX_ = veloInstY_ = veloInstZ_ = 0; // Initialize for now
        char flag = 'F';
        // Note that the RDI orients +Y forward (beam 3) so X navigation frame actually corresponds to Y in the RDI spec. Thus the flip below
        // Here we read in Y,X,Z even because the RDI calls X Port->Starboard (beam 1 -> beam 2)
        scanRes = sscanf( ptr, ":BS,%lf,%lf,%lf,%c", &veloInstY_, &veloInstX_, &veloInstZ_, &flag );
        if( flag == 'A' )
        {
            veloInstFlag_ = 1;
        }
        else
        {
            veloInstFlag_ = 0;
        }

        if( debug_ ) logger_.syslog( "Ship Velocity X:", veloInstX_, Syslog::INFO );
        if( debug_ ) logger_.syslog( "Ship Velocity Y:", veloInstY_, Syslog::INFO );
        if( debug_ ) logger_.syslog( "Ship 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 :BS and didn't
    }

    // 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 bottom track 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. Device response is:" + Str( ptr ), 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;
}


// Deprecated
void RDI_Pathfinder::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 );
    }
    else
    {
        altitudeWriter_->setInvalid( true );
    }
}


void RDI_Pathfinder::processPD13()
{
    ///////////////
    // WATER MASS
    // populate the vectors for water mass writers using ship referenced data (navigation frame)
    waterVeloInst_.setU( waterVeloInstX_ );
    waterVeloInst_.setV( waterVeloInstY_ );
    waterVeloInst_.setW( waterVeloInstZ_ );

    // perform validity checks for water mass velocity readings
    // water mass velocity is invalid when the velocity flag is false
    bool waterSpeedInvalid = true; // ***DEBUG: set water vel to always be invalid for initial testing :DEBUG***
    // bool waterSpeedInvalid = waterVeloInstFlag_ == 0;
    setWaterVelWritersInvalid( waterSpeedInvalid );

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

    // perform validity checks for bottom track velocity readings
    // bottom track velocity is invalid when the velocity flag is false
    bool groundSpeedInvalid = veloInstFlag_ == 0;
    setGroundVelWritersInvalid( groundSpeedInvalid );

    // finally, altitude has to be greater than 0 to be valid
    bool altInvalid = altEstimate_ <= 0;
    altitudeWriter_->setInvalid( altInvalid );
}

void RDI_Pathfinder::writeData()
{
    // Vehicle referenced water-mass data
    velocityWrtWaterWriter_->setAccuracy( Units::METER_PER_SECOND, DVL_M_ACCURACY ); // writeWithAccuracy does not exist for blob writers
    velocityWrtWaterWriter_->write1DClass( Units::MILLIMETER_PER_SECOND, waterVeloInst_, startTime_ );

    xVelWrtWaterWriter_->write( Units::MILLIMETER_PER_SECOND, waterVeloInstX_, startTime_ );
    yVelWrtWaterWriter_->write( Units::MILLIMETER_PER_SECOND, waterVeloInstY_, startTime_ );
    zVelWrtWaterWriter_->write( Units::MILLIMETER_PER_SECOND, waterVeloInstZ_, startTime_ );

    waterVeloInstFlagWriter_->write( Units::COUNT, waterVeloInstFlag_, startTime_ );
    waterMassRangeWriter_->write( Units::METER, waterMassRange_, startTime_ );

    // 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_ );

    beam1RangeWriter_->write( Units::METER, beam1Range_, startTime_ );
    beam2RangeWriter_->write( Units::METER, beam2Range_, startTime_ );
    beam3RangeWriter_->write( Units::METER, beam3Range_, startTime_ );
    beam4RangeWriter_->write( Units::METER, beam4Range_, startTime_ );

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

}

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

void RDI_Pathfinder::setWaterVelWritersInvalid( bool invalid )
{
    xVelWrtWaterWriter_->setInvalid( invalid );
    yVelWrtWaterWriter_->setInvalid( invalid );
    zVelWrtWaterWriter_->setInvalid( invalid );
    velocityWrtWaterWriter_->setInvalid( invalid );
}

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