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

DynamicControlLog::DynamicControlLog(DynamicControlServer *dynamicControl,
				     DataLog::FileFormat fileFormat)
  : DataLogWriter(DynamicControlLogName, fileFormat, AutoTimeStamp)
{

  setMnemonic("dataDC");

  addField(_myTimeStamp       = new DoubleData("myTime"));

  addField(_xte               = new DoubleData("xTrackError"));
  addField(_xteWay            = new DoubleData("xTrackErrorWay"));
  addField(_heading           = new DoubleData("cPsi"));
  addField(_rudder            = new DoubleData("cDeltaR"));
  addField(mT_Psi             = new DoubleData("tPsi"));
  addField(mPsiProp           = new DoubleData("psiProp"));
  addField(mPsiInt            = new DoubleData("psiInt"));
  addField(mPsiRate           = new DoubleData("psiRate"));
  addField(_xteInt            = new DoubleData("xteIntegral"));
  addField(_kxte              = new DoubleData("kxte"));
  addField(_dPsi              = new DoubleData("dPsi"));

  addField(_speed             = new DoubleData("speedCmd"));
  addField(_propOmega         = new DoubleData("omegaCmd"));

  addField(mDepth             = new DoubleData("cDepth"));
  addField(mT_depth           = new DoubleData("tDepth"));
  addField(_depthError        = new DoubleData("DepthError"));
  addField(mDepthIntegral     = new DoubleData("dIntegral"));
  addField(mDepthRateTerm     = new DoubleData("mDepthRateTerm"));
  addField(mcTheta            = new DoubleData("cTheta"));
  addField(_pitchProportional = new DoubleData("pitchProp"));
  addField(_pitchIntegral     = new DoubleData("pitchInt"));
  addField(_pitchRate         = new DoubleData("pitchRate"));
  addField(_p_dbg             = new DoubleData("pitchIntInp"));
  addField(_elevBefLim        = new DoubleData("cDeltaEBL"));
  addField(_elevator          = new DoubleData("cDeltaE"));
  addField(_thetaFF           = new DoubleData("thetaFF"));

#ifdef DOCKING
  addField((_targetNorthing = new DoubleData("targetNorthing")));
  addField((_targetEasting  = new DoubleData("targetEasting")));
  _targetNorthing->setAsciiFormat("%13.2f");
  _targetEasting->setAsciiFormat("%13.2f"); //Necessary for large UTM values.

  addField((_newBearing  = new DoubleData("newBearing")));
  addField((_newNorthing = new DoubleData("newNorthing")));
  addField((_newEasting  = new DoubleData("newEasting")));
  addField((_first       = new ShortData("first")));
  _newNorthing->setAsciiFormat("%13.2f");
  _newEasting->setAsciiFormat("%13.2f"); //Necessary for large UTM values.
#endif

#ifdef WALLFOLLOWING
  addField((_beamIndx    = new IntegerData("SideBeamIndex")));
  addField((_rangeToWall = new DoubleData("rangeToWall")));
  addField(_wallBearing  = new DoubleData("wallBearing"));
  addField((_hCmdDot     = new DoubleData("headingCmdDot")));
  addField(_dNorthing    = new DoubleData("deltaNorthingTrn"));
  addField(_dEasting     = new DoubleData("deltaEastingTrn"));
#endif

  _myTimeStamp->setAsciiFormat("%.2f");

  _dynamicControl = dynamicControl;
}


DynamicControlLog::~DynamicControlLog()
{
  delete _myTimeStamp;

  delete _xte;
  delete _xteWay;
  delete _heading;
  delete _rudder;
  delete mT_Psi;
  delete mPsiProp;
  delete mPsiInt;
  delete mPsiRate;
  delete mDepthRateTerm;

  delete _speed;
  delete _propOmega;

  delete mDepth;
  delete mT_depth;
  delete _depthError;
  delete mDepthIntegral;
  delete mcTheta;
  delete _pitchProportional;
  delete _pitchIntegral;
  delete _pitchRate;    
  delete _p_dbg;        
  delete _elevBefLim;
  delete _elevator;
  delete _thetaFF;
  delete _xteInt;
  delete _kxte;
  delete _dPsi;
#ifdef DOCKING
  delete _targetNorthing;
  delete _targetEasting;
  delete _newBearing;
  delete _newNorthing;
  delete _newEasting;
  delete _first;
#endif

#ifdef WALLFOLLOWING
  delete _beamIndx;
  delete _rangeToWall;
  delete _wallBearing;
  delete _hCmdDot;
  delete _dNorthing;
  delete _dEasting;
#endif
}


void DynamicControlLog::setFields()
{
  _myTimeStamp->setValue(_dynamicControl->_navLoopTimeDC);

  _xte        ->setValue(_dynamicControl->_xte);
  _xteWay     ->setValue(_dynamicControl->_xteWay);
  _heading    ->setValue(_dynamicControl->_command.horizontal);
  _rudder     ->setValue(_dynamicControl->_rudder);
  mT_Psi      ->setValue(_dynamicControl->mT_Psi);
  mPsiProp    ->setValue(_dynamicControl->mPsiProp);
  mPsiInt     ->setValue(_dynamicControl->_headingIntegral);
  mPsiRate    ->setValue(_dynamicControl->mPsiRate);

  _speed      ->setValue(_dynamicControl->_command.speed);
  _propOmega  ->setValue(_dynamicControl->_propOmega);

  mDepth            ->setValue(_dynamicControl->_command.vertical);
  mT_depth          ->setValue(_dynamicControl->_des_depth);
  _depthError       ->setValue(_dynamicControl->_err_depth);
  mDepthIntegral    ->setValue(_dynamicControl->_depthIntegral);
  mDepthRateTerm    ->setValue(_dynamicControl->_kdZdot);
  mcTheta           ->setValue(_dynamicControl->mDes_pitch);
  _pitchProportional->setValue(_dynamicControl->_pitchProportional);
  _pitchIntegral    ->setValue(_dynamicControl->_pitchIntegral);
  _pitchRate        ->setValue(_dynamicControl->_pitchRate);
  _p_dbg            ->setValue(_dynamicControl->_p_dbg);
  _elevBefLim       ->setValue(_dynamicControl->_elevBefLim);
  _elevator         ->setValue(_dynamicControl->_elevator);
  _thetaFF          ->setValue(_dynamicControl->_thetaFF);
  _xteInt           ->setValue(_dynamicControl->_xteIntegral);
  _kxte             ->setValue(_dynamicControl->_kxte);
  _dPsi             ->setValue(_dynamicControl->_dPsi);
#ifdef DOCKING
  _targetNorthing   ->setValue(_dynamicControl->_command.north);
  _targetEasting    ->setValue(_dynamicControl->_command.east);
  _newBearing       ->setValue(_dynamicControl->_command.newBearing);
  _newNorthing      ->setValue(_dynamicControl->_command.newNorthing);
  _newEasting       ->setValue(_dynamicControl->_command.newEasting);
  _first            ->setValue(_dynamicControl->_command.first);
#endif

#ifdef WALLFOLLOWING
  _beamIndx         ->setValue(_dynamicControl->_command.beamIndx);
  _rangeToWall      ->setValue(_dynamicControl->_command.rangeToWall);
  _wallBearing      ->setValue(_dynamicControl->_wallBearing);
  _hCmdDot          ->setValue(_dynamicControl->_headingTraj.xdot);
  _dNorthing        ->setValue(_dynamicControl->_dx);
  _dEasting         ->setValue(_dynamicControl->_dy);
#endif
}