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

  DESCRIPTION: Server for Octans DMU_AHRS Attitude Heading Reference
  System.

  AUTHOR: Aaron Marsh

  $Id: KearfottServer.cc,v 1.7 2001/07/19 06:01:55 hthomas Exp $
 *-----------------------------------------------------------------------*/

#include <process.h>

#include "SharedData.h"
#include "Kearfott.h"
#include "KearfottServer.h"
#include "KearfottOutput.h"
#include "Syslog.h"

KearfottServer::KearfottServer(SerialParameters *serialParams,
			       char *serverName)
  :  InsIF_SK(serverName)
{
  Syslog::write("creating if %s\n", serverName);
  _kearfottOutput = new KearfottOutput( SharedData::ReadWrite );
  _kearfottOutput->data.status = DeviceIF::Initializing;
  _kearfottOutput->write();

}

KearfottServer::~KearfottServer()
{
  delete _kearfottOutput;
}


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


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


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


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


DeviceIF::Status KearfottServer::status()
{
  _kearfottOutput->read();
  return _kearfottOutput->data.status;
}


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


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

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


DeviceIF::Status
KearfottServer::getInertialState(InsIF::InertialState *insState,
				 TimeIF::TimeSpec *sampleTime)
{

  _kearfottOutput->read();
  insState->lat = _kearfottOutput->data.navData.lat;
  insState->lon = _kearfottOutput->data.navData.lon;
  insState->depth = _kearfottOutput->data.navData.depth;
  insState->roll = _kearfottOutput->data.navData.roll;
  insState->pitch = _kearfottOutput->data.navData.pitch;
  insState->yaw = _kearfottOutput->data.navData.heading;
  insState->vbodyx = _kearfottOutput->data.navData.vbodyx;
  insState->vbodyy = _kearfottOutput->data.navData.vbodyy;
  insState->vbodyz = _kearfottOutput->data.navData.vbodyz;
  insState->accelx = _kearfottOutput->data.navData.accelx;
  insState->accely = _kearfottOutput->data.navData.accely;
  insState->accelz = _kearfottOutput->data.navData.accelz;
  insState->rollRate = _kearfottOutput->data.navData.prate;
  insState->pitchRate = _kearfottOutput->data.navData.qrate;
  insState->yawRate = _kearfottOutput->data.navData.rrate;

  return DeviceIF::Ok;
}


int KearfottServer::spawnAuxTasks()
{
  Boolean debug = True;
  char *program = "kearfott";
  char errorBuf[256];
  pid_t pid;

  dprintf("Preparing to fork\n");

  switch ((pid = fork())) {
    
  case 0:
    // In child
    // Execute server program
    dprintf("I'm the child...forking\n");

    execlp(program, program, "-v", "-dev", "//1/dev/ser8", 0);
    Syslog::write("Task::spawnServer() - execlp() of \"%s\" failed", program);

    break;

  case -1:
    Syslog::write("Task::spawnServer() - fork() failed\n");
    return -1;
  }

  return 0;
}