#include "Usbl.h"

Usbl::Usbl(const char *name, NavSensors *sensors, int maxBad)
  : NavSensor(name, sensors, maxBad)
{
  _usblIF = 0;
  _usblStatus = UsblIF::Undefined;
}


TaskInterface *Usbl::createTaskIF(int timeout)
{
  try {
    _usblIF = new UsblIF(name(), timeout);
  }
  catch (...) {
    _usblIF = 0;
  }

  return _usblIF;
}


DeviceIF::Status Usbl::readTaskIF(Boolean *valid, 
				 TimeIF::TimeSpec *sampleTime)
{
   float rangef, x, y, z;
      
   DeviceIF::Status status;
   status = _usblIF->get_range_data( &rangef, &x, &y, &z, &_tat,
				    &_sampleTime, &_usblStatus);
   _rangeMag  = (double) rangef;
   _rangeB[0] = (double) x;
   _rangeB[1] = (double) y;
   _rangeB[2] = (double) z;

   sampleTime->seconds = _sampleTime.seconds;
   sampleTime->nanoSeconds = _sampleTime.nanoSeconds;
   //
   // We will call the Usbl output valid when it gets new data, and the
   // output will remain valid until either the next ping times out, or the
   // Usbl returns with invalid data.  If we were to set "valid" to false as
   // soon as the data was not new, DynamicControl would revert to Waypoint
   // (instead of remaining in Homing) for the intermediate samples between
   // Usbl hits.
   //
  if (_usblStatus == UsblIF::AllGood)
  {
     *valid = True;
     _rangeGood = True;
  }
  else if (_usblStatus == UsblIF::RangeOnly)
  {
     *valid = False;
     _rangeGood = True;
  }
  else
  {
     *valid = False;
     _rangeGood = False;
  }

  return status;
}