#include "RangeFinder.h"


RangeFinder::RangeFinder(const char *name, NavSensors *sensors, int maxBad)
  : NavSensor(name, sensors, maxBad)
{
  _echoReceived = False;
  _range = 0.;
  _xzMountAngle = 0.;
  _maxMeasurableRange = 0.;
}


double RangeFinder::range()
{
  return _range;
}


double RangeFinder::xzMountAngle()
{
  return _xzMountAngle;
}


double RangeFinder::maxMeasurableRange()
{
  return _maxMeasurableRange;
}


TaskInterface *RangeFinder::createTaskIF(int timeout)
{
  try {
    _rangeFinderIF = new RangeFinderIF(name(), timeout);
    _rangeFinderIF->maxRange(&_maxMeasurableRange);
    _rangeFinderIF->xzAngle(&_xzMountAngle);
  }
  catch (...) {
    _rangeFinderIF = 0;
  }

  return _rangeFinderIF;
}


DeviceIF::Status RangeFinder::readTaskIF(Boolean *valid,
					 TimeIF::TimeSpec *sampleTime)
{
  DeviceIF::Status status = 
    _rangeFinderIF->range(&_echoReceived, &_range, sampleTime);


  if (!_echoReceived) {
    _range = _maxMeasurableRange;
  }

  *valid = _echoReceived;

  return status;
}