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

#ifndef LINECAPTURE_H_
#define LINECAPTURE_H_

#include <vector>
#include "DockIF.h"
#include "controlModule/HorizontalControlIF.h"
#include "component/Behavior.h"
#include "data/Point2D.h"
#include "utils/SinglePoleIIRFilter.h"


class UniversalDataReader;

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

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

    virtual ~LineCapture();

    /// 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();

    /// 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:

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

    // Slate input setting variables

    SettingReader *iirFilterDecaySettingReader_;

    /// Arming range to trigger actions before reaching intercept range.
    ConfigReader *armRangeCfgReader_;

    /// Thruster speed when armed.
    ConfigReader *armSpeedCfgReader_;

    /// Range in which terminal guidance stops updating the heading cmd
    ConfigReader *lockoutRangeCfgReader_;

    /// Range at which further ranges are ignored in final approach. Used because azimuth becomes very noisy as elevation angle increases to 90. Vehicle will continue navigating to the projected waypoint.
    ConfigReader *shortFinalRangeCfgReader_;

    /// Distance to move away from the target (rollout) in response to a miss.
    ConfigReader *rolloutDistanceCfgReader_;

    /// Thruster speed during rollout.
    ConfigReader *rolloutSpeedCfgReader_;

    /// Max time duration to run in rollout mode
    ConfigReader *rolloutTimeoutCfgReader_;

    /// Time duration to run in intercept mode until satisfying
    /// Only used when DDM is not available
    ConfigReader *interceptTimeoutCfgReader_;

    // Time duration to wait after cable present first turns true until closing the latch
    ConfigReader *latchDelayTimeoutCfgReader_;

    /// Sets the HorizontalControl heading proportional gain during TERMINAL_GUIDANCE
    ConfigReader *kpHeadingTerminalCfgReader_;

    /// Sets the HorizontalControl heading integral gain during TERMINAL_GUIDANCE
    ConfigReader *kiHeadingTerminalCfgReader_;

    /// Sets the HorizontalControl heading proportional gain during FINAL_APPROACH
    ConfigReader *kpHeadingFinalCfgReader_;

    /// Sets the HorizontalControl heading integral gain during FINAL_APPROACH
    ConfigReader *kiHeadingFinalCfgReader_;

    /// Sets the Proportional Navigation gain
    ConfigReader *navigationGainCfgReader_;

    /// LRAUV max turn rate
    ConfigReader *maxHdgRateCfgReader_;

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


    // Slate input measurements

    /// Vehicle latitude
    UniversalDataReader* vehicleLatReader_;

    /// Vehicle longitude
    UniversalDataReader* vehicleLonReader_;

    /// Current orientation of the vehicle
    UniversalDataReader *orientationReader_;

    /// Unversal range
    UniversalDataReader *universalRangeReader_;

    /// Current velocity wrt seafloor
    UniversalDataReader *xVelocityWrtSeafloorReader_;

    /// Target latitude
    DataReader *contactLatReader_;

    /// Target longitude
    DataReader *contactLonReader_;

    /// Bearing to target in North-East-Down coordinate frame
    DataReader *headingToContactReader_;

    /// Slant range to target
    DataReader *rangeToContactReader_;

    /// Power interface to camera/lights
    DataReader *samplePowerOnlyReader_;

    /// Power interface to camera
    DataReader *sampleNanoDvrReader_;

    /// Docking module state
    DataReader *dockingStateReader_;

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


    // Slate output variables

    /// Commands vehicle speed
    DataWriter* speedCmdWriter_;

    /// Sets the mode for HorizontalControl
    DataWriter *horizontalModeWriter_;

    /// Commands HorizontalControl heading
    DataWriter *headingCmdWriter_;

    /// Logs the PN acceleration command (filtered, PN gain applied)
    DataWriter *proNavCmdWriter_;

    /// Logs bearing rate (filtered)
    DataWriter *bearingRateWriter_;

    /// Logs raw bearing rate (un-filtered)
    DataWriter *rawBearingRateWriter_;

    /// Logs rangeClosing return value
    DataWriter *rangeClosingWriter_;

    /// Sets the heading proportional gain for HorizontalControl
    DataWriter *kpHeadingGainWriter_;

    /// Sets the heading integral gain for HorizontalControl
    DataWriter *kiHeadingGainWriter_;

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

    /// Reflects the state of the internal LineCapture guidance mode
    DataWriter *guidanceModeWriter_;


    // Settings

    /// Objective range for arming actions (e.g., deploy docking whiskers)
    float armRangeCfg_;

    /// Range in which terminal guidance stops updating the heading cmd
    float lockoutRangeCfg_;

    /// Range at which further ranges are ignored in final approach. Used because azimuth becomes very noisy as elevation angle increases to 90. Vehicle will continue navigating to the projected waypoint.
    float shortFinalRangeCfg_;

    /// Distance to move away from the target (rollout) in response to a miss.
    float rolloutDistanceCfg_;

    /// Max duration to hold last heading (rollout) if target was missed
    Timespan rolloutTimeoutCfg_;

    /// Time duration to run in intercept mode until satisfying
    Timespan interceptTimeoutCfg_;

    /// Time duration to wait after cable present first turns true until closing the latch
    Timespan latchDelayTimeoutCfg_;

    /// HorizontalControl heading proportional gain during TERMINAL_GUIDANCE
    float kpHeadingTerminalCfg_;

    /// HorizontalControl heading integral gain during TERMINAL_GUIDANCE
    float kiHeadingTerminalCfg_;

    /// HorizontalControl heading proportional gain during FINAL_APPROACH
    float kpHeadingFinalCfg_;

    /// HorizontalControl heading integral gain during FINAL_APPROACH
    float kiHeadingFinalCfg_;

    /// Proportional Navigation gain
    /// The proportionality constant generally has a value of 3-5 (dimensionless)
    float navigationGainCfg_;

    /// Max turn rate
    float maxHdgRateCfg_;

    // Terminal Guidance Values

    typedef enum
    {
        UNINITIALIZED,
        TERMINAL_GUIDANCE,
        FINAL_APPROACH,
        SHORT_FINAL,
        INTERCEPT_LOCKOUT,
        ROLLOUT
    } GuidanceMode;

    /// The current guidance mode
    GuidanceMode guidanceMode_;

    void setGuidanceMode( GuidanceMode newMode );

    void setSpeed( GuidanceMode mode );

    // Target Values

    typedef struct TargetPos
    {
        TargetPos();

        /// Time of target measurement
        Timestamp time_;

        /// Traget latitude
        double lat_;

        /// Traget longitude
        double lon_;

        /// Slant range to the target
        float range_;

        /// World frame-of-reference acoustic bearing to the target.  Updates
        /// when the sonar has new data.
        double acBearing_;

        /// World frame-of-reference bearing to the target a.k.a
        /// Line-of-sight (LOS).  Updates at each computation cycle using
        /// current vehicle and target locations.  Not synchronized with
        /// acBearing_.
        double bearing_;

        /// Horizontal range to the target
        float hrange_;

        /// Time delta since last acoustic update
        double deltaT_;

        /// Range delta from last acoustic range
        double deltaX_;

        /// Approach rate to target in meter/second
        float approachRate_;

        /// Approach rate to target in meter/second
        float hApproachRate_;

        /// LOS rotation rate in rad/second
        double losRotationRate_;

        bool operator==( const TargetPos& pos ) const
        {
            return time_ == pos.time_ && range_ == pos.range_;
        }

    } TargetPos;

    TargetPos previousTarget_;

    /// Number of target positions to store in the repo
    static const size_t TARGET_POS_BIN_SIZE;

    /// Holds target positions
    std::vector<TargetPos> posRepo_;

    /// Holds target ranges
    std::vector<TargetPos> rangeRepo_;

    /// Read new target position data from the slate
    bool readTargetPos( void );

    /// Add new target position data to the repo
    bool addTargetPos( TargetPos &newPos );

    /// Add new target range data to the range repository:
    bool addTargetRange( TargetPos &newPos );

    /// Retrieve latest target range
    float getRange( void );

    /// Retrieve latest target bearing
    float getBearing( void );

    /// Determine if the vehicle is closing on the target
    bool rangeClosing( void );

    /// Determine if the ranges are stable (vehicle is on the dock)
    bool rangeStable( void );

    /// Request docking module state change
    bool reqDockingState( DockIF::DockingState stateReq );

    /// Issue state change commands to the docking module
    bool cmdDockingState( void );

    /// Power on the camera and lights
    bool powerCameraAndLights( void );

    /// Power on the camera
    bool powerCamera( void );

    /// Power on the camera lights
    bool powerLights( void );

    /// Intercept timeout check. Returns true when expired.
    bool interceptTimeout( void );

    /// Returns true when latch close delay has expired.
    bool latchDelayTimeout( void );

    /// Computes the heading rate command using Proportional Navigation guidace rule
    float proNav( TargetPos& target );

    void calcTargetLOS( TargetPos& target );

    /// Min approach rate, in m/s, to determine if the vehicle is closing on the target
    static const float MIN_APPROACH_RATE;

    /// Max approach rate, in m/s, to determine if the incoming range reading is an outlier
    static const float MAX_APPROACH_RATE;

    SinglePoleIIR<double> iirFilter_;

    /// Marks the rollout start time
    Timestamp rolloutTime_;

    /// Marks intercept start time
    Timestamp interceptTime_;

    /// Marks latch delay start time
    Timestamp latchDelayTime_;

    /// Marks lockout start time
    Timestamp lockoutTime_;

    /// Terminal guidance speed
    float terminalGuidanceSpeed_;

    /// Final approach speed
    float finalApproachSpeed_;

    /// Rollout speed
    float rolloutSpeed_;

    /// Vehicle speed to command
    float speedCmd_;

    /// HorizontalControl heading proportional gain override
    float kpHeadingOverride_;

    /// HorizontalControl heading integral gain override
    float kiHeadingOverride_;

    /// Vehicle heading rate to command
    double headingRateCmd_;

    /// Vehicle relative position to the target
    Point2D relPosLast_;

    /// Vehicle heading to command
    double headingCmd_;

    /// Indicates a docking module is loaded and available
    bool dockingModuleLoaded_;

    /// Indicates obscuration across the docking servo IR emitter
    bool cablePresent_;

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

    /// Commanded state of the docking hardware
    DockIF::DockingState dockingStateCmd_;

    /// Indicates the LCB power interface component is loaded and available
    bool powerOnlyLoaded_;

    /// Indicates the camera is loaded and available
    bool cameraLoaded_;

    /// Counts the number of rejected outlier positions and ranges:
    int rejectCount_;
    int rejectRangeCount_;

    bool verbose_;

    bool debug_;

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

};

#endif /*LINECAPTURE_H_*/