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

/***

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


***/

#include "DUSBL_Hydroid.h"
#include "DUSBL_HydroidIF.h"
#include "MicromodemIF.h"

#include <cstdlib>
#include <unistd.h>  // include for the sleep method
#include <math.h>    /* atan2 and asin */

#include "data/BlobWriter.h"
#include "data/ConfigReader.h"
#include "data/Matrix3x3.h"
#include "data/Point3D.h"
#include "data/Point6D.h"
#include "data/SimSlate.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"
#include "units/Units.h"
#include "utils/AuvMath.h"

#define MIN_RESP_SIZE (6) // Minimum number of bytes for expected for a valid response (either !U1,P or !U1R... or !U1,N... )

DUSBL_Hydroid::DUSBL_Hydroid( const Module* module )
    : SyncSensorComponent( DUSBL_HydroidIF::NAME, module ),
      debug_( false ),
      verbosityCfgSetting_( 0 ),            // Verbosity: 0=concise to 3=most verbose
      transmitLockoutCfgSetting_( 40 ),     // Time after transmit to ignore replies. Max 50 ms", Units::MILLISECOND );
      recieveTimeoutCfgSetting_( 2048 ),    // Time after transmit to listen for replies. Max 4095 ms", Units::MILLISECOND );
      detectionThresholdCfgSetting_( 32 ),  // Ignore singals lower than this value. Max 0xFF", Units::COUNT );
      xCenterCfgSetting_( 0 ),              // X-axis offset, up to +/- 6.35 arcdeg
      yCenterCfgSetting_( 0 ),              // Y-axis offset, up to +/- 6.35 arcdeg
      turnAroundTimeCfgSetting_( 0.05 ),      // Turn around time of the transponder
      defaultSoundSpeedCfgSetting_( 1500 ), // Speed of sound in water if not measured
      soundSpeed_( 1500 ),
      loadControl_( DUSBL_HydroidIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      poTimeout_( 20 ),
      powerDownTimeout_( 3 ),
      uart_( DUSBL_HydroidIF::UART, DUSBL_HydroidIF::BAUD, 0.6, logger_, 4095 ),
      commandSent_( false ),
      commandAcknowledged_( false ),
      transmitPingTime_( Timestamp::NOT_SET_TIME ),
      receivePingTime_( Timestamp::NOT_SET_TIME ),
      acousticResponseTimeout_( 6.0 ), // Observered to be 5 seconds on the bench and per e-mail from Tom Austin
      rearmLockTimeout_( 2.5 ), // Observered to be 2.5 seconds on the bench and per e-mail from Tom Austin
      waitingForRangeResponse_( false ), // This is set true when we're expecting a response back from a range request
      transponderCodeRequested_( 2 ), // Transponder code: 2 for DR1-A, 3 for DR1-B, etc", Units::ENUM );
      numPingsRequested_( 1 ),
      gotRangeMessage_( false ),
      gotDirectionMessage_( false ),
      gotResponseNotReceived_( false ),
      xAngle_( nan( "" ) ),
      yAngle_( nan( "" ) ),
      travelTime_( nan( "" ) ),
      latency_( nan( "" ) ),
      gain_( nan( "" ) ),
      inBandSignalToNoise_( nan( "" ) ),
      outBandSignalToNoise_( nan( "" ) ),
      range_( nan( "" ) ),
      direction_()
{
    // Configuration inputs
    verbosityCfgReader_ = newConfigReader( DUSBL_HydroidIF::VERBOSITY_CFG );
    transmitLockoutCfgReader_ = newConfigReader( DUSBL_HydroidIF::TRANSMIT_LOCKOUT_CFG );
    recieveTimeoutCfgReader_ = newConfigReader( DUSBL_HydroidIF::RECIEVE_TIMEOUT_CFG );
    detectionThresholdCfgReader_ = newConfigReader( DUSBL_HydroidIF::DETECTION_THRESHOLD_CFG );
    xCenterCfgReader_ = newConfigReader( DUSBL_HydroidIF::X_CENTER_CFG );
    yCenterCfgReader_ = newConfigReader( DUSBL_HydroidIF::Y_CENTER_CFG );
    defaultSoundSpeedCfgReader_ = newConfigReader( DUSBL_HydroidIF::DEFAULT_SOUND_SPEED_CFG );
    defaultTATCfgReader_ = newConfigReader( DUSBL_HydroidIF::DEFAULT_TAT_CFG );

    // Slate inputs
    transponderCodeRequestedReader_ = newDataReader( DUSBL_HydroidIF::TRANSPONDER_CODE_REQUESTED );
    pingRequestedReader_ = newDataReader( DUSBL_HydroidIF::PING_REQUESTED );

    // Universal inputs
    soundSpeedReader_ = newUniversalReader( UniversalURI::SPEED_OF_SOUND_IN_SEA_WATER );

    // Interface outputs
    dusblPingCodeWriter_ = newDataWriter( MicromodemIF::DUSBL_PING_CODE_REQUESTED );

    // Slate outputs
    xAngleWriter_ = newDataWriter( DUSBL_HydroidIF::X_ANGLE_READING );
    yAngleWriter_ = newDataWriter( DUSBL_HydroidIF::Y_ANGLE_READING );
    travelTimeWriter_ = newDataWriter( DUSBL_HydroidIF::TRAVEL_TIME_READING );
    latencyWriter_ = newDataWriter( DUSBL_HydroidIF::LATENCY_READING );
    gainWriter_ = newDataWriter( DUSBL_HydroidIF::GAIN_READING );
    inBandSignalToNoiseWriter_ = newDataWriter( DUSBL_HydroidIF::IN_BAND_SINGAL_TO_NOISE_READING );
    outBandSignalToNoiseWriter_ = newDataWriter( DUSBL_HydroidIF::OUT_BAND_SINGAL_TO_NOISE_READING );

    rangeToContactWriter_ = newUniversalWriter( UniversalURI::ACOUSTIC_CONTACT_RANGE_READING, Units::METER, 0.5 );
    directionToContactWriter_ = newUniversalBlobWriter( UniversalURI::ACOUSTIC_CONTACT_DIRECTION_VF, Units::NONE, 0.5 );
    contactAddressWriter_ = newUniversalWriter( UniversalURI::ACOUSTIC_CONTACT_ADDRESS_READING, Units::ENUM, 0.1 );
    rxTimeWriter_ = newUniversalWriter( UniversalURI::ACOUSTIC_RECEIVE_TIME, Units::SECOND, 0.1 );

    deviceResponse_[0] = '\0';

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

}


DUSBL_Hydroid::~DUSBL_Hydroid()
{
}


void DUSBL_Hydroid::run()
{
}


void DUSBL_Hydroid::readConfig()
{
    verbosityCfgReader_->read( Units::COUNT, verbosityCfgSetting_ );
    if( !transmitLockoutCfgReader_->read( Units::MILLISECOND, transmitLockoutCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for transmitLockout, using", transmitLockoutCfgSetting_, Syslog::ERROR );
    }
    if( !recieveTimeoutCfgReader_->read( Units::MILLISECOND, recieveTimeoutCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for recieveTimeout, using", recieveTimeoutCfgSetting_, Syslog::ERROR );
    }
    if( !detectionThresholdCfgReader_->read( Units::COUNT, detectionThresholdCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for detectionThreshold, using", detectionThresholdCfgSetting_, Syslog::ERROR );
    }
    if( !xCenterCfgReader_->read( Units::ANGULAR_DEGREE, xCenterCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for xCenter, using", xCenterCfgSetting_, Syslog::ERROR );
    }
    if( !yCenterCfgReader_->read( Units::ANGULAR_DEGREE, yCenterCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for yCenter, using", yCenterCfgSetting_, Syslog::ERROR );
    }
    if( !defaultSoundSpeedCfgReader_->read( Units::METER_PER_SECOND, defaultSoundSpeedCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for defaultSoundSpeed, using", defaultSoundSpeedCfgSetting_, Syslog::ERROR );
    }
    if( !defaultTATCfgReader_->read( Units::SECOND, turnAroundTimeCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for defaultTAT, using", turnAroundTimeCfgSetting_, Syslog::ERROR );
    }
    if( !soundSpeedReader_->read( Units::METER_PER_SECOND, soundSpeed_ ) )
    {
        soundSpeed_ = defaultSoundSpeedCfgSetting_;
    }

}

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


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

    commandSent_ = false;
    commandAcknowledged_ = false;
    rearmLockTime_ = Timestamp::NOT_SET_TIME;
    // TODO: These state flags are clunky -- it would be cleaner if we just made this component asynchronous and let the serial port wait for a reasonable time for a response.

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

    if( simulateHardware() )
    {
        startTime_ = Timestamp::Now();
        return STARTING;
    }

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

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

    logger_.syslog( "Initializing DUSBL_Hydroid." );

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

    readConfig();

    if( simulateHardware() )
    {
        return RUNNABLE;
    }

    //On power-up, request version to be returned. !U1,V,O(CR) where the letter is the version
    if( uart_.canReadUntil( "!U1,V," ) && uart_.canReadUntil( "\r" ) )
    {
        uart_.readUntil( deviceResponse_, sizeof( deviceResponse_ ), "\r", 1 );
        if( debug_ ) logger_.syslog( Str( deviceResponse_ ), Syslog::DEBUG );
        if( uart_.bytesRead() >= 8 )
        {
            // Read for the full number of bytes listed in the spec just in case.
            // At the time of writing, the hardware output did not match the spec which showed !U1,V,xxx.xx(CR)
            logger_.syslog( "DUSBL Version:" + Str( deviceResponse_, 6, 6 ), Syslog::INFO );
            uart_.flush();
            return RUNNABLE;
        }
    }
    else if( startTime_.elapsed() > poTimeout_ )
    {
        char msg[uart_.dataAvailable()];
        uart_.read( msg, sizeof( msg ) );
        logger_.syslog( "failed to initialize; deviceResponse_ loaded: " + Str( msg ) + ", available: " + Str( sizeof( msg ) ), Syslog::FAULT );
        this->setFailure( FailureMode::COMMUNICATIONS );
        return STOP;
    }
    // Rev G 8/3/2017 of the spec indicates on page 3 that the DUSBL will automatically
    // send a version message once upon power up. This does not appear to be the case so
    // we will request it. Also, the DUSBL does not seem to respond to any serial input
    // commands until about 15 second after power is applied. Thus, we'll start trying
    // once that time has passed.
    else if( startTime_.elapsed() > ( poTimeout_ - Timespan( 3 ) ) )
    {
        uart_ << "#U1,V\r"; // request version
    }

    return STARTING;
}


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


Component::RunState DUSBL_Hydroid::resume()
{
    if( debug_ ) logger_.syslog( "Resume", Syslog::INFO );

    readConfig();

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


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


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

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

    if( simulateHardware() )
    {
        if( transponderCodeRequestedReader_->wasTouchedSinceLastRun( this ) && pingRequestedReader_->wasTouchedSinceLastRun( this ) )
        {
            bool validTransponderCode = transponderCodeRequestedReader_->read( Units::ENUM, transponderCodeRequested_ );
            bool validPingsRequested = pingRequestedReader_->read( Units::COUNT, numPingsRequested_ );

            if( validTransponderCode && validPingsRequested )
            {
                // Start the transmit ping timer
                transmitPingTime_ = Timestamp::Now();
                waitingForRangeResponse_ = true;
            }
        }

        double estimatedTravelTime = 2 * ( isnan( range_ ) ? 1000 : range_ ) / 1500.;
        if( waitingForRangeResponse_ && estimatedTravelTime < transmitPingTime_.elapsed().asDouble() )
        {
            if( getSimulatedMeasurements() )
            {
                contactAddressWriter_->write( Units::ENUM, transponderCodeRequested_ );
                waitingForRangeResponse_ = false;
                publishData();
                this->resetFailCount();
            }
        }
    }
    else
    {
        // This wait time is required in order to reliably re-arm the DUSBL between pings.
        if( !waitingForRangeResponse_ && ( rearmLockTime_.elapsed() < rearmLockTimeout_ ) )
        {
            uart_.flush();
            return RUNNABLE;
        }

        // This section of code "arms" the DUSBL and prepares it to receive the discrete input for travel time as well as the hydrophone inputs.
        // It's at the top of runnable so that if we're arming, we have a chance of receiving the "armed" response of !U1,P(CR) before we leave this loop.
        if( gotNewQuery() )  // if you read a full query since the last cycle, try to ping again
        {
            //if( acousticResponseTimeout_ < transmitPingTime_.elapsed().asDouble() )
            if( !waitingForRangeResponse_ )
            {
                transmitPingTime_ = requestRange( transponderCodeRequested_ ); // requestRange will write the address
                if( verbosityCfgSetting_ > 0 )logger_.syslog( "****** ping requested ******", Syslog::INFO );
                waitingForRangeResponse_ = true; // Set this true to avoid more than one interrogation at a time.
                commandAcknowledged_ = false; // We haven't received the ack from the DUSBL arming yet so set this false so we check for it.
            }
            else
            {
                if( debug_ ) logger_.syslog( "received new query, but waiting for acoustic response period to elapse", Syslog::DEBUG );
            }
        }

        logVoltageAndCurrent();
        receivePingTime_ = 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_ && gotDirectionMessage_ ) // This indicates that we have solicited a response
        {
            anglesToDirection();
            travelTimeToRange();

            // If range is invalid, let's not use it
            if( isnan( range_ ) )
            {
                gotRangeMessage_ = false;
                logger_.syslog( "Range is NaN. Not publishing.", Syslog::ERROR );
            }

            publishData();
            this->resetFailCount();
            waitingForRangeResponse_ = false; // we're not waiting anymore
            commandAcknowledged_ = false;
            rearmLockTime_ = Timestamp::Now(); // Set the timer and return to give the USBL time so it responds. Immediate queries did not work.
            return RUNNABLE;
        }
        else if( gotResponseNotReceived_ )
        {
            logger_.syslog( "No response from remote modem.", Syslog::ERROR );
            this->resetFailCount();
            waitingForRangeResponse_ = false; // We got confirmation that the DUSBL's rx gate is over with no reply.
            commandAcknowledged_ = false;
            gotResponseNotReceived_ = false; // reset for next time
            rearmLockTime_ = Timestamp::NOT_SET_TIME;
            return RUNNABLE; // Here we return to give the USBL another cycle so it responds. Immediate queries did not work.
        }
    }

    return RUNNABLE;
}


Component::RunState DUSBL_Hydroid::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 DUSBL_Hydroid::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 DUSBL_Hydroid::stopped()
{
    if( debug_ ) logger_.syslog( "Stopped", Syslog::INFO );
    if( isDataRequested() )
    {
        return START;
    }
    if( !simulateHardware() )
    {
        if( ( loadControl_.getPowerState() != LoadControl::OFF ) && ( loadControl_.getPowerState() != LoadControl::POWER_DOWN ) )
        {
            return stop();
        }

        // Close if the uart if it is open
        if( uart_.isReadable() )
        {
            uart_.close();
        }
    }
    return STOPPED;
}


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

bool DUSBL_Hydroid::isDataRequested()
{
    return rangeToContactWriter_->isDataRequested()
           || directionToContactWriter_->isDataRequested();
}

bool DUSBL_Hydroid::gotNewQuery()
{
    // If we're still waiting for a response and have passed our timeout, let's just quit waiting and start again.
    // The timeout on the USBL has likely expired and we're ready to arm again.
    if( ( waitingForRangeResponse_ ) && ( transmitPingTime_.elapsed() > acousticResponseTimeout_ ) )
    {
        waitingForRangeResponse_ = false; // We've waiting long enough and nothing is happening
        commandAcknowledged_ = false;
        uart_.flush();
        if( verbosityCfgSetting_ > 2 ) logger_.syslog( "Setting waiting for range response to false due to timeout. Elapsed:" + Str( transmitPingTime_.elapsed().asFloat() ), Syslog::INFO );
    }

    if( waitingForRangeResponse_ )
    {
        return false; // Explicitly avoid new interrogations while waiting for response to previous.
    }
    else if( transponderCodeRequestedReader_->wasTouchedSinceLastRun( this ) && pingRequestedReader_->wasTouchedSinceLastRun( this ) )
    {
        bool validTransponderCode = transponderCodeRequestedReader_->read( Units::ENUM, transponderCodeRequested_ );
        bool validPingsRequested = pingRequestedReader_->read( Units::COUNT, numPingsRequested_ );
        // TODO: Filter valid addresses and ping requests here.
        if( verbosityCfgSetting_ > 0 )
        {
            if( validTransponderCode ) logger_.syslog( "****** received transponder code query ******", Syslog::INFO );
            else logger_.syslog( "****** received invalid address query ******", Syslog::INFO );

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


Timestamp DUSBL_Hydroid::requestRange( const int transponderCode )
{
    // You might be wondering why were aren't setting the flags to request the modem to ping here.
    // That's because even though the spec doesn't indicate it, the DUSBL must first finish responding
    // with the ACK (!U1,P(CR) ) before it's actually armed. Thus we generate the request here and
    // then ask for the ping following receipt of the !U1,P response.
    if( verbosityCfgSetting_ > 2 ) logger_.syslog( "Arming DUSBL. detectThresh: " + Str( detectionThresholdCfgSetting_ ), Syslog::INFO );
    if( !simulateHardware() )
    {
        uart_ << GenerateRangeRequest(
                  transponderCode,
                  transmitLockoutCfgSetting_,
                  recieveTimeoutCfgSetting_,
                  detectionThresholdCfgSetting_,
                  xCenterCfgSetting_,
                  yCenterCfgSetting_,
                  soundSpeed_
              );
    }

    return Timestamp::Now();
}

char* DUSBL_Hydroid::GenerateRangeRequest(
    const int transponderCode,
    const int transmitLockoutCfgSetting,
    const int recieveTimeoutCfgSetting,
    const int detectionThresholdCfgSetting,
    const int xCenterCfgSetting,
    const int yCenterCfgSetting,
    const int soundSpeed )
{
    /*  #U1,P,nn,ttt,ggg, hh,xx,yy,sss(CR)
        nn = signal number to be received, (0-FF hex).
        ttt = transmit lockout in 0.001 second increments (000-FFF hex).
        ggg = receiver gate time in 0.001 second increments (000-FFF hex).
        hh = detection threshold (00-FF hex)
        xx = x center degrees (2’s complement hex)
        yy = y center degrees (2’s complement hex)
        sss = local sound speed (m/s) (unsigned hex integer)

     E.G. #U1,P,02,014,800,15,00,00,578 wold work for a buoy with nav.dt.type = 1
    */
    static char cmd[40];
    const char* cmdFmt = "#U1,P,%02X,%03X,%03X,%02X,%02X,%02X,%03X\r";
    sprintf( cmd,
             cmdFmt,
             transponderCode & 0xff,
             transmitLockoutCfgSetting & 0xfff,
             recieveTimeoutCfgSetting & 0xfff,
             detectionThresholdCfgSetting & 0xff,
             xCenterCfgSetting & 0xff,
             yCenterCfgSetting & 0xff,
             soundSpeed & 0xfff
           );
    return cmd;
}

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

void DUSBL_Hydroid::readAndParseResponses( void )
{
    int bytesAvailable =  uart_.dataAvailable();

    // Something valid has come in from the DUSBL (but not too much something)
    if( uart_.canReadUntil( "!U1," ) && bytesAvailable >= MIN_RESP_SIZE && ( unsigned int )bytesAvailable < sizeof( deviceResponse_ ) )
    {
        uart_.read( deviceResponse_, bytesAvailable );
        if( verbosityCfgSetting_ > 2 ) logger_.syslog( "DUSBL response received:" + Str( deviceResponse_ ), Syslog::INFO );
        if( uart_.hasError() )
        {
            logger_.syslog( "DUSBL_Hydroid uart error: ", uart_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::COMMUNICATIONS );
        }
        parseResponses();
    }
}

void DUSBL_Hydroid::parseResponses()
{
    // Just belt and suspenders using the member boolean to make sure we only request from the micromodem once
    if( strstr( deviceResponse_, "!U1,P" ) && !commandAcknowledged_ )
    {
        // Here we have an ack that the DUSBL is armed and ready to receive. Now we can request the ping from the micromodem.
        commandAcknowledged_ = true;
        if( verbosityCfgSetting_ > 2 ) logger_.syslog( "Command Ack", Syslog::INFO );
        if( verbosityCfgSetting_ > 0 ) logger_.syslog( "Requesting micromodem ping code " + Str( transponderCodeRequested_ ), Syslog::INFO );
        dusblPingCodeWriter_->write( Units::ENUM, transponderCodeRequested_ );
        contactAddressWriter_->write( Units::ENUM, transponderCodeRequested_ );
    }
    // Check for a range response
    else if( parseRangeAndDirectionMessage() )
    {
        gotRangeMessage_ = true;
        gotDirectionMessage_ = true;
    }
    // A U1,N message indicates the receive gate is over with no reply.
    // this can eventually be parsed if the auto gain settings and noise readings are of interest
    else if( strstr( deviceResponse_, "!U1,N" ) )
    {
        if( verbosityCfgSetting_ > 2 ) logger_.syslog( "No reply:", deviceResponse_, Syslog::INFO );
        gotResponseNotReceived_ = true;
    }
    else
    {
        logger_.syslog( "unknown deviceResponse_: ", deviceResponse_, Syslog::ERROR );
        // Don't bother failing. Just get the next one.
        uart_.flush(); // But also clear the uart
    }
}

bool DUSBL_Hydroid::parseRangeAndDirectionMessage( void )
{
    return ParseRangeAndDirectionMessage( deviceResponse_, xAngle_, yAngle_, travelTime_,
                                          latency_, gain_, inBandSignalToNoise_, outBandSignalToNoise_ );
}

bool DUSBL_Hydroid::ParseRangeAndDirectionMessage( const char* deviceResponse, float& xAngle, float& yAngle, float& travelTime,
        float& latency, float& gain, float& inBandSignalToNoise, float& outBandSignalToNoise )
{
    // !U1,R,xxx,yyy,ttttt,lll,gg,ii,oo(CR)
    // !U1,R,2A2,000,002C3,189,80,80,4D,FF
    // xxx = x angle in 0.05 degrees (2’s complement hex)
    // yyy = y angle in 0.05 degrees (2’s complement hex)
    // ttttt = travel time in 80 us  (unsigned hex)
    // lll = latency in 1 ms increments (unsigned hex)
    // gg = gain (00-FF unsigned hex)
    // ii = in-band snr, (unsigned hex)
    // oo = out-band snr, (unsigned hex)
    struct
    {
        signed int xAngleIn0_05deg_ : 12;
        signed int yAngleIn0_05deg_ : 12;
    } bitfields;
    unsigned int xAngleIn0_05deg, yAngleIn0_05deg, travelTimeIn80us,
             uilatency, uigain, uiinBandSignalToNoise, uioutBandSignalToNoise;
    int scanned = sscanf( deviceResponse,
                          "!U1,R,%03x,%03x,%05x,%03x,%*02x,%02x,%02x,%02x",
                          &xAngleIn0_05deg, &yAngleIn0_05deg, &travelTimeIn80us,
                          &uilatency, &uigain, &uiinBandSignalToNoise, &uioutBandSignalToNoise );
    if( 7 == scanned )
    {
        bitfields.xAngleIn0_05deg_ = xAngleIn0_05deg;
        bitfields.yAngleIn0_05deg_ = yAngleIn0_05deg;

        // Flipping the sign here to expect positive to port
        // since the USBL was originally mounted upside down
        xAngle = D2R( bitfields.xAngleIn0_05deg_ * -0.05 );

        yAngle = D2R( bitfields.yAngleIn0_05deg_ * 0.05 );
        travelTime = travelTimeIn80us * 0.000080;
        latency = uilatency;
        gain = uigain;
        inBandSignalToNoise = uiinBandSignalToNoise;
        outBandSignalToNoise = uioutBandSignalToNoise;
        return true;
    }
    return false;
}

void DUSBL_Hydroid::publishData( void )
{

    if( gotRangeMessage_ && gotDirectionMessage_ ) // Only publish if we got range too
    {
        gotRangeMessage_ = gotDirectionMessage_ = false; // reset these members for next response

        if( verbosityCfgSetting_ > 1 ) logger_.syslog( "publishing direction and range info", Syslog::INFO );
        xAngleWriter_->write( Units::RADIAN, xAngle_, receivePingTime_ );
        yAngleWriter_->write( Units::RADIAN, yAngle_, receivePingTime_ );
        travelTimeWriter_->write( Units::SECOND, travelTime_, receivePingTime_ );
        latencyWriter_->write( Units::MILLISECOND, latency_, receivePingTime_ );
        gainWriter_->write( Units::RATIO, gain_, receivePingTime_ );
        inBandSignalToNoiseWriter_->write( Units::RATIO, inBandSignalToNoise_, receivePingTime_ );
        outBandSignalToNoiseWriter_->write( Units::RATIO, outBandSignalToNoise_, receivePingTime_ );
        rangeToContactWriter_->write( Units::METER, range_, receivePingTime_ );
        directionToContactWriter_->write1DClass( Units::NONE, direction_, receivePingTime_ );
        rxTimeWriter_->write( Units::EPOCH_SECOND, receivePingTime_.asDouble(), receivePingTime_ );
    }
}

void DUSBL_Hydroid::anglesToDirection()
{
    // populate the direction vector in the vehicle frame
    direction_ = 0.0;
    AuvMath::AzimuthAndElevationToUnitVector( xAngle_, yAngle_, direction_ );
}

void DUSBL_Hydroid::travelTimeToRange()
{
    range_ = ( ( ( travelTime_ - turnAroundTimeCfgSetting_ )  * soundSpeed_ ) / 2 ) ; // Range is half of roundtrip travel time less any TAT
    if( range_ <= 0 )
    {
        // Something isn't right. Observations of a negative range have happened when:
        // 1.) and incorrect TAT is applied
        // 2.) GPIO timing with the micromodem is off.
        // 3.) The detection threshold was set too low
        // We'll catch it here
        range_ = nanf( "" );
    }
}

bool DUSBL_Hydroid::getSimulatedMeasurements( )
{
    // see ExternalSim.cpp, 527     if( startTime_.elapsed() >= homingSensorTat_ )
    // sim currently writes out azimuth and elevation in vehicle
    // coordinate frame, _assuming space-fixed angles_

    gotRangeMessage_ = SimSlate::Read( SimSlate::HOMING_SENSOR_RANGE_M, range_ );
    gotDirectionMessage_ = SimSlate::Read( SimSlate::HOMING_SENSOR_AZIM_RAD, xAngle_ );
    gotDirectionMessage_ &= SimSlate::Read( SimSlate::HOMING_SENSOR_ELEV_RAD, yAngle_ );
    if( gotRangeMessage_ && gotDirectionMessage_ )
    {
        rangeToContactWriter_->setInvalid( false );
        directionToContactWriter_->setInvalid( false );
        contactAddressWriter_->setInvalid( false );

        receivePingTime_ = Timestamp::Now();
        anglesToDirection();
        return true;
    }
    else
    {
        logger_.syslog( "Could not read simulated DUSBL_Hydroid measurements from SimSlate.", Syslog::ERROR );
    }
    return false;
}