/****************************************************************************/
/* 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: OctansServer.cc,v 1.3 2001/06/24 20:39:38 hthomas Exp $
 *-----------------------------------------------------------------------*/

#include <process.h>

#include "SharedData.h"

#include "OctansServer.h"
#include "OctansOutput.h"
#include "Syslog.h"

OctansServer::OctansServer(SerialParameters *serialParams, char *IFServerName)
     :  OctansIF_SK(IFServerName)
{
  _output = new OctansOutput( SharedData::ReadWrite );
  _output->data.status = DeviceIF::Initializing;
  _output->write();
  _serialParams = serialParams;
}

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


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


void OctansServer::serialNumber(DeviceIF::Name number)
{
  _output->read();
  strncpy(number, _output->data.serialNumber, sizeof(number)-1);
}


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


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


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


DeviceIF::Status OctansServer::status()
{
  _output->read();
  return _output->data.status;
}


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


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


DeviceIF::Status OctansServer::acceleration(double *accelX, 
					      double *accelY, 
					      double *accelZ, 
					      TimeIF::TimeSpec *sampleTime)
{
  _output->read();
  *accelX = _output->data.accelX;
  *accelY = _output->data.accelY;
  *accelZ = _output->data.accelZ;
  sampleTime->seconds = _output->data.sampleTime.tv_sec;
  sampleTime->nanoSeconds = _output->data.sampleTime.tv_nsec;
  return _output->data.status;
}

DeviceIF::Status OctansServer::getAttitude(AhrsIF::Attitude *attitude, 
					     TimeIF::TimeSpec *sampleTime)
{
  _output->read();
  attitude->roll = _output->data.roll;
  attitude->pitch = _output->data.pitch;
  attitude->yaw = _output->data.yaw;
  attitude->rollRate = _output->data.rollRate;
  attitude->pitchRate = _output->data.pitchRate;
  attitude->yawRate = _output->data.yawRate;
  sampleTime->seconds = _output->data.sampleTime.tv_sec;
  sampleTime->nanoSeconds = _output->data.sampleTime.tv_nsec;
  return _output->data.status;
}


Boolean OctansServer::magneticCompass()
{
  return False;
}


DeviceIF::Status OctansServer::all(AhrsIF::Attitude *attitude, 
				     double *accelX, 
				     double *accelY, 
				     double *accelZ,
				     long *status,
				     TimeIF::TimeSpec *sampleTime)
{
  _output->read();
  attitude->roll = _output->data.roll;
  attitude->pitch = _output->data.pitch;
  attitude->yaw = _output->data.yaw;
  attitude->rollRate = _output->data.rollRate;
  attitude->pitchRate = _output->data.pitchRate;
  attitude->yawRate = _output->data.yawRate;
  *accelX = _output->data.accelX;
  *accelY = _output->data.accelY;
  *accelZ = _output->data.accelZ;
  *status = _output->data.octansStatus;
  sampleTime->seconds = _output->data.sampleTime.tv_sec;
  sampleTime->nanoSeconds = _output->data.sampleTime.tv_nsec;
  return _output->data.status;
}




int OctansServer::spawnAuxTasks()
{
  Boolean debug = True;
  char *program = "octans";
  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, "-serial", _serialParams->string(), 0);
    Syslog::write("Task::spawnServer() - execlp() of \"%s\" failed", program);

    break;

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

  return 0;
}