/****************************************************************************/
/* 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 "DynamicControl.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")));
#endif

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

  _dynamicControl = dynamicControl;
    _dynamicControlInstance=NULL;
}

DynamicControlLog::DynamicControlLog(DynamicControl *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")));
#endif
    
    _myTimeStamp->setAsciiFormat("%.2f");
    
    _dynamicControlInstance = dynamicControl;
    _dynamicControl=NULL;
}

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;
#endif
}

void DynamicControlLog::setFields()
{
    Boolean debug=True;
    if (_dynamicControl) {
        _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);
#endif

    }else if(_dynamicControlInstance){
        _myTimeStamp->setValue(_dynamicControlInstance->_navLoopTimeDC);
        
        _xte        ->setValue(_dynamicControlInstance->_xte);
        _xteWay     ->setValue(_dynamicControlInstance->_xteWay);
        _heading    ->setValue(_dynamicControlInstance->_command.horizontal);
        _rudder     ->setValue(_dynamicControlInstance->_rudder);
        mT_Psi      ->setValue(_dynamicControlInstance->mT_Psi);
        mPsiProp    ->setValue(_dynamicControlInstance->mPsiProp);
        mPsiInt     ->setValue(_dynamicControlInstance->_headingIntegral);
        mPsiRate    ->setValue(_dynamicControlInstance->mPsiRate);
        
        _speed      ->setValue(_dynamicControlInstance->_command.speed);
        _propOmega  ->setValue(_dynamicControlInstance->_propOmega);
        
        mDepth            ->setValue(_dynamicControlInstance->_command.vertical);
        mT_depth          ->setValue(_dynamicControlInstance->_des_depth);
        _depthError       ->setValue(_dynamicControlInstance->_err_depth);
        mDepthIntegral    ->setValue(_dynamicControlInstance->_depthIntegral);
        mDepthRateTerm    ->setValue(_dynamicControlInstance->_kdZdot);
        mcTheta           ->setValue(_dynamicControlInstance->mDes_pitch);
        _pitchProportional->setValue(_dynamicControlInstance->_pitchProportional);
        _pitchIntegral    ->setValue(_dynamicControlInstance->_pitchIntegral);
        _pitchRate        ->setValue(_dynamicControlInstance->_pitchRate);
        _p_dbg            ->setValue(_dynamicControlInstance->_p_dbg);
        _elevBefLim       ->setValue(_dynamicControlInstance->_elevBefLim);
        _elevator         ->setValue(_dynamicControlInstance->_elevator);
        _thetaFF          ->setValue(_dynamicControlInstance->_thetaFF);
        _xteInt           ->setValue(_dynamicControlInstance->_xteIntegral);
        _kxte             ->setValue(_dynamicControlInstance->_kxte);
        _dPsi             ->setValue(_dynamicControlInstance->_dPsi);
#ifdef DOCKING
        _targetNorthing   ->setValue(_dynamicControlInstance->_command.north);
        _targetEasting    ->setValue(_dynamicControlInstance->_command.east);
        _newBearing       ->setValue(_dynamicControlInstance->_command.newBearing);
        _newNorthing      ->setValue(_dynamicControlInstance->_command.newNorthing);
        _newEasting       ->setValue(_dynamicControlInstance->_command.newEasting);
        _first            ->setValue(_dynamicControlInstance->_command.first);
#endif
        
#ifdef WALLFOLLOWING
        _beamIndx         ->setValue(_dynamicControlInstance->_command.beamIndx);
        _rangeToWall      ->setValue(_dynamicControlInstance->_command.rangeToWall);
        _wallBearing      ->setValue(_dynamicControlInstance->_wallBearing);
        _hCmdDot          ->setValue(_dynamicControlInstance->_headingTraj.xdot);
#endif

    }else{
        dprintf("DynamicControlLog::setFields - ERROR both dynamicControl references are NULL\n");
    }
}