#include "DepthSensor.h"
#include "Syslog.h"
#include <math.h>     //So we can use fabs().

DepthSensor::DepthSensor(const char *name, NavSensors *sensors, int maxBad, double badDepthThresh)
  : _badDepthThresh(badDepthThresh), NavSensor(name, sensors, maxBad)
{
  _depthSensorIF = 0;
  _depth = 0.;
}


double DepthSensor::depth()
{
  return _depth;
}


TaskInterface *DepthSensor::createTaskIF(int timeout)
{
  try {
    _depthSensorIF = new DepthSensorIF(name(), timeout);
  }
  catch (...) {
    _depthSensorIF = 0;
  }

  return _depthSensorIF;
}

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

  //
  // Get the current depth reading
  //
  double currentDepth = 0.;
  DeviceIF::Status status =  _depthSensorIF->depth(&currentDepth, sampleTime);

  if (status == DeviceIF::Initializing) {
    printf("DepthSensor::Initializing\n");
    return status;
  }

  //
  // Filter depth outliers. 
  //
  // If the difference of the current depth and the last good depth reading
  // is more than badDepthThres, it is a "bad" depth reading,
  // and should be ignored unless we've had _maxConsecutiveBad 
  // bad readings in a row.
  //
  dprintf("DepthSensor(Navigation) Before currentDepth = %f\n",currentDepth);
  dprintf("DepthSensor(Navigation) Before _depth       = %f\n",_depth);
  if (fabs(currentDepth - _depth) > _badDepthThresh )
  {
    //
    // After so many bad readings, accept the depth reading
    //
    if (++_badCount > _maxConsecutiveBad)
    {
      _depth = currentDepth;
      _badCount = 0;
      Syslog::write(
          "DepthSensor(Navigation): _maxConsecutiveBad exceeded, "
	  "accept reading: \"%f\"",
          _depth);
    }
    else
    {
      //
      // Reject depth reading
      //
      Syslog::write("DepthSensor (Navigation): Received bad depth reading: %f\n"
		    " Substitute previous depth value of %f\n",
                    currentDepth, _depth);
    }
  }
  else
  {
    //
    // Nominal behavior,we got a good depth reading
    //
    _depth = currentDepth;
    _badCount = 0;
    dprintf("DepthSensor(Navigation) Received good depth reading.\n");
  }
  dprintf("DepthSensor(Navigation) currentDepth = %f\n",currentDepth);
  dprintf("DepthSensor(Navigation) _depth       = %f\n",_depth);

  return status;
}