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

#ifndef SLOWYO_H_
#define SLOWYO_H_

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

#include "SlowYoGenerator.h"


class UniversalDataReader;

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

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

    virtual ~SlowYo();

    /// 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* maxDiveRateCfgReader_;
    ConfigReader* maxBuoyDiveRateCfgReader_;

    // Current control commands

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

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

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

    // Behavior settings

    // Behavior depth rate setting
    SettingReader *diveAngleSettingReader_;
    SettingReader *speedSettingReader_;
    SettingReader *minDepthSettingReader_;
    SettingReader *maxDepthSettingReader_;
    SettingReader *smoothingFactorSettingReader_;

    /// Current depth
    UniversalDataReader *depthReader_;

    /// Current depth rate
    UniversalDataReader *depthRateReader_;

    // Vertical Control settings

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

    /// Sets the pitch for VerticalControl
    DataWriter *pitchCmdWriter_;
    /// Sets the depth for VerticalControl
    DataWriter *depthCmdWriter_;
    DataWriter *speedCmdWriter_;

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

    /// Desired dive angle of the vehicle
    float diveAngleSetting_;
    /// Desired speed of the vehicle
    float speedSetting_;
    /// Minimum Depth of vehicle
    float minDepthSetting_;
    /// Maximum Depth of vehicle
    float maxDepthSetting_;
    /// Smoothing factor blending trapezoidal & sinusoidal dive rate profile
    float smoothingFactorSetting_;

    /// 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 when we inflect
    bool satisfied_;

    // True once we run once
    bool ranOnce_;

    // Depth of start of yo
    float startDepth_;
    bool reachedStartDepth_;

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

    /// Command to set pitch
    float pitchCmd_;
    /// Command to set depth
    float depthCmd_;
    /// 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();

    bool reachedStartDepth();
    bool reachedEndOfProfile();

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

    SlowYoGenerator * slowyogen_;
    std::vector<double>::const_iterator pitchProfileIterator_;
    std::vector<double>::const_iterator depthProfileIterator_;
    std::vector<double>::const_iterator depthRateProfileIterator_;
};

#endif /*SLOWYO_H_*/