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

#define LOG_LBL

NavigationLog::NavigationLog(Navigation *navigation) 
  : DataLogWriter(NavigationLogName, DataLog::BinaryFormat, AutoTimeStamp) 
{
  setMnemonic("dataNav");

  addField((_x = new DoubleData("mPos_x")));
  addField((_y = new DoubleData("mPos_y")));
  addField((_z = new DoubleData("mDepth")));
  _x->setAsciiFormat("%13.2f");              //Necessary for large UTM values.
  _y->setAsciiFormat("%13.2f");

  addField((_gpsNorth = new DoubleData("mGpsNorth")));
  addField((_gpsEast  = new DoubleData("mGpsEast")));
  addField((_gpsValid = new IntegerData("mGpsValid")));
  _gpsNorth->setAsciiFormat("%13.2f");       //Necessary for large UTM values.
  _gpsEast ->setAsciiFormat("%13.2f");       //Necessary for large UTM values.

  addField((_roll  = new DoubleData("mPhi")));
  addField((_pitch = new DoubleData("mTheta")));
  addField((_yaw   = new DoubleData("mPsi")));

  addField((_omega_B_x = new DoubleData("mOmega_x")));
  addField((_omega_B_y = new DoubleData("mOmega_y")));
  addField((_omega_B_z = new DoubleData("mOmega_z")));

  addField((mMAltimeterRange = new DoubleData("mPsaRange")));
  addField((mMAltitude = new DoubleData("mAltitude")));

  addField((mMWaterSpeed = new DoubleData("mWaterSpeed")));

#ifdef LOG_LBL
  addField((_nfix = new DoubleData("nfix")));
  addField((_efix = new DoubleData("efix")));
  _nfix->setAsciiFormat("%13.2f");       //Necessary for large UTM values.
  _efix->setAsciiFormat("%13.2f");       //Necessary for large UTM values.
  addField((_filter_north  = new DoubleData("filter_north" )));
  addField((_filter_east   = new DoubleData("filter_east"  )));
  _nfix->setAsciiFormat("%13.2f");       //Necessary for large UTM values.
  _efix->setAsciiFormat("%13.2f");       //Necessary for large UTM values.
  _filter_north->setAsciiFormat("%13.2f");       //Necessary for large UTM values.
  _filter_east ->setAsciiFormat("%13.2f");       //Necessary for large UTM values.
  addField((_filter_depth  = new DoubleData("filter_depth" )));
  addField((_north_current = new DoubleData("north_current")));
  addField((_east_current  = new DoubleData("east_current" )));
  addField((_speed_bias    = new DoubleData("speed_bias"   )));
  addField((_heading_bias  = new DoubleData("heading_bias" )));
#endif

  _navigation = navigation;
}


NavigationLog::~NavigationLog()
{
  delete _x;
  delete _y;
  delete _z;
  delete _gpsNorth;
  delete _gpsEast;
  delete _gpsValid;
  delete _roll;
  delete _pitch;
  delete _yaw;
  delete _omega_B_x;
  delete _omega_B_y;
  delete _omega_B_z;
  delete mMAltimeterRange;
  delete mMAltitude;
  delete mMWaterSpeed;
#ifdef LOG_LBL
  delete _nfix;
  delete _efix;
  delete _filter_north;
  delete _filter_east;
  delete _filter_depth;
  delete _north_current;
  delete _east_current;
  delete _speed_bias;
  delete _heading_bias;
#endif
}


void NavigationLog::setFields()
{
  Boolean debug = True;

  // Set x, y, and z values
  _x->setValue(_navigation->_state.position.x);
  _y->setValue(_navigation->_state.position.y);
  _z->setValue(_navigation->_state.position.z);

  _gpsNorth->setValue(_navigation->_northing);
  _gpsEast ->setValue(_navigation->_easting);
  _gpsValid->setValue( (int) _navigation->_gpsValid);

  _roll->   setValue(_navigation->_state.attitude.roll);
  _pitch->  setValue(_navigation->_state.attitude.pitch);
  _yaw->    setValue(_navigation->_state.attitude.yaw);

  _omega_B_x->   setValue(_navigation->_state.attitude.omega_B_x);
  _omega_B_y->   setValue(_navigation->_state.attitude.omega_B_y);
  _omega_B_z->   setValue(_navigation->_state.attitude.omega_B_z);

  mMAltimeterRange->setValue(_navigation->_altimeter->range());
  mMAltitude->      setValue(_navigation->_state.position.altitude);
  mMWaterSpeed->    setValue(_navigation->_waterSpeed);

#ifdef LOG_LBL
  _nfix->setValue(_navigation->_nfix);
  _efix->setValue(_navigation->_efix);
  _filter_north->  setValue(_navigation->_filter_north);
  _filter_east->   setValue(_navigation->_filter_east);
  _filter_depth->  setValue(_navigation->_filter_depth);
  _north_current-> setValue(_navigation->_north_current);
  _east_current->  setValue(_navigation->_east_current);
  _speed_bias->    setValue(_navigation->_speed_bias);
  _heading_bias->  setValue(_navigation->_heading_bias);
#endif
}