/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : KvhServer.cc                                                  */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
/*-----------------------------------------------------------------------*
  CLASS: KvhServer

  DESCRIPTION: Server for data from the Kvh instruments

  $Id: KvhServer.cc,v 1.8 2000/05/12 15:29:55 oreilly Exp $
  *-----------------------------------------------------------------------*/

#include <process.h>

#include "Syslog.h"
#include "KvhServer.h"

KvhServer::KvhServer(char *incDevName, char *compDevName) 
  : AhrsIF_SK()
{
  _incDevName = incDevName;
  _compDevName = compDevName;

  Boolean debug = False;

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

  try {
    // Create shared memory objects for communication with 
    // kvh driver tasks
    _inclinoOutput = new KvhInclinoOutput(SharedData::ReadWrite);
    _inclinoOutput->data.deviceReady = False;
    _inclinoOutput->write();

    _compassOutput = new KvhCompassOutput(SharedData::ReadWrite);
    _compassOutput->data.deviceReady = False;
    _compassOutput->write();
  }
  catch (...) {
    Syslog::write("KvhServer::KvhServer() - "
		  "output constructor failed\n");
	  
    return;
  }

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


KvhServer::~KvhServer()
{
  delete _compassOutput;
  delete _inclinoOutput;
}


void KvhServer::name(DeviceIF::Name name)
{
  strcpy(name, "Kvh");
}


void KvhServer::serialNumber(DeviceIF::Name number)
{
  strcpy(number, "XXX");
}


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


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


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


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


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


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


DeviceIF::Status KvhServer::getAttitude(AhrsIF::Attitude *attitude, 
					TimeIF::TimeSpec *sampleTime)
{
  Boolean debug = False;

  _inclinoOutput->read();
  if (_inclinoOutput->data.deviceReady) {
    dprintf("KvhServer::all() - inclinometer IS ready!");
    attitude->roll        = _inclinoOutput->data.roll;
    attitude->pitch       = _inclinoOutput->data.pitch;
    attitude->rollRate    = _inclinoOutput->data.rollRate;
    attitude->pitchRate   = _inclinoOutput->data.pitchRate;
  }
  else {
    dprintf("KvhServer::all() - inclinometer NOT ready!");
    attitude->roll = attitude->pitch = 
      attitude->rollRate = attitude->pitchRate = 0.;
  }

  _compassOutput->read();
  if (_compassOutput->data.deviceReady) {
    dprintf("KvhServer::all() - compass IS ready!");
    attitude->yaw    = _compassOutput->data.heading;
    attitude->yawRate = _compassOutput->data.headingRate;
  }
  else {
    dprintf("KvhServer::all() - compass NOT ready!");
    attitude->yaw = attitude->yawRate = 0.;
  }

  // Use time from compass???
  sampleTime->seconds = _compassOutput->data.updateTime.tv_sec;
  sampleTime->nanoSeconds = _compassOutput->data.updateTime.tv_nsec;

  if (_inclinoOutput->data.deviceReady && _compassOutput->data.deviceReady)
    return DeviceIF::Ok;
  else
    return DeviceIF::Error;
}



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


int KvhServer::spawnAuxTasks()
{
  char *programs[] = {"kvhInclinoDriver", "kvhCompassDriver"};
  char *devNames[] = {_incDevName, _compDevName};

  for (int i = 0; i < 2; i++) {

    char *program = programs[i];
    char *devName = devNames[i];

    char errorBuf[256];
    char buf[10];

    pid_t pid;

    switch ((pid = fork())) {
    
    case 0:
      // In child
      // Execute driver program
      execlp(program, program, "-dev", devName, 0);	

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

      perror(errorBuf);
      return -1;
      break;

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

  return 0;
}


void KvhServer::sensorReadyCallback(TaskInterface *taskInterface,
				    EventCode event)
{
  Boolean debug = False;
  dprintf("KvhServer::sensorReadyCallback()");
}


int KvhServer::getOptions(int argc, char **argv, 
			  char **incDevice, char **compDevice)
{
  Boolean error = False;
  Boolean gotInc = False;
  Boolean gotComp = False;

  for (int i = 1; i < argc; i++) {
    if (!strcmp(argv[i], "-inc") && i < argc - 1) {
      *incDevice = argv[++i];
      gotInc = True;
    }
    else if (!strcmp(argv[i], "-comp") && i < argc - 1) {
      *compDevice = argv[++i];
      gotComp = True;
    }
    else {
      Syslog::write("Invalid or incomplete option: %s\n", argv[i]);
      error = True;
    }
  }

  if (!gotInc) {
    Syslog::write("Inclinometer devicefile name not specified", argv[0]);
    error = True;
  }

  if (!gotComp) {
    Syslog::write("Compass devicefile name not specified", argv[0]);
    error = True;
  }

  if (error) {
    Syslog::write("Usage: %s -inc devname -comp devname\n", argv[0]);
    return -1;
  }
  else {
    return 0;
  }
}