#include "TailCone.h"
#include "VehicleConfigurationIF.h"

TailCone::TailCone(const char *name, NavSensors *sensors, int maxBad)
  : NavSensor(name, sensors, maxBad)
{
  _tailConeIF = 0;
  _waterSpeed = 0.;

  // Get prop-pitch; used to compute vehicle speed from prop omega
  VehicleConfigurationIF config("config");
  _propPitch = config.propPitch();
}



TaskInterface *TailCone::createTaskIF(int timeout)
{
  try {
    _tailConeIF = new TailConeIF(name(), timeout);
  }
  catch (...) {
    _tailConeIF = 0;
  }

  return _tailConeIF;
}


DeviceIF::Status TailCone::readTaskIF(Boolean *valid, 
				      TimeIF::TimeSpec *sampleTime)
{
  *valid = True;

  float motorCurrent, actuatorCurrent;
  double propOmega, elevator, rudder;

  DeviceIF::Status devStatus = 
    _tailConeIF->propellerSpeed(&propOmega, sampleTime);

  _waterSpeed = propOmega * _propPitch;

  return devStatus;
}



double TailCone::waterSpeed()
{
  return _waterSpeed;
}