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

#ifndef YOYO_H_
#define YOYO_H_

#include "component/Behavior.h"
#include "controlModule/VerticalControlIF.h"
#include "data/Location.h"

class UniversalDataReader;

/**
 *  Contains the YoYo Behavior.  YoYo is very simple --
 *  it simply causes the vehicle to descend as quickly as
 *  possible when it it pointing down, and to ascend as quickly
 *  as possible when it is pointing up.  An outer DepthEnvelope
 *  is responsible for causing the inflections at the top and
 *  bottom of the yo-yos.
 *
 *  YoYo.h should only be included by YoYo.cpp
 *  Other classes should include YoYoIF.h
 *
 *  \ingroup modules_guidance
 */
class YoYo : public Behavior
{
public:

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

    virtual ~YoYo();

    /// Initialize function
    void initialize( void );

    /// Returns true when vehicle is starting to inflect
    bool isSatisfied();

    /// runs only if unsatisfied
    bool runIfUnsatisfied();

    /// The actual "payload" of the component
    void run();

    /// Uninit function
    void uninitialize( void );

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

protected:

    // Configuration readers
    ConfigReader* pitchLimitCfgReader_;
    ConfigReader* maxDiveRateCfgReader_;
    ConfigReader* maxBuoyDiveRateCfgReader_;

    // Current control commands

    /// Current vertical control depth mode
    DataReader *verticalModeReader_;

    /// Current vertical control depth rate command
    DataReader *depthRateCmdReader_;

    /// Current vertical control pitch command
    DataReader *pitchCmdReader_;

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

    // Behavior settings

    /// Behavior pitch setting
    SettingReader *pitchSettingReader_;
    SettingReader *downPitchSettingReader_;
    SettingReader *upPitchSettingReader_;

    // Behavior depth rate setting
    SettingReader *depthRateSettingReader_;
    SettingReader *downDepthRateSettingReader_;
    SettingReader *upDepthRateSettingReader_;

    SettingReader *satisfiedThresholdSettingReader_;

    /// Current depth
    UniversalDataReader *depthReader_;

    /// Current depth rate
    UniversalDataReader *depthRateReader_;

    /// Current pitch
    UniversalDataReader *pitchReader_;

    // Vertical Control settings

    /// Sets the vertical mode for VerticalControl
    DataWriter *verticalModeWriter_;

    /// Sets the depth rate for VerticalControl
    DataWriter *depthRateCmdWriter_;

    /// Sets the pitch for VerticalControl
    DataWriter *pitchCmdWriter_;

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

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

    /// Desired "down" depth rate of the vehicle
    float downDepthRateSetting_;

    /// Desired "up" depth rate of the vehicle
    float upDepthRateSetting_;

    /// Minimum depth change required to enable sequence behavior to be satisfied
    float satisfiedThresholdSetting_;

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

    /// Indicates initialization success
    bool initialized_;

    /// True if going down.
    bool goingDown_;

    // True when we inflect
    bool satisfied_;

    // True once we run once
    bool ranOnce_;

    // Depth of start of yo
    float startDepth_;

    /// Command to set verticalMode
    VerticalControlIF::VerticalMode verticalMode_;

    /// Command to set pitch
    float pitchCmd_;

    /// Command to set depth rate
    float depthRateCmd_;

    // Carries out behavior on parameters
    void writeCmds();

    // Determines if behavior is satisfied
    bool calcSatisfied();

    // Reads in configuration
    void readConfig();

    // Reads in behavior settings
    void readSettings();

    // Reads in parameters used by writeCmds and calcSatisfied
    bool calculate();

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

    // Sets value of going down
    // Sets satified true if changed, and ranOnce_ is true
    void setGoingDown( bool goingDown );

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

};

#endif /*YOYO_H_*/