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

#include "Docked.h"
#include "DockedIF.h"
#include "DockIF.h"

#include "bitModule/CBITIF.h"
#include "controlModule/HorizontalControlIF.h"
#include "controlModule/SpeedControlIF.h"
#include "controlModule/VerticalControlIF.h"
#include "estimationModule/TrackAcousticContactIF.h"
#include "sensorModule/DataOverHttpsIF.h"

#include "data/Slate.h"
#include "data/ConfigReader.h"
#include "data/UniversalDataReader.h"
#include "units/Units.h"

using namespace DockedIF;

Docked::Docked( const Str& prefix, const Module* module )
    : Behavior( prefix + DockedIF::NAME, module, true, true ),
      // See initializeVariables() for member variable default values
      state_( UNINITIALIZED ),
      timerArmed_( false ),
      loggedOffDock_( false ),
      verbose_( true )
{
    logger_.syslog( "Construct." );

    initializeVariables();

    // Config inputs
    dockRangeCfgReader_ = newConfigReader( DOCK_RANGE_CFG );
    detachTimeoutCfgReader_ = newConfigReader( DETACH_TIMEOUT_CFG );
    dockTimeoutCfgReader_ = newConfigReader( DOCK_TIMEOUT_CFG );
    dataTimeoutCfgReader_ = newConfigReader( DATA_TIMEOUT_CFG );
    rangeTimeoutCfgReader_ = newConfigReader( RANGE_TIMEOUT_CFG );
    verboseCfgReader_  = newConfigReader( VERBOSE );

    surfaceThresholdCfgReader_ = newConfigReader( VerticalControlIF::SURFACE_THRESHOLD_CFG );
    stopDepthCfgReader_ = newConfigReader( CBITIF::STOP_DEPTH_CFG );
    buoyancyLoCfgReader_ = newConfigReader( VerticalControlIF::BUOYANCY_LIMIT_LO_CC_CFG );
    buoyNeutralCfgReader_ = newConfigReader( VerticalControlIF::BUOYANCY_NEUTRAL_CFG );
    massPosLimitFwdCfgReader_ = newConfigReader( VerticalControlIF::MASS_POSITION_LIMIT_FWD_CFG );
    massPosLimitAftCfgReader_ = newConfigReader( VerticalControlIF::MASS_POSITION_LIMIT_AFT_CFG );

    // Setting inputs
    slideTimeoutSettingReader_ = newSettingReader( SLIDE_TIMEOUT_SETTING );
    sinkDurationSettingReader_ = newSettingReader( SINK_DURATION_SETTING );
    closeDurationSettingReader_ = newSettingReader( CLOSE_DURATION_SETTING );
    driveDurationSettingReader_ = newSettingReader( DRIVE_DURATION_SETTING );
    slideRetrySettingReader_ = newSettingReader( SLIDE_RETRY_COUNT_SETTING );
    wiggleCountSettingReader_ = newSettingReader( WIGGLE_COUNT_SETTING );
    trySlideSettingReader_ = newSettingReader( TRY_SLIDE_SETTING );
    tryWiggleSettingReader_ = newSettingReader( TRY_WIGGLE_SETTING );
    tryJogSettingReader_ = newSettingReader( TRY_JOG_SETTING );
    jogLengthSettingReader_ = newSettingReader( JOG_LENGTH_SETTING );
    tryWhirlSettingReader_ = newSettingReader( TRY_WHIRL_SETTING );
    whirlSpeedSettingReader_ = newSettingReader( WHIRL_SPEED_SETTING );

    // Slate input measurements
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );
    dischargingReader_ = newUniversalReader( UniversalURI::PLATFORM_BATTERY_DISCHARGING );
    massPositionReader_ = newUniversalReader( UniversalURI::PLATFORM_MASS_POSITION );
    dockRangeReader_ = newDataReader( TrackAcousticContactIF::RANGE_TO_CONTACT );
    dockingStateReader_ = newDataReader( DockIF::DOCKING_STATE );
    cablePresentReader_ = newDataReader( DockIF::DOCK_CABLE_PRESENT );

    // Slate output variables
    dockingStateCmdWriter_ = newDataWriter( DockIF::DOCKING_STATE_CMD );
    buoyancyCmdWriter_ = newDataWriter( VerticalControlIF::BUOYANCY_CMD );
    massPositionCmdWriter_ = newDataWriter( VerticalControlIF::MASS_POSITION_CMD );
    verticalModeWriter_ = newDataWriter( VerticalControlIF::VERTICAL_MODE );
    elevatorCmdWriter_ = newDataWriter( VerticalControlIF::ELEVATOR_ANGLE_CMD );
    horizontalModeWriter_ = newDataWriter( HorizontalControlIF::HORIZONTAL_MODE );
    rudderCmdWriter_ = newDataWriter( HorizontalControlIF::RUDDER_ANGLE_CMD );
    speedCmdWriter_ = newDataWriter( SpeedControlIF::SPEED_CMD );
}

Docked::~Docked()
{}

void Docked::initializeVariables()
{
    logger_.syslog( "Initializing internal variables to default values." );
    // This may seem unnecessary; however, if the behavior is run as part of the
    // default mission, it will remain on the stack indefinitely, meaning the
    // constructor will get called only once at load time. So, we package this init
    // routine and invoke it upon construction and at runtime from initialize().
    state_ = UNINITIALIZED;

    dockRangeSetting_           = 8.0;   // Range limit to concider on dock (meters)
    detachedTimeoutSetting_     = 30.0; // Time duration after which the vehicle is considered detached (seconds).
    dockTimeoutSetting_         = 30.0;  // Time duration after which the vehicle is considered on the dock (seconds)
    stopDepthSetting_           = nanf( "" ); // Vehicle depth limit (meters), defaults to CBITIF::STOP_DEPTH_CFG
    dataTimeoutSetting_         = 600;
    rangeTimeoutCfgSetting_     = Timespan( 10 * 60 );
    slideTimeoutSetting_        = Timespan( 5 ); // Time duration after which the vehicle is considered in slide (seconds)
    sinkDurationSetting_        = Timespan( 10 * 60 );
    closeDurationSetting_       = Timespan( 5 * 60 );
    driveDurationSetting_       = Timespan( 2 * 60 );
    jogLengthSetting_           = Timespan( 3 );
    surfaceThresholdCfgSetting_ = 1;
    buoyLoCfgSetting_           = 100;
    buoyNeutralCfgSetting_      = 200;
    massPosLimitFwdCfgSetting_  = 20;
    massPosLimitAftCfgSetting_  = -20;
    whirlSpeedSetting_          = 1.0;
    slideRetrySetting_          = 3;
    wiggleCountSetting_         = 1;
    trySlideSetting_            = false;
    tryWiggleSetting_           = false;
    tryJogSetting_              = false;
    tryWhirlSetting_            = false;
    dockTime_        = Timestamp::NOT_SET_TIME;
    dockRangeTime_   = Timestamp::NOT_SET_TIME;
    dataStartTime_   = Timestamp::NOT_SET_TIME;
    dataTime_        = Timestamp::NOT_SET_TIME;
    dockingState_    = DockIF::STANDBY;
    depth_           = nanf( "" );
    dockRange_       = nanf( "" );
    massPositionMm_  = nanf( "" );
    cablePresent_    = false;
    timerArmed_      = false;
    verbose_         = true;
    discharging_     = true;
    slideInfo_       = new SlideInfo;
    slideInfo_->state_ = UNINIT;                       // slide substate
    slideInfo_->slideTime_ = Timestamp::NOT_SET_TIME;  // time of start of substate
    slideInfo_->jogTime_ = Timestamp::NOT_SET_TIME;    // time of start of jog in jog substate
    slideInfo_->jogStageSpeed_ = 0;                    // speed of jog substage (forwards, backwards, stopped)
    slideInfo_->slideTries_ = 0;                       // completed slide mode iterations
    slideInfo_->wiggleCount_ = 0;                      // completed wiggles this iteration
    slideInfo_->needInitialCheck_ = true;              // initial power check started?
    slideInfo_->wigglePitchUp_ = true;                 // direction of wiggle
}

/// Initialize function
void Docked::initialize()
{
    logger_.syslog( "Initialize." );

    initializeVariables();

    cablePresentReader_->requestData( true );
    dockingStateReader_->requestData( true );
    dockRangeReader_->requestData( true );
    dischargingReader_->requestData( true );

    // Make sure DDM is in STANDBY mode
    dockingStateCmdWriter_->write( Units::ENUM, DockIF::STANDBY );

    // Read Slate/Config input setting variables, default to constructor values if unspecified

    float detachedTimeout( nanf( "" ) );
    if( detachTimeoutCfgReader_->read( Units::SECOND, detachedTimeout ) && !isnan( detachedTimeout ) )
    {
        detachedTimeoutSetting_ = Timespan( detachedTimeout );
    }

    float dockTimeout( nanf( "" ) );
    if( dockTimeoutCfgReader_->read( Units::SECOND, dockTimeout ) && !isnan( dockTimeout ) )
    {
        dockTimeoutSetting_ = Timespan( dockTimeout );
    }

    float dataTimeout( nanf( "" ) );
    if( dataTimeoutCfgReader_->read( Units::SECOND, dataTimeout ) && !isnan( dataTimeout ) )
    {
        dataTimeoutSetting_ = Timespan( dataTimeout );
    }

    float rangeTimeout( nanf( "" ) );
    if( rangeTimeoutCfgReader_->read( Units::SECOND, rangeTimeout ) && !isnan( rangeTimeout ) )
    {
        rangeTimeoutCfgSetting_ = Timespan( rangeTimeout );
    }

    float slideTimeout( nanf( "" ) );
    if( slideTimeoutSettingReader_->read( Units::SECOND, slideTimeout ) && !isnan( slideTimeout ) )
    {
        slideTimeoutSetting_ = Timespan( slideTimeout );
    }

    float sinkDuration( nanf( "" ) );
    if( sinkDurationSettingReader_->read( Units::SECOND, sinkDuration ) && !isnan( sinkDuration ) )
    {
        sinkDurationSetting_ = Timespan( sinkDuration );
    }

    float closeDuration( nanf( "" ) );
    if( closeDurationSettingReader_->read( Units::SECOND, closeDuration ) && !isnan( closeDuration ) )
    {
        closeDurationSetting_ = Timespan( closeDuration );
    }

    float driveDuration( nanf( "" ) );
    if( driveDurationSettingReader_->read( Units::SECOND, driveDuration ) && !isnan( driveDuration ) )
    {
        driveDurationSetting_ = Timespan( driveDuration );
    }

    float jogLength( nanf( "" ) );
    if( jogLengthSettingReader_->read( Units::SECOND, jogLength ) && !isnan( jogLength ) )
    {
        jogLengthSetting_ = Timespan( jogLength );
    }

    float dockRange( nanf( "" ) );
    if( dockRangeCfgReader_->read( Units::METER, dockRange ) && !isnan( dockRange ) )
    {
        dockRangeSetting_ = fabs( dockRange );
    }

    float stopDepth( nanf( "" ) );
    if( stopDepthCfgReader_->read( Units::METER, stopDepth ) && !isnan( stopDepth ) )
    {
        stopDepthSetting_ = stopDepth;
    }

    float surfaceThresh( nanf( "" ) );
    if( surfaceThresholdCfgReader_->read( Units::METER, surfaceThresh ) && !isnan( surfaceThresh ) )
    {
        surfaceThresholdCfgSetting_ = surfaceThresh;
    }

    float buoyLoCfg( nanf( "" ) );
    if( buoyancyLoCfgReader_->read( Units::CUBIC_CENTIMETER, buoyLoCfg ) && !isnan( buoyLoCfg ) )
    {
        buoyLoCfgSetting_ = buoyLoCfg;
    }

    float buoyNeutralCfg( nanf( "" ) );
    if( buoyNeutralCfgReader_->read( Units::CUBIC_CENTIMETER, buoyNeutralCfg ) && !isnan( buoyNeutralCfg ) )
    {
        buoyNeutralCfgSetting_ = buoyNeutralCfg;
    }

    float massPosLimitFwdCfg( nanf( "" ) );
    if( massPosLimitFwdCfgReader_->read( Units::MILLIMETER, massPosLimitFwdCfg ) && !isnan( massPosLimitFwdCfg ) )
    {
        massPosLimitFwdCfgSetting_ = massPosLimitFwdCfg;
    }

    float masPosLimitAftCfg( nanf( "" ) );
    if( massPosLimitAftCfgReader_->read( Units::MILLIMETER, masPosLimitAftCfg ) && !isnan( masPosLimitAftCfg ) )
    {
        massPosLimitAftCfgSetting_ = masPosLimitAftCfg;
    }

    float whirlSpeed( nanf( "" ) );
    if( whirlSpeedSettingReader_->read( Units::METER_PER_SECOND, whirlSpeed ) && !isnan( whirlSpeed ) )
    {
        whirlSpeedSetting_ = whirlSpeed;
    }

    float slideRetry( nanf( "" ) );
    if( slideRetrySettingReader_->read( Units::COUNT, slideRetry ) && !isnan( slideRetry ) )
    {
        slideRetrySetting_ = slideRetry;
    }

    float wiggleCount( nanf( "" ) );
    if( wiggleCountSettingReader_->read( Units::COUNT, wiggleCount ) && !isnan( wiggleCount ) )
    {
        wiggleCountSetting_ = wiggleCount;
    }

    int trySlide;
    if( trySlideSettingReader_->read( Units::BOOL, trySlide ) && !isnan( trySlide ) )
    {
        trySlideSetting_ = trySlide;
    }

    int tryWiggle;
    if( tryWiggleSettingReader_->read( Units::BOOL, tryWiggle ) && !isnan( tryWiggle ) )
    {
        tryWiggleSetting_ = tryWiggle;
    }

    int tryJog;
    if( tryJogSettingReader_->read( Units::BOOL, tryJog ) && !isnan( tryJog ) )
    {
        tryJogSetting_ = tryJog;
    }

    int tryWhirl;
    if( tryWhirlSettingReader_->read( Units::BOOL, tryWhirl ) && !isnan( tryWhirl ) )
    {
        tryWhirlSetting_ = tryWhirl;
    }

    verboseCfgReader_->read( verbose_ );
    dockRangeTime_ = Timestamp::Now();
    dataTime_ = Timestamp::Now();
    dockTime_ = Timestamp::Now();
    // If we're going to try to slide, go straight into the slide sequence (which starts with a success check)
    trySlideSetting_ ? state_ = SLIDE : state_ = DOCKED;
}

/// Read in the parameters for satisfied or runIfUnsatisfied: return true if OK.
bool Docked::readParams()
{
    bool ok( true );

    switch( state_ )
    {
    case UNINITIALIZED:
        initialize();
    // no break
    case SLIDE:
        // Since discharging is only checked in slide, once it's gone false it'll
        // never be written again (since discharging must be false to enter slide)
        // This prevents us from re-sliding once we've docked.
        if( dischargingReader_->isActive() && dischargingReader_->wasTouchedSinceLastRun( this ) )
        {
            dischargingReader_->read( discharging_ );
        }
        if( massPositionReader_->isActive() && massPositionReader_->wasTouchedSinceLastRun( this ) )
        {
            massPositionReader_->read( Units::MILLIMETER, massPositionMm_ );
        }
    // no break
    case DOCKED:
    case DETACHED:

        // Get the range to the dock transponder
        if( dockRangeReader_->isActive() && dockRangeReader_->wasTouchedSinceLastRun( this ) )
        {
            if( dockRangeReader_->read( Units::METER, dockRange_ ) && !isnan( dockRange_ ) )
            {
                dockRangeTime_ = dockRangeReader_->getTimestamp();
            }
        }

        // Read state of DDM IR emitter, latch state, and mode
        if( cablePresentReader_->isActive() && cablePresentReader_->wasTouchedSinceLastRun( this ) )
        {
            int dockingState( 0 );
            if( dockingStateReader_->read( Units::ENUM, dockingState ) ) dockingState_ = ( DockIF::DockingState )dockingState;

            if( cablePresentReader_->read( cablePresent_ ) )
            {
                dataTime_ = cablePresentReader_->getTimestamp();

                // Mark the time of the first data update in the current duty cycle
                if( dataStartTime_ == Timestamp::NOT_SET_TIME )
                {
                    dataStartTime_ = dataTime_;
                }
            }
        }

        depth_ = nanf( "" );
        depthReader_->read( Units::METER, depth_ );
        break;

    default:
        state_ = UNINITIALIZED;
        ok = false;
        break;
    }
    return ok;
}

/// Perform the satisfied: return true if envelope "satisfied"
bool Docked::calcSatisfied( const DockState newState )
{
    bool satisfied( false );

    // No data
    // This should be a redundant check, if this is true onDock should return false and we should transition
    // to detached. But putting it here too guarantees that we don't bother waiting the detachedTimeout if
    // we're just not getting data
    if( dataTime_.elapsed() > dataTimeoutSetting_ && dockRangeTime_.elapsed() > rangeTimeoutCfgSetting_ )
    {
        // Satisfy
        logger_.syslog( "DATA AND RANGE TIMEOUT: VEHICLE DETACHED FROM DOCK.", Syslog::IMPORTANT );
        return true;
    }

    // No change in state
    if( state_ == newState )
    {
        // Keep satisfying if the state is DETACHED to allow subsequent behaviors to kick in...
        satisfied = ( state_ == DETACHED );
        if( timerArmed_ ) resetTimer();
    }
    // State has changed
    else
    {
        switch( newState )
        {
        case DOCKED:
            // Transition from SLIDE or DETACHED to DOCKED
            if( timerArmed_ && dockTime_.elapsed() > dockTimeoutSetting_ )
            {
                // If this is the transition to docked, and we have power, we might be at the right
                // position on the rod. We probably got here via slide mode. Report the depth for
                // future reference.
                logger_.syslog( "VEHICLE DOCKED at depth of " + Str( depth_ ) + " with"
                                + Str( discharging_ ? "out" : "" ) + " power", Syslog::IMPORTANT );
                state_ = newState;
                resetTimer();
                satisfied = false;
            }
            break;
        case SLIDE:
            // Transition from DOCKED to SLIDE
            if( timerArmed_ && dockTime_.elapsed() > slideTimeoutSetting_ )
            {
                logger_.syslog( "VEHICLE IN SLIDE.", Syslog::IMPORTANT );
                state_ = newState;
                resetTimer();
                satisfied = false;
            }
            break;
        case DETACHED:
            // Transition from DOCKED or SLIDE to DETACHED
            if( timerArmed_ && dockTime_.elapsed() > detachedTimeoutSetting_ )
            {
                logger_.syslog( "VEHICLE DETACHED FROM DOCK.", Syslog::IMPORTANT );
                state_ = newState;
                resetTimer();
                satisfied = true;
            }
            break;

        case UNINITIALIZED:
        default:
            break;
        }
    }

    return satisfied;
}

/// Just do the run
void Docked::run()
{
    runIfUnsatisfied();
}

/// Just do the satisfied: return true if envelope "satisfied"
bool Docked::isSatisfied()
{
    // Vehicle is detached from the dock
    return runIfUnsatisfied();
}

/// Do the run, and return true if envelope "satisfied"
bool Docked::runIfUnsatisfied()
{
    bool satisfied( false );

    DockState newDockState = DOCKED;
    if( readParams() )
    {
        newDockState = getState();

        if( state_ != newDockState && !timerArmed_ )
        {
            // Start the dock timer
            if( verbose_ ) logger_.syslog( "State switched to " +
                                               Str( newDockState == DOCKED ? "DOCKED." : ( newDockState == DETACHED ? "DETACHED" : "SLIDE" ) )
                                               + ". Waiting for state timeout to act...",
                                               Syslog::IMPORTANT );
            dockTime_ = Timestamp::Now();
            timerArmed_ = true;
        }

        satisfied = calcSatisfied( newDockState );
    }

    // Control actions occur here
    switch( state_ )
    {
    case DOCKED:
        // If we docked successfully, we don't actually want to control on anything at all!
        // Just hang out with the arm closed.
        dockingStateCmdWriter_->write( Units::ENUM, DockIF::STANDBY );
        verticalModeWriter_->write( Units::ENUM, VerticalControlIF::NONE );
        if( trySlideSetting_ )
        {
            buoyancyCmdWriter_->write( Units::CUBIC_CENTIMETER, buoyLoCfgSetting_ );
        }
        else
        {
            buoyancyCmdWriter_->write( Units::CUBIC_CENTIMETER, buoyNeutralCfgSetting_ );
        }
        break;
    case SLIDE:
    {
        SlideState newSlideState = getSlideState();
        if( slideInfo_->state_ != newSlideState )
        {
            if( verbose_ ) logger_.syslog( "Slide state transition from " + Str( slideInfo_->state_ ) +
                                               " to " + Str( newSlideState ), Syslog::IMPORTANT );
            slideInfo_->slideTime_ = Timestamp::Now();
        }
        switch( newSlideState )
        {
        // Close the arm to check for power/comms
        case CHECK:
            dockingStateCmdWriter_->write( Units::ENUM, DockIF::STANDBY );
            verticalModeWriter_->write( Units::ENUM, VerticalControlIF::NONE );
            // Stay neutral for the initial check in case something makes us come off the
            // dock early, remain pumped down otherwise
            if( slideInfo_->needInitialCheck_ )
            {
                buoyancyCmdWriter_->write( Units::CUBIC_CENTIMETER, buoyNeutralCfgSetting_ );
            }
            else
            {
                buoyancyCmdWriter_->write( Units::CUBIC_CENTIMETER, buoyLoCfgSetting_ );
            }
            speedCmdWriter_->write( Units::METER_PER_SECOND, 0 );
            break;
        // Pump down to passively slide down rod
        case SINK:
            dockingStateCmdWriter_->write( Units::ENUM, DockIF::SLIDE );
            verticalModeWriter_->write( Units::ENUM, VerticalControlIF::NONE );
            buoyancyCmdWriter_->write( Units::CUBIC_CENTIMETER, buoyLoCfgSetting_ );
            break;
        // Move mass forward and aft to slide down rod
        case WIGGLE:
            dockingStateCmdWriter_->write( Units::ENUM, DockIF::SLIDE );
            verticalModeWriter_->write( Units::ENUM, VerticalControlIF::MASS_AND_ELEVATOR );
            buoyancyCmdWriter_->write( Units::CUBIC_CENTIMETER, buoyLoCfgSetting_ );
            if( slideInfo_->wigglePitchUp_ )
            {
                massPositionCmdWriter_->write( Units::MILLIMETER, massPosLimitAftCfgSetting_ );
            }
            else
            {
                massPositionCmdWriter_->write( Units::MILLIMETER, massPosLimitFwdCfgSetting_ );
            }
            break;
        // Repeatedly bump prop forward and reverse to work down rod
        case JOG:
            dockingStateCmdWriter_->write( Units::ENUM, DockIF::SLIDE );
            verticalModeWriter_->write( Units::ENUM, VerticalControlIF::NONE );
            buoyancyCmdWriter_->write( Units::CUBIC_CENTIMETER, buoyLoCfgSetting_ );
            speedCmdWriter_->write( Units::METER_PER_SECOND, slideInfo_->jogStageSpeed_ );
            break;
        // Elevators to dive, rudder to turn, drive forward to spiral down rod
        case WHIRL:
            dockingStateCmdWriter_->write( Units::ENUM, DockIF::SLIDE );
            verticalModeWriter_->write( Units::ENUM, VerticalControlIF::MASS_AND_ELEVATOR );
            buoyancyCmdWriter_->write( Units::CUBIC_CENTIMETER, buoyLoCfgSetting_ );
            elevatorCmdWriter_->write( Units::DEGREE, WHIRL_ELEVATOR_ANGLE_DEG );
            horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::RUDDER_ANGLE );
            rudderCmdWriter_->write( Units::DEGREE, WHIRL_RUDDER_ANGLE_DEG );
            speedCmdWriter_->write( Units::METER_PER_SECOND, whirlSpeedSetting_ );
            break;
        case UNDEFINED:
            logger_.syslog( "Undefined slide state! Something went wrong.", Syslog::FAULT );
        default:
            break;
        }
        slideInfo_->state_ = newSlideState;
        break;
    }
    case DETACHED:
        dockingStateCmdWriter_->write( Units::ENUM, DockIF::ARM );
        break;
    case UNINITIALIZED:
    default:
        dockingStateCmdWriter_->write( Units::ENUM, DockIF::STANDBY );
        break;
    }
    return satisfied;
}

/// Returns true if sensors indicate the vehicle is on the dock
bool Docked::isOnDock()
{
    bool onDock( false );
    Str offReason = "";

    // Monitor the docking state and IR emitter
    if( dataTime_.elapsed() < dataTimeoutSetting_ )
    {
        onDock |= ( ( dockingState_ == DockIF::STANDBY || dockingState_ == DockIF::SLIDE ) && cablePresent_ );
        if( !onDock )
        {
            offReason = Str( cablePresent_ ? "cable present" : "cable absent" ) + " and arm state "
                        + Str( dockingState_ );
        }

    }

    // Monitor range from dock
    if( depth_ > surfaceThresholdCfgSetting_ || isnan( depth_ ) )
    {
        if( dockRangeTime_.elapsed() < rangeTimeoutCfgSetting_ && !isnan( dockRange_ ) )
        {
            onDock |= ( dockRange_ <= dockRangeSetting_ );
            if( !onDock )
            {
                offReason = "range to dock " + Str( dockRange_ ) + " exceeded threshold.";
            }

        }
        else
        {
            onDock = false;
            offReason = "range timeout exceeded";
        }
        // TODO: add check DR range from dock position if AC ranges not available
    }
    else
    {
        if( !onDock ) offReason =  "depth " + Str( depth_ ) + " above surface threshold";
    }

    if( !onDock )
    {
        if( !loggedOffDock_ )
        {
            logger_.syslog( "Off dock, " + offReason,  Syslog::FAULT );
            loggedOffDock_ = true;
        }
    }
    else
    {
        loggedOffDock_ = false;
    }
    return onDock;
}

// Main docking state determination/decision-making
// Control based on this state happens in the run
DockState Docked::getState()
{
    DockState newState = DETACHED;

    // Monitor vehicle depth
    if( depth_ > stopDepthSetting_ && !isnan( stopDepthSetting_ ) )
    {
        logger_.syslog( "Depth " + Str( depth_ ) + " exceeded stop depth, detaching from dock", Syslog::FAULT );
        return DETACHED;
    }

    bool onDock = isOnDock();
    // If not using slide, being on the dock is enough to consider ourselves docked
    // Otherwise, we're only docked if we have power.
    // If that hasn't been achieved, we should slide until we succeed or give up.
    // Note that a transition from SLIDE to DOCKED here can only be achieved in the
    // CHECK state of slide, because we won't have power with the arm open,
    // even if we're in the right place.
    if( onDock && ( !trySlideSetting_ || !discharging_ ) )
    {
        newState = DOCKED;
    }
    else if( onDock )
    {
        if( slideInfo_->slideTries_ < slideRetrySetting_ )
        {

            newState = SLIDE;
        }
        else
        {
            newState = DETACHED;
        }
    }
    return newState;
}

// Slide state determination/decision-making
// Doesn't handle transitions into/out of slide mode, that's the main docking state machine
// And doesn't handle slide mode control, that's the run
// But does modify vars that impact the main state machine's decisions and the slide state's
// control.
Docked::SlideState Docked::getSlideState()
{
    SlideState newState = CHECK;
    switch( slideInfo_->state_ )
    {
    case SINK:
        if( slideInfo_->slideTime_.elapsed() > sinkDurationSetting_ )
        {
            if( verbose_ )logger_.syslog( "Done sinking", Syslog::IMPORTANT );
            // Take the least aggressive next requested action
            if( tryWiggleSetting_ )
            {
                newState = WIGGLE;
            }
            else if( tryJogSetting_ )
            {
                newState = JOG;
            }
            else if( tryWhirlSetting_ )
            {
                newState = WHIRL;
            }
            else
            {
                newState = CHECK;
            }
        }
        else
        {
            newState = SINK;
        }
        break;
    case WIGGLE:
        if( slideInfo_->wiggleCount_ > wiggleCountSetting_ )
        {
            if( verbose_ )logger_.syslog( "Done wiggling", Syslog::IMPORTANT );
            slideInfo_->wiggleCount_ = 0;
            newState = CHECK;
        }
        else if( slideInfo_->wigglePitchUp_ )
        {
            if( massPositionMm_ <= ( WIGGLE_THROW_FACTOR * massPosLimitAftCfgSetting_ ) )
            {
                if( verbose_ )logger_.syslog( "Halfway through wiggle, pitching down next", Syslog::IMPORTANT );
                slideInfo_->wigglePitchUp_ = false; // Pitch down now
            }
            newState = WIGGLE;
        }
        else
        {
            if( massPositionMm_ >= ( WIGGLE_THROW_FACTOR * massPosLimitFwdCfgSetting_ ) )
            {
                if( verbose_ )logger_.syslog( "Finished wiggle, pitching up next", Syslog::IMPORTANT );
                slideInfo_->wiggleCount_++;
                slideInfo_->wigglePitchUp_ = true;
            }
            newState = WIGGLE;
        }
        break;
    case JOG:
        if( slideInfo_->slideTime_.elapsed() > driveDurationSetting_ )
        {
            if( verbose_ ) logger_.syslog( "Done jogging", Syslog::IMPORTANT );
            slideInfo_->jogTime_ = Timestamp::NOT_SET_TIME;
            slideInfo_->jogStageSpeed_ = 0;
            newState = CHECK;
        }
        else
        {
            // Time to change jog direction
            if( slideInfo_->jogTime_.elapsed() > jogLengthSetting_ || slideInfo_->jogTime_ == Timestamp::NOT_SET_TIME )
            {
                slideInfo_->jogTime_ = Timestamp::Now();
                // First run forward...
                if( slideInfo_->jogStageSpeed_ == 0 )
                {
                    slideInfo_->jogStageSpeed_ = JOG_SPEED_M_PER_S;
                }
                // Then back...
                else if( slideInfo_->jogStageSpeed_ == JOG_SPEED_M_PER_S )
                {
                    slideInfo_->jogStageSpeed_ = -1 * JOG_SPEED_M_PER_S;
                }
                // Then pause.
                else if( slideInfo_->jogStageSpeed_ == -1 * JOG_SPEED_M_PER_S )
                {
                    slideInfo_->jogStageSpeed_ = 0;
                }
                else
                {
                    logger_.syslog( "Unexpected jog speed! " + Str( slideInfo_->jogStageSpeed_ ), Syslog::FAULT );
                    slideInfo_->jogStageSpeed_ = 0;
                }
            }
            newState = JOG;
        }
        break;
    case WHIRL:
        if( slideInfo_->slideTime_.elapsed() > driveDurationSetting_ )
        {
            if( verbose_ ) logger_.syslog( "Done whirling", Syslog::IMPORTANT );
            newState = CHECK;
        }
        else
        {
            newState = WHIRL;
        }
        break;
    case CHECK:
        if( slideInfo_->slideTime_.elapsed() > closeDurationSetting_ )
        {
            // We don't want to open the arm while there's still current flowing. Wait for a human to turn off the power.
            if( !discharging_ )
            {
                logger_.syslog( "Slide check interval over, but still charging. Extending check until no longer charging.", Syslog::IMPORTANT );
                slideInfo_->slideTime_ = Timestamp::Now();
                newState = CHECK;
            }
            else
            {
                if( verbose_ )logger_.syslog( "Done checking for power, continuing with slide", Syslog::IMPORTANT );
                newState = SINK;
            }
            // First iteration of slide should both start and end with a check
            if( slideInfo_->needInitialCheck_ )
            {
                slideInfo_->needInitialCheck_ = false;
            }
            else
            {
                slideInfo_->slideTries_++;
                if( slideInfo_->slideTries_ >= slideRetrySetting_ )
                {
                    logger_.syslog( "Reached max slide retries without successful charge, docking attempt will be aborted", Syslog::FAULT );
                }
            }
        }
        else
        {
            newState = CHECK;
        }
        break;
    case UNDEFINED:
        logger_.syslog( "Undefined docking state! Something went wrong.", Syslog::FAULT );
    default:
        break;
    }
    return newState;
}

void Docked::resetTimer()
{
    dockTime_ = Timestamp::NOT_SET_TIME;
    timerArmed_ = false;
}

/// Uninit function
void Docked::uninitialize()
{
    cablePresentReader_->requestData( false );
    dockingStateReader_->requestData( false );
    dockRangeReader_->requestData( false );
    dischargingReader_->requestData( false );

    state_ = UNINITIALIZED;
}

/// Mission Component factory interface
Behavior* Docked::CreateBehavior( const Str& prefix, const Module* module )
{
    return new Docked( prefix, module );
}