/** \file
 *
 *  Contains the RDI_PathfinderUp 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,19022622091777,35.0, -0.1,   0.0,1448.9,  0
:RA,  0.00,   0.00,   0.00,   0.00,   0.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, 54.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 inscreased as desired up to 2.5 Hz.
The upward looking configuration should also be used via the EU1 command

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


*/

#include "RDI_PathfinderUp.h"
#include "RDI_PathfinderUpIF.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_PathfinderUp::RDI_PathfinderUp( const Module* module )
    : SyncSensorComponent( RDI_PathfinderUpIF::NAME, module ),
      hasPD13_( false ),
      dvlTimeout_( 10.0 ),
      dvlFailTimeout_( 180 ),
      powerOnTimeout_( 12 ),
      debug_( false ),
      dvlTimeoutArmed_( false ),
      uart_( RDI_PathfinderUpIF::UART, RDI_PathfinderUpIF::BAUD, 0.010, logger_, UART_BUFSIZE ),
      loadControl_( RDI_PathfinderUpIF::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 )
{
    power24vConverterDataReader_ = newDataReader( Power24vConverterIF::POWER_24V_CONVERTER );

    //altitudeWriter_ = newUniversalWriter( UniversalURI::HEIGHT_ABOVE_SEA_FLOOR, Units::METER, DVL_M_ACCURACY );
    altitudeWriter_ = newDataWriter( RDI_PathfinderUpIF::ALTITUDE_READING );
    xVelWrtGroundWriter_ = newDataWriter( RDI_PathfinderUpIF::X_VELOCITY_WRT_ABOVE );
    yVelWrtGroundWriter_ = newDataWriter( RDI_PathfinderUpIF::Y_VELOCITY_WRT_ABOVE );
    zVelWrtGroundWriter_ = newDataWriter( RDI_PathfinderUpIF::Z_VELOCITY_WRT_ABOVE );
    veloInstFlagWriter_  = newDataWriter( RDI_PathfinderUpIF::BOTTOM_VELOCITY_FLAG_READING );

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

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

RDI_PathfinderUp::~RDI_PathfinderUp()
{
}

void RDI_PathfinderUp::run()
{
}


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

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

    // 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_PathfinderUp::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_PathfinderUp::pause()
{
    if( debug_ ) logger_.syslog( "Pause", Syslog::INFO );
    return PAUSED;
}

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


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


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

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

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

}


bool RDI_PathfinderUp::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_PathfinderUp::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 RDI_PathfinderUp::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 (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;
}

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


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

    // Line :TS
    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 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 :BI
    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 bottom track data
    if( ( ptr = strstr( deviceResponse_, ":BI" ) ) )
    {
        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
        scanRes = sscanf( ptr, ":BI,%lf,%lf,%lf,%lf,%c", &veloInstY_, &veloInstX_, &veloInstZ_, &veloError_, &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( debug_ ) logger_.syslog( "Real Flag:", veloInstFlag_, 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 :BI and didn't
    }

    // Line :BS and :BE are throwaway
    uart_.readLines( deviceResponse_, sizeof( deviceResponse_ ), 2 );

    // Line :BD
    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 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
    }

    // 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_PathfinderUp::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", 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 :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", Syslog::ERROR );
            return false;
        }
    }
    else
    {
        return false; // Expecting to see :RA 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", 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", 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_PathfinderUp::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
    {
        xVelWrtGroundWriter_->setInvalid( false );
        yVelWrtGroundWriter_->setInvalid( false );
        zVelWrtGroundWriter_->setInvalid( false );
    }
    else // ground speed is not valid
    {
        xVelWrtGroundWriter_->setInvalid( true );
        yVelWrtGroundWriter_->setInvalid( true );
        zVelWrtGroundWriter_->setInvalid( 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_PathfinderUp::processPD13()
{

    // 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
    {
        xVelWrtGroundWriter_->setInvalid( false );
        yVelWrtGroundWriter_->setInvalid( false );
        zVelWrtGroundWriter_->setInvalid( false );
    }
    else // ground speed is not valid
    {
        xVelWrtGroundWriter_->setInvalid( true );
        yVelWrtGroundWriter_->setInvalid( true );
        zVelWrtGroundWriter_->setInvalid( true );
    }

    // finally, perform validity checks for quality control
    if( veloInstFlag_ == 1 ) // Ground speed is valid
    {
        xVelWrtGroundWriter_->setInvalid( false );
        yVelWrtGroundWriter_->setInvalid( false );
        zVelWrtGroundWriter_->setInvalid( false );
    }
    else // ground speed is not valid
    {
        xVelWrtGroundWriter_->setInvalid( true );
        yVelWrtGroundWriter_->setInvalid( true );
        zVelWrtGroundWriter_->setInvalid( 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_PathfinderUp::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_ );
    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_ );
}

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

bool RDI_PathfinderUp::isDataRequested()
{
    return true; // *** DEBUG for testing offshohre
    /*          return altitudeWriter_->isDataRequested()
               || xVelWrtGroundWriter_->isDataRequested()
               || yVelWrtGroundWriter_->isDataRequested()
               || zVelWrtGroundWriter_->isDataRequested();
    */
}