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


SimulatedCrossbow::SimulatedCrossbow()
  : AhrsIF_SK()
{

  VehicleConfigurationIF vehicleConfig("vehicleConfig");


  _simulator = new SimulatorIF("simulator");

}


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

void SimulatedCrossbow::name(DeviceIF::Name name)
{
  sprintf(name, "Crossbow");
}


void SimulatedCrossbow::serialNumber(DeviceIF::Name number)
{
  strcpy(number, "XBow serialNO");
}


DeviceIF::Status SimulatedCrossbow::initialize()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedCrossbow::powerOn()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedCrossbow::powerOff()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedCrossbow::status()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedCrossbow::dataLoggingOn()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedCrossbow::dataLoggingOff()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedCrossbow::acceleration(double *accelX, 
						 double *accelY, 
						 double *accelZ, 
						 TimeIF::TimeSpec *sampleTime)
{
  *accelX = 0.;
  *accelY = 0.;
  *accelZ = 0.;
  sampleTime->seconds = time(0);
  sampleTime->nanoSeconds = 0;
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedCrossbow::magneticVector(double *magX, 
						   double *magY, 
						   double *magZ, 
						   TimeIF::TimeSpec 
						   *sampleTime)
{
  *magX = 0.;
  *magY = 0.;
  *magZ = 0.;
  sampleTime->seconds = time(0);
  sampleTime->nanoSeconds = 0;
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedCrossbow::getAttitude(AhrsIF::Attitude *attitude, 
						TimeIF::TimeSpec *sampleTime)
{
  getState();
  attitude->roll = _euler[0];
  attitude->pitch = _euler[1];
  attitude->yaw = _euler[2];
  attitude->rollRate = _omega[0];
  attitude->pitchRate = _omega[1];
  attitude->yawRate = _omega[2];

  sampleTime->seconds = time(0);
  sampleTime->nanoSeconds = 0;
  return DeviceIF::Ok;
}


Boolean SimulatedCrossbow::magneticCompass()
{
  return True;
}


DeviceIF::Status SimulatedCrossbow::all(AhrsIF::Attitude *attitude, 
					double *accelX, 
					double *accelY, 
					double *accelZ,
					double *magX, 
					double *magY, 
					double *magZ, 
					double *temperature, 
					TimeIF::TimeSpec *sampleTime)
{
  getState();
  attitude->roll = _euler[0];
  attitude->pitch = _euler[1];
  attitude->yaw = _euler[2];
  attitude->rollRate = _omega[0];
  attitude->pitchRate = _omega[1];
  attitude->yawRate = _omega[2];
  *accelX = 0.;
  *accelY = 0.;
  *accelZ = 0.;
  *magX = 0.;
  *magY = 0.;
  *magZ = 0.;
  *temperature = 35.;
  sampleTime->seconds = time(0);
  sampleTime->nanoSeconds = 0;
  return DeviceIF::Ok;
}


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

     return OK;
}


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