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

#include "Undock.h"
#include "UndockIF.h"

#include "controlModule/SpeedControlIF.h"
#include "controlModule/VerticalControlIF.h"
#include "estimationModule/TrackAcousticContactIF.h"
#include "servoModule/DockingServoIF.h"
#include "servoModule/DockingStepperIF.h"
#include "sensorModule/DDMIF.h"
#include "sensorModule/PowerOnlyIF.h"
#include "sensorModule/NanoDVRIF.h"

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

#include <math.h>


Undock::Undock( const Str& prefix, const Module* module )
    : Behavior( prefix + UndockIF::NAME, module, true, true ),
      // See initializeVariables() for member variable default values
      undockMode_( UNINITIALIZED ),
      dockingModuleLoaded_( false ),
      cameraLoaded_( false ),
      powerOnlyLoaded_( false ),
      timerArmed_( false ),
      verbose_( false )
{
    logger_.syslog( "Construct." );

    initializeVariables();

    // Slate input setting variables
    undockDepthCfgReader_ = newConfigReader( UndockIF::UNDOCK_DEPTH_CFG );
    undockTimeoutCfgReader_ = newConfigReader( UndockIF::UNDOCK_TIMEOUT_CFG );
    undockRangeCfgReader_ = newConfigReader( UndockIF::UNDOCK_RANGE_CFG );
    reverseThrustTimeoutCfgReader_ = newConfigReader( UndockIF::REVERSE_THRUST_TIMEOUT_CFG );
    verboseCfgReader_  = newConfigReader( UndockIF::VERBOSE );

    surfaceThresholdCfgReader_ = newConfigReader( VerticalControlIF::SURFACE_THRESHOLD_CFG );
    massDefaultCfgReader_ = newConfigReader( VerticalControlIF::MASS_DEFAULT_CFG );

    // Slate input measurements
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );
    dockRangeReader_ = newDataReader( TrackAcousticContactIF::RANGE_TO_CONTACT );
    samplePowerOnlyReader_ = newDataReader( PowerOnlyIF::SAMPLE_POWERONLY );
    sampleNanoDvrReader_  = newDataReader( NanoDVRIF::SAMPLE_NANO_DVR );
    dockingStateReader_ = newDataReader( DockIF::DOCKING_STATE );
    cablePresentReader_ = newDataReader( DockIF::DOCK_CABLE_PRESENT );

    // Slate output variables
    dockingStateCmdWriter_ = newDataWriter( DockIF::DOCKING_STATE_CMD );
    depthCmdWriter_ = newDataWriter( VerticalControlIF::DEPTH_CMD );
    elevatorAngleCmdWriter_ = newDataWriter( VerticalControlIF::ELEVATOR_ANGLE_CMD );
    massPositionCmdWriter_ = newDataWriter( VerticalControlIF::MASS_POSITION_CMD );
    speedCmdWriter_ = newDataWriter( SpeedControlIF::SPEED_CMD );
    verticalModeWriter_ = newDataWriter( VerticalControlIF::VERTICAL_MODE );

    // Check DDM/DockingServo and camera status
    bool ddmLoaded( false ), dockingServoLoaded( false ), dockingStepperLoaded( false );
    Slate::ReadOnce( DDMIF::LOAD_AT_STARTUP, Units::BOOL, ddmLoaded, logger_ );
    Slate::ReadOnce( DockingServoIF::LOAD_AT_STARTUP, Units::BOOL, dockingServoLoaded, logger_ );
    Slate::ReadOnce( DockingStepperIF::LOAD_AT_STARTUP, Units::BOOL, dockingStepperLoaded, logger_ );
    dockingModuleLoaded_ = ( ddmLoaded || dockingServoLoaded || dockingStepperLoaded );

    // Check PowerOnly/camera status
    Slate::ReadOnce( PowerOnlyIF::LOAD_AT_STARTUP, Units::BOOL, powerOnlyLoaded_, logger_ );
    Slate::ReadOnce( NanoDVRIF::LOAD_AT_STARTUP, Units::BOOL, cameraLoaded_, logger_ );
}

Undock::~Undock()
{}

/// Init internal member variables to default values
void Undock::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().
    undockMode_ = UNINITIALIZED;

    surfaceThresholdCfgSetting_  = 1.0;
    massDefaultCfgSetting_       = nanf( "" );
    undockDepthSetting_          = 5.5;  // Undocking depth (meter).
    undockRangeSetting_          = 8.0;  // Objective range for undocking in meters
    undockTimeoutSetting_        = 20.0; // Time duration after which the vehicle is considered detached from the dock (seconds)
    reverseThrustTimeoutSetting_ = 20.0; // Max time duration to engage the thruster in reverse to break off the dock (sec)
    speedSetting_                = -1.0; // Speed to use for disengaging from the dock (m/s)
    validRangeTimeout_           = 120.0;

    undockTime_        = Timestamp::NOT_SET_TIME;
    reverseThrustTime_ = Timestamp::NOT_SET_TIME;
    dockRange_         = nanf( "" );
    dockingState_      = DockIF::DETACH;
    cablePresent_      = false;
    timerArmed_        = false;
    verbose_           = false;

}

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

    initializeVariables();

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

    // Read Slate input setting variables, default to constructor values if unspecified
    float undockDepth( nanf( "" ) );
    if( undockDepthCfgReader_->read( Units::METER, undockDepth ) && !isnan( undockDepth ) )
    {
        undockDepthSetting_ = fabs( undockDepth );
    }

    float undockRange( nanf( "" ) );
    if( undockRangeCfgReader_->read( Units::METER, undockRange ) && !isnan( undockRange ) )
    {
        undockRangeSetting_ = fabs( undockRange );
    }

    float undockTimeout( nanf( "" ) );
    if( undockTimeoutCfgReader_->read( Units::SECOND, undockTimeout ) && !isnan( undockTimeout ) )
    {
        undockTimeoutSetting_ = Timespan( undockTimeout );
        validRangeTimeout_ = Timespan( undockTimeout * 4.0 );
    }

    float reverseThrustTimeout( nanf( "" ) );
    if( reverseThrustTimeoutCfgReader_->read( Units::SECOND, reverseThrustTimeout ) && !isnan( reverseThrustTimeout ) )
    {
        reverseThrustTimeoutSetting_ = fabs( reverseThrustTimeout );
    }

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

    massDefaultCfgReader_->read( Units::METER, massDefaultCfgSetting_ );
    verboseCfgReader_->read( verbose_ );
    undockMode_ = DETACH;
}

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

    switch( undockMode_ )
    {
    case UNINITIALIZED:
        initialize();
    // no break

    case DETACH:
    case REVERSE:
    case WAIT:
    case DETACHED:
    case UNDOCKED:

        if( dockingModuleLoaded_ )
        {
            // Read state of cable present sensor and docking module mode
            cablePresentReader_->read( cablePresent_ );

            int dockingState;
            if( dockingStateReader_->read( Units::ENUM, dockingState ) )
            {
                dockingState_ = ( DockIF::DockingState )dockingState;
            }
        }
        break;

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

/// Perform the satisfied: return true if envelope "satisfied"
bool Undock::calcSatisfied()
{
    bool satisfied( false );

    if( undockMode_ == UNDOCKED )
    {
        logger_.syslog( "Undocking sequence complete.", Syslog::IMPORTANT );
        satisfied = true;
    }

    return satisfied;
}

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

/// Just do the satisfied: return true if envelope "satisfied"
bool Undock::isSatisfied()
{
    bool satisfied( false );
    if( readParams() )
    {
        // Check if the vehicle is on the dock (range within dock range and IR sensor high).
        if( !rangeUndocked( false ) && cablePresent_ )
        {
            if( !timerArmed_ || undockTime_ == Timestamp::NOT_SET_TIME )
            {
                logger_.syslog( "Detected possible attachment. Starting timer.", Syslog::INFO );
                undockTime_ = Timestamp::Now();
                timerArmed_ = true;
            }
            else if( undockTime_.elapsed() > undockTimeoutSetting_ )
            {
                logger_.syslog( "Attached to the dock at range: " + Str( dockRange_, 2 ) + " m. Initiating undocking sequence.", verbose_ ? Syslog::IMPORTANT : Syslog::INFO );
                undockMode_ = DETACH;
            }
        }
        else if( timerArmed_ )
        {
            // Reset the clock
            undockTime_ = Timestamp::NOT_SET_TIME;
            timerArmed_ = false;
        }

        satisfied = calcSatisfied();
    }
    return satisfied;
}

/// Do the run, and return true if envelope "satisfied"
bool Undock::runIfUnsatisfied()
{
    bool satisfied( false );
    if( readParams() )
    {
        satisfied = calcSatisfied();
        if( !satisfied )
        {
            switch( undockMode_ )
            {
            case DETACH:
            {
                bool detachRequsted( true );
                // Power the camera, and open
                detachRequsted &= powerCameraAndLights();
                detachRequsted &= cmdDockState( DockIF::DETACH );

                if( detachRequsted )
                {
                    logger_.syslog( "Detaching from dock.", verbose_ ? Syslog::IMPORTANT : Syslog::INFO );
                    reverseThrustTime_ = Timestamp::NOT_SET_TIME;
                    undockTime_ = Timestamp::NOT_SET_TIME;
                    timerArmed_ = false;

                    // TODO: for now always do the reverse maneuver unless we're on the surface
                    //undockMode_ = ddmCablePresent_ ? REVERSE : WAIT;
                    float depth( nanf( "" ) );
                    depthReader_->read( Units::METER, depth );
                    undockMode_ = ( depth > surfaceThresholdCfgSetting_ ) ? REVERSE : WAIT;
                }
            }
            break;

            case REVERSE:

                if( dockingState_ != DockIF::DETACH )
                {
                    // No longer in DETACH mode (possibly due to a failure/device power cycle)
                    // Go back to the DETACH state to re-issue the mode request
                    undockMode_ = DETACH;
                }

                if( reverseThrustTime_ == Timestamp::NOT_SET_TIME )
                {
                    logger_.syslog( "Engaging thruster for " + Str( reverseThrustTimeoutSetting_.asFloat(), 1 ) + " seconds (max) to disengage.", Syslog::INFO );
                    reverseThrustTime_ = Timestamp::Now();
                }
                // Engage the thruster while the vehicle is within the dock range and timer hasn't expired
                else if( !rangeUndocked( false ) && reverseThrustTime_.elapsed() < reverseThrustTimeoutSetting_ )
                {
                    // Engage the thruster, hold mass and elevator at default positions
                    speedCmdWriter_->write( Units::METER_PER_SECOND, speedSetting_ );
                    massPositionCmdWriter_->write( Units::METER, massDefaultCfgSetting_ );
                    elevatorAngleCmdWriter_->write( Units::RADIAN, 0.0f );
                }
                else
                {
                    speedCmdWriter_->write( Units::METER_PER_SECOND, 0.0 );
                    logger_.syslog( "Thruster off.", Syslog::INFO );
                    reverseThrustTime_ = Timestamp::NOT_SET_TIME;
                    undockMode_ = WAIT;
                }
                break;

            case WAIT:

                if( dockingState_ != DockIF::DETACH )
                {
                    // No longer in DETACH mode (possibly due to a failure/device power cycle)
                    // Go back to the DETACH state to re-issue the mode request
                    undockMode_ = DETACH;
                }

                // Wait for the vehicle to drift away from the dock (range exceed dock range and IR sensor is high).
                if( rangeUndocked() && !cablePresent_ )
                {
                    if( !timerArmed_ || undockTime_ == Timestamp::NOT_SET_TIME )
                    {
                        logger_.syslog( "Detected possible detachment. Starting timer.", Syslog::INFO );
                        undockTime_ = Timestamp::Now();
                        timerArmed_ = true;
                    }
                    else if( undockTime_.elapsed() > undockTimeoutSetting_ )
                    {
                        logger_.syslog( "Detached at range: " + Str( dockRange_, 2 ) + " m. Transitioning docking module to standby.", verbose_ ? Syslog::IMPORTANT : Syslog::INFO );
                        undockMode_ = DETACHED;
                    }
                }
                else if( timerArmed_ )
                {
                    // Reset the clock
                    undockTime_ = Timestamp::NOT_SET_TIME;
                    timerArmed_ = false;
                }
                break;

            case DETACHED:
                if( cmdDockState( DockIF::STANDBY ) )
                {
                    logger_.syslog( "Docking module at standby.", Syslog::INFO );
                    undockTime_ = Timestamp::NOT_SET_TIME;
                    timerArmed_ = false;
                    undockMode_ = UNDOCKED;
                }
                break;

            case UNINITIALIZED:
            default:
                break;
            }
        }
    }

    if( !satisfied )
    {
        // Maintain depth at the specified undock setting
        verticalModeWriter_->write( Units::ENUM, VerticalControlIF::DEPTH );
        depthCmdWriter_->write( Units::METER, undockDepthSetting_ );
    }

    return satisfied;
}


/// Power on the camera and lights
bool Undock::powerCameraAndLights()
{
    // Power up the camera, then lights
    return powerLights() && powerCamera();
}


/// Power on camera
bool Undock::powerCamera()
{
    bool cameraOn( false );

    if( cameraLoaded_ )
    {
        // Power the camera, sampleNanoDvrReader_ will read true once on
        sampleNanoDvrReader_->requestData( true );
        sampleNanoDvrReader_->read( cameraOn );
        if( cameraOn )
        {
            sampleNanoDvrReader_->requestData( false );
        }
    }
    else
    {
        // Camera isn't loaded, so simply satisfy
        return true;
    }
    return cameraOn;
}


/// Power on the camera lights
bool Undock::powerLights()
{
    bool lightsOn( false );

    if( powerOnlyLoaded_ )
    {
        // Power the camera, samplePowerOnly will read true once on
        samplePowerOnlyReader_->requestData( DataAccessor::MIN_ERROR );
        samplePowerOnlyReader_->read( lightsOn );
        if( lightsOn )
        {
            samplePowerOnlyReader_->requestData( false );
        }
    }
    else
    {
        // PowerOnly component isn't loaded, so simply satisfy
        return true;
    }
    return lightsOn;
}


/// Command the state of the docking module
bool Undock::cmdDockState( DockIF::DockingState stateCmd )
{
    if( dockingModuleLoaded_ )
    {
        int dockingState( 0 );
        if( dockingStateReader_->read( Units::ENUM, dockingState ) )
        {
            dockingState_ = ( DockIF::DockingState )dockingState;
            if( dockingState_ == stateCmd )
            {
                return true;
            }
        }

        // Request the mode change
        dockingStateCmdWriter_->write( Units::ENUM, ( int )stateCmd );
    }
    else
    {
        // Docking module hardware isn't loaded, so simply satisfy
        logger_.syslog( "Docking module hardware isn't loaded, so simply satisfying DockingState request.", Syslog::INFO );
        dockingState_ = stateCmd;
        return true;
    }

    return false;
}

bool Undock::rangeUndocked( bool noRangeDefaultVal )
{
    // Monitor range from dock, return noRangeDefaultVal in case we didn't get a
    // range reading in the recent past.
    bool undockRange( noRangeDefaultVal );

    dockRange_ = nanf( "" );
    if( dockRangeReader_->read( Units::METER, dockRange_ ) && !isnan( dockRange_ )
            && dockRangeReader_->getTimestamp().elapsed() < validRangeTimeout_ )
    {
        undockRange = dockRange_ >= undockRangeSetting_;
    }

    return undockRange;
}

/// Uninit function
void Undock::uninitialize()
{
    samplePowerOnlyReader_->requestData( false );
    cablePresentReader_->requestData( false );
    dockingStateReader_->requestData( false );

    undockMode_ = UNINITIALIZED;
}

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