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

SimulatorServer::SimulatorServer()
  : SimulatorIF_SK(),
    _input(SharedData::Write), 
    _output(SharedData::Read)
{
  VehicleConfigurationIF config("vehicleConfiguration");

  _inputData.vehicleMass = config.mass();
}


SimulatorServer::~SimulatorServer()
{
}


long SimulatorServer::dropWeight(float droppedMass)
{
  _inputData.vehicleMass -= droppedMass;

  _input.write(&_inputData);
  return 0;
}


long SimulatorServer::setPropSpeed(double propSpeed)
{ 
  _inputData.propSpeed = propSpeed;
  _input.write(&_inputData);
  return 0;
}


long SimulatorServer::setRudder(double rudder)
{ 
  _inputData.rudder = rudder;
  _input.write(&_inputData);
  return 0;
}


long SimulatorServer::setElevator(double elevator)
{ 
  _inputData.elevator = elevator;
  _input.write(&_inputData);
  return 0;
}


long SimulatorServer::setCurrent(double north, double east)
{
  _inputData.northCurrent = north;
  _inputData.eastCurrent = east;

  _input.write(&_inputData);
  return 0;
}


long SimulatorServer::rotationalVelocity(SimulatorIF::Vector velocity, 
					 SimulatorIF::Vector backDiff)
{
  _output.read(&_outputData);

  memcpy((void *)velocity, (void *)_outputData.rotationalVelocity, 
	 sizeof(SimulatorIF::Vector));

  memcpy((void *)backDiff, (void *)_outputData.rotationalBackDiff,
	 sizeof(SimulatorIF::Vector));

  return 0;
}


long SimulatorServer::translationalVelocity(SimulatorIF::Vector velocity, 
					    SimulatorIF::Vector backDiff)
{
  _output.read(&_outputData);

  memcpy((void *)velocity, (void *)_outputData.translationalVelocity, 
	 sizeof(SimulatorIF::Vector));

  memcpy((void *)backDiff, (void *)_outputData.translationalBackDiff,
	 sizeof(SimulatorIF::Vector));

  return 0;
}


long SimulatorServer::state(SimulatorIF::Vector position,
			    SimulatorIF::Vector positionRate,
			    SimulatorIF::Vector eulerAngles,
			    SimulatorIF::Vector rotationRate)
{
  _output.read(&_outputData);

  memcpy((void *)position, (void *)_outputData.position, 
	 sizeof(SimulatorIF::Vector));

  memcpy((void *)positionRate, (void *)_outputData.translationalVelocity,
	 sizeof(SimulatorIF::Vector));

  memcpy((void *)eulerAngles, (void *)_outputData.eulerAngles, 
	 sizeof(SimulatorIF::Vector));

  memcpy((void *)rotationRate, (void *)_outputData.rotationalVelocity,
	 sizeof(SimulatorIF::Vector));

  return 0;
}


long SimulatorServer::controlSurfaces(double *rudder, double *elevator)
{
  _output.read(&_outputData);

  *rudder = _outputData.rudder;
  *elevator = _outputData.elevator;

  return 0;
}


long SimulatorServer::force(SimulatorIF::Vector vector)
{
  _output.read(&_outputData);

  memcpy((void *)vector, (void *)_outputData.force, 
	 sizeof(SimulatorIF::Vector));

  return 0;
}


long SimulatorServer::torque(SimulatorIF::Vector vector)
{
  _output.read(&_outputData);

  memcpy((void *)vector, (void *)_outputData.torque,
	 sizeof(SimulatorIF::Vector));

  return 0;
}


long SimulatorServer::propSpeed(double *rpm)
{
  _output.read(&_outputData);

  *rpm = _outputData.propSpeed;

  return 0;
}


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

  pid_t pid;

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

    execlp(program, program, 0);

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

    perror(errorBuf);

    break;

  case -1:
    perror("SimulatorServer::spawnAuxTasks() - fork() failed");
    return -1;
  }

  return 0;
}