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

/****


!!! The NAL9602-LP requires the following active profile to be set on the bench prior to use in the vehicle !!!

Firmware version 1.3.4 or better

NOTE PNAV command in manual isn't implemented in NAL9602
Set AT^LEDS=0,0,0,0,0,0

***
The following command is UNDOCUMENTED as of AT command manual TN2011-002-V1.0 Jan 24, 2011
^S0=1,1,0,1
The ^S command is shown to have 3 input parameters. The fourth undocumented input is equivalent to the ^EST command
in that a "1" means latching input and a "0" is momentary. Emergency mode will only function for 3 seconds if the
last number is a 0 in the current LRAUV architecture.
While the ^S command is stored in the active profile, it will not be displayed upon issuing a &V command. Manually
check the configuration via the ^S0? command.

Also, emergency mode via the discrete only works if the 9602 is in tracking mode. This is a large change from the 9601
and it doesn't seem to be documented. Thus, the interface must now issue a +++ to enter command mode.
***

ACTIVE PROFILE:

E0 Q0 V1 ^CAL1 ^ID0 ^RMF3 ^GAO0 ^TBR5.0 ^TTKT255 ^UE0 ^START1 +PNAV4

+IPR6 +SBDMTA1 +SBDAREG0 +CIER0000 *R1 *S1 +PP1 +PG0,1,1,0002

^TBRT0.0 ^TTKTT255 ^TBRE0.0 ^TTKTE255 ^CALT1 ^CALE1

^TBRA3.0 ^TTKTA255 ^TBRTA1.0 ^TTKTTA255 ^TBREA1.0 ^TTKTEA255

^PR0 ^MSA000 ^MSW10 ^MSB3,10 ^MSE3

^SPSR0,10,4,120 ^SPSRT0,10,4,120 ^SPSRE0,10,4,120

^BIGR000 ^ERF0 ^TRF0 ^DLTRK1 ^SSI1 ^EST0 ^LEDS0,0,0,0,0,0 ^IPS1

***/


#include "NAL9602.h"
#include "NAL9602IF.h"

#include <cstdlib>
#include <ctime>
#include <errno.h>
#include <unistd.h>        // include for the sleep method
#include <arpa/inet.h>
#include <sys/stat.h>

#include "controlModule/VerticalControlIF.h"
#include "data/ConfigReader.h"
#include "data/Location.h"
#include "data/SimSlate.h"
#include "data/Slate.h"
#include "data/StrValue.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"
#include "io/FileInStream.h"
#include "io/FileOutStream.h"
#include "io/StrIOStream.h"
#include "logger/LogQueue.h"
#include "logger/SyslogEntry.h"
#include "supervisor/DataReceiver.h"
#include "supervisor/Supervisor.h"
#include "units/Units.h"
#include "VehicleIF.h"

#define GGA_PREFIX "$GPGGA"
#define GSA_PREFIX "$GPGSA"
#define RMC_PREFIX "$GPRMC"
#define ZDA_PREFIX "$GPZDA"
#define SBDRT_RESPONSE_PREFIX "\r\n+SBDRT:\r\n"

NAL9602::NAL9602( const Module* module )
    : SyncSensorComponent( NAL9602IF::NAME, module ),
      commsState_( AWAKE ),
      powerDownTimeout_( 30 ),
      poResetTimeout_( 10 ),
      requestTimeout_( 30 ),
      gpsFailTimeout_( 1800 ),
      iridiumMTQueueTimeout_( 600 ),
      bufferFillTimeout_( 65 ),
      startTime_(),
      gotFix_( false ),
      gotMTQueue_( false ),
      snrData_( true ),
      ok_( false ),
      surfaceThreshold_( 1.0 ), // This is overwritten by a config value
      sendDataToShore_( true ), // This is overwritten by a config value
      checkMTQueue_( true ), // This is overwritten by a config value
      requestGGA_( false ),  // This is overwritten by a config value
      fastGPS_( false ), // This is overwritten by a config value
      handleZDAMessages_( false ), // Deals with incoming ZDA messages. For use with mapping LRAUV.
      maxDownlinkMsgSize_( MAX_DOWNLINK_MSG_BYTES ), // This should be overwritten by a config value
      maxDownlinkDataSize_( MAX_DOWNLINK_DATA_BYTES ), // This should be overwritten by a config value derivation
      uplinkStatus_( 0 ),
      uplinkSeqNo_( 0 ),
      nUplinkBytes_( 0 ),
      nPendingUplinks_( 0 ),
      communicationsDone_( true ),
      cmdReceived_( false ),
      fixRequested_( false ),
      gpsTimeoutArmed_( false ),
      iridiumTimeoutArmed_( false ),
      gsvRequested_( false ),
      qualityRequested_( false ),
      debug_( false ),
      uart_( NAL9602IF::UART, NAL9602IF::BAUD, 0.5, logger_, 4095 ),
      loadControl_( NAL9602IF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      sigQuality_( 0 ),
      radioEnabled_( false ),
      /// Name of SBD currently being re-sent to shore
      resendSBDFilename_( NULL ),
      sendPacket_( MAX_DOWNLINK_MSG_BYTES, &logger_ ),
      strobeMode_( STROBE_ON )
{

    this->setAllowableFailures( 5 ); // Since the fail count is only reset after the completion of GPS/comms, it's possible that these can stack up. Will allow for 5 consecutive per "session".
    this->setRetryTimeout( 120 );

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

    // Slate inputs
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );
    latitudeReader_ = newUniversalReader( UniversalURI::LATITUDE );
    longitudeReader_ = newUniversalReader( UniversalURI::LONGITUDE );
    strobeModeReader_ = newDataReader( NAL9602IF::STROBE_MODE );

    // TODO: Change from deprecated Slate::NewOutputWriter to newDataWriter. Leaving as-is for now because of the adaptive way it is initializing them based on MAX_GPS_SATS_9602.
    for( int i = 0; i < MAX_GPS_SATS_9602; i++ )
    {
        snrWriters_[i].snrWriter_ = Slate::NewOutputWriter( NAL9602IF::NAL_SNR_BASENAME + i, this, Units::COUNT( 0 ) );
    } // TODO: would be nice to make this a vector in the slate instead...

    // Slate outputs
    goodFixWriter_       = newDataWriter( NAL9602IF::GOOD_FIX_STATE );
    numSatellitesWriter_ = newDataWriter( NAL9602IF::NUM_SATELLITES_READING );
    sigQualityWriter_    = newDataWriter( NAL9602IF::SIG_QUALITY_READING );
    sogWriter_           = newDataWriter( NAL9602IF::SOG_READING );
    cogWriter_           = newDataWriter( NAL9602IF::COG_READING );

    timeFixWriter_ = newUniversalWriter( UniversalURI::TIME_FIX, Units::SECOND, 0.01 ); // epoch seconds
    latitudeFixWriter_ = newUniversalWriter( UniversalURI::LATITUDE_FIX, Units::DEGREE, 0.00001 ); //TODO add real accuracy here (actually, use writeWithAccuracy to adapt)
    longitudeFixWriter_ = newUniversalWriter( UniversalURI::LONGITUDE_FIX, Units::DEGREE, 0.00001 ); //TODO add real accuracy here (actually, use writeWithAccuracy to adapt)
    locationFixWriter_ = newUniversalBlobWriter( UniversalURI::LOCATION_FIX, Units::DEGREE, 0.00001 ); //TODO add real accuracy here (actually, use writeWithAccuracy to adapt)
    communicationsWriter_ = newUniversalWriter( UniversalURI::PLATFORM_COMMUNICATIONS, Units::BOOL, 0 );

    // Configuration
    surfaceThresholdCfgReader_ = newConfigReader( VerticalControlIF::SURFACE_THRESHOLD_CFG );
    sendDataToShoreCfgReader_ = newConfigReader( VehicleIF::SEND_DATA_TO_SHORE_CFG );
    checkMTQueueCfgReader_ = newConfigReader( VehicleIF::CHECK_MT_QUEUE_CFG );
    gpsFailTimeoutCfgReader_ = newConfigReader( NAL9602IF::GPS_FAIL_TIMEOUT_CFG );
    iridiumFailTimeoutCfgReader_ = newConfigReader( NAL9602IF::IRIDIUM_MTQUEUE_FAIL_TIMEOUT_CFG );
    requestGGACfgReader_ = newConfigReader( NAL9602IF::REQUEST_GGA_CFG );
    fastGPSFixReader_ = newConfigReader( NAL9602IF::FAST_GPS_FIX );
    handleZDACfgReader_ = newConfigReader( NAL9602IF::HANDLE_ZDA_MSG_CFG );
    maxDownlinkMsgSizeReader_ = newConfigReader( NAL9602IF::MAX_DOWNLINK_MSG_SIZE );

    // Set timeout for start in case we've just restarted the application
    powerOnTimeStart_ = Timestamp::Now();

    // This configures the advanced run modes.
    setRunState( START );
    persistentPause_ = true;
}

NAL9602::~NAL9602()
{
}


// Load parameters
bool NAL9602::loadParams( void )
{
    // Check if all the parameters are read correctly
    bool ok = true;
    ok &= Slate::ReadOnce( VehicleIF::KEY_TEXT_CFG, keyText_, logger_ );
    ok &= readConfig();
    return ok;
}

bool NAL9602::readConfig()
{
    // Check if all the parameters are read correctly
    bool ok = true;
    unsigned char sendDataToShore = sendDataToShore_;
    ok &= sendDataToShoreCfgReader_->read( Units::BOOL, sendDataToShore );
    sendDataToShore_ = sendDataToShore;

    unsigned char checkMTQueue = checkMTQueue_;
    ok &= checkMTQueueCfgReader_->read( Units::BOOL, checkMTQueue );
    checkMTQueue_ = checkMTQueue;

    float GPSFailTimeout;
    ok &= gpsFailTimeoutCfgReader_->read( Units::SECOND, GPSFailTimeout );
    gpsFailTimeout_ = Timespan::Seconds( GPSFailTimeout );

    float IridiumFailTimeout;
    ok &= iridiumFailTimeoutCfgReader_->read( Units::SECOND, IridiumFailTimeout );
    iridiumMTQueueTimeout_ = Timespan::Seconds( IridiumFailTimeout );

    ok &= requestGGACfgReader_->read( Units::BOOL, requestGGA_ );
    ok &= fastGPSFixReader_->read( Units::BOOL, fastGPS_ );

    ok &= handleZDACfgReader_->read( Units::BOOL, handleZDAMessages_ );

    ok &= maxDownlinkMsgSizeReader_->read( Units::BYTE, maxDownlinkMsgSize_ );
    if( ok )
    {
        maxDownlinkDataSize_ = AuvMath::Min( MAX_DOWNLINK_DATA_BYTES, ( long )maxDownlinkMsgSize_ - SENDPACKET_HEADER_SIZE );
    }

    ok &= surfaceThresholdCfgReader_->read( Units::METER, surfaceThreshold_ );

    return ok;
}


void NAL9602::run()
{
}

bool NAL9602::belowSurfaceThreshold( void ) // TODO: This could be a superclass method for radio components.
{
    float depth;
    return depthReader_->isActive() && depthReader_->read( Units::METER, depth ) && ( depth > surfaceThreshold_ );
}

//////////////////////
///    WAKING
//////////////////////
void NAL9602::waking()
{
    if( debug_ ) logger_.syslog( Str( "************** WAKING **************\n" ), Syslog::INFO );
    if( !simulateHardware() )
    {
        uart_.flush();
        uart_ << "+++"; // Get in command mode
        startTime_ = Timestamp::Now();
        commsState_ = CMDMODE;
    }
    else
    {
        commsState_ = CMDMODE;
    }
}

//////////////////////
///    CMDMODE
//////////////////////
void NAL9602::cmdMode()
{
    if( debug_ ) logger_.syslog( Str( "************** CMDMODE **************\n" ), Syslog::INFO );
    if( !simulateHardware() )
    {
        if( uart_.canReadUntil( "COMMAND MODE: Ready for Input" ) )
        {

            logger_.syslog( "NAL9602 initialized", Syslog::INFO );
            commsState_ = AWAKE;
        }
        else if( startTime_.elapsed() > poResetTimeout_ )
        {
            logger_.syslog( "NAL9602 initialization error.", Syslog::ERROR );
            this->setFailure( FailureMode::COMMUNICATIONS );
        }
    }
    else
    {
        commsState_ = AWAKE;
    }

}

//////////////////////
///    AWAKE
//////////////////////
void NAL9602::awake()
{
    if( debug_ ) logger_.syslog( Str( "************** AWAKE **************\n" ), Syslog::INFO );

    if( handleZDAMessages_ )
    {
        uart_.flush();
        logger_.syslog( "Turning off ZDA messages", Syslog::INFO );
        uart_ << "AT+PG=0\r"; // Turn off ZDA messages while we work
    }

    // ...Decide what to do- GPS first if possible
    if( latitudeFixWriter_->isDataRequested()
            || longitudeFixWriter_->isDataRequested()
            || timeFixWriter_->isDataRequested()
            || !latitudeReader_->isActive()
            || !longitudeReader_->isActive() )
    {
        commsState_ = NEED_FIX;
    }
    else if( communicationsWriter_->isDataRequested() )
    {
        commsState_ = GET_SIG_QUALITY;
    }
    // Otherwise we might as well try to sleep
    else
    {
        communicationsDone_ = true;
        this->resetFailCount(); // we were awake and no longer need a fix or comms. Assuming it's a good time to clear faults.
        commsState_ = GO_TO_SLEEP;
    }
}

//////////////////////
///    NEED_FIX
//////////////////////
void NAL9602::needFix()
{
    if( debug_ ) logger_.syslog( Str( "************** NEED_FIX **************\n" ), Syslog::INFO );

    gotFix_ = false;
    double latDeg;
    double lonDeg;

    if( !gpsTimeoutArmed_ )
    {
        needFixStartTime_ = Timestamp::Now();
        gpsTimeoutArmed_ = true;
    }

    if( needFixStartTime_.elapsed() > gpsFailTimeout_ )
    {
        logger_.syslog( "GPS failed to acquire within timeout.", Syslog::FAULT );
        this->setFailure( FailureMode::DATA );
        gpsTimeoutArmed_ = false;
        return;
    }


    if( !requestFix() )
    {
        return;
    }
    if( !simulateHardware() )
    {
        if( getFix() )
        {
            // For the NAL9602-LP:
            // 0 = Invalid Fix, 1 or 2 = Valid Fix, 3 = Estimated Dead Reckoning
            switch( fix_.quality_ )
            {
            case INVALID:
                logger_.syslog( Str( "Invalid GPS fix" ), Syslog::ERROR );
                break;
            case VALID_ONE:
            case VALID_TWO:
                // Write location to the slate
                latDeg =  fix_.latitude_;
                lonDeg = fix_.longitude_;
                gotFix_ = true;
                break;
            case DEAD_RECKON:
                logger_.syslog( Str( "Fix quality is estimated dead reckoning" ), Syslog::ERROR );
                break;
            default:
                logger_.syslog( Str( "Invalid fix quality reported: " + ( Str ) fix_.quality_ ), Syslog::ERROR );
                break;
            }
        }
    }

    // Always include simulated values, even if hardware exists.
    // If we are in the water, there will be no simulated values available.
    if( !belowSurfaceThreshold() )
    {
        if( SimSlate::Read( SimSlate::LATITUDE_DEGREE, latDeg )
                && SimSlate::Read( SimSlate::LONGITUDE_DEGREE, lonDeg ) )
        {
            Timestamp now( Timestamp::Now() );
            struct tm structTm( now.asStructTm() );
            fix_.utcdate_.year_ = structTm.tm_year + 1900;
            fix_.utcdate_.month_ = structTm.tm_mon + 1;
            fix_.utcdate_.day_ = structTm.tm_mday;
            fix_.utctime_.hours_ = structTm.tm_hour;
            fix_.utctime_.minutes_ = structTm.tm_min;
            fix_.utctime_.seconds_ = structTm.tm_sec;
            fix_.utctime_.centiSeconds_ = now.getTimeval().tv_usec / 10000;
            if( simulateHardware() )
            {
                gotFix_ = true;
                fix_.quality_ = ( DataQuality )1; // Surely it was a good fix
            }
        }
    }
    // Write the values if we got a valid fix
    if( gotFix_ )
    {
        Timestamp dataStartTime( Timestamp::Now() );
        gpsTimeoutArmed_ = false;

        if( fix_.quality_ == 1 || fix_.quality_ == 2 )
        {
            goodFixWriter_->write( Units::BOOL, true, dataStartTime );
        }
        else
        {
            goodFixWriter_->write( Units::BOOL, false, dataStartTime );
        }

        int deltaYear = fix_.utcdate_.year_ > 2000 ? 0 : 2000;
        utcTime_ = Timestamp( fix_.utcdate_.year_ + deltaYear, fix_.utcdate_.month_, fix_.utcdate_.day_, fix_.utctime_.hours_, fix_.utctime_.minutes_, fix_.utctime_.seconds_ );
        utcTime_ += Timespan( fix_.utctime_.centiSeconds_ / 100. );

        timeFixWriter_->setInvalid( false );
        timeFixWriter_->write( Units::EPOCH_SECOND, utcTime_.asDouble(), dataStartTime );
        latitudeFixWriter_->setInvalid( false );
        latitudeFixWriter_->write( Units::DEGREE, latDeg, dataStartTime );
        longitudeFixWriter_->setInvalid( false );
        longitudeFixWriter_->write( Units::DEGREE, lonDeg, dataStartTime );
        locationFixWriter_->setInvalid( false );
        Location location( Units::DEGREE, latDeg, lonDeg );
        // TODO note that Location, when accessed via I1D methods, returns RADIANS
        locationFixWriter_->write1DClass( Units::RADIAN, location, dataStartTime );

        logger_.syslog( "GPS fix at " + utcTime_.toSmallString() + ": (" + Str( latDeg ) + ", " + Str( lonDeg ) + ")", Syslog::INFO );

        // Write GGA specific values here
        if( requestGGA_ )
        {
            numSatellitesWriter_->write( Units::COUNT, fix_.nSatellites_, dataStartTime );
        }
        else // And RMC specific values here
        {
            sogWriter_->write( Units::KNOT, fix_.sog_, dataStartTime );
            cogWriter_->write( Units::DEGREE, fix_.cog_, dataStartTime );
        }
    }
    commsState_ = NEED_GSV;
}


//////////////////////
///    needGSV
//////////////////////
void NAL9602::needGSV()
{
    if( debug_ ) logger_.syslog( Str( "************** NEED GSV **************\n" ), Syslog::INFO );

    if( !simulateHardware() )
    {
        if( !requestGSV() )
        {
            return;
        }

        if( parseGSV() )
        {
            int i = 0;
            while( ( !snrData_.isEmpty() ) && ( i < MAX_GPS_SATS_9602 ) )
            {
                int val = *snrData_.pop( 0 );
                if( ( val > 0 ) && ( val < 100 ) )
                {
                    snrWriters_[i].snrWriter_->write( Units::COUNT, val );
                }
                i++;
            }
        }
    }

    // Now see if we need to do satcoms
    if( communicationsWriter_->isDataRequested() )
    {
        commsState_ = GET_SIG_QUALITY;
    }
    // If not, are we supposed to look for incoming messages?
    else if( !gotMTQueue_ && checkMTQueue_ )
    {
        commsState_ = MTQUEUE_QUALITY;
    }
    // Nothing else to do
    else
    {
        commsState_ = GO_TO_SLEEP;
    }
}


//////////////////////
///    SENDING1
//////////////////////
void NAL9602::sendingFillBuffer()
{
    if( debug_ ) logger_.syslog( Str( "************** SENDING FILL BUFFER **************\n" ), Syslog::INFO );

    // Send messages
    if( sendDataToShore_ )
    {
        Str sendFilename = Supervisor::GetToShoreFilename( Service::SERVICE_COURIER );
        if( sendFilename == Str::EMPTY_STR )
        {
            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 )
                                       / maxDownlinkDataSize_;
            char sentName[] = "Logs/YYYYMMDDTHHMMSS/shore####.lzma.parts/####.sbd...";
            snprintf( sentName, sizeof( sentName ) - 1, "%s.parts", sendFilename.cStr() );
            if( 0 == access( sentName,  X_OK ) )
            {
                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 )
    {
        FileInStream inStream( sendPacket_.sendFilename_.cStr() );
        if( inStream.isReadable() )
        {
            uint16_t maxPacket = ( sendPacket_.sendFilesize_ - 1 )
                                 / maxDownlinkDataSize_;
            size_t sendPosition = ( maxPacket - sendPacket_.packetsLeft_ ) * maxDownlinkDataSize_;
            inStream.seek( sendPosition );

            sendPacket_.dataSize_ = inStream.read( ( char* )sendPacket_.data_,
                                                   maxDownlinkDataSize_ ).bytesRead();
            // Never send 30 bytes, or we will be mistaken for an emergency packet!
            // Add one more byte.
            if( sendPacket_.dataSize_ + SENDPACKET_HEADER_SIZE == 30 )
            {
                sendPacket_.data_[sendPacket_.dataSize_] = 0;
                ++sendPacket_.dataSize_;
            }
            if( fillBuffer( ( unsigned char* )&sendPacket_,
                            sendPacket_.dataSize_ + SENDPACKET_HEADER_SIZE ) )
            {
                // Start the clock and move to next state
                startTime_ = Timestamp::Now();
                commsState_ = SENDING_TRANSMIT;
            }
            else
            {
                commsState_ = GO_TO_SLEEP;
            }
        }
        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

        }
    }
    else
    {
        resendSBDFilename_ = Supervisor::GetQueuedSBD();
        if( NULL != resendSBDFilename_ )
        {
            FileInStream inStream( resendSBDFilename_ );
            if( inStream.isReadable() )
            {
                sendPacket_.dataSize_ =
                    inStream.read( ( char* ) & sendPacket_, sizeof( sendPacket_ ) )
                    .bytesRead() - SENDPACKET_HEADER_SIZE;
                if( fillBuffer( ( unsigned char* )&sendPacket_,
                                sendPacket_.dataSize_ + SENDPACKET_HEADER_SIZE ) )
                {
                    // Start the clock and move to next state
                    startTime_ = Timestamp::Now();
                    commsState_ = SENDING_TRANSMIT;
                }
                else
                {
                    commsState_ = GO_TO_SLEEP;
                }
            }
            else
            {
                logger_.syslog( "Could not open SBD ", resendSBDFilename_, Syslog::FAULT );
                delete[] resendSBDFilename_;
                resendSBDFilename_ = NULL;
            }
        }

    }
    // Queue is empty. Initiate a satcoms session to see if we have anything waiting for us.
    if( sendPacket_.sendFilename_ == Str::EMPTY_STR && NULL == resendSBDFilename_ )
    {
        // Get the satellite signal quality
        if( sigQuality_ <= 0 )
        {
            // Abort the send if there is no connection
            logger_.syslog( Str( "Inadequate Iridium signal strength reported: " + ( Str ) sigQuality_ ), Syslog::ERROR );
            commsState_ = GO_TO_SLEEP;
        }
        // Good enough signal to initiate a session
        else
        {
            if( !simulateHardware() )
            {
                uart_.flush(); // Clear the uart
                uart_ << "AT+SBDI\r"; // Terminator is \r only
            }

            // Start the clock and move to next state
            startTime_ = Timestamp::Now();
            commsState_ = SENDING_VERIFY;
        }
    }
}

//////////////////////
///    SENDING2
//////////////////////
void NAL9602::sendingTransmit()
{
    if( debug_ ) logger_.syslog( Str( "************** SENDING_TRANSMIT **************\n" ), Syslog::INFO );
    // Slow down simulation of sending packets.
    if( simulateHardware() && startTime_.elapsed().asFloat() < 30 )
    {
        commsState_ = SENDING_TRANSMIT;
    }
    // First check for a timeout
    else if( startTime_.elapsed() > bufferFillTimeout_ )
    {
        logger_.syslog( Str( "Buffer fill timout failure." ), Syslog::ERROR );
        commsState_ = SENDING_FILL_BUFFER;
    }
    // Otherwise, see if there's anything to read, process it, and initiate a satellite session
    else if( simulateHardware() || uart_.dataAvailable() )
    {
        // If we kick off a session then move to the next state
        if( initiateSession() )
        {
            // Start the clock and move to next state
            startTime_ = Timestamp::Now();
            commsState_ = SENDING_VERIFY;
        }
        else
        {
            commsState_ = SENDING_FILL_BUFFER;
        }
    }
}

//////////////////////
///    SENDING3
//////////////////////
void NAL9602::sendingVerify()
{
    if( debug_ ) logger_.syslog( Str( "************** SENDING_VERIFY **************\n" ), Syslog::INFO );
    // First check for a timeout
    if( startTime_.elapsed() > bufferFillTimeout_ )
    {
        logger_.syslog( Str( "Verify xmit timeout failure." ), Syslog::FAULT );
        commsState_ = GO_TO_SLEEP;
        this->setFailure( FailureMode::DATA );
    }
    // Otherwise, see if there's anything to read, process it
    else if( simulateHardware() || uart_.canReadUntil( "OK" ) )
    {
        if( verifySessionXmit() )
        {
            gotMTQueue_ = true; // Since we got the queue status during normal use, no need to do it explicitly later.

            // This message was sent successfully, so remove it from the MO buffer
            if( !clearMOBuffer() )
            {
                logger_.syslog( Str( "Error clearing MO buffer." ), Syslog::ERROR );
            }

            if( sendPacket_.sendFilename_ != Str::EMPTY_STR )
            {
                logger_.syslog( "Sent " + Str( sendPacket_.dataSize_, 10 ) + " bytes from file " + sendPacket_.sendFilename_, Syslog::INFO );
                logger_.syslog( "Packets left to send: " + Str( sendPacket_.packetsLeft_, 10 ), Syslog::INFO );
                char sentName[] = "Logs/YYYYMMDDTHHMMSS/shore####.lzma.parts/####.sbd...";
                snprintf( sentName, sizeof( sentName ), "%s.parts", sendPacket_.sendFilename_.cStr() );
                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 );
            }

            // Then, see if there are uplinks to retrieve from the buffer
            //
            // <MT status>: The MT status provides an indication of the disposition of the mobile terminated transaction. The field can take on the following values:
            // 0 No SBD message to receive from the GSS
            // 1 SBD message successfully received from the GSS
            // 2 An error occurred while attempting to perform a mailbox check or receive a message from the GSS
            if( ( uplinkStatus_ == 1 ) || ( nPendingUplinks_ > 0 ) )
            {
                commsState_ = RECEIVING_1;
            }
            // Check to see if there was an uplink error while attempting to receive a message from the GSS
            else if( uplinkStatus_ == 2 )
            {
                logger_.syslog( Str( "MT Status is 2. Failed to received message from GSS" ), Syslog::FAULT );
            }

            // Finally, take care of the next things we need to send (if we have anything more)
            // There is nothing to be received anymore if we've gotten here
            else
            {
                // See if there's anything else to send
                if( ( sendPacket_.sendFilename_ != Str::EMPTY_STR
                        && communicationsWriter_->isDataRequested() )
                        || NULL != resendSBDFilename_ )
                {
                    commsState_ = SENDING_FILL_BUFFER;
                    if( NULL != resendSBDFilename_ )
                    {
                        delete[] resendSBDFilename_;
                        resendSBDFilename_ = NULL;
                    }
                }
                // We're all done with comms - no uplinks available and our queue is empty
                else
                {

                    // But wait... if we had previously received a command, let's break the logs and check once more.
                    // or at least tell shore we've gotten the last command...
                    if( cmdReceived_ )
                    {
                        cmdReceived_ = false;
                        // Close and compress the current shore log
                        Supervisor::IncrementShoreFile();
                        commsState_ = AWAKE;
                        return;
                    }

                    if( !communicationsDone_ )
                    {
                        communicationsDone_ = true;
                        communicationsWriter_->write( Units::BOOL, true );
                        if( gotFix_ )
                        {
                            this->resetFailCount(); // Comms are done and we have a GPS fix.
                            gpsTimeoutArmed_ = false;
                        }
                    }
                    if( !simulateHardware() )
                    {
                        uart_.flush();
                    }
                    commsState_ = GO_TO_SLEEP;
                }
            }
        }
        else
        {
            // Failed to xmit. Start over
            commsState_ = GO_TO_SLEEP;
        }
    }
}

//////////////////////
///    RECEIVING1
//////////////////////
void NAL9602::receiving1()
{
    if( debug_ ) logger_.syslog( Str( "************** RECEIVING_1 **************\n" ), Syslog::INFO );
    // Get messages that have come in
    if( !simulateHardware() )
    {
        if( !retrieveAndQueueIncoming( nUplinkBytes_ ) )
        {
            if( nPendingUplinks_ > 1 )
            {
                commsState_ = SENDING_FILL_BUFFER;
            }
            else
            {
                commsState_ = GO_TO_SLEEP;
            }
            return;
        }
        cmdReceived_ = true;
    }

    commsState_ = GO_TO_SLEEP;
}

//////////////////////
///    RECEIVING2
//////////////////////
void NAL9602::receiving2()
{
    //printf( "**************RECEIVING_2**************\n" );
}


//////////////////////
///    GO_TO_SLEEP
//////////////////////
void NAL9602::goToSleep()
{
    if( debug_ ) logger_.syslog( Str( "************** GoToSleep **************\n" ), Syslog::INFO );

    // Check one more time to be sure we don't have anything else to do
    if( latitudeFixWriter_->isDataRequested()
            || longitudeFixWriter_->isDataRequested()
            || timeFixWriter_->isDataRequested()
            || !latitudeReader_->isActive()
            || !longitudeReader_->isActive() )
    {
        commsState_ = NEED_FIX;
    }
    else if( communicationsWriter_->isDataRequested() )
    {
        commsState_ = AWAKE;
    }
    else
    {
        if( handleZDAMessages_ )
        {
            logger_.syslog( "Turning on ZDA messages", Syslog::INFO );
            uart_ << "AT+PG=1\r"; // Turn on ZDA messages while we aren't working
        }

        commsState_ = SLEEPING;
        startTime_ = Timestamp::Now();
    }
}


void NAL9602::uninitialize()
{
    if( debug_ ) logger_.syslog( "Uninitialize", Syslog::INFO );

    logger_.syslog( "Powering down", Syslog::INFO );
    if( !simulateHardware() )
    {
        uart_.close();
        // Disable xceiver
        uart_.disableUART();
        // Temp - turn off power here
        if( !loadControl_.powerDown() )
        {
            logger_.syslog( "Failed to power down", Syslog::ERROR );
        }
        powerOnTimeStart_ = Timestamp::Now(); // Start the clock
        cmdReceived_ = false;
    }
}


// Fill buffer for sending message to GSS
bool NAL9602::fillBuffer( const unsigned char* msg, size_t nMsgBytes )
{
    if( !simulateHardware() )
    {

        // Clear the input buffer
        uart_.flush();

        // Get the satellite signal quality
        if( sigQuality_ <= 0 )
        {
            return false;
        }

        unsigned short checksum = 0;
        // Work backwards, since nMsgBytes might increase
        for( unsigned i = nMsgBytes; i > 0 ; --i )
        {
            checksum += ( unsigned char )( msg[i - 1] );
        }
        // Command a buffer write
        uart_.flush();
        uart_ << "AT+SBDWB=" << ( int )nMsgBytes << '\r';

        unsigned int bytesToRead = uart_.canReadUntil( "READY" );
        int i = 0;
        while( ( bytesToRead <= 0 ) && ( i < 100 ) )
        {
            Timespan::Milliseconds( 0.1 ).sleepFor();
            bytesToRead = uart_.canReadUntil( "READY" );
            i++;
        }
        if( ( bytesToRead > 0 ) && ( bytesToRead <= sizeof( modemResponse_ ) ) )
        {
            uart_.read( modemResponse_, bytesToRead );
        }

        if( uart_.hasError() )
        {
            logger_.syslog( "Fill buffer uart error: ", uart_.errorString(), Syslog::ERROR );
            //**** Don't return here as the modem may be expecting the buffer to be filled.
            // Better to send it anyway just to be safe.
        }

        //Look for "READY response here
        if( ( strstr( modemResponse_, "READY" ) ) )
        {
            uart_.flush();
            // Write the data to the buffer
            uart_.write( ( char * )msg, nMsgBytes );
            uart_ << ( unsigned char )( checksum >> 8 );   // high byte first
            uart_ << ( unsigned char )( checksum & 0xFF ); // then low byte
        }
        else
        {
            logger_.syslog( "Failed to receive READY. Modem reported: ", modemResponse_, Syslog::ERROR );
            return false;
        }
    }
    return true;
}


// Checks the response from filling the buffer and looks for errors.
// Then initiates an SBD session
bool NAL9602::initiateSession()
{
    if( !simulateHardware() )
    {
        uart_.flushCRLF().readUntil( modemResponse_, sizeof( modemResponse_ ), 'K' ); // Grab the CR/LF,status, and "OK"
        if( uart_.hasError() )
        {
            logger_.syslog( "No ack after buffer fill. Uart error: ", uart_.errorString(), Syslog::ERROR );
            return false;
        }
        else
        {
            // Check the status
            if( ( strstr( modemResponse_, "0" ) ) )
            {
                // Message was sent to downlink buffer
                ;
            }
            else // There was an error so report it
            {
                switch( atoi( modemResponse_ ) )
                {
                case 1:
                    logger_.syslog( "Error: SBD Timeout.", Syslog::ERROR );
                    break;

                case 2:
                    logger_.syslog( "Error: SBD checksum mismatch.", Syslog::ERROR );
                    break;

                case 3:
                    logger_.syslog( "SBD incorrect message size", Syslog::ERROR );
                    break;

                default:
                    logger_.syslog( Str( "Unknown error while filling downlink buffer. Modem responded with:: " + ( Str ) modemResponse_ ), Syslog::ERROR );
                    break;
                }
                return false;
            }
        }
        // read back the "OK"
        uart_.flushCRLF().setStartTimeout( 2.0 ).readLine( modemResponse_, sizeof( modemResponse_ ) );
        uart_.resetStartTimeout();

        // Initiate a sattelite connection to send what was just put in the buffer
        uart_.flush(); // Clear the uart
        uart_ << "AT+SBDI\r"; // Terminator is \r only
    }
    return true;
}


// Checks the response from session initiation
// And verifies that the data was indeed transmitted
bool NAL9602::verifySessionXmit()
{
    bool bufferEmpty = false;

    if( !simulateHardware() )
    {
        char *ptr;
        // Get the response
        uart_.readUntil( modemResponse_, sizeof( modemResponse_ ), 'K' );
        if( uart_.hasError() )
        {
            logger_.syslog( "No xmit acknowledgment. Uart error: ", uart_.errorString(), Syslog::ERROR );
            return false;
        }
        else if( ( ptr = strstr( modemResponse_, "+SBDI:" ) ) )
        {
            int downlinkStatus, downlinkSeqNo;

            //Command Response: +SBDI: <MO status>, <MOMSN>, <MT status>, <MTMSN>, <MT length>, <MT queued>
            // See pg. 26 of the "AT COMMAND SET FOR MODEL 9601-DGS-LP Technical Note" April 2, 2008 for a detailed description
            // coverity[secure_coding] // indicate that the code below is indeed safe
            sscanf( ptr, "+SBDI:%d,%d,%d,%d,%d,%d",
                    &downlinkStatus, &downlinkSeqNo, &uplinkStatus_, &uplinkSeqNo_,
                    &nUplinkBytes_, &nPendingUplinks_ );
            logger_.syslog( "SBD MO Status=" + Str( downlinkStatus ) + ", MOMSN=" + Str( downlinkSeqNo ) +
                            ", MT Status=" + Str( uplinkStatus_ ) + ", MTMSN=" + Str( uplinkSeqNo_ ),
                            uplinkSeqNo_ == 0 ? Syslog::INFO : Syslog::IMPORTANT );

            // Downlink status can be 0 if our buffer is empty and we're simply initiating a session to check for incoming commands.
            if( sendPacket_.sendFilename_ == Str::EMPTY_STR && NULL == resendSBDFilename_ ) // This is the case where the buffer is empty
            {
                bufferEmpty = true;
            }

            if( downlinkStatus != 1 ) // If the status isn't 1, we need to see why not.
            {
                if( downlinkStatus == 0 ) // 0 is a valid option if the buffer is empty or we're checking the MT queue status
                {
                    if( !bufferEmpty && !checkMTQueue_ ) // If the buffer isn't empty and we're not just checking the MT queue, we shouldn't get a 0 status
                    {
                        logger_.syslog( Str( "Buffer populated but no message sent from NAL9602. Error code: " + ( Str ) downlinkStatus ), Syslog::ERROR );
                        return false;
                    }
                }
                else // The downlink status is something other than 1 or 0
                {
                    logger_.syslog( Str( "Failed to initiate SBD session. Error code: " + ( Str ) downlinkStatus ), Syslog::ERROR );
                    return false;
                }
            }
        }
    }
    return true;
}


// Clears the mobile originated buffer via the +SBDD0 command
bool NAL9602::clearMOBuffer()
{
    bool retVal = false;
    if( !simulateHardware() )
    {
        uart_.flush();
        uart_ << "AT+SBDD0\r";
        uart_.flushCRLF().setStartTimeout( 2.0 ).readUntil( modemResponse_, sizeof( modemResponse_ ), 'K' ); // Then get the response
        uart_.resetStartTimeout();
    }
    if( simulateHardware() || strstr( modemResponse_, "0" ) )
    {
        retVal = true;
    }
    return retVal;
}


// Retrieve (uplink) message from modem and queue it for clients
bool NAL9602::retrieveAndQueueIncoming( int nUplinkInBytes )
{

    if( !simulateHardware() )
    {
        // Get the uplink message from modem buffer
        uart_.flush();
        uart_ << "AT+SBDRT\r"; // Terminator is \r only

        uart_.setStartTimeout( 2.0 ).readUntil( modemResponse_, sizeof( modemResponse_ ), ':' ); // Then read until "+SBDRT:"
        uart_.read( modemResponse_, 2 ); // Then grab the <CR> (and <LF> for some reason??? - not per the spec)
        uart_.read( modemResponse_, nUplinkInBytes ); // Then grab the data.
        uart_.resetStartTimeout();
        unsigned int dataLen = uart_.getBytesRead();
        modemResponse_[nUplinkInBytes] = '\0';
        uart_.flush();
        if( uart_.hasError() )
        {
            logger_.syslog( "Reading from uplink buffer failed due to uart error:", uart_.errorString(), Syslog::ERROR );
            return false;
        }

        bool parsed( false );
        // Check that we read all the bytes
        if( dataLen == ( unsigned int )nUplinkInBytes )
        {
            parsed = DataReceiver::ReceiveSbd( modemResponse_, nUplinkInBytes, keyText_.asString().cStr(), logger_ );
        }

        if( parsed )
        {
            return true;
        }
        else // Parse failed
        {
            Str hex;
            StrIOStream hexStream( hex );
            hexStream.writeHex( modemResponse_, dataLen );
            logger_.syslog( Str( "Failed to parse uplink message:" ) + hex, Syslog::CRITICAL );
        }
    }
    return false;
}


double NAL9602::ParseLatitude( const char *token )
{
    char buf[16];
    double value;

    // First two characters are degrees
    strncpy( buf, token, 2 );
    buf[2] = 0;
    value = atof( buf );

    token += 2;
    value += ( atof( token ) / 60. );

    // Return value in degrees
    return value;
}


double NAL9602::ParseLongitude( const char *token )
{
    char buf[16];
    double value;

    // First three characters are degrees
    strncpy( buf, token, 3 );
    buf[3] = 0;
    value = atof( buf );

    token += 3;
    value += ( atof( token ) / 60. );

    // Return value in degrees
    return value;
}


NAL9602::Hemisphere NAL9602::char2Hemisphere( unsigned char hem )
{
    static Hemisphere h;

    switch( hem )
    {
    case 'N':
        h = NORTHERN_HEMI;
        break;

    case 'S':
        h = SOUTHERN_HEMI;
        break;

    case 'E':
        h = EASTERN_HEMI;
        break;

    case 'W':
        h = WESTERN_HEMI;
        break;

    default:
        logger_.syslog( Str( "Invalid hemisphere value reported: " + ( Str ) hem ), Syslog::ERROR );
        break;
    }
    return h;
}

bool NAL9602::getFix( void )
{
    bool retVal = true;

    if( !simulateHardware() )
    {
        /// Stores the response from the modem
        char modemResponse[255];

        uart_.flushCRLF().setStartTimeout( 0.5 ).readLines( modemResponse, sizeof( modemResponse ), 2 ); // Then grab the data
        uart_.resetStartTimeout();

        if( uart_.hasError() )
        {
            modemResponse[0] = '\0';
            logger_.syslog( "getFix uart error: ", uart_.errorString(), Syslog::ERROR );
            retVal = false;
        }
        else // No error
        {
            if( requestGGA_ )
            {
                //If we have a good fix, then parse it
                if( goodGGAFixIndicator( strchr( modemResponse, '$' ) ) )
                {
                    return parseGGAFix( strchr( modemResponse, '$' ) );
                }
                // Otherwise return false and try again later
                else
                {
                    retVal = false;
                }
            }
            else
            {
                //If we have a good fix, then parse it
                if( goodRMCFixIndicator( strchr( modemResponse, '$' ) ) )
                {
                    return parseRMCFix( strchr( modemResponse, '$' ) );
                }
                // Otherwise return false and try again later
                else
                {
                    retVal = false;
                }
            }
        }
        uart_.flush();
    }
    return retVal;
}


bool NAL9602::requestFix( void )
{
    bool retVal = false;
    if( !fixRequested_ )
    {
        if( !simulateHardware() )
        {
            uart_.flush();
            if( requestGGA_ )
            {
                // Send modem command for GGA NMEA string
                uart_ << "AT+PA=1\r"; // Terminator is \r only
            }
            else
            {
                // Send modem command for RMC NMEA string
                uart_ << "AT+PA=6\r"; // Terminator is \r only
            }
        }

        startTime_ = Timestamp::Now(); // Start the clock
        fixRequested_ = true;
        if( debug_ ) logger_.syslog( "Fix Requested", Syslog::DEBUG );
    }
    else
    {
        if( ( startTime_.elapsed() > requestTimeout_ )
                || ( !simulateHardware() && uart_.dataAvailable() )
                || ( simulateHardware() && startTime_.elapsed() > Timespan( 3 ) )
          )
        {
            retVal = true;
            fixRequested_ = false;
        }
    }
    return retVal;
}


bool NAL9602::requestGSV( void )
{
    bool retVal = false;
    if( !gsvRequested_ )
    {
        if( !simulateHardware() )
        {
            uart_.flush();
            // Send modem command for NMEA string
            uart_ << "AT+PA=4\r\n"; // Terminator is \r only
        }

        startTime_ = Timestamp::Now(); // Start the clock
        gsvRequested_ = true;
    }
    else
    {
        if( ( startTime_.elapsed() > requestTimeout_ )
                || ( !simulateHardware() && uart_.canReadUntil( "OK" ) )
          )
        {
            retVal = true;
            gsvRequested_ = false;
        }
    }
    return retVal;
}


void NAL9602::getReceivedSignalStrength()
{
    if( debug_ ) logger_.syslog( Str( "************** GET RECEIVED SIGNAL QUALITY **************\n" ), Syslog::INFO );

    // Moved the following code from where commsState_ was being assigned to GET_SIG_QUALITY
    // Should cause shore logs to "break" correctly.
    if( communicationsDone_ )
    {
        // Close and compress the current shore log
        // Moved the following code to LogSplitterComponent
        //Supervisor::IncrementShoreFile();
        communicationsDone_ = false;
    }
    if( !simulateHardware() )
    {
        // First clear the old value
        if( sigQuality_ != -1 )
        {
            uart_.flush();
            modemResponse_[0] = '\0';
            // Get signal quality
            uart_.flush();
            uart_ << "AT+CSQ\r";
            sigQuality_ = -1;
            startTime_ = Timestamp::Now();
        }
        else if(	unsigned int bytesToRead = uart_.canReadUntil( "OK" ) )
        {
            uart_.read( modemResponse_, bytesToRead );

            if( uart_.hasError() )
            {
                logger_.syslog( "getReceivedSignalStrength uart error ", uart_.errorString(), Syslog::ERROR );
            }
            char *ptr;
            if( ( ptr = strstr( modemResponse_, "CSQ:" ) ) )
            {
                // coverity[secure_coding] // indicate that the code below is indeed safe
                sscanf( ptr, "CSQ:%hd", &sigQuality_ );
                // Got it. Write and move on
                sigQualityWriter_->write( Units::COUNT, sigQuality_ );
                commsState_ = SENDING_FILL_BUFFER;
            }
            uart_.flush(); // Clear the uart
            // We intentionally don't reset the sigQuality to 0 here so that repeat invalid responses will be caught by the
            // timeout below.
        }
        else if( sigQuality_ == -1 && startTime_.elapsed() > requestTimeout_ )
        {
            sigQuality_ = 0;
            logger_.syslog( "Queried for signal strength and failed to receive proper response. ", Syslog::ERROR ); // Logging as error here to reduce comms to shore
            uart_.read( modemResponse_, 4095 );
            // non-blocking may be better: uart_.read(buffer,MIN(4096,uart_.dataAvailable())
            logger_.syslog( "received: ", modemResponse_, Syslog::ERROR ); // Not setting a failure here since component validation occurs at a higher level
            commsState_ = GO_TO_SLEEP;
        }
    }
    else if( simulateHardware() && !belowSurfaceThreshold() )
    {
        commsState_ = SENDING_FILL_BUFFER;
        sigQuality_ = 5;
        sigQualityWriter_->write( Units::COUNT, sigQuality_ );
    }
}


void NAL9602::getMTQueueSignalStrength() // Used only for session initiation for checking status of MT queue.
{
    if( debug_ ) logger_.syslog( Str( "************** GET MTQueue SIGNAL QUALITY **************\n" ), Syslog::INFO );


    // First arm the overall timeout for checking the MT Queue
    if( !iridiumTimeoutArmed_ )
    {
        needMTQueueStartTime_ = Timestamp::Now();
        iridiumTimeoutArmed_ = true;
    }
    // Did the MT queue timeout expire?
    if( needMTQueueStartTime_.elapsed() > iridiumMTQueueTimeout_ )
    {
        // If so, report the fault and go back to our sleep state. Try again next time.
        logger_.syslog( "MT Queue status failed to be acquired within timeout. Will not retry this session.", Syslog::FAULT );
        // We don't actually fail the component here or it will end up power cycling and trying constantly.
        iridiumTimeoutArmed_ = false;
        gotMTQueue_ = true; // so we don't try again this session
        commsState_ = GO_TO_SLEEP;
        return;
    }

    if( !simulateHardware() )
    {
        // First clear the old value
        if( sigQuality_ != -1 )
        {
            uart_.flush();
            modemResponse_[0] = '\0';
            // Get signal quality
            uart_ << "AT+CSQ\r";
            sigQuality_ = -1;
            startTime_ = Timestamp::Now();
        }
        else if(	unsigned int bytesToRead = uart_.canReadUntil( "OK" ) )
        {
            uart_.read( modemResponse_, bytesToRead );
            if( uart_.hasError() )
            {
                logger_.syslog( "getMTQueueSignalStrength uart error ", uart_.errorString(), Syslog::ERROR );
            }

            // Now parse the response
            char *ptr;
            if( ( ptr = strstr( modemResponse_, "CSQ:" ) ) )
            {
                // coverity[secure_coding] // indicate that the code below is indeed safe
                sscanf( ptr, "CSQ:%hd", &sigQuality_ );
                // Got it. Write the value.
                sigQualityWriter_->write( Units::COUNT, sigQuality_ );
                startTime_ = Timestamp::Now(); // We got a quality (good or bad) so we can reset the clock

                // If we have enough quality to initiate a session, go for it. Otherwise we keep checking
                if( sigQuality_ > 1 ) // Intentionally > 1 instead of >= 1 here. We don't often have a sucessful session with a 1.
                {
                    uart_ << "AT+SBDI\r"; // Terminator is \r only
                    commsState_ = MTQUEUE_SESSION;
                }
            }
            uart_.flush(); // Clear the uart
            // We intentionally don't reset the sigQuality to 0 here so that repeat invalid responses will be caught by the
            // timeout below.
        }
        // If sigQuality_ wasn't sucessfully written, this block catches it after the timeout.
        else if( sigQuality_ == -1 && startTime_.elapsed() > requestTimeout_ )
        {
            sigQuality_ = 0;
            logger_.syslog( "Failed to receive proper response when querying signal strength for MT queue check.", Syslog::ERROR ); // Logging as error here to reduce comms to shore
            uart_.read( modemResponse_, 4095 );
            // non-blocking may be better: uart_.read(buffer,MIN(4096,uart_.dataAvailable())
            logger_.syslog( "received: ", modemResponse_, Syslog::ERROR ); // Not setting a failure here since component validation occurs at a higher level
        }
    }
    else // if( !simulateHardware() )
    {
        commsState_ = MTQUEUE_SESSION;
        sigQuality_ = 5;
        sigQualityWriter_->write( Units::COUNT, sigQuality_ );
    }
}


void NAL9602::sessionMTQueueVerify() // Used only for session initiation verification for checking status of MT queue.
{
    if( debug_ ) logger_.syslog( Str( "************** SESSION MT QUEUE VERIFY **************\n" ), Syslog::INFO );

    // First check for a timeout and start over if necessary.
    if( startTime_.elapsed() > bufferFillTimeout_ ) // bufferFillTimeout_ is the max time that it _could_ take to get a response from the +SBDI command
    {
        logger_.syslog( Str( "Timeout waiting for SBDI response." ), Syslog::FAULT );
        commsState_ = MTQUEUE_QUALITY;
        this->setFailure( FailureMode::DATA );
    }

    // Otherwise, see if there's anything to read, process it
    else if( simulateHardware() || uart_.canReadUntil( "OK" ) )
    {
        if( verifySessionXmit() )
        {
            // At the least we have sucessfully checked for MT messages in the queue. Clear the timeout and set the comms state back to sleep.
            iridiumTimeoutArmed_ = false;
            gotMTQueue_ = true;
            commsState_ = GO_TO_SLEEP;

            // See if there are uplinks to retrieve from the buffer
            //
            // <MT status>: The MT status provides an indication of the disposition of the mobile terminated transaction. The field can take on the following values:
            // 0 No SBD message to receive from the GSS
            // 1 SBD message successfully received from the GSS
            // 2 An error occurred while attempting to perform a mailbox check or receive a message from the GSS
            if( ( uplinkStatus_ == 1 ) || ( nPendingUplinks_ > 0 ) )
            {
                logger_.syslog( "Data available in MT queue", Syslog::INFO );
                commsState_ = RECEIVING_1;
            }
            // Check to see if there was an uplink error while attempting to receive a message from the GSS
            else if( uplinkStatus_ == 2 )
            {
                logger_.syslog( Str( "MT Status is 2. Failed to received message from GSS" ), Syslog::FAULT );
            }
            else
            {
                logger_.syslog( "No messages in MT queue", Syslog::INFO );
                this->resetFailCount(); // If we get here, there's nothing to receive and we had a successful SBD sessin. Also, all other comms are done or not needed.
            }
        }
        // SBD session failed so try again
        else
        {
            commsState_ = MTQUEUE_QUALITY;
        }
    }
}


NAL9602::SentenceType NAL9602::GetNmeaSentenceType( const char *sentence )
{

    //char errorBuf[100];
    const char *endPtr = strchr( sentence, ',' );
    if( ( endPtr == 0 ) || ( endPtr == NULL ) )
    {
        return UNKNOWN_NMEA_TYPE;
    }

    // First field is the NMEA label
    char nmeaLabel[32];
    strncpy( nmeaLabel, sentence, endPtr - sentence );
    nmeaLabel[endPtr - sentence] = '\0';

    if( !strncmp( nmeaLabel, GGA_PREFIX, strlen( GGA_PREFIX ) ) )
    {
        return GPGGA;
    }
    else if( !strncmp( nmeaLabel, GSA_PREFIX, strlen( GSA_PREFIX ) ) )
    {
        return GPGSA;
    }
    else if( !strncmp( nmeaLabel, RMC_PREFIX, strlen( RMC_PREFIX ) ) )
    {
        return GPRMC;
    }
    else if( !strncmp( nmeaLabel, ZDA_PREFIX, strlen( RMC_PREFIX ) ) )
    {
        return GPZDA;
    }
    else
    {
        return UNKNOWN_NMEA_TYPE;
    }
}

bool NAL9602::goodGGAFixIndicator( const char *ggaSentence )
{
    if( ggaSentence == NULL )
    {
        return false;
    }

    // Check for a valid NMEA string
    if( GetNmeaSentenceType( ggaSentence ) != GPGGA )
    {
        logger_.syslog( Str( "Not a GGA sentence: " + ( Str ) ggaSentence ), Syslog::ERROR );
        return false;
    }

    // Checksum is at the end
    const char *endPtr = strchr( ggaSentence, '*' );
    //const char *checkSumPtr;

    if( endPtr )
    {
        //checkSumPtr = endPtr;
    }
    else
    {
        // No checksum on this sentence.
        //checkSumPtr = 0;
        endPtr = ( char * )ggaSentence + strlen( ggaSentence );
    }
    const size_t bufSize( 256 );
    char buf[bufSize];
    unsigned int nBytes = MIN( ( unsigned int )( endPtr - ggaSentence ), bufSize - 1 );

    strncpy( buf, ggaSentence, nBytes );
    buf[nBytes] = '\0';
    char *token = ( char * )buf;

    // Scroll over to the fix indicator
    for( int i = 0; i < 6; i++ )
    {
        token = strchr( token, ',' );
        if( token != NULL )
            token++;
        else
            return false;
    }

    if( ( atoi( token ) == 1 ) || ( atoi( token ) == 2 ) )
    {
        return true;
    }
    else
        return false;
}


bool NAL9602::goodRMCFixIndicator( const char *rmcSentence )
{
    if( rmcSentence == NULL )
    {
        return false;
    }

    // Check for a valid NMEA string
    if( GetNmeaSentenceType( rmcSentence ) != GPRMC )
    {
        logger_.syslog( Str( "Not a RMC sentence: " + ( Str ) rmcSentence ), Syslog::ERROR );
        return false;
    }

    // Checksum is at the end
    const char *endPtr = strchr( rmcSentence, '*' );
    //const char *checkSumPtr;

    if( endPtr )
    {
        //checkSumPtr = endPtr;
    }
    else
    {
        // No checksum on this sentence.
        //checkSumPtr = 0;
        endPtr = ( char * )rmcSentence + strlen( rmcSentence );
    }
    const size_t bufSize( 256 );
    char buf[bufSize];
    unsigned int nBytes = MIN( ( unsigned int )( endPtr - rmcSentence ), bufSize - 1 );

    strncpy( buf, rmcSentence, nBytes );
    buf[nBytes] = '\0';
    char *token = ( char * )buf;

    // Scroll over to the fix indicator
    for( int i = 0; i < 2; i++ )
    {
        token = strchr( token, ',' );
        if( token != NULL )
            token++;
        else
            return false;
    }
    if( token[0] == 'A' )
    {
        return true;
    }
    else
    {
        return false;
    }
}


/*
NOTE: This method implementation is not very smart yet. The following
should probably be added:
  Check for "GGA" in the label
  Check for valid values in each field

Also, the NMEA GGA standard apparently just contains UTC time-of-day, and
does NOT include the date!

T.O'R.
*/
bool NAL9602::parseGGAFix( const char *ggaSentence )
{
    // First check for null
    if( ggaSentence == NULL )
    {
        logger_.syslog( Str( "GGA sentence equals NULL" ) );
        return false;
    }

    // Check for a valid NMEA string
    if( GetNmeaSentenceType( ggaSentence ) != GPGGA )
    {
        logger_.syslog( Str( "Not a GGA sentence: " + ( Str ) ggaSentence ), Syslog::ERROR );
        return false;
    }

    // Log real string before parsing
    logger_.syslog( Str( "Parsing GGA sentence: " + ( Str ) ggaSentence ), Syslog::DEBUG );

    enum
    {
        InLabel,
        InTime,
        InLatitude,
        InNsHemisphere,
        InLongitude,
        InEwHemisphere,
        InQuality,
        InNSatellites,
        InHdop,
        InAltitude,
        InAltitudeUnits,
        InGeoidHeight,
        InGeoidHeightUnits,
        InDgpsAge,
        InDgpsStationId,
        Finished
    };

    int state = InLabel;

    // Checksum is at the end
    const char *endPtr = strchr( ggaSentence, '*' );
    //const char *checkSumPtr;

    if( endPtr )
    {
        //checkSumPtr = endPtr;
    }
    else
    {
        // No checksum on this sentence.
        //checkSumPtr = 0;
        endPtr = ggaSentence + strlen( ggaSentence );
    }

    const size_t bufSize( 256 );
    char buf[bufSize];

    unsigned int nBytes = MIN( ( unsigned int )( endPtr - ggaSentence ), bufSize - 1 );

    strncpy( buf, ggaSentence, nBytes );
    buf[nBytes] = '\0';

    char *token = ( char * )buf;
    Timestamp now( Timestamp::Now() ); // To record the current date
    struct tm structTm( now.asStructTm() );

    while( state != Finished && token != 0 )
    {

        switch( state )
        {

        case InLabel:
            break;

        case InTime:
            char timeStr[16];
            // coverity[secure_coding] // indicate that the code below is indeed safe
            sscanf( token, "%9s", timeStr );  //Adds a terminating null
            // coverity[secure_coding] // indicate that the code below is indeed safe
            sscanf( timeStr, "%2d%2d%2d.%2d", &( fix_.utctime_.hours_ ), &( fix_.utctime_.minutes_ ), &( fix_.utctime_.seconds_ ), &( fix_.utctime_.centiSeconds_ ) );

            // Let's add the date as today since that's not included in the GGA string.
            fix_.utcdate_.year_ = structTm.tm_year + 1900;
            fix_.utcdate_.month_ = structTm.tm_mon + 1;
            fix_.utcdate_.day_ = structTm.tm_mday;
            break;

        case InLatitude:
            fix_.latitude_ = ParseLatitude( token );
            break;

        case InNsHemisphere:
            fix_.nsHemisphere_ = char2Hemisphere( token[0] );
            // Make negative for Southern hemishere
            if( fix_.nsHemisphere_ == SOUTHERN_HEMI )
            {
                fix_.latitude_ *= -1;
            }
            break;

        case InLongitude:
            fix_.longitude_ = ParseLongitude( token );
            break;

        case InEwHemisphere:
            fix_.ewHemisphere_ = char2Hemisphere( token[0] );
            if( fix_.ewHemisphere_ == WESTERN_HEMI )
            {
                fix_.longitude_ *= -1;
            }
            break;

        case InQuality:
            fix_.quality_ = ( DataQuality )atoi( token );
            break;

        case InNSatellites:
            fix_.nSatellites_ = atoi( token );
            break;

        case InHdop:
            fix_.hdop_ = atof( token );
            break;

        case InAltitude:
            fix_.altitude_ = atof( token );
            break;

        case InAltitudeUnits:
            break;

        case InGeoidHeight:
            fix_.geoidHeight_ = atof( token );
            break;

        case InGeoidHeightUnits:
            break;

        case InDgpsAge:
            fix_.dgpsDataAge_ = atoi( token );
            break;

        case InDgpsStationId:
            fix_.dgpsStationId_ = atoi( token );
            break;
        }

        // Advance to next state
        state++;

        // Point to next token
        if( ( token = strchr( token, ',' ) ) == 0 )
            // No more tokens
            break;

        token++;
    }

    // Check for errors
    if( state != Finished )
    {
        logger_.syslog( Str( "Incomplete GGA sentence: " + ( Str ) ggaSentence ), Syslog::ERROR );
        return false;
    }
    else if( token )
    {
        logger_.syslog( Str( "Extra characters in GGA sentence: " + ( Str ) ggaSentence ), Syslog::ERROR );
        return false;
    }
    // Check for exactly a 0,0 fix. The GGA string can return blank lat/lon with a quality of 1 (good fix).
    else if( fix_.latitude_ == 0.0 && fix_.longitude_ == 0.0 )
    {
        return false;
    }

    // Otherwise we can assume success
    return true;

}


bool NAL9602::parseRMCFix( const char *rmcSentence )
{
    // First check for null
    if( rmcSentence == NULL )
    {
        logger_.syslog( Str( "RMC sentence equals NULL" ) );
        return false;
    }

    // Check for a valid NMEA string
    if( GetNmeaSentenceType( rmcSentence ) != GPRMC )
    {
        logger_.syslog( Str( "Not a RMC sentence: " + ( Str ) rmcSentence ), Syslog::ERROR );
        return false;
    }

    // Log real string before parsing
    logger_.syslog( Str( "Parsing RMC sentence: " + ( Str ) rmcSentence ), Syslog::DEBUG );

    enum
    {
        InLabel,
        InTime,
        InStatus,
        InLatitude,
        InNsHemisphere,
        InLongitude,
        InEwHemisphere,
        InSOG,
        InCOG,
        InDate,
        InMagVariation,   // Not supported
        InMagVariationEW, // Not supported
        InMode,
        Finished
    };

    int state = InLabel;

    // Checksum is at the end
    const char *endPtr = strchr( rmcSentence, '*' );
    //const char *checkSumPtr;

    if( endPtr )
    {
        //checkSumPtr = endPtr;
    }
    else
    {
        // No checksum on this sentence.
        //checkSumPtr = 0;
        endPtr = rmcSentence + strlen( rmcSentence );
    }

    const size_t bufSize( 256 );
    char buf[bufSize];

    unsigned int nBytes = MIN( ( unsigned int )( endPtr - rmcSentence ), bufSize - 1 );

    strncpy( buf, rmcSentence, nBytes );
    buf[nBytes] = '\0';

    char *token = ( char * )buf;

    while( state != Finished && token != 0 )
    {
        switch( state )
        {

        case InLabel:
            break;

        case InTime:
            char timeStr[16];
            // coverity[secure_coding] // indicate that the code below is indeed safe
            sscanf( token, "%9s", timeStr );  //Adds a terminating null
            // coverity[secure_coding] // indicate that the code below is indeed safe
            sscanf( timeStr, "%2d%2d%2d.%2d", &( fix_.utctime_.hours_ ), &( fix_.utctime_.minutes_ ), &( fix_.utctime_.seconds_ ), &( fix_.utctime_.centiSeconds_ ) );
            break;

        case InStatus:
            if( strstr( token, "A" ) )
            {
                fix_.status_ = true;
            }
            else
            {
                fix_.status_ = false;
            }
            break;

        case InLatitude:
            fix_.latitude_ = ParseLatitude( token );
            break;

        case InNsHemisphere:
            fix_.nsHemisphere_ = char2Hemisphere( token[0] );
            // Make negative for Southern hemishere
            if( fix_.nsHemisphere_ == SOUTHERN_HEMI )
            {
                fix_.latitude_ *= -1;
            }
            break;

        case InLongitude:
            fix_.longitude_ = ParseLongitude( token );
            break;

        case InEwHemisphere:
            fix_.ewHemisphere_ = char2Hemisphere( token[0] );
            if( fix_.ewHemisphere_ == WESTERN_HEMI )
            {
                fix_.longitude_ *= -1;
            }
            break;

        case InSOG:
            fix_.sog_ = atof( token );
            break;

        case InCOG:
            fix_.cog_ = atof( token );
            break;

        case InDate:
            char dateStr[16];
            // coverity[secure_coding] // indicate that the code below is indeed safe
            sscanf( token, "%9s", dateStr );  //Adds a terminating null
            // coverity[secure_coding] // indicate that the code below is indeed safe
            sscanf( dateStr, "%2d%2d%2d", &( fix_.utcdate_.day_ ), &( fix_.utcdate_.month_ ), &( fix_.utcdate_.year_ ) );
            // TODO: Should really fix the year part of the date here where you read it in.
            break;

        case InMagVariation:
            // Not supported
            break;

        case InMagVariationEW:
            // Not supported
            break;

        case InMode:
            fix_.quality_ = ( DataQuality )0;

            if( strstr( token, "A" ) )
            {
                fix_.quality_ = ( DataQuality )1;
            }
            else if( strstr( token, "D" ) )
            {
                fix_.quality_ = ( DataQuality )2;
            }
            else if( strstr( token, "E" ) )
            {
                fix_.quality_ = ( DataQuality )3;
            }
            break;
        }

        // Advance to next state
        state++;

        // Point to next token
        if( ( token = strchr( token, ',' ) ) == 0 )
            // No more tokens
            break;

        token++;
    }

    // Check for errors
    if( state != Finished )
    {
        logger_.syslog( Str( "Incomplete RMC sentence: " + ( Str ) rmcSentence ), Syslog::ERROR );
        return false;
    }
    else if( token )
    {
        logger_.syslog( Str( "Extra characters in RMC sentence: " + ( Str ) rmcSentence ), Syslog::ERROR );
        return false;
    }

    // Otherwise we can assume success
    return true;

}


bool NAL9602::parseGSV( void )
{
    bool retVal = true;

    // Clear the stack
    while( !snrData_.isEmpty() )
    {
        snrData_.pop();
    }

    if( !simulateHardware() )
    {
        /// Stores the response from the modem
        char modemResponse[960]; // roughly 80 character lines and up to 12 satellites used
        char* strTokPtr;

        uart_.flushCRLF().setStartTimeout( 0.0 ).readUntil( modemResponse, sizeof( modemResponse ), ':' ); // First discard the echo and the "+PA:"
        uart_.flushCRLF().setStartTimeout( 0.0 ).readUntil( modemResponse, sizeof( modemResponse ), 'K' );// Then grab the data
        uart_.resetStartTimeout();

        if( uart_.hasError() )
        {
            modemResponse[0] = '\0';
            logger_.syslog( "parseGSV uart error: ", uart_.errorString(), Syslog::ERROR );
            retVal = false;
        }
        else // No error
        {
            //If we have a good fix, then parse it
            char* response = strtok_r( modemResponse, "$GPGSV", &strTokPtr );

            if( response == NULL )
            {
                retVal = false;
            }

            int line_num = 0;
            while( ( response != NULL ) && ( line_num < ( MAX_GPS_SATS_9602 / MAX_SATS_PER_LINE ) ) )
            {
                char* commaTokPtr;
                char* snr = strtok_r( response + 7, ",", &commaTokPtr );
                int posNum = 1;
                while( snr != NULL )
                {
                    if( posNum % 4 == 0 )
                    {
                        int snrVal = atoi( snr );
                        snrData_.push( new int( snrVal ) );
                    }
                    snr = strtok_r( NULL, ",", &commaTokPtr );
                    posNum++;
                }
                response = strtok_r( NULL, "$GPGSV", &strTokPtr );
                line_num++;
            }
        }
    }
    return retVal;
}

bool NAL9602::checkStrobe( void )
{
    int mode;
    static StrobeMode oldStrobeMode = STROBE_OFF;
    static short cycles = 0;

    if( simulateHardware() )
    {
        return false;
    }

    if( strobeModeReader_->isActive() && strobeModeReader_->wasTouchedSinceLastRun( this )
            && strobeModeReader_->read( Units::COUNT, mode ) )
    {
        strobeMode_ = ( StrobeMode ) mode;
    }

    if( belowSurfaceThreshold() )
    {
        // Once we are under, the strobe should be off regardless of the
        // temporary user setting. This will also clear the STROBE_TEMP_OFF
        // setting so when we get back up the strobe turns on.
        strobeMode_ = STROBE_OFF;
    }
    else
    {
        // on surface
        if( strobeMode_ != STROBE_TEMP_OFF )
        {
            strobeMode_ = STROBE_ON;
        }
    }

    // Only send SPI commands if there is an actual change to keep unnecessary
    // traffic off the bus. But send at least one every minute for paranoia
    // reasons.
    if( oldStrobeMode == strobeMode_ && cycles++ < 150 )
    {
        return false;
    }
    cycles = 0;
    oldStrobeMode = strobeMode_;

    if( strobeMode_ == STROBE_ON )
    {
        if( debug_ ) logger_.syslog( "Turning strobe on", Syslog::IMPORTANT );

        if( loadControl_.getPowerState() == LoadControl::OFF ||
                loadControl_.getPowerState() == LoadControl::POWER_DOWN )
        {
            if( debug_ ) logger_.syslog( "Turning LCB on", Syslog::IMPORTANT );
            if( !loadControl_.powerUp() )
            {
                logger_.syslog( "Failed to power up", Syslog::ERROR );
                this->setFailure( FailureMode::HARDWARE );
            }
            // Return here and set discrete next round to not send two commands
            // to LCB to fast in succession.
            oldStrobeMode = STROBE_TRANSITION;
            return true;
        }
        if( !loadControl_.discreteOff() )
        {
            logger_.syslog( "Discrete off failed", Syslog::FAULT );
        }
    }
    else if( strobeMode_ == STROBE_OFF || strobeMode_ == STROBE_TEMP_OFF )
    {
        if( debug_ ) logger_.syslog( "Turning strobe off", Syslog::IMPORTANT );
        if( !loadControl_.discreteOn() )
        {
            logger_.syslog( "Discrete on failed", Syslog::FAULT );
        }
    }
    return true;
}

NAL9602::NmeaUTCTime::NmeaUTCTime()
    : hours_( 0 ),
      minutes_( 0 ),
      seconds_( 0 ),
      centiSeconds_( 0 )
{}


NAL9602::NmeaUTCDate::NmeaUTCDate()
    : day_( 0 ),
      month_( 0 ),
      year_( 0 )
{}

NAL9602::Fix::Fix()
    : utctime_(),
      utcdate_(),
      latitude_( nanf( "" ) ),
      nsHemisphere_( INVALID_HEMISPHERE ),
      longitude_( nanf( "" ) ),
      ewHemisphere_( INVALID_HEMISPHERE ),
      status_( 0 ),
      sog_( 0 ),
      cog_( 0 ),
      quality_( INVALID ),
      nSatellites_( -1 ),
      hdop_( nanf( "" ) ),
      altitude_( nanf( "" ) ),
      geoidHeight_( nanf( "" ) ),
      dgpsDataAge_( -1 ),
      dgpsStationId_( -1 )
{}


bool NAL9602::isDataRequested()
{
    return latitudeFixWriter_->isDataRequested()
           || longitudeFixWriter_->isDataRequested()
           || timeFixWriter_->isDataRequested()
           || !latitudeReader_->isActive()
           || !longitudeReader_->isActive()
           || communicationsWriter_->isDataRequested();
}

/// Should return [myNamespace]::SIMULATE_HARDWARE, or [myNamespace]::POWER, etc
ConfigURI NAL9602::getConfigURI( ConfigOption configOption ) const
{
    switch( configOption )
    {
    case CONFIG_POWER:
        return NAL9602IF::POWER;
    case CONFIG_SIMULATE_HARDWARE:
        return NAL9602IF::SIMULATE_HARDWARE;
    default:
        return ConfigURI::NO_CONFIG_URI;
    }
}

/// Should return [myNamespace]::POWER_[forElement], [myNamespace]::LATENCY_[forElement], etc
ConfigURI NAL9602::getConfigURI( ConfigOption configOption, ElementURI forElement ) const
{
    switch( configOption )
    {
    case CONFIG_POWER:
        if( forElement == UniversalURI::PLATFORM_COMMUNICATIONS )
        {
            return NAL9602IF::POWER_PLATFORM_COMMUNICATIONS;
        }
        else
        {
            return ConfigURI::NO_CONFIG_URI;
        }
    default:
        return ConfigURI::NO_CONFIG_URI;
    }
}


// Initialize values
NAL9602::SNRWriters::SNRWriters()
    :
    snrWriter_( NULL )

{}


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

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

    if( simulateHardware() )
    {
        commsState_ = AWAKE;
        return STARTING;
    }

    // Stores the response from the modem
    modemResponse_[0] = '\0';

    sigQuality_ = 0; // assume no signal to start so we will trigger a query

    ok_ = loadParams();
    if( !ok_ )
    {
        logger_.syslog( Str( "Error: Error loading parameters in initialization routine. Returning.\n" ), Syslog::CRITICAL );
    }

    // Temp - turn on power here
    logger_.syslog( "Powering up NAL9602", Syslog::INFO );
    if( !loadControl_.powerUp() )
    {
        logger_.syslog( "Failed to power up", Syslog::ERROR );
    }

    // Enable xceiver
    uart_.enableUART();

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


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

    if( simulateHardware() )
    {
        return RUNNABLE;
    }

    if( powerOnTimeStart_.elapsed() > poResetTimeout_ )
    {
        commsState_ = WAKING;
        return RUNNABLE;
    }
    return STARTING;
}


/// Pause for a short period (indicated by pauseTime)
Component::RunState NAL9602::pause()
{
    if( debug_ ) logger_.syslog( "Pause", Syslog::INFO );

    // Last chance...
    if( isDataRequested() )
    {
        commsState_ = AWAKE;
        return RUNNABLE;
    }

    if( startTime_.elapsed() > powerDownTimeout_ )
    {
        if( fastGPS_ ) // If we want to try getting faster fixes, leave the unit powered.
        {
            logger_.syslog( "Not Powering down - fast GPS", Syslog::INFO );
        }
        else
        {
            logger_.syslog( "Powering down", Syslog::INFO );
            if( !simulateHardware() )
            {
                if( !loadControl_.powerDown() )
                {
                    logger_.syslog( "Failed to power down", Syslog::ERROR );
                }
            }
        }
        cmdReceived_ = false;
        gotMTQueue_ = false;

        goodFixWriter_->write( Units::BOOL, false );
        numSatellitesWriter_->write( Units::COUNT, nan( "" ) );
        sigQualityWriter_->write( Units::COUNT, nan( "" ) );
        sogWriter_->write( Units::KNOT, nan( "" ) );
        cogWriter_->write( Units::DEGREE, nan( "" ) );
        for( int i = 0; i < MAX_GPS_SATS_9602; i++ )
        {
            snrWriters_[i].snrWriter_->write( Units::COUNT, nan( "" ) );
        }
        return PAUSED;
    }

    checkStrobe();
    return PAUSE;
}


/// Should eventually follow a PAUSE request: should set continueTime
Component::RunState NAL9602::paused()
{
    if( debug_ ) logger_.syslog( "Paused", Syslog::INFO );

    // See if it's time to get a fix
    if( isDataRequested() )
    {
        // If we're already powered and got to paused, just jump right in
        if( ( fastGPS_ ) && ( loadControl_.getPowerState() != LoadControl::OFF ) && ( loadControl_.getPowerState() != LoadControl::POWER_DOWN ) )
        {
            commsState_ = AWAKE;
            return RUNNABLE;
        }

        logger_.syslog( "Powering up", Syslog::INFO );
        if( !simulateHardware() )
        {
            if( !loadControl_.powerUp() )
            {
                logger_.syslog( "Failed to power up", Syslog::ERROR );
                this->setFailure( FailureMode::HARDWARE );
            }
        }
        powerOnTimeStart_ = Timestamp::Now(); // Start the clock
        return STARTING;
    }
    checkStrobe();
    return PAUSED;
}


Component::RunState NAL9602::resume()
{
    if( debug_ ) logger_.syslog( "Resume", Syslog::INFO );
    return RESUMING; // State not used at this time
}


Component::RunState NAL9602::resuming()
{
    if( debug_ ) logger_.syslog( "Resuming", Syslog::INFO );
    return RUNNABLE; // State not used at this time
}


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

    readConfig();

    // Only check voltage/current if we did not just modify the strobe state
    // since the LCB might not process SPI commands that fast in succession.
    if( !checkStrobe() && !simulateHardware() )
    {
        // Log voltage and current and check for any faults
        loadControl_.requestVoltageAndCurrent();
        if( loadControl_.hasError() )
        {
            logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }
    }

    switch( commsState_ )
    {
    case SLEEPING:
        return PAUSE;
        break;
    case WAKING:
        // End states CMDMODE, WAKING
        waking();
        break;
    case CMDMODE:
        cmdMode(); // Waits for command prompt. End states CMDMODE, AWAKE
        break;
    case AWAKE:
        // Check dataRequested. End states NEED_FIX, GET_SIG_QUALITY, GO_TO_SLEEP
        awake();
        break;
    case NEED_FIX:
        // Requests & gets fix. End states NEED_FIX, NEED_GSV
        needFix();
        break;
    case NEED_GSV:
        // Requests & gets gsv. NEED_GSV, GET_SIG_QUALITY, GO_TO_SLEEP
        needGSV();
        break;
    case GET_SIG_QUALITY:
        // Get signal strength. End states SENDING_FILL_BUFFER, GO_TO_SLEEP
        getReceivedSignalStrength();
        break;
    case SENDING_FILL_BUFFER:
        // Query Supervisor to see if data to be sent, if so, queue it up
        // End states SENDING_TRANSMIT, GO_TO_SLEEP,  SENDING_VERIFY
        sendingFillBuffer();
        break;
    case SENDING_TRANSMIT:
        // Wait for OK.  End states SENDING_TRANSMIT, SENDING_FILL_BUFFER,  SENDING_VERIFY
        sendingTransmit();
        break;
    case SENDING_VERIFY:
        // End states GO_TO_SLEEP, RECEIVING_1, SENDING_FILL_BUFFER, etc
        sendingVerify();
        break;
    case RECEIVING_1:
        receiving1();
        break;
    case RECEIVING_2:
        receiving2();
        break;
    case MTQUEUE_QUALITY:
        getMTQueueSignalStrength(); // End states GO_TO_SLEEP, MTQUEUE_SESSION
        break;
    case MTQUEUE_SESSION:
        sessionMTQueueVerify(); // End states GO_TO_SLEEP, MTQUEUE_QUALITY, RECEIVING_1
        break;
    case GO_TO_SLEEP:
        goToSleep();
        break;
    default:
        logger_.syslog( "Invalid comms state: ", commsState_, Syslog::ERROR );
        break;
    }

    return RUNNABLE;
}


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


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

    }
    // Currently used to delay one more cycle to allow the EZ Servo to fully power down
    return STOPPED;
}


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