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

#ifndef ALTITUDESERVO_H_
#define ALTITUDESERVO_H_

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

class UniversalDataReader;

/**
 *  Contains the AltitudeServo Behavior/Command
 *  Specifies the depth of the vehicle such that
 *  the vehicle stays at a constant altitude.
 *
 *  AltitudeServo.h should only be included by AltitudeServo.cpp
 *  Other classes should include AltitudeServoIF.h
 *
 *  \ingroup modules_guidance
 */
class AltitudeServo : public Behavior
{
public:

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

    virtual ~AltitudeServo();

    /// Initialize function
    void initialize( void );

    /// Read in settings
    bool readSettings( void );

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

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

    /// Just do the run: ignore the results of the satisfied
    void run( void );

    /// Just do the satisfied: return true if envelope "satisfied"
    bool isSatisfied( void );

    /// Do the run, and return true if envelope "satisfied"
    bool runIfUnsatisfied( void );

    /// Uninit function
    void uninitialize( void );

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

protected:

    // Setting readers

    ///  Altitude setting reader
    SettingReader *targetAltitudeSettingReader_;

    /// Reads the deviation allowed between desired altitude
    SettingReader *altitudeDeviationSettingReader_;

    /// Reads the duration allowed with no valid altitude
    SettingReader *invalidAltitudeTimeoutSettingReader_;

    /// Optional init depth command reader
    SettingReader *initDepthSettingReader_;

    /// Max depth envelope
    SettingReader *maxDepthSettingReader_;

    SettingReader *iirFilterDecaySettingReader_;

    // Configuration Readers

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

    /// Depth controller deadband
    ConfigReader *depthDeadbandConfigReader_;

    // 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 altitude of the vehicle
    float targetAltitudeSetting_;

    /// Deviation allowed between desired altitude and actual
    float altitudeDeviationSetting_;

    /// Specifies the duration allowed with no valid altitude reading
    float invalidAltitudeTimeoutSetting_;

    /// Keeps track of time since last valid altitude reading
    Timestamp validAltitudeTimer_;

    /// Indicates valid altitude reading has been rported
    bool invalidAltitudeReported_;

    /// Stores optional init depth command setting that can be used
    /// to bring the vehicle down to DVL bottom lock range
    float initDepthSetting_;

    /// Stores the depth envelope max depth
    double maxDepthSetting_;

    /// Commanded depth low-pass filter
    SinglePoleIIR<double> depthCmdFilter_;

    /// Stores the commanded depth
    double depthCmd_;

    /// Holds the depth controller deadband
    float depthDeadband_;

    /// Indicates behavior should command the vehicle to the inital
    /// dive depth to get within DVL bottom lock range
    bool doInitDepth_;

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

    // Accessors
    /// Current height above sea floor
    UniversalDataReader *altitudeReader_;

    /// Current depth (in meter from sea surface -- positive down)
    UniversalDataReader *depthReader_;

};

#endif /*DEPTHSERVO_H_*/