/** \file
 *
 *  Contains the DAT class implementation.
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

/***

!!! This DAT interface requires that the DAT to which it is connected be configured prior to use !!!


@DatVerbose parameter should be set to 27440 (0x6B30 per spec).


response to "ATR" or "dat" command should look something like this:

19:49:26.6093 LVL= 14368, 13937, 18930, 15587, AGC= 35, IDX= 868, 0.20,-0.122, 0.229,-0.335,-0.067, PHS= 0.034,344,-0.271, RAW= 359.7,  -4.1, CAL= 359.2,  -8.7, ROT= 359.1,  -8.7
Bearing 307.5,   9.3
Range 2 to 11 : 1.5 m


!!! The configuration below may not be correct. Specifically, the homing related values need to be updates (rotation, acoustic response timeout, orientation, etc) !!!
Configuration below:
<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

SEE BOTTOM OF FILE FOR CFG ALL SETTINGS - vthresh is set with diag privs and determines USBL input threshold. At time of writing, 0.7 was recommended by the vendor.
Additionally the DAT.Arrival value should be set to peak to avoid the PEAK output on the DAT:
| ^Arrival         | 0           | 1 (First), 0 (Peak)


S- register settings should be as follows:
user:2>ats?
Local Sregisters
S00=000  S01=000  S02=000  S03=002  S04=005  S05=000  S06=008  ** S03=007 <- baud 115200
S07=007  S08=060  S09=000  S10=255  S11=000  S12=000  S13=003
S14=000  S15=001  S16=001  S17=001  S18=002  S19=000  S20=000

Set sregisters with command ATS<n>=<value> and save with at&w or cfg store.



The DAT also now uses the internal compass to rotate the bearing/elevation into vehicle frame. The compass orientation at the time of writing was set using the xsens command:
xsens {-0.5,0.5,-0.5,0.5}

compass check will display the compass results


| ^Rotation        | 150.0       | 0 to 359.9 degrees
The rotation value set by @rotation at the time of writing was 150 (connetor forward 30 degrees to port head mounted up)


Serial Parsing:
The DAT has a number of potential responses. Some arrive upon the RX of a chirp, while other are soliciated (e.g. ATR command). Here are exmamples of the options this code can parse.

option 1: ATR1
Rx Time:17:36:53.0488
17:36:53.0488 LVL= 32032, 32753, 22386, 31315, AGC= 56, IDX= 438,-0.25, 2.397, 2.398, 2.620, 2.405, PHS=-0.006, 0.009, 0.171, RAW= 214.4, -19.9, CAL= 215.2, -22.0, ROT=  33.9, -22.0
Bearing 206,  29 (Local)
Range 2 to 1 : 1.5 m  (Round-trip  2.0 ms) speed  0.0 m/s
user:88>

Option 2: ATR1
Tx time:17:37:09.3928
Rx Time:17:37:11.4240
17:37:11.4240 LVL= 32176, 32753, 32754, 32755, AGC= 57, IDX= 372,-0.05,-2.102,-2.098,-1.880,-2.095, PHS=-0.003, 0.013, 0.172, RAW= 214.9, -21.0, CAL= 215.8, -23.1, ROT=  34.5, -23.1
Bearing  14, -10 (Remote)
Bearing 207,  30 (Local)
Range 2 to 1 : 1.5 m  (Round-trip  2.0 ms) speed  0.0 m/s
user:89>

Option 3: RX no bearing (see option 1)
user:16>Rx Time:19:48:50.5255
19:48:50.5255 LVL= 32752, 32753, 32754,     0, AGC= 56, IDX= 438, 0.00, 2.726, 2.869, 2.988, 1.389, PHS= 1.304, 1.466, 1.587, RAW= 244.7, -80.9, CAL= 246.9, -80.5, ROT=  58.3, -80.5
range request
Tx time:19:48:52.1284

Option 4: RX bearing (see option 2)
Rx Time:19:49:09.2756
19:49:09.2756 LVL= 32752, 32753, 32754,     0, AGC= 55, IDX= 664, 0.00,-0.925,-0.819,-0.722, 1.162, PHS=-2.121,-1.996,-1.896, RAW= 243.6,  84.8, CAL= 243.8,  85.8, ROT=  55.2,  85.8
bearing request
Tx time:19:49:10.8785

Option 5: ATR2 - no local bearing but remote bearing (see option 4)
Tx time:19:50:54.3432
Rx Time:19:50:56.3745
Bearing  27, -30 (Remote)
Range 1 to 2 : 1.5 m  (Round-trip  2.0 ms) speed  0.0 m/s
user:17>

Option 6: ATR2
Tx time:20:01:43.1973
Response Not Received
user:19>

Option 7: A packet destined for another modem (with solid USBL chirp)
Rx Time:23:16:05.5288
23:16:05.5288 LVL= 32752, 32753, 32754, 32755, AGC= 51, IDX= 618,-0.04,-1.275,-1.279,-1.071,-1.267, PHS=-0.005, 0.004, 0.152,  RAW= 213.0, -19.3, CAL= 213.7, -21.4, ROT=  32.4, -21.4

$Packet for address 5
 [05]<[01] ATR


Option 8: A packet destined for another modem (without solid USBL chirp)
Rx Time:01:28:57.6978

$Packet for address 5
 [05]<[02] ATR

Option 9: An invalid header
$Error in header
Acquisition stats SNR:38.2 MPD:04.0 SPD:+00.0
Header stats CRC:Fail SNR:05.2 CCERR:005

Option 10: A low SNR chirp
$Low SNR acquistion detected


***/

#include "DAT.h"
#include "DATIF.h"

#include <cstdlib>
#include <errno.h>
#include <unistd.h>  // include for the access method
#include <math.h>    /* atan2 and asin */
#include <sys/stat.h>

#include "Power24vConverterIF.h"
#include "data/BlobWriter.h"
#include "data/ConfigReader.h"
#include "data/Matrix3x3.h"
#include "data/Point3D.h"
#include "data/Point6D.h"
#include "data/SimSlate.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"
#include "io/FileInStream.h"
#include "io/FileOutStream.h"
#include "io/StrIOStream.h"
#include "supervisor/DataReceiver.h"
#include "supervisor/Supervisor.h"
#include "units/Units.h"
#include "utils/AuvMath.h"
#include "VehicleIF.h"

const Timespan DAT::PERIOD( 0.25 );

const Str DAT::ACK( "~~" );


DAT::DAT( const Module* module )
    : AsyncComponent( DATIF::NAME, module, PERIOD ),
      commsState_( SENDING_FILL_BUFFER ),
      outgoingCommsBuffer_( true ),
      outgoingCommsNow_( NULL ),
      debug_( false ),
      verbosity_( 3 ),
      datVerbose_( 27440 ),
      loadControl_( DATIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      startTime_( Timestamp::NOT_SET_TIME ),
      connectTime_( Timestamp::NOT_SET_TIME ),
      cmdTimeStart_( Timestamp::NOT_SET_TIME ),
      powerOffTimeStart_( Timestamp::NOT_SET_TIME ),
      powerOnTimeout_( 60 ), // measured to be ~12 sec on bench
      postConnectTimeout_( 2 ), // Time to wait after getting " CONNECT 00800 bits/sec ..."
      cmdTimeout_( 15 ), // measured 12 seconds to wake from lowpower mode
      powerDownTimeout_( 3 ),
      cmdTries_( 0 ),
      maxCmdTries_( 4 ),
      uart_( DATIF::UART, DATIF::BAUD, 0.1, logger_, 4095 ),
      keyText_(),
      verboseCfgSetting_( 3 ),
      txPowerCfgSetting_( 8 ),
      localAddressCfgSetting_( 0 ),
      sbdAddressCfgSetting_( -1 ),
      transponderAddressCfgSetting_( -1 ),
      surfaceThresholdCfgSetting_( 1 ),
      sendExpressCfgSetting_( 0 ),
      maxAckTimeoutsCfgSetting_( 0 ),
      sendDataToShoreCfgSetting_( true ),
      commandModeSent_( false ),
      commandModeAcknowledged_( false ),
      onlineModeSent_( false ),
      onlineModeAcknowledged_( false ),
      skipNextError_( false ),
      verboseSettingSent_( false ),
      verboseSettingAcknowledged_( false ),
      datVerboseSent_( false ),
      datVerboseAcknowledged_( false ),
      txPowerSettingSent_( false ),
      txPowerSettingAcknowledged_( false ),
      localAddressSettingSent_( false ),
      localAddressSettingAcknowledged_( false ),
      currentRemoteAddressSent_( false ),
      currentRemoteAddressAcknowledged_( false ),
      localTimeSent_( false ),
      localTimeSetAcknowledged_( false ),
      acousticResponseTimeout_( 10.0 ), // measured to be ~10 sec on bench
      dataBytesReceiving_( 0 ),
      dataBytesReceived_( 0 ),
      remoteAddressRequested_( 0 ),
      numPingsRequested_( 1 ),
      numPingsReceived_( 0 ),
      gotAcousticWakeup_( false ),
      gotResponseNotReceived_( false ),
      gotDebugRxMessage_( false ),
      gotDebugTxMessage_( false ),
      gotRangeRequestMessage_( false ),
      gotRangeMessage_( false ),
      gotDirectionMessage_( false ),
      gotAckMessage_( false ),
      awaitingAckTransmitAck_( false ),
      commRate_( 0 ),
      commRateReported_( false ),
      ackTimeouts_( 0 ),
      rangeRequestPending_( false ),
      oneWayRequested_( false ),
      range_( nan( "" ) ),
      phaseA_( nan( "" ) ),
      phaseB_( nan( "" ) ),
      phaseC_( nan( "" ) ),
      dataTimestamp_( Timestamp::NOT_SET_TIME ),
      transmitPingTime_( Timestamp::NOT_SET_TIME ),
      receivePingTime_( Timestamp::NOT_SET_TIME ),
      levelA_( 0 ),
      levelB_( 0 ),
      levelC_( 0 ),
      levelD_( 0 ),
      agc_( 0 ),
      rawAzimuth_( nan( "" ) ), // Transducer frame azimuth, uncalibrated.
      rawElevation_( nan( "" ) ), // Transducer frame elevation, uncalibrated.
      calibratedAzimuth_( nan( "" ) ), // Transducer frame azimuth, calibrated.
      calibratedElevation_( nan( "" ) ), // Transducer frame elevation, calibrated.
      rotatedAzimuth_( nan( "" ) ), // Vehicle frame azimuth (as read from DAT).
      rotatedElevation_( nan( "" ) ),  // Vehicle frame elevation (as read from DAT).
      remoteAddress_( 0 ),
      localAddress_( 0 ),
      deviceEnableRequested_( false ), // Indicates a request to enable the remote device enable line at the remote address has been sent.
      currentRemoteAddress_( -1 ),
      incomingRemoteAddress_( -1 ),
      incomingLocalAddress_( -1 ),
      sendPacket_( MAX_DOWNLINK_DATA_SIZE, &logger_ )
{
    // Configuration inputs
    verbosityCfgReader_ = newConfigReader( DATIF::VERBOSITY_CFG );
    txPowerCfgReader_ = newConfigReader( DATIF::TX_POWER_CFG );
    localAddressCfgReader_ = newConfigReader( VehicleIF::ID_CFG );
    sbdAddressCfgReader_ = newConfigReader( DATIF::SBD_ADDRESS_CFG );
    transponderAddressCfgReader_ = newConfigReader( DATIF::TRANSPONDER_ADDRESS_CFG );
    phaseDataToDirectionCfgReader_ = newConfigReader( DATIF::PHASE_TO_DIRECTION_CFG );
    ignoreElevationCfgReader_ = newConfigReader( DATIF::IGNORE_ELEVATION_ANGLE_CFG );
    surfaceThresholdCfgReader_ = newConfigReader( DATIF::SURFACE_THRESHOLD_CFG );
    sendExpressCfgReader_ = newConfigReader( DATIF::SEND_EXPRESS_CFG );
    maxAckTimeoutsCfgReader_ = newConfigReader( DATIF::MAX_ACK_TIMEOUTS );
    sendDataToShoreCfgReader_ = newConfigReader( VehicleIF::SEND_DATA_TO_SHORE_CFG );

    // Slate inputs
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );
    queryAddressRequestedReader_ = newDataReader( DATIF::QUERY_ADDRESS_REQUESTED );
    queryAddressRequestedReader_->setImplementor( true );
    numberOfPingsRequestedReader_ = newDataReader( DATIF::NUMBER_OF_PINGS_REQUESTED );
    numberOfPingsRequestedReader_->setImplementor( true );
    power24vConverterDataReader_ = newDataReader( Power24vConverterIF::POWER_24V_CONVERTER );

    // universal outputs
    contactAddressWriter_ = newUniversalWriter( UniversalURI::ACOUSTIC_CONTACT_ADDRESS_READING, Units::ENUM, 0.1 );
    directionToContactWriter_ = newUniversalBlobWriter( UniversalURI::ACOUSTIC_CONTACT_DIRECTION_VF, Units::NONE, 0.5 );
    platformCommunicationsWriter_ = newUniversalWriter( UniversalURI::PLATFORM_COMMUNICATIONS, Units::BOOL, 0 );
    rangeToContactWriter_ = newUniversalWriter( UniversalURI::ACOUSTIC_CONTACT_RANGE_READING, Units::METER, 0.5 );
    rxTimeWriter_ = newUniversalWriter( UniversalURI::ACOUSTIC_RECEIVE_TIME, Units::SECOND, 0.1 );
    txTimeWriter_ = newUniversalWriter( UniversalURI::ACOUSTIC_TRANSMIT_TIME, Units::SECOND, 0.1 );

    sendDataBuffer_ = SendData::GetBuffer( "modem" );

    // Slate outputs
    lvl1Writer_ = newDataWriter( DATIF::LVL1_READING );
    lvl2Writer_ = newDataWriter( DATIF::LVL2_READING );
    lvl3Writer_ = newDataWriter( DATIF::LVL3_READING );
    lvl4Writer_ = newDataWriter( DATIF::LVL4_READING );
    agcWriter_ = newDataWriter( DATIF::AGC_READING );
    phaseAWriter_ = newDataWriter( DATIF::PHASEA_READING );
    phaseBWriter_ = newDataWriter( DATIF::PHASEB_READING );
    phaseCWriter_ = newDataWriter( DATIF::PHASEC_READING );
    rawAzimuthWriter_ = newDataWriter( DATIF::RAWAZIMUTH_READING );
    rawElevationWriter_ = newDataWriter( DATIF::RAWELEVATION_READING );
    calibratedAzimuthWriter_ = newDataWriter( DATIF::CALAZIMUTH_READING );
    calibratedElevationWriter_ = newDataWriter( DATIF::CALELEVATION_READING );
    rotatedAzimuthWriter_ = newDataWriter( DATIF::ROTAZIMUTH_READING );
    rotatedElevationWriter_ = newDataWriter( DATIF::ROTELEVATION_READING );
    acousticWakeupWriter_ = newDataWriter( DATIF::ACOUSTIC_WAKEUP );
    rangeRequestReceivedWriter_ = newDataWriter( DATIF::RANGE_REQUEST );
    localAddressWriter_ = newDataWriter( DATIF::LOCAL_ADDRESS_READING );
    deviceEnableRequestedWriter_ = newDataWriter( DATIF::DEVICE_ENABLE_REQUESTED ); // Indicates a request to enable the remote device enable line at the remote address has been sent.
    msgAcknowledgedWriter_ = newDataWriter( DATIF::MSG_ACKNOWLEDGED );

    tAzimuthWriter_ = newDataWriter( DATIF::AZIMUTH_IF );
    tElevationWriter_ = newDataWriter( DATIF::ELEVATION_IF );
    vAzimuthWriter_ = newDataWriter( DATIF::AZIMUTH_VF );
    vElevationWriter_ = newDataWriter( DATIF::ELEVATION_VF );
    tDirectionWriter_ = newBlobWriter( DATIF::DIRECTION_IF );

    deviceResponse_[0] = '\0';

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

    this->setAllowableFailures( 5 ); // TODO: Should this be a configuration variable?
    this->setRetryTimeout( 120 );
    if( !this->configSetFailureMissionCritical() )
    {
        this->setFailureMissionCritical( false );
    }

    if( simulateHardware() )
    {
        readConfig();
        Slate::ReadOnce( VehicleIF::NAME_CFG, vehicleNameCfg_, logger_ );
        SimSlate::SetAcousticResponseTimeout( acousticResponseTimeout_ );
        SimSlate::SetLocalAddress( vehicleNameCfg_.asString(), localAddressCfgSetting_ );
    }
}


DAT::~DAT()
{
}


void DAT::run()
{
}


void DAT::readConfig()
{
    verbosityCfgReader_->read( Units::COUNT, verbosity_ );

    // keyText -- only read once
    if( keyText_.asString() == Str::EMPTY_STR )
    {
        Slate::ReadOnce( VehicleIF::KEY_TEXT_CFG, keyText_, logger_ );
    }

    int oldTxPower = txPowerCfgSetting_;
    if( !txPowerCfgReader_->read( Units::COUNT, txPowerCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for transmit power, using", txPowerCfgSetting_, Syslog::ERROR );
    }
    if( oldTxPower != txPowerCfgSetting_ )
    {
        txPowerSettingAcknowledged_ = txPowerSettingSent_ = false;
    }

    int localAddress( -1 );
    if( localAddressCfgReader_->read( Units::COUNT, localAddress ) )
    {
        if( localAddressCfgSetting_ != localAddress )
        {
            SimSlate::SetLocalAddress( vehicleNameCfg_.asString(), localAddress );
        }
        localAddressCfgSetting_ = localAddress;
    }
    else
    {
        logger_.syslog( "Could not read configuration setting for local address, using", localAddressCfgSetting_, Syslog::ERROR );
    }

    if( !sbdAddressCfgReader_->read( Units::ENUM, sbdAddressCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for sbd address, using", sbdAddressCfgSetting_, Syslog::ERROR );
    }

    if( !transponderAddressCfgReader_->read( Units::ENUM, transponderAddressCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for transponder address, using", transponderAddressCfgSetting_, Syslog::ERROR );
    }

    // surfaceThreshold
    if( !surfaceThresholdCfgReader_->read( Units::METER, surfaceThresholdCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for surfaceThreshold, using", surfaceThresholdCfgSetting_, Syslog::ERROR );
    }

    // sendExpress
    if( !sendExpressCfgReader_->read( Units::BOOL, sendExpressCfgSetting_ ) )
    {
        //logger_.syslog( "Could not read configuration setting for sendExpress, using", sendExpressCfgSetting_, Syslog::ERROR );
        sendExpressCfgSetting_  = false;
    }

    // maxAckTimeouts
    if( !maxAckTimeoutsCfgReader_->read( Units::BOOL, maxAckTimeoutsCfgSetting_ ) )
    {
        maxAckTimeoutsCfgSetting_  = 0;
    }

    // sendDataToShore
    unsigned char sendDataToShore = sendDataToShoreCfgSetting_;
    if( !sendDataToShoreCfgReader_->read( Units::BOOL, sendDataToShore ) )
    {
        logger_.syslog( "Could not read configuration setting for sending data to shore, using", sendDataToShore, Syslog::ERROR );
    }
    sendDataToShoreCfgSetting_ = sendDataToShore;

    if( phaseDataToDirectionCfgReader_->read( Units::BOOL, phaseToDirectionCfgSetting_ )
            && phaseToDirectionCfgSetting_ )
    {
        if( verbosity_ > 0 ) logger_.syslog( "Will construct direction to contact in vehicle frame from tetrahedron phase data.", Syslog::INFO );
    }

    ignoreElevationSetting_ = 0;
    if( ignoreElevationCfgReader_->read( Units::BOOL, ignoreElevationSetting_ )
            && ignoreElevationSetting_ )
    {
        if( verbosity_ > 0 ) logger_.syslog( "Will construct direction to contact in vehicle frame with elevation angle set to 0.", Syslog::INFO );
    }

// TODO    Matrix3x3 M_iv(rotation or calibration matrix read from config file)
//    Matrix3x3 M_iv( 1, 0, 0, 0, 1, 0, 0, 0, 1 ); // XXX identity for now

// General linear fit calibration matrix
    transformationFromTetrahedronToVehicleFrame_ = Matrix3x3( -0.10741884, -0.03043104, -0.99932695,
            -0.3696566,  -0.83706356,  0.05405989,
            0.80422921, -0.41815993, -0.06667038 );

// Rotoreflection fit calibration matrix
//    Matrix3x3 M_iv( -0.10132846, -0.02122281, -0.99462663,
//                       -0.4221235,  -0.90439496,  0.0623017,
//                       0.90085753, -0.42616821, -0.08268231 );

    transformationFromVehicleToTetrahedronFrame_ = transformationFromTetrahedronToVehicleFrame_; // TODO: confirm this copies instead of referencing
    transformationFromVehicleToTetrahedronFrame_.transpose(); // XXX this is *actually* transformationFromVehicleToTetrahedronFrame_ now...

    // TODO use a different LA package. The transposed matrices above should be renamed, but the transposition is done in-place right now, making the math harder to follow. We really want to write: transformationFromVehicleToInstrumentFrame_ = transformationFromInstrumentToVehicleFrame_.transpose();

}

void DAT::uninitialize()
{
    if( debug_ ) logger_.syslog( "uninitialize", Syslog::INFO );
    if( !simulateHardware() )
    {
        logger_.syslog( "Powering down", Syslog::INFO );
        if( !loadControl_.powerDown() )
        {
            logger_.syslog( "Failed to power down", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }
        powerOffTimeStart_ = Timestamp::Now();
        uart_.close();
    }

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


/// Do what needs to be done to run
/// Similar to initialize, in old init/run/uninit sequence
Component::RunState DAT::start()
{
    if( debug_ ) logger_.syslog( "Start", Syslog::INFO );

    commRate_ = 0;
    commRateReported_ = false;
    commandModeSent_ = false;
    commandModeAcknowledged_ = false;
    verboseSettingSent_ = false;
    verboseSettingAcknowledged_ = false;
    datVerboseSent_ = false;
    datVerboseAcknowledged_ = false;
    txPowerSettingSent_ = false;
    txPowerSettingAcknowledged_ = false;
    localAddressSettingSent_ = false;
    localAddressSettingAcknowledged_ = false;
    currentRemoteAddressSent_ = false;
    currentRemoteAddressAcknowledged_ = false;
    phaseToDirectionCfgSetting_ = 0;
    ackTimeouts_ = 0;
    oneWayRequested_ = false;
    localTimeSent_ = false;
    localTimeSetAcknowledged_ = false;

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

    if( powerOffTimeStart_.elapsed() < powerDownTimeout_ )
    {
        return START;
    }

    if( simulateHardware() )
    {
        return STARTING;
    }

    deviceResponse_[0] = '\0';
    this->setAllowableFailures( 8 ); // TODO: make this into a config setting?
    this->setRetryTimeout( 300 );

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

    logger_.syslog( "Initializing DAT." );

    // Enable xceiver
    uart_.enableUART();

    // Open the uart
    uart_.open();
    if( uart_.hasError() )
    {
        logger_.syslog( "Error opening port: ", uart_.errorString(), Syslog::ERROR );
        this->setFailure( FailureMode::COMMUNICATIONS );
        return STOP;
    }
    startTime_ = Timestamp::Now();
    uart_.flush();
    return STARTING;
}


// Might follow a STOP...START sequence
Component::RunState DAT::starting()
{
    if( debug_ ) logger_.syslog( "Starting", Syslog::INFO );

    readConfig();

    // Deal with incoming data
    bool gotLines = readAndParseResponses();

    if( simulateHardware() )
    {
        commandModeAcknowledged_ = localAddressSettingSent_ = currentRemoteAddressSent_ = true;
        return RUNNABLE;
    }
    else if( startTime_.elapsed() > powerOnTimeout_ )
    {
        if( uart_.dataAvailable() == 0 )
        {
            logger_.syslog( "failed to initialize, no bytes available on serial interface", Syslog::FAULT );
        }
        else
        {
            char msg[uart_.dataAvailable()];
            uart_.read( msg, sizeof( msg ) );
            logger_.syslog( "failed to initialize; deviceResponse_ loaded: " + Str( deviceResponse_ ) + ", available: " + Str( msg ), Syslog::FAULT );
        }
        this->setFailure( FailureMode::COMMUNICATIONS );
        return STOP;
    }
    else if( !commandModeSent_ && gotLines && !commRateReported_ )
    {
        if( commRate_ > 0 )
        {
            commRateReported_ = true;
            return STARTING;
        }
        else if( commRate_ < 0 )
        {
            return STOP;
        }
    }
    else if( ( commRate_ > 0 ) && !verboseSettingAcknowledged_ ) // you've seen the CONNECT message, one cycle ago...
    {
        if( !sendVerboseCfgSetting() )
        {
            verboseSettingSent_ = false;
        }
    }
    else if( verboseSettingAcknowledged_ && !datVerboseAcknowledged_ )
    {
        if( !sendDatVerbose() )
        {
            datVerboseSent_ = false;
        }
    }
    else if( datVerboseAcknowledged_ && !txPowerSettingAcknowledged_ )
    {
        if( !sendTxPowerFromCfg() )
        {
            txPowerSettingSent_ = false;
        }
    }
    else if( txPowerSettingAcknowledged_ && !localAddressSettingAcknowledged_ )
    {
        if( !sendLocalAddressFromCfg() )
        {
            localAddressSettingSent_ = false;
        }
    }
    else if( localAddressSettingAcknowledged_ && !localTimeSetAcknowledged_ )
    {
        if( !setTime()	)
        {
            localTimeSent_ = false;
        }
    }
    else if( localTimeSetAcknowledged_ )
    {
        return runnable();
    }

    return STARTING;
}

bool DAT::enterCommandMode()
{
    if( simulateHardware() )
    {
        commandModeSent_ = commandModeAcknowledged_ = true;
        return true;
    }
    if( connectTime_.elapsed() < postConnectTimeout_ ) // wait a bit after getting "CONNECT 00800 bits/sec..."
    {
        return true;
    }
    if( !commandModeSent_ ) // you've seen the CONNECT message, one cycle ago...
    {
        uart_ << "+++"; // enter command mode
        Timespan::Milliseconds( 50 ).sleepFor();
        uart_ << "\r\n";

        commandModeSent_ = true;
        logger_.syslog( "entering command mode", Syslog::INFO );
        onlineModeSent_ = onlineModeAcknowledged_ = commandModeAcknowledged_ = false;
        cmdTimeStart_ = Timestamp::Now();
        ++cmdTries_;
        return true;
    }
    if( commandModeSent_ && !commandModeAcknowledged_ )
    {
        logger_.syslog( "checking for command mode acknowledgment", Syslog::DEBUG );
        if( cmdTimeStart_.elapsed() > cmdTimeout_ )
        {
            logger_.syslog( "failed to enter command mode", Syslog::FAULT );
            ++cmdTries_;
            if( cmdTries_ > maxCmdTries_ )
            {
                this->setFailure( FailureMode::COMMUNICATIONS );
            }
            else
            {
                commandModeSent_ = false;
            }
            return false;
        }
    }
    cmdTries_ = 0;
    return true;
}

bool DAT::enterOnlineMode()
{
    if( simulateHardware() )
    {
        onlineModeSent_ = onlineModeAcknowledged_ = true;
        return true;
    }
    if( !onlineModeSent_ ) // you've seen the CONNECT message, one cycle ago...
    {
        uart_ << "ATO\r"; // enter online mode
        onlineModeSent_ = true;
        logger_.syslog( "entering online mode", Syslog::INFO );
        commandModeSent_ = commandModeAcknowledged_ = onlineModeAcknowledged_ = false;
        cmdTimeStart_ = Timestamp::Now();
        ++cmdTries_;
        return true;
    }
    if( onlineModeSent_ && !onlineModeAcknowledged_ )
    {
        logger_.syslog( "checking for online mode acknowledgment", Syslog::DEBUG );
        if( cmdTimeStart_.elapsed() > cmdTimeout_ )
        {
            logger_.syslog( "failed to enter online mode", Syslog::FAULT );
            if( cmdTries_ > maxCmdTries_ )
            {
                this->setFailure( FailureMode::COMMUNICATIONS );
            }
            else
            {
                onlineModeSent_ = false;
            }
            return false;
        }
    }
    cmdTries_ = 0;
    return true;
}

bool DAT::sendVerboseCfgSetting()
{
    if( !commandModeAcknowledged_ )
    {
        if( !enterCommandMode() )
        {
            commandModeSent_ = false;
            return false;
        }
        if( !commandModeAcknowledged_ )
        {
            return true;
        }
    }
    if( simulateHardware() )
    {
        verboseSettingSent_ = verboseSettingAcknowledged_ = true;
        return true;
    }
    if( !verboseSettingSent_ )
    {
        logger_.syslog( "setting verbose to ", verboseCfgSetting_, Syslog::INFO );
        uart_ << "@verbose=" << verboseCfgSetting_ << "\r";
        verboseSettingSent_ = true;
        verboseSettingAcknowledged_ = false;
        cmdTimeStart_ = Timestamp::Now();
        ++cmdTries_;
        return true;
    }
    else if( verboseSettingSent_ && !verboseSettingAcknowledged_ )
    {
        logger_.syslog( "checking for verbose setting acknowledgment", Syslog::DEBUG );
        if( cmdTimeStart_.elapsed() > cmdTimeout_ )
        {
            logger_.syslog( "failed to set verbose", Syslog::FAULT );
            if( cmdTries_ > maxCmdTries_ )
            {
                this->setFailure( FailureMode::COMMUNICATIONS );
            }
            else
            {
                verboseSettingSent_ = false;
            }
            return false;
        }
    }
    cmdTries_ = 0;
    return true;
}

bool DAT::sendDatVerbose()
{
    if( !commandModeAcknowledged_ )
    {
        if( !enterCommandMode() )
        {
            commandModeSent_ = false;
            return false;
        }
        if( !commandModeAcknowledged_ )
        {
            return true;
        }
    }
    if( simulateHardware() )
    {
        datVerboseSent_ = datVerboseAcknowledged_ = true;
        return true;
    }
    if( !datVerboseSent_ )
    {
        logger_.syslog( "setting DatVerbose to ", datVerbose_, Syslog::INFO );
        uart_ << "@DatVerbose=" << datVerbose_ << "\r";
        datVerboseSent_ = true;
        datVerboseAcknowledged_ = false;
        cmdTimeStart_ = Timestamp::Now();
        ++cmdTries_;
        return true;
    }
    else if( datVerboseSent_ && !datVerboseAcknowledged_ )
    {
        logger_.syslog( "checking for DatVerbose setting acknowledgment", Syslog::DEBUG );
        if( cmdTimeStart_.elapsed() > cmdTimeout_ )
        {
            logger_.syslog( "failed to set DatVerbose", Syslog::FAULT );
            if( cmdTries_ > maxCmdTries_ )
            {
                this->setFailure( FailureMode::COMMUNICATIONS );
            }
            else
            {
                datVerboseSent_ = false;
            }
            return false;
        }
    }
    cmdTries_ = 0;
    return true;
}

bool DAT::sendTxPowerFromCfg()
{
    if( !commandModeAcknowledged_ )
    {
        if( !enterCommandMode() )
        {
            commandModeSent_ = false;
            return false;
        }
        if( !commandModeAcknowledged_ )
        {
            return true;
        }
    }
    if( simulateHardware() )
    {
        txPowerSettingSent_ = txPowerSettingAcknowledged_ = true;
        return true;
    }
    if( !txPowerSettingSent_ )
    {
        logger_.syslog( "setting transmit power to ", txPowerCfgSetting_, Syslog::INFO );
        uart_ << "@txPower=" << txPowerCfgSetting_ << "\r";
        txPowerSettingSent_ = true;
        txPowerSettingAcknowledged_ = false;
        cmdTimeStart_ = Timestamp::Now();
        ++cmdTries_;
        return true;
    }
    else if( txPowerSettingSent_ && !txPowerSettingAcknowledged_ )
    {
        logger_.syslog( "checking for transmit power setting acknowledgment", Syslog::DEBUG );
        if( cmdTimeStart_.elapsed() > cmdTimeout_ )
        {
            logger_.syslog( "failed to set transmit power", Syslog::FAULT );
            if( cmdTries_ > maxCmdTries_ )
            {
                this->setFailure( FailureMode::COMMUNICATIONS );
            }
            else
            {
                txPowerSettingSent_ = false;
            }
            return false;
        }
    }
    cmdTries_ = 0;
    return true;
}

bool DAT::sendLocalAddressFromCfg()
{
    if( !commandModeAcknowledged_ )
    {
        if( !enterCommandMode() )
        {
            commandModeSent_ = false;
            return false;
        }
        if( !commandModeAcknowledged_ )
        {
            return true;
        }
    }
    if( simulateHardware() )
    {
        localAddressSettingSent_ = localAddressSettingAcknowledged_ = true;
        return true;
    }
    if( !localAddressSettingSent_ )
    {
        logger_.syslog( "setting local address to ", localAddressCfgSetting_, Syslog::INFO );
        uart_ << "@localaddr=" << localAddressCfgSetting_ << "\r";
        localAddressSettingSent_ = true;
        localAddressSettingAcknowledged_ = false;
        cmdTimeStart_ = Timestamp::Now();
        ++cmdTries_;
        return true;
    }
    else if( localAddressSettingSent_ && !localAddressSettingAcknowledged_ )
    {
        logger_.syslog( "checking for local address setting acknowledgment", Syslog::DEBUG );
        if( cmdTimeStart_.elapsed() > cmdTimeout_ )
        {
            logger_.syslog( "failed to set local address", Syslog::FAULT );
            if( cmdTries_ > maxCmdTries_ )
            {
                this->setFailure( FailureMode::COMMUNICATIONS );
            }
            else
            {
                localAddressSettingSent_ = false;
            }
            return false;
        }
    }
    cmdTries_ = 0;
    return true;
}

bool DAT::sendCurrentRemoteAddress()
{
    if( !commandModeAcknowledged_ )
    {
        if( !enterCommandMode() )
        {
            commandModeSent_ = false;
            return false;
        }
        if( !commandModeAcknowledged_ )
        {
            return true;
        }
    }
    if( simulateHardware() )
    {
        currentRemoteAddressSent_ = currentRemoteAddressAcknowledged_ = true;
        return true;
    }
    if( !currentRemoteAddressSent_ )
    {
        logger_.syslog( "setting remote address to ", currentRemoteAddress_, Syslog::INFO );
        uart_ << "@remoteaddr=" << currentRemoteAddress_ << "\r";
        currentRemoteAddressSent_ = true;
        currentRemoteAddressAcknowledged_ = false;
        cmdTimeStart_ = Timestamp::Now();
        ++cmdTries_;
        return true;
    }
    if( currentRemoteAddressSent_ && !currentRemoteAddressAcknowledged_ )
    {
        logger_.syslog( "checking for remote address setting acknowledgment", Syslog::DEBUG );
        if( cmdTimeStart_.elapsed() > cmdTimeout_ )
        {
            logger_.syslog( "failed to set remote address", Syslog::FAULT );
            if( cmdTries_ > maxCmdTries_ )
            {
                this->setFailure( FailureMode::COMMUNICATIONS );
            }
            else
            {
                currentRemoteAddressSent_ = false;
            }
            return false;
        }
    }
    cmdTries_ = 0;
    return true;
}

/// Pause
Component::RunState DAT::pause()
{
    if( debug_ ) logger_.syslog( "Pause", Syslog::INFO );
    // TODO: Change pause to use lowpower state of modem
    return PAUSED;
}


/// Should eventually follow a PAUSE request: should set continueTime
Component::RunState DAT::paused()
{
    if( debug_ ) logger_.syslog( "Paused", Syslog::INFO );
    if( keepPowerOn() || isDataRequested() ) return RESUME;
    if( !keepPowerOn() ) return STOP;
    return PAUSED;
}


Component::RunState DAT::resume()
{
    if( debug_ ) logger_.syslog( "Resume", Syslog::INFO );
    if( !simulateHardware() )
    {
        if( debug_ ) logger_.syslog( "sending wake-up to local modem", Syslog::DEBUG );
        // TODO: Change resume to wake up from lowpower state of modem
    }
    startTime_ = Timestamp::Now();
    return RESUMING;
}


Component::RunState DAT::resuming()
{
    if( debug_ ) logger_.syslog( "Resuming", Syslog::INFO );
    if( !simulateHardware() )
    {
        if( debug_ ) logger_.syslog( "confirming wake-up of local modem", Syslog::DEBUG );
        // TODO: Check for wake up from lowpower state of modem
    }
    return RUNNABLE;
}


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

    // Keep our config up-to-date
    readConfig();

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

    if( !simulateHardware() )
    {
        logVoltageAndCurrent();
    }

    // Reset flags set in readAndParseResponses()
    gotResponseNotReceived_ = false;
    gotRangeMessage_ = false;
    gotDirectionMessage_ = false;
    gotAcousticWakeup_ = false;
    gotDebugRxMessage_ = false;
    gotDebugTxMessage_ = false;
    gotRangeRequestMessage_ = false;
    gotAckMessage_ = false;

    receivePingTime_ = Timestamp::Now(); // Used to mark the beginning of runnable
    dataTimestamp_ = Timestamp::Now(); // Will be set to mark the actual time the DAT received an incoming chirp, but set here just in case it doesn't get set to something more accurate this time around.

    // Deal with incoming data
    readAndParseResponses();

    // Allow tx power setting to change dynamically
    if( !txPowerSettingAcknowledged_ )
    {
        if( !sendTxPowerFromCfg() )
        {
            // TODO: This may be too harsh.  Maybe try a few times?
            return STOP;
        }
        if( !txPowerSettingAcknowledged_ )
        {
            return RUNNABLE;
        }
    }

    // Outgoing modem comms
    if( belowSurfaceThreshold() &&
            ( isSendDataAvailable() || isCommsRequested() || NULL != outgoingCommsNow_ || !outgoingCommsBuffer_.isEmpty() ) )
    {
        switch( commsState_ )
        {
        case SENDING_FILL_BUFFER:
            if( debug_ ) logger_.syslog( Str( "************** SENDING_FILL_BUFFER **************\n" ), Syslog::INFO );
            sendingFillBuffer();
            break;
        case SENDING_TRANSMIT:
            if( debug_ ) logger_.syslog( Str( "************** SENDING_TRANSMIT **************\n" ), Syslog::INFO );
            sendingTransmit();
            if( onlineModeSent_ && !onlineModeAcknowledged_ && cmdTimeStart_.elapsed() <= cmdTimeout_ )
            {
                // If we're in the process of entering online mode, return so we don't issue any commands (below)
                return RUNNABLE;
            }
            break;
        case SENDING_TRANSMIT_VERIFY:
            if( debug_ ) logger_.syslog( Str( "************** SENDING_TRANSMIT_VERIFY **************\n" ), Syslog::INFO );
            sendingTransmitVerify();
            break;
        case SENDING_ACK_WAITING:
            if( debug_ ) logger_.syslog( Str( "************** SENDING_ACK_WAITING **************\n" ), Syslog::INFO );
            sendingAckWaiting();
            break;
        case SENDING_VERIFIED:
            if( debug_ ) logger_.syslog( Str( "************** SENDING_VERIFIED **************\n" ), Syslog::INFO );
            sendingVerified();
            break;
        }
    }

    if( gotRangeMessage_ && gotDirectionMessage_ ) // This indicates that we have solicited a response
    {
        numPingsReceived_ += 1;
        if( verbosity_ > 1 ) logger_.syslog( "#Rx " + Str( numPingsReceived_ ) + ": Read range and direction messages.", Syslog::INFO );
        assembleDirectionInVehicleFrame();
        directionsToAngles();
        publishData();
        this->resetFailCount();
        rangeRequestPending_ = false; // we've gotten a response and the range request cycle is done
    }
    else if( gotRangeMessage_ ) // A range only but no USBL information
    {
        numPingsReceived_ += 1;
        if( verbosity_ > 0 ) logger_.syslog( "#Rx " + Str( numPingsReceived_ ) + ": Read range message, but no direction.", Syslog::ERROR );
        this->resetFailCount();
        publishData();
        rangeRequestPending_ = false; // we've gotten a response and the range request cycle is done
    }
    else if( gotDirectionMessage_ ) // USBL info only means we received a chirp
    {
        numPingsReceived_ += 1;
        if( verbosity_ > 0 ) logger_.syslog( "#Rx " + Str( numPingsReceived_ ) + ": Read direction message, but no range.", Syslog::INFO );
        range_ = nanf( "" );
        assembleDirectionInVehicleFrame();
        directionsToAngles();
        publishData(); // still want to record the rx time and direction data to the slate
        this->resetFailCount();
        // Not setting rangeRequestPending_ to false here becaues until a valid range is returned the ATR command won't satisfy. We have to wait to ping
        // again until we get a response not received or the timeout expires.
    }
    if( gotAcousticWakeup_ || gotDebugTxMessage_ || gotRangeRequestMessage_ || gotAckMessage_ )
    {
        publishData();
        this->resetFailCount();
    }
    else if( gotResponseNotReceived_ )
    {
        logger_.syslog( "No response from remote modem.", Syslog::ERROR );
        this->resetFailCount();
        rangeRequestPending_ = false; // we've gotten a response and the range request cycle is done
    }

    if( numPingsReceived_ == 1 )
    {
        remoteResponseTime_ = receivePingTime_ - transmitPingTime_; // Log how long it took for the first response to come in. Useful mainly for USBL/oneway mode.
    }

    if( transponderAddressCfgSetting_ >= 0 && gotNewQuery() ) // if you read a full query since the last cycle, try to ping again
    {
        if( !commandModeAcknowledged_ )
        {
            if( !enterCommandMode() )
            {
                commandModeSent_ = false;
            }
            if( !commandModeAcknowledged_ )
            {
                return RUNNABLE;
            }
        }
        if( currentRemoteAddress_ != transponderAddressCfgSetting_ )
        {
            currentRemoteAddress_ = transponderAddressCfgSetting_;
            currentRemoteAddressSent_ = currentRemoteAddressAcknowledged_ = false;
        }
        if( !currentRemoteAddressAcknowledged_ )
        {
            if( !sendCurrentRemoteAddress() )
            {
                currentRemoteAddressSent_ = false;
            }
            if( !currentRemoteAddressAcknowledged_ )
            {
                return RUNNABLE;
            }
        }

        // Let's see if it's time to request a range
        if( !rangeRequestPending_ || ( transmitPingTime_.elapsed() > acousticResponseTimeout_ ) )
        {
            numPingsReceived_ = 0;
            requestRange( remoteAddressRequested_, numPingsRequested_ ); // requestRange will write the address and number of pings requested to the slate
        }
        else
        {
            if( verbosity_ > 2 ) logger_.syslog( "received new query, but waiting for acoustic response period to elapse", Syslog::INFO );
        }
    }

    // See if we should request a device enable set/clear
    if( isDeviceEnableRequested() )
    {
        // send request once and don't interfere with any range requests that may be happening
        if( !deviceEnableRequested_ && !rangeRequestPending_ )
        {
            requestDeviceEnableSet();
        }
    }
    else // Clear the device enable on the remote
    {
        if( deviceEnableRequested_ && !rangeRequestPending_ )
        {
            requestDeviceEnableClr();
        }
    }

    return RUNNABLE;
}


Component::RunState DAT::stop()
{
    if( debug_ ) logger_.syslog( "Stop", Syslog::INFO );
    uninitialize(); // First power down then query for faults next cycle
    startTime_ = Timestamp::Now();
    return STOPPING;
}


Component::RunState DAT::stopping()
{
    if( debug_ ) logger_.syslog( "Stopping", Syslog::INFO );
    if( !simulateHardware() )
    {
        loadControl_.readFaults(); // See if anything went wrong that may have caused this request for uninitialize
        if( loadControl_.hasError() )
        {
            logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }

    }
    return STOPPED;
}


Component::RunState DAT::stopped()
{
    if( debug_ ) logger_.syslog( "Stopped", Syslog::INFO );
    if( keepPowerOn() || 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 DAT::sendingFillBuffer()
{
    // First see if we have sbds to send.
    if( isCommsRequested() )
    {
        Str sendFilename = Supervisor::GetToShoreFilename( Service::SERVICE_COURIER );
        if( sendFilename == Str::EMPTY_STR && sendExpressCfgSetting_ )
        {
            sendFilename = Supervisor::GetToShoreFilename( Service::SERVICE_EXPRESS );
        }
        if( sendFilename != Str::EMPTY_STR && sendFilename != sendPacket_.sendFilename_ )
        {
            // The timestamp is common to all Logs since a restart
            Str timestampStr( sendFilename.substr( 5, 15 ) );
            Timestamp sendTimestamp = Timestamp( timestampStr.cStr() );
            sendPacket_.timeT_ = ( uint32_t )sendTimestamp.asTimeT();
            // The index is pertinent to the chunk (lzma file) of Logs being sent
            sendPacket_.index_ = atoi( sendFilename.substr( sendFilename.length() - 9, 4 ).cStr() );
            struct stat st;
            if( 0 == stat( sendFilename.cStr(), &st ) )
            {
                sendPacket_.sendFilesize_ = st.st_size;
            }
            else
            {
                logger_.syslog( "Could not stat file " + sendFilename, Syslog::IMPORTANT );
                sendPacket_.sendFilesize_ = 0;
            }
            sendPacket_.packetsLeft_ = ( sendPacket_.sendFilesize_ - 1 )
                                       / MAX_DOWNLINK_DATA_SIZE;
            char sentName[] = "Logs/YYYYMMDDTHHMMSS/shore####.lzma.parts/####.sbd...";
            snprintf( sentName, sizeof( sentName ) - 1, "%s.parts", sendFilename.cStr() );
            if( 0 == access( sentName,  X_OK ) )
            {
                // Don't send packets that have already been sent.
                while( sendPacket_.packetsLeft_ > 0 )
                {
                    snprintf( sentName, sizeof( sentName ) - 1, "%s.parts/%04d.sbd", sendFilename.cStr(), sendPacket_.packetsLeft_ );
                    if( 0 == access( sentName, F_OK ) )
                    {
                        -- sendPacket_.packetsLeft_;
                    }
                    else
                    {
                        break;
                    }
                }
            }
        }
        sendPacket_.sendFilename_ = sendFilename;
    }
    else
    {
        sendPacket_.sendFilename_ = Str::EMPTY_STR;
    }

    if( sendPacket_.sendFilename_ == Str::EMPTY_STR )
    {
        platformCommunicationsWriter_->write( Units::BOOL, true );
    }
    else
    {
        FileInStream inStream( sendPacket_.sendFilename_.cStr() );
        if( inStream.isReadable() )
        {
            uint16_t maxPacket = ( sendPacket_.sendFilesize_ - 1 )
                                 / MAX_DOWNLINK_DATA_SIZE;
            size_t sendPosition = ( maxPacket - sendPacket_.packetsLeft_ ) * MAX_DOWNLINK_DATA_SIZE;
            inStream.seek( sendPosition );

            sendPacket_.dataSize_ = inStream.read( ( char* )sendPacket_.data_,
                                                   MAX_DOWNLINK_DATA_SIZE ).bytesRead();
            // Never send 30 bytes, or we will be mistaken for an emergency packet!
            // Add one more byte.
            if( sendPacket_.dataSize_ + SENDPACKET_HEADER_SIZE == 30 )
            {
                sendPacket_.data_[sendPacket_.dataSize_] = 0;
                ++sendPacket_.dataSize_;
            }
            outgoingCommsBuffer_.insert( 0, new OutgoingComms( ( char* )&sendPacket_, sendPacket_.dataSize_ + SENDPACKET_HEADER_SIZE, sbdAddressCfgSetting_, OUTGOING_SBD ) );
            if( verbosity_ > 0 )
            {
                logger_.syslog( "#Outgoing data=", ( int )outgoingCommsBuffer_.size(), Syslog::INFO );
            }
            // Start the clock and move to next state

        }
        else // Entry is null?
        {
            logger_.syslog( "Could not open file " + sendPacket_.sendFilename_, Syslog::CRITICAL );
            sendPacket_.sendFilename_ = Str::EMPTY_STR;

            //TODO: Need to tell supervisor to skip the file in the future

        }
    }

    // TODO: implement sbd resend logic


    // Any SendData calls?
    if( isSendDataAvailable() )
    {
        SendData* data = sendDataBuffer_->pop();
        if( NULL != data )
        {
            SendDestination* destination = data->getDestination();
            if( NULL != destination && NULL != data->getValue() )
            {
                Str cmd;
                DataValue& dataVal = *data->getValue();
                if( destination->getPath() != Str::EMPTY_STR )
                {
                    cmd = "set " + destination->getPath() + " ";
                    if( NULL != data->getUnit() )
                    {
                        cmd += dataVal.toString( *data->getUnit() ) + " " + data->getUnit()->getName();
                    }
                    else
                    {
                        cmd += "string \"" + dataVal.toString() + "\"";
                    }
                }
                else
                {
                    cmd = dataVal.toString();
                }
                logger_.syslog( destination->getScheme() + "://" + destination->getHost() + ": " + cmd, Syslog::INFO );

                if( cmd != Str::EMPTY_STR )
                {
                    outgoingCommsBuffer_.push( new OutgoingComms( cmd, destination->getHostId(), OUTGOING_DATA ) );
                    if( verbosity_ > 0 )
                    {
                        logger_.syslog( "#Outgoing data=", ( int )outgoingCommsBuffer_.size(), Syslog::INFO );
                    }
                }

                delete data;
            }
        }
    }

    // Deal with data packets queued for delivery
    if( NULL == outgoingCommsNow_ && !outgoingCommsBuffer_.isEmpty() )
    {
        outgoingCommsNow_ = outgoingCommsBuffer_.pop( 0 );
    }

    if( NULL != outgoingCommsNow_ )
    {
        commsState_ = SENDING_TRANSMIT;
        logger_.syslog( "In sendingFillBuffer, set commsState_ = SENDING_TRANSMIT", Syslog::DEBUG );
    }

}

void DAT::sendingTransmit()
{
    if( NULL == outgoingCommsNow_ )
    {
        commsState_ = SENDING_FILL_BUFFER;
        logger_.syslog( "In sendingTransmit, NULL == outgoingCommsNow_ so set commsState_ = SENDING_FILL_BUFFER", Syslog::DEBUG );
        return;
    }

    if( currentRemoteAddress_ != outgoingCommsNow_->getAddress() )
    {
        currentRemoteAddress_ = outgoingCommsNow_->getAddress();
        currentRemoteAddressSent_ = false;
        currentRemoteAddressAcknowledged_ = false;
    }

    if( !currentRemoteAddressAcknowledged_ )
    {
        if( !sendCurrentRemoteAddress() )
        {
            currentRemoteAddressSent_ = false;
            logger_.syslog( "Failure setting remote address to ", currentRemoteAddress_, Syslog::ERROR );
            return;
        }
        if( !currentRemoteAddressAcknowledged_ )
        {
            return;
        }
    }

    if( !onlineModeAcknowledged_ )
    {
        if( !enterOnlineMode() )
        {
            logger_.syslog( Str( "Failure returning to online mode" ), Syslog::FAULT );
            this->setFailure( FailureMode::COMMUNICATIONS );
            onlineModeSent_ = false;
        }
        if( !onlineModeAcknowledged_ )
        {
            return;
        }
    }
    if( awaitingAckTransmitAck_ )
    {
        return;
    }

    if( simulateHardware() )
    {
        SimSlate::SendMessage( currentRemoteAddress_, localAddressCfgSetting_, outgoingCommsNow_->getData() );
    }
    else
    {
        uart_ << outgoingCommsNow_->getData();
    }

    startTime_ = Timestamp::Now();
    commsState_ = SENDING_TRANSMIT_VERIFY;
    logger_.syslog( "In sendingTransmit, set commsState_ = SENDING_TRANSMIT_VERIFY", Syslog::DEBUG );
}

void DAT::sendingTransmitVerify()
{
    if( startTime_.elapsed() > acousticResponseTimeout_ )
    {
        if( simulateHardware() )
        {
            commsState_ = SENDING_VERIFIED;
        }
        else
        {
            logger_.syslog( Str( "Buffer send receipt timeout failure." ), Syslog::FAULT );
            commsState_ = SENDING_FILL_BUFFER;
            onlineModeAcknowledged_ = onlineModeSent_ = false;
            logger_.syslog( "In sendingTransmitVerify, timeout so go online and set commsState_ = SENDING_FILL_BUFFER", Syslog::DEBUG );
        }
    }
}

void DAT::sendingAckWaiting()
{
    int dataXferTime = ( int ) sendPacket_.dataSize_ / 32; // Calculate time to actually send the data. Assume roughly 300 bps acomms xfer rate
    if( startTime_.elapsed() > ( acousticResponseTimeout_ + dataXferTime ) )
    {
        ++ackTimeouts_;
        if( maxAckTimeoutsCfgSetting_ > 0 && ackTimeouts_ >= maxAckTimeoutsCfgSetting_ )
        {
            commsState_ = SENDING_VERIFIED;
            ackTimeouts_ = 0; // reset the timeouts in case there are multiple variables in the queue.
            logger_.syslog( "In sendingAckWaiting, hit max timeouts so set commsState_ = SENDING_VERIFIED", Syslog::DEBUG );
            logger_.syslog( "Ack receipt timeout failure.", Syslog::ERROR );
        }
        else
        {
            commsState_ = SENDING_FILL_BUFFER;
            logger_.syslog( "In sendingAckWaiting, timeout so set commsState_ = SENDING_FILL_BUFFER", Syslog::DEBUG );
        }
    }
}

void DAT::sendingVerified()
{
    this->resetFailCount();
    if( outgoingCommsNow_->getCommsType() == OUTGOING_SBD )
    {
        if( sendPacket_.sendFilename_ != Str::EMPTY_STR )
        {
            char sentName[] = "Logs/YYYYMMDDTHHMMSS/shore####.lzma.parts/####.sbd...";
            snprintf( sentName, sizeof( sentName ), "%s.parts", sendPacket_.sendFilename_.cStr() );
            logger_.syslog( "Sent " + Str( sendPacket_.dataSize_, 10 ) + " bytes from file " + sentName, Syslog::INFO );
            logger_.syslog( "Packets left to send: " + Str( sendPacket_.packetsLeft_, 10 ), Syslog::INFO );
            int error = mkdir( sentName, ACCESSPERMS );
            if( error && EEXIST != errno )
            {
                printf( "Error opening sent sbd directory due to error %d: %s\n", errno, strerror( errno ) );
            }
            snprintf( sentName, sizeof( sentName ) - 1, "%s.parts/%04d.sbd", sendPacket_.sendFilename_.cStr(), sendPacket_.packetsLeft_ );
            FileOutStream outStream( sentName );
            outStream.write( ( char* )&sendPacket_, sendPacket_.dataSize_ + SENDPACKET_HEADER_SIZE );
            if( debug_ ) logger_.syslog( "Stored copy of sent data in ", sentName, Syslog::DEBUG );
            // Are done sending the file?
            if( sendPacket_.packetsLeft_ == 0 )
            {
                Str sendFilenameBak = sendPacket_.sendFilename_ + ".bak";
                if( debug_ ) logger_.syslog( "Completed sending " + sendPacket_.sendFilename_, Syslog::DEBUG );
                rename( sendPacket_.sendFilename_.cStr(), sendFilenameBak.cStr() );
            }
            else
            {
                // Increment our position in the file
                --sendPacket_.packetsLeft_;
            }
        }
        /*else if( NULL != resendSBDFilename_ )
        {
            logger_.syslog( Str( "Re-sent " ) + Str( sendPacket_.dataSize_, 10 ) + " bytes from file " + resendSBDFilename_, Syslog::INFO );
        }*/

        // See if there's anything else to send
        if( ( sendPacket_.sendFilename_ != Str::EMPTY_STR
                && isDataRequested() )
                /*|| NULL != resendSBDFilename_ */ )
        {
            commsState_ = SENDING_FILL_BUFFER;
            logger_.syslog( "In sendingVerified, sbd waiting so set commsState_ = SENDING_FILL_BUFFER", Syslog::DEBUG );

            /*
            if( NULL != resendSBDFilename_ )
            {
                delete[] resendSBDFilename_;
                resendSBDFilename_ = NULL;
            }
            */
        }
        // We're all done with comms - no uplinks available and our queue is empty
        else
        {
            commsState_ = SENDING_FILL_BUFFER;
            logger_.syslog( "In sendingVerified, sbd done so set commsState_ = SENDING_FILL_BUFFER", Syslog::DEBUG );
        }
    }
    // Not much to do when sending ordinary data
    else
    {
        commsState_ = SENDING_FILL_BUFFER;
        logger_.syslog( "In sendingVerified, data done so set commsState_ = SENDING_FILL_BUFFER", Syslog::DEBUG );
    }

    if( commsState_ == SENDING_FILL_BUFFER )
    {
        delete outgoingCommsNow_;
        outgoingCommsNow_ = NULL;
    }

}

// Returns true if below surface (as defined in Vertical Control)
bool DAT::belowSurfaceThreshold( void ) // TODO: This could be a superclass method for radio components.
{
    float depth;
    return depthReader_->isActive() && depthReader_->read( Units::METER, depth ) && ( depth > surfaceThresholdCfgSetting_ );
}


// Log voltage and current and check for any faults
void DAT::logVoltageAndCurrent() // TODO: Elevate to superclass
{
    loadControl_.requestVoltageAndCurrent();
    if( loadControl_.hasError() )
    {
        logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
        stop();
    }
}


bool DAT::keepPowerOn()
{
    // always leave the modem and software interface on so that we can ping
    // from a search platform and so that acoustic timeouts work for missions
    // without awkward readData requirements on Insert aggregates.
    // TODO: Come back and figure out how to let the modem enter sleep mode,
    // but make sure the software interface starts running again if the modem
    // is woken by a remote request
    return true;
}


bool DAT::isCommsRequested()
{
    return sbdAddressCfgSetting_ >= 0 && sendDataToShoreCfgSetting_ && platformCommunicationsWriter_->isAnyDataRequested() && belowSurfaceThreshold();
}

bool DAT::isSendDataAvailable()
{
    return !sendDataBuffer_->isEmpty();;
}

bool DAT::isDataRequested()
{
    return isCommsRequested()
           || isSendDataAvailable()
           || rxTimeWriter_->isDataRequested()
           || txTimeWriter_->isDataRequested()
           || directionToContactWriter_->isDataRequested()
           || contactAddressWriter_->isDataRequested()
           || rangeToContactWriter_->isDataRequested()
           || phaseAWriter_->isDataRequested()
           || phaseBWriter_->isDataRequested()
           || phaseCWriter_->isDataRequested()
           || tDirectionWriter_->isDataRequested()
           || localAddressWriter_->isDataRequested();
}


bool DAT::isDeviceEnableRequested()
{
    return deviceEnableRequestedWriter_->isDataRequested();
}


bool DAT::gotNewQuery()
{
    float acousticResponseTimeout;
    if( numPingsReceived_ < 2 ) // For single ping cases (i.e. non-oneway/USBL mode) we'll be very patient for the first response from the remote modem
    {
        acousticResponseTimeout = acousticResponseTimeout_.asFloat();
    }
    else // Here we've asked for more than 1 ping and received at least one response so we won't wait long for subsequent ones as they should be on a 1Hz schedule.
    {
        acousticResponseTimeout = ( numPingsRequested_ * 0.5 ) + remoteResponseTime_.asFloat(); // Total wait time from TX is number of pings * duty cycle (0.5 sec) plus the time it took for the first ping to arrive.
    }

    if( ( numPingsReceived_ < numPingsRequested_ ) && ( transmitPingTime_.elapsed().asFloat() < acousticResponseTimeout ) )
    {
        if( verbosity_ > 2 ) logger_.syslog( "checking for new query: numPingsReceived=" + Str( numPingsReceived_ ) + ", elapsed TxPingTime=" + Str( transmitPingTime_.elapsed().asFloat() ), Syslog::INFO );
        return false; // Explicitly avoid new interrogations while waiting for response to previous.
    }
    else if( queryAddressRequestedReader_->isActive()
             && numberOfPingsRequestedReader_->isActive()
             && queryAddressRequestedReader_->getTimestamp() > transmitPingTime_
             && numberOfPingsRequestedReader_->getTimestamp() > transmitPingTime_ )
    {
        bool validAddress = queryAddressRequestedReader_->read( Units::COUNT, remoteAddressRequested_ );
        bool validPingsRequested = numberOfPingsRequestedReader_->read( Units::COUNT, numPingsRequested_ );
        // TODO: Filter valid addresses and ping requests here.
        if( verbosity_ > 0 )
        {
            if( validAddress ) logger_.syslog( "****** received valid address query ******", Syslog::INFO );
            else logger_.syslog( "****** received invalid address query ******", Syslog::INFO );

            if( validPingsRequested ) logger_.syslog( "****** received valid ping request ******", Syslog::INFO );
            else logger_.syslog( "****** received invalid ping request ******", Syslog::INFO );
        }
        oneWayRequested_ = false;
        return validAddress && validPingsRequested;
    }
    else
    {
        oneWayRequested_ = false;
        return false;
    }
}


void DAT::requestRange( const int remoteAddress, const int numPings = 1 )
{
    if( numPings < 1 )
    {
        if( debug_ ) logger_.syslog( "Received request for less than 1 ping -- ignoring.", Syslog::DEBUG );
    }
    else
    {
        if( !commandModeAcknowledged_ )
        {
            if( !enterCommandMode() )
            {
                commandModeSent_ = false;
                return;
            }
            if( !commandModeAcknowledged_ )
            {
                return;
            }
        }
        if( numPings == 1 )
        {
            if( verbosity_ > 0 ) logger_.syslog( "Querying Benthos address " + Str( remoteAddress ) + " with one ping in standard two-way mode.", Syslog::INFO );
            if( !simulateHardware() )
            {
                uart_ << "atr" << remoteAddress << "\r";
            }
            else
            {
                if( SimSlate::SendRangeRequst( remoteAddress, localAddressCfgSetting_ ) )
                {
                    transmitPingTime_ = Timestamp::Now();
                }
            }

        }
        else if( !oneWayRequested_ ) // ( numPings > 1 )
        {
            if( verbosity_ > 0 ) logger_.syslog( "Querying Benthos address " + Str( remoteAddress ) + " with " + Str( numPings ) + " pings in terminal homing one-way mode.", Syslog::INFO );
            if( !simulateHardware() )
            {
                uart_ << "usbl " << remoteAddress << " " << numPings << "\r"; // oneway command expects both modem and DAT to have @syncpps set to RTC (2).
            }
            else
            {
                if( SimSlate::SendRangeRequst( remoteAddress, localAddressCfgSetting_, numPings ) )
                {
                    transmitPingTime_ = Timestamp::Now();
                }
            }

            oneWayRequested_ = true;
        }
        rangeRequestPending_ = true;
    }
}


void DAT::requestDeviceEnableSet()
{
    // First enter command mode
    if( !commandModeAcknowledged_ )
    {
        if( !enterCommandMode() )
        {
            commandModeSent_ = false;
            return;
        }
        if( !commandModeAcknowledged_ )
        {
            return;
        }
    }
    if( verbosity_ > 0 ) logger_.syslog( "Requesting device enable set for address " + Str( remoteAddressRequested_ ) + ".", Syslog::IMPORTANT );
    if( !simulateHardware() ) uart_ << "AT$X" << remoteAddressRequested_ << ",1\r";
    deviceEnableRequested_ = true;
}


void DAT::requestDeviceEnableClr()
{
    // First enter command mode
    if( !commandModeAcknowledged_ )
    {
        if( !enterCommandMode() )
        {
            commandModeSent_ = false;
            return;
        }
        if( !commandModeAcknowledged_ )
        {
            return;
        }
    }
    if( verbosity_ > 0 ) logger_.syslog( "Requesting device enable clr for address " + Str( remoteAddressRequested_ ) + ".", Syslog::IMPORTANT );
    if( !simulateHardware() ) uart_ << "AT$X" << remoteAddressRequested_ << ",0\r";
    deviceEnableRequested_ = false;
}


// Should return [myNamespace]::SIMULATE_HARDWARE, or [myNamespace]::POWER, etc
ConfigURI DAT::getConfigURI( ConfigOption configOption ) const
{
    switch( configOption )
    {
    case CONFIG_SIMULATE_HARDWARE:
        return DATIF::SIMULATE_HARDWARE;
    case CONFIG_MISSION_CRITICAL:
        return DATIF::MISSION_CRITICAL;
    default:
        return ConfigURI::NO_CONFIG_URI;
    }
}

bool DAT::readAndParseResponses( void )
{
    bool gotLines = false;
    if( simulateHardware() )
    {
        // For now mimic the hardware and simply clear range messages that are backed up in the queue
        while( getSimulatedMeasurements() )
        {
            gotLines = true;
        }
    }
    else
    {
        while( uart_.canReadUntil( '\n' ) )
        {
            uart_.readUntil( deviceResponse_, sizeof( deviceResponse_ ), '\n' );

            if( verbosity_ > 1 )
            {
                logger_.syslog( "DAT read: ", deviceResponse_, Syslog::INFO );
            }

            if( uart_.hasError() )
            {
                logger_.syslog( "DAT uart error: ", uart_.errorString(), Syslog::FAULT );
                this->setFailure( FailureMode::COMMUNICATIONS );
            }
            parseResponses();
            gotLines = true;
        }
    }
    skipNextError_ = false;
    return gotLines;
}


void DAT::parseResponses()
{
    size_t data_bytes = 0; // #comms bytes expected
    int tmp;
    if( dataBytesReceiving_ > 0 || 1 == sscanf( deviceResponse_, "DATA(%4zu):", &data_bytes ) )
    {
        if( verbosity_ > 0 )
        {
            logger_.syslog( "Got DATA ", ( int )data_bytes, Syslog::INFO );
        }

        if( data_bytes > 0 )
        {
            dataBytesReceiving_ = data_bytes;
            dataBytesReceived_ = 0;
        }

        // Ignore extra bytes in pkt (typically trailing "\n")
        size_t pktDataBytes = AuvMath::Min( dataBytesReceiving_, uart_.bytesRead() - 11 );

        // Don't overflow comms buffer
        pktDataBytes = AuvMath::Min( pktDataBytes, ( size_t )( DAT_MAX_RCV - dataBytesReceived_ ) );

        // Move received data into comms buffer
        memmove( commsData_ + dataBytesReceived_, deviceResponse_ + sizeof( "DATA(XXXX)" ), pktDataBytes );

        dataBytesReceived_ += pktDataBytes;
        dataBytesReceiving_ -= pktDataBytes;
        commsData_[dataBytesReceived_] = 0;

        incomingRemoteAddress_ = incomingLocalAddress_ = -1;
        return;
    }
    else if( strstr( deviceResponse_, "Source:" ) && strstr( deviceResponse_, "Destination:" ) )
    {
        if( verbosity_ > 0 )
        {
            logger_.syslog( "Got Src/Dest after DATA ", Syslog::INFO );
        }

        if( 2 == sscanf( deviceResponse_, "Source:%3d  Destination:%3d:", &incomingRemoteAddress_, &incomingLocalAddress_ ) )
        {
            if( verbosity_ > 0 )
            {
                logger_.syslog( Str( "DATA Src=" ) + incomingRemoteAddress_ + ", Dst=" + incomingLocalAddress_, Syslog::INFO );
            }
            return;
        }
        else
        {
            logger_.syslog( "Could not parse Src/Dst in ", deviceResponse_, Syslog::FAULT );
        }
    }
    else if( strstr( deviceResponse_, "CRC:Pass" ) )
    {
        if( verbosity_ > 0 )
        {
            logger_.syslog( "Got CRC:Pass", Syslog::INFO );
        }
        if( incomingLocalAddress_ == localAddressCfgSetting_ || 255 == incomingLocalAddress_ )
        {
            if( verbosity_ > 0 )
            {
                logger_.syslog( "Got CRC:Pass", Syslog::INFO );
            }
            if( ( dataBytesReceiving_ == 0 && dataBytesReceived_ > 0 ) || dataBytesReceived_ == DAT_MAX_RCV )
            {
                if( verbosity_ > 0 )
                {
                    logger_.syslog( "Incoming data is intended for us", Syslog::INFO );
                }

                while( dataBytesReceived_ >= 2 &&
                        ( 0 == strncmp( commsData_, "~~", 2 ) || 0 == strncmp( commsData_ + dataBytesReceived_ - 2, "~~", 2 ) ) )
                {
                    if( SENDING_ACK_WAITING == commsState_ || SENDING_TRANSMIT_VERIFY == commsState_ )
                    {
                        commsState_ = SENDING_VERIFIED;
                        ackTimeouts_ = 0;
                        logger_.syslog( "In parseResponses, got ack so set commsState_ = SENDING_VERIFIED", Syslog::DEBUG );
                    }
                    logger_.syslog( "Got ack", Syslog::INFO );
                    gotAckMessage_ = true;
                    dataBytesReceived_ -= 2;
                    if( 0 == strncmp( commsData_, "~~", 2 ) )
                    {
                        memmove( commsData_, commsData_ + 2, dataBytesReceived_ );
                    }
                    commsData_[dataBytesReceived_] = 0;
                }

                bool parsed = true;
                if( dataBytesReceived_ > 0 && incomingRemoteAddress_ == sbdAddressCfgSetting_ )
                {
                    // This is maybe an SBD packet
                    Str errorMsg = DataReceiver::TryReceiveSbd( ( const char* )commsData_, dataBytesReceived_, keyText_.asString().cStr(), logger_ );

                    // Not an SBD... maybe text?
                    if( errorMsg != Str::EMPTY_STR )
                    {
                        for( size_t i = 0; i < dataBytesReceived_ && parsed; ++i )
                        {
                            parsed &= isprint( commsData_[i] ) || commsData_[i] == '\n' || commsData_[i] == '\r' || commsData_[i] == '\t';
                        }
                        if( parsed )
                        {
                            DataReceiver::Receive( ( const char* )commsData_, dataBytesReceived_, logger_ );
                        }
                    }

                    if( !parsed )
                    {
                        // Parse failed
                        Str hex;
                        StrIOStream hexStream( hex );
                        hexStream.writeHex( ( const char* )commsData_, dataBytesReceived_ );
                        logger_.syslog( errorMsg, Syslog::FAULT );
                        logger_.syslog( Str( "Failed to parse uplink message:" ) + hex, Syslog::FAULT );
                    }
                }
                else if( dataBytesReceived_ > 0 )
                {
                    // This is a non-SBD packet
                    DataReceiver::Receive( ( const char* )commsData_, dataBytesReceived_, logger_ );
                }

                if( dataBytesReceived_ > 0 && parsed && incomingLocalAddress_ == localAddressCfgSetting_ )
                {
                    // Send acknowledgement
                    if( onlineModeAcknowledged_ )
                    {
                        uart_ << "~~";
                        awaitingAckTransmitAck_ = true;
                    }
                    else
                    {
                        outgoingCommsBuffer_.insert( 0, new OutgoingComms( "~~", 2, incomingRemoteAddress_, OUTGOING_ACK ) );
                        if( verbosity_ > 0 )
                        {
                            logger_.syslog( "#Outgoing data=", ( int )outgoingCommsBuffer_.size(), Syslog::INFO );
                        }
                    }
                    logger_.syslog( "Sending ack", Syslog::INFO );
                }

                this->resetFailCount();
            }
            else
            {
                logger_.syslog( "Got message confirmation too early", Syslog::ERROR );
            }
        }
        else if( verbosity_ > 1 )
        {
            logger_.syslog( Str( "Ignoring message sent to address" ) + incomingLocalAddress_, Syslog::INFO );
        }
        dataBytesReceiving_ = dataBytesReceived_ = 0;
        return;
    }


    // TODO: Record full raw response for debugging.
    //cleanResponse();
    else if( strstr( deviceResponse_, "Teledyne Benthos" ) )
    {
        /* Ignore */
    }
    else if( strstr( deviceResponse_, "Frequency Band" ) )
    {
        /* Ignore */
    }
    else if( strstr( deviceResponse_, "version" ) )
    {
        /* Ignore */
    }
    // Here we ignore the initial date string that shows up upon boot. Also they forgot the comma. Just saying....
    else if( sscanf( deviceResponse_, "%*c%*c%*c %*d %d %*d:%*d:%*d", &tmp ) == 1 )
    {
        /* Ignore: Dec 14 2021 00:31:03 */
    }
    // But we don't ignore the slightly different time/date string that shows up with day of week when you set the clock.
    // Okay okay we get it, Benthos knows how to use a calendar.
    // Tue Dec 14, 2021  00:29:52
    else if( sscanf( deviceResponse_, "%*c%*c%*c %*c%*c%*c %*d, %d  %*d:%*d:%*d", &tmp ) == 1 )
    {
        if( localTimeSent_ ) // Let's only set variables if we actually set the clock.
        {
            logger_.syslog( "Local DAT time set to ", deviceResponse_, Syslog::INFO );
            localTimeSetAcknowledged_ = true;
        }
    }
    else if( strstr( deviceResponse_, "Features enabled" ) )
    {
        /* Ignore */
    }
    else if( strstr( deviceResponse_, "CONNECT" ) && strstr( deviceResponse_, "bits/sec" ) )
    {
        if( sscanf( deviceResponse_, "CONNECT %d bits/sec", &commRate_ ) == 1 )
        {
            logger_.syslog( "commRate: ", commRate_, Syslog::INFO );
            connectTime_ = Timestamp::Now();
        }
        else
        {
            commRate_ = 1;
            logger_.syslog( "invalid communications rate; deviceResponse_:", deviceResponse_, Syslog::FAULT );
            this->setFailure( FailureMode::COMMUNICATIONS );
        }
        if( onlineModeSent_ && !onlineModeAcknowledged_ )
        {
            onlineModeAcknowledged_ = true;
            logger_.syslog( "online mode acknowledged", Syslog::INFO );
        }
    }
    else if( strstr( deviceResponse_, "Verbose         | " ) )
    {
        if( verboseSettingSent_ && !verboseSettingAcknowledged_ )
        {
            int verboseRead;
            if( sscanf( deviceResponse_, "Verbose         | %d", &verboseRead ) == 1 )
            {
                if( verboseCfgSetting_ == verboseRead )
                {
                    logger_.syslog( "set verbose to ", verboseRead, Syslog::INFO );
                    verboseSettingAcknowledged_ = true;
                }
                else
                {
                    logger_.syslog( "failed to set verbose to " + Str( verboseCfgSetting_ ) + ", device returned " + Str( verboseRead ) + " instead.", Syslog::ERROR );
                    this->setFailure( FailureMode::COMMUNICATIONS );
                }
            }
            else
            {
                logger_.syslog( "failed to set verbose to " + Str( verboseCfgSetting_ ) + ".", Syslog::ERROR );
                this->setFailure( FailureMode::COMMUNICATIONS );
            }
        }
    }
    else if( strstr( deviceResponse_, "DatVerbose      | " ) )
    {
        if( datVerboseSent_ && !datVerboseAcknowledged_ )
        {
            int datVerboseRead;
            if( sscanf( deviceResponse_, "DatVerbose      | %d", &datVerboseRead ) == 1 )
            {
                if( datVerbose_ == datVerboseRead )
                {
                    logger_.syslog( "set DatVerbose to ", datVerboseRead, Syslog::INFO );
                    datVerboseAcknowledged_ = true;
                }
                else
                {
                    logger_.syslog( "failed to set DatVerbose to " + Str( datVerbose_ ) + ", device returned " + Str( datVerboseRead ) + " instead.", Syslog::ERROR );
                    this->setFailure( FailureMode::COMMUNICATIONS );
                }
            }
            else
            {
                logger_.syslog( "failed to set DatVerbose to " + Str( datVerbose_ ) + ".", Syslog::ERROR );
                this->setFailure( FailureMode::COMMUNICATIONS );
            }
        }
    }
    else if( strstr( deviceResponse_, "TxPower         | " ) )
    {
        if( txPowerSettingSent_ && !txPowerSettingAcknowledged_ )
        {
            int txPower = 0;
            if( sscanf( deviceResponse_, "TxPower         | %d", &txPower ) == 1 )
            {
                if( txPowerCfgSetting_ == txPower )
                {
                    logger_.syslog( "set transmit power to ", txPower, Syslog::INFO );
                    txPowerSettingAcknowledged_ = true;
                }
                else
                {
                    logger_.syslog( "failed to set transmit power to " + Str( txPowerCfgSetting_ ) + ", device returned " + Str( txPower ) + " instead.", Syslog::ERROR );
                    this->setFailure( FailureMode::COMMUNICATIONS );
                }
            }
            else
            {
                logger_.syslog( "failed to set transmit power to " + Str( txPowerCfgSetting_ ) + ".", Syslog::ERROR );
                this->setFailure( FailureMode::COMMUNICATIONS );
            }
        }
    }
    else if( strstr( deviceResponse_, "LocalAddr       | " ) )
    {
        if( localAddressSettingSent_ && !localAddressSettingAcknowledged_ )
        {
            if( sscanf( deviceResponse_, "LocalAddr       | %d", &localAddress_ ) == 1 )
            {
                if( localAddressCfgSetting_ == localAddress_ )
                {
                    logger_.syslog( "set local address to ", localAddress_, Syslog::INFO );
                    localAddressSettingAcknowledged_ = true;
                }
                else
                {
                    logger_.syslog( "failed to set local address to " + Str( localAddressCfgSetting_ ) + ", device returned " + Str( localAddress_ ) + " instead.", Syslog::ERROR );
                    this->setFailure( FailureMode::COMMUNICATIONS );
                }
            }
            else
            {
                logger_.syslog( "failed to set local address to " + Str( localAddressCfgSetting_ ) + ".", Syslog::ERROR );
                this->setFailure( FailureMode::COMMUNICATIONS );
            }
        }
    }
    else if( strstr( deviceResponse_, "RemoteAddr      | " ) )
    {
        if( currentRemoteAddressSent_ && !currentRemoteAddressAcknowledged_ )
        {
            if( sscanf( deviceResponse_, "RemoteAddr      | %d", &remoteAddress_ ) == 1 )
            {
                if( currentRemoteAddress_ == remoteAddress_ )
                {
                    logger_.syslog( "set remote address to ", remoteAddress_, Syslog::INFO );
                    currentRemoteAddressAcknowledged_ = true;
                }
                else
                {
                    logger_.syslog( "failed to set remote address to " + Str( currentRemoteAddress_ ) + ", device returned " + Str( remoteAddress_ ) + " instead.", Syslog::ERROR );
                    this->setFailure( FailureMode::COMMUNICATIONS );
                }
            }
            else
            {
                logger_.syslog( "failed to set remote address to " + Str( currentRemoteAddress_ ) + ".", Syslog::ERROR );
                this->setFailure( FailureMode::COMMUNICATIONS );
            }
        }
    }
    else if( strstr( deviceResponse_, "Command '+++' not found" ) )
    {
        if( commandModeSent_ && !commandModeAcknowledged_ )
        {
            commandModeAcknowledged_ = true;
        }
        skipNextError_ = true;
    }
    else if( skipNextError_ && strstr( deviceResponse_, "Error" ) )
    {
        skipNextError_ = false;
    }
    else if( strstr( deviceResponse_, "UART Wakeup" ) )
    {
        // Do nothing
    }

    // Check for message transmitted
    else if( ( awaitingAckTransmitAck_ || commsState_ == SENDING_TRANSMIT_VERIFY ) && strstr( deviceResponse_, "Forwarding Delay UpTx" ) )
    {
        if( awaitingAckTransmitAck_ )
        {
            awaitingAckTransmitAck_ = false;
        }
        else if( currentRemoteAddress_ == 255 )
        {
            commsState_ = SENDING_VERIFIED;
            ackTimeouts_ = 0;
            logger_.syslog( "In parseResponses, remote == 255 so set commsState_ = SENDING_VERIFIED", Syslog::DEBUG );
        }
        else if( outgoingCommsNow_->getCommsType() == OUTGOING_ACK )
        {
            commsState_ = SENDING_VERIFIED;
            logger_.syslog( "In parseResponses, sent ack so set commsState_ = SENDING_VERIFIED", Syslog::DEBUG );
        }
        else
        {
            commsState_ = SENDING_ACK_WAITING;
            startTime_ = Timestamp::Now(); // Reset the clock for ack.
            logger_.syslog( "In parseResponses, set commsState_ = SENDING_ACK_WAITING", Syslog::DEBUG );
        }
    }
    // Check for acoustic wakeup
    else if( strstr( deviceResponse_, "Acoustic Wakeup\n" ) )
    {
        if( verbosity_ > 0 ) logger_.syslog( "got acoustic wakeup", Syslog::INFO );
        gotAcousticWakeup_ = true; // modem was asleep, but received message
    }
    else if( strstr( deviceResponse_, "Response Not Received" ) )
    {
        if( verbosity_ > 0 ) logger_.syslog( "response not received", Syslog::INFO );
        gotResponseNotReceived_ = true; // modem is still working fine but may be out of acoustic range
    }

    // RX/TX gets displayed when verbosity is turned up
    else if( parseDebugRxMessage() )
    {
        if( verbosity_ > 0 ) logger_.syslog( "received an acoustic signal", Syslog::INFO );
        gotDebugRxMessage_ = true;
    }
    else if( parseDebugTxMessage() )
    {
        if( verbosity_ > 0 ) logger_.syslog( "transmitted an acoustic signal", Syslog::INFO );
        gotDebugTxMessage_ = true;
    }

    // These next two are triggered from an incoming ATR command (range request). If the chirp phases are good enough then the receiving modem
    // will send back it's respective bearing to the requesting modem
    else if( strstr( deviceResponse_, "range request" ) )
    {
        if( verbosity_ > 0 ) logger_.syslog( "received a range request message", Syslog::INFO );
        gotRangeRequestMessage_ = true;
    }
    else if( strstr( deviceResponse_, "bearing request" ) )
    {
        if( verbosity_ > 0 ) logger_.syslog( "received a bearing request message", Syslog::INFO );
        gotRangeRequestMessage_ = true; // Just using range request here since it's essentially the same thing. One can expect the the receiving modem will get our bearing when you see this message.
    }

    // Check for remote bearing
    else if( strstr( deviceResponse_, "(Remote)" ) )  //Bearing 198,  13 (Remote)
    {
        if( verbosity_ > 0 ) logger_.syslog( "Remote Bearing received:", deviceResponse_, Syslog::INFO );
    }
    // Check for local azimuth/bearing
    else if( strstr( deviceResponse_, "(Local)" ) )
    {
        if( verbosity_ > 0 ) logger_.syslog( "Local bearing/azimuth received:\n " + Str( deviceResponse_ ), Syslog::INFO );
    }
    // Check for a range response
    else if( parseRangeMessage() )
    {
        gotRangeMessage_ = true;
    }
    // See if there's a USBL message
    else if( parseDirectionMessage() )
    {
        gotDirectionMessage_ = true;
        if( verbosity_ > 0 ) logger_.syslog( "got valid direction response:\n " + Str( deviceResponse_ ), Syslog::INFO );
    }

    // Check for a packet headed for another modem
    else if( strstr( deviceResponse_, "$Packet for address" ) )
    {
        if( verbosity_ > 0 ) logger_.syslog( "received a packet notification", Syslog::INFO );
        uart_.flush(); // Clear things out from this response
    }
    // Check for a bad header
    else if( strstr( deviceResponse_, "$Error in header" ) )
    {
        if( verbosity_ > 0 ) logger_.syslog( "Received a bad header", Syslog::INFO );
        uart_.flush(); // Clear things out from this response
    }
    // Check for a low SNR in chirp
    else if( strstr( deviceResponse_, "$Low SNR" ) )
    {
        if( verbosity_ > 0 ) logger_.syslog( "Received low SNR in chirp", Syslog::INFO );
        uart_.flush(); // Clear things out from this response
    }
    else if( ( deviceResponse_[0] == '\n' ) || ( deviceResponse_[0] == '\r' ) )
    {
        //if( verbosity_ > 0 ) logger_.syslog( "uncaught empty line in deviceResponse_: ", deviceResponse_, Syslog::DEBUG );
    }
    else if( strstr( deviceResponse_, "<EOP>" ) )
    {
        // Ignore for now
    }
    else if( commandModeAcknowledged_
             && ( strstr( deviceResponse_, "Lowpower" )
                  || strcasestr( deviceResponse_, "watchdog" )
                  || strcasestr( deviceResponse_, "forwarding delay uptime" ) ) )
    {
        if( verbosity_ > 0 ) logger_.syslog( "Re-entering command mode due to deviceResponse_: ", deviceResponse_, Syslog::DEBUG );
        commandModeSent_ = false;
        commandModeAcknowledged_ = false;
    }
    else if( !strstr( deviceResponse_, "user:" ) )
    {
        logger_.syslog( "unknown deviceResponse_: ", deviceResponse_, Syslog::INFO );
        //this->setFailure( FailureMode::COMMUNICATIONS );
    }

    // Handle at end since it can be combined with other things
    if( commandModeSent_ && !commandModeAcknowledged_ && strstr( deviceResponse_, "user:" ) )
    {
        int prompt_number( -1 );
        if( sscanf( deviceResponse_, "user:%d>", &prompt_number ) == 1 )
        {
            commandModeAcknowledged_ = true;
        }
        if( debug_ ) logger_.syslog( "read user prompt " + Str( prompt_number ) + ": " + deviceResponse_, Syslog::DEBUG );
    }
}


void DAT::cleanResponse()
{
    char * i;
    while( ( i = strstr( deviceResponse_, "- " ) ) != NULL )
    {
        strncpy( i, " -", 2 );
    }
}


bool DAT::parseDirectionMessage( void )
{
//    19:49:26.6093 LVL= 14368, 13937, 18930, 15587, AGC= 35, IDX= 868, 0.20,-0.122, 0.229,-0.335,-0.067, PHS= 0.034,344,-0.271,  RAW= 359.7,  -4.1, CAL= 359.2,  -8.7, ROT= 359.1,  -8.7
    return ( 14 == sscanf( deviceResponse_,
                           "%*d:%*d:%*d.%*d LVL=%d,%d,%d,%d, AGC= %d, IDX=%*d,%*f,%*f,%*f,%*f,%*f, PHS=%lf,%lf,%lf,  RAW=%f,%f, CAL=%f,%f, ROT= %f, %f",
                           // TODO: Record instrument time.
                           &levelA_, &levelB_, &levelC_, &levelD_,
                           &agc_,
                           &phaseA_, &phaseB_, &phaseC_,
                           &rawAzimuth_, &rawElevation_,
                           &calibratedAzimuth_, &calibratedElevation_,
                           &rotatedAzimuth_, &rotatedElevation_ ) );
}

bool DAT::parseLocalAzimuthMessage()
{
    //Azimuth: <azimuth>, <elevation>
    if( 2 == sscanf( deviceResponse_, "Azimuth %f, %f ", &rotatedAzimuth_, &rotatedElevation_ ) )
    {
        return true;
    }
    return false;
}

bool DAT::parseRangeMessage( void )
{
    if( 3 == sscanf( deviceResponse_, "Range %d to %d : %lf m", &localAddress_, &remoteAddress_, &range_ ) && ( range_ > 0 ) )
    {
        return true;
    }
    return false;
}


bool DAT::parseDebugRxMessage( void )
{
    // (@ DEBUG level 3)  Rx Time:04:07:39.5194
    int hours, minutes;
    float seconds;
    if( 3 == sscanf( deviceResponse_, "Rx Time:%d:%d:%f", &hours, &minutes, &seconds ) )
    {
        tm start = startTime_.asStructTm(); // Let's use startTime_ for year month day.

        // And then we'll use the DAT output for actual hours, minutes, seconds.
        Str rxTimeString = Str( start.tm_year + 1900 ) + "-" + Str( start.tm_mon + 1 ) + "-" + Str( start.tm_mday )
                           + "T" + Str( hours ) + ":" + Str( minutes ) + ":" + Str( seconds );
        dataTimestamp_ = Timestamp( rxTimeString.cStr() ); // And finally set the dataTimestamp_ variable.
        if( verbosity_ > 2 )logger_.syslog( "Rx dataTimestamp_ set to:" + Str( dataTimestamp_.asDouble() ), Syslog::INFO );
        return true;
    }
    return false;
}


bool DAT::parseDebugTxMessage( void )
{
    // (@ DEBUG level 3)  Tx Time:04:07:39.5194
    int hours, minutes, seconds, tenthsMilliseconds;
    if( 4 == sscanf( deviceResponse_, "Tx time:%d:%d:%d.%d", &hours, &minutes, &seconds, &tenthsMilliseconds ) )
    {
        transmitPingTime_ = Timestamp::Now();
        logger_.syslog( "Ping request sent.", Syslog::INFO );
        return true;
    }
    return false;
}


void DAT::publishData( void )
{

    if( gotAcousticWakeup_ )
    {
        if( verbosity_ > 1 ) logger_.syslog( "publishing acoustic wakeup flag", Syslog::INFO );
        acousticWakeupWriter_->write( Units::COUNT, 1, receivePingTime_ );
        rxTimeWriter_->write( Units::EPOCH_SECOND, receivePingTime_.asDouble(), dataTimestamp_ );
    }
    if( gotDebugRxMessage_ )
    {
        // We rely on "bearing request" or "range request" messages since any RX could be a packet that's not for us
        if( verbosity_ > 1 ) logger_.syslog( "not publishing receive ping time as it could be a packet for anyone", Syslog::INFO );
        //rxTimeWriter_->write( Units::EPOCH_SECOND, modemReceivePingEpochSeconds_, dataTimestamp_ );
        //rxTimeWriter_->write( Units::EPOCH_SECOND, receivePingTime_.asDouble(), receivePingTime_ );
    }
    if( gotDebugTxMessage_ )
    {
        if( verbosity_ > 1 ) logger_.syslog( "publishing transmit ping time", Syslog::INFO );
        //txTimeWriter_->write( Units::EPOCH_SECOND, modemTransmitPingEpochSeconds_, dataTimestamp_ );
        txTimeWriter_->write( Units::EPOCH_SECOND, transmitPingTime_.asDouble(), transmitPingTime_ );
    }
    if( gotRangeRequestMessage_ )
    {
        if( verbosity_ > 1 ) logger_.syslog( "publishing range request flag", Syslog::INFO );
        rangeRequestReceivedWriter_->write( Units::COUNT, 1, dataTimestamp_ );
        rxTimeWriter_->write( Units::EPOCH_SECOND, receivePingTime_.asDouble(), dataTimestamp_ );
    }
    if( gotAckMessage_ )
    {
        msgAcknowledgedWriter_->write( Units::BOOL, true, dataTimestamp_ );
    }
    if( gotRangeMessage_ && gotDirectionMessage_ ) // Only publish if we got range too
    {
        if( verbosity_ > 1 ) logger_.syslog( "publishing direction and range info", Syslog::INFO );

        // Universal data
        rxTimeWriter_->write( Units::EPOCH_SECOND, dataTimestamp_.asDouble(), dataTimestamp_ );
        directionToContactWriter_->write1DClass( Units::NONE, directionInVehicleFrame_, dataTimestamp_ );
        rangeToContactWriter_->write( Units::METER, range_, dataTimestamp_ );
        contactAddressWriter_->write( Units::ENUM, remoteAddress_, dataTimestamp_ );

        // Data reported directly by the DAT
        lvl1Writer_->write( Units::COUNT, levelA_, dataTimestamp_ );
        lvl2Writer_->write( Units::COUNT, levelB_, dataTimestamp_ );
        lvl3Writer_->write( Units::COUNT, levelC_, dataTimestamp_ );
        lvl4Writer_->write( Units::COUNT, levelD_, dataTimestamp_ );
        agcWriter_->write( Units::COUNT, agc_, dataTimestamp_ );
        phaseAWriter_->write( Units::RADIAN, phaseA_, dataTimestamp_ );
        phaseBWriter_->write( Units::RADIAN, phaseB_, dataTimestamp_ );
        phaseCWriter_->write( Units::RADIAN, phaseC_, dataTimestamp_ );
        rawAzimuthWriter_->write( Units::DEGREE, rawAzimuth_, dataTimestamp_ );
        rawElevationWriter_->write( Units::DEGREE, rawElevation_, dataTimestamp_ );
        calibratedAzimuthWriter_->write( Units::DEGREE, calibratedAzimuth_, dataTimestamp_ );
        calibratedElevationWriter_->write( Units::DEGREE, calibratedElevation_, dataTimestamp_ );
        rotatedAzimuthWriter_->write( Units::DEGREE, rotatedAzimuth_, dataTimestamp_ );
        rotatedElevationWriter_->write( Units::DEGREE, rotatedElevation_, dataTimestamp_ );
        localAddressWriter_->write( Units::COUNT, localAddress_, dataTimestamp_ );
        deviceEnableRequestedWriter_->write( Units::BOOL, deviceEnableRequested_, dataTimestamp_ );

        // then the computed data
        tDirectionWriter_->write1DClass( Units::NONE, directionInTetrahedronFrame_, dataTimestamp_ );
        tAzimuthWriter_->write( Units::RADIAN, azimuthInTetrahedronFrame_, dataTimestamp_ );     // (Should be redundant with internally reported angles,
        tElevationWriter_->write( Units::RADIAN, elevationInTetrahedronFrame_, dataTimestamp_ ); // but rotated, because orientation of hydrophone tetrahedron
        vAzimuthWriter_->write( Units::RADIAN, azimuthInVehicleFrame_, dataTimestamp_ );         // wrt internal coordinate system is unknown.)
        vElevationWriter_->write( Units::RADIAN, elevationInVehicleFrame_, dataTimestamp_ );
    }
    else if( gotRangeMessage_ ) // Publish just range or just direction as needed
    {
        rxTimeWriter_->write( Units::EPOCH_SECOND, receivePingTime_.asDouble(), dataTimestamp_ );
        rangeToContactWriter_->write( Units::METER, range_, dataTimestamp_ );
        contactAddressWriter_->write( Units::ENUM, remoteAddress_, dataTimestamp_ );
    }
    else if( gotDirectionMessage_ )
    {
        // Universal data
        rxTimeWriter_->write( Units::EPOCH_SECOND, receivePingTime_.asDouble(), dataTimestamp_ );
        directionToContactWriter_->write1DClass( Units::NONE, directionInVehicleFrame_, dataTimestamp_ );
        contactAddressWriter_->write( Units::ENUM, remoteAddress_, dataTimestamp_ );

        // Data reported directly by the DAT
        lvl1Writer_->write( Units::COUNT, levelA_, dataTimestamp_ );
        lvl2Writer_->write( Units::COUNT, levelB_, dataTimestamp_ );
        lvl3Writer_->write( Units::COUNT, levelC_, dataTimestamp_ );
        lvl4Writer_->write( Units::COUNT, levelD_, dataTimestamp_ );
        agcWriter_->write( Units::COUNT, agc_, dataTimestamp_ );
        phaseAWriter_->write( Units::RADIAN, phaseA_, dataTimestamp_ );
        phaseBWriter_->write( Units::RADIAN, phaseB_, dataTimestamp_ );
        phaseCWriter_->write( Units::RADIAN, phaseC_, dataTimestamp_ );
        rawAzimuthWriter_->write( Units::DEGREE, rawAzimuth_, dataTimestamp_ );
        rawElevationWriter_->write( Units::DEGREE, rawElevation_, dataTimestamp_ );
        calibratedAzimuthWriter_->write( Units::DEGREE, calibratedAzimuth_, dataTimestamp_ );
        calibratedElevationWriter_->write( Units::DEGREE, calibratedElevation_, dataTimestamp_ );
        rotatedAzimuthWriter_->write( Units::DEGREE, rotatedAzimuth_, dataTimestamp_ );
        rotatedElevationWriter_->write( Units::DEGREE, rotatedElevation_, dataTimestamp_ );
        localAddressWriter_->write( Units::COUNT, localAddress_, dataTimestamp_ );

        // then the computed data
        tDirectionWriter_->write1DClass( Units::NONE, directionInTetrahedronFrame_, dataTimestamp_ );
        tAzimuthWriter_->write( Units::RADIAN, azimuthInTetrahedronFrame_, dataTimestamp_ );     // (Should be redundant with internally reported angles,
        tElevationWriter_->write( Units::RADIAN, elevationInTetrahedronFrame_, dataTimestamp_ ); // but rotated, because orientation of hydrophone tetrahedron
        vAzimuthWriter_->write( Units::RADIAN, azimuthInVehicleFrame_, dataTimestamp_ );         // wrt internal coordinate system is unknown.)
        vElevationWriter_->write( Units::RADIAN, elevationInVehicleFrame_, dataTimestamp_ );
    }

}

// TODO Need to make this work in C/C++ with vectors used in other LRAUV code
Point3D DAT::phaseToUnitVector( const double phaseA = 0.0, const double phaseB = 0.0, const double phaseC = 0.0, const double phaseD = 0.0 )
{
    // TODO optional arg for method (outward vs. inward)
    const char* method = "outward";

    Point3D da( 0.0 ), db( 0.0 ), dc( 0.0 ), dd( 0.0 ), u( 0.0 ); // intermediate vectors

    double sqrt3 = sqrt( 3.0 );
    double sqrt3_o6 = sqrt3 / 6.0;
    double n_sqrt3_o3 = -sqrt3 / 3.0;
    double sqrt6_o3 = sqrt( 6.0 ) / 3;

    Point3D la( -0.5, sqrt3_o6, sqrt6_o3 ); // should be a column vector
    Point3D lb( 0.5, sqrt3_o6, sqrt6_o3 ); // should be a column vector
    Point3D lc( 0.0, n_sqrt3_o3, sqrt6_o3 ); // should be a column vector
    Point3D ld( 0.0 ) ; // assume hydrophone D is the origin
    Point3D le = ( la + lb + lc + ld ) * .25; // should be a column vector

    if( strcmp( method, "outward" ) == 0 )
    {
        da = le - la;
        db = le - lb;
        dc = le - lc;
        dd = le - ld;
    }
    else if( strcmp( method, "inward" ) == 0 )
    {
        da = la - le;
        db = lb - le;
        dc = lc - le;
        dd = ld - le;
    }
    u = da * phaseA + db * phaseB + dc * phaseC + dd * phaseD;
    return u * ( 1.0 / u.getMagnitude() ); // want a unit vector
}


void DAT::assembleDirectionInVehicleFrame()
{
    // populate the tetrahedron frame direction vector
    directionInTetrahedronFrame_ = phaseToUnitVector( phaseA_, phaseB_, phaseC_ );

    // populate the vehicle frame direction vector
    directionInVehicleFrame_ = 0.0;
    if( phaseToDirectionCfgSetting_ )
    {
        // rotate the tetrahedron direction vector into vehicle coordinates
        // uhat_v = M_iv * uhat_i;
        directionInVehicleFrame_.addProduct( transformationFromTetrahedronToVehicleFrame_, directionInTetrahedronFrame_ );
        // Make sure it's really a unit vector
        directionInVehicleFrame_ *= ( 1.0 / directionInVehicleFrame_.getMagnitude() );
    }
    else
    {
        // use the rotated angles from the DAT, we'll need these angles in radians
        float rotatedAzimuthRad = D2R( rotatedAzimuth_ );
        float rotatedElevationRad = D2R( rotatedElevation_ );

        if( ignoreElevationSetting_ )
        {
            // ignore elevation (i.e., propgate forward a 2D horizontal solution).
            // projecting the solution down to 2D has shown to produce superior target localization in the field.
            float ignoreElevation( 0.0 );
            AuvMath::AzimuthAndElevationToUnitVector( rotatedAzimuthRad, ignoreElevation, directionInVehicleFrame_ );
        }
        else
        {
            AuvMath::AzimuthAndElevationToUnitVector( rotatedAzimuthRad, rotatedElevationRad, directionInVehicleFrame_ );
        }
    }

    if( verbosity_ > 2 )
    {
        logger_.syslog( "direction in FSK: " + directionInVehicleFrame_.toString(), Syslog::INFO );
    }
}

void DAT::directionsToAngles()
{
    AuvMath::UnitVectorToAzimuthAndElevation( directionInTetrahedronFrame_, azimuthInTetrahedronFrame_, elevationInTetrahedronFrame_ );
    if( fabs( directionInVehicleFrame_.getZ() ) < 1.0 )
        AuvMath::UnitVectorToAzimuthAndElevation( directionInVehicleFrame_, azimuthInVehicleFrame_, elevationInVehicleFrame_ );
}

bool DAT::getSimulatedMeasurements( )
{
    if( !simulateHardware() )
        return false;
    // see ExternalSim.cpp
    // sim currently writes out azimuth and elevation in vehicle
    // coordinate frame, _assuming space-fixed angles_
    float range( nanf( "" ) ), azimuth( nanf( "" ) ), elevation( nanf( "" ) );
    if( SimSlate::ReceiveRange( remoteAddressRequested_, range, azimuth, elevation, dataTimestamp_ ) )
    {
        // DAT expects angles in degrees
        rotatedAzimuth_ = R2D( azimuth );
        rotatedElevation_ = R2D( elevation );
        range_ = range;

        if( verbosity_ > 2 )
            logger_.syslog( "Received range message from " + Str( remoteAddressRequested_ ) + ". "
                            "Range: " + Str( range ) + " m, "
                            "Azimuth: " + Str( rotatedAzimuth_ ) + " deg, "
                            "Elevation: " + Str( rotatedElevation_ ) + " deg."
                            , Syslog::INFO );

        remoteAddress_ = remoteAddressRequested_;
        gotDirectionMessage_ = !isnan( rotatedAzimuth_ ) && !isnan( rotatedElevation_ );
        gotRangeMessage_ = !isnan( range_ );
        return true;
    }

    Str msg;
    if( SimSlate::ReceiveMessage( incomingLocalAddress_, incomingRemoteAddress_, msg ) )
    {
        if( verbosity_ > 2 )
            logger_.syslog( "Received message from " + Str( incomingRemoteAddress_ ) + ": " + msg, Syslog::INFO );

        if( msg.size() <= 0 )
        {
            // Empty message. Move on.
            return true;
        }

        //(byraanan)TODO: pkg incoming data msg processing in a function that's used in both hardware/sim cases.
        // Is the message an ACK?
        if( msg.find( ACK.cStr() ) != Str::NO_POS )
        {
            if( SENDING_ACK_WAITING == commsState_ || SENDING_TRANSMIT_VERIFY == commsState_ )
            {
                commsState_ = SENDING_VERIFIED;
                ackTimeouts_ = 0;
                logger_.syslog( "In parseResponses, got ack so set commsState_ = SENDING_VERIFIED", Syslog::DEBUG );
            }
            logger_.syslog( "Got ACK.", Syslog::INFO );
            gotAckMessage_ = true;

            return true;
        }

        // Is the message an SBD?
        bool parsed = true;
        commsData_[0] = 0;
        dataBytesReceived_ = msg.size();
        strncpy( commsData_, msg.cStr(), dataBytesReceived_ );

        Str errorMsg = DataReceiver::TryReceiveSbd( ( const char* )commsData_, dataBytesReceived_, keyText_.asString().cStr(), logger_ );

        if( errorMsg != Str::EMPTY_STR )
        {
            // Not an SBD... maybe text?
            for( size_t i = 0; i < dataBytesReceived_ && parsed; ++i )
            {
                parsed &= isprint( commsData_[i] ) || commsData_[i] == '\n' || commsData_[i] == '\r' || commsData_[i] == '\t';
            }
            if( parsed )
            {
                DataReceiver::Receive( ( const char* )commsData_, dataBytesReceived_, logger_ );
            }
        }

        if( !parsed )
        {
            // Parse failed
            Str hex;
            StrIOStream hexStream( hex );
            hexStream.writeHex( ( const char* )commsData_, dataBytesReceived_ );
            logger_.syslog( errorMsg, Syslog::FAULT );
            logger_.syslog( Str( "Failed to parse uplink message:" ) + hex, Syslog::FAULT );
        }

        if( parsed && incomingLocalAddress_ == localAddressCfgSetting_ )
        {
            logger_.syslog( "Sending ACK.", Syslog::INFO );
            SimSlate::SendMessage( incomingRemoteAddress_, localAddressCfgSetting_, ACK );
            awaitingAckTransmitAck_ = false;
        }

        this->resetFailCount();
        return true;
    }

    return false;
}


// Sets the time on the unit to the current application time
bool DAT::setTime()
{
    tm now = Timestamp::Now().asStructTm();

    if( !commandModeAcknowledged_ )
    {
        if( !enterCommandMode() )
        {
            commandModeSent_ = false;
            return false;
        }
        if( !commandModeAcknowledged_ )
        {
            return true;
        }
    }
    if( simulateHardware() )
    {
        localTimeSent_ = localTimeSetAcknowledged_ = true;
        return true;
    }
    if( !localTimeSent_ )
    {
        logger_.syslog( "Setting time to: " + Str( now.tm_hour ) + ":" + Str( now.tm_min ) + ":" + Str( now.tm_sec )
                        + " And date to:" + Str( now.tm_mon + 1 ) + "/" + Str( now.tm_mday ) + "/" + Str( now.tm_year + 1900 ), Syslog::INFO );
        uart_ << "date -t" << Str( now.tm_hour ) << ":" << Str( now.tm_min ) << ":" << Str( now.tm_sec ) << " -d" << Str( now.tm_mon + 1 ) << "/" << Str( now.tm_mday ) << "/" << Str( now.tm_year + 1900 ) << "\r";
        localTimeSent_ = true;
        localTimeSetAcknowledged_ = false;
        cmdTimeStart_ = Timestamp::Now();
        ++cmdTries_;
        return true;
    }
    else if( localTimeSent_ && !localTimeSetAcknowledged_ )
    {
        logger_.syslog( "checking for time setting acknowledgment", Syslog::DEBUG );
        if( cmdTimeStart_.elapsed() > cmdTimeout_ )
        {
            logger_.syslog( "failed to set time", Syslog::FAULT );
            if( cmdTries_ > maxCmdTries_ )
            {
                this->setFailure( FailureMode::COMMUNICATIONS );
            }
            else
            {
                localAddressSettingSent_ = false;
            }
            return false;
        }
    }
    cmdTries_ = 0;
    return true;
}


/*

cfg all

  S|  Version Params  |   Value     |       Range
--+------------------+-------------+-------------------------------------
  | *SWAppName       | Directional Acoustic Transponder|
 0| *SWVersion       | 8.12.21     |
  | *DBVersion       | 1.1         |

 S|  Serial Params   |   Value     |       Range
--+------------------+-------------+-------------------------------------
 3|  P1Baud          | 9600        | 1200, 2400, 4800, 9600, 19200, 38400,
  |                  |             | 57600, 115200
 3|  P1EchoChar      | Dis         | Ena, Dis
  |  P1FlowCtl       | 0 (None)    | 0 (None), 1 (SW), 2 (HW)
  | ^P1Mode          | 0 (Cooked)  | 0 (Cooked), 1 (Raw)
 3|  P1Protocol      | 0 (RS-232)  | 0 (RS-232), 1 (RS-422), 2 (RS-485)
 3|  P1StripB7       | Dis         | Ena, Dis
  | ^P1NoSleep       | Dis         | Ena, Dis
  |  P2Baud          | 115200      | 1200, 2400, 4800, 9600, 19200, 38400,
  |                  |             | 57600, 115200
  |  P2EchoChar      | Dis         | Ena, Dis
  |  P2FlowCtl       | 0 (None)    | 0 (None), 1 (SW), 2 (HW)
  | ^P2Mode          | 1 (Raw)     | 0 (Cooked), 1 (Raw)
  |  P2Protocol      | 1 (CMOS)    | 0 (RS-232), 1 (CMOS), 2 (Emul232)
  |  P2StripB7       | Dis         | Ena, Dis
  | ^P2NoSleep       | Ena         | Ena, Dis
  |  LPFlowCtl       | 0 (Lowpower)| 0 (Lowpower), 2 (Always_on)

 S|  System Params   |   Value     |       Range
--+------------------+-------------+-------------------------------------
  | ^ARWakeHib       | 2 (60sec)   | -1 (Off), 1 (48sec), 2 (60sec),
  |                  |             | 3 (72sec), 4 (84sec), 5 (96sec)
20| ^AuxInp          | Dis         | Ena, Dis
20|  AuxOut          | 0 (Default) | 0 (Default), 1 (Force)
  | ^BatteryType     | 0 (Std)     | 0 (Std), 3 (LiPrimary), 4 (ExtDC)
24| ^CarrFreq        | 100         | 56 = LF, 100 = MF, 141 = C, 156 = HF
  |  CMWakeHib       | -1 (Off)    | -1 (Off), 0 (2sec), 1 (3sec),
  |                  |             | 2 (4sec), 3 (6sec), 4 (8sec),
  |                  |             | 5 (12sec), 6 (16sec), 7 (24sec),
  |                  |             | 8 (32sec), 9 (48sec), 11 (96sec)
  |  CMFastWake      | Ena         | Ena, Dis
23| ^HalfBW          | 1           | 1..2
10|  IdleTimer       | 00:00:00    | hh(0-23):mm(0-59):ss(0-59)
  |                  |             | All zeros - timer disabled
  | ^FHThresh        | 100         | 0..200
  |  MinOpVoltage    | 9.6         | 0.0..24.0
  |  Prompt          | 7           | 0 (None), 1 (Arrow), 2 (Priv),
  |                  |             | 4 (CmdNum), 7 (All)
  |  Pullup0         | Dis         | Ena, Dis
  |  Pullup1         | Dis         | Ena, Dis
44| ^RlsType         | 0 (None)    | 0 (None), 1 (SmRel), 2 (SmMdm),
  |                  |             | 3 (OEMBurnWire), 8 (SmOEM)
  |  SyncPPS         | 2 (RTC)     | 2 (RTC), 1 (ExtRise), 4 (ExtFall)
  | ^SyncOut         | 0 (Off)     | 0 (Off), 1 (Port1), 2 (Port2),
  |                  |             | 3 (PulseOnly)
  | ^TiltAxis        | 0 (X+)      | 0 (X+), 1 (X-), 2 (Y+), 3 (Y-),
  |                  |             | 4 (Z+), 5 (Z-)
13|  Verbose         | 3           | 0 (none), 1-3 (customer), > 3 (diag)
  | ^WakeThresh      | 524         | 0..1000
  |  RxSensitivity   | -175.0000   | -200..0 dB (uPa)
  |  AwakePower      | 1 (3.3V)    | 0 (Off), 1 (3.3V), 2 (12V+3.3V)

 S|  Coproc Params   |   Value     |       Range
--+------------------+-------------+-------------------------------------
 9|  CPBoard         | 0 (Off)     | 0 (Off), 1 (PwrSave), 2 (AlwaysOn), 3
  |                  |             |  (Program)
30| ^FdFwdTaps       | 20          | 1..32
31| ^FdBckTaps       | 4           | 1..32

 S|  Datalog Params  |   Value     |       Range
--+------------------+-------------+-------------------------------------
22|  AcData          | 0 (UART)    | 0 (UART), 1 (Datalog),
  |                  |             | 2 (UART+Datalog)
29|  AcStats         | 0 (Off)     | 0 (Off), 1 (Stats),  4 (TimeStamp),
  |                  |             | 5 (Stats+Time)
37|  RingBuf         | Dis         | Ena, Dis
  |  LogMode         | 0 (FwdDelay)| 0 (FwdDelay), 1 (Sentinel),
  |                  |             | 2 (ChrCount)
  |  Sentinel        | 0           | 0..255
  |  ChrCount        | 1024        | 0..4095
  |  LogStore        | 0 (Internal)| 0 (Internal), 1 (SDHC)

 S|  Modem Params    |   Value     |       Range
--+------------------+-------------+-------------------------------------
 7|  AcRspTmOut      | 7.500       | 2.0 - 99.5 sec (0.5 sec increments)
  |  DataRetry       | 0           | 0..6
28|  DevEnable       | 0 (Auto)    | 0 (Auto), 1 (MBARI), 2 (Manual-LP),
  |                  |             | 3 (Manual)
 8|  FwdDelay        | 3.000       | 0.05 to 5 sec (50ms increments)
  |  DomainKey       |             | String of up to 12 characters
18|  LocalAddr       | 2           | 0..249
15|  OpMode          | 1 (Online)  | 0 (Command), 1 (Online), 2 (Datalog),
  |                  |             | 6 (ContXpnd), 8 (Sink)
  |  PrintHex        | Dis         | Ena, Dis
14|  RemoteAddr      | 0           | 0 - 249, 255
 5| ^RxPktType       | 0 (MFSK)    | 0 (MFSK), 1 (FH_MA), 2 (Xpnd),
  |                  |             | 3 (Chirp)
16|  ShowBadData     | Ena         | Ena, Dis
  |  SmartRetry      | Dis         | Ena, Dis
54|  StartTones      | 0           | Power Level, 0 = silent
  |  StrictAT        | Dis         | Ena, Dis
 4|  TxRate          | 5 (800)     | 1 (80), 2 (140), 3 (300), 4 (600),
  |                  |             | 5 (800), 6 (1066), 7 (1200),
  |                  |             | 8 (2400), 9 (2560), 10 (5120),
  |                  |             | 11 (7680), 12 (10240), 13 (15360)
  |  HeaderRate      | 2 (140)     | 1 (80), 2 (140)
 6|  TxPower         | 1 (-21dB)   | 1 (-21dB), 2 (-18dB), 3 (-15dB),
  |                  |             | 4 (-12dB), 5 (-9dB), 6 (-6dB),
  |                  |             | 7 (-3dB), 8 (Max)
17|  WakeTones       | Ena         | Ena, Dis
  |  AutoDetectHdr   | Dis         | Ena, Dis
  | ^ChirpThresh     | 27          | 15..27
  |  AddrGroup       | 0           | 0..4

 S|  Release Params  |   Value     |       Range
--+------------------+-------------+-------------------------------------
51| ^FSKRlsDur       | 8           | 4..12
39| ^LstCommsCnt     | 0           | 0..999
41| ^RlsCode         | 0           | 0..65535
50|  TimedRelease    | 0           | Release in 0-999 hours
  |  RlsMinEnaTime   | 10          | 0..10
  |  RlsMaxEnaTime   | 40          | 10..120

 S|  Transport Params|   Value     |       Range
--+------------------+-------------+-------------------------------------
  | ^L4Enable        | Ena         | Ena, Dis
  |  TPortMode       | 0 (InpMode) | 0 (InpMode), 1 (AlwaysOn)
  |  SrcP1           | 1           | 1..4
  |  SrcP2           | 2           | 1..4
  |  Dst1            | 1 (P1)      | 1 (P1), 2 (P2)
  |  Dst2            | 2 (P2)      | 1 (P1), 2 (P2)
  |  Dst3            | 1 (P1)      | 1 (P1), 2 (P2)
  |  Dst4            | 2 (P2)      | 1 (P1), 2 (P2)

 S|  Test Params     |   Value     |       Range
--+------------------+-------------+-------------------------------------
  | ^DbgLvl          | 0           | 0..2147483647
  |  RcvAll          | Dis         | Ena, Dis
42| ^RspDelay        | Dis         | Ena, Dis
12|  PktEcho         | Dis         | Ena, Dis
12|  PktSize         | 0 (8B)      | 0 (8B), 1 (32B), 2 (128B), 3 (256B),
  |                  |             | 4 (512B), 5 (1024B), 6 (2048B),
  |                  |             | 7 (4096B)
  |  SimAcDly        | 0           | 0..30000 ms
  | ^Arrival         | 0           | 1 (First), 0 (Peak)

 S|  Xpnd Params     |   Value     |       Range
--+------------------+-------------+-------------------------------------
53|  RxFreq          | 10000       | 250 Hz steps within frequency band
  |  RxToneDur       | 10 (10ms)   | 0 (12.5ms), 1 (6.25ms), 5 (5ms),
  |                  |             | 6 (6ms), 7 (7ms), 8 (8ms), 9 (9ms),
  |                  |             | 10 (10ms), 11 (11ms), 12 (12ms),
  |                  |             | 13 (13ms), 14 (14ms), 15 (15ms)
21|  RxThresh        | 100         | 10..999
55|  RxLockout       | 30          | 10..1500 (ms)
  |  TxToneDur       | 10 (10ms)   | 0 (12.5ms), 1 (6.25ms), 5 (5ms),
  |                  |             | 6 (6ms), 7 (7ms), 8 (8ms), 9 (9ms),
  |                  |             | 10 (10ms), 11 (11ms), 12 (12ms),
  |                  |             | 13 (13ms), 14 (14ms), 15 (15ms)
40|  TAT             | 300         | 0..2000, 0.1 ms steps
58| ^AGCRef          | 60          | 10..80
  |  RespFreq        | 11000       | 250 Hz steps within frequency band
  |  LBLmode         | 0 (Off)     | 0 (Off), 1 (Tone), 2 (Listen),
  |                  |             | 3 (Chirp)
  |  Responder       | 0 (Off)     | 0 (Off), 1 (Rise), 2 (Fall)
  |  ChirpResp       | 1 (Down)    | 0 (Up), 1 (Down)
  | ^XpndBW          | 9 (9kHz)    | 5 (5kHz), 9 (9kHz)
  | ^XpndLog         | Dis         | Ena, Dis

 S|  Dat Params      |   Value     |       Range
--+------------------+-------------+-------------------------------------
  |  DatVerbose      | 27440       | 0..65535
  |  RXonDAT         | Dis         | Ena, Dis
  |  PreGain         | 0           | -89..89
  |  Rotation        | 210.0       | 0 to 359.9 degrees
  |  Orientation     | 0 (up)      | 0 (up), 1 (down), 2 (fwd)
  | ^PhaseA          | -0.088      | radians
  | ^PhaseB          | -0.047      | radians
  | ^PhaseC          | 0.003       | radians
  | ^PhaseD          | 0.000       | radians
  | ^VThresh         | 0.700       | 0.000..1.000
  |  MinElev         | -90.0       | degrees
  |  MaxElev         | 90.0        | degrees
  |  PhaseRef        | 0 (Active)  | 0 (Active), 1 (Passive)

 S|  Usbl Params     |   Value     |       Range
--+------------------+-------------+-------------------------------------
  |  USBLformat      | 0 (Benthos) | 0 (Benthos), 1 (ORE_STD),
  |                  |             | 2 (ORE_WPR), 4 (NMEA),
  |                  |             | 5 (compact)
  |  USBLheadDepth   | 0.0         | 0..100.0 meters
  |  USBLauto        | 0 (Off)     | 0 (Off), 1 (Acoustic),
  |                  |             | 2 (Electric)
  |  USBLgetDepth    | Dis         | Ena, Dis
  |  USBLrepeat      | 0           | 0..255
  |  USBLdelay       | 0           | 0..3000 seconds

 S|  Nav Params      |   Value     |       Range
--+------------------+-------------+-------------------------------------
  |  Latitude        |   41.638246 | degrees
  |  Longitude       |  -70.609376 | degrees
  |  GpsAlt          | 0.0         | meters above mean sea level
  |  GpsSyncMsg      | 1 (ZDA)     | 0 (None), 1 (ZDA), 2 (GGA),
  |                  |             | 3 (RMC)
  |  Altitude        | -1.00       | meters above sea floor
  |  Depth           | -1.0        | meters below sea level
  |  Compass         | 276.6       | 0 to 359.9 degrees
  |  Pitch           | -2.9        | -90.0 to  90.0 degrees
  |  Roll            | -0.4        | -180.0 to 180.0 degrees
  |  SpeedOfSound    | 1500.0      | speed of sound in meters per second
  |  ReplyData       | 1 (LatLong) | 0 (Off), 1 (LatLong), 2 (Depth),
  |                  |             | 3 (SeaFloor), 4 (GpsAlt)
  |  HeadOffset      | 0.0         |
  | ^PitchOffset     | 0.0         | -10.0..10.0
  | ^RollOffset      | 0.0         | -10.0..10.0


*/