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

#ifndef DEPTHENVELOPE_H_
#define DEPTHENVELOPE_H_

#include "component/Behavior.h"

/**
 *  Contains the DepthEnvelope Behavior/Command
 *  If the vehicle's depth is within the specified
 *  max/min envelope, the command is satisfied. Otherwise,
 *  the vehicle is sent to a depth within the specified max/min
 *  envelope.
 *
 *  Via the DepthSurrogate setting, values other than depth
 *  may be used to control the depth bounds. If so, then the min and max
 *  variables should be specified in units compatible with the specified surrogate.
 *
 *  DepthEnvelope.h should only be included by DepthEnvelope.cpp
 *  Other classes should include DepthEnvelopeIF.h
 *
 *  \ingroup modules_guidance
 */
class DepthEnvelope : public Behavior
{
public:

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

    virtual ~DepthEnvelope();

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

    // Config Settings
    ConfigReader* pitchLimitCfgReader_;
    ConfigReader* maxDiveRateCfgReader_;
    ConfigReader* maxBuoyDiveRateCfgReader_;
    ConfigReader* elevTurnTimeCfgReader_;
    ConfigReader* massTurnTimeCfgReader_;


    // Current control commands

    /// Current vertical control mode
    DataReader *verticalModeReader_;

    /// Current vertical control hold mode
    DataReader *elevatorAngleCmdReader_;
    DataReader *massPositionCmdReader_;

    /// Current speed control speed command
    DataReader *speedCmdReader_;

    // Behavior setting readers

    /// Desired minimum height above sea floor for the vehicle
    SettingReader *minDepthSettingReader_;

    /// Desired maximum height above sea floor for the vehicle
    SettingReader *maxDepthSettingReader_;

    /// Surrogate values to use instead of depth
    SettingReader *depthSurrogateSettingReader_;

    /// Desired up/down depthRate of the vehicle
    SettingReader *depthRateSettingReader_;

    /// Desired up depthRate of the vehicle
    SettingReader *upDepthRateSettingReader_;

    /// Desired down depthRate of the vehicle
    SettingReader *downDepthRateSettingReader_;

    /// Desired up/down pitch of the vehicle
    SettingReader *pitchSettingReader_;

    /// Desired up pitch of the vehicle
    SettingReader *upPitchSettingReader_;

    /// Desired down pitch of the vehicle
    SettingReader *downPitchSettingReader_;

    // Actual values from settings

    /// Desired minimum height above sea floor for the vehicle
    float minDepthSetting_;

    /// Desired maximum height above sea floor for the vehicle
    float maxDepthSetting_;

    /// Desired down depthRate of the vehicle
    float downDepthRateSetting_;

    /// Desired up depthRate of the vehicle
    float upDepthRateSetting_;

    /// Desired down pitch of the vehicle
    float downPitchSetting_;

    /// Desired up pitch of the vehicle
    float upPitchSetting_;

    /// Maximum dive rate of the vehicle
    float maxDiveRate_;

    /// Maximum dive rate of the vehicle under only buoyancy control
    float maxBuoyDiveRate_;

    /// Current speed cmd
    float speedCmd_;

    // Slate input measurements

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

    // Current depth rate
    DataReader* depthRateReader_;

    // Slate output variables

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

    /// Sets the depth rate for Vertical Control
    DataWriter *depthRateCmdWriter_;

    /// Sets the pitch for Vertical Control
    DataWriter *pitchCmdWriter_;

    float elevatorTurnSeconds_;
    float massTurnSeconds_;

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

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

    // Reads in configuration settings
    void readConfig();

    // Reads in behavior settings
    void readSettings();

    // If depthRate is nan, set to appropriate max dive rate.
    float adjustDepthRate( float depthRate, bool goDown );

    // If usingDepth_ is true, project the depth out to when it can be switched
    float projectDepth( float depth );

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

};

#endif /*DEPTHENVELOPE_H_*/