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

NavigationServer::NavigationServer()
  : NavigationIF_SK()
{
  Boolean debug = False;

  dprintf("*** NavigationServer::NavigationServer()...\n");

  try {
    _output = new NavigationOutput(SharedData::Read);
  }
  catch (...) {
    Syslog::write("NavigationServer::NavigationServer() - "
		  "output constructor failed\n");

    return;
  }

  try {
    WorkSiteIF workSite("workSite");
    _utmZone = workSite.utmZone();
  }
  catch (...) {
    Syslog::write("NavigationServer::NavigationServer() - "
		  "couldn't connect to WorkSiteIF\n");

    return;
  }

  dprintf("*** NavigationServer::NavigationServer() - DONE\n");
}


NavigationServer::~NavigationServer()
{
  delete _output;
}


void NavigationServer::state(NavigationIF::Position *position, 
			     NavigationIF::Attitude *attitude)
{
  Boolean debug = True;

  _output->read(&_state);

  memcpy((void *)position, (void *)&_state.position, 
	 sizeof(NavigationIF::Position));

  memcpy((void *)attitude, (void *)&_state.attitude,
	 sizeof(NavigationIF::Attitude));
}


void NavigationServer::heading(double *heading)
{
  _output->read(&_state);
  *heading = _state.attitude.yaw;
}


void NavigationServer::location(double *latitude, double *longitude, 
				double *depth, 
				double *altitude)
{
  _output->read(&_state);

  // Convert x/y to latitude/longitude
  NavUtils::utmToGeo(_state.position.x, _state.position.y, _utmZone,
		     latitude, longitude);

  *depth = _state.position.z;
  *altitude = _state.position.altitude;
}


float NavigationServer::speed()
{
  _output->read(&_state);
  return sqrt(pow(_state.position.xRate, 2.) + 
	      pow(_state.position.yRate, 2.) + 
	      pow(_state.position.zRate, 2.));
}


double NavigationServer::course()
{
  return 0.;
}


double NavigationServer::depth()
{
  _output->read(&_state);
  return _state.position.z;
}


double NavigationServer::altitude()
{
  _output->read(&_state);
  return _state.position.altitude;
}


double NavigationServer::pitch()
{
  _output->read(&_state);
  return _state.attitude.pitch;
}


void NavigationServer::setMagneticVariation(double angle)
{
}


double NavigationServer::magneticVariation()
{
  return 0.;
}


void NavigationServer::mode(NavigationIF::Mode *mode)
{
}


void NavigationServer::getLogData(NavigationIF::LogData *logData)
{
  _output->read(&_state);

  logData->x = _state.position.x;
  logData->y = _state.position.y;
  logData->z = _state.position.z;
}


int NavigationServer::spawnAuxTasks()
{
  char *program = "navigation";
  char errorBuf[256];

  pid_t pid;

  switch ((pid = fork())) {
    
  case 0:
    // In child
    // Execute server program
    execlp(program, program, 0);

    sprintf(errorBuf, 
	    "Task::spawnServer() - execlp() of \"%s\" failed", program);

    perror(errorBuf);

    break;

  case -1:
    perror("Task::spawnServer() - fork() failed");
    return -1;
  }

  return 0;
}


void NavigationServer::deviceStatusCallback(TaskInterface *taskInterface,
					    EventCode eventCode)
{
  Boolean debug = True;
  dprintf("NavigationServer - Got event %d from device\n", 
	  eventCode);
}