/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : SimulatedGps.cc                                               */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <process.h>
#include <time.h>
#include "SimulatedGps.h"
#include "Syslog.h"
#include "SimulatorIF.h"
#include "WorkSiteIF.h"
#include "GpsIF.h"
#include "GpsUtils.h"
#include "NavUtils.h"

SimulatedGps::SimulatedGps()
  : GpsIF_SK(), LoggableGps()
{
  _simulator = new SimulatorIF("simulator");
  _log = new GpsLog(this, DataLog::BinaryFormat);

  WorkSiteIF workSite("workSite");
  _utmZone = workSite.utmZone();
}


SimulatedGps::~SimulatedGps()
{
  delete _simulator;
  delete _log;
}


void SimulatedGps::name(DeviceIF::Name name)
{
  // Dummy implementation for now
  strcpy(name, "Simulated GPS");
}


void SimulatedGps::serialNumber(DeviceIF::Name number)
{
  // Dummy implementation for now
  strcpy(number, "XXX");
}


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


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


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


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


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


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



DeviceIF::Status SimulatedGps::getFix(GpsIF::Fix *fix)
{

  SimulatorIF::Vector position, euler;
  SimulatorIF::Vector vel_B_N_B, omega_B_N_B;
  Boolean debug = False;
  //
  // Extract the "truth" position from the simulated vehicle state:
  //
  _simulator->state(position, vel_B_N_B, euler, omega_B_N_B);

  _fix.sampleTime.seconds = time(0);
  _fix.sampleTime.nanoSeconds = 0;
  //
  // Simulate valid gps reception when the vehicle is near the surface:
  //
  if( position[2] < .5 )  {
    _fix.quality = GpsIF::NonDifferential;

    NavUtils::utmToGeo(position[0], position[1], _utmZone,
		       &_fix.latitude, &_fix.longitude);
    _fix.ewHemisphere  = GpsIF::Eastern;
    if( _fix.longitude < 0. ) 
    {
      _fix.ewHemisphere  = GpsIF::Western;
      _fix.longitude *= -1.;
    }

    _fix.nsHemisphere  = GpsIF::Northern;
    if( _fix.latitude < 0. ) 
    {
      _fix.nsHemisphere  = GpsIF::Southern;
      _fix.latitude *= -1.;
    }
    
  }
  else
    _fix.quality = GpsIF::Invalid;

  //
  // This code doesn't simulate the following parameters.  Pass out zeros.
  //
  _fix.nSatellites   = 0;
  _fix.hdop          = 0.;
  _fix.altitude      = 0.;
  _fix.geoidHeight   = 0.;
  _fix.dgpsDataAge   = 0;
  _fix.dgpsStationId = 0;
  
  memcpy((void *)fix, (void *)&_fix, sizeof(GpsIF::Fix));

  _log->write();

  return DeviceIF::Ok;
}


long SimulatedGps::sampleTime() 
{
  return _fix.sampleTime.seconds;
}


double SimulatedGps::latitude()
{
  return _fix.latitude;
}


char SimulatedGps::nsHemisphere()
{
  return GpsUtils::hemisphere(_fix.nsHemisphere);
}


double SimulatedGps::longitude()
{
  return _fix.longitude;
}


char SimulatedGps::ewHemisphere()
{
  return GpsUtils::hemisphere(_fix.ewHemisphere);
}

 
GpsIF::Quality SimulatedGps::quality()
{
  return _fix.quality;
}


short SimulatedGps::nSatellites()
{
  return _fix.nSatellites;
}


double SimulatedGps::hdop()
{
  return _fix.hdop;
}


double SimulatedGps::altitude()
{
  return _fix.altitude;
}


double SimulatedGps::geoidHeight()
{
  return _fix.geoidHeight;
}


short SimulatedGps::dgpsDataAge()
{
  return _fix.dgpsDataAge;
}


short SimulatedGps::dgpsStationId()
{
  return _fix.dgpsStationId;
}



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