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

  DESCRIPTION: Server for Crossbow DMU_AHRS Attitude Heading Reference
  System.

  AUTHOR: Aaron Marsh

  $Id: CrossbowServer.cc,v 1.11 2001/06/04 10:58:31 hthomas Exp $
 *-----------------------------------------------------------------------*/

#include <process.h>

#include "SharedData.h"

#include "CrossbowServer.h"
#include "CrossbowOutput.h"
#include "CrossbowCommand.h"
#include "Syslog.h"

CrossbowServer::CrossbowServer(SerialParameters *serialParams,
			       char *serverName)
     :  CrossbowIF_SK(serverName)
{
  _output = new CrossbowOutput( SharedData::ReadWrite );
  _command = new CrossbowCommand( SharedData::ReadWrite );
  _serialParams = serialParams;
}

CrossbowServer::~CrossbowServer()
{
  delete _output;
  delete _command;
}


void CrossbowServer::name(DeviceIF::Name name)
{
  sprintf(name, "Crossbow");
}


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


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


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


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


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


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


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


DeviceIF::Status CrossbowServer::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 CrossbowServer::magneticVector(double *magX, 
						double *magY, 
						double *magZ, 
						TimeIF::TimeSpec *sampleTime)
{
  _output->read();
  *magX = _output->data.magX;
  *magY = _output->data.magY;
  *magZ = _output->data.magZ;
  sampleTime->seconds = _output->data.sampleTime.tv_sec;
  sampleTime->nanoSeconds = _output->data.sampleTime.tv_nsec;
  return _output->data.status;
}


DeviceIF::Status CrossbowServer::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 CrossbowServer::magneticCompass()
{
  return True;
}


DeviceIF::Status CrossbowServer::all(AhrsIF::Attitude *attitude, 
				     double *accelX, 
				     double *accelY, 
				     double *accelZ,
				     double *magX, 
				     double *magY, 
				     double *magZ, 
				     double *temperature, 
				     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;
  *magX = _output->data.magX;
  *magY = _output->data.magY;
  *magZ = _output->data.magZ;
  *temperature = _output->data.temperature;
  sampleTime->seconds = _output->data.sampleTime.tv_sec;
  sampleTime->nanoSeconds = _output->data.sampleTime.tv_nsec;
  return _output->data.status;
}




int CrossbowServer::spawnAuxTasks()
{
  Boolean debug = True;
  char *program = "crossbow";
  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;
}