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

/***

!!! This AcousticModem_Benthos_ATM900 interface requires that the Acoustic Modem to which it is connected be configured prior to use !!!

***/

#include "AcousticModem_Benthos_ATM900.h"
#include "AcousticModem_Benthos_ATM900IF.h"

#include <cstdlib>
#include <errno.h>
#include <unistd.h>  // include for the access method
#include <sys/stat.h>

#include "data/ConfigReader.h"
#include "data/SimSlate.h"
#include "data/UniversalDataWriter.h"
#include "io/StrIOStream.h"
#include "supervisor/DataReceiver.h"
#include "supervisor/Supervisor.h"
#include "units/Units.h"
#include "VehicleIF.h"

AcousticModem_Benthos_ATM900::AcousticModem_Benthos_ATM900( const Module* module )
    : SyncSensorComponent( AcousticModem_Benthos_ATM900IF::NAME, module ),
      commsState_( SENDING_FILL_BUFFER ),
      outgoingCommsBuffer_( true ),
      outgoingCommsNow_( NULL ),
      debug_( false ),
      verbosity_( 0 ),
      loadControl_( AcousticModem_Benthos_ATM900IF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      loadControl2_( AcousticModem_Benthos_ATM900IF::LOAD_CONTROL2, !simulateHardware(), logger_, this ),
      hasLoadControl2_( false ),
      startTime_( Timestamp::NOT_SET_TIME ),
      commandModeTimeStart_( Timestamp::NOT_SET_TIME ),
      onlineModeTimeStart_( Timestamp::NOT_SET_TIME ),
      powerOffTimeStart_( Timestamp::NOT_SET_TIME ),
      poTimeout_( 20 ), // measured to be ~12 sec on bench
      commandModeTimeout_( 10 ),
      onlineModeTimeout_( 10 ),
      powerDownTimeout_( 3 ),
      uart_( AcousticModem_Benthos_ATM900IF::UART, AcousticModem_Benthos_ATM900IF::BAUD, 0.6, logger_, 4095 ),
      keyText_(),
      verboseCfgSetting_( 3 ),
      txPowerCfgSetting_( 8 ),
      localAddressCfgSetting_( 0 ),
      sbdAddressCfgSetting_( -1 ),
      transponderAddressCfgSetting_( -1 ),
      surfaceThresholdCfgSetting_( 1 ),
      sendExpressCfgSetting_( 0 ),
      sendDataToShoreCfgSetting_( true ),
      commandModeSent_( false ),
      commandModeAcknowledged_( false ),
      onlineModeSent_( false ),
      onlineModeAcknowledged_( false ),
      verboseSettingSent_( false ),
      verboseSettingAcknowledged_( false ),
      txPowerSettingSent_( false ),
      txPowerSettingAcknowledged_( false ),
      localAddressSettingSent_( false ),
      localAddressSettingAcknowledged_( false ),
      currentRemoteAddressSent_( false ),
      currentRemoteAddressAcknowledged_( false ),
      acousticResponseTimeout_( 20.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 ),
      awaitingAckTransmitAck_( false ),
      commRate_( 0 ),
      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 ),
      currentRemoteAddress_( -1 ),
      incomingRemoteAddress_( -1 ),
      incomingLocalAddress_( -1 ),
      sendPacket_( MAX_DOWNLINK_DATA_SIZE, &logger_ )
{
    // Configuration inputs
    verbosityCfgReader_ = newConfigReader( AcousticModem_Benthos_ATM900IF::VERBOSITY_CFG );
    txPowerCfgReader_ = newConfigReader( AcousticModem_Benthos_ATM900IF::TX_POWER_CFG );
    localAddressCfgReader_ = newConfigReader( AcousticModem_Benthos_ATM900IF::LOCAL_ADDRESS_CFG );
    sbdAddressCfgReader_ = newConfigReader( AcousticModem_Benthos_ATM900IF::SBD_ADDRESS_CFG );
    surfaceThresholdCfgReader_ = newConfigReader( AcousticModem_Benthos_ATM900IF::SURFACE_THRESHOLD_CFG );
    sendExpressCfgReader_ = newConfigReader( AcousticModem_Benthos_ATM900IF::SEND_EXPRESS_CFG );
    transponderAddressCfgReader_ = newConfigReader( VehicleIF::ID_CFG );
    sendDataToShoreCfgReader_ = newConfigReader( VehicleIF::SEND_DATA_TO_SHORE_CFG );

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

    // Slate outputs
    acousticWakeupWriter_ = newDataWriter( AcousticModem_Benthos_ATM900IF::ACOUSTIC_WAKEUP );
    rangeRequestReceivedWriter_ = newDataWriter( AcousticModem_Benthos_ATM900IF::RANGE_REQUEST );
    remoteAddressWriter_ = newDataWriter( AcousticModem_Benthos_ATM900IF::REMOTE_ADDRESS_READING );
    localAddressWriter_ = newDataWriter( AcousticModem_Benthos_ATM900IF::LOCAL_ADDRESS_READING );
    rangeWriter_ = newDataWriter( AcousticModem_Benthos_ATM900IF::RANGE_READING );

    // Universal outputs
    rxTimeWriter_ = newUniversalWriter( UniversalURI::ACOUSTIC_RECEIVE_TIME, Units::SECOND, 0.11 );
    txTimeWriter_ = newUniversalWriter( UniversalURI::ACOUSTIC_TRANSMIT_TIME, Units::SECOND, 0.11 );
    platformCommunicationsWriter_ = newUniversalWriter( UniversalURI::PLATFORM_COMMUNICATIONS, Units::BOOL, 0 );

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

    deviceResponse_[0] = '\0';

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

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

    // Secondary power supply
    StrValue loadCtrl2;
    if( Slate::ReadOnce( AcousticModem_Benthos_ATM900IF::LOAD_CONTROL2, loadCtrl2, logger_ ) )
    {
        if( !loadCtrl2.asString().endsWith( "null" ) )
        {
            logger_.syslog( "Found secondary power supply at: " + loadCtrl2.asString(), Syslog::INFO );
            hasLoadControl2_ = true;
        }
    }
}


AcousticModem_Benthos_ATM900::~AcousticModem_Benthos_ATM900()
{
}


void AcousticModem_Benthos_ATM900::run()
{
}


void AcousticModem_Benthos_ATM900::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;
    }

    if( !localAddressCfgReader_->read( Units::COUNT, localAddressCfgSetting_ ) )
    {
        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;
    }

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

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

void AcousticModem_Benthos_ATM900::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 );
        }

        if( hasLoadControl2_ )
        {
            logger_.syslog( "Powering down secondary power supply.", Syslog::INFO );
            if( !loadControl2_.powerDown() )  // First power down then query for faults next cycle
            {
                logger_.syslog( "Failed to power down secondary power supply.", Syslog::FAULT );
                this->setFailure( FailureMode::HARDWARE );
            }
        }

        powerOffTimeStart_ = Timestamp::Now();
        uart_.close();
    }
}


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

    commRate_ = 0;
    commandModeSent_ = false;
    commandModeAcknowledged_ = false;
    verboseSettingSent_ = false;
    verboseSettingAcknowledged_ = false;
    txPowerSettingSent_ = false;
    txPowerSettingAcknowledged_ = false;
    localAddressSettingSent_ = false;
    localAddressSettingAcknowledged_ = false;
    currentRemoteAddressSent_ = false;
    currentRemoteAddressAcknowledged_ = false;

    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: AcousticModem_Benthos_ATM900 load controller failed to power up.\n" ), Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
        return START;
    }
    if( hasLoadControl2_ )
    {
        logger_.syslog( "Powering up secondary power supply.", Syslog::INFO );
        Timespan::Milliseconds( 15 ).sleepFor();
        if( !loadControl2_.powerUp() )
        {
            logger_.syslog( "Failed to power up secondary load control board", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }
    }

    logger_.syslog( "Initializing AcousticModem_Benthos_ATM900." );

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

    readConfig();

    if( simulateHardware() )
    {
        commandModeAcknowledged_ = localAddressSettingSent_ = currentRemoteAddressSent_ = true;
        return RUNNABLE;
    }
    else if( startTime_.elapsed() > poTimeout_ )
    {
        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_ && uart_.canReadUntil( '\n' ) ) // then get the header lines
    {
        while( uart_.canReadUntil( '\n' ) )
        {
            int bytesRead = uart_.canReadUntil( '\n' );
            uart_.readUntil( deviceResponse_, sizeof( deviceResponse_ ), '\n' );
            logger_.syslog( Str( deviceResponse_, bytesRead - 1 ), Syslog::DEBUG );
            char* connectAt = strstr( deviceResponse_, "CONNECT" );
            char* bitsSecAt = strstr( deviceResponse_, "bits/sec" );

            // Parse out comms rate
            if( connectAt && bitsSecAt )
            {
                if( sscanf( deviceResponse_, "CONNECT %d bits/sec", &commRate_ ) == 1 )
                {
                    logger_.syslog( "commRate: ", commRate_, Syslog::INFO );
                    return STARTING;
                }
                else
                {
                    commRate_ = 0;
                    logger_.syslog( "invalid communications rate; deviceResponse_:", deviceResponse_, Syslog::FAULT );
                    this->setFailure( FailureMode::COMMUNICATIONS );
                    return STOP;
                }
            }
            // TODO: Do something else with the banner lines.
        }
    }
    // TODO: set instrument time here
    else if( ( commRate_ > 0 ) && !verboseSettingAcknowledged_ ) // you've seen the CONNECT message, one cycle ago...
    {
        if( !sendVerboseCfgSetting() )
        {
            verboseSettingSent_ = false;
        }
    }
    else if( verboseSettingAcknowledged_ && !txPowerSettingAcknowledged_ )
    {
        if( !sendTxPowerFromCfg() )
        {
            txPowerSettingSent_ = false;
        }
    }
    else if( txPowerSettingAcknowledged_ && !localAddressSettingAcknowledged_ )
    {
        if( !sendLocalAddressFromCfg() )
        {
            localAddressSettingSent_ = false;
        }
    }
    else if( localAddressSettingAcknowledged_ )
    {
        return runnable();
    }

    return STARTING;
}

bool AcousticModem_Benthos_ATM900::enterCommandMode()
{
    if( simulateHardware() )
    {
        commandModeSent_ = commandModeAcknowledged_ = true;
        return true;
    }
    if( !commandModeSent_ ) // you've seen the CONNECT message, one cycle ago...
    {
        uart_ << "+++\r"; // enter command mode
        commandModeSent_ = true;
        logger_.syslog( "entering command mode", Syslog::INFO );
        onlineModeSent_ = onlineModeAcknowledged_ = commandModeAcknowledged_ = false;
        commandModeTimeStart_ = Timestamp::Now();
        return true;
    }
    if( commandModeSent_ && !commandModeAcknowledged_ )
    {
        logger_.syslog( "checking for command mode acknowledgment", Syslog::DEBUG );
        if( clearUserPrompt() > 0 )
        {
            commandModeAcknowledged_ = true;
            logger_.syslog( "command mode acknowledged", Syslog::INFO );
        }
        else if( commandModeTimeStart_.elapsed() > commandModeTimeout_ )
        {
            logger_.syslog( "failed to enter command mode", Syslog::FAULT );
            return false;
        }
    }
    return true;
}

bool AcousticModem_Benthos_ATM900::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;
        onlineModeTimeStart_ = Timestamp::Now();
        return true;
    }
    if( onlineModeSent_ && !onlineModeAcknowledged_ )
    {
        logger_.syslog( "checking for online mode acknowledgment", Syslog::DEBUG );
        if( uart_.canReadUntil( "CONNECT" ) && uart_.canReadUntil( "bits/sec" ) )
        {
            uart_.readUntil( deviceResponse_, sizeof( deviceResponse_ ), "bits/sec", 8 );
            uart_.flush(); // TODO: come back and read the rest ( 1 of 4, Rate 1/2 CC 12.50ms MGP)
            onlineModeAcknowledged_ = true;
            logger_.syslog( "online mode acknowledged", Syslog::INFO );
        }
        else if( onlineModeTimeStart_.elapsed() > onlineModeTimeout_ )
        {
            logger_.syslog( "failed to enter online mode", Syslog::FAULT );
            return false;
        }
    }
    return true;
}

bool AcousticModem_Benthos_ATM900::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;
        return true;
    }
    else if( verboseSettingSent_ && !verboseSettingAcknowledged_ )
    {
        logger_.syslog( "checking for verbose setting acknowledgment", Syslog::DEBUG );
        if( uart_.flushCRLF().canReadUntil( '\n' ) )
        {
            int verboseRead;
            uart_.readUntil( deviceResponse_, sizeof( deviceResponse_ ), '\n' );
            if( sscanf( deviceResponse_, "Verbose         | %d", &verboseRead ) == 1 )
            {
                if( verboseCfgSetting_ == verboseRead )
                {
                    logger_.syslog( "set verbose to ", verboseRead, Syslog::INFO );
                    verboseSettingAcknowledged_ = true;
                    return true;
                }
                else
                {
                    logger_.syslog( "failed to set verbose to " + Str( verboseCfgSetting_ ) + ", device returned " + Str( verboseRead ) + " instead.", Syslog::ERROR );
                    this->setFailure( FailureMode::COMMUNICATIONS );
                    return false;
                }
            }
            else if( NULL == strstr( deviceResponse_, "user:" ) )
            {
                logger_.syslog( "failed to set verbose; deviceResponse_: ", deviceResponse_, Syslog::ERROR );
                this->setFailure( FailureMode::COMMUNICATIONS );
                return false;
            }
        }
    }
    return true;
}

bool AcousticModem_Benthos_ATM900::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;
        return true;
    }
    else if( txPowerSettingSent_ && !txPowerSettingAcknowledged_ )
    {
        logger_.syslog( "checking for transmit power setting acknowledgment", Syslog::DEBUG );
        if( uart_.flushCRLF().canReadUntil( '\n' ) )
        {
            int txPower = 0;
            uart_.readUntil( deviceResponse_, sizeof( deviceResponse_ ), '\n' );
            if( sscanf( deviceResponse_, "TxPower         | %d", &txPower ) == 1 )
            {
                if( txPowerCfgSetting_ == txPower )
                {
                    logger_.syslog( "set transmit power to ", txPower, Syslog::INFO );
                    txPowerSettingAcknowledged_ = true;
                    return true;
                }
                else
                {
                    logger_.syslog( "failed to set transmit power to " + Str( txPowerCfgSetting_ ) + ", device returned " + Str( txPower ) + " instead.", Syslog::ERROR );
                    this->setFailure( FailureMode::COMMUNICATIONS );
                    return false;
                }
            }
            else if( NULL == strstr( deviceResponse_, "user:" ) )
            {
                logger_.syslog( "failed to set transmit power; deviceResponse_: ", deviceResponse_, Syslog::ERROR );
                this->setFailure( FailureMode::COMMUNICATIONS );
                return false;
            }
        }
    }
    return true;
}

bool AcousticModem_Benthos_ATM900::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;
        return true;
    }
    else if( localAddressSettingSent_ && !localAddressSettingAcknowledged_ )
    {
        logger_.syslog( "checking for local address setting acknowledgment", Syslog::DEBUG );
        if( uart_.flushCRLF().canReadUntil( '\n' ) )
        {
            uart_.readUntil( deviceResponse_, sizeof( deviceResponse_ ), '\n' );
            if( sscanf( deviceResponse_, "LocalAddr       | %d", &localAddress_ ) == 1 )
            {
                if( localAddressCfgSetting_ == localAddress_ )
                {
                    logger_.syslog( "set local address to ", localAddress_, Syslog::INFO );
                    localAddressSettingAcknowledged_ = true;
                    return true;
                }
                else
                {
                    logger_.syslog( "failed to set local address to " + Str( localAddressCfgSetting_ ) + ", device returned " + Str( localAddress_ ) + " instead.", Syslog::ERROR );
                    this->setFailure( FailureMode::COMMUNICATIONS );
                    return false;
                }
            }
            else if( NULL == strstr( deviceResponse_, "user:" ) )
            {
                logger_.syslog( "failed to set local address; deviceResponse_: ", deviceResponse_, Syslog::ERROR );
                this->setFailure( FailureMode::COMMUNICATIONS );
                return false;
            }
        }
    }
    return true;
}

bool AcousticModem_Benthos_ATM900::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;
        return true;
    }
    if( currentRemoteAddressSent_ && !currentRemoteAddressAcknowledged_ )
    {
        logger_.syslog( "checking for remote address setting acknowledgment", Syslog::DEBUG );
        if( uart_.flushCRLF().canReadUntil( '\n' ) )
        {
            uart_.readUntil( deviceResponse_, sizeof( deviceResponse_ ), '\n' );
            if( sscanf( deviceResponse_, "RemoteAddr      | %d", &remoteAddress_ ) == 1 )
            {
                if( currentRemoteAddress_ == remoteAddress_ )
                {
                    logger_.syslog( "set remote address to ", remoteAddress_, Syslog::INFO );
                    currentRemoteAddressAcknowledged_ = true;
                    return true;
                }
                else
                {
                    logger_.syslog( "failed to set remote address to " + Str( currentRemoteAddress_ ) + ", device returned " + Str( remoteAddress_ ) + " instead.", Syslog::ERROR );
                    this->setFailure( FailureMode::COMMUNICATIONS );
                    return false;
                }
            }
            else if( NULL == strstr( deviceResponse_, "user:" ) )
            {
                logger_.syslog( "failed to set remote address; deviceResponse_: ", deviceResponse_, Syslog::ERROR );
                this->setFailure( FailureMode::COMMUNICATIONS );
                return false;
            }
        }
    }
    return true;
}

/// Pause
Component::RunState AcousticModem_Benthos_ATM900::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 AcousticModem_Benthos_ATM900::paused()
{
    if( debug_ ) logger_.syslog( "Paused", Syslog::INFO );
    if( keepPowerOn() || isDataRequested() || ( uart_.dataAvailable() > 0 ) ) return RESUME;
    return PAUSED;
}


Component::RunState AcousticModem_Benthos_ATM900::resume()
{
    if( debug_ ) 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();
    return RESUMING;
}


Component::RunState AcousticModem_Benthos_ATM900::resuming()
{
    if( debug_ ) 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 AcousticModem_Benthos_ATM900::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
    }

    // Allow tx power setting to change dynamically
    if( !txPowerSettingAcknowledged_ )
    {
        if( !sendTxPowerFromCfg() )
        {
            return STOP;
        }
        if( !txPowerSettingAcknowledged_ )
        {
            return RUNNABLE;
        }
    }

    // Outgoing modem comms
    if( 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();
            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;
        }
    }

    gotResponseNotReceived_ = false;
    gotAcousticWakeup_ = false;
    gotDebugRxMessage_ = false;
    gotDebugTxMessage_ = false;
    gotRangeRequestMessage_ = false;
    gotRangeMessage_ = false;

    if( simulateHardware() )
    {
        if( isnan( range_ ) ) range_ = 1000.; // TODO: Remove this temporary hack that gets the simulator started.
        double estimatedTravelTime = range_ / 1500.; // TODO: Make another (Estimation) component that will pass range and direction through a sound channel model to apply corrections.
        if( numPingsRequested_ == 1 ) range_ *= 2;
        if( ( numPingsReceived_ < numPingsRequested_ ) && ( ( transmitPingTime_.elapsed() > 2 * estimatedTravelTime ) || ( receivePingTime_.elapsed() > estimatedTravelTime ) ) )
        {
            gotRangeMessage_ = SimSlate::Read( SimSlate::HOMING_SENSOR_RANGE_M, range_ );
            if( gotRangeMessage_ )
            {
                dataTimestamp_ = Timestamp::Now();
                receivePingTime_ = dataTimestamp_;
                // TODO: gotDebugRxMessage_, etc
                numPingsReceived_ += 1;
                publishData();
                this->resetFailCount();
            }
        }
    }
    else
    {
        logVoltageAndCurrent();
        dataTimestamp_ = Timestamp::Now(); // 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.
        readAndParseResponses();
        if( gotRangeMessage_ ) numPingsReceived_ += 1;
        if( gotAcousticWakeup_ || gotDebugRxMessage_ || gotDebugTxMessage_ || gotRangeRequestMessage_ || gotRangeMessage_ )
        {
            publishData();
            this->resetFailCount();
        }
        else if( gotResponseNotReceived_ )
        {
            logger_.syslog( "No response from remote modem.", Syslog::ERROR );
            this->resetFailCount();
        }
    }

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

        if( acousticResponseTimeout_ < transmitPingTime_.elapsed().asDouble() )
        {
            numPingsReceived_ = 0;
            transmitPingTime_ = requestRange( remoteAddressRequested_, numPingsRequested_ ); // requestRange will write the address and number of pings requested to the slate
            logger_.syslog( "****** ping requested ******", Syslog::INFO );
        }
        else
        {
            logger_.syslog( "received new query, but waiting for acoustic response period to elapse", Syslog::DEBUG );
        }
    }

    return RUNNABLE;
}


Component::RunState AcousticModem_Benthos_ATM900::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 AcousticModem_Benthos_ATM900::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 AcousticModem_Benthos_ATM900::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 AcousticModem_Benthos_ATM900::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 );
            }
        }
        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();
        SendDestination* destination = data->getDestination();
        Str cmd;
        if( destination->getPath() != Str::EMPTY_STR )
        {
            cmd = "set " + destination->getPath() + " ";
            if( NULL != data->getUnit() )
            {
                cmd += data->getValue()->toString( *data->getUnit() ) + " " + data->getUnit()->getName();
            }
            else
            {
                cmd += "string \"" + data->getValue()->toString() + "\"";
            }
        }
        else
        {
            cmd = data->getValue()->toString();
        }
        logger_.syslog( destination->getScheme() + "://" + destination->getHost() + ": " + cmd, Syslog::INFO );
        delete data;

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

    }

    // 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 AcousticModem_Benthos_ATM900::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 );
        }
        if( !onlineModeAcknowledged_ )
        {
            return;
        }
    }

    if( awaitingAckTransmitAck_ )
    {
        return;
    }

    uart_ << outgoingCommsNow_->getData();
    startTime_ = Timestamp::Now();
    commsState_ = SENDING_TRANSMIT_VERIFY;
    logger_.syslog( "In sendingTransmit, set commsState_ = SENDING_TRANSMIT_VERIFY", Syslog::DEBUG );
}

void AcousticModem_Benthos_ATM900::sendingTransmitVerify()
{
    // Calculate time to actually send the data. Assume roughly 300 bps
    // acomms xfer rate
    int dataXferTime = ( int ) sendPacket_.dataSize_ / 32;
    if( startTime_.elapsed() > ( acousticResponseTimeout_ + dataXferTime ) )
    {
        logger_.syslog( Str( "Buffer send receipt timeout failure." ), Syslog::FAULT );
        commsState_ = SENDING_FILL_BUFFER;
        logger_.syslog( "In sendingTransmitVerify, timeout so set commsState_ = SENDING_FILL_BUFFER", Syslog::DEBUG );
    }
}

void AcousticModem_Benthos_ATM900::sendingAckWaiting()
{
    if( startTime_.elapsed() > acousticResponseTimeout_ )
    {
        logger_.syslog( Str( "Ack receipt timeout failure." ), Syslog::FAULT );
        commsState_ = SENDING_FILL_BUFFER;
        logger_.syslog( "In sendingAckWaiting, timeout so set commsState_ = SENDING_FILL_BUFFER", Syslog::DEBUG );
    }
}

void AcousticModem_Benthos_ATM900::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 AcousticModem_Benthos_ATM900::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 AcousticModem_Benthos_ATM900::logVoltageAndCurrent() // TODO: Elevate to superclass
{
    loadControl_.requestVoltageAndCurrent();
    if( loadControl_.hasError() )
    {
        logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
        stop();
    }
}


bool AcousticModem_Benthos_ATM900::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 AcousticModem_Benthos_ATM900::isCommsRequested()
{
    return sbdAddressCfgSetting_ >= 0 && sendDataToShoreCfgSetting_ && platformCommunicationsWriter_->isAnyDataRequested() && belowSurfaceThreshold();
}

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


bool AcousticModem_Benthos_ATM900::isDataRequested()
{
    return isCommsRequested()
           || isSendDataAvailable()
           || rxTimeWriter_-> isDataRequested()
           || txTimeWriter_-> isDataRequested()
           || rangeWriter_->isDataRequested()
           || remoteAddressWriter_->isDataRequested()
           || localAddressWriter_->isDataRequested();
}


bool AcousticModem_Benthos_ATM900::gotNewQuery()
{
    if( ( numPingsReceived_ < numPingsRequested_ ) && ( transmitPingTime_.elapsed().asFloat() < acousticResponseTimeout_.asFloat() * ( 1 + 0.5 * ( numPingsRequested_ - 1 ) ) ) )
    {
        return false; // Explicitly avoid new interrogations while waiting for response to previous.
    }
    else if( queryAddressRequestedReader_->wasTouchedSinceLastRun( this ) && numberOfPingsRequestedReader_->wasTouchedSinceLastRun( this ) )
    {
        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 );
        }
        return validAddress && validPingsRequested;
    }
    else
    {
        return false;
    }
}


Timestamp AcousticModem_Benthos_ATM900::requestRange( const int remoteAddress, const int numPings = 1 )
{
    if( numPings < 1 )
    {
        logger_.syslog( "Received request for less than 1 ping -- ignoring.", Syslog::DEBUG );
    }
    else 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 // ( 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_ << "oneway " << remoteAddress << " " << numPings << "\r"; // oneway command expects both modem and AcousticModem_Benthos_ATM900 to have @syncpps set to RTC (2).
    }
    clearUserPrompt();
    return Timestamp::Now();
}


// TODO: atx and aty



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

int AcousticModem_Benthos_ATM900::clearUserPrompt( void )
{
    int a_prompt_number( -1 ), prompt_number( -1 );
    uart_.flushCRLF(); // flush any empty lines
    if( uart_.canReadUntil( "UART Wakeup" ) )
    {
        commandModeSent_ = false;
    }
    while( uart_.canReadUntil( "user:", 5 ) && uart_.canReadUntil( '>' ) )
    {
        if( uart_.canReadUntil( '\n' ) )
        {
            uart_.readUntil( deviceResponse_, sizeof( deviceResponse_ ), '\n' );
        }
        else
        {
            uart_.readUntil( deviceResponse_, sizeof( deviceResponse_ ), '>' );
        }
        if( sscanf( deviceResponse_, "user:%d>", &a_prompt_number ) == 1 )
        {
            prompt_number = a_prompt_number;
            commandModeSent_ = true;
        }
        if( debug_ ) logger_.syslog( "read user prompt " + Str( prompt_number ) + ": " + deviceResponse_, Syslog::DEBUG );
    }
    return prompt_number;
}

void AcousticModem_Benthos_ATM900::readAndParseResponses( void )
{
    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( "AcousticModem_Benthos_ATM900 uart error: ", uart_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::COMMUNICATIONS );
        }
        parseResponses();
    }
    clearUserPrompt();
}

void AcousticModem_Benthos_ATM900::parseResponses( void )
{
    if( verbosity_ > 0 ) logger_.syslog( "serial response: ", deviceResponse_, Syslog::INFO );

    size_t data_bytes = 0; // #comms bytes expected
    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 )( ATM900_MAX_RCV - dataBytesReceived_ ) );

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

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

        incomingRemoteAddress_ = incomingLocalAddress_ = -1;
        return;
    }
    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 );
        }
    }
    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_ == ATM900_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;
                        logger_.syslog( "In parseResponses, got ack so set commsState_ = SENDING_VERIFIED", Syslog::DEBUG );
                    }
                    logger_.syslog( "Got ack", Syslog::INFO );
                    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();
    if( strstr( deviceResponse_, "user:" ) )
    {
        logger_.syslog( "unexpected user prompt in deviceResponse_: ", deviceResponse_, Syslog::INFO ); // should not get this
    }
    // 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;
            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;
            logger_.syslog( "In parseResponses, set commsState_ = SENDING_ACK_WAITING", Syslog::DEBUG );
        }
    }
    else if( strstr( deviceResponse_, "Acoustic Wakeup\n" ) )
    {
        logger_.syslog( "got acoustic wakeup", Syslog::INFO );
        gotAcousticWakeup_ = true; // modem was asleep, but received message
    }
    else if( strstr( deviceResponse_, "Response Not Received\n" ) )
    {
        logger_.syslog( "did not receive response from remote modem", Syslog::INFO );
        gotResponseNotReceived_ = true; // modem is still working fine but may be out of acoustic range
    }
    else if( parseDebugRxMessage() )
    {
        logger_.syslog( "received an acoustic signal", Syslog::INFO );
        gotDebugRxMessage_ = true;
    }
    else if( parseDebugTxMessage() )
    {
        logger_.syslog( "transmitted an acoustic signal", Syslog::INFO );
        gotDebugTxMessage_ = true;
    }
    else if( strstr( deviceResponse_, "range request" ) )
    {
        logger_.syslog( "received a range request message", Syslog::INFO );
        gotRangeRequestMessage_ = true;
    }
    else if( parseRangeMessage() )
    {
        logger_.syslog( "received a range response message", Syslog::INFO );
        gotRangeMessage_ = true;
    }
    else if( ( deviceResponse_[0] == '\n' ) || ( deviceResponse_[0] == '\r' ) )
    {
        //logger_.syslog( "uncaught empty line in deviceResponse_: ", deviceResponse_, Syslog::DEBUG );
    }
    else if( strstr( deviceResponse_, "<EOP>" ) )
    {
        // Ignore for now
    }
    else
    {
        logger_.syslog( "unknown deviceResponse_: ", deviceResponse_, Syslog::INFO );
        //this->setFailure( FailureMode::COMMUNICATIONS );
        //break;
    }
}


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


bool AcousticModem_Benthos_ATM900::parseDebugRxMessage( void )
{
    // (@ DEBUG level 3)  Rx Time:04:07:39.5194
    int hours, minutes, seconds, tenthsMilliseconds;
    if( 4 == sscanf( deviceResponse_, "Rx Time:%d:%d:%d.%d", &hours, &minutes, &seconds, &tenthsMilliseconds ) )
    {
        modemTransmitPingTime_ = Timestamp::Now();
        modemTransmitPingTime_.round( 3600.0 * 24.0 ); // get today
        modemReceivePingTime_ += Timespan::Hours( hours );
        modemReceivePingTime_ += Timespan::Minutes( minutes );
        modemReceivePingTime_ += Timespan::Seconds( seconds );
        modemReceivePingTime_ += Timespan::Milliseconds( tenthsMilliseconds / 10.0 );
        modemReceivePingEpochSeconds_ = modemReceivePingTime_.asDouble();
        receivePingTime_ = dataTimestamp_;
        return true;
    }
    return false;
}

bool AcousticModem_Benthos_ATM900::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 ) )
    {
        modemTransmitPingTime_ = Timestamp::Now();
        modemTransmitPingTime_.round( 3600.0 * 24.0 ); // get today
        modemTransmitPingTime_ += Timespan::Hours( hours );
        modemTransmitPingTime_ += Timespan::Minutes( minutes );
        modemTransmitPingTime_ += Timespan::Seconds( seconds );
        modemTransmitPingTime_ += Timespan::Milliseconds( tenthsMilliseconds / 10.0 );
        modemTransmitPingEpochSeconds_ = modemTransmitPingTime_.asDouble();
        return true;
    }
    return false;
}


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


void AcousticModem_Benthos_ATM900::publishData( void )
{
    if( gotAcousticWakeup_ )
    {
        if( verbosity_ > 1 ) logger_.syslog( "publishing acoustic wakeup flag", Syslog::INFO );
        acousticWakeupWriter_->write( Units::COUNT, 1, dataTimestamp_ );
    }
    if( gotDebugRxMessage_ )
    {
        if( verbosity_ > 1 ) logger_.syslog( "publishing receive ping time", Syslog::INFO );
        rxTimeWriter_->write( Units::EPOCH_SECOND, modemReceivePingEpochSeconds_, dataTimestamp_ );
    }
    if( gotDebugTxMessage_ )
    {
        if( verbosity_ > 1 ) logger_.syslog( "publishing transmit ping time", Syslog::INFO );
        txTimeWriter_->write( Units::EPOCH_SECOND, modemTransmitPingEpochSeconds_, 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_ );
    }
}