/** \file
 *
 *  Contains the Docked class declaration.
 *
 *  Docked.h should only be included by Docked.cpp
 *  Other classes should include DockedIF.h
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#ifndef DOCKED_H_
#define DOCKED_H_

#include "component/Behavior.h"
#include "DockedIF.h"
#include "DockIF.h"

class UniversalDataReader;

/**
 *  Contains the Docked Behavior/Command.
 *
 *  Docked.h should only be included by Docked.cpp
 *  Other classes should include DockedIF.h
 *
 *  \ingroup modules_dock
 */
class Docked : public Behavior
{
public:

    Docked( const Str& prefix, const Module* module );

    virtual ~Docked();

    /// Initialize function
    void initialize();

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

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

    /// Just do the run: ignore the results of the satisfied
    void run();

    /// Just do the satisfied: return true if envelope "satisfied"
    bool isSatisfied();

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

    /// Uninit function
    void uninitialize();

    /// Mission Component factory interface
    static Behavior* CreateBehavior( const Str& prefix, const Module* module );

protected:

    // Additional state information needed for slide mode
    typedef enum
    {
        UNDEFINED,
        CHECK,
        SINK,
        WIGGLE,
        JOG,
        WHIRL,
        UNINIT,
    } SlideState;

    typedef struct
    {
        SlideState state_;       // Slide substate
        Timestamp slideTime_;    // Time of start of substate
        Timestamp jogTime_;      // Time of start of jog in jog substate
        float jogStageSpeed_;    // Speed of jog substage (forwards, backwards, stopped)
        int slideTries_;         // Number of completed slide mode iterations
        int wiggleCount_;        // Number of completed slide mode wiggles this iteration
        bool needInitialCheck_;  // Has initial power check been started?
        bool wigglePitchUp_;     // Direction of next wiggle
    } SlideInfo;

    /// Init internal member variables to default values
    void initializeVariables( void );

    /// Configuration readers

    /// Range limit from dock
    ConfigReader* dockRangeCfgReader_;

    /// Time duration for determining if the vehicle is DETACHED
    ConfigReader* detachTimeoutCfgReader_;

    /// Time duration for determining if the vehicle is DOCKED
    ConfigReader* dockTimeoutCfgReader_;

    /// Max time duration with no stepper updates
    ConfigReader* dataTimeoutCfgReader_;

    /// Max time duration with no range updates;
    ConfigReader* rangeTimeoutCfgReader_;

    /// Depth at which the vehicle detaches from the dock
    ConfigReader* stopDepthCfgReader_;

    /// Micromodem surface threshold
    ConfigReader* surfaceThresholdCfgReader_;

    /// Report state transitions to shore when true
    ConfigReader* verboseCfgReader_;

    /// Buoyancy limitlo, used for buoyancy goal in slide mode
    ConfigReader* buoyancyLoCfgReader_;

    /// Buoyancy neutral, used for buoyancy goal in slide mode inital check
    ConfigReader* buoyNeutralCfgReader_;

    /// Mass limitfwd, used for pitch down wiggle goal in slide mode
    ConfigReader* massPosLimitFwdCfgReader_;

    /// Mass limitaft, used for pitch up wiggle goal in slide mode
    ConfigReader* massPosLimitAftCfgReader_;

    /// Slide state timeout
    SettingReader* slideTimeoutSettingReader_;

    /// Time duration for sink slide mode substate
    SettingReader* sinkDurationSettingReader_;

    /// Time duration for check/arm closed slide mode substate
    SettingReader* closeDurationSettingReader_;

    /// Time duration for jog/whirl slide mode substates
    SettingReader* driveDurationSettingReader_;

    /// Number of slide mode iterations to try before giving up
    SettingReader* slideRetrySettingReader_;

    /// Number of wiggles to attempt each slide mode iteration, if allowed
    SettingReader* wiggleCountSettingReader_;

    /// Use slide mode?
    SettingReader* trySlideSettingReader_;

    /// Within slide mode, use mass wiggles?
    SettingReader* tryWiggleSettingReader_;

    /// Within slide mode, use prop jog?
    SettingReader* tryJogSettingReader_;

    /// If using jog, run the prop for how long each jog?
    SettingReader* jogLengthSettingReader_;

    /// Within slide mode, use drive whirl?
    SettingReader* tryWhirlSettingReader_;

    /// If using whirl, drive forward at what speed?
    SettingReader* whirlSpeedSettingReader_;

    // Slate input measurements

    /// Vehicle depth
    UniversalDataReader* depthReader_;

    /// Battery state, false if charging
    UniversalDataReader* dischargingReader_;

    /// Mass position
    UniversalDataReader* massPositionReader_;

    /// Slant range to dock
    DataReader* dockRangeReader_;

    /// Docking module state
    DataReader* dockingStateReader_;

    /// Docking module IR sensor reading
    DataReader* cablePresentReader_;

    // Slate output variables

    /// Sets the state of the docking module hardware
    DataWriter *dockingStateCmdWriter_;

    /// Sets the mode for Depth Control
    DataWriter *verticalModeWriter_;

    /// Sets buoyancy command
    DataWriter *buoyancyCmdWriter_;

    /// Sets mass position command
    DataWriter *massPositionCmdWriter_;

    /// Sets elevator command
    DataWriter *elevatorCmdWriter_;

    /// Sets the mode for horizontal control
    DataWriter *horizontalModeWriter_;

    /// Sets the rudder command
    DataWriter *rudderCmdWriter_;

    /// Sets the speed command
    DataWriter *speedCmdWriter_;

    // Settings

    /// Time duration for determining if the vehicle is DETACHED
    Timespan detachedTimeoutSetting_;

    /// Time duration for determining if the vehicle is DOCKED
    Timespan dockTimeoutSetting_;

    /// Max time duration with no stepper updates
    Timespan dataTimeoutSetting_;

    /// Max time duration with no range updates
    Timespan rangeTimeoutCfgSetting_;

    /// Overall slide mode timeout
    Timespan slideTimeoutSetting_;

    /// Time duration for sink slide mode substate
    Timespan sinkDurationSetting_;

    /// Time duration for check/arm closed slide mode substate
    Timespan closeDurationSetting_;

    /// Time duration for jog or whirl slide mode substates
    Timespan driveDurationSetting_;

    /// Time duration for each bump in jog substate
    Timespan jogLengthSetting_;

    /// Range to dock in which the vehicle is considered detached
    float dockRangeSetting_;

    /// Vehicle stop depth
    float stopDepthSetting_;

    /// Vehicle surface threshold
    float surfaceThresholdCfgSetting_;

    /// Buoyancy limitlo, used for buoyancy goal in slide mode
    float buoyLoCfgSetting_;

    /// Buoyancy neutral, used for buoyancy goal in first check of slide mode
    float buoyNeutralCfgSetting_;

    /// Mass limitfwd, used for pitch down wiggle goal in slide mode
    float massPosLimitFwdCfgSetting_;

    /// Mass limitaft, used for pitch up wiggle goal in slide mode
    float massPosLimitAftCfgSetting_;

    /// Drive speed for whirl slide substate
    float whirlSpeedSetting_;

    /// Number of slide mode iterations to try before giving up
    int slideRetrySetting_;

    /// Number of wiggles to attempt each slide mode iteration, if allowed
    int wiggleCountSetting_;

    /// Use slide mode?
    bool trySlideSetting_;

    /// Within slide mode, use mass wiggles?
    bool tryWiggleSetting_;

    /// Within slide mode, use prop jog?
    bool tryJogSetting_;

    /// Within slide mode, use drive whirl?
    bool tryWhirlSetting_;

    // Docked Values

    /// Returns true when sensors indicate the vehicle is on the dock
    bool isOnDock( void );

    /// Returns next dock state
    DockedIF::DockState getState( void );

    /// Returns next slide mode substate
    SlideState getSlideState( void );

    /// Reset dock state change timer
    void resetTimer( void );

    /// The current dock state
    DockedIF::DockState state_;

    /// Reflects the current state of the docking module hardware
    DockIF::DockingState dockingState_;

    /// Additional state information for slide mode
    SlideInfo* slideInfo_;

    /// Marks time of dock state change
    Timestamp dockTime_;

    /// Marks time of last dock range update
    Timestamp dockRangeTime_;

    /// Marks the time of the first docking module data update in the current duty cycle
    Timestamp dataStartTime_;

    /// Marks the time of last docking module update
    Timestamp dataTime_;

    /// Vehicle depth
    float depth_;

    /// Slant range to the dock transponder
    float dockRange_;

    /// Mass position
    float massPositionMm_;

    /// Indicates obscuration across the docking module's IR emitter
    bool cablePresent_;

    /// Indicates the state of the state change timer
    bool timerArmed_;

    /// Indicates whether or not a reason for being off dock has been logged
    /// this state transition
    bool loggedOffDock_;

    /// Send state transition info back to shore?
    bool verbose_;

    /// Are batteries discharging? False when charging.
    bool discharging_;

private:
    // Note that the copy constructor below is private and not given a body.
    // Any attempt to call it will return a compiler error.
    Docked( const Docked& old ); // disallow copy constructor

#ifdef __GAZEBO
    static constexpr float WIGGLE_THROW_FACTOR = 0.9;
    static constexpr float JOG_SPEED_M_PER_S = 1.0;
    static constexpr float WHIRL_ELEVATOR_ANGLE_DEG = 13;
    static constexpr float WHIRL_RUDDER_ANGLE_DEG = 13;
#else
    /// What fraction of full mass shifter throw should slide mode mass wiggles use?
    /// Goal will always be fwd/aft limits, but wiggle will inflect when this fraction
    /// of full throw is reached.
    static const float WIGGLE_THROW_FACTOR = 0.9;
    /// Speed at which to run the prop during slide mode substate prop jogs, m/s
    static const float JOG_SPEED_M_PER_S = 1.0;
    /// Elevator angle for whirl slide mode substate, degrees
    static const float WHIRL_ELEVATOR_ANGLE_DEG = 13;
    /// Rudder angle for whirl slide mode substate, degrees
    static const float WHIRL_RUDDER_ANGLE_DEG = 13;

#endif

};

#endif /*DOCKED_H_*/