/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : SimulatedKvh.cc                                               */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
//////////////////////////////////////////////////////////////////////
//
// PURPOSE:  Simulate the Kvh Compass and Inclinometer
// AUTHOR:   Marsh, Following O'Reilly's Template
//
// $Id: SimulatedKvh.cc,v 1.5 2001/06/02 21:33:32 hthomas Exp $
//
//////////////////////////////////////////////////////////////////////
//
#include <time.h>
#include "SimulatedKvh.h"
#include "MathP.h"
#include "Syslog.h"
#include "VehicleConfigurationIF.h"


SimulatedKvh::SimulatedKvh()
  : AHRSIF_SK()
{

  VehicleConfigurationIF vehicleConfig("vehicleConfig");

  Boolean debug = True;

  _simulator = new SimulatorIF("simulator");

  dprintf("SimulatedKvh::SimulatedKvh() - triggerEvent()");
  triggerEvent(AHRSIF::Initialized);
}


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

long SimulatedKvh::attitude(double *roll, double *pitch, double *heading, 
		      long *time)
{
     getState();

     *roll       = _euler[0];
     *pitch      = _euler[1];
     *heading    = _euler[2];

     *time = 0.0;

     return OK;
}

long SimulatedKvh::roll(double *roll, double *rollRate, long *time)
{
     getState();

     *roll       = _euler[0];
     *rollRate   = _omega[0];

     *time = 0.0;

     return AHRSIF::Ok;
}

long SimulatedKvh::pitch(double *pitch, double *pitchRate, long *time)
{
     getState();

     *pitch      = _euler[1];
     *pitchRate  = _omega[1];

     *time = 0.0;

     return AHRSIF::Ok;
}

long SimulatedKvh::heading(double *heading, double *headingRate, long *time)
{
     getState();

     *heading     = _euler[2];
     *headingRate = _omega[2];

     *time = 0.0;

     return AHRSIF::Ok;
}

long SimulatedKvh::angRates(double *rollRate, double *pitchRate, 
		      double *headingRate, long *time)
{
     getState();

     *rollRate      = _omega[0];
     *pitchRate     = _omega[1];
     *headingRate   = _omega[2];

     *time = 0.0;

     return AHRSIF::Ok;
}

long SimulatedKvh::all(double *roll, double *pitch, double *heading, 
		 double *rollRate, double *pitchRate, double *headingRate, 
		 long *time)
{
     getState();

     *roll       = _euler[0];
     *pitch      = _euler[1];
     *heading    = _euler[2];

     *rollRate      = _omega[0];
     *pitchRate     = _omega[1];
     *headingRate   = _omega[2];

     *time = 0.0;

     return AHRSIF::Ok;
}

long SimulatedKvh::getState()
{
     Boolean debug = False;
     
     double altitude;
     //
     // Extract the vehicle state from the simulation.
     //
     _simulator->state( _position, _velocity, _euler, _omega );

     return AHRSIF::Ok;
}


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