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

  DESCRIPTION: Server for data from the AHRS instruments

  $Id: AHRSServer.cc,v 1.9 2001/06/02 21:33:37 hthomas Exp $
  *-----------------------------------------------------------------------*/

#include <process.h>

#include "SemaphoreP.h"
#include "Syslog.h"
#include "AHRSServer.h"

AHRSServer::AHRSServer(const char *driverProgram) : AHRSIF_SK()
{
     _driverProgram = strdup(driverProgram);

     Boolean debug = False;

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

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

     _outputSem = new Semaphore( AHRSSemName, 1 );

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


AHRSServer::~AHRSServer()
{
  free((void *)_driverProgram);
  delete _output;
  delete _outputSem;
}


long AHRSServer::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 AHRSServer::roll(double *roll, double *rollRate, 
		     long *time)
{
     _output->read( &_outputData );
     *roll     = _outputData.roll;
     *rollRate = _outputData.rollRate;
     *time     = _outputData.updateTime.tv_sec;

     return OK;
}

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

     return OK;
}

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

     return OK;
}

long AHRSServer::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 AHRSServer::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 AHRSServer::spawnAuxTasks()
{
  const char *program = _driverProgram;
  char errorBuf[256];
  char buf[10];

  pid_t pid;

  switch ((pid = fork())) {
    
  case 0:
    // In child
    // Execute AHRS 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;
}