#include "EchoSounder.h"
#include "MathP.h"
#include "Syslog.h"
#include "TimeP.h"
#include "LowAltitudeFollowing.h"

EchoSounder::EchoSounder(const char *name, NavSensors *sensors, int maxBad)
  : NavSensor(name, sensors, maxBad)
{
  _echoReceived = False;
  _rangeEng = 0.;
  _xzMountAngle = 0.;
  _maxRange = 0.;
  _timeLast = Time::milliseconds();
  _numberOfPings = 0;
  for( int i=0; i<EchoSounderIF::ScanArraySize; i++ )
  {
     _dtrainArray[i]   = 0.;
     _altitudeArray[i] = 100.;
     _hRangeArray[i]   = 100.;
  }
  _tindxMinAltitude = 0;
  Time::getEpoch( &_localEpoch );
  Syslog::write("Navigation/EchoSounder:: epoch = (%d,%d)",
                 _localEpoch.tv_sec, _localEpoch.tv_nsec);
  _epochOffset = 0;
}


double EchoSounder::range()
{
  return _data.range;
}

#if 0
double EchoSounder::altitude(NavigationIF::Attitude *attitude)
{
   double result, dtrain;

   dtrain = _echoSounderIF->getHeadAngle();

   result   = _rangeEng * ( cos(attitude->pitch) *
                         cos(attitude->roll)  *
                         cos(_xzMountAngle + dtrain) -
                         sin(attitude->pitch) *
                         sin(_xzMountAngle + dtrain) );
   return result;
}
#endif

Boolean First = True;

double EchoSounder::altitude(NavigationIF::Attitude *attitude,
                             NavigationIF::Position *position )

{
   short tindx;
   double minAltitude = INVALID_ALTITUDE;
   double dtrain;
   double deltaDepth, deltaDmg;
   Boolean debug = False;
   //
   // Assuming a long int is 4 bytes, the milliseconds() function becomes
   // invalid 49 days after the start of the mission.  That's OK for now.

   _depth = position->z;

   if( First )
   {
      _depthLast = _depth;
      _xLast = position->x;
      _yLast = position->y;
      First = False;
   }

   deltaDepth = _depth - _depthLast;

   deltaDmg   = sqrt( pow( (position->x  - _xLast),2.) +
		      pow( (position->y  - _yLast),2.) );

   _numberOfPings = 0;
   for(tindx = _data.tindxMin; tindx <= _data.tindxMax; tindx++)
   {

      dprintf("Navigation/EchoSounder::altitude timeNow = %d \n "
                    "timeLast = %d, _data.times[%d] = %d, _epochOffset=%d",
                      Time::milliseconds(),
                     _timeLast, tindx, _data.times[tindx], 
                     _epochOffset);


      if( _data.times[tindx] - _epochOffset > _timeLast )
      {
	 _numberOfPings++;
	 _tindxLast = tindx;
	 _dtrainArray[tindx] = _echoSounderIF->getHeadAngle(tindx);
	 if( _data.ranges[tindx] > 0 )
	 {
	    //
	    // Need an IF function to get the range array in eng. units.
	    // Can't just call IF->range() because the driver is asynchronous
	    // and if it's scanning it may have gotten more than one new
	    // return.
	    _altitudeArray[tindx] = _data.ranges[tindx]/100. *
	       ( 
		  cos(attitude->pitch) *
		  cos(attitude->roll)  *
		  cos(_xzMountAngle + _dtrainArray[tindx]) -
		  sin(attitude->pitch) *
		  sin(_xzMountAngle + _dtrainArray[tindx]) 
	       );

	    _hRangeArray[tindx] = _data.ranges[tindx]/100. *
	       ( 
		  cos(attitude->pitch) * 
		  sin(_xzMountAngle + _dtrainArray[tindx]) +  
		  sin(attitude->pitch) *
		  cos(attitude->roll)  *
		  cos(_xzMountAngle + _dtrainArray[tindx])
	       );
	    Boolean debug = False;
	    dprintf("Nav/Echo _hRange[%d] = %.2f at t=%d.",
		    tindx,_hRangeArray[tindx], Time::milliseconds());
	 }
	 else if( 0 == _data.ranges[tindx] )
	 {
	    //
	    // We're looking over the horizon OR we're closer than minRangeEng.
	    _altitudeArray[tindx] = INVALID_ALTITUDE;
	    _hRangeArray[tindx]   = INVALID_ALTITUDE;
	 }
	 else 
	 {
	    Syslog::write("Navigation/EchoSounder::altitude - Error "
	    "Uninitialized data:  _data.ranges[%d]=%d\n"
	    "     altitudeArray[%d] = %d.", 
	    tindx, _data.ranges[tindx],
	    tindx, _altitudeArray[tindx]);
	 }

	 dprintf("Navigation/EchoSounder::altitude - "
	         "_tindxMax = %d, _tindxMin = %d\n"
	         "_altitudeArray[tindx] = %.2f " 
	         "_data.ranges[tindx] = %d", 
	 _data.tindxMax, _data.tindxMin, _altitudeArray[tindx],
	 _data.ranges[tindx]);

      }
      else
      {
	 //
	 // We need to update the altitude and hrange arrays with vehicle
	 // motion that has occured since the last ping(s).  This scheme
	 // assumes the vehicle is not turning.
	 _altitudeArray[tindx] -= deltaDepth;
	 _hRangeArray[tindx]   -= deltaDmg;
      }
   }
   if(_numberOfPings) _timeLast = Time::milliseconds();
   _depthLast = _depth;
   _xLast     = position->x;
   _yLast     = position->y;
   //
   // Select the smallest altitude from the array:
   for(tindx = _data.tindxMin; tindx <= _data.tindxMax; tindx++)
   {
      if( _altitudeArray[tindx] < minAltitude && _hRangeArray[tindx] > 0 ) 
      {
	 minAltitude = _altitudeArray[tindx];
	 _tindxMinAltitude = tindx;

	 dprintf("Navigation/EchoSounder::altitude 2 - "
	         "minAltitude = %.2f ",minAltitude);

      }
   }

   return minAltitude;
}

void EchoSounder::obstacleDetect(NavigationIF::Attitude *attitude,
                                 NavigationIF::Position *position,
				 RangeToObstacle *rangeToObstacle)
{
   rangeToObstacle->vertMin   = altitude( attitude, position );
   rangeToObstacle->horizMin  = _hRangeArray[_tindxMinAltitude];
   rangeToObstacle->vertLast  = _altitudeArray[_tindxLast];
   rangeToObstacle->horizLast = _hRangeArray[_tindxLast];
   rangeToObstacle->numberOfPings = _numberOfPings;

   //Syslog::write("Nav/Echo Range = %.2f", _hRangeArray[_tindxMinAltitude]);

   return;
}				 



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

double EchoSounder::maxRange()
{
   return _maxRange;
}

Boolean EchoSounder::isScanning()
{
   return _echoSounderIF->isScanning();
}

TaskInterface *EchoSounder::createTaskIF(int timeout)
{
  try {
    _echoSounderIF = new EchoSounderIF(name(), timeout);
    _echoSounderIF->get(&_data);
  }
  catch (...) {
    _echoSounderIF = 0;
  }

  return _echoSounderIF;
}


DeviceIF::Status EchoSounder::readTaskIF(Boolean *valid,
					 TimeIF::TimeSpec *sampleTime)
{
  DeviceIF::Status status = _echoSounderIF->get(&_data);
  _rangeEng        = _data.range;
  _xzMountAngle    = _data.xzAngle;
  _maxRange = _data.maxRange;

   sampleTime->seconds     = _data.updateTime.seconds;
   sampleTime->nanoSeconds = _data.updateTime.nanoSeconds;

   _epochOffset = (_localEpoch.tv_sec - _data.epoch.seconds) * 1000 + 
      (_localEpoch.tv_nsec - _data.epoch.nanoSeconds) / 1.e6;

// 
// The "serial status" flag.  65 means the "switch data" command was
// accepted.  See page 7 of "Ethernet Interface Specification, Model 881L
// Digital Sonar Head, 881-000-50x" v1.00, 23Jan07.
//
  if( _data.serialStatus == 65 )
     *valid = True;
  else
     *valid = False;

//  Syslog::write("EchoSounder - _data.serialStatus = %d, valid = %d", 
//  _data.serialStatus, *valid);


  return status;
}