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

SimulatedInclinometer::SimulatedInclinometer()
  : InclinometerIF_SK()
{
  _simulator = new SimulatorIF("simulator");
}


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


long SimulatedInclinometer::attitude(double *roll, 
				     double *pitch, 
				     double *yaw, 
				     double *rollRate, 
				     double *pitchRate, 
				     double *yawRate, 
				     short *quality, long *sampleTime)
{
  SimulatorIF::Vector position, eulerAngles;
  SimulatorIF::Vector positionRate, rotationRate;

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

  *roll = eulerAngles[0];
  *pitch = eulerAngles[1];
  *yaw = eulerAngles[2];

  *rollRate = rotationRate[0];
  *pitchRate = rotationRate[1];
  *yawRate = rotationRate[2];

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

  return 0;
}



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