////////////////////////////////////////////////////////////////////////////////
//
// PURPOSE:  Log Class.
// AUTHOR:   Rob McEwen.
// DATE:     07/4/17
// COMMENTS: 
//
////////////////////////////////////////////////////////////////////////////////
//
#include "TerrainAidLog.h"
#include "TerrainAidDriver.h"

TerrainAidLog::TerrainAidLog(TerrainAid *terrainAid, 
		     DataLog::FileFormat fileFormat)
  : DataLogWriter(TerrainAidLogName, fileFormat, AutoTimeStamp)
{
  _terrainAid= terrainAid;
  setMnemonic("ta.data");

  //addField((_navTimeS = new IntegerData("ta.navSec")));
  //addField((_navTimeN = new IntegerData("ta.navNanoSec")));
  addField((_navTime = new DoubleData("ta.navTime")));
  addField((_navTimeInit = new DoubleData("ta.navTime0")));
  _navTime->setAsciiFormat("%13.3f");           //Necessary for large UT values.
  _navTimeInit->setAsciiFormat("%13.3f");          //Necessary for large UT values.
  addField((_navN = new DoubleData("ta.navN")));
  addField((_navE = new DoubleData("ta.navE")));
  _navN->setAsciiFormat("%13.2f");             //Necessary for large UTM values.
  _navE->setAsciiFormat("%13.2f");
  addField((_depth = new DoubleData("ta.depth")));
  addField((_roll  = new DoubleData("ta.roll")));
  addField((_pitch = new DoubleData("ta.pitch")));
  addField((_yaw   = new DoubleData("ta.yaw")));

  addField((_omega[0] = new DoubleData("ta.omega(1)")));
  addField((_omega[1] = new DoubleData("ta.omega(2)")));
  addField((_omega[2] = new DoubleData("ta.omega(3)")));

  //addField((_dvlTimeS = new IntegerData("ta.dvlSec")));
  //addField((_dvlTimeN = new IntegerData("ta.dvlNanoSec")));
  addField((_dvlTime = new DoubleData("ta.dvlTime")));
  _dvlTime->setAsciiFormat("%13.3f");          //Necessary for large UT values.
  addField((_beam[0]  = new DoubleData("ta.beam(1)")));
  addField((_beam[1]  = new DoubleData("ta.beam(2)")));
  addField((_beam[2]  = new DoubleData("ta.beam(3)")));
  addField((_beam[3]  = new DoubleData("ta.beam(4)")));
  
  addField((_dvlVelocity[0]  = new DoubleData("ta.dvlVelocity(1)")));
  addField((_dvlVelocity[1]  = new DoubleData("ta.dvlVelocity(2)")));
  addField((_dvlVelocity[2]  = new DoubleData("ta.dvlVelocity(3)")));
  addField((_dvlValid = new IntegerData("ta.dvlValid")));
  addField((_gpsValid = new IntegerData("ta.gpsValid")));
  addField((_bottomLock = new IntegerData("ta.bottomLock")));

  addField((_dvlStatus[0] = new IntegerData("ta.dvlStatus(1)")));
  addField((_dvlStatus[1] = new IntegerData("ta.dvlStatus(2)")));
  addField((_dvlStatus[2] = new IntegerData("ta.dvlStatus(3)")));
  addField((_dvlStatus[3] = new IntegerData("ta.dvlStatus(4)")));

  addField((_mleN = new DoubleData("ta.mleN")));
  addField((_mleE = new DoubleData("ta.mleE")));
  _mleN->setAsciiFormat("%13.2f");             //Necessary for large UTM values.
  _mleE->setAsciiFormat("%13.2f");
  addField((_mleZ = new DoubleData("ta.mleZ")));
  addField((_mlePhi = new DoubleData("ta.mlePhi")));
  addField((_mleTheta = new DoubleData("ta.mleTheta")));
  addField((_mlePsi = new DoubleData("ta.mlePsi")));
  addField((_mleVarN = new DoubleData("ta.mleVarN")));
  addField((_mleVarE = new DoubleData("ta.mleVarE")));
  addField((_mleVarNE = new DoubleData("ta.mleVarNE")));
  addField((_mleVarZ = new DoubleData("ta.mleVarZ")));
  addField((_mleTime = new DoubleData("ta.mleTime")));

  addField((_mmseN = new DoubleData("ta.mmseN")));
  addField((_mmseE = new DoubleData("ta.mmseE")));
  _mmseN->setAsciiFormat("%13.2f");              //Necessary for large UTM values.
  _mmseE->setAsciiFormat("%13.2f");
  addField((_mmseZ = new DoubleData("ta.mmseZ")));
  addField((_mmsePhi = new DoubleData("ta.mmsePhi")));
  addField((_mmseTheta = new DoubleData("ta.mmseTheta")));
  addField((_mmsePsi = new DoubleData("ta.mmsePsi")));
  addField((_mmseVarN = new DoubleData("ta.mmseVarN")));
  addField((_mmseVarE = new DoubleData("ta.mmseVarE")));
  addField((_mmseVarNE = new DoubleData("ta.mmseVarNE")));
  addField((_mmseVarZ = new DoubleData("ta.mmseVarZ")));
  addField((_mmseVarPhi = new DoubleData("ta.mmseVarPhi")));
  addField((_mmseVarTheta = new DoubleData("ta.mmseVarTheta")));
  addField((_mmseVarPsi = new DoubleData("ta.mmseVarPsi")));
  addField((_mmseTime = new DoubleData("ta.mmseTime")));

  addField((_numReinits = new IntegerData("ta.numReinits")));
  addField((_filterState = new IntegerData("ta.filterState")));
}


TerrainAidLog::~TerrainAidLog()
{
   //delete _navTimeS;
   //delete _navTimeN;
   delete _navTime;
   delete _navTimeInit;
   delete _navN;
   delete _navE;
   delete _depth;
   delete _roll;
   delete _pitch;
   delete _yaw;

   delete _mleN;
   delete _mleE;

   //delete _dvlTimeS;
   //delete _dvlTimeN;
   delete _dvlTime;

   delete _beam[0];
   delete _beam[1];
   delete _beam[2];
   delete _beam[3];

   delete _omega[0];
   delete _omega[1];
   delete _omega[2];

   delete _dvlVelocity[0];
   delete _dvlVelocity[1];
   delete _dvlVelocity[2];
   delete _dvlValid;
   delete _gpsValid;
   delete _bottomLock;

   delete _dvlStatus[0];
   delete _dvlStatus[1];
   delete _dvlStatus[2];
   delete _dvlStatus[3];

   delete _mleN;
   delete _mleE;
   delete _mleZ;
   delete _mlePhi;
   delete _mleTheta;
   delete _mlePsi;
   delete _mleVarN;
   delete _mleVarE;
   delete _mleVarNE;
   delete _mleVarZ;
   delete _mleTime;

   delete _mmseN;
   delete _mmseE;
   delete _mmseZ;
   delete _mmsePhi;
   delete _mmseTheta;
   delete _mmsePsi;
   delete _mmseVarN;
   delete _mmseVarE;
   delete _mmseVarNE;
   delete _mmseVarZ;
   delete _mmseVarPhi;
   delete _mmseVarTheta;
   delete _mmseVarPsi;
   delete _mmseTime;

   delete _numReinits;
   delete _filterState;
}


void TerrainAidLog::setFields()
{

   //_navTimeS->setValue(_terrainAid->_position.updateTime.seconds);
   //_navTimeN->setValue(_terrainAid->_position.updateTime.nanoSeconds);
   _navTime-> setValue(_terrainAid->_navData->time);
   _navTimeInit-> setValue(_terrainAid->_navTime0);
   _navN->    setValue(_terrainAid->_navData->x);
   _navE->    setValue(_terrainAid->_navData->y);
   _depth->   setValue(_terrainAid->_navData->z);
   _roll->    setValue(_terrainAid->_navData->phi);
   _pitch->   setValue(_terrainAid->_navData->theta);
   _yaw->     setValue(_terrainAid->_navData->psi);

   _mleN->    setValue(_terrainAid->_mleEst->x);
   _mleE->    setValue(_terrainAid->_mleEst->y);

   //_dvlTimeS->setValue(_terrainAid->_dvlIFData.sampleTime.seconds);
   //_dvlTimeN->setValue(_terrainAid->_dvlIFData.sampleTime.nanoSeconds);
   _dvlTime-> setValue(_terrainAid->_dvlData->time);

   _beam[0]-> setValue(_terrainAid->_dvlData->ranges[0]);
   _beam[1]-> setValue(_terrainAid->_dvlData->ranges[1]);
   _beam[2]-> setValue(_terrainAid->_dvlData->ranges[2]);
   _beam[3]-> setValue(_terrainAid->_dvlData->ranges[3]);

   _omega[0]-> setValue(_terrainAid->_navData->wx);
   _omega[1]-> setValue(_terrainAid->_navData->wy);
   _omega[2]-> setValue(_terrainAid->_navData->wz);

   _dvlVelocity[0]-> setValue(_terrainAid->_navData->vx);
   _dvlVelocity[1]-> setValue(_terrainAid->_navData->vy);
   _dvlVelocity[2]-> setValue(_terrainAid->_navData->vz);
   _dvlValid-> setValue(_terrainAid->_navData->dvlValid);
   _gpsValid-> setValue(_terrainAid->_navData->gpsValid);
   if( _terrainAid->_navData->bottomLock )
      _bottomLock-> setValue(1);
   else
      _bottomLock-> setValue(0);
   
   _dvlStatus[0]-> setValue(int(_terrainAid->_dvlData->measStatus[0]));
   _dvlStatus[1]-> setValue(int(_terrainAid->_dvlData->measStatus[1]));
   _dvlStatus[2]-> setValue(int(_terrainAid->_dvlData->measStatus[2]));
   _dvlStatus[3]-> setValue(int(_terrainAid->_dvlData->measStatus[3]));

   _mleN->    setValue(_terrainAid->_mleEst->x);
   _mleE->    setValue(_terrainAid->_mleEst->y);
   _mleZ->    setValue(_terrainAid->_mleEst->z);
   _mlePhi->  setValue(_terrainAid->_mleEst->phi);
   _mleTheta->  setValue(_terrainAid->_mleEst->theta);
   _mlePsi->  setValue(_terrainAid->_mleEst->psi);
   _mleVarN-> setValue(_terrainAid->_mleEst->covariance[0]);
   _mleVarE-> setValue(_terrainAid->_mleEst->covariance[2]);
   _mleVarNE-> setValue(_terrainAid->_mleEst->covariance[1]);
   _mleVarZ-> setValue(_terrainAid->_mleEst->covariance[5]);
   _mleTime-> setValue(_terrainAid->_mleEst->time);

   _mmseN->    setValue(_terrainAid->_mmseEst->x);
   _mmseE->    setValue(_terrainAid->_mmseEst->y);
   _mmseZ->    setValue(_terrainAid->_mmseEst->z);
   _mmsePhi->  setValue(_terrainAid->_mmseEst->phi);
   _mmseTheta->  setValue(_terrainAid->_mmseEst->theta);
   _mmsePsi->  setValue(_terrainAid->_mmseEst->psi);
   _mmseVarN-> setValue(_terrainAid->_mmseEst->covariance[0]);
   _mmseVarE-> setValue(_terrainAid->_mmseEst->covariance[2]);
   _mmseVarNE-> setValue(_terrainAid->mmseEst->covariance[1]);
   _mmseVarZ-> setValue(_terrainAid->_mmseEst->covariance[5]);
   _mmseVarPhi-> setValue(_terrainAid->_mmseEst->covariance[9]);
   _mmseVarTheta-> setValue(_terrainAid->_mmseEst->covariance[14]);
   _mmseVarPsi-> setValue(_terrainAid->_mmseEst->covariance[20]);
   _mmseTime-> setValue(_terrainAid->_mmseEst->time);

   _numReinits-> setValue(_terrainAid->_numReinits);
   _filterState-> setValue(_terrainAid->_filterState);
}