/****************************************************************************/
/* Copyright (c) 2004 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : SimulatedKearfott.cc                                          */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 12/17/2004                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
/*-----------------------------------------------------------------------*
 *-----------------------------------------------------------------------*/

#include <process.h>

#include "SharedData.h"
#include "SimulatedKearfott.h"
#include "Syslog.h"

SimulatedKearfott::SimulatedKearfott()
  :  InsIF_SK("SimulatedKearfott")
{

}

SimulatedKearfott::~SimulatedKearfott()
{
}


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


DeviceIF::Status SimulatedKearfott::initialize()
{
  try {
    _simServer = new SimulatorIF("simulator");
  }
  catch (Exception e) {
    Syslog::write("SimulatedKearfott - could open IF to SImulator");
    return DeviceIF::Error;
  }

  return DeviceIF::Ok;
}


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


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


DeviceIF::Status SimulatedKearfott::status()
{
  return DeviceIF::Ok;
}


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


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

void SimulatedKearfott::serialNumber(DeviceIF::Name number)
{
  strncpy(number,"1", sizeof(number)-1);
}


DeviceIF::Status
SimulatedKearfott::getInertialState(InsIF::InertialState *insState,
				 TimeIF::TimeSpec *sampleTime)
{
  if (!_simServer)
    return DeviceIF::Error;

  SimulatorIF::Vector positions, positionRates, eulers, rotations;
  _simServer->state ( positions, positionRates, eulers, rotations);

#if 0
  insState->accelx = _kearfottOutput->data.navData.accelx;
  insState->accely = _kearfottOutput->data.navData.accely;
  insState->accelz = _kearfottOutput->data.navData.accelz;

  insState->lat   = _kearfottOutput->data.navData.lat;
  insState->lon   = _kearfottOutput->data.navData.lon;
#endif
  insState->depth = positions[2];

  insState->roll  = eulers[0];
  insState->pitch = eulers[1];
  insState->yaw   = eulers[2];

  insState->vbodyx = positionRates[0];
  insState->vbodyy = positionRates[1];
  insState->vbodyz = positionRates[2];

  insState->rollRate  = rotations[0];
  insState->pitchRate = rotations[1];
  insState->yawRate   = rotations[2];

  return DeviceIF::Ok;
}


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