/****************************************************************************/
/* 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"
#include "NavUtils.h"
#include "WorkSiteIF.h"

SimulatedKearfott::SimulatedKearfott()
  :  InsIF_SK(InsIFServerName)
{
   Syslog::write("SimulatedKearfott:: Constructor.");
  try {
    _simulator = new SimulatorIF("simulator");
  }
  catch (Exception e) {
    Syslog::write("SimulatedKearfott - couldn't open IF to SImulator");
    //return DeviceIF::Error;
  }

  WorkSiteIF workSite("workSite");
  _utmZone = workSite.utmZone();
  Syslog::write("SimulatedKearfott::Setting utm zone to %d",_utmZone);

}

SimulatedKearfott::~SimulatedKearfott()
{
}


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


DeviceIF::Status SimulatedKearfott::initialize()
{
  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 (!_simulator)
  {
     Syslog::write("SimulatedKearfott::getInertialState - Can't open simServer.");
     return DeviceIF::Error;
  }

  SimulatorIF::Vector position, velocity, euler, angularVelocity;
  _simulator->state ( position, velocity, euler, angularVelocity);

//  Syslog::write("SimulatedKearfott::  x = %.2f, y = %.2f, z = %.2f",
//		position[0],
//		position[1],
//		position[2]);

  NavUtils::utmToGeo(position[0], position[1], _utmZone,
		     &(insState->lat), &(insState->lon));

  insState->depth = position[2];

  insState->roll  = euler[0];
  insState->pitch = euler[1];
  insState->yaw   = euler[2];
  //
  // Mislabeled in InsIF::InertialState. These can be coordinatized in either body or
  // NED, depending on an EEPROM switch in the Kearfott.  It is apparently
  // set to NED.
  insState->vbodyx = velocity[0];
  insState->vbodyy = velocity[1];
  insState->vbodyz = velocity[2];

  insState->accelx = 0.;
  insState->accely = 0.;
  insState->accelz = 0.;
  //
  // Also mislabeled in InsIF::InertialState.
  insState->rollRate  = angularVelocity[0];
  insState->pitchRate = angularVelocity[1];
  insState->yawRate   = angularVelocity[2];

  insState->status = 0x40 << 24;


  Time::gettime( &_sampleTime );
  sampleTime->seconds = _sampleTime.seconds;
  sampleTime->nanoSeconds = _sampleTime.nanoSeconds;

  return DeviceIF::Ok;
}


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