/** \file
 *
 *  Contains the Micromodem class implementation.
 *
 *  Copyright (c) 2019 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

/***

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

***/

#include "Micromodem.h"
#include "MicromodemIF.h"

#include <cstdlib>
#include <errno.h>
#include <sys/stat.h>
#include <unistd.h>

#include "Power24vConverterIF.h"
#include "controlModule/VerticalControlIF.h"
#include "data/ConfigReader.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 "logger/LogWriter.h"
#include "supervisor/DataReceiver.h"
#include "supervisor/Supervisor.h"
#include "units/Units.h"
#include "VehicleIF.h"

const int Micromodem::FrameCounts_[] = { 1, 3, 3, 2, 2, 8 };
const int Micromodem::FrameSizes_[] = { 32, 64, 64, 256, 256, 256 };
const char* const Micromodem::FrameFmts_[] = { "%64s", "%128s", "%128s", "%512s", "%512s", "%512s" };

Micromodem::Micromodem( const Module* module )
    : SyncSensorComponent( MicromodemIF::NAME, module ),
      commsState_( SENDING_FILL_BUFFER ),
      verbosity_( 2 ),
      loadControl_( MicromodemIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      startTime_( Timestamp::NOT_SET_TIME ),
      sendTime_( Timestamp::NOT_SET_TIME ),
      powerOffTimeStart_( Timestamp::NOT_SET_TIME ),
      poPause_( 5 ), // time to wait for Micromodem to boot, etc.
      poTimeout_( 120 ), // Todo: accurately measure
      powerDownTimeout_( 3 ),
      caRevTimeout_( 30 ), // How long to wait for the routine CAREV messages
      uart_( MicromodemIF::UART, MicromodemIF::BAUD, 0.6, logger_, 4095 ),
      localAddressCfgSetting_( 0 ),
      destinationAddressCfgSetting_( 0 ),
      dataRateCfgSetting_( 1 ),
      sendExpressCfgSetting_( 0 ),
      acousticResponseTimeoutCfgSetting_( 20 ),
      resendPeriodCfgSetting_( 60 ),
      dusblPingCodeCfgSetting_(),
      rangeTXSeqCodeCfgSetting_(),
      rangeTxFreqCfgSetting_( 10000 ),
      rangeTxTimeCfgSetting_( 10 ),
      rangeRxTimeCfgSetting_( 10 ),
      rangeTATCfgSetting_( 50 ),
      surfaceThresholdCfgSetting_( 1 ),
      pwrampTXLevelCfgSetting_( 0 ),
      centerFrequencyCfgSetting_( 14500 ),
      bandwidthCfgSetting_( 4000 ),
      sendDataToShore_( true ),
      maxDownlinkMsgSize_( MAX_UMODEM_DOWNLINK_MSG_BYTES ),
      maxDownlinkDataSize_( MAX_UMODEM_DOWNLINK_MSG_BYTES - SENDPACKET_HEADER_SIZE ),
      keyText_(),
      majorRevision_( 0 ),
      startingState_( NOT_STARTING ),
      outbuffer_( true ),
      simbuffer_( true ),
      cmdSentTime_( Timestamp::NOT_SET_TIME ),
      cmdResponseWait_( 11.0 ), // based on observation -- goby uses 5.0
      remoteAddressRequested_( 0 ),
      pingsRequested_( 0 ),
      pingsSent_( 0 ),
      wasDusblRequested_( false ),
      gotAcousticWakeup_( false ),
      gotResponseNotReceived_( false ),
      gotRangeRequestMessage_( false ),
      gotRangeMessage_( false ),
      commRate_( 0 ),
      communicationsDone_( true ),
      range_( nan( "" ) ),
      dataTimestamp_( Timestamp::NOT_SET_TIME ),
      transmitPingTime_( Timestamp::NOT_SET_TIME ),
      receivePingTime_( Timestamp::NOT_SET_TIME ),
      modemTransmitPingTime_( Timestamp::NOT_SET_TIME ),
      modemReceivePingTime_( Timestamp::NOT_SET_TIME ),
      modemTransmitPingEpochSeconds_( 0.0 ),
      modemReceivePingEpochSeconds_( 0.0 ),
      remoteAddress_( 0 ),
      localAddress_( 0 ),
      maxSimAcousticDistance_( 4000.0 ),
      sendPacket_( MAX_UMODEM_DOWNLINK_MSG_BYTES, &logger_ )

{
    // Configuration inputs
    verbosityCfgReader_ = newConfigReader( MicromodemIF::VERBOSITY_CFG );
    destinationAddressCfgReader_ = newConfigReader( MicromodemIF::DESTINATION_ADDRESS_CFG );
    dataRateCfgReader_ = newConfigReader( MicromodemIF::DATA_RATE_CFG );
    sendExpressCfgReader_ = newConfigReader( MicromodemIF::SEND_EXPRESS_CFG );
    acousticResponseTimeoutCfgReader_ = newConfigReader( MicromodemIF::ACOUSTIC_RESPONSE_TIMEOUT_CFG );
    resendPeriodCfgReader_ = newConfigReader( MicromodemIF::RESEND_PERIOD_CFG );
    dusblPingCodeCfgReader_ = newConfigReader( MicromodemIF::DUSBL_PING_CODE_CFG );
    rangeTXSeqCodeCfgReader_ = newConfigReader( MicromodemIF::RANGE_TX_SEQ_CODE_CFG );
    rangeTxFreqCfgReader_ = newConfigReader( MicromodemIF::RANGE_TX_FREQ_CFG );
    rangeTxTimeCfgReader_ = newConfigReader( MicromodemIF::RANGE_TX_TIME_CFG );
    rangeRxTimeCfgReader_ = newConfigReader( MicromodemIF::RANGE_RX_TIME_CFG );
    rangeTATCfgReader_ = newConfigReader( MicromodemIF::RANGE_TAT_CFG );
    surfaceThresholdCfgReader_ = newConfigReader( MicromodemIF::SURFACE_THRESHOLD_CFG );
    pwrampTXLevelCfgReader_ = newConfigReader( MicromodemIF::PWRAMP_TXLEVEL );
    centerFrequencyCfgReader_ = newConfigReader( MicromodemIF::CENTER_FREQUENCY );
    bandwidthCfgReader_ = newConfigReader( MicromodemIF::BANDWIDTH );
    localAddressCfgReader_ = newConfigReader( VehicleIF::ID_CFG );
    sendDataToShoreCfgReader_ = newConfigReader( VehicleIF::SEND_DATA_TO_SHORE_CFG );
    for( int iTrans = 0; iTrans < MicromodemIF::NUM_TRANS; ++iTrans )
    {
        transChannelCfgReaders_[iTrans] = newConfigReader( *MicromodemIF::TRANS_CHANNEL_CFGS[iTrans] );
        transChannelCfgSetting_[iTrans] = -1;
        transChannelType_[iTrans] = TRANS_CHANNEL_NONE;
        transChannelPinged_[iTrans] = false;
    }

    // Universal inputs
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );

    // Slate inputs
    queryAddressRequestedReader_ = newDataReader( MicromodemIF::QUERY_ADDRESS_REQUESTED );
    queryAddressRequestedReader_->setImplementor( true );
    pingsRequestedReader_ = newDataReader( MicromodemIF::PINGS_REQUESTED );
    pingsRequestedReader_->setImplementor( true );
    dusblPingCodeReader_ = newDataReader( MicromodemIF::DUSBL_PING_CODE_REQUESTED );
    dusblPingCodeCfgReader_->setImplementor( true );
    power24vConverterDataReader_ = newDataReader( Power24vConverterIF::POWER_24V_CONVERTER );

    // Slate outputs
    acousticWakeupWriter_ = newDataWriter( MicromodemIF::ACOUSTIC_WAKEUP );
    rxTimeWriter_ = newUniversalWriter( UniversalURI::ACOUSTIC_RECEIVE_TIME, Units::SECOND, 0.11 );
    txTimeWriter_ = newUniversalWriter( UniversalURI::ACOUSTIC_TRANSMIT_TIME, Units::SECOND, 0.11 );
    rangeRequestReceivedWriter_ = newDataWriter( MicromodemIF::RANGE_REQUEST );
    remoteAddressWriter_ = newDataWriter( MicromodemIF::REMOTE_ADDRESS_READING );
    localAddressWriter_ = newDataWriter( MicromodemIF::LOCAL_ADDRESS_READING );
    rangeWriter_ = newDataWriter( MicromodemIF::RANGE_READING );
    platformCommunicationsWriter_ = newUniversalWriter( UniversalURI::PLATFORM_COMMUNICATIONS, Units::BOOL, 0 );
    for( int iPing = 0; iPing < LBLNavigationIF::NUM_PINGS; ++iPing )
    {
        pingTimeOfFightWriters_[iPing] = newDataWriter( *LBLNavigationIF::PING_TIME_OF_FLIGHTS[iPing] );
    }

    memset( deviceResponse_, 0, MAX_DEVICE_RESPONSE + 1 );

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

    this->setAllowableFailures( 3 ); // TODO: Should this be a configuration variable?
    this->setRetryTimeout( 120 );
    this->setFailureMissionCritical( false );

    sendPacket_.timeT_ = 0;
    sendPacket_.index_ = 0;
    sendPacket_.packetsLeft_ = 0;
    sendPacket_.data_[0] = 0;
    sendPacket_.dataSize_ = 0;
    sendPacket_.sendFilename_ = Str::EMPTY_STR;
    sendPacket_.sendFilesize_ = 0;

    initUmodemPacket( outgoingPacket_ );
    initUmodemPacket( incomingPacket_ );

    pingChannels_[0] = -1;
    pingChannels_[1] = -1;
    pingChannels_[2] = -1;
    pingChannels_[3] = -1;

    //debugLevel_ = Syslog::INFO;
}


Micromodem::~Micromodem()
{
}


void Micromodem::run()
{
}


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

    // verbosity
    if( !verbosityCfgReader_->read( Units::ENUM, verbosity_ ) )
    {
        logger_.syslog( "Could not read verbosity setting, using", verbosity_, Syslog::ERROR );
    }

    // localAddress
    if( !localAddressCfgReader_->read( Units::COUNT, localAddressCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for local address, using", localAddressCfgSetting_, Syslog::ERROR );
    }

    // destinationAddress - defaults to -1 (no data comms)
    if( !destinationAddressCfgReader_->read( Units::COUNT, destinationAddressCfgSetting_ ) )
    {
        destinationAddressCfgSetting_ = -1;
    }

    // dataRate - always 1
    dataRateCfgSetting_ = 1;
    //if( !dataRateCfgReader_->read( Units::COUNT, dataRateCfgSetting_ ) )
    //{
    //    logger_.syslog( "Could not read configuration setting for data rate, using", dataRateCfgSetting_, Syslog::ERROR );
    //}

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

    //  acousticResponseTimeout
    if( !acousticResponseTimeoutCfgReader_->read( Units::SECOND, acousticResponseTimeoutCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for acousticResponseTimeout, using ", acousticResponseTimeoutCfgSetting_, Syslog::ERROR );
    }

    //  resendPeriod
    if( !resendPeriodCfgReader_->read( Units::SECOND, resendPeriodCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for resendPeriod, using ", resendPeriodCfgSetting_, Syslog::ERROR );
    }

    //dusblPingCode
    if( !dusblPingCodeCfgReader_->read( dusblPingCodeCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for dusblPingCode, using" + dusblPingCodeCfgSetting_.toString(), Syslog::ERROR );
    }

    //rangeTXSeqCode
    if( !rangeTXSeqCodeCfgReader_->read( rangeTXSeqCodeCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for rangeTXSeqCode, using" + rangeTXSeqCodeCfgSetting_.toString(), Syslog::ERROR );
    }

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

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

    // pwrampTXLevel
    if( !pwrampTXLevelCfgReader_->read( Units::ENUM, pwrampTXLevelCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for pwrampTXLevel, using ", pwrampTXLevelCfgSetting_, Syslog::ERROR );
    }

    // centerFrequency
    if( !centerFrequencyCfgReader_->read( Units::HERTZ, centerFrequencyCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for centerFrequency, using ", centerFrequencyCfgSetting_, Syslog::ERROR );
    }

    // bandwidth
    if( !bandwidthCfgReader_->read( Units::ENUM, bandwidthCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for bandwidth, using", bandwidthCfgSetting_, Syslog::ERROR );
    }

    for( int iTrans = 0; iTrans < MicromodemIF::NUM_TRANS; ++iTrans )
    {
        if( !transChannelCfgReaders_[iTrans]->read( Units::ENUM, transChannelCfgSetting_[iTrans] ) )
        {
            logger_.syslog( "Could not read configuration setting for transponder channel#", iTrans + 1, Syslog::ERROR );
        }
        if( transChannelCfgSetting_[iTrans] == -1 )
        {
            transChannelType_[iTrans] = TRANS_CHANNEL_NONE;
        }
        else if( transChannelCfgSetting_[iTrans] >= 0 && transChannelCfgSetting_[iTrans] < 1000 )
        {
            transChannelType_[iTrans] = TRANS_CHANNEL_ID;
        }
        else if( transChannelCfgSetting_[iTrans] >= 1000 && transChannelCfgSetting_[iTrans] < 100000 )
        {
            transChannelType_[iTrans] = TRANS_CHANNEL_FREQENCY;
        }
        else
        {
            transChannelType_[iTrans] = TRANS_CHANNEL_CODE;
        }
    }

    // TODO: Set instrument firmware DEBUG from config parameter
}

void Micromodem::uninitialize()
{
    if( verbosity_ > 4 ) logger_.syslog( "uninitialize", Syslog::INFO );
    if( !simulateHardware() )
    {
        logger_.syslog( "Powering down", Syslog::INFO );
        if( !loadControl_.powerDown() )
        {
            logger_.syslog( "Failed to power down load control 1", 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 Micromodem::start()
{
    if( verbosity_ > 4 ) logger_.syslog( "Start", Syslog::INFO );

    commRate_ = 0;
    pingsSent_ = 0;
    wasDusblRequested_ = false;

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

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

    // rangeTxTime (narrowband/generic ping transmit freq)
    if( !rangeTxFreqCfgReader_->read( Units::HERTZ, rangeTxFreqCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for rangeTxFreqCfgSetting_, using ", rangeTxFreqCfgSetting_, Syslog::ERROR );
    }

    // rangeTxTime (narrowband transmit time)
    if( !rangeTxTimeCfgReader_->read( Units::MILLISECOND, rangeTxTimeCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for rangeTxTimeCfgSetting_, using ", rangeTxTimeCfgSetting_, Syslog::ERROR );
    }

    // rangeTxTime (narrowband recieve time)
    if( !rangeRxTimeCfgReader_->read( Units::MILLISECOND, rangeRxTimeCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for rangeRxTimeCfgSetting_, using ", rangeRxTimeCfgSetting_, Syslog::ERROR );
    }

    // rangeTAT (turn around time)
    if( !rangeTATCfgReader_->read( Units::MILLISECOND, rangeTATCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for rangeTATCfgSetting_, using ", rangeTATCfgSetting_, Syslog::ERROR );
    }

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

    startingState_ = STARTING_POWER_ON;
    startTime_ = Timestamp::Now();

    return STARTING;
}


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

    readConfig();
    tryRcvNMEA();
    trySendNMEA();
    Str addrStr;
    Str freqStr;
    Str bwStr;

    // Allow up to poTimeout_ to startup
    if( !simulateHardware() && startTime_.elapsed() > poTimeout_ )
    {
        if( uart_.dataAvailable() == 0 && uart_.bytesRead() == 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;
    }

    // Initialize vars used in switch statement
    char buf[24];
    struct tm result;
    Timestamp nowPlus1;
    size_t buflen;

    switch( startingState_ )
    {
    case NOT_STARTING:
        // Not sure how we got here.
        return stop();
    case STARTING_POWER_ON:
        // First thing to do is to turn on power
        if( !simulateHardware() )
        {
            logger_.syslog( "Powering up", Syslog::INFO );
            if( !loadControl_.powerUp() )
            {
                logger_.syslog( Str( "Error: Micromodem load controller 1 failed to power up.\n" ), Syslog::FAULT );
                this->setFailure( FailureMode::HARDWARE );
                return START;
            }
            logger_.syslog( "Initializing Micromodem." );

            // 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;
            }
            uart_.flush();
        }
        startingState_ = STARTING_POWER_WAIT;
        break;

    case STARTING_POWER_WAIT:
        // Wait in this state until system booted, etc
        if( startTime_.elapsed() < poPause_ )
        {
            break;
        }
        startingState_ = STARTING_WRITE_CFG;
        return starting();

    case STARTING_WRITE_CFG:
        // Start with a fresh configuration to enable default values. Then just set the stuff we care about.
        bufferNMEA( "$CCCFG", 6, "ALL", 3, "0", 1 );

        // Set detection power threshold to 20 db
        bufferNMEA( "$CCCFG", 6, "POW", 3, "20", 2 );

        // Set local address
        addrStr = Str( localAddressCfgSetting_ );
        bufferNMEA( "$CCCFG", 6, "SRC", 3, addrStr.cStr(), addrStr.length() );

        if( dusblPingCodeCfgSetting_.toString() != Str::EMPTY_STR )
        {
            // use gpio4 hi to lo as transmit start trigger
            bufferNMEA( "$CCCFG", 6, "nav.dt.txtrig_gpio4", 19, "1", 1 );
        }

        // Set pwrampTXLevel
        if( pwrampTXLevelCfgSetting_ >= 0 && pwrampTXLevelCfgSetting_ <= 3 )
        {
            *buf = '0' + pwrampTXLevelCfgSetting_;
            bufferNMEA( "$CCCFG", 6, "pwramp.txlevel", 14, buf, 1 );
        }

        // Set data timeout to 30 seconds
        bufferNMEA( "$CCCFG", 6, "DTO", 3, "30", 2 );

        // Set centerFrequency
        if( centerFrequencyCfgSetting_ >= 0 )
        {
            addrStr = Str( centerFrequencyCfgSetting_, 10 );
            bufferNMEAStrs( "$CCCFG,FC0", addrStr );
        }

        // Set bandwidth
        if( bandwidthCfgSetting_ >= 0 )
        {
            addrStr = Str( bandwidthCfgSetting_, 10 );
            bufferNMEAStrs( "$CCCFG,BW0", addrStr );
        }

        // Set bandwidth to freq bank 0
        bufferNMEA( "$CCCFG", 6, "BND", 3, "0", 1 );

        // Set TAT
        if( rangeTATCfgSetting_ != 50 )
        {
            Str rageTATCfgStr = Str( ( int ) rangeTATCfgSetting_ );
            bufferNMEA( "$CCCFG", 6, "TAT", 3, rageTATCfgStr.cStr(), rageTATCfgStr.size() );
        }

        startingState_ = STARTING_WRITE_CFG_WAIT;
        break;

    case STARTING_WRITE_CFG_WAIT:
        // STARTING_SET_CLOCK via a call to receivedCACFG() with "BND" in the reply
        break;

    case STARTING_SET_CLOCK:
        nowPlus1 = Timestamp::Now();
        nowPlus1 += 1;
        gmtime_r( &nowPlus1.getTimeval().tv_sec, &result );
        buflen = strftime( buf, 23, "%Y-%m-%dT%H:%M:%SZ", &result );
        switch( majorRevision_ )
        {
        default:
            //$CCCLK,YYYY,MM,DD,hh,mm,ss*CS
            bufferNMEA( "$CCCLK", 6, buf, 4, buf + 5, 2, buf + 8, 2, buf + 11, 2, buf + 14, 2, buf + 17, 2 );
            break;
        case 2:
            bufferNMEA( "$CCTMS", 6, buf, buflen, "0", 1 );
            break;
        }
        startingState_ = STARTING_SET_CLOCK_WAIT;
        break;

    case STARTING_SET_CLOCK_WAIT:
        // We wait here until we get the clock set response
        break;

    case STARTING_DONE:
        startingState_ = NOT_STARTING;
        this->resetFailCount();
        return runnable();
    }

    // If we haven't gottten to STARTING_DONE, keep looking
    return STARTING;
}


/// Pause
Component::RunState Micromodem::pause()
{
    if( verbosity_ > 4 ) 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 Micromodem::paused()
{
    if( verbosity_ > 4 ) logger_.syslog( "Paused", Syslog::INFO );
    if( keepPowerOn() || isDusblRequested() || pingsRequested() || isDataRequested() || !outbuffer_.isEmpty() || ( simulateHardware() || uart_.dataAvailable() > 0 ) ) return RESUME;
    return PAUSED;
}


Component::RunState Micromodem::resume()
{
    if( verbosity_ > 4 ) logger_.syslog( "Resume", Syslog::INFO );
    if( !simulateHardware() )
    {
        logger_.syslog( "sending wake-up to local modem", Syslog::DEBUG );
        // TODO: Change resume to wake up from lowpower state of modem
    }
    startTime_ = Timestamp::Now();
    caRevTime_ = Timestamp::Now();
    pingsSent_ = 0;
    wasDusblRequested_ = false;
    return RESUMING;
}


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


Component::RunState Micromodem::runnable()
{
    //if( verbosity_ > 4 ) logger_.syslog( "Runnable", Syslog::INFO );

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

    if( !keepPowerOn() && !isDusblRequested() && pingsRequested() == 0 && !isDataRequested() && outbuffer_.isEmpty() && ( simulateHardware() || uart_.dataAvailable() == 0 ) )
    {
        // Pause if we don't want data
        return PAUSE;
    }

    tryRcvNMEA();
    trySendNMEA();

    gotAcousticWakeup_ = false;
    gotResponseNotReceived_ = false;
    gotRangeRequestMessage_ = false;
    gotRangeMessage_ = false;

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

    if( isDusblRequested() && !wasDusblRequested_ )
    {
        if( verbosity_ > 4 ) logger_.syslog( "**** isDusblRequested IN Runnable **** ", Syslog::INFO );
        sendDUSBLPing();
    }
    else if( pingsRequested() && pingsSent_ == 0 )
    {
        if( verbosity_ > 4 ) logger_.syslog( "**** isPingRequested IN Runnable **** ", Syslog::INFO );
        sendRangePings();
    }
    else if( isDataRequested() )
    {
        if( verbosity_ > 4 ) logger_.syslog( "**** isDataRequested  IN Runnable ****, commsState_= ", commsState_, Syslog::INFO );
        switch( commsState_ )
        {
        case SENDING_FILL_BUFFER:
            if( belowSurfaceThreshold() )
            {
                sendingFillBuffer();
            }
            else
            {
                if( verbosity_ > 4 ) logger_.syslog( "Not below surface threshold", Syslog::INFO );
            }
            break;
        case SENDING_TRANSMIT:
            sendingTransmit();
            break;
        case SENDING_VERIFIED:
            sendingVerified();
            break;
        case SENDING_WAIT_FOR_SIGNAL:
            sendingWaitForSignal();
            break;
        }
    }
    else if( commsState_ == SENDING_TRANSMIT )
    {
        // Handle cases when isDataRequested() goes false while trying to send (for example, iridium transmit)
        sendingTransmit();
    }
    else if( keepPowerOn() )
    {
        // want to set the timestamp as the instant it is read from the
        // port, but the ordering is a bit hokey for this component
        // since we may get different kinds of data on different lines
        // of the response. We set the timestamp just before trying to
        // read from the port, and will only use it if range and/or
        // direction data gets read successfully.
        dataTimestamp_ = Timestamp::Now();
        tryRcvNMEA();
    }

    // Finally confirm that the routine CAREV messages are flowing in and we haven't lost the modem.
    // This message is sent from the modem to host at the end of every cycle and confirms good comms from the modem.
    if( !simulateHardware() && caRevTime_.elapsed() > caRevTimeout_ )
    {
        logger_.syslog( "Failed to receive CAREV within timeout", Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
    }

    return RUNNABLE;
}


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


Component::RunState Micromodem::stopping()
{
    if( verbosity_ > 4 ) 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 1 fault: " + loadControl_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }
    }
    return STOPPED;
}


Component::RunState Micromodem::stopped()
{
    if( verbosity_ > 4 ) logger_.syslog( "Stopped", Syslog::INFO );
    if( keepPowerOn() || isDusblRequested() || isDataRequested() || !outbuffer_.isEmpty() )
    {
        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 Micromodem::sendRangePings()
{
    // TODO, actually track when we are waiting for SNTTA.
    // For now, we just
    if( !outbuffer_.isEmpty() )
    {
        return;
    }

    // If all Pinged flags set, then reset them all.
    bool allPinged = true;
    for( int iTrans = 0; iTrans < MicromodemIF::NUM_TRANS; ++iTrans )
    {
        if( transChannelType_[iTrans] != TRANS_CHANNEL_NONE && !transChannelPinged_[iTrans] )
        {
            allPinged = false;
        }
    }
    if( allPinged )
    {
        for( int iTrans = 0; iTrans < MicromodemIF::NUM_TRANS; ++iTrans )
        {
            transChannelPinged_[iTrans] = false;
        }
    }

    sendRangePingsById();
    sendRangePingsByFreq();
    sendRangePingsByCode();
}

void Micromodem::sendRangePingsById()
{
    for( int iTrans = 0; iTrans < MicromodemIF::NUM_TRANS; ++iTrans )
    {
        if( transChannelType_[iTrans] == TRANS_CHANNEL_ID )
        {
            const Str nmeaCmd( "$CCMPC" );
            if( !transChannelPinged_[iTrans] )
            {
                const Str localAddr( localAddressCfgSetting_ );
                const Str transChannel( ( int )transChannelCfgSetting_[iTrans] );
                bufferNMEAStrs( nmeaCmd, localAddr, transChannel );
                pingsSent_ |= ( 1 << iTrans );
                pingChannels_[0] = iTrans;
                pingChannels_[1] = -1;
                pingChannels_[2] = -1;
                pingChannels_[3] = -1;
                transChannelPinged_[iTrans] = true;
                break;
            }
            if( verbosity_ > 1 ) logger_.syslog( "In sendRangePingsById, pingsSent_=", pingsSent_, Syslog::INFO );
        }
    }
}

void Micromodem::sendRangePingsByFreq()
{
    // Narrowband transponder interrogation via CCPNT
    bool anyFreqs = false;
    for( int iTrans = 0; iTrans < MicromodemIF::NUM_TRANS; ++iTrans )
    {
        if( transChannelType_[iTrans] == TRANS_CHANNEL_FREQENCY && !transChannelPinged_[iTrans] )
        {
            anyFreqs = true;
            break;
        }
    }

    if( !anyFreqs )
    {
        return;
    }

    const Str nmeaCmd( "$CCPNT" );
    Str rangeTxFreqCfgStr = Str( ( int ) rangeTxFreqCfgSetting_ );
    Str rangeTxTimeCfgStr = Str( ( int ) rangeTxTimeCfgSetting_ );
    Str rangeRxTimeCfgStr = Str( ( int ) rangeRxTimeCfgSetting_ );
    const Str tflag( "3" );
    // Assume 3km max range.
    // Assume 50 ms turn-around time, assume 1500 m/s speed of sound
    //const Str timeout( int( ( 3 * 2.0 / 1500 ) * 1000 + TAT ) );
    const Str timeout( 4000 + ( int ) rangeTATCfgSetting_ );

    // up to four receive_seq_codes for Sellers xponder
    int nFreqs = 0;
    Str freqs[4];
    for( int iTrans = 0; iTrans < MicromodemIF::NUM_TRANS && nFreqs < 4; ++iTrans )
    {
        if( transChannelType_[iTrans] == TRANS_CHANNEL_FREQENCY && !transChannelPinged_[iTrans] )
        {
            freqs[nFreqs] = Str( ( size_t ) transChannelCfgSetting_[iTrans], 10 );
            pingChannels_[nFreqs] = iTrans;
            ++nFreqs;
            transChannelPinged_[iTrans] = true;
            pingsSent_ |= ( 1 << iTrans );
        }
    }
    for( ; nFreqs < 4; ++nFreqs )
    {
        freqs[nFreqs] = Str( "0" );
        pingChannels_[nFreqs] = -1;
    }

    if( verbosity_ > 1 ) logger_.syslog( "In sendRangePingsByFreq, pingsSent_=", pingsSent_, Syslog::INFO );

    bufferNMEAStrs( nmeaCmd, rangeTxFreqCfgStr, rangeTxTimeCfgStr, rangeRxTimeCfgStr, timeout,
                    freqs[0], freqs[1], freqs[2], freqs[3], tflag );

}

void Micromodem::sendRangePingsByCode()
{
    // TODO: add more config options for hard-coded values below...
    bool anySeqCodes = false;
    for( int iTrans = 0; iTrans < MicromodemIF::NUM_TRANS; ++iTrans )
    {
        if( transChannelType_[iTrans] == TRANS_CHANNEL_CODE && !transChannelPinged_[iTrans] )
        {
            anySeqCodes = true;
            break;
        }
    }

    if( anySeqCodes )
    {

        // $CCPGT,Ftx,nbits,tx_seq_code,timeout,Frx,rx_code1,rx_code2,rx_code3,rx_code4,bandwidth,reserved*CS
        // Previous versions of firmware

        // In version 2.0.32731 of the micromodem firmware, the CCPGT command was changed as follows
        // $CCPGT,Mode,Ftx,nbits_tx/nsyms,tx_seq_code/dir,transponder_timeout_ms,Frx,nbits_rx,rx_seq_code1, rx_seq_code2,rx_seq_code3,rx_seq_code4, Bandwidth_Hz_tx,Bandwidth_Hz_rx,reserved*CS
        // For example: $CCPGT,1,10000,40,0,1000,24000,28,4A1C0370,6C8F7D30,2107E320,37BCD1E0,4000,4000,0

        const Str nmeaCmd( "$CCPGT" );

        const Str mode( 1 ); // For gateway buoy
        //const Str mode( 0 ); // For Sellers xponder

        const Str transmitFreq( 14500 ); // For Sellers xponder and HF Gateway buoy

        const Str nBitsTX( 40 ); // For gateway buoy
        //const Str nBitsTX( 28 ); // For Sellers xponder

        const Str seqCodeDir( 0 ); // For gateway buoy
        //const Str seqCodeDir( "6F8F7A90" ); // For Sellers xponder

        // Assume 3km max range.
        // REMUS LBL is 50 ms turn-around time, assume 1500 m/s speed of sound
        //const Str timeout( int( ( 3 * 2.0 / 1500 ) * 1000 + 50 ) );
        const Str timeout( 1000 ); // per default in manual

        const Str receiveFreq( 24000 );

        const Str nBitsRX( 28 );

        // up to four receive_seq_codes for Sellers xponder
        int nRxSeqCode = 0;
        Str rxSeqCodes[4];
        for( int iTrans = 0; iTrans < MicromodemIF::NUM_TRANS && nRxSeqCode < 4; ++iTrans )
        {
            if( transChannelType_[iTrans] == TRANS_CHANNEL_CODE && !transChannelPinged_[iTrans] )
            {
                rxSeqCodes[nRxSeqCode] = Str( ( size_t ) transChannelCfgSetting_[iTrans] );
                pingChannels_[nRxSeqCode] = iTrans;
                ++nRxSeqCode;
                transChannelPinged_[iTrans] = true;
                pingsSent_ |= ( 1 << iTrans );
            }
        }
        for( ; nRxSeqCode < 4; ++nRxSeqCode )
        {
            rxSeqCodes[nRxSeqCode] = Str( "0" );
            pingChannels_[nRxSeqCode] = -1;
        }

        const Str bandwidthTX( 4000 ); // For Sellers xponder and gateway buoy

        const Str bandwidthRX( 4000 ); // This doesn't really matter if we're receiving on the DUSBL.
        const Str reserved( "0" );

        bufferNMEAStrs( nmeaCmd, mode, transmitFreq, nBitsTX, seqCodeDir, timeout, receiveFreq, nBitsRX,
                        rxSeqCodes[0], rxSeqCodes[1], rxSeqCodes[2], rxSeqCodes[3], bandwidthTX, bandwidthRX, reserved );

    }
}

void Micromodem::sendDUSBLPing()
{
    // $CCPGT,Ftx,nbits,tx_seq_code,timeout,Frx,rx_code1,rx_code2,rx_code3,rx_code4,bandwidth,reserved*CS
    // Previous versions of firmware

    // In version 2.0.32731 of the micromodem firmware, the CCPGT command was changed as follows
    // $CCPGT,Mode,Ftx,nbits_tx/nsyms,tx_seq_code/dir,transponder_timeout_ms,Frx,nbits_rx,rx_seq_code1, rx_seq_code2,rx_seq_code3,rx_seq_code4, Bandwidth_Hz_tx,Bandwidth_Hz_rx,reserved*CS
    // For example: $CCPGT,1,10000,40,0,1000,24000,28,4A1C0370,6C8F7D30,2107E320,37BCD1E0,4000,4000,0

    const Str nmeaCmd( "$CCPGT" );
    const Str transmitFreq( 14500 ); // For Sellers xponder and HF Gateway buoy

    const Str nBitsTX( 40 ); // For gateway buoy
    //const Str nBitsTX( 28 ); // For Sellers xponder

    const Str nBitsRX( 28 );
    //const Str transmitSeqCode( "4A1C0370" ); // replaced by dusblPingCodeCfgSetting_.toString()
    // //const Str transmitSeqCode( "6F8F7A90" );

    // Assume 3km max range.
    // REMUS LBL is 50 ms turn-around time, assume 1500 m/s speed of sound
    //const Str timeout( int( ( 3 * 2.0 / 1500 ) * 1000 + 50 ) );
    const Str timeout( 1000 ); // per default in manual
    const Str receiveFreq( 24000 );

    const Str mode( 1 ); // For gateway buoy
    //const Str mode( 0 ); // For Sellers xponder

    const Str seqCodeDir( 0 ); // For gateway buoy
    //const Str seqCodeDir( "6F8F7A90" ); // For Sellers xponder

    // up to four receive_seq_codes for Sellers xponder
    const Str receiveSeqCode1( "4A1C0370" );
    const Str receiveSeqCode2( "6C8F7D30" );
    const Str receiveSeqCode3( "2107E320" );
    const Str receiveSeqCode4( "37BCD1E0" );

    const Str bandwidthTX( 4000 ); // For Sellers xponder and gateway buoy

    const Str bandwidthRX( 4000 ); // This doesn't really matter if we're receiving on the DUSBL.
    const Str reserved( "0" );

    bufferNMEAStrs( nmeaCmd, mode, transmitFreq, nBitsTX, seqCodeDir, timeout, receiveFreq, nBitsRX,
                    receiveSeqCode1, receiveSeqCode2, receiveSeqCode3, receiveSeqCode4, bandwidthTX, bandwidthRX, reserved );

    // Ignore TOF retuned in SNTTA response
    pingChannels_[0] = -1;
    pingChannels_[1] = -1;
    pingChannels_[2] = -1;
    pingChannels_[3] = -1;
    wasDusblRequested_ = true;
}

void Micromodem::sendingWaitForSignal()
{
    if( belowSurfaceThreshold() && sendTime_.elapsed() > resendPeriodCfgSetting_ )
    {
        sendTime_ = Timestamp::Now();
        const Str nmeaCmd( "$CCMPC" );
        const Str localAddr( localAddressCfgSetting_ );
        const Str destStr = Str( destinationAddressCfgSetting_ );
        logger_.syslog( "Sending ping looking for remote modem", Syslog::INFO );
        bufferNMEAStrs( nmeaCmd, localAddr, destStr );
    }
}

void Micromodem::sendingFillBuffer()
{
    // First see if we have data to send.
    // Send messages
    if( sendDataToShore_ )
    {
        Str sendFilename = Supervisor::GetToShoreFilename( Service::SERVICE_COURIER );
        if( sendFilename == Str::EMPTY_STR && sendExpressCfgSetting_ )
        {
            sendFilename = Supervisor::GetToShoreFilename( Service::SERVICE_EXPRESS );
        }
        if( verbosity_ > 2 ) logger_.syslog( "sendFilename = " + sendFilename, Syslog::INFO );
        if( sendFilename != Str::EMPTY_STR && sendFilename != sendPacket_.sendFilename_ )
        {
            communicationsDone_ = false;
            // 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 )
                                       / maxDownlinkDataSize_;
            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;
                    }
                }
            }
        }
        else if( sendFilename == Str::EMPTY_STR )
        {
            if( !communicationsDone_ )
            {
                communicationsDone_ = true;
            }
        }
        sendPacket_.sendFilename_ = sendFilename;
    }
    else
    {
        sendPacket_.sendFilename_ = Str::EMPTY_STR;
    }

    if( sendPacket_.sendFilename_ == Str::EMPTY_STR )
    {
        if( verbosity_ > 2 ) logger_.syslog( "No more packets to send", Syslog::INFO );
        platformCommunicationsWriter_->write( Units::BOOL, true );
    }
    else
    {
        FileInStream inStream( sendPacket_.sendFilename_.cStr() );
        if( inStream.isReadable() )
        {
            uint16_t maxPacket = ( sendPacket_.sendFilesize_ - 1 )
                                 / maxDownlinkDataSize_;
            size_t sendPosition = ( maxPacket - sendPacket_.packetsLeft_ ) * maxDownlinkDataSize_;
            inStream.seek( sendPosition );

            sendPacket_.dataSize_ = inStream.read( ( char* )sendPacket_.data_,
                                                   maxDownlinkDataSize_ ).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_;
            }
            if( fillBuffer( ( unsigned char* )&sendPacket_,
                            sendPacket_.dataSize_ + SENDPACKET_HEADER_SIZE ) )
            {
                // Start the clock and move to next state
                sendTime_ = Timestamp::Now();
                commsState_ = SENDING_TRANSMIT;
            }
            else // we never get here
            {
                commsState_ = SENDING_FILL_BUFFER;
            }
        }
        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 resend logic
}

// Fill buffer for sending message to GSS
bool Micromodem::fillBuffer( const unsigned char* msg, size_t nMsgBytes )
{
    fillOutgoingPacket( outgoingPacket_, msg, nMsgBytes );
    //$CCCYC,Cmd,Src,Dest,Rate,ACK,#frames
    Str addrStr = Str( localAddressCfgSetting_ );
    Str destStr = Str( destinationAddressCfgSetting_ );
    Str rateStr = Str( dataRateCfgSetting_ );
    Str framesStr = Str( outgoingPacket_.frameCount_ );
    bufferNMEA( "$CCCYC", 6, "0", 1, addrStr.cStr(), addrStr.size(), destStr.cStr(), destStr.size(), rateStr.cStr(), rateStr.size(), "0", 1, framesStr.cStr(), framesStr.size() );
    return true;
}

void Micromodem::sendingTransmit()
{
    if( verbosity_ > 4 ) logger_.syslog( Str( "************** SENDING_TRANSMIT **************\n" ), Syslog::INFO );
    // Slow down simulation of sending packets.
    if( sendTime_.elapsed() > acousticResponseTimeoutCfgSetting_ )
    {
        if( isDataRequested() )
        {
            logger_.syslog( Str( "Ack response timeout failure, going into ping mode." ), Syslog::ERROR );
            commsState_ = SENDING_WAIT_FOR_SIGNAL;
        }
        else
        {
            logger_.syslog( Str( "Ack response timeout, but no longer waiting." ), Syslog::INFO );
            commsState_ = SENDING_FILL_BUFFER;
        }
    }
}

void Micromodem::sendingVerified()
{
    if( verbosity_ > 4 ) logger_.syslog( Str( "************** SENDING_VERIFIED **************\n" ), Syslog::INFO );

    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( verbosity_ > 4 ) 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( verbosity_ > 4 ) 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;
        /*
        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;
    }
}

// Returns true if below surface (as defined in Vertical Control)
bool Micromodem::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 Micromodem::logVoltageAndCurrent() // TODO: Elevate to superclass
{
    loadControl_.requestVoltageAndCurrent();
    if( loadControl_.hasError() )
    {
        logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
        stop();
    }
}

bool Micromodem::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 Micromodem::isDataRequested()
{
    return platformCommunicationsWriter_->isAnyDataRequested();
    /*
        return rxTimeWriter_-> isDataRequested()
               || txTimeWriter_-> isDataRequested()
               || rangeWriter_->isDataRequested()
               || remoteAddressWriter_->isDataRequested()
               || localAddressWriter_->isDataRequested();
    */
}

bool Micromodem::isDusblRequested()
{
    DEBUG_LOG( "isDusblRequested returning " + Str( dusblPingCodeReader_->wasTouchedSinceLastRun( this ) ? "true" : "false" ) );

    return dusblPingCodeReader_->wasTouchedSinceLastRun( this );
}

int Micromodem::pingsRequested()
{
    if( !pingsRequestedReader_->wasTouchedSinceLastRun( this ) || !pingsRequestedReader_->read( Units::ENUM, pingsRequested_ ) )
    {
        pingsRequested_ = 0;
    }

    DEBUG_LOG( "isPingRequested returning " + Str( pingsRequested_, 10 ) );

    return pingsRequested_;
}

Str* Micromodem::createNMEA( const char* nmeaCmd, size_t cmdLength,
                             const char* nmeaArg1, size_t length1,
                             const char* nmeaArg2, size_t length2,
                             const char* nmeaArg3, size_t length3,
                             const char* nmeaArg4, size_t length4,
                             const char* nmeaArg5, size_t length5,
                             const char* nmeaArg6, size_t length6,
                             const char* nmeaArg7, size_t length7,
                             const char* nmeaArg8, size_t length8,
                             const char* nmeaArg9, size_t length9,
                             const char* nmeaArg10, size_t length10,
                             const char* nmeaArg11, size_t length11,
                             const char* nmeaArg12, size_t length12,
                             const char* nmeaArg13, size_t length13,
                             const char* nmeaArg14, size_t length14 )
{
    // ToDo: Handle hyroid_gateway_modem_prefix

    if( cmdLength == 0 )
    {
        cmdLength = strlen( nmeaCmd );
    }

    size_t len = cmdLength + 5;
    if( nmeaArg1 != NULL && nmeaArg1 != Str::EMPTY_STR.cStr() )
    {
        len += length1 + 1;
    }
    if( nmeaArg2 != NULL && nmeaArg2 != Str::EMPTY_STR.cStr() )
    {
        len += length2 + 1;
    }
    if( nmeaArg3 != NULL && nmeaArg3 != Str::EMPTY_STR.cStr() )
    {
        len += length3 + 1;
    }
    if( nmeaArg4 != NULL && nmeaArg4 != Str::EMPTY_STR.cStr() )
    {
        len += length4 + 1;
    }
    if( nmeaArg5 != NULL && nmeaArg5 != Str::EMPTY_STR.cStr() )
    {
        len += length5 + 1;
    }
    if( nmeaArg6 != NULL && nmeaArg6 != Str::EMPTY_STR.cStr() )
    {
        len += length6 + 1;
    }
    if( nmeaArg7 != NULL && nmeaArg7 != Str::EMPTY_STR.cStr() )
    {
        len += length7 + 1;
    }
    if( nmeaArg8 != NULL && nmeaArg8 != Str::EMPTY_STR.cStr() )
    {
        len += length8 + 1;
    }
    if( nmeaArg9 != NULL && nmeaArg9 != Str::EMPTY_STR.cStr() )
    {
        len += length9 + 1;
    }
    if( nmeaArg10 != NULL && nmeaArg10 != Str::EMPTY_STR.cStr() )
    {
        len += length10 + 1;
    }
    if( nmeaArg11 != NULL && nmeaArg11 != Str::EMPTY_STR.cStr() )
    {
        len += length11 + 1;
    }
    if( nmeaArg12 != NULL && nmeaArg12 != Str::EMPTY_STR.cStr() )
    {
        len += length12 + 1;
    }
    if( nmeaArg13 != NULL && nmeaArg13 != Str::EMPTY_STR.cStr() )
    {
        len += length13 + 1;
    }
    if( nmeaArg14 != NULL && nmeaArg14 != Str::EMPTY_STR.cStr() )
    {
        len += length14 + 1;
    }
    Str* nmea = new Str();
    nmea->set( NULL, len );

    size_t pos = 0;
    nmea->setInside( nmeaCmd, pos, cmdLength );

    pos += cmdLength;
    if( nmeaArg1 != NULL && nmeaArg1 != Str::EMPTY_STR.cStr() )
    {
        nmea->setChar( pos, ',' );
        nmea->setInside( nmeaArg1, pos + 1, length1 );
        pos += length1 + 1;
    }
    if( nmeaArg2 != NULL && nmeaArg2 != Str::EMPTY_STR.cStr() )
    {
        nmea->setChar( pos, ',' );
        nmea->setInside( nmeaArg2, pos + 1, length2 );
        pos += length2 + 1;
    }
    if( nmeaArg3 != NULL && nmeaArg3 != Str::EMPTY_STR.cStr() )
    {
        nmea->setChar( pos, ',' );
        nmea->setInside( nmeaArg3, pos + 1, length3 );
        pos += length3 + 1;
    }
    if( nmeaArg4 != NULL && nmeaArg4 != Str::EMPTY_STR.cStr() )
    {
        nmea->setChar( pos, ',' );
        nmea->setInside( nmeaArg4, pos + 1, length4 );
        pos += length4 + 1;
    }
    if( nmeaArg5 != NULL && nmeaArg5 != Str::EMPTY_STR.cStr() )
    {
        nmea->setChar( pos, ',' );
        nmea->setInside( nmeaArg5, pos + 1, length5 );
        pos += length5 + 1;
    }
    if( nmeaArg6 != NULL && nmeaArg6 != Str::EMPTY_STR.cStr() )
    {
        nmea->setChar( pos, ',' );
        nmea->setInside( nmeaArg6, pos + 1, length6 );
        pos += length6 + 1;
    }
    if( nmeaArg7 != NULL && nmeaArg7 != Str::EMPTY_STR.cStr() )
    {
        nmea->setChar( pos, ',' );
        nmea->setInside( nmeaArg7, pos + 1, length7 );
        pos += length7 + 1;
    }
    if( nmeaArg8 != NULL && nmeaArg8 != Str::EMPTY_STR.cStr() )
    {
        nmea->setChar( pos, ',' );
        nmea->setInside( nmeaArg8, pos + 1, length8 );
        pos += length8 + 1;
    }
    if( nmeaArg9 != NULL && nmeaArg9 != Str::EMPTY_STR.cStr() )
    {
        nmea->setChar( pos, ',' );
        nmea->setInside( nmeaArg9, pos + 1, length9 );
        pos += length9 + 1;
    }
    if( nmeaArg10 != NULL && nmeaArg10 != Str::EMPTY_STR.cStr() )
    {
        nmea->setChar( pos, ',' );
        nmea->setInside( nmeaArg10, pos + 1, length10 );
        pos += length10 + 1;
    }
    if( nmeaArg11 != NULL && nmeaArg11 != Str::EMPTY_STR.cStr() )
    {
        nmea->setChar( pos, ',' );
        nmea->setInside( nmeaArg11, pos + 1, length11 );
        pos += length11 + 1;
    }
    if( nmeaArg12 != NULL && nmeaArg12 != Str::EMPTY_STR.cStr() )
    {
        nmea->setChar( pos, ',' );
        nmea->setInside( nmeaArg12, pos + 1, length12 );
        pos += length12 + 1;
    }
    if( nmeaArg13 != NULL && nmeaArg13 != Str::EMPTY_STR.cStr() )
    {
        nmea->setChar( pos, ',' );
        nmea->setInside( nmeaArg13, pos + 1, length13 );
        pos += length13 + 1;
    }
    if( nmeaArg14 != NULL && nmeaArg14 != Str::EMPTY_STR.cStr() )
    {
        nmea->setChar( pos, ',' );
        nmea->setInside( nmeaArg14, pos + 1, length14 );
        pos += length14 + 1;
    }
    unsigned char c = 0;
    nmea->nmeaChecksum( c );

    char const* hex = "0123456789ABCDEF";
    nmea->setChar( pos, '*' );
    nmea->setChar( pos + 1, hex[( c >> 4 ) & 0xF] );
    nmea->setChar( pos + 2, hex[c & 0xF] );
    nmea->setInside( "\r\n", pos + 3, 2 );

    return nmea;
}

void Micromodem::bufferNMEA( const char* nmeaCmd, size_t cmdLength,
                             const char* nmeaArg1, size_t length1,
                             const char* nmeaArg2, size_t length2,
                             const char* nmeaArg3, size_t length3,
                             const char* nmeaArg4, size_t length4,
                             const char* nmeaArg5, size_t length5,
                             const char* nmeaArg6, size_t length6,
                             const char* nmeaArg7, size_t length7,
                             const char* nmeaArg8, size_t length8,
                             const char* nmeaArg9, size_t length9,
                             const char* nmeaArg10, size_t length10,
                             const char* nmeaArg11, size_t length11,
                             const char* nmeaArg12, size_t length12,
                             const char* nmeaArg13, size_t length13,
                             const char* nmeaArg14, size_t length14 )
{
    Str* nmea = createNMEA( nmeaCmd, cmdLength,
                            nmeaArg1, length1,
                            nmeaArg2, length2,
                            nmeaArg3, length3,
                            nmeaArg4, length4,
                            nmeaArg5, length5,
                            nmeaArg6, length6,
                            nmeaArg7, length7,
                            nmeaArg8, length8,
                            nmeaArg9, length9,
                            nmeaArg10, length10,
                            nmeaArg11, length11,
                            nmeaArg12, length12,
                            nmeaArg13, length13,
                            nmeaArg14, length14 );

    if( verbosity_ > 4 )logger_.syslog( "Nmea buf: " + *nmea, Syslog::DEBUG );

    outbuffer_.push( nmea );

    trySendNMEA();
}

void Micromodem::trySendNMEA()
{
    if( verbosity_ > 4 ) logger_.syslog( Str( "outbuffer_.isEmpty() = " ) + outbuffer_.isEmpty() +  ", cmdSentTime_.elapsed() = " + ( cmdSentTime_ == Timestamp::NOT_SET_TIME ? Str( "N/A" ) : Str( cmdSentTime_.elapsed().asDouble() ) ), Syslog::INFO );
    if( outbuffer_.isEmpty() )
    {
        return;
    }
    // Send or re-send if appropriate
    if( cmdSentTime_ == Timestamp::NOT_SET_TIME || cmdSentTime_.elapsed() > cmdResponseWait_ )
    {
        Str* nmea = outbuffer_.peek();
        if( cmdSentTime_ == Timestamp::NOT_SET_TIME )
        {
            if( verbosity_ > 0 ) logger_.syslog( "Nmea out: " + *nmea, Syslog::INFO );
        }
        else if( pingsSent_ != 0 && ( nmea->startsWith( "$CCMPC", 6 ) || nmea->startsWith( "$CCPNT", 6 ) || nmea->startsWith( "$CCPGT", 6 ) ) )
        {
            // Don't automatically resend pings
            cmdSentTime_ = Timestamp::NOT_SET_TIME;
            logger_.syslog( "Ping timeout for: " + *nmea, Syslog::INFO );
            // Note that this invalidates nmea variables
            // Note that we checked for outbuffer_.isEmpty() above
            delete outbuffer_.pop();

            // Send the next ping ASAP
            pingsSent_ = 0;
            if( pingsRequested() )
            {
                sendRangePings();
            }
            return;
        }
        else
        {
            logger_.syslog( "Nmea resend: " + *nmea, Syslog::ERROR );
        }

        if( !simulateHardware() )
        {
            uart_ << nmea->cStr();
        }
        else if( nmea->size() > 3 && nmea->startsWith( "$CC", 3 ) )
        {
            // Simulate a response (basic echo)
            size_t star = nmea->find( '*' );
            if( star != Str::NO_POS )
            {
                Str* simNmea = new Str( *nmea );
                simNmea->setInside( "$CA", 0, 3 );
                unsigned char c = 0;
                simNmea->nmeaChecksum( c );

                char const* hex = "0123456789ABCDEF";
                simNmea->setChar( star + 1, hex[( c >> 4 ) & 0xF] );
                simNmea->setChar( star + 2, hex[c & 0xF] );

                simbuffer_.push( simNmea );
            }
        }
        cmdSentTime_ = Timestamp::Now();
    }
}

void Micromodem::tryRcvNMEA()
{
    size_t bytesRead = 0;
    if( !simulateHardware() && uart_.canReadUntil( "\r", 1 ) )
    {
        //  Try to read CRLF if possible, otherwise, just CR
        uart_.readUntil( deviceResponse_, MAX_DEVICE_RESPONSE, "\r\n", 2 );
        if( uart_.bytesRead() == 0 )
        {
            uart_.readUntil( deviceResponse_, MAX_DEVICE_RESPONSE, "\r", 1 );
        }
        bytesRead = uart_.bytesRead();
    }
    else if( simulateHardware() && !simbuffer_.isEmpty() && ( cmdSentTime_ == Timestamp::NOT_SET_TIME || cmdSentTime_.elapsed() > 5 ) )
    {
        Str* simResponse = simbuffer_.pop();
        strncpy( deviceResponse_, simResponse->cStr(), simResponse->length() );
        bytesRead = simResponse->length();
        deviceResponse_[bytesRead] = 0;
        delete simResponse;
    }
    if( bytesRead > 0 )
    {
        if( verbosity_ > 3 || ( verbosity_ > 1 && strncmp( deviceResponse_, "$CAREV", 6 ) != 0 ) ) logger_.syslog( "Nmea in: ", deviceResponse_, Syslog::INFO );
        // Check if the checksum is valid
        if( !Str::NmeaValidate( deviceResponse_, bytesRead ) )
        {
            unsigned char expected = 0;
            Str::NmeaChecksum( expected, deviceResponse_, bytesRead );
            logger_.syslog( Str( "Response from modem failed NMEA checksum: " ) + deviceResponse_ + " expected " + Str( ( int )expected, 16 ), Syslog::ERROR );
            // Triggers a resend of last command.
            cmdSentTime_ = Timestamp::NOT_SET_TIME;
            return;
        }

        // Check if we got the right response
        Str* cmdStr = outbuffer_.peek();
        const char* cmd = cmdStr ? cmdStr->cStr() : "";
        size_t ccStar = cmdStr ? cmdStr->find( '*' ) : Str::NO_POS;
        size_t outStar = Str::Find( deviceResponse_, '*' );

        // Seems that $CAREV comes along randomly, no need to acknowlege anything
        if( strncmp( deviceResponse_, "$CAREV", 6 ) == 0 )
        {
            receivedCAREV();
        }
        // $CAACK comes along asynchronously
        else if( strncmp( deviceResponse_, "$CAACK", 6 ) == 0 )
        {
            receivedCAACK();
        }
        // Response to $CACST:
        else if( strncmp( deviceResponse_, "$CACST", 6 ) == 0 )
        {
            // Basically ignore
            receivedNMEA();
        }
        // $CAERR comes along unexpectedlu
        else if( strncmp( deviceResponse_, "$CAERR", 6 ) == 0 )
        {
            receivedCAERR( cmdStr );
        }

        // Response to $CAMPR:
        else if( strncmp( deviceResponse_, "$CAMPR", 6 ) == 0 )
        {
            // Basically ignore
            receivedCAMPR();
        }

        // Response to $CATXF:
        else if( strncmp( deviceResponse_, "$CATXF", 6 ) == 0 )
        {
            // Basically ignore
            receivedNMEA();
        }

        // Response to $CATXP:
        else if( strncmp( deviceResponse_, "$CATXP", 6 ) == 0 )
        {
            // Basically ignore
            receivedNMEA();
        }

        // Response to $CAXST:
        else if( strncmp( deviceResponse_, "$CAXST", 6 ) == 0 )
        {
            // Basically ignore
            receivedNMEA();
        }


        // Response to $CCCFG:
        else if( strncmp( cmd + 3, "CFG", 3 ) == 0 )
        {
            if( outStar == ccStar && strncmp( cmd + 3, deviceResponse_ + 3, ccStar - 4 ) == 0 )
            {
                receivedNMEA();
            }
            else
            {
                logger_.syslog( "CFG response from modem unexpected: ", deviceResponse_, Syslog::ERROR );
            }
        }

        // Response to $CCCFQ:
        else if( strncmp( cmd + 3, "CFQ", 3 ) == 0 )
        {
            if( strncmp( deviceResponse_ + 3, "CFG", 3 ) == 0 )
            {
                receivedNMEA();
            }
            else
            {
                logger_.syslog( "CFQ response from modem unexpected: ", deviceResponse_, Syslog::ERROR );
            }
        }

        // Response to $CCMSC:
        else if( strncmp( cmd + 3, "MSC", 3 ) == 0 )
        {
            if( strncmp( deviceResponse_ + 3, "REV", 3 ) == 0 || strncmp( deviceResponse_ + 3, "MSC", 3 ) == 0 )
            {
                receivedNMEA();
            }
            else
            {
                logger_.syslog( "MSC response from modem unexpected: ", deviceResponse_, Syslog::ERROR );
            }
        }

        // Response to $CCTXD:
        else if( strncmp( cmd + 3, "TXD", 3 ) == 0 )
        {
            if( strncmp( deviceResponse_ + 3, "TXD", 3 ) == 0 )
            {
                receivedNMEA();
            }
            else if( strncmp( deviceResponse_ + 3, "ACK", 3 ) == 0 )
            {
                receivedNMEA();
            }
            else
            {
                logger_.syslog( "TXD response from modem unexpected: ", deviceResponse_, Syslog::ERROR );
            }
        }

        // Response to various range commands:
        else if( strncmp( deviceResponse_, "$SNTTA", 6 ) == 0 )
        {
            receivedSNTTA();
        }

        // Other responses that match the command:
        else if( strncmp( cmd + 3, deviceResponse_ + 3, 3 ) == 0 )
        {
            receivedNMEA();
        }

        // Seems that $CATMS can also come along randomly
        else if( strncmp( deviceResponse_, "$CATMS", 6 ) == 0 )
        {
            receivedCATMS();
        }

        // Maybe we got a power-up response...
        else if( outbuffer_.isEmpty() )
        {
            receivedNMEA();
            return;
        }

        else
        {
            logger_.syslog( "Response from modem unexpected: ", deviceResponse_, Syslog::ERROR );
        }

    }
}

void Micromodem::receivedNMEA()
{

    if( !outbuffer_.isEmpty() )
    {
        if( pingsSent_ == 0 && !wasDusblRequested_ )
        {
            delete outbuffer_.pop();
            cmdSentTime_ = Timestamp::NOT_SET_TIME;
        }
    }

    if( strncmp( deviceResponse_, "$CAACK", 6 ) == 0 )
    {
        receivedCAACK();
    }
    else if( strncmp( deviceResponse_, "$CACFG", 6 ) == 0 )
    {
        receivedCACFG();
    }
    else if( strncmp( deviceResponse_, "$CACLK", 6 ) == 0 )
    {
        receivedCACLK();
    }
    else if( strncmp( deviceResponse_, "$CACYC", 6 ) == 0 )
    {
        receivedCACYC();
    }
    else if( strncmp( deviceResponse_, "$CADRQ", 6 ) == 0 )
    {
        receivedCADRQ();
    }
    else if( strncmp( deviceResponse_, "$CAMPC", 6 ) == 0 )
    {
        receivedCAMPC();
    }
    else if( strncmp( deviceResponse_, "$CAPGT", 6 ) == 0 )
    {
        receivedCAPGTorCAPNT();
    }
    else if( strncmp( deviceResponse_, "$CAPNT", 6 ) == 0 )
    {
        receivedCAPGTorCAPNT();
    }
    else if( strncmp( deviceResponse_, "$CAREV", 6 ) == 0 )
    {
        receivedCAREV();
    }
    else if( strncmp( deviceResponse_, "$CARXD", 6 ) == 0 )
    {
        receivedCARXD();
    }
    else if( strncmp( deviceResponse_, "$CATMQ", 6 ) == 0 )
    {
        receivedCATMQ();
    }
    else if( strncmp( deviceResponse_, "$CATMS", 6 ) == 0 )
    {
        receivedCATMS();
    }
    else if( strncmp( deviceResponse_, "$CATXD", 6 ) == 0 )
    {
        receivedCATXD();
    }
    else if( strncmp( deviceResponse_, "$SNPGT", 6 ) == 0 )
    {
        receivedSNPGT();
    }
}

void Micromodem::simCAACK()
{
    //$CAACK,Src (of ACK),Dst,Frame#,ACK bit
    double distance;
    SimSlate::Read( SimSlate::LBL_TRANSPONDER_1_RANGE_M, distance );
    if( distance <= maxSimAcousticDistance_ )
    {
        Str src = Str( outgoingPacket_.destination_ );
        Str dst = Str( outgoingPacket_.source_ );
        Str frame = Str( outgoingPacket_.frameToSend_ + 1 );
        Str* simNmea = createNMEAStrs( "$CAACK", src, dst, frame, "1" );
        simbuffer_.push( simNmea );
    }
    else
    {
        logger_.syslog( "Not sending simCAACK because distance=", distance, Syslog::INFO );
    }
}

void Micromodem::receivedCAACK()
{
    // Ack data sent
    //$CAACK,Src (of ACK),Dst,Frame#,ACK bit

    int srcOfAck, dstOfAck, frame;
    if( 3 != sscanf( deviceResponse_, "$CAACK,%d,%d,%d", &srcOfAck, &dstOfAck, &frame ) )
    {
        logger_.syslog( "Received invalid CAACK response", Syslog::ERROR );
        commsState_ = SENDING_FILL_BUFFER;
        return;
    }

    if( srcOfAck != destinationAddressCfgSetting_ )
    {
        logger_.syslog( "Received CAACK with unexpected srcOfAck address: ", srcOfAck, Syslog::INFO );
        return;
    }

    if( dstOfAck != localAddressCfgSetting_ )
    {
        logger_.syslog( "Received CAACK with unexpected dstOfAck address: ", dstOfAck, Syslog::INFO );
        return;
    }

    if( frame > outgoingPacket_.frameCount_ || frame < 1 )
    {
        logger_.syslog( "Received CAACK with bad frame#: ", frame, Syslog::INFO );
        return;
    }
    outgoingPacket_.ack_[frame - 1] = true;

    bool allAck = true;
    for( int iFrame = 0; iFrame < outgoingPacket_.frameCount_; ++iFrame )
    {
        allAck &= outgoingPacket_.ack_[iFrame];
    }

    if( allAck )
    {
        commsState_ = SENDING_VERIFIED;
    }


}

void Micromodem::receivedCACFG()
{
    if( startingState_ == STARTING_WRITE_CFG_WAIT && strncmp( deviceResponse_ + 7, "BND", 3 ) == 0 )
    {
        startingState_ = STARTING_SET_CLOCK;
    }
}

void Micromodem::receivedCACLK()
{
    // TODO: check that the time drift is acceptable
    if( startingState_ == STARTING_SET_CLOCK_WAIT )
    {
        startingState_ = STARTING_DONE;
    }
}

void Micromodem::receivedCACYC()
{
    // Ignore CACYC on outgoing data unless simulating, pay attention on incoming data
    // $CACYC,Cmd,Src,Dest,Rate,ACK,#frames
    // Ignore cmd and ack
    int src, dst, rate, frames;
    if( 4 != sscanf( deviceResponse_, "$CACYC,%*d,%d,%d,%d,%*d,%d", &src, &dst, &rate, &frames ) )
    {
        logger_.syslog( "Received invalid $CACYC: ", deviceResponse_, Syslog::ERROR );
        return;
    }

    if( dst == localAddressCfgSetting_ )
    {
        if( verbosity_ > 1 )
        {
            logger_.syslog( "Have incoming data", Syslog::INFO );
        }
        // We have incoming data
        prepIncomingPacket( incomingPacket_, src, dst, rate );
        if( frames < 0 )
        {
            logger_.syslog( "Received invalid $CACYC frames#: ", frames, Syslog::ERROR );
            initUmodemPacket( incomingPacket_ );
            return;
        }
        incomingPacket_.frameCount_ = frames;
    }

    if( src == localAddressCfgSetting_ && simulateHardware() && dst == outgoingPacket_.destination_ )
    {
        if( verbosity_ > 1 )
        {
            logger_.syslog( "Simulate outgoing data request", Syslog::INFO );
        }
        simCADRQ();
    }
}

void Micromodem::simCADRQ()
{
    // Fake a $CADRQ,Time  ,Src,Dst, ACK,NBytes, Frame#
    //        $CADRQ,HHMMSS,SRC,DEST,ACK,N,      F#
    Str time = Timestamp::Now().toSmallString().substr( 9 );
    Str src = Str( outgoingPacket_.source_ );
    Str dst = Str( outgoingPacket_.destination_ );
    Str nbytes = Str( outgoingPacket_.frameSize_[ outgoingPacket_.frameToSend_ ] );
    Str frame = Str( outgoingPacket_.frameToSend_ + 1 );
    Str* simNmea = createNMEAStrs( "$CADRQ", time, src, dst, "0", nbytes, frame );
    simbuffer_.push( simNmea );
}

void Micromodem::receivedCADRQ()
{
    // Data is being requested
    // $CADRQ,Time,Src,Dst,ACK,NBytes, Frame#
    if( outgoingPacket_.frameToSend_ >= outgoingPacket_.frameCount_ )
    {
        logger_.syslog( "Received CADRQ with no data to send", Syslog::INFO );
        return;
    }
    int time, src, dst, ack, nbytes, frame;

    if( 6 != sscanf( deviceResponse_, "$CADRQ,%d,%d,%d,%d,%d,%d", &time, &src, &dst, &ack, &nbytes, &frame ) )
    {
        logger_.syslog( "Received invalid CADRQ: ", deviceResponse_, Syslog::ERROR );
        commsState_ = SENDING_FILL_BUFFER;
        return;
    }

    if( src != localAddressCfgSetting_ )
    {
        logger_.syslog( "Received CADRQ with unexpected source address: ", src, Syslog::INFO );
        //return; // In this case we prefer not to retrn and attempt to send the message anyway
    }

    if( dst != destinationAddressCfgSetting_ )
    {
        logger_.syslog( "Received CADRQ with unexpected dest address: ", dst, Syslog::INFO );
        //return; // In this case we prefer not to retrn and attempt to send the message anyway
    }

    if( frame - 1 != outgoingPacket_.frameToSend_ )
    {
        if( frame > outgoingPacket_.frameCount_ || frame < 1 )
        {
            logger_.syslog( "Received CADRQ with bad frame#: ", frame, Syslog::ERROR );
            commsState_ = SENDING_FILL_BUFFER;
            //return; // In this case we prefer not to retrn and attempt to send the message anyway
        }
        else
        {
            logger_.syslog( "Received CADRQ with unexpected frame#: ", frame, Syslog::INFO );
            outgoingPacket_.frameToSend_ = frame - 1;
        }
    }

    if( nbytes  < 0 || nbytes > FrameSizes_[outgoingPacket_.rate_] )
    {
        logger_.syslog( Str( "Received CADRQ with bad size: " ) + nbytes + " expected <= " + FrameSizes_[outgoingPacket_.rate_], Syslog::ERROR );
        commsState_ = SENDING_FILL_BUFFER;
        //return; // In this case we prefer not to retrn and attempt to send the message anyway
    }

    // $CCTXD,Src,Dst,ACK,<Hex data>
    Str addrStr = Str( localAddressCfgSetting_ );
    Str destStr = Str( destinationAddressCfgSetting_ );
    if( nbytes > outgoingPacket_.frameSize_[ outgoingPacket_.frameToSend_ ] )
    {
        nbytes = outgoingPacket_.frameSize_[ outgoingPacket_.frameToSend_ ];
    }
    char* dataPtr = ( char* )( outgoingPacket_.hexData_[outgoingPacket_.frameToSend_] );
    bufferNMEA( "$CCTXD", 6, addrStr.cStr(), addrStr.size(), destStr.cStr(), destStr.size(), "1", 1, dataPtr, nbytes * 2 );
    trySendNMEA();
}

void Micromodem::receivedCAERR( const Str* cmdStr )
{
    //$CAERR,170308,DATA_TIMEOUT,1
    logger_.syslog( "Got error from modem: ", deviceResponse_, Syslog::ERROR );
    if( NULL != cmdStr && cmdStr->startsWith( "$CCTXD", 6 ) )
    {
        while( !outbuffer_.isEmpty() )
        {
            delete outbuffer_.pop();
            pingsSent_ = 0;
            wasDusblRequested_ = false;
            cmdSentTime_ = Timestamp::NOT_SET_TIME;
        }
        commsState_ = SENDING_FILL_BUFFER;
    }

}

void Micromodem::receivedCAMPC()
{
    /* Ignore for now and wait for SNTTA unless simulating */
    if( simulateHardware() )
    {
        if( verbosity_ > 1 ) logger_.syslog( "In receivedCAMPC, pingsSent_=", pingsSent_, Syslog::INFO );
        if( pingsSent_ != 0 || commsState_ == SENDING_WAIT_FOR_SIGNAL )
        {
            simCAMPR();
        }
    }
}

void Micromodem::simCAMPR()
{
    // Fake a $CAMPR,SRC,DEST,TRAVELTIME*CS
    // Assume speed of sound in water is 1500 m/s
    double distance;
    Str src, dst = Str( localAddressCfgSetting_, 10 ),  travelTime;
    if( commsState_ == SENDING_WAIT_FOR_SIGNAL )
    {
        src = Str( destinationAddressCfgSetting_ );
        SimSlate::Read( SimSlate::LBL_TRANSPONDER_1_RANGE_M, distance );
        if( distance <= maxSimAcousticDistance_ )
        {
            travelTime = Str( distance / 1500 );
        }
        else
        {
            logger_.syslog( "Not sending simCAMPR because distance=", distance, Syslog::INFO );
        }
    }
    else if( ( pingsSent_ & 0x01 && transChannelType_[0] == TRANS_CHANNEL_ID ) )
    {
        src = Str( transChannelCfgSetting_[0], 10 );
        SimSlate::Read( SimSlate::LBL_TRANSPONDER_1_RANGE_M, distance );
        if( distance <= maxSimAcousticDistance_ ) travelTime = Str( distance / 1500 );
    }
    else if( ( pingsSent_ & 0x02 && transChannelType_[1] == TRANS_CHANNEL_ID ) )
    {
        src = Str( transChannelCfgSetting_[1], 10 );
        SimSlate::Read( SimSlate::LBL_TRANSPONDER_2_RANGE_M, distance );
        if( distance <= maxSimAcousticDistance_ ) travelTime = Str( distance / 1500 );
    }
    else if( ( pingsSent_ & 0x04 && transChannelType_[2] == TRANS_CHANNEL_ID ) )
    {
        src = Str( transChannelCfgSetting_[2], 10 );
        SimSlate::Read( SimSlate::LBL_TRANSPONDER_3_RANGE_M, distance );
        if( distance <= maxSimAcousticDistance_ ) travelTime = Str( distance / 1500 );
    }

    if( travelTime != Str::EMPTY_STR )
    {
        Str* simNmea = createNMEAStrs( "$CAMPR", src, dst, travelTime );
        simbuffer_.push( simNmea );
    }
}

void Micromodem::receivedCAMPR()
{
    logger_.syslog( "In receivedCAMPR, commsState_=", commsState_, Syslog::INFO );
    if( commsState_ == SENDING_WAIT_FOR_SIGNAL || pingsSent_ != 0 )
    {
        // We didn't do this in receivedNMEA, so let's do it now...
        if( !outbuffer_.isEmpty() )
        {
            delete outbuffer_.pop();
        }
        cmdSentTime_ = Timestamp::NOT_SET_TIME;

        // Parse $CAMPR,SRC,DEST,TRAVELTIME*CS
        int src, dst;
        double travelTime;
        if( 3 != sscanf( deviceResponse_, "$CAMPR,%d,%d,%lf", &src, &dst, &travelTime ) )
        {
            logger_.syslog( "Received invalid CAMPR: ", deviceResponse_, Syslog::ERROR );
            return;
        }
        else if( dst != localAddressCfgSetting_ )
        {
            // ignore
            if( verbosity_ > 1 ) logger_.syslog( "Ignoring CAMPR with dst=", dst, Syslog::INFO );
            return;
        }

        if( commsState_ == SENDING_WAIT_FOR_SIGNAL )
        {
            commsState_ = SENDING_FILL_BUFFER;
        }
        else
        {
            for( int iTrans = 0; iTrans < MicromodemIF::NUM_TRANS && iTrans < LBLNavigationIF::NUM_PINGS; ++iTrans )
            {
                if( transChannelType_[iTrans] == TRANS_CHANNEL_ID )
                {
                    if( !isnan( travelTime ) )
                    {
                        //printf( "Writing TOF %d: %f\n", iTrans, travelTime );
                        pingTimeOfFightWriters_[iTrans]->write( Units::SECOND, travelTime );
                    }

                    // Send the next ping ASAP
                    pingsSent_ = ( pingsSent_ & ~( 1 << iTrans ) ) | ( 1 << iTrans );
                    if( pingsRequested() )
                    {
                        sendRangePings();
                    }
                }
            }
        }
    }
}

void Micromodem::receivedCAPGTorCAPNT()
{
    /* Ignore for now and wait for SNTTA unless simulating */
    if( simulateHardware() )
    {
        if( verbosity_ > 1 ) logger_.syslog( "In receivedCAPGTorCAPNT, pingsSent_=", pingsSent_, Syslog::INFO );
        if( pingsSent_ != 0 )
        {
            simSNTTA();
        }
    }
}

void Micromodem::receivedCAREV()
{
    // Read the modem major revision, ignore everything else for now.
    // $CAREV,HHMMSS,IDENT,V.VV.V.VV*CS
    int commaIndex = 0;
    for( size_t i = 0; i < MAX_DEVICE_RESPONSE; ++i )
    {
        if( commaIndex == 3 )
        {
            majorRevision_ = ( int )( deviceResponse_[i] - '0' );
            break;
        }
        if( deviceResponse_[i] == ',' )
        {
            ++commaIndex;
        }
    }
    caRevTime_ = Timestamp::Now(); // Log the time that CAREV was received
}

void Micromodem::receivedCARXD()
{
    // There's incomnig data.
    // $CARXD,SRC,DEST,ACK,F#,HH…HH*CS
    int src( -1 ), dst( -1 ), frame( -1 ), dataAt( -1 ), dataEnd( -1 );
    if( 3 != sscanf( deviceResponse_, "$CARXD,%d,%d,%*d,%d,%n%*[0-9A-F]%n*%*2c", &src, &dst, &frame, &dataAt, &dataEnd ) )
    {
        logger_.syslog( "Received invalid $CARXD: ", deviceResponse_, Syslog::ERROR );
        return;
    }
    if( src != incomingPacket_.source_ )
    {
        logger_.syslog( "Received unexpected src in CARXD: ", src, Syslog::ERROR );
        return;
    }
    if( dst != incomingPacket_.destination_ )
    {
        logger_.syslog( "Received unexpected dst in CARXD: ", dst, Syslog::ERROR );
        return;
    }
    if( frame < 1 || frame > incomingPacket_.frameCount_ )
    {
        logger_.syslog( "Received bad frame# in CARXD: ", frame, Syslog::ERROR );
        return;
    }
    if( 1 != sscanf( deviceResponse_ + dataAt, FrameFmts_[incomingPacket_.rate_], incomingPacket_.hexData_[frame - 1] ) )
    {
        logger_.syslog( "Received invalid data in CARXD: ", deviceResponse_, Syslog::ERROR );
        return;
    }

    // Check to make sure the end comes after the beginning. sscanf will populate dataEnd with -1 when it can't find the end.
    if( dataAt < 0 || dataEnd <= dataAt )
    {
        logger_.syslog( "Could not determine end of data in CARXD:", deviceResponse_, Syslog::ERROR );
        return;
    }

    // Populate the frame size for this frame
    incomingPacket_.frameSize_[frame - 1] = ( dataEnd - dataAt );

    // Log data if desired
    if( verbosity_ > 2 )
    {
        logger_.syslog( Str( "Frame:" ) + frame, Syslog::INFO );
        logger_.syslog( Str( "Frame Size:" ) + ( dataEnd - dataAt ), Syslog::INFO );
    }

    // Populate the ack
    incomingPacket_.ack_[frame - 1] = true;
    bool allAck = true;
    for( int iFrame = 0; iFrame < incomingPacket_.frameCount_; ++iFrame )
    {
        allAck &= incomingPacket_.ack_[iFrame];
    }

    // once we have all the acks we can deserialize
    if( allAck )
    {
        deserializeIncomingPacket( incomingPacket_ );
    }
}

void Micromodem::receivedCATMS()
{
    // TODO: check that the time drift is acceptable
    if( startingState_ == STARTING_SET_CLOCK_WAIT )
    {
        startingState_ = STARTING_DONE;
    }
}

void Micromodem::receivedCATMQ()
{
    // TODO: check that the time drift is acceptable
    if( startingState_ == STARTING_SET_CLOCK_WAIT )
    {
        startingState_ = STARTING_DONE;
    }
}

void Micromodem::receivedCATXD()
{
    outgoingPacket_.frameToSend_++;
    // Advance the clock a bit
    sendTime_ = Timestamp::Now();

    if( simulateHardware() )
    {
        if( outgoingPacket_.frameToSend_ < outgoingPacket_.frameCount_ )
        {
            simCADRQ();
        }
        else
        {
            for( outgoingPacket_.frameToSend_ = 0; outgoingPacket_.frameToSend_ < outgoingPacket_.frameCount_; ++outgoingPacket_.frameToSend_ )
            {
                simCAACK();
            }
        }
    }
}

void Micromodem::receivedSNPGT()
{
    wasDusblRequested_ = false;
    // We didn't do this in receivedNMEA, so let's do it now...
    if( !outbuffer_.isEmpty() )
    {
        delete outbuffer_.pop();
    }
    cmdSentTime_ = Timestamp::NOT_SET_TIME;
    /* Here we expect a response that looks something like the following:
    $SNPGT,14500,28,4A1C0370,1000,24000,0,0,0,0,2000,0*4E
    $CATXP,280*48
    $CATXF,280*5E
    $SNTTA,,,,,170444.66*5C
    So we'll simply flush the buffer for now
    */
    if( !simulateHardware() )
    {
        uart_.flush();
    }
}

void Micromodem::simSNTTA()
{
    // Fake a $SNTTA,TA,TB,TC,TD,hhmmsss.ss*C
    // Assume speed of sound in water is 1500 m/s
    double a, b, c;
    SimSlate::Read( SimSlate::LBL_TRANSPONDER_1_RANGE_M, a );
    SimSlate::Read( SimSlate::LBL_TRANSPONDER_2_RANGE_M, b );
    SimSlate::Read( SimSlate::LBL_TRANSPONDER_3_RANGE_M, c );

    Str ta = ( pingsSent_ & 0x01 && ( transChannelType_[0] == TRANS_CHANNEL_FREQENCY || transChannelType_[0] == TRANS_CHANNEL_CODE ) ) ? Str( a / 1500 ) : Str::EMPTY_STR;
    Str tb = ( pingsSent_ & 0x02 && ( transChannelType_[1] == TRANS_CHANNEL_FREQENCY || transChannelType_[1] == TRANS_CHANNEL_CODE ) ) ? Str( b / 1500 ) : Str::EMPTY_STR;
    Str tc = ( pingsSent_ & 0x04 && ( transChannelType_[2] == TRANS_CHANNEL_FREQENCY || transChannelType_[2] == TRANS_CHANNEL_CODE ) ) ? Str( c / 1500 ) : Str::EMPTY_STR;
    Str time = Timestamp::Now().toSmallString().substr( 9 );

    Str* simNmea = createNMEA( "$SNTTA", 6,
                               ta.length() > 0 ? ta.cStr() : "_", ta.length(),
                               tb.length() > 0 ? tb.cStr() : "_", tb.length(),
                               tc.length() > 0 ? tc.cStr() : "_", tc.length(),
                               "_", 0,
                               time.cStr(), time.length() );
    simbuffer_.push( simNmea );
}

void Micromodem::receivedSNTTA()
{
    if( pingsSent_ != 0 )
    {
        // We didn't do this in receivedNMEA, so let's do it now...
        if( !outbuffer_.isEmpty() )
        {
            delete outbuffer_.pop();
        }
        cmdSentTime_ = Timestamp::NOT_SET_TIME;

        /*
            Expect to see something like:
            $SNTTA,TA,TB,TC,TD,hhmmsss.ss*CS
            Example: $SNTTA,0.0733,0.0416,,,014524.00*5C
        */
        Str resp = deviceResponse_;
        int nTokens = 0;
        Str* tokens = resp.split( nTokens, ",", 1 );
        double t[4];
        for( int iToken = 1; iToken <= 4; ++iToken )
        {
            if( iToken < nTokens )
            {
                double foo = Str::ToDouble( tokens[iToken].cStr() );
                t[ iToken - 1 ] = foo;
            }
            else
            {
                t[ iToken ] = nanf( "" );
            }

        }
        delete[] tokens;

        bool pingsToSend = false;
        for( int iT = 0; iT < 4; ++iT )
        {
            if( transChannelType_[iT] == TRANS_CHANNEL_FREQENCY || transChannelType_[iT] == TRANS_CHANNEL_CODE )
            {
                if( pingChannels_[iT] >= 0 && pingChannels_[iT] < LBLNavigationIF::NUM_PINGS && !isnan( t[ iT ] ) )
                {
                    pingTimeOfFightWriters_[pingChannels_[iT]]->write( Units::SECOND, t[ iT ] );
                }
                pingsToSend = true;
                // Clear the bit.
                pingsSent_ = ( pingsSent_ & ~( 1 << iT ) ) | ( 1 << iT );
            }
        }

        // Send the next ping ASAP
        if( pingsToSend && pingsRequested() )
        {
            sendRangePings();
        }
    }
}


// TODO: atx and aty

/// Should return [myNamespace]::SIMULATE_HARDWARE, or [myNamespace]::POWER, etc
ConfigURI Micromodem::getConfigURI( ConfigOption configOption ) const
{
    return configOption == CONFIG_SIMULATE_HARDWARE ? MicromodemIF::SIMULATE_HARDWARE : ConfigURI::NO_CONFIG_URI;
}

void Micromodem::publishData( void )
{
    if( gotAcousticWakeup_ )
    {
        if( verbosity_ > 1 ) logger_.syslog( "publishing acoustic wakeup flag", Syslog::INFO );
        acousticWakeupWriter_->write( Units::COUNT, 1, dataTimestamp_ );
    }
    if( gotRangeRequestMessage_ )
    {
        if( verbosity_ > 1 ) logger_.syslog( "publishing range request flag", Syslog::INFO );
        rangeRequestReceivedWriter_->write( Units::COUNT, 1, dataTimestamp_ );
    }
    if( gotRangeMessage_ )
    {
        if( verbosity_ > 1 ) logger_.syslog( "publishing range", Syslog::INFO );
        remoteAddressWriter_->write( Units::COUNT, remoteAddress_, dataTimestamp_ );
        localAddressWriter_->write( Units::COUNT, localAddress_, dataTimestamp_ );
        rangeWriter_->write( Units::METER, range_, dataTimestamp_ );
    }
}


void Micromodem::initUmodemPacket( MicromodemPacket& packet, int rate )
{
    packet.source_ = 0;
    packet.destination_ = 0;
    packet.rate_ = AuvMath::Limit( 0, rate, 5 );
    packet.frameCount_ = 0;
    packet.frameToSend_ = 999;
    for( int iFrame = 0; iFrame < FrameCounts_[packet.rate_]; ++iFrame )
    {
        packet.frameSize_[iFrame] = 0;
        *packet.hexData_[iFrame] = 0;
        packet.ack_[iFrame] = false;
    }
    packet.binSize_ = 0;
    *packet.binData_ = 0;
}

void Micromodem::prepIncomingPacket( MicromodemPacket& packet, int src, int dst, int rate )
{
    initUmodemPacket( packet, rate );
    packet.source_ = src;
    packet.destination_ = dst;
}

void Micromodem::deserializeIncomingPacket( MicromodemPacket& packet )
{
    uint8_t c;
    packet.binSize_ = 0;
    for( int iFrame = 0; iFrame < packet.frameCount_; ++iFrame )
    {
        for( int iData = 0; iData < packet.frameSize_[iFrame] / 2; ++iData )
        {
            c = hex2uint8_t( packet.hexData_[ iFrame ][ iData * 2 ] ) << 4;
            c += hex2uint8_t( packet.hexData_[ iFrame ][ iData * 2 + 1 ] );
            packet.binData_[packet.binSize_++] = c;
        }
    }
    bool parsed = DataReceiver::ReceiveSbd( ( const char* )packet.binData_, packet.binSize_, keyText_.asString().cStr(), logger_ );

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

void Micromodem::fillOutgoingPacket( MicromodemPacket& packet, const unsigned char* msg, int nMsgBytes )
{
    int rate = AuvMath::Limit( 0, dataRateCfgSetting_, 5 );
    // Reduce the rate if we don't have much data to send.
    for( int iRate = 0; iRate < rate; ++iRate )
    {
        if( nMsgBytes <= FrameCounts_[iRate] * FrameSizes_[iRate] )
        {
            rate = iRate;
            break;
        }
    }
    if( rate != dataRateCfgSetting_ && verbosity_ > 1 )
    {
        logger_.syslog( Str( "Using rate " ) + packet.rate_ + " for " + ( int )nMsgBytes + " bytes.", Syslog::INFO );
    }
    initUmodemPacket( packet, rate );
    packet.source_ = localAddressCfgSetting_;
    packet.destination_ = destinationAddressCfgSetting_;
    packet.frameToSend_ = 0;

    char const* hex = "0123456789ABCDEF";

    for( int iFrame = 0; iFrame < FrameCounts_[rate] && packet.binSize_ < nMsgBytes; ++iFrame )
    {
        for( int iData = 0; iData < FrameSizes_[rate] && packet.binSize_ < nMsgBytes; ++iData )
        {
            uint8_t datum = msg[packet.binSize_];
            packet.hexData_[ iFrame ][ iData * 2 ] = hex[( datum >> 4 ) & 0xF];
            packet.hexData_[ iFrame ][ iData * 2 + 1 ] = hex[datum & 0xF];
            ++packet.frameSize_[ iFrame ];
            packet.binData_[ packet.binSize_ ] = datum;
            ++packet.binSize_;
        }
        packet.ack_[ iFrame ] = false;
        packet.hexData_[ iFrame ][ packet.frameSize_[ iFrame ] * 2 ] = 0;
        ++packet.frameCount_;
        if( verbosity_ > 1 )
        {
            logger_.syslog( Str( "Outgoing frame #" ) + packet.frameCount_ + ", "  + packet.frameSize_[ iFrame ] + " bytes: "  + ( char* ) packet.hexData_[ iFrame ], Syslog::INFO );
        }
    }
}

uint8_t Micromodem::hex2uint8_t( uint8_t hex )
{
    if( hex >= '0' && hex <= '9' ) return hex - '0';
    else if( hex >= 'A' && hex <= 'F' ) return hex - 'A' + 0X0a;
    else if( hex >= 'a' && hex <= 'f' ) return hex - 'a' + 0x0a;
    else return 0;
}