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

#ifndef DEPTHSERVO_H_
#define DEPTHSERVO_H_

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

class UniversalDataReader;

/**
 *  Contains the DepthServo Behavior/Command
 *  Uses depth, or the specified surrogate for depth
 *  to control the depth of the vehicle such that
 *  the vehicle stays at a constant depth, or other
 *  variable (if specified).
 *
 *  If a variable other than depth (i.e., a surrogate) is specified,
 *  PID loop constants must also be supplied.
 *
 *  DepthServo.h should only be included by DepthServo.cpp
 *  Other classes should include DepthServoIF.h
 *
 *  \ingroup modules_guidance
 */
class DepthServo : public Behavior
{
public:

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

    virtual ~DepthServo();

    /// Initialize function
    void initialize( void );

    /// Read in relevant configuration values
    bool readConfig();

    /// Read in settings
    bool readSettings( float& measuredValue );

    /// 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 float measuredValue );

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

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

protected:

    // Setting readers

    /// Desired depth of the vehicle, or surrogate for depth
    SettingReader *holdValueSettingReader_;

    /// Input variable to control
    SettingReader *inputSettingReader_;

    /// Specifies PID loop proportional gain -- required if DepthSurrogate specified
    SettingReader *proportionalGainSettingReader_;

    /// Specifies PID loop integral gain -- required if DepthSurrogate specified
    SettingReader *integralGainSettingReader_;

    /// Specifies PID loop derivative gain -- required if DepthSurrogate specified
    SettingReader *derivativeGainSettingReader_;

    /// Specifies approximate change per meter depth
    SettingReader *changePerMeterSettingReader_;

    /// Configuration Readers

    /// Depth at which we drop the weight. Should be greater than all depth envelopes
    ConfigReader* abortDepthCfgReader_;

    /// Speed being fed to speed Control
    DataReader *speedCmdReader_;

    /// From Vertical Control, get max dive rate
    ConfigReader* maxDiveRateCfgReader_;

    /// From Vertical Control, get max dive rate under buoy control
    ConfigReader* maxBuoyDiveRateCfgReader_;

    /// From Vertical Control, get max dive accel
    ConfigReader*  maxDiveAccelCfgReader_;

    /// From Vertical Control, get max dive accel under buoy control
    ConfigReader* maxBuoyDiveAccelCfgReader_;


    // Slate output variables

    /// Sets the depth mode for dynamic control
    DataWriter *verticalModeWriter_;

    /// Sets the depth specification for Vertical Control
    DataWriter *depthCmdWriter_;

    // Actual values from settings

    /// Desired depth of the vehicle, or surrogate for depth
    float holdValueSetting_;

    /// Specifies PID loop proportional gain
    float proportionalGainSetting_;

    /// Specifies PID loop integral gain
    float integralGainSetting_;

    /// Specifies PID loop derivative gain
    float derivativeGainSetting_;

    /// Used in state estimation
    float changePerMeterSetting_;

    /// Actual integral of depth (or its surrogate)
    float holdValueIntegral_;

    /// Used to determine rate of change in hold value
    float lastMeasuredValue_;

    /// Estimator for depth or its surrogate
    Trajectory trajectory_;

    /// Used in estimating maxChangeRate_
    float maxDiveRate_;
    float maxBuoyDiveRate_;
    float maxDiveAccel_;
    float maxBuoyDiveAccel_;

    /// Stores the abort depth of the vehicle
    float abortDepth_;

    /// Current speedCmd
    float speedCmd_;

    /// Indicates difference between setting and position at initialization
    bool startBelowSetting_;

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

    // Accessors
    UniversalDataReader* depthReader_;

};

#endif /*DEPTHSERVO_H_*/