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

double SimulatedCompass::_earthBx = 0.275;
double SimulatedCompass::_earthBy = 0.0;
double SimulatedCompass::_earthBz = 0.476;

SimulatedCompass::SimulatedCompass()
  : CompassIF_SK()
{
  try {
    _simulator = new SimulatorIF("simulator");
  }
  catch (SharedObject::Error e) {
    Syslog::write("SimulatedCompass::SimulatedCompass() - %s", e.msg);
  }
  catch (...) {
    Syslog::write("SimulatedCompass::SimulatedCompass() - "
		  "caught exception creating simIF...");
  }
}


SimulatedCompass::~SimulatedCompass()
{
  delete _simulator;
}


long SimulatedCompass::heading(double *hdg, long *sampleTime, short *quality)
{
  SimulatorIF::Vector position, eulerAngles;
  SimulatorIF::Vector positionRate, rotationRate;

  _simulator->state(position, positionRate, eulerAngles, rotationRate);

  *hdg = eulerAngles[2];

  *sampleTime = time(0);
  *quality = 0;

  return 0;
}


#if 0
long SimulatedCompass::heading(double *hdg, long *sampleTime, short *quality)
{
  SimulatorIF::Vector position, eulerAngles;
  SimulatorIF::Vector positionRate, rotationRate;

  _simulator->state(position, positionRate, eulerAngles, rotationRate);

  double phi = eulerAngles[0];
  double theta = eulerAngles[1];
  double psi = eulerAngles[2];
  
  double matrix[3][3];
  matrix[X][X] = cos(psi) * cos(theta);
  matrix[X][Y] = cos(theta) * sin(psi);
  matrix[X][Z] = -sin(theta);
  
  matrix[Y][X] = -(cos(phi) *sin(psi)) + cos(psi) * sin(phi) * sin(theta);
  matrix[Y][Y] = cos(phi) * cos(psi) + sin(phi) * sin(psi) * sin(theta);
  matrix[Y][Z] = cos(theta) * sin(phi); 
  
  double mx = 
    matrix[X][X] * _earthBx + matrix[X][Y] * _earthBy + 
    matrix[X][Z] * _earthBz;

  double my = 
    matrix[Y][X] * _earthBx + matrix[Y][Y] * _earthBy + 
    matrix[Y][Z] * _earthBz;

  if (mx > 0.0 && my <= 0.0 ) 
    *hdg = -atan(my/mx);

  if (mx <= 0.0 && my < 0.0) 
    *hdg = atan(mx/my) + M_PI / 2.0;

  if (mx < 0.0 && my >= 0.0) 
    *hdg = -atan(my/mx) + M_PI;

  if (mx >= 0.0 && my > 0.0) 
    *hdg = atan(mx/my) + 3.0* M_PI / 2.0;

  *sampleTime = time(0);
  *quality = 0;

  return 0;
}
#endif   // 0 - don't use


long SimulatedCompass::calibrate()
{
  return 0;
}


int SimulatedCompass::spawnAuxTasks()
{
  return 0;
}