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

SimulatedTailCone::SimulatedTailCone()
  : TailConeIF_SK()
{
  _rudder = _elevator = _propSpeed = 0.;
  _simulator = new SimulatorIF("simulator");
  enableThruster();
}


SimulatedTailCone::~SimulatedTailCone()
{
  delete _simulator;
}

void SimulatedTailCone::name(DeviceIF::Name name)
{
    strcpy(name, "Simulated TailCone");
}

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

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


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

DeviceIF::Status SimulatedTailCone::status()
{
  return DeviceIF::Ok;
}

TailConeIF::Type SimulatedTailCone::tailConeType()
{
  return TailConeIF::Simulated;
}

DeviceIF::Status SimulatedTailCone::setPropellerSpeed(double speed)
{
  _propSpeed = speed;
  _simulator->command(_propSpeed, _elevator, _rudder);
  return DeviceIF::Ok;
}

DeviceIF::Status SimulatedTailCone::setElevator(double angle)
{
  _elevator = angle;
  _simulator->command(_propSpeed, _elevator, _rudder);
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedTailCone::setRudder(double angle)
{
  _rudder = angle;
  _simulator->command(_propSpeed, _elevator, _rudder);
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedTailCone::command(double speed, double elevator, double rudder)
{
  _rudder = rudder;
  _elevator = elevator;
  _propSpeed = speed;
  _simulator->command(_propSpeed, _elevator, _rudder);
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedTailCone::initialize()
{
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedTailCone::setDeadman(short milliseconds)
{
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedTailCone::deadman(short *milliseconds)
{
  *milliseconds = 1000;
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedTailCone::propellerSpeed(double *omega, TimeIF::TimeSpec *sampleTime)
{
  _simulator->propOmega(omega);
  Time::gettime(sampleTime);
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedTailCone::elevator(double *angle,  TimeIF::TimeSpec *sampleTime)
{
  double rudder;
  _simulator->controlSurfaces(&rudder, angle);
  Time::gettime(sampleTime);
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedTailCone::rudder(double *angle, TimeIF::TimeSpec *sampleTime)
{
  double elevator;
  _simulator->controlSurfaces(angle, &elevator);
  Time::gettime(sampleTime);
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedTailCone::actual(double *omega, double *elevator, 
			       double *rudder, 
			       TimeIF::TimeSpec *sampleTime)
{
  _simulator->propOmega(omega);
  _simulator->controlSurfaces(rudder, elevator);
  Time::gettime(sampleTime);
  return DeviceIF::Ok;
}


int SimulatedTailCone::spawnAuxTasks()
{
  return 0;
}


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


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


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


DeviceIF::Status SimulatedTailCone::readTelemValues(double *omega, double *elevator, double *rudder,
   TimeIF::TimeSpec *sampleTime) 
{
  _simulator->propOmega(omega);
  _simulator->controlSurfaces(rudder, elevator);

  Time::gettime(sampleTime);
  return DeviceIF::Ok;
}