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

#ifndef PITCHENVELOPE_H_
#define PITCHENVELOPE_H_

#include "component/Behavior.h"

class UniversalDataReader;

/**
 *  Contains the PitchEnvelope Behavior/Command.  Simply
 *  sends the vehicle to the surface.  Satisfied when the vehicle
 *  has reached the surface.
 *
 *  PitchEnvelope.h should only be included by PitchEnvelope.cpp
 *  Other classes should include PitchEnvelopeIF.h
 *
 *  \ingroup modules_guidance
 */
class PitchEnvelope : public Behavior
{
public:

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

    virtual ~PitchEnvelope();

    /// Initialize function
    void initialize( void );

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

    /// Perform the satisfied: return true if envelope "satisfied"
    bool calcSatisfied( 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:

    // Slate input setting variables

    /// Setting Readers
    SettingReader* surfaceTimeoutSettingReader_;
    SettingReader* speedSettingReader_;

    // Slate input measurements

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

    // Current height above sea floor
    UniversalDataReader *altitudeReader_;

    // Current pitch of vehicle
    UniversalDataReader *pitchReader_;

    // Current depthCmd of vehicle
    DataReader *depthCmdReader_;

    // Current speedCmd of vehicle
    DataReader *speedCmdReader_;

    // Slate output variables

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

    // Sets Control variables
    DataWriter* depthRateCmdWriter_;
    DataWriter* pitchCmdWriter_;
    DataWriter* speedCmdWriter_;

    // Configuration readers
    ConfigReader* pitchLimitCfgReader_;
    ConfigReader* massPositionLimitAftCfgReader_;
    ConfigReader* pitchTimeoutCfgReader_;

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

    // indicates whether things are ok to run
    bool ok_;
    bool surfaceTimeoutReported_, pitchTimeoutReported_; // used to only report failures once

    ///*------------------- vehicle parameters --------------------------*/
    float depth_;
    float depthCmd_;
    float pitch_;
    float speedCmd_;
    float pitchLimit_;
    float massPositionLimitAft_;
    ///////////////////////////////////

    /// Settings
    Timespan surfacingTimeoutSetting_;
    Timespan pitchTimeoutCfgSetting_;
    float speedSetting_;

    // Time that behavior was started
    Timestamp surfaceStartTime_, pitchStartTime_;

    // initialize vehicle config variables
    bool loadParams( void );

    // Returns true if there is sufficient altitude to turn up and pitch is responding within the timeout.
    bool checkPitchAndAltitude( void );

    void resetTimeouts( void );

};

#endif /*PITCHENVELOPE_H_*/