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

#ifndef PITCHSERVO_H_
#define PITCHSERVO_H_

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

/**
 *  Contains the PitchServo 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.
 *
 *  PitchServo.h should only be included by PitchServo.cpp
 *  Other classes should include PitchServoIF.h
 *
 *  \ingroup modules_guidance
 */
class PitchServo : public Behavior
{
public:

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

    virtual ~PitchServo();

    /// Initialize function
    void initialize( void );

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

    /// Read in the configuration parameters
    void readConfig();

    /// Read in the mission parameters
    bool readSettings( float& measuredValue );

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

    /// Perform the satisfied: return true if envelope "satisfied"
    bool calcSatisfied( const float measuredValue );

    // Configuration variables
    ConfigReader* kdDepthCfgReader_;
    ConfigReader* kiDepthCfgReader_;
    ConfigReader* kpDepthCfgReader_;
    ConfigReader* maxDiveRateCfgReader_;
    ConfigReader* maxDiveAccelCfgReader_;
    ConfigReader* maxDepthIntCfgReader_;

    // Slate input setting variables

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

    /// Surrogate variable to use instead of depth
    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_;

    // Data Readers

    UniversalDataReader* depthReader_;

    // Data Writers

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

    /// Sets the pitch specification for Depth Control
    DataWriter *pitchCmdWriter_;

    // 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_;

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

    /// Maximum integral
    float maxDepthInt_;

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

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

    /// Used in state estimation
    float maxChangeRate_;

    /// Used in estimating maxChangeRate_
    float maxDiveRate_;

    /// Used in estimating maxChangeRate_
    float maxDiveAccel_;

    /// 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.
    PitchServo( const PitchServo& old ); // disallow copy constructor

};

#endif /*PITCHSERVO_H_*/