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

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

#include "TailConeIF.h"


#ifndef PI
#define PI 3.1415926535898
#endif

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

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


class DynamicControlServer : public DynamicControlIF_SK {

     friend class DynamicControlLog;

public:

     DynamicControlServer();
     ~DynamicControlServer();

protected:

     //====================================================================
     // Server callback functions

     virtual void getLogData(DynamicControlIF::LogData *logData);
     virtual void setCommand(DynamicControlIF::Command *command);
     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 int spawnAuxTasks();

     //====================================================================
     // Interfaces

     LayeredControlIF *_layeredControl;
     NavigationIF *_navigation;
     TailConeIF *_tailCone;

     DynamicControlLog *_log;

     //====================================================================
     // 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 _pitchLimit;
     double _effDragCoef;
     double _tau;
     double _maxRudder;
     double _maxElevator;
     double _maxDiveRate;
     double _Ts;
     double _maxDepthInt;
     double _maxHdgInt;
     double _maxPitchInt;
     double _propEfficiency;
     double _Xuabu;
     Boolean _decouple;

     //====================================================================
     // Input and output structures

     DynamicControlIF::Command _command;
     
     double _rudder, _elevator, _propOmega;
     double _des_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 doHeading(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
	  _headingIntegral, 
	  _pitchIntegral; 

     // Intermediate values
     double mDes_pitch;
     double mT_Psi;
     double mPsiProp;
     double mPsiRate;

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

     double _time;        // "Synthetic"
     double _clockTime;   // Actual clock
     double _lastTime;
     double _lastTimeDC;    //State for the nav loop computation.
     double _navLoopTimeDC; //Nav loop time, measured at the start of dynamicCntrl

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