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

#ifndef HORIZONTALCONTROL_H_
#define HORIZONTALCONTROL_H_

#include "component/SyncComponent.h"
#include "utils/Trajectory.h"

class Timestamp;
class UniversalDataReader;

/**
 *  This is the primary "control" for the vehicle's heading.
 *  It accepts a number of inputs that
 *  specify the desired heading or waypoint for the vehicle, and
 *  combined with heading and rudder sensor inputs, converts
 *  the settings to commands that are ultimately relayed to devices via
 *  RudderServo.
 *
 *  HorizontalControl.h should only be included by HorizontalControl.cpp
 *  Other classes should include HorizontalControlIF.h
 *
 *  \ingroup modules_control
 */
class HorizontalControl : public SyncControlComponent
{
public:

    HorizontalControl( const Module* module );

    virtual ~HorizontalControl();

    /// Initialize function
    void initialize( void );

    /// The actual "payload" of the component
    void run();

    /// Uninit function
    void uninitialize( void );

protected:

    float headingIntegral_;
    float xteIntegral_;

private:

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

    //Command Generator states
    Trajectory headingTrajectory_;

    /// indicates whether things are ok to run
    bool ok_;

    ///*------------------- vehicle parameters --------------------------*/

    float kdHeading_;
    float kiHeading_;
    float kpHeading_;
    float kwpHeading_;
    float kiwpHeading_;
    float maxKxte_;
    float maxHeadingInt_;
    float rudderLimit_;
    float headingRate_;
    float maxHeadingRateCfg_;
    float maxHdgAccel_;
    float xte_;

    float rudDeadband_;

    ///////////////////////////////////

    /// read vehicle config variables
    bool readConfig( void );

    /// control heading
    void controlHeading();

    /// attempts to achieve the desired heading
    void setHeading( float desiredHeading, bool useInt = true );

    /// attempts to achieve the desired heading rate
    void setHeadingRate( float desiredheadingRate );

    /// attempts to achieve the desired rudder angle
    void setRudderAngle( float desiredRudderAngle );

    /// returns rudder angle
    float headingControl( float heading, float headingCmd, float yawRate, bool useInt = true );

    /// reads optional heading PID control gains
    void headingControlGainOverrides( void );

    // Slate input setting variables

    /// Desired heading mode (none, waypoint, specified).
    /// Default is NONE.
    DataReader *horizontalModeReader_;

    /// Desired waypoint latitude to head towards.
    /// Only applies if horizontalModeReader_'s value is WAYPOINT
    /// Default is current location (no change).
    DataReader *latitudeCmdReader_;

    /// Desired waypoint longitude to head towards.
    /// Only applies if horizontalModeReader_'s value is WAYPOINT
    /// Default is current location (no change).
    DataReader *longitudeCmdReader_;

    /// Bearing to the Desired waypoint.
    /// Only applies if horizontalModeReader_'s value is WAYPOINT
    /// Default is current location (no change).
    DataReader *bearingCmdReader_;

    /// Desired heading (radian) of the vehicle.
    /// Zero corresponds to true North, and PI/2 true East.
    /// (-PI <= Range < PI).
    /// Only applies if HorizontalMode value is HEADING
    /// Default is zero.
    DataReader *headingCmdReader_;

    /// Desired yaw / heading change rate (radian/second).
    /// Positive values correspond to positive changes in heading
    /// Applies to both waypoint and specified heading modes
    /// Default is 0.2 (to allow a ~15 second about-face)
    DataReader *headingRateCmdReader_;

    /// Desired rudder angle (radian).
    /// Positive values correspond to negative changes in heading
    /// Applies only to rudder angle heading mode
    DataReader *rudderAngleCmdReader_;

    /// Optional heading PID control gains (overrides configs)
    /// Only applies if HorizontalMode value is HEADING
    DataReader *kdHeadingOverrideReader_;   // Derivative gain
    DataReader *kiHeadingOverrideReader_;   // Integral gain
    DataReader *kpHeadingOverrideReader_;   // Proportional gain

    // Configuration readers
    ConfigReader* kdHeadingCfgReader_;   // Derivative gain
    ConfigReader* kiHeadingCfgReader_;   // Integral gain
    ConfigReader* kpHeadingCfgReader_;   // Proportional gain
    ConfigReader* kwpHeadingCfgReader_;  // Cross-track error gain
    ConfigReader* kiwpHeadingCfgReader_; // Cross-track integral error gain
    ConfigReader* maxHdgAccelCfgReader_; // Max turn accel
    ConfigReader* maxHdgIntCfgReader_;   // Max cmded rudder from hdg int
    ConfigReader* maxHdgRateCfgReader_;  // Max turn rate
    ConfigReader* maxKxteCfgReader_;     // Max heading correction due to kwpHeading
    ConfigReader* rudDeadbandCfgReader_; // Degree of rounding in output values
    ConfigReader* rudLimitCfgReader_;    // Max rudder angle


    // Slate input measurements

    /// Current heading (radian) of the vehicle.
    /// Zero corresponds to true North, and PI/2 true East.
    /// (-PI <= Range < PI).
    UniversalDataReader *headingReader_;

    /// Commanded heading angle in radians.
    DataWriter *headingCmdInternalWriter_;

    /// Smoothed commanded heading angle in radians.
    DataWriter *smoothHeadingCmdInternalWriter_;

    /// Heading integral in radians.
    DataWriter *headingIntegralInternalWriter_;
    DataWriter *xteIntegralInternalWriter_;

    /// Cross-track error, meter
    DataWriter *xteInternalWriter_;

    /// Cross-track error correction, radian
    DataWriter *kxteInternalWriter_;

    /// Commanded bearing, radians.  In UTM coordinates, this should be a constant.
    DataWriter *bearingInternalWriter_;

    /// Current yaw / heading change rate (radian/second).
    /// Positive values correspond to positive changes in heading
    UniversalDataReader *headingRateReader_;

    /// Current latitude
    UniversalDataReader *latitudeReader_;

    /// Current latitude
    UniversalDataReader *longitudeReader_;

    UniversalDataReader *rudderAngleReader_;

    // Slate output variables

    /// Angle (in radian) of Elevator.  Zero corresponds to
    /// aligned with major axis of vehicle.  Nominal range is -PI/4 to PI/4.
    //Output *outputRudderAngle_;
    DataWriter *rudderAngleActionWriter_;

    /// Used to read last action, even if from another source
    DataReader *rudderAngleActionReader_;

};

#endif /*SAMPLECOMPONENT_H_*/