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

#ifndef VERTICALCONTROL_H_
#define VERTICALCONTROL_H_

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

class Timestamp;
class UniversalDataReader;

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

    VerticalControl( const Module* module );

    virtual ~VerticalControl();

    void initialize( void );

    void run();

    void uninitialize( void );

private:

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

    bool readConfig( void );

    void controlVertical( void );

    void controlDepth( float depthCmd );

    void controlDepthRate( float depthRateCmd, float pitchLimit = nanf( "" ) );

    void setPitch( float pitchCmd );

    void setPitchRate( float pitchRateCmd );

    void setMassAndElevator( float massPositionCmd, float elevatorAngleCmd );

    void setBuoyancy( float buoyancyCmd );

    float elevatorControl( float pitch, float pitchCmd, float pitchRate );

    float massControl( float pitch, float pitchCmd, float pitchRate, bool forceIntegrator = false );

    void massAndElevatorControl( float pitch, float pitchCmd, float pitchRate,
                                 float &massPositionAction, float &elevatorAngleAction );

    void floatAtSurface();

    float getMassPitchFilterError( float pitch, float pitchCmd, float dt );

    void overrideVertical();

    float elevatorPitchIntegral_;

    float massPitchIntegral_;

    float* massFilter_;
    int massFilterPos_;
    int massFilterBins_;

    float lastPitchCmd_;

    Timestamp lastPitchChange_;

    float depthIntegral_;

    float depth2buoyInt_;

    float lastDepthRateCmd_;

    Trajectory depthTrajectory_;

    bool ok_;

    float buoyancyCmd_;

    float lastBuoyancyCmd_;

    float buoyancyPosition_;

    float lastBuoyancyPosition_;

    float lastPitch_;

    float lastDepthCmd_;

    float periodCmd_;

    float speedCmd_;

    bool waitingForDepthCrossing_;

    // Holds the time of when depth excusion was reached
    Timestamp excursionStartTime_;

    bool previouslyBelowExcursion_;

    float minAscendPitchCfg_;

    float minDepthExcursion_;

    float* depthRateFilter_;

    int depthRateFilterIndex_;

    Timestamp failToGoDownStart_;

    Timestamp failToGoUpStart_;

    float failToGoDownDepth_;

    float failToGoUpDepth_;

    bool missionEnded_;

    bool burnwireActivated_;

    VerticalControlIF::VerticalMode verticalMode_;

    /// Configuration values
    float buoyancyDefault_;
    float buoyancyLimitHiCC_;
    float buoyancyLimitLoCC_;
    float buoyancyNeutral_;
    float buoyancyPumpDepth_;
    float depthDeadband_;
    float depthRateDeadband_;
    int depthRateSamples_;
    float dropWtDepthExcursion_;
    Timespan dropWtOverrideDelay_;
    float elevDeadband_;
    float elevatorLimit_;
    float elevatorTurnTime_;
    float elevDeviation_; // **
    Timespan excursionDepthTimeout_;
    float kdDepth_;
    float kdDepthBuoy_;
    float kdDepthRateBuoy_;
    float kdPitchElevator_;
    float kdPitchMass_;
    float kiDepth_;
    float kiDepthBuoy_;
    float kiDepthOff_;
    float kiDepthRateBuoy_;
    float kiPitchElevator_;
    float kiPitchMass_;
    float kpDepth_;
    float kpDepthBuoy_;
    float kpDepthRateBuoy_;
    float kpPitchElevator_;
    float kpPitchMass_;
    int limitDepthTrajectory_;
    float massDeadband_;
    float massDefault_;
    float massFilterLimit_;
    float massFilterWidth_;
    float massPositionLimitFwd_;
    float massPositionLimitAft_;
    float massTurnTime_;
    float maxBuoyDiveAccel_;
    float maxBuoyDiveRate_;
    float maxBuoyInt_;
    float maxDepthInt_;
    float maxDiveAccel_;
    float maxDiveRate_;
    float maxPitchElevatorInt_;
    float maxPitchMassInt_;
    float maxPitchRate_;
    float nominalDt_;
    float pitchLimit_;
    float stopDepthExcursion_;
    Timespan stopOverrideDelay_;
    Timespan stopOverrideDelayBuoy_;
    float surfaceThreshold_;
    int useElevIntInDepthMode_;

    /// Command readers
    DataReader *verticalModeReader_;
    DataReader *depthCmdReader_;
    DataReader *depthRateCmdReader_;
    DataReader *pitchCmdReader_;
    DataReader *pitchRateCmdReader_;
    DataReader *massPositionCmdReader_;
    DataReader *elevatorAngleCmdReader_;
    DataReader *buoyancyCmdReader_;

    /// Other controllers' Command readers
    DataReader *periodCmdReader_;
    DataReader *speedCmdReader_;

    /// Config readers
    ConfigReader* buoyancyDefaultCfgReader_;
    ConfigReader* buoyancyLimitHiCCCfgReader_;
    ConfigReader* buoyancyLimitLoCCCfgReader_;
    ConfigReader* buoyancyNeutralCfgReader_;
    ConfigReader* buoyancyPumpDepthCfgReader_;
    ConfigReader* depthDeadbandCfgReader_;
    ConfigReader* depthRateDeadbandCfgReader_;
    ConfigReader* depthRateSamplesCfgReader_;
    ConfigReader* dropWtDepthExcursionCfgReader_;
    ConfigReader* dropWtOverrideDelayCfgReader_;
    ConfigReader* elevatorDeadbandCfgReader_;
    ConfigReader* elevatorLimitCfgReader_;
    ConfigReader* elevatorTurnTimeCfgReader_;
    ConfigReader* excursionDepthTimeoutCfgReader_;
    ConfigReader* kdDepthBuoyCfgReader_;
    ConfigReader* kdDepthCfgReader_;
    ConfigReader* kdDepthRateBuoyCfgReader_;
    ConfigReader* kdPitchElevatorCfgReader_;
    ConfigReader* kdPitchMassCfgReader_;
    ConfigReader* kiDepthBuoyCfgReader_;
    ConfigReader* kiDepthCfgReader_;
    ConfigReader* kiDepthOffCfgReader_;
    ConfigReader* kiDepthRateBuoyCfgReader_;
    ConfigReader* kiPitchElevatorCfgReader_;
    ConfigReader* kiPitchMassCfgReader_;
    ConfigReader* kpDepthBuoyCfgReader_;
    ConfigReader* kpDepthCfgReader_;
    ConfigReader* kpDepthRateBuoyCfgReader_;
    ConfigReader* kpPitchElevatorCfgReader_;
    ConfigReader* kpPitchMassCfgReader_;
    ConfigReader* limitDepthTrajectoryCfgReader_;
    ConfigReader* massDeadbandCfgReader_;
    ConfigReader* massDefaultCfgReader_;
    ConfigReader* massFilterLimitCfgReader_;
    ConfigReader* massFilterWidthCfgReader_;
    ConfigReader* massPositionLimitFwdCfgReader_;
    ConfigReader* massPositionLimitAftCfgReader_;
    ConfigReader* massTurnTimeCfgReader_;
    ConfigReader* maxBuoyDiveAccelCfgReader_;
    ConfigReader* maxBuoyDiveRateCfgReader_;
    ConfigReader* maxBuoyIntCfgReader_;
    ConfigReader* maxDepthIntCfgReader_;
    ConfigReader* maxDiveAccelCfgReader_;
    ConfigReader* maxDiveRateCfgReader_;
    ConfigReader* maxPitchElevatorIntCfgReader_;
    ConfigReader* maxPitchMassIntCfgReader_;
    ConfigReader* maxPitchRateCfgReader_;
    ConfigReader* minAscendPitchCfgReader_;
    ConfigReader* minDepthExcursionCfgReader_;
    ConfigReader* nominalDtCfgReader_;
    ConfigReader* pitchLimitCfgReader_;
    ConfigReader* stopDepthExcursionCfgReader_;
    ConfigReader* stopOverrideDelayCfgReader_;
    ConfigReader* stopOverrideDelayBuoyCfgReader_;
    ConfigReader* surfaceThresholdCfgReader_;
    ConfigReader* useElevIntInDepthModeCfgReader_;

    // External Config reader
    ConfigReader* elevDeviationCfgReader_;

    /// Universal Readers
    UniversalDataReader *depthReader_;
    UniversalDataReader *pitchReader_;
    UniversalDataReader *pitchRateReader_;
    UniversalDataReader *depthRateReader_;
    UniversalDataReader *elevatorAngleReader_;
    UniversalDataReader *massPositionReader_;
    UniversalDataReader *buoyancyPositionReader_;

    /// Action Readers -- for reading last positions, even if from Maintain command
    DataReader *elevatorAngleActionReader_;
    DataReader *massPositionActionReader_;

    /// Action Writers
    DataWriter *buoyancyActionWriter_;
    DataWriter *elevatorAngleActionWriter_;
    DataWriter *massPositionActionWriter_;

    // Special output from here to the slate to SpeedControl
    DataWriter* speedCmdWriter_;

    /// Debugging Writers
    DataWriter *depth2buoyIntInternalWriter_;
    DataWriter *depthErrorInternalWriter_;
    DataWriter *depthIntegralInternalWriter_;
    DataWriter *dtWriter_;
    DataWriter *elevatorIntegralWriter_;
    DataWriter *massIntegralWriter_;
    DataWriter *massPitchErrorWriter_;
    DataWriter *pitchInternalWriter_;
    DataWriter *smoothDepthInternalWriter_;

};

#endif /*SAMPLECOMPONENT_H_*/