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

  DESCRIPTION: Server for data from the CrossbowAHRS instruments

  $Id: CrossbowAHRSServer.cc,v 1.3 2000/02/07 23:52:40 pean Exp $
  *-----------------------------------------------------------------------*/

#include <process.h>

#include "Syslog.h"
#include "CrossbowAHRSServer.h"

CrossbowAHRSServer::CrossbowAHRSServer() : AHRSIF_SK()
{
     Boolean debug = False;

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

     try {
	  // Create shared memory objects for communication with 
	  // CrossbowAHRS driver task
	  _output = new AHRSOutput(SharedData::ReadWrite);
     }
     catch (...) {
	  Syslog::write("CrossbowAHRSServer::CrossbowAHRSServer() - "
			"output constructor failed\n");
	  
    return;
  }

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


CrossbowAHRSServer::~CrossbowAHRSServer()
{
}


long CrossbowAHRSServer::attitude(double *roll, double *pitch, double *heading, 
			 long *time)
{
     _output->read( &_outputData );
     *roll    = _outputData.roll;
     *pitch   = _outputData.pitch;
     *heading = _outputData.heading;
     *time    = _outputData.updateTime.tv_sec;

     return OK;
}

long CrossbowAHRSServer::roll(double *roll, double *rollRate, 
		     long *time)
{
     _output->read( &_outputData );
     *roll     = _outputData.roll;
     *rollRate = _outputData.rollRate;
     *time     = _outputData.updateTime.tv_sec;

     return OK;
}

long CrossbowAHRSServer::pitch(double *pitch, double *pitchRate,
			long *time )
{
     _output->read( &_outputData );
     *pitch     = _outputData.pitch;
     *pitchRate = _outputData.pitchRate;
     *time      = _outputData.updateTime.tv_sec;

     return OK;
}

long CrossbowAHRSServer::heading(double *heading, double *headingRate,
			  long *time )
{
     _output->read( &_outputData );
     *heading     = _outputData.heading;
     *headingRate = _outputData.headingRate;
     *time        = _outputData.updateTime.tv_sec;

     return OK;
}

long CrossbowAHRSServer::angRates(double *rollRate, 
			   double *pitchRate, 
			   double *headingRate,
			   long *time )
{
     _output->read( &_outputData );
     *rollRate    = _outputData.rollRate;
     *pitchRate   = _outputData.pitchRate;
     *headingRate = _outputData.headingRate;
     *time        = _outputData.updateTime.tv_sec;

     return OK;
}


long CrossbowAHRSServer::all(double *roll, 
		    double *pitch, 
		    double *heading, 
		    double *rollRate, 
		    double *pitchRate, 
		    double *headingRate,
		    long *time )
{
     _output->read( &_outputData );
     *roll        = _outputData.roll;
     *pitch       = _outputData.pitch;
     *heading     = _outputData.heading;
     *rollRate    = _outputData.rollRate;
     *pitchRate   = _outputData.pitchRate;
     *headingRate = _outputData.headingRate;
     *time        = _outputData.updateTime.tv_sec;

     return OK;
}

int CrossbowAHRSServer::spawnAuxTasks()
{
  char *program = "crossbow";
  char errorBuf[256];
  char buf[10];

  pid_t pid;

  // I should really look for to see if 'program' is already running,
  // and only start it if it isn't.
  


  switch ((pid = fork())) {
    
  case 0:
    // In child
    // Execute CrossbowAHRS driver program

    execlp(program, program, 0);	

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

    perror(errorBuf);

    break;

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

  return 0;
}