/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : DynamicControl.h                                        */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#ifndef _DynamicControl_H
#define _DynamicControl_H

#include "DynamicControlIF_SK.h"

#include "NavigationIF.h"
#include "LayeredControlIF.h"
#include "VehicleConfigurationIF.h"
#include "LogFile.h"
#include "EventService.h"
#include "DynamicControlLog.h"

#include "TailConeIF.h"
#include "TerrainAidIF.h"

#ifndef PI
#define PI 3.1415926535898
#endif

#ifndef TWOPI
#define TWOPI (2.*PI)
#endif

#ifndef PIUPON2
#define PIUPON2 (PI/2.)
#endif

#ifndef PIUPON4
#define PIUPON4 (PI/4.)
#endif


//class DynamicControl : public DynamicControlIF_SK {
class DynamicControl {
    
    friend class DynamicControlLog;
    friend class LayeredControl;
    friend class Gnc;
    
public:
    
    DynamicControl();
    virtual ~DynamicControl();
    void run();
    // added for GNC [klh]
    void callback();
   
protected:
    // added for GNC [klh]
    void notifyEvent(EventCode event);
    double _lastWallBearing, _lastWallTime;

    //====================================================================
    // Server callback functions
    
    virtual void getLogData(DynamicControlIF::LogData *logData);
    virtual void setCommand(DynamicControlIF::Command *command);
    virtual void setGains(DynamicControlIF::Gains *gains);
    virtual void revertGains(void);
    virtual Boolean newGains(void);
    virtual void setMaxHeadingRate(double rate);
    virtual double maxHeadingRate();
    virtual void setMaxDepthRate(double rate);
    virtual double maxDepthRate();
    virtual void setTau(float tau);
    virtual float tau();
    virtual void setPitchServoGains(double proportional, double integral,
                                    double derivative);
    
    virtual void pitchServoGains(double *proportional, double *integral,
                                 double *derivative);
    
    virtual void setHeadingServoGains(double proportional, double integral,
                                      double derivative);
    
    virtual void headingServoGains(double *proportional, double *integral,
                                   double *derivative);
    virtual void getWallData(double *lastWallTime, double *wallBearing);

    
    // no need to implement
//         virtual int spawnAuxTasks();
    
    //====================================================================
    // Interfaces
    
    LayeredControlIF *_layeredControl;
    NavigationIF *_navigation;
    TailConeIF *_tailCone;
    
    TerrainAidIF *_terrainAidIF;
    Boolean _enableTRN;
    
    DynamicControlLog *_log;
    
    VehicleConfigurationIF _vehicleConfig;
    
    //====================================================================
    // Flags
    
    Boolean _missionStarted;
    
    // Set to True when first event is received from Navigation
    Boolean _navigationReady;
    
    //====================================================================
    // Constants
    
    struct Gains {
        double p;
        double i;
        double d;
    };
    
    Gains _pitchGains;
    Gains _headingGains;
    
    double _maxHeadingRate;
    double _maxDepthRate;
    
    // Gains gathered from VehicleConfigurationServer
    double _ductArea;
    double _propPitch;
    double _kwph;
    double _maxHdgRate;
    double _kpp;
    double _kdp;
    double _kip;
    double _kpDepth;
    double _kiDepth;
    double _kiDepthOff;
    double _kiWPDepthOff;
    double _kdDepth;
    double _pitchLimit;
    double _lowerPitchLimit;
    double _effDragCoef;
    double _tau;
    double _maxRudder;
    double _maxElevator;
    double _maxDiveRate;
    double _Ts;
    double _maxDepthInt;
    double _maxHdgInt;
    double _maxPitchInt;
    double _propEfficiency;
    double _Xuabu;
    Boolean _decouple;
    Boolean _wallFeedForward;
    Boolean _useWallEstimator;
    Boolean _rhtOnly;
    Boolean _inTheFOV;
    unsigned int _lastSwitches;
    Boolean _gainsChanged;
    Boolean _intLatched;
    double _pitchTrim;
    
    
    //====================================================================
    // Input and output structures
    
    DynamicControlIF::Command _command;
    DynamicControlIF::Gains _gains;
    
    DynamicControlIF::VerticalMode _lastVMode;
    DynamicControlIF::HorizontalMode _lastHMode;
    
    double _rudder, _elevator, _propOmega;
    double _des_depth;
    double _cmd_depth;
    double _propSpeedCmd;
    
    //====================================================================
    // Internal functions
    
    struct Trajectory
    {
        Trajectory() {
            init = False;
            dt = tau = alpha = beta = x = xdot = 0.;
        }
        
        Boolean init;
        double dt, tau, alpha, beta, x, xdot;
    };
    
    ///////////////////////////////////////////////////////////////////
    // Trajectory generator initialization:
    void init_traj( Trajectory *traj, double x, double tau, double dt );
    
    ///////////////////////////////////////////////////////////////////
    // Generate depth trajectory:
    double do_traj( double xf, double xdotmax, Trajectory *traj );
    
    ///////////////////////////////////////////////////////////////////
    // Generate commanded heading angle:
    double do_hdg_traj( double xf, double xdotmax, Trajectory *traj );
    
    ///////////////////////////////////////////////////////////////////
    // keeps heading between 0-2PI, only corrects once though.
    void wrap_heading(double *h);
    
    ///////////////////////////////////////////////////////////////////
    // Corrects the difference between 2 headings to keep the value
    // between +/- pi.  In other words, it takes the shortest distance
    // around the circle.
    double delta_heading(double dh);
    
    ///////////////////////////////////////////////////////////////////
    // PID Pitch controller:
    double pitch_control( double pitch,      double des_pitch,
                         double pitch_rate );
    
    ///////////////////////////////////////////////////////////////////
    // Process new navigation data
    void process();
    
    ///////////////////////////////////////////////////////////////////
    // PID Heading controller:
    void HeadingControl(double heading, double headingCmd,
                        NavigationIF::Attitude *attitude);
    
    
    //
    // Allocate memory here so that these structures retain their values
    // between function calls.
    //
    Trajectory _depthTraj, _headingTraj;            //Command Generator states
    
    double _depthIntegral,                 //Integrator states
    _pitchIntegral,
    _xteIntegral,
    _headingIntegral,
    _wallBearing;
    
    
    // Intermediate values
    double mDes_pitch;
    double mT_Psi;
    double mPsiProp;
    double mPsiRate;
    double _err_depth, _xte, _xteWay;
    double _dPsi, _maxXteInt, _maxXteOff, _kiwp, _kxte;
    double _thetaFF;
    
    double _kdZdot;
    
    //
    // The following variables are from the pitch PID control.
    //
    double _pitchProportional;           //Kp * pitch_error
    double _pitchRate;                   //Kd * omega_B_N_B_y
    double _elevBefLim;                  //PID elev cmd before maxElev limiter
    double _p_dbg;                       //Input to pitch integrator
    int    _wasLimited;                  //True when PID elev cmd exceeds maxElev
    
    NavigationIF::Attitude _attitude;
    NavigationIF::Position _position;
    TerrainAidIF::Data _terrainNavData;
    
    double _time;        // "Synthetic"
    double _clockTime;   // Actual clock
    double _clockTime0;  // Time at the start of the mission.
    double _clockTimeSinceStart;
    double _commandTime; // time we last got a command
    double _gainTime;    // time we last got a gain change
    double _lastTime;
    double _lastTerNavTime;
    double _lastTimeDC;    //State for the nav loop computation.
    double _navLoopTimeDC; //Nav loop time, measured at the start of dynamicCntrl
    
    double _dx, _dy;        //Terrain Nav biases.
    
    
    double _bearingToTarget; //Bearing to the transponder measured by Usbl.
    Boolean _first;
    Boolean _firstWall, _initializeWallBearing, _turnReset, _wallPresent;
    
    //====================================================================
    // Callbacks
    
    ///////////////////////////////////////////////////////////////////
    // Called when the Tailcone is reset
    void tailConeCallback(TaskInterface *taskInterface, EventCode eventCode);
    
    ///////////////////////////////////////////////////////////////////
    // Called when LayeredControl says mission has started
    void missionStartCallback(TaskInterface *taskInterface,
                              EventCode code);
    
    ///////////////////////////////////////////////////////////////////
    // Called when Navigation says new data is ready
    void newNavDataCallback(TaskInterface *taskInterface,
                            EventCode code);
    
    
};

#endif