/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : SimulationLog.cc                                              */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
//
// PURPOSE: Log data from the Altex/Odyssey simulator object.
// AUTHORS: Oreilly, McEwen
// DATE:    99/11/30
// NOTES:   1) The log points (or variables) below are not prefaced with
//          an underscore in this class, although they are class variables.
//
#include "SimulationLog.h"
#include "Simulator.h"
#include "Syslog.h"
//
// The "NoAutoTimeStamp" below turns off time-tagging.  Here, we instead use 
// simulation time (non real-time).  The reason for this is that motion() is
// called 10 times in sucession without any pause (in Simulator.cc).  
// Then, the loop waits until the next control system (.2 sec ) sampling 
// period occurs in "real" time before starting the 10 calls to motion() again.
// 
//
// 00/8/30: Changed it back to time-tagging to observe the jitter and cpu load.
// The call to this has been moved outside of motion().  
//
SimulationLog::SimulationLog(Simulator *simulator, 
                             DataLog::FileFormat fileFormat)
  : DataLogWriter(SimulationLogName, fileFormat, AutoTimeStamp)
  //  : DataLogWriter(SimulationLogName, fileFormat, NoAutoTimeStamp)
{

  setMnemonic("dataSim");
  //
  // I'll avoid the exact Odyssey naming convention here so that 
  // original Odyssey data won't be confused with DorOdyssey data when
  // they're both present on the Matlab stack.
  //
  addField( pos_N_x          = new DoubleData("sPos_x") );
  addField( pos_N_y          = new DoubleData("sPos_y") );
  addField( pos_N_z          = new DoubleData("sPos_z") );
  pos_N_x->setAsciiFormat("%12.1f");     //Necessary for large UTM values.
  pos_N_y->setAsciiFormat("%12.1f");

  addField( phi              = new DoubleData("sPhi") );
  addField( theta            = new DoubleData("sTheta") );
  addField( psi              = new DoubleData("sPsi") );

  addField( vel_Bo_N_B_x     = new DoubleData("sVel_u") );
  addField( vel_Bo_N_B_y     = new DoubleData("sVel_v") );
  addField( vel_Bo_N_B_z     = new DoubleData("sVel_w") );

  addField( omega_B_N_B_x    = new DoubleData("sOmega_p") );
  addField( omega_B_N_B_y    = new DoubleData("sOmega_q") );
  addField( omega_B_N_B_z    = new DoubleData("sOmega_r") );

  addField( deltaRCmd        = new DoubleData("scDeltaR") );
  addField( deltaECmd        = new DoubleData("scDeltaE") );

  addField( deltaR           = new DoubleData("sDeltaR") );
  addField( deltaE           = new DoubleData("sDeltaE") );

  addField( M1_1             = new DoubleData("sM1_1") );
  addField( M2_1             = new DoubleData("sM2_1") );
  addField( M3_1             = new DoubleData("sM3_1") );
  addField( M4_1             = new DoubleData("sM4_1") );
                          
  addField( M1_2             = new DoubleData("sM1_2") );
  addField( M2_2             = new DoubleData("sM2_2") );
  addField( M3_2             = new DoubleData("sM3_2") );
  addField( M4_2             = new DoubleData("sM4_2") );

  addField( thrAmps          = new DoubleData("cThrAmps") );
  addField( omega_P          = new DoubleData("sOmega_P") );

  _simulator = simulator;
}


SimulationLog::~SimulationLog()
{
  delete pos_N_x;
  delete pos_N_y;
  delete pos_N_z;

  delete phi;
  delete theta;
  delete psi;

  delete vel_Bo_N_B_x;
  delete vel_Bo_N_B_y;
  delete vel_Bo_N_B_z;

  delete omega_B_N_B_x;
  delete omega_B_N_B_y;
  delete omega_B_N_B_z;

  delete deltaRCmd;
  delete deltaECmd;

  delete deltaR;
  delete deltaE;

  delete M1_1;
  delete M2_1;
  delete M3_1;
  delete M4_1;

  delete M1_2;
  delete M2_2;
  delete M3_2;
  delete M4_2;

  delete thrAmps;
  delete omega_P;
}


void SimulationLog::setFields()
{
  //
  // timeTag() sets up the logging of (non real-time) simulation time.
  //
  //timeTag()     ->setValue( _simulator->_simTime );
  pos_N_x	->setValue( _simulator->_output.position[0]);
  pos_N_y	->setValue( _simulator->_output.position[1]);
  pos_N_z	->setValue( _simulator->_output.position[2]);
                                               
  phi		->setValue( _simulator->_output.eulerAngles[0]);
  theta		->setValue( _simulator->_output.eulerAngles[1]);
  psi		->setValue( _simulator->_output.eulerAngles[2]);

  vel_Bo_N_B_x	->setValue( _simulator->_output.translationalVelocity[0]);
  vel_Bo_N_B_y	->setValue( _simulator->_output.translationalVelocity[1]);
  vel_Bo_N_B_z	->setValue( _simulator->_output.translationalVelocity[2]);

  omega_B_N_B_x	->setValue( _simulator->_output.rotationalVelocity[0]);
  omega_B_N_B_y	->setValue( _simulator->_output.rotationalVelocity[1]);
  omega_B_N_B_z	->setValue( _simulator->_output.rotationalVelocity[2]);

  deltaRCmd	->setValue( _simulator->_output.rudderCmd);
  deltaECmd	->setValue( _simulator->_output.elevatorCmd);

  deltaR	->setValue( _simulator->_output.rudder);
  deltaE	->setValue( _simulator->_output.elevator);

  M1_1		->setValue( _simulator->_output.M1_1);
  M2_1		->setValue( _simulator->_output.M2_1);
  M3_1		->setValue( _simulator->_output.M3_1);
  M4_1		->setValue( _simulator->_output.M4_1);

  M1_2		->setValue( _simulator->_output.M1_2);
  M2_2		->setValue( _simulator->_output.M2_2);
  M3_2		->setValue( _simulator->_output.M3_2);
  M4_2		->setValue( _simulator->_output.M4_2);

  thrAmps	->setValue( _simulator->thrAmps);
  omega_P       ->setValue( _simulator->omega_P);
}