/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : TailConeServer.cc                                             */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <process.h>
#include "TailConeServer.h"


TailConeServer::TailConeServer(SerialParameters *params) 
  : DoradoTailConeIF_SK(TailConeIFServerName)
{
  _driverSerialParams = params;
  _input = new TailConeInput(SharedData::Write);
  _output = new TailConeOutput(SharedData::Read);

  setElevator(0.0); setRudder(0.0);
  setDeadman(250.0);
  enableThruster();
}



TailConeServer::~TailConeServer() 
{

  command(0.0, 0.0, 0.0);  

  delete _input;
  delete _output;
}


void TailConeServer::name(DeviceIF::Name name)
{
  strcpy(name, "DoradoTailCone");
}


void TailConeServer::serialNumber(DeviceIF::Name number)
{
  strcpy(number, "XXX");
}


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


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


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


DeviceIF::Status TailConeServer::initialize()
{
  triggerEvent(TailConeIF::Initializing);
  return DeviceIF::Initializing;
}


DeviceIF::Status TailConeServer::setPropellerSpeed(double radsPerSec)
{
  if (!_thrusterEnabled) 
    return DeviceIF::Error;

  _input->data.propOmega = radsPerSec;
  _input->write();
  return DeviceIF::Ok;
}

 
DeviceIF::Status TailConeServer::setElevator(double angle)
{
  _input->data.elevator = angle;
  _input->write();
  return DeviceIF::Ok;
}

 
DeviceIF::Status TailConeServer::setRudder(double angle)
{
  _input->data.rudder = angle;
  _input->write();
  return DeviceIF::Ok;
}


DeviceIF::Status TailConeServer::health(long *statusWord, TimeIF::TimeSpec *time)
{
  return DeviceIF::Ok;
}

DeviceIF::Status TailConeServer::setRudderCounts(short counts)
{
  return DeviceIF::Ok;
}

DeviceIF::Status TailConeServer::setElevatorCounts(short counts)
{
  return DeviceIF::Ok;
}

DeviceIF::Status TailConeServer::setMotorCounts(long counts)
{
  return DeviceIF::Ok;
}


DeviceIF::Status TailConeServer::command(double propOmega, double elevator, 
					 double rudder)
{
  if (!_thrusterEnabled) 
    return DeviceIF::Error;

  _input->data.elevator = elevator;
  _input->data.rudder = rudder;
  _input->data.propOmega = propOmega;
  _input->write();
  return DeviceIF::Ok;
}


DeviceIF::Status TailConeServer::setDeadman(short milliseconds)
{
  _input->data.deadmanMillisec = milliseconds;
  _input->write();
  triggerEvent(TailConeIF::SetDeadman);
  return DeviceIF::Ok;
}


DeviceIF::Status TailConeServer::deadman(short *milliseconds)
{
  _output->read();
  *milliseconds = _output->data.deadmanMillisec;
  return _output->data.status;
}

 
void TailConeServer::enableThruster()
{
  _thrusterEnabled = True;
}


void TailConeServer::disableThruster()
{
  _thrusterEnabled = False;
}


Boolean TailConeServer::thrusterEnabled()
{
  return _thrusterEnabled;
}


TailConeIF::Type TailConeServer::tailConeType()
{
  return TailConeIF::Dorado;
}


DeviceIF::Status TailConeServer::propellerSpeed(double *omega_M, 
						TimeIF::TimeSpec *time)
{
  _output->read();
  *omega_M = _output->data.propOmega;

  memcpy((void *)time, (void *)&_output->data.sampleTime, 
	 sizeof(TimeIF::TimeSpec));

  return _output->data.status;
}


DeviceIF::Status TailConeServer::elevator(double *angle, 
					  TimeIF::TimeSpec *time)
{
  _output->read();
  *angle = _output->data.elevator;

  memcpy((void *)time, (void *)&_output->data.sampleTime, 
	 sizeof(TimeIF::TimeSpec));

  return _output->data.status;
}


DeviceIF::Status TailConeServer::rudder(double *angle, TimeIF::TimeSpec *time)
{
  _output->read();
  *angle = _output->data.rudder;

  memcpy((void *)time, (void *)&_output->data.sampleTime, 
	 sizeof(TimeIF::TimeSpec));

  return _output->data.status;
}


DeviceIF::Status TailConeServer::actual(double *omega_M, 
					double *elevator, 
					double *rudder, 
					TimeIF::TimeSpec *time)
{
  _output->read();
  *omega_M = _output->data.propOmega;
  *elevator = _output->data.elevator;
  *rudder = _output->data.rudder;

  memcpy((void *)time, (void *)&_output->data.sampleTime, 
	 sizeof(TimeIF::TimeSpec));

  return _output->data.status;
}



DeviceIF::Status TailConeServer::currents(short *elevatorCurrent,
					  short *rudderCurrent,
					  short *propCurrent1,
					  short *propCurrent2,
					  TimeIF::TimeSpec *time)
{
  _output->read();
  *elevatorCurrent = _output->data.elevatorCurrent;
  *rudderCurrent = _output->data.rudderCurrent;
  *propCurrent1 = _output->data.propCurrent1;
  *propCurrent2 = _output->data.propCurrent2;

  memcpy((void *)time, (void *)&_output->data.sampleTime, 
	 sizeof(TimeIF::TimeSpec));

  return _output->data.status;
}

DeviceIF::Status TailConeServer::readTelemValues(double *omega_M,
						 double *elevator, 
						 double *rudder,
						 TimeIF::TimeSpec *time)
{
  return DeviceIF::Ok;
}


int TailConeServer::spawnAuxTasks()
{
  char *program = "newTailConeDriver";
  char params[256];
  char errorBuf[256];

  switch ((_driverPid = fork())) {
    
  case 0:
    // In child
    // Execute server program
    execlp(program, program, "-dev", _driverSerialParams->string(), 0);

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

    perror(errorBuf);

    break;

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

  return 0;
}