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

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

  _first = True;
  _firstSurfaceDetect = True;

  _thetaB = 0.;
  _lsAlt  = INVALID_ALTITUDE;

  VehicleConfigurationIF vehicleConfig("vehicleConfig");
  _fwdCutover = vehicleConfig.fwdCutover();
  if( _fwdCutover < 0 || _fwdCutover > 30 )
  {
     Syslog::write("Navigation/Multibeam - Error: fwdCutover is %d, "
		   " and must be >=0 and <=30.  Setting it to zero.");
     _fwdCutover = 0;
  }
}


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,
                        double *minBeamRange, int *minBeamCnts,
			double *thetaB, double *lsAlt,
			double *obstacleSlopeAlt,
			double *obstacleSlopeRange,
			int    *obstacleSlopeCnts)
{
   short tindx;
   double dtrain;
   double deltaDepth, deltaArc;
   Boolean debug = False;
   int i;
   //
   // 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;
      //
      // 11 Nov 2014.  For the 13 Nov dive, the desire is to reduce the
      // down-looking sector and increase the forward-looking sector.  The
      // requirement is that both sectors add to 180 degrees, and that they
      // don't overlap.  
      //
      // This can be accounted for here by adding an offset to
      // _xzMountAngleCnts, _sectorOffset.
      //
      // Sector sizes:
      //
      // aft = 120,  fwd = 60  =>  _sectorOffset = 0
      // aft = 90,   fwd = 90  =>  _sectorOffset = 15
      // aft = 60,   fwd = 120 =>  _sectorOffset = 30
      //
      // 9 Oct 2015: Removed _sectorOffset.  We no longer require both
      //sectors to add to 180.  The fwd sector will now be set to 120 in
      //deltat_server.cfg, and accounted for here with fwdCutover.
      _xzMountAngleCnts = 0;

      _first = False;

      _nbeams = _data.nbeams;

      if( _useFwd )
      {
	 //
	 // Assume that the forward sonar is mounted along the vehicle
	 // centerline, +x, and the aft sonar is perpendicular along vehicle
	 // +z.
	 //
	 // _nbeams is the number of contiguous beams, which should always be
	 // 210 given the above mounting angles.
	 //
	 // aft = 120, fwd = 60  => _beamOverlap = 0,     _fwdCutover = 0
	 // aft = 120, fwd = 90  => _beamOverlap = 15, 0<=_fwdCutover<=15
	 // aft = 120, fwd = 120 => _beamOverlap = 30, 0<=_fwdCutover<=30
	 _beamOverlap = (_dataFwd.nbeams-60)/2;
	 _nbeams = _data.nbeams + _dataFwd.nbeams - _beamOverlap;
	 Syslog::write("Navigation/Multibeam: Number of beams in the array = %d, "
		       "beamOverlap=%d fwdCutover=%d", _nbeams, _beamOverlap,
		       _fwdCutover);
      }
   }
   //
   // Don't recompute unless we have new data.  For the Imagenex multibeams
   // and beamformer, valid is defined to mean the instrument is operating
   // and has new data.
   if( !valid() )  return;

   for( i=0; i<_data.nbeams; i++) _BeamRanges[i] = _data.beam_ranges[i];

   if( _useFwd ) 
   {
      int j;
      int startCnt  = _data.nbeams - _fwdCutover;
      int fwdOffset = _beamOverlap - _fwdCutover;
      for( i=startCnt, j=fwdOffset; i<_nbeams; i++, j++) 
	 _BeamRanges[i] = _dataFwd.beam_ranges[j];
   }

   deltaDepth = _depth - _depthLast;

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

   _minAltitude  = INVALID_ALTITUDE;
   _minSlopeAlt  = INVALID_ALTITUDE;
   _minBeamRange = INVALID_ALTITUDE;
   _minBeamCnts  = 0;
   for(tindx = 0; tindx < _nbeams; tindx++)
   {

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

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

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

	 _altitudeArrayLastLast[tindx] = _altitudeArrayLast[tindx];
	 _altitudeArrayLast[tindx]     = _altitudeArray[tindx];


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

	 
	 _hRangeArray[tindx] = _BeamRanges[tindx] *
	    ( 
	       cos(attitude->pitch) * 
	       sin(beamAngle_B) +  
	       sin(attitude->pitch) *
	       cos(attitude->roll)  *
	       cos(beamAngle_B)
	       );
	 //
	 // The cos(_thetaB) factors put these arrays in the thetaB frame.
	 _altitudeSlopeArray[tindx] = cos(_thetaB) * (_altitudeArray[tindx] +
				      _hRangeArray[tindx]*tan(_thetaB));
	 _hRangeSlopeArray[tindx] = _hRangeArray[tindx]/cos(_thetaB);


	 Boolean debug = False;
	 dprintf("Nav/Echo _hRange[%d] = %.2f at t=%d.",
		 tindx,_hRangeArray[tindx], Time::milliseconds());

	 if( _BeamRanges[tindx]< _minBeamRange &&
	     _altitudeArrayLast[tindx]     != INVALID_ALTITUDE &&
	     _altitudeArrayLastLast[tindx] != INVALID_ALTITUDE )
	 {
	    _minBeamRange = _BeamRanges[tindx];
	    _minBeamCnts  = tindx;
	 }


      }
      else if( 0. == _BeamRanges[tindx] )
      {
	 //
	 // We're looking over the horizon OR we're closer than minRangeEng.
	 _altitudeArray[tindx] = INVALID_ALTITUDE;
	 _hRangeArray[tindx]   = INVALID_ALTITUDE;
	 _altitudeSlopeArray[tindx] = INVALID_ALTITUDE;
	 _hRangeSlopeArray[tindx]   = INVALID_ALTITUDE;
      }
      else 
      {
	 Syslog::write("Navigation/Multibeam::altitude - Error "
		       "Uninitialized data:  _data.ranges[%d]=%d\n"
		       "     altitudeArray[%d] = %d.", 
		       tindx, _BeamRanges[tindx],
		       tindx, _altitudeArray[tindx]);
      }

      dprintf("Navigation/Multibeam::altitude - "
	      "_altitudeArray[tindx] = %.2f " 
	      "_BeamRanges[tindx] = %d\n", 
	      _altitudeArray[tindx],	 _BeamRanges[tindx]);
   }
   _depthLast = _depth;
   _xLast     = position->x;
   _yLast     = position->y;

   //
   // Find nadir in instrument-fixed coordinates.
   _nadirCnts = 60 - _xzMountAngleCnts - (int) (attitude->pitch*180./PI);
   if( _nadirCnts < 0 ) _nadirCnts = 0;
   if( _nadirCnts >= _data.nbeams ) _nadirCnts = _data.nbeams-1;

   Boolean surfaceDetect=False;
   double detectedSurfaceDepth;
   double surfaceDetectError = .1;  //*100 = percent depth error allowed.
   //
   // Select the smallest altitude from the array.  The -.1 is to prevent
   // numerical noise from causing tindxMin to discontinuously jump at each
   // sampling instant, which in turn wreaks havoc with theta feedforward.
   //
   // 25 June 2014: Search over only the center 60 degree sectors of each
   // sonar:
   //
   // 9 Oct 2015: Change this to search all beams forward of nadir up to 30
   // deg above vehicle centerline, meaning we are discarding beams 90-120 of
   // the forward look.  So, _nbeams-_beamOverlap should always be 180.
   //
   for(tindx = _nadirCnts; tindx < _nbeams-_beamOverlap; tindx++)
   {
      if( _altitudeArray[tindx] < _minAltitude -.1 && _hRangeArray[tindx] > 0 &&
	  _hRangeArray[tindx] < avoidRange &&
	  _altitudeArrayLast[tindx]     != INVALID_ALTITUDE &&
	  _altitudeArrayLastLast[tindx] != INVALID_ALTITUDE &&
	  _depth + _altitudeArray[tindx] > surfaceDetectError*_depth )
      {
	 _minAltitude = _altitudeArray[tindx];
	 _tindxMinAltitude = tindx;

	 dprintf("Navigation/Multibeam::altitude 2 - "
		 "minAltitude = %.2f ",_minAltitude);
      }
      detectedSurfaceDepth = _depth + _altitudeArray[tindx];
      if( detectedSurfaceDepth < surfaceDetectError*_depth ) 
      {
	 surfaceDetect = True;
      }
   }
   //
   // Repeat for the slope-adjusted altitudes:
   for(tindx = 0; tindx < _nbeams-_beamOverlap; tindx++)
   {
      if( _altitudeSlopeArray[tindx] < _minSlopeAlt -.1 &&
	  _hRangeSlopeArray[tindx] > 0 &&
	  _hRangeSlopeArray[tindx] < avoidRange &&
	  _altitudeArrayLast[tindx]     != INVALID_ALTITUDE &&
	  _altitudeArrayLastLast[tindx] != INVALID_ALTITUDE &&
	  _depth + _altitudeArray[tindx] > surfaceDetectError*_depth )
      {
	 _minSlopeAlt = _altitudeSlopeArray[tindx];
	 _tindxMinSlopeAlt = tindx;

	 dprintf("Navigation/Multibeam::altitude 2 - "
		 "minAltitude = %.2f ",_minAltitude);
      }
   }
   if( surfaceDetect )
   {
      if( _firstSurfaceDetect )
      {
	 Syslog::write("Multibeam: Surface detected at depth "
		       "%.1f meters at %.2f sec.",
		       detectedSurfaceDepth,
		       ((float)Time::milliseconds())/1000.);
	 _firstSurfaceDetect = False;
      }
   }
   else
   {
      if( !_firstSurfaceDetect )
      {
	 Syslog::write("Multibeam: Surface lost at %.2f sec.",
		       ((float)Time::milliseconds())/1000.);
	 _firstSurfaceDetect = True;
      }
   }
   Boolean computeThetaB = True;
   if( computeThetaB )
   {
      // 
      // Find the bottom slope by fitting a line to the altitude array using our
      // old friend least squares.  For now, ignore the outlier problem and rely
      // on the mass of the vehicle to act as a low-pass filter.
      //
      // Use only data from the down-looking sonar.
      int ngoodBeams = 0;
      double xBottom[120], yBottom[120];
      for(tindx = 0; tindx < _data.nbeams; tindx++)  //Include sectorOffset?
      {
	 if( _BeamRanges[tindx] > 0 && 
	     _altitudeArrayLast[tindx]     != INVALID_ALTITUDE &&
	     _altitudeArrayLastLast[tindx] != INVALID_ALTITUDE &&
	     _depth + _altitudeArray[tindx] > surfaceDetectError*_depth )
	 {
	    xBottom[ngoodBeams] = _hRangeArray[tindx];
	    yBottom[ngoodBeams] = _altitudeArray[tindx];
	    ngoodBeams++;
	 }
      }
      //
      // OK, it's a 2x2.  Rather than involve newmat, use brute force.
      double A[120][2], AtA[2][2] = { {0.,0.}, {0.,0.} };
      double detAtA, AtAInv[2][2] = { {0.,0.}, {0.,0.} };
      for( i=0; i<ngoodBeams; i++ )
      {
	 A[i][0] = xBottom[i]; A[i][1] = 1.;
      }
      for( i=0; i<ngoodBeams; i++ )
      {
	 AtA[0][0] += A[i][0]*A[i][0];
	 AtA[0][1] += A[i][0]*A[i][1];
	 AtA[1][0] += A[i][1]*A[i][0];
	 AtA[1][1] += A[i][1]*A[i][1];
      }
      detAtA = AtA[0][0]*AtA[1][1] - AtA[1][0]*AtA[0][1];
      if( fabs(detAtA) > 0. )
      {
	 AtAInv[0][0] =  AtA[1][1]/detAtA;
	 AtAInv[0][1] = -AtA[0][1]/detAtA;
	 AtAInv[1][0] = -AtA[1][0]/detAtA;
	 AtAInv[1][1] =  AtA[0][0]/detAtA;
	 double temp[2] = {0.,0.};
	 for( i=0; i<ngoodBeams; i++ )
	 {
	    temp[0] += A[i][0]*yBottom[i];
	    temp[1] += A[i][1]*yBottom[i];
	 }
	 _thetaB = AtAInv[0][0]*temp[0] + AtAInv[0][1]*temp[1];
	 _lsAlt  = AtAInv[1][0]*temp[0] + AtAInv[1][1]*temp[1];
	 //
	 // Well, we have the slope.  Convert this to an angle.  Also, since
	 // +z is down, we have to flip the sign.
	 _thetaB = atan(-_thetaB);
      }
      else
      {
	 _thetaB = INVALID_ALTITUDE;
	 _lsAlt  = INVALID_ALTITUDE;
      }
   }   
   //
   // _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 = 36;
   const int nadirMin = 3;
   int nadirLow, nadirHigh;
   double *nadirPtrArray[nadirSector];
   int indexArray[nadirSector];

   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];
	 indexArray[*nGood] = 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];
      *medianBeamNo   =  indexArray[ nGoodMedian ];
   }

   *nadirCnts        = _nadirCnts;
   *obstacleAltitude = _minAltitude;
   *obstacleRange    = _hRangeArray[_tindxMinAltitude];
   *obstacleCnts     = _tindxMinAltitude;
   *minBeamRange     = _minBeamRange;
   *minBeamCnts      = _minBeamCnts;
   *thetaB           = _thetaB;
   *lsAlt            = _lsAlt;
   *obstacleSlopeAlt = _minSlopeAlt;
   *obstacleSlopeRange = _hRangeSlopeArray[_tindxMinSlopeAlt];
   *obstacleSlopeCnts  = _tindxMinSlopeAlt;
}

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)
{
   Boolean validFwd[1];
  _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)
  {
     _nNoResponseAft++;
     *valid = False;
  }
  else
  {
     _lastSampleTime.seconds     = _data.update_time.seconds;
     _lastSampleTime.nanoSeconds = _data.update_time.nanoSeconds;

     _nNoResponseAft = 0;
     *valid = True;

     _rangeEng = _deltatIF->range(DeltaTIF::Side);
  }
  if( _useFwd )
  {
     _deltatIF->get(DeltaTIF::Fwd, &_dataFwd);

     //
     // In this NavSensor, "valid" means it has new data.
     //
     if( _dataFwd.update_time.seconds     == _lastSampleTimeFwd.seconds &&
	 _dataFwd.update_time.nanoSeconds == _lastSampleTimeFwd.nanoSeconds)
     {
	_nNoResponseFwd++;
	*validFwd = False;
     }
     else
     {
	_lastSampleTimeFwd.seconds     = _dataFwd.update_time.seconds;
	_lastSampleTimeFwd.nanoSeconds = _dataFwd.update_time.nanoSeconds;

	_nNoResponseFwd = 0;
	*validFwd = True;
	*valid = True;
     }
  }
  //
  // For now, we'll hardcode nBad non-responses to mean the instrument is
  // off-line.  At some future time this may be replaced by having the call
  // _deltatIF->get(DeltaTIF::Aft, &_data); above return a status, so that
  // the driver then can decide if the beamformer or instrument or both are
  // hosed.
  //
  const int nBad = 100;
  if( _nNoResponseAft > nBad || _nNoResponseFwd > nBad )
     return DeviceIF::Error;
  else
     return DeviceIF::Ok;
}