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

Multibeam::Multibeam(const char *name, NavSensors *sensors, int maxBad)
  : NavSensor(name, sensors, maxBad)
{
  _echoReceived = False;
  _rangeEng = 0.;
  _xzMountAngleCnts = 0;
  _maxRange = 0.;
  _timeLast = Time::milliseconds();
  _numberOfPings = 0;
  for( int i=0; i<DeltaTIF::MaxBeams; i++ )
  {
     _altitudeArray[i] = 100.;
     _hRangeArray[i]   = 100.;
  }
  _tindxMinAltitude = 0;
  Time::getEpoch( &_localEpoch );
  Syslog::write("Navigation/Multibeam:: epoch = (%d,%d)",
                 _localEpoch.tv_sec, _localEpoch.tv_nsec);
  _epochOffset = 0;
  _minAltitude = INVALID_ALTITUDE;
  _nNoResponse = 0;
  _nadirCnts = 15;

  _first = True;
}


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

void Multibeam::compute(NavigationIF::Attitude *attitude,
			NavigationIF::Position *position,
			double avoidRange,
			double *altitude, int *nadirCnts, 
			int *nGood, int *medianBeamNo,
			double *obstacleAltitude, 
			double *obstacleRange, 
			int *obstacleCnts)
{
   short tindx;
   double dtrain;
   double deltaDepth, deltaArc;
   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;
      //
      // Call DeltaTIF::get_mounting_angles() here, and set _xzMountAngleCnts =
      // theta*180/pi + 90.  Here, _xzMountAngleCnts is in counts, which = deg.
      // _xzMountAngleCnts is the angle the multibeam CL forms with the vehicle
      // +z axis, going from vehicle +z towards +x.  90 is straight ahead.
      _xzMountAngleCnts = 45;
      _first = False;
   }

   //
   // Don't recompute unless we have new data:
   if( !valid() )  return;

   deltaDepth = _depth - _depthLast;

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

   _minAltitude = INVALID_ALTITUDE;
   for(tindx = 0; tindx < _data.nbeams; tindx++)
   {

      dprintf("Navigation/Multibeam::altitude timeNow = %d \n "
	      "timeLast = %d, _epochOffset=%d",
	      Time::milliseconds(),
	      _timeLast, tindx,
	      _epochOffset);

      if( _data.beam_ranges[tindx] > 0 )
      {
	 //_dtrainArray[tindx] = _echoSounderIF->getHeadAngle(tindx);

	 double beamAngle_B = (_xzMountAngleCnts - 60 + tindx)*PI/180.;

	 _altitudeArray[tindx] = _data.beam_ranges[tindx] *
	    ( 
	       cos(attitude->pitch) *
	       cos(attitude->roll)  *
	       cos(beamAngle_B) -
	       sin(attitude->pitch) *
	       sin(beamAngle_B) 
	       );

	 _hRangeArray[tindx] = _data.beam_ranges[tindx] *
	    ( 
	       cos(attitude->pitch) * 
	       sin(beamAngle_B) +  
	       sin(attitude->pitch) *
	       cos(attitude->roll)  *
	       cos(beamAngle_B)
	       );
	 Boolean debug = False;
	 dprintf("Nav/Echo _hRange[%d] = %.2f at t=%d.",
		 tindx,_hRangeArray[tindx], Time::milliseconds());
      }
      else if( 0. == _data.beam_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/Multibeam::altitude - Error "
		       "Uninitialized data:  _data.ranges[%d]=%d\n"
		       "     altitudeArray[%d] = %d.", 
		       tindx, _data.beam_ranges[tindx],
		       tindx, _altitudeArray[tindx]);
      }

      dprintf("Navigation/Multibeam::altitude - "
	      "_altitudeArray[tindx] = %.2f " 
	      "_data.beam_ranges[tindx] = %d\n", 
	      _altitudeArray[tindx],	 _data.beam_ranges[tindx]);
   }
   _depthLast = _depth;
   _xLast     = position->x;
   _yLast     = position->y;
   //
   // Select the smallest altitude from the array:
   //
   for(tindx = 0; tindx < _data.nbeams; tindx++)
   {
      if( _altitudeArray[tindx] < _minAltitude && _hRangeArray[tindx] > 0 &&
	 _hRangeArray[tindx] < avoidRange ) 
      {
	 _minAltitude = _altitudeArray[tindx];
	 _tindxMinAltitude = tindx;

	 dprintf("Navigation/Multibeam::altitude 2 - "
	         "minAltitude = %.2f ",_minAltitude);
      }
   }
   //
   // _altitudeArray is corrected for vehicle attitude.  That is, the values
   // are in local-level coordinates.  However, we still need to select the
   // nadir altitude based on the vehicle pitch angle and median-filtered
   // sector of beams.
   //
   const int nadirSector = 30;
   const int nadirMin = 5;
   int i, nadirLow, nadirHigh;
   double *nadirPtrArray[nadirSector];

   _nadirCnts = 60 - _xzMountAngleCnts - (int) (attitude->pitch*180./PI);
   if( _nadirCnts < 0 ) _nadirCnts = 0;
   if( _nadirCnts >= _data.nbeams ) _nadirCnts = _data.nbeams-1;

   nadirLow  = _nadirCnts - nadirSector/2;
   nadirHigh = _nadirCnts + nadirSector/2;
   if( nadirLow < 0 ) nadirLow = 0;
   if( nadirHigh >= _data.nbeams ) nadirHigh = _data.nbeams-1;
   //
   // Select the good beams:
   *nGood = 0;
   for( i=nadirLow; i<nadirHigh; i++ )
   {
      if( _altitudeArray[i] != INVALID_ALTITUDE ) 
      {
	 nadirPtrArray[*nGood] = &_altitudeArray[i];
	 (*nGood)++;
      }
   }
   
   if( *nGood<nadirMin )
   {
      *altitude = INVALID_ALTITUDE;
      *medianBeamNo = 0;
   }
   else
   {
      Math::shellSort( nadirPtrArray, *nGood );
      int nGoodMedian = *nGood/2;
      *altitude       = *nadirPtrArray[ nGoodMedian ];
      *medianBeamNo   =  nadirPtrArray[ nGoodMedian ] - &_altitudeArray[nadirLow];
   }

   *nadirCnts        = _nadirCnts;
   *obstacleAltitude = _minAltitude;
   *obstacleRange    = _hRangeArray[_tindxMinAltitude];
   *obstacleCnts     = _tindxMinAltitude;
}


double Multibeam::xzMountAngle()
{
   return _xzMountAngleCnts*PI/180.;
}

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

TaskInterface *Multibeam::createTaskIF(int timeout)
{
  try {
    _deltatIF = new DeltaTIF(name(), timeout);
    _deltatIF->get(DeltaTIF::Side, &_data);
  }
  catch (...) {
    _deltatIF = 0;
  }

  return _deltatIF;
}


DeviceIF::Status Multibeam::readTaskIF(Boolean *valid,
					 TimeIF::TimeSpec *sampleTime)
{
  _deltatIF->get(DeltaTIF::Side, &_data);

  sampleTime->seconds     = _data.update_time.seconds;
  sampleTime->nanoSeconds = _data.update_time.nanoSeconds;
  //
  // In this NavSensor, "valid" means it has new data.
  //
  if( _data.update_time.seconds     == _lastSampleTime.seconds &&
      _data.update_time.nanoSeconds == _lastSampleTime.nanoSeconds)
  {
     _nNoResponse++;
     *valid = False;
  }
  else
  {
     _lastSampleTime.seconds     = _data.update_time.seconds;
     _lastSampleTime.nanoSeconds = _data.update_time.nanoSeconds;

     _nNoResponse = 0;
     *valid = True;

     _rangeEng = _deltatIF->range(DeltaTIF::Side);
  }
  //
  // For now, we'll hardcode 10 non-responses to mean the instrument is
  // off-line.  At some future time this may be replaced by having the call
  // _deltatIF->get(DeltaTIF::Side, &_data); above return a status, so that
  // the driver then can decide if the beamformer or instrument or both are
  // hosed.
  //
  if( _nNoResponse > 10 )
     return DeviceIF::Error;
  else
     return DeviceIF::Ok;
}