/*** \file
 *
 *  Contains the Dynamic Docking Module (DDM) class implementation.
 *
 *  Copyright (c) 2007,2008,2009-2019 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#include "DDM.h"
#include "DDMIF.h"

#include <cstdlib>

#include "Power24vConverterIF.h"
#include "data/ConfigReader.h"
#include "data/SimSlate.h"
#include "units/Units.h"

/* DDM docking states
 *
 * STANDBY: Powered on, whiskers retracted, latch closed.
 * ARM:     Whiskers extended, latch open, latch armed to close after opto switch closes.
 * DETACH:  Whiskers retracted, latch open.
 * SLIDE:   Null, not used for DDM module
 *
 */

#define CABLE_PRESENT_DEBOUNCE_COUNTS (5)       // how many successive cable present readings needed to call it really present 

DDM::DDM( const Module* module )
    : SyncSensorComponent( DDMIF::NAME, module ),
      debug_( false ),
      cablePresent_( false ),         // set to true when the IR emmitter shows an obstruction (and we just hope it's the dock cable)
      cablePresentCounts_( 0 ),       // Keeps track of how many successive present readings there are
      cableStatus_( 0 ),              // hold the actual value read from the sensor
      cableStatusFailed_( false ),    // set to true if there was a problem getting the state of the cable. Allows for another try before failing the component.
      verbosityCfgSetting_( 0 ),      // Verbosity: 0=concise to 3=most verbose
      PWMLimitCfgSetting_( 96 ),      // Recommended value from WHOI
      currentLimitCfgSetting_( 144 ), // Recommended value from WHOI
      lastDDMMode_( 0 ),              // Store the last known commanded mode
      modeChanged_( false ),          // Lets us know if the mode has changed this cycle to allow command change mid transit
      whiskerState_( 2 ),             // 1 is whiskers opened, 2 is whiskers closed, 0 is whiskers in the middle
      latchState_( 1 ),               // 1 is latch closed, 2 is latch open, 0 is latch in the middle.
      cableState_( 255 ),             // 255 is wide open, < 200 or so indicates obscuration across IR emitter
      DDMMode_( DockIF::STANDBY ),    // We'll start in stanbdy mode
      DDMModeCmd_( DockIF::STANDBY ), // This is the commanded mode. We'll start in stanbdy mode unless we hear otherwise.
      loadControl_( DDMIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      poTimeout_( 25 ),
      powerDownTimeout_( 5 ),
      hardwareTimeout_( 30 ),          // Max time allowed for hardware to make it to its commanded position
      initRequested_( false ),         // Was initilization status requested
      uart_( DDMIF::UART, DDMIF::BAUD, 0.6, logger_, 4095 )
{
    // Configuration inputs
    verbosityCfgReader_ = newConfigReader( DDMIF::VERBOSITY_CFG );
    PWMLimitCfgReader_ = newConfigReader( DDMIF::PWM_LIMIT_CFG );
    currentLimitCfgReader_ = newConfigReader( DDMIF::CURRENT_LIMIT_CFG );

    // Slate inputs
    DDMModeCmdReader_  = newDataReader( DockIF::DOCKING_STATE_CMD );
    power24vConverterDataReader_ = newDataReader( Power24vConverterIF::POWER_24V_CONVERTER );

    // Slate outputs
    whiskerStateWriter_ = newDataWriter( DDMIF::WHISKER_STATE_READING );
    latchStateWriter_ = newDataWriter( DDMIF::LATCH_STATE_READING );
    cableValueWriter_ = newDataWriter( DDMIF::CABLE_VALUE_READING );

    cablePresentWriter_ = newDataWriter( DockIF::DOCK_CABLE_PRESENT );
    DDMModeWriter_ = newDataWriter( DockIF::DOCKING_STATE );

    deviceResponse_[0] = '\0';

    this->setFailureMissionCritical( true );
    this->setAllowableFailures( 3 );
    this->setRetryTimeout( 60 );

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


DDM::~DDM()
{
}


void DDM::run()
{
}


void DDM::readConfig()
{
    verbosityCfgReader_->read( Units::COUNT, verbosityCfgSetting_ );
    if( !PWMLimitCfgReader_->read( Units::COUNT, PWMLimitCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for PWM Limit, using", PWMLimitCfgSetting_, Syslog::ERROR );
    }
    if( !currentLimitCfgReader_->read( Units::COUNT, currentLimitCfgSetting_ ) )
    {
        logger_.syslog( "Could not read configuration setting for current limit, using", currentLimitCfgSetting_, Syslog::ERROR );
    }
}

void DDM::uninitialize()
{
    if( debug_ ) logger_.syslog( "uninitialize", Syslog::INFO );
    if( !simulateHardware() )
    {
        logger_.syslog( "Powering down", Syslog::INFO );
        if( !loadControl_.powerDown() )
        {
            logger_.syslog( "Failed to power down", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }
        powerOffTimeStart_ = Timestamp::Now();
        uart_.close();
    }
    // 24V power is no longer needed
    power24vConverterDataReader_->requestData( false );
}


Component::RunState DDM::start()
{
    if( debug_ ) logger_.syslog( "Start", Syslog::INFO );

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

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

    if( simulateHardware() )
    {
        startTime_ = Timestamp::Now();
        DDMModeCmd_ = ( DockIF::STANDBY ); // Initialize to a known state in case we're coming out of a failure
        return STARTING;
    }

    deviceResponse_[0] = '\0';

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

    logger_.syslog( "Initializing DDM." );

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

    DDMModeCmd_ = DockIF::STANDBY; // Initialize to a known state in case we're coming out of a failure
    modeChanged_ = true; // Initialize to a known state and force issue the first command
    startTime_ = Timestamp::Now();
    initRequested_ = false;
    uart_.flush();
    return STARTING;
}


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

    readConfig();

    if( simulateHardware() )
    {
        return RUNNABLE;
    }

    // This is the main initialization confirmation for the DDM. The DDM includes 3 separate boards
    // all wired to one serial port. As such, commands need to be staggered to avoid bus contention.
    // Additionally the capture detect processor board will occasionally output a boot banner ending with
    // a \r. Comms with the other two boards needs to be timed around this and it is accounted for on the
    // serial port. Additionally, querying for the capture detect processor board status does not return
    // a \r (CR) character so we unfortunately assume a 1.x revision being reported by the board. This
    // is brittle but until .x is > 9 we will have a valid log of the DDM version being booted.

    // Check to see if version information has arrived from all 3 boards.
    if( uart_.canReadUntil( "LATCH" ) && uart_.canReadUntil( "WHISKERS" ) )
    {
        uart_.readUntil( deviceResponse_, sizeof( deviceResponse_ ), "WHISKERS", 8 );
        logger_.syslog( "Latch/Whisker Boards:" + Str( deviceResponse_, 1,  int( uart_.bytesRead() ) ), Syslog::INFO );
        uart_.flush();

        // Latch and whisker boards are initialized so let's check in on the capture detect processor board
        // We bound this check by the latch/whisker boards in case the capture detect processor boards does decide to display its boot banner.
        uart_ << "#C0Q\r";
        Timespan::Milliseconds( 15 ).sleepFor();
        if( uart_.canReadUntil( "Rev" ) )
        {
            uart_.readUntil( deviceResponse_, sizeof( deviceResponse_ ), "Rev", 7 );
            if( debug_ ) logger_.syslog( "Capture board:" + Str( deviceResponse_ ), Syslog::DEBUG );
            // Less than actually expected because sometimes the DDM tends to scrog serial output and we'd like to see what it looks like in the log. Note value is in hex
            if( uart_.bytesRead() >= 14 )
            {
                logger_.syslog( "Dynamic Docking Module:" + Str( deviceResponse_, 1,  int( uart_.bytesRead() ) ), Syslog::INFO );
                uart_.flush();

                // After confirming the boot banner, turn on the cable detect circuit
                if( enableIREmitter() )
                {
                    // Finally, get an initial status from all 3 boards
                    logLatchStatus();
                    logWhiskerStatus();
                    logCableStatus();

                    startTime_ = Timestamp::Now();
                    publishData(); // write the initial values
                    return RUNNABLE;
                }
                else
                {
                    logger_.syslog( "failed to activate IR emitter.", Syslog::FAULT );
                    this->setFailure( FailureMode::HARDWARE );
                    return STOP;
                }
            }
        }
    }
    else if( startTime_.elapsed() > poTimeout_ )
    {
        char msg[uart_.dataAvailable()];
        uart_.read( msg, sizeof( msg ) );
        logger_.syslog( "failed to initialize; deviceResponse_ loaded:" + Str( msg ) + "\r\nAvailable: " + Str( sizeof( msg ) ), Syslog::FAULT );
        this->setFailure( FailureMode::COMMUNICATIONS );
        return STOP;
    }
    else if( startTime_.elapsed() > ( poTimeout_ - Timespan( 10 ) ) && !initRequested_ )
    {
        uart_.flush(); // Occasionally a boot banner will show up from the capture detect processor board. We want to get rid of that if it does and start fresh.
        uart_ << "#V1\r"; // request version from latch board
        Timespan::Milliseconds( 15 ).sleepFor();
        uart_ << "#V2\r"; // request version from whisker board
        initRequested_ = true; // only need to do this once so flip the flag
    }
    return STARTING;
}


// Pause
Component::RunState DDM::pause()
{
    if( debug_ ) logger_.syslog( "Pause", Syslog::INFO );

    if( simulateHardware() )
    {
        // Skip pausing and go straight to PAUSED
        return PAUSED;
    }

    if( whiskerState_ != 2 || latchState_ != 1 )
    {
        if( verbosityCfgSetting_ > 0 ) logger_.syslog( "Pause: Retracting whiskers", Syslog::INFO );
        uart_ << "#M2,R,60," << Str( currentLimitCfgSetting_ ) << "\r"; // Retract the whiskers
        uart_ << "#M1,X,60," << Str( currentLimitCfgSetting_ ) << "\r"; // Close the latch
        startTime_ = Timestamp::Now();
        return PAUSING;
    }
    else
    {
        if( disableIREmitter() ) // Save some energy here by turning off the IR emitter
        {
            return PAUSED;
        }
        else
        {
            logger_.syslog( "failed to deactivate IR emitter.", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }
    }
    return PAUSED; // Skip pausing and go straight to PAUSED
}


// Pausing
Component::RunState DDM::pausing()
{
    if( debug_ ) logger_.syslog( "Pausing", Syslog::INFO );
    if( whiskerState_ != 2 ) // If whiskers are not retracted yet or not moving, retract them
    {
        // If it's not already moving, command it to move
        if( whiskerState_ != 0 )
        {
            if( verbosityCfgSetting_ > 0 ) logger_.syslog( "Pausing: Retracting Whiskers", Syslog::INFO );
            uart_ << "#M2,R,60," << Str( currentLimitCfgSetting_ ) << "\r"; // Retract the whiskers
        }

        if( startTime_.elapsed() > hardwareTimeout_ )
        {
            logger_.syslog( "Failed to retract whisker before timeout in Pausing.", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }
        // Get status for next time around
        logWhiskerStatus();
    }

    if( latchState_ != 1 ) // If latch is not closed or moving closed, close it.
        //if( latchState_ != 2 ) // If latch is not open or moving open, open it. // *** DEBUG
    {
        // If it's not already moving, command it to move
        if( latchState_ != 0 )
        {
            if( verbosityCfgSetting_ > 0 ) logger_.syslog( "Pausing: Closing Latch", Syslog::INFO );
            uart_ << "#M1,X,60," << Str( currentLimitCfgSetting_ ) << "\r"; // Close the latch
        }
        if( startTime_.elapsed() > hardwareTimeout_ )
        {
            //logger_.syslog( "Failed to close latch before timeout in pausing.", Syslog::FAULT );
            logger_.syslog( "Failed to open latch before timeout in pausing.", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }
        // Get status for next time around
        logLatchStatus();
    }
    publishData();

// All put away so be paused
    if( whiskerState_ == 2 && latchState_ == 1 )
        //if( whiskerState_ == 2 && latchState_ == 2 ) // *** DEBUG
    {
        if( disableIREmitter() ) // Save some energy here by turning off the IR emitter
        {
            return PAUSED;
        }
        else
        {
            logger_.syslog( "failed to deactivate IR emitter.", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }
    }
    return PAUSING;
}


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


Component::RunState DDM::resume()
{
    if( debug_ ) logger_.syslog( "Resume", Syslog::INFO );
    readConfig();
    startTime_ = Timestamp::Now();
    return RESUMING;
}


Component::RunState DDM::resuming()
{
    if( debug_ ) logger_.syslog( "Resuming", Syslog::INFO );

    if( simulateHardware() )
    {
        return RUNNABLE;
    }

    if( enableIREmitter() )
    {
        // Get a status from all 3 boards since we're going back to runnable
        logLatchStatus();
        logWhiskerStatus();
        logCableStatus();

        return RUNNABLE;
    }
    else
    {
        logger_.syslog( "failed to activate IR emitter.", Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
        return STOP;
    }
}


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

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

    // See what mode might be requested if it's being written.
    if( DDMModeCmdReader_->isActive() && DDMModeCmdReader_->wasTouchedSinceLastRun( this ) )
    {
        // Do a little conversion to actually read in the enum properly
        DDMModeCmd_ = ( DockIF::DockingState )DDMModeCmdReader_->asInt( Units::ENUM );

        // Here we can do things on the edge between states if desired.
        if( lastDDMMode_ != DDMModeCmd_ )
        {
            if( verbosityCfgSetting_ > 1 ) logger_.syslog( "Changing to mode: " + Str( DDMModeCmd_ ), Syslog::INFO );
            modeChanged_ = true;
            startTime_ = Timestamp::Now(); // since a mode change is going to involve a hardware change, let's start the clock.
        }
    }

    if( simulateHardware() )
    {
        if( getSimulatedMeasurements() )
        {
            publishData();
            lastDDMMode_ = DDMModeCmd_; // reset the last known to current
            this->resetFailCount();
        }
        else
        {
            logger_.syslog( "Could not read simulated DDM measurements from SimSlate.", Syslog::ERROR );
        }
    }
    else
    {
        logVoltageAndCurrent();

        switch( DDMModeCmd_ )
        {
        case DockIF::STANDBY:
            if( debug_ ) logger_.syslog( "Standby mode.", Syslog::INFO );
            if( whiskerState_ != 2 ) // If whiskers are not retracted yet or not moving, retract them
            {
                if( latchState_ == 0 || latchState_ == 2 ) // Due to an undocumented mechanical requirement we need to wait for the latch to close prior to retracting the whiskers.
                {
                    if( verbosityCfgSetting_ > 0 ) logger_.syslog( "Waiting for latch before retracting whiskers", Syslog::INFO );
                }
                else
                {
                    // If it's not already moving, or needs to be commanded to a new mode, send command
                    if( whiskerState_ != 0 || modeChanged_ )
                    {
                        if( verbosityCfgSetting_ > 0 ) logger_.syslog( "Retracting Whiskers", Syslog::INFO );
                        uart_ << "#M2,R,60," << Str( currentLimitCfgSetting_ ) << "\r"; // Retract the whiskers
                    }

                    if( startTime_.elapsed() > hardwareTimeout_ )
                    {
                        logger_.syslog( "Failed to retract whiskers before timeout.", Syslog::FAULT );
                        this->setFailure( FailureMode::HARDWARE );
                        return STOP;
                    }
                    // Get status for next time around
                    logWhiskerStatus();
                }
            }

            if( latchState_ != 1 ) // If latch is not closed or moving closed, close it.
            {
                // If it's not already moving, or needs to be commanded to a new mode, send command
                if( latchState_ != 0 || modeChanged_ )
                {
                    if( verbosityCfgSetting_ > 0 ) logger_.syslog( "Closing Latch", Syslog::INFO );
                    uart_ << "#M1,X,60," << Str( currentLimitCfgSetting_ ) << "\r"; // Close the latch
                }
                if( startTime_.elapsed() > hardwareTimeout_ )
                {
                    logger_.syslog( "Failed to close latch before timeout.", Syslog::FAULT );
                    this->setFailure( FailureMode::HARDWARE );
                    return STOP;
                }
                // Get status for next time around
                logLatchStatus();
            }

            if( whiskerState_ == 2 && latchState_ == 1 )
            {
                // We can officially report that any transition to standby mode has been completed.
                DDMMode_ = DockIF::STANDBY;
                logCableStatus(); // check this in case we're on a dock in stanbdy
            }
            break;

        case DockIF::ARM:
            if( debug_ ) logger_.syslog( "Armed mode.", Syslog::INFO );
            if( whiskerState_ != 1 ) // If whiskers are not extended yet or not moving, extend them
            {
                // If it's not already moving, or needs to be commanded to a new mode, send command
                if( whiskerState_ != 0 || modeChanged_ )
                {
                    if( verbosityCfgSetting_ > 0 ) logger_.syslog( "Extending Whiskers.", Syslog::INFO );
                    uart_ << "#M2,X,60," << Str( currentLimitCfgSetting_ ) << "\r"; // Extend the whiskers
                }
                if( startTime_.elapsed() > hardwareTimeout_ )
                {
                    logger_.syslog( "Failed to extend whisker before timeout.", Syslog::FAULT );
                    this->setFailure( FailureMode::HARDWARE );
                    return STOP;
                }
                // Get status for next time around
                logWhiskerStatus();
            }

            if( latchState_ != 2 ) // If latch is not open or moving, open it.
            {
                // If it's not already moving, or needs to be commanded to a new mode, send command
                if( latchState_ != 0 || modeChanged_ )
                {
                    if( verbosityCfgSetting_ > 0 ) logger_.syslog( "Arming. Opening Latch", Syslog::INFO );
                    uart_ << "#M1,R,60," << Str( currentLimitCfgSetting_ ) << "\r"; // Open the latch
                }
                if( startTime_.elapsed() > hardwareTimeout_ )
                {
                    logger_.syslog( "Failed to open latch before timeout.", Syslog::FAULT );
                    this->setFailure( FailureMode::HARDWARE );
                    return STOP;
                }
                // Get status for next time around
                logLatchStatus();
            }

            // Once fully armed, check for the cable to appear
            if( latchState_ == 2 && whiskerState_ == 1 )
            {
                // We can officially report that any transition to armed mode has been completed.
                DDMMode_ = DockIF::ARM;

                logCableStatus();
                if( cablePresent_ )
                {
                    cablePresentCounts_ += 1; // Keep counting up until we can latch
                    // If we see the cable for long enough, close the latch and go to standby mode.
                    if( cablePresentCounts_ >= CABLE_PRESENT_DEBOUNCE_COUNTS )
                    {
                        uart_ << "#M1,X,60," << Str( currentLimitCfgSetting_ ) << "\r"; // Close the latch
                        DDMModeCmd_ =  DockIF::STANDBY; // Kick over to standby here on our own (yes it's a little unconventional but being expeditious beats a pretty architecture in this case)
                        startTime_ = Timestamp::Now();
                        cablePresentCounts_ = 0; // reset this for next time
                    }
                }
                else
                {
                    cablePresentCounts_ = 0; // One strike and you're out.
                }
            }
            break;

        case DockIF::DETACH:
            if( debug_ ) logger_.syslog( "Detach mode.", Syslog::INFO );

            // Whiskers need to be confirmed retracted because of mechanical latch that will not allow us to detach in the unlikely event that they were extended.
            if( whiskerState_ != 2 ) // If whiskers are not retracted yet or not moving, retract them
            {
                // If it's not already moving, or needs to be commanded to a new mode, send command
                if( whiskerState_ != 0 || modeChanged_ )
                {
                    if( verbosityCfgSetting_ > 0 ) logger_.syslog( "Detaching. Retracting Whiskers. Should already have been retracted.", Syslog::FAULT );
                    uart_ << "#M2,R,60," << Str( currentLimitCfgSetting_ ) << "\r"; // Retract the whiskers
                }

                if( startTime_.elapsed() > hardwareTimeout_ )
                {
                    logger_.syslog( "Failed to retract whisker before timeout during detach.", Syslog::FAULT );
                    this->setFailure( FailureMode::HARDWARE );
                    return STOP;
                }
                // Get status for next time around
                logWhiskerStatus();
            }

            if( latchState_ != 2 ) // If latch is not open or moving, open it.
            {
                // If it's not already moving, or needs to be commanded to a new mode, send command
                if( latchState_ != 0 || modeChanged_ )
                {
                    if( verbosityCfgSetting_ > 0 ) logger_.syslog( "Detaching. Opening Latch", Syslog::INFO );
                    uart_ << "#M1,R,60," << Str( currentLimitCfgSetting_ ) << "\r"; // Open the latch
                }
                if( startTime_.elapsed() > hardwareTimeout_ )
                {
                    logger_.syslog( "Failed to open latch before timeout during detach.", Syslog::FAULT );
                    this->setFailure( FailureMode::HARDWARE );
                    return STOP;
                }
                // Get status for next time around
                logLatchStatus();
            }

            if( whiskerState_ == 2 && latchState_ == 2 )
            {
                // We can officially report that any transition to detach mode has been completed.
                DDMMode_ = DockIF::DETACH;

                // Keep track of the status of the cable for behaviors to monitor
                logCableStatus();
            }
            break;
        case DockIF::SLIDE:
            logger_.syslog( "Slide mode not implemented for DDM. Going to standby.", Syslog::FAULT );
            DDMMode_ = DockIF::STANDBY;
            break;
        default:
            logger_.syslog( "No such mode! Going to standby.", Syslog::FAULT );
            DDMMode_ =  DockIF::STANDBY;
            break;
        }

    }
    lastDDMMode_ = DDMModeCmd_; // reset the last known to current
    modeChanged_ = false; // reset the mode changed flag
    publishData(); // write everything

    if( startTime_.elapsed() > ( hardwareTimeout_ + Timespan( 2 ) ) ) // Wait until any possible motion (plus a little buffer) has completed before we claim all is well.
    {
        this->resetFailCount(); // if we made it this far we're doing okay
    }
    return RUNNABLE;
}


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

bool DDM::isDataRequested()
{
    return whiskerStateWriter_->isDataRequested() || latchStateWriter_->isDataRequested() || DDMModeWriter_->isDataRequested();
}


bool DDM::enableIREmitter()
{
    uart_.flush();
    uart_ << "#C0DI\r"; // Command the IR Emitter on for cable detection board
    Timespan::Milliseconds( 10 ).sleepFor();
    if( uart_.canReadUntil( "CDI" ) ) // Check for expected response
    {
        return true;
    }
    return false;
}


bool DDM::disableIREmitter()
{
    uart_.flush();
    uart_ << "#C0D0\r"; // Command the IR Emitter off for cable detection board
    Timespan::Milliseconds( 10 ).sleepFor();
    if( uart_.canReadUntil( "CD0" ) ) // Check for expected response
    {
        return true;
    }
    return false;
}


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


void DDM::publishData( void )
{
    whiskerStateWriter_->write( Units::COUNT, whiskerState_ );
    latchStateWriter_->write( Units::COUNT, latchState_ );
    cablePresentWriter_->write( Units::BOOL, cablePresent_ );
    cableValueWriter_->write( Units::COUNT, cableStatus_ );
    DDMModeWriter_->write( Units::COUNT, DDMMode_ );
}


void DDM::logLatchStatus( void )
{
    uart_.flush();
    uart_ << "#F1\r"; // Request latch status
    Timespan::Milliseconds( 10 ).sleepFor();
    if( uart_.canReadUntil( ",F" ) ) // Check for expected response
    {
        uart_.read( deviceResponse_, 21 ); // Grab the fixed width response
        if( verbosityCfgSetting_ > 1 ) logger_.syslog( "Latch Status Query:" + Str( deviceResponse_ ), Syslog::INFO );

        int switchStatus;
        if( 1 == sscanf( deviceResponse_, "!F1,%d,%*d,%*c,%*d,%*d,F", &switchStatus ) )
        {
            latchState_ = AuvMath::binaryToDecimal( switchStatus );
            if( verbosityCfgSetting_ > 1 ) logger_.syslog( "Latch State:" + Str( latchState_ ), Syslog::INFO );
        }
        else
        {
            logger_.syslog( "Could not parse latch state", Syslog::ERROR );
        }
    }
}


void DDM::logWhiskerStatus( void )
{
    uart_.flush();
    uart_ << "#F2\r"; // Request latch status
    Timespan::Milliseconds( 10 ).sleepFor();
    if( uart_.canReadUntil( ",F" ) ) // Check for expected response
    {
        uart_.read( deviceResponse_, 21 ); // Grab the fixed width response
        if( verbosityCfgSetting_ > 1 ) logger_.syslog( "Whisker Status Query:" + Str( deviceResponse_ ), Syslog::INFO );

        int switchStatus;
        if( 1 == sscanf( deviceResponse_, "!F2,%d,%*d,%*c,%*d,%*d,F", &switchStatus ) )
        {
            whiskerState_ = AuvMath::binaryToDecimal( switchStatus );
            if( verbosityCfgSetting_ > 1 ) logger_.syslog( "Whisker State:" + Str( whiskerState_ ), Syslog::INFO );
        }
        else
        {
            logger_.syslog( "Could not parse whisker state", Syslog::ERROR );
        }
    }
}


void DDM::logCableStatus( void )
{
    uart_.flush();
    uart_ << "#C0P\r"; // Request latch status
    Timespan::Milliseconds( 10 ).sleepFor();
    if( uart_.canReadUntil( "!CP" ) ) // Check for expected response
    {
        uart_.readUntil( deviceResponse_, sizeof( deviceResponse_ ), "\r", 1 );
        if( verbosityCfgSetting_ > 1 ) logger_.syslog( "Cable Status Query:" + Str( deviceResponse_ ), Syslog::INFO );

        if( 1 == sscanf( deviceResponse_, "!CP%d", &cableStatus_ ) )
        {
            cableStatusFailed_ = false; // Good read of the status noted
            cableState_ = cableStatus_;
            if( cableState_ < 50 )
            {
                cablePresent_ = true;
            }
            else
            {
                cablePresent_ = false;
            }
            if( verbosityCfgSetting_ > 1 ) logger_.syslog( "Cable State:" + Str( cableState_ ), Syslog::INFO );
        }
        else
        {
            if( cableStatusFailed_ )
            {
                logger_.syslog( "Could not parse cable state", Syslog::FAULT );
                this->setFailure( FailureMode::COMMUNICATIONS );
            }
            else
            {
                logger_.syslog( "Could not parse cable state. Trying again.", Syslog::ERROR );
                cableStatusFailed_ = true;
            }
        }
    }
}


bool DDM::getSimulatedMeasurements( )
{
    // More like data stubs...
    // TODO: add cable present writer to SimSlate (could set to true/false based on range < some thresh).
    switch( DDMModeCmd_ )
    {
    case DockIF::STANDBY:
        // Whiskers retracted (2), latch closed (1)
        whiskerState_ = 2;
        latchState_ = 1;
        cablePresent_ = false;
        DDMMode_ = DockIF::STANDBY;
        return true;

    case DockIF::ARM:
        // Whiskers extended (1), latch open (2)
        whiskerState_ = 1;
        latchState_ = 2;
        cablePresent_ = false;
        DDMMode_ = DockIF::ARM;
        return true;

    case DockIF::DETACH:
        // Whiskers retracted (2), latch open (2)
        whiskerState_ = 2;
        latchState_ = 2;
        cablePresent_ = false;
        DDMMode_ = DockIF::DETACH;
        return true;

    default:
        break;
    }

    return false;
}