/****************************************************************************/
/* 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"

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

  setMnemonic("dataDC");

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

  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(_speed             = new DoubleData("speedCmd"));
  addField(_propOmega         = new DoubleData("omegaCmd"));

  addField(mDepth             = new DoubleData("cDepth"));
  addField(mT_depth           = new DoubleData("tDepth"));
  addField(mDepthIntegral     = new DoubleData("dIntegral"));
  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"));

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

  _dynamicControl = dynamicControl;
}


DynamicControlLog::~DynamicControlLog()
{
  delete _myTimeStamp;

  delete _heading;
  delete _rudder;
  delete mT_Psi;
  delete mPsiProp;
  delete mPsiInt;
  delete mPsiRate;

  delete _speed;
  delete _propOmega;

  delete mDepth;
  delete mT_depth;
  delete mDepthIntegral;
  delete mcTheta;
  delete _pitchProportional;
  delete _pitchIntegral;
  delete _pitchRate;    
  delete _p_dbg;        
  delete _elevBefLim;
  delete _elevator;
}


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

  _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);
  mDepthIntegral    ->setValue(_dynamicControl->_depthIntegral);
  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);
}