#include "ObsAvoid.h"
#include "TimeP.h"
#include "TimeIF.h"
#include "MathP.h"

#include "macrologger.h"

//ObsAvoid::ObsAvoid(): NotSpecified(-10000.), _useFwd(false)
ObsAvoid::ObsAvoid(): _useFwd(false)
{
   //
   //---  Move to a configuration file:  ---
   //
   // One cnt = one degree. Zero is nadir, + is moving towards the bow in xz.
   _xzMountAngleCnts = 45;
   _fwdCutover = 0;
   _useThetaB = true;
   _tau = 3.;  //Seconds
   _Ts = .2;   //Seconds
   _maxPitch = PI/6.;


   //---  End move to cfg ---


   _echoReceived = false;
   _rangeEng = 0.;
   _maxRange = 0.;
   _timeLast = TimeP::milliseconds();
   TimeIF::TimeSpec now;
   TimeP::gettime(&now);
   _stime0 = TimeP::seconds(&now);
   _numberOfPings = 0;
   _lastPingTime = 0;
   for( int i=0; i<OA_MAX_BEAMS; i++ )
   {
      _altitudeArray[i]         = INVALID_ALTITUDE;
      _altitudeArrayLast[i]     = INVALID_ALTITUDE;
      _altitudeArrayLastLast[i] = INVALID_ALTITUDE;
      _hRangeArray[i]           = INVALID_ALTITUDE;
   }
   _tindxMinAltitude = 0;
   _tindxMinAltAll   = 0;
   _epochOffset = 0;
   _minAltitude = INVALID_ALTITUDE;
   _noResponseAft = 0;
   _noResponseFwd = 0;
   _nadirCnts = 15;
   _nbeams = 120;
   _feedThetaForward = false;
   _avoidRangeActive = false;
   _avoidAngleActive = false;

   _first = true;
   _firstSurfaceDetect = true;

   _thetaB = 0.;
   _lsAlt  = INVALID_ALTITUDE;

   _beamOverlap = 0;
   if( _fwdCutover < 0 || _fwdCutover > 30 )
   {
      LOG_ERROR("ObsAvoid - Error: fwdCutover is %d, "
		" and must be >=0 and <=30.  Setting it to zero.", _fwdCutover);
      _fwdCutover = 0;
   }

   // Set up first order LP on obstacle altitude:
   //
   _x = 0.;
   _y = 0.;

   if(_tau)
   {
      _pole = exp( -_Ts/_tau);
      _gain =  1. - _pole;
      LOG_INFO("ObsAvoid:: Initializing obstacle altitude filter.");
      LOG_INFO("ObsAvoid:: gain = %.2e, tau = %.2f seconds, pole=%.2e.",
	       _gain, _tau, _pole);

   }
   if(_useThetaB)
   {
      LOG_INFO("ObsAvoid::execute - using thetaB.");
   }

   _log = new ObsAvoidLog();

   LOG_INFO("ObsAvoid::Constructor finished.");
}

ObsAvoid::~ObsAvoid()
{

}

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

// Returns 0 if computation was successful.
// Inputs:
//
// vehState, sonar data, altitudeCmdIn, native altitude control, avoidRange,
// avoidAngle, tau, maxLowerPitch, useThetaB
//
// Outputs:
//
// depthCmdOut, stopProp, blowBallast

int ObsAvoid::compute(OaVehState *vehState, Idt83pData *aftSonar,
		      Idt83pData *fwdSonar, double altitudeCmdIn, double avoidRange,
		      double avoidAngle, double depthCmd, double *depthCmdOut)
{

//
// Check for valid data.  Both sonars must have new data, and each must have OA_MAX_BEAMS.
   _valid = true;

   _pingTime = aftSonar->pingtime;

   if( _pingTime == _lastPingTime )
   {
      _noResponseAft++;
   }
   else
   {
      _noResponseAft = 0;
      LOG_DEBUG("ObsAvoid: Aft ping at %.2f sec.", _pingTime);
   }
  if( _useFwd )
  {

     _fwdPingTime = fwdSonar->pingtime;

     if( _fwdPingTime == _lastFwdPingTime )
     {
	_noResponseFwd++;
	ulong msec = TimeP::milliseconds();
	LOG_INFO("ObsAvoid: No fwd ping at %.2f sec.", ((float) (msec))/1000.);
     }
     else
     {
	LOG_DEBUG("ObsAvoid: Fwd ping at time since sim start = %.2f sec.", ((float)TimeP::milliseconds())/1000.);
	LOG_DEBUG("ObsAvoid: Fwd ping at deltaT clock =  %.2f.", _fwdPingTime);
	_noResponseFwd = 0;
     }
  }
  else
  {
     _noResponseFwd = 0;
     fwdSonar->nbeams = OA_MAX_BEAMS;
  }

  LOG_DEBUG("ObsAvoid: Aft ping at deltaT clock =  %.2f.", _pingTime);
  LOG_DEBUG("ObsAvoid: No aft: %ld  No fwd: %ld",_noResponseAft,_noResponseFwd);
  if( !_noResponseAft && !_noResponseFwd )
  {
     _lastPingTime = _pingTime;
     _lastFwdPingTime = _fwdPingTime;
  }
  else
  {
     _valid = false;
  }

  //LOG_INFO("ObsAvoid: Aft sonar reports %d beams at %.2f sec.",
  //	   aftSonar->nbeams, ((float)TimeP::milliseconds())/1000.);
  /*
  if( fwdSonar->nbeams != OA_MAX_BEAMS )
  {
     _valid = false;
     LOG_ERROR("ObsAvoid: Fwd sonar reports %d beams at system time %.2f sec.",
	       fwdSonar->nbeams, ((float)TimeP::milliseconds())/1000.);
  }
  */
   short tindx;
   //double dtrain;
   //double deltaDepth, deltaArc;
   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 = vehState->depth;

   if( _first )
   {
      _depthLast = _depth;
      _xLast = vehState->northings;
      _yLast = vehState->eastings;
      //
      // 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 = aftSonar->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 = (fwdSonar->nbeams-60)/2;
	 _nbeams = aftSonar->nbeams + fwdSonar->nbeams - _beamOverlap;
	 LOG_INFO("ObsAvoid: 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 )
   {
      LOG_ERROR("ObsAvoid: valid is false.");
      return -1;
   }
   LOG_DEBUG("ObsAvoid: valid beams!.");

   for( i=0; i<aftSonar->nbeams; i++) _BeamRanges[i] = aftSonar->range[i];

   // Set debug True to see OA-init bug details
   //
   LOG_DEBUG("ObsAvoid:_useFwd is %d", _useFwd);
   if( _useFwd )
   {
      int j;
      int startCnt  = aftSonar->nbeams - _fwdCutover;
      int fwdOffset = _beamOverlap - _fwdCutover;
      LOG_DEBUG("ObsAvoid:Fwd indices into ranges: %d, %d",
        startCnt, fwdOffset);

      for( i=startCnt, j=fwdOffset; i<_nbeams; i++, j++)
        _BeamRanges[i] = fwdSonar->range[j];
   }
   LOG_DEBUG("ObsAvoid:_useFwd is %d", _useFwd);


   //deltaDepth = _depth - _depthLast;

   //deltaArc   = sqrt( pow( (vehState->northings  - _xLast),2.) +
   //		      pow( (vehState->eastings   - _yLast),2.) );

   _minAltitude  = INVALID_ALTITUDE;
   _minSlopeAlt  = INVALID_ALTITUDE;
   _minAltAll    = INVALID_ALTITUDE;
   _minBeamRange = INVALID_ALTITUDE;
   _minBeamCnts  = 0;

//xxx


   for(tindx = 0; tindx < _nbeams; tindx++)
   {

      LOG_DEBUG("ObsAvoid::altitude timeNow = %ld \n "
	      "timeLast = %ld, tindx = %d, _epochOffset=%ld",
	      TimeP::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(vehState->pitch) *
	       cos(vehState->roll)  *
	       cos(beamAngle_B) -
	       sin(vehState->pitch) *
	       sin(beamAngle_B)
	       );


	 _hRangeArray[tindx] = _BeamRanges[tindx] *
	    (
	       cos(vehState->pitch) *
	       sin(beamAngle_B) +
	       sin(vehState->pitch) *
	       cos(vehState->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);


	 LOG_DEBUG("Nav/Echo _hRange[%d] = %.2f at t=%ld.",
		 tindx,_hRangeArray[tindx], TimeP::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
      {
	 LOG_ERROR("ObsAvoid::altitude - Error "
		   "Uninitialized data:  _data.ranges[%d]=%.2f\n"
		   "     altitudeArray[%d] = %.2f.",
		   tindx, _BeamRanges[tindx],
		   tindx, _altitudeArray[tindx]);
      }

      LOG_DEBUG("ObsAvoid::altitude - "
	      "_altitudeArray[tindx] = %.2f "
	      "_BeamRanges[tindx] = %.2f\n",
	      _altitudeArray[tindx],	 _BeamRanges[tindx]);
   }
   _depthLast = _depth;
   _xLast     = vehState->northings;
   _yLast     = vehState->eastings;

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

   bool 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;

	 LOG_DEBUG("ObsAvoid::altitude 2 - "
		 "minAltitude = %.2f ",_minAltitude);
      }
      detectedSurfaceDepth = _depth + _altitudeArray[tindx];
      if( detectedSurfaceDepth < surfaceDetectError*_depth )
      {
	 surfaceDetect = True;
      }
   }
   //
   // Repeat for the slope-adjusted altitudes:
   for(tindx = _nadirCnts; 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;

	 LOG_DEBUG("ObsAvoid::altitude 2 - "
		 "minAltitude = %.2f ",_minAltitude);
      }
   }
   //
   // Repeat for the whole array.  That is, for all ranges, not just <avoidRange.
   for(tindx = _nadirCnts; tindx < _nbeams-_beamOverlap; tindx++)
   {
      if( _altitudeArray[tindx] < _minAltAll -.1 &&
	  _hRangeArray[tindx] > 0 &&
	  _altitudeArrayLast[tindx]     != INVALID_ALTITUDE &&
	  _altitudeArrayLastLast[tindx] != INVALID_ALTITUDE &&
	  _depth + _altitudeArray[tindx] > surfaceDetectError*_depth )
      {
	 _minAltAll      = _altitudeArray[tindx];
	 _tindxMinAltAll = tindx;

	 LOG_DEBUG("ObsAvoid::altitude 2 - "
		 "minAltitude = %.2f ",_minAltitude);
      }
   }

   if( surfaceDetect )
   {
      if( _firstSurfaceDetect )
      {
	 LOG_INFO("ObsAvoid: Surface detected at depth "
		   "%.1f meters at %.2f sec.",
		   detectedSurfaceDepth,
		   ((float)TimeP::milliseconds())/1000.);
	 _firstSurfaceDetect = False;
      }
   }
   else
   {
      if( !_firstSurfaceDetect )
      {
	 LOG_INFO("Multibeam: Surface lost at %.2f sec.",
		   ((float)TimeP::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 = _nadirCnts; tindx < aftSonar->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. Both depth
	 // (z) and altitude are postive down.
	 _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 >= aftSonar->nbeams ) nadirHigh = aftSonar->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 )
   {
      _vehAltitude = INVALID_ALTITUDE;
      _medianBeamNo = 0;
   }
   else
   {
      Math::shellSort( nadirPtrArray, _nGood );
      int nGoodMedian = _nGood/2;
      _vehAltitude       = *nadirPtrArray[ nGoodMedian ];
      //_medianBeamNo   =  nadirPtrArray[ nGoodMedian ] + &_altitudeArray[nadirLow];
      _medianBeamNo   =  indexArray[ nGoodMedian ];
   }

   /*
   _nadirCnts     = _nadirCnts;
   _minBeamRange  = _minBeamRange;
   _minBeamCnts   = _minBeamCnts;
   _thetaB        = _thetaB;
   _lsAlt         = _lsAlt;
   */

   _obsAltitude      = _minAltitude;
   _obsRange         = _hRangeArray[_tindxMinAltitude];
   _obsCnts          = _tindxMinAltitude;
   _obsSlopeAlt      = _minSlopeAlt;
   _obsSlopeRange    = _hRangeSlopeArray[_tindxMinSlopeAlt];
   _obsSlopeCnts     = _tindxMinSlopeAlt;
   _obsAltAll        = _minAltAll;
   _obsRangeAll      = _hRangeArray[_tindxMinAltAll];
   //
   // Compute flight path angle to horizon+altitudeCmd.  Minus
   // because + altitude is down from vehicle.
   if( _obsRangeAll > 1.0 )
   {
      _flightPathAngle = atan( -_obsAltAll / _obsRangeAll );
   }
   else _flightPathAngle = atan( -_obsAltAll );
   if( _flightPathAngle < 0. ) _flightPathAngle = 0.;

   //
   // Begin waypoint bottom:

   double measuredDepth    = vehState->depth;
   double measuredAltitude = vehState->altitude;
   //
   // PI/7 ~= 26 degrees.  Because a single Imagenex at 45 degrees can only
   // see 15 degrees aft of vehicle z.
   if(vehState->pitch > PI/7.) measuredAltitude = vehState->altitude;

   //
   // Move this to the constructor if/when we are sure that the constructor
   // runs immediately before the first call to compute().
   //if(_tau && (fabs(obstacleAltitude) < INVALID_ALTITUDE))
   if(_tau && _obsAltitude != INVALID_ALTITUDE)
   {
      if( _firstFilter )
      {
	 _x = measuredAltitude * (1. - _gain)/_gain;
	 LOG_INFO("ObsAvoid: Initializing _x to %.2f at t = "
		  "%.2f.",_x, ((float)TimeP::milliseconds())/1000.);
	 _firstFilter = False;
      }

      double u = _obsAltitude;

      _xNext = _pole * ( _x + u );
      _y = _gain * ( _x + u );
      _x = _xNext;

      _obsAltitude = _y;

      //_log->setFields(this);
      //_log->write();
   }

   _altitudeCmd = altitudeCmdIn;

   double depthToAltitude     = measuredDepth + measuredAltitude - _altitudeCmd;
   double depthToObsAltitude  = measuredDepth + _obsAltitude     - _altitudeCmd;
   double depthToObsAltAll    = measuredDepth + _obsAltAll       - _altitudeCmd;

   LOG_INFO("northings        = %.2f",vehState->northings);
   LOG_INFO("eastings         = %.2f",vehState->eastings);
   LOG_INFO("measuredDepth    = %.2f",measuredDepth);
   LOG_INFO("measuredAltitude = %.2f",measuredAltitude);
   LOG_INFO("_altitudeCmd     = %.2f",_altitudeCmd);
   LOG_INFO("depthToAltitude = %.2f",depthToAltitude);






   if( _useThetaB )
   {
      //
      //Convert back to the LV frame for control.  Beware that when
      //using thetaB, the altitude command is perpendicular altitude,
      //not vertical altitude.
      depthToAltitude     = measuredDepth +
	 measuredAltitude - _altitudeCmd/cos(_thetaB);
      depthToObsAltitude  = measuredDepth +
	 ( _obsSlopeAlt - _altitudeCmd ) / cos(_thetaB);
      _obsRange = _obsSlopeRange * cos(_thetaB);

      _thetaFF = _thetaB;
   }
   else
      _thetaB = 0.;

   //
   // Now reset _altitudeCmd to turn off native altitude control and use
   // depth instead:
   _altitudeCmd = NotSpecified;
   //bool depthBasedAltitudeControl = True;
   //_depthCmd = _depth;
   _depthCmd = depthCmd;

   if( (depthToAltitude || depthToObsAltAll) < _depthCmd )
   {
      if( depthToAltitude < _depthCmd )  _depthCmdOut = depthToAltitude;

      if( avoidAngle != NotSpecified && depthToObsAltAll < _depthCmdOut )
      {

	 if( _flightPathAngle > avoidAngle )
	 {
	    if( !_avoidAngleActive )
	    {
	       LOG_INFO("ObsAvoid::Obstacle horizon detected above maximum "
			"flight path angle %.1f meters ahead at t = %15.2f.  Climb!",
			_obsRangeAll, ((float)TimeP::milliseconds())/1000.);

	       _avoidAngleActive = True;
	    }
	 }
	 //else _avoidAngleActive = False; (above) Keep avoidAngleActive
	 //true until depthToObsAltAll < _depthCmdOut, rather than the
	 //flightPathAngle being less than the specified avoidAngle.
      }
      else _avoidAngleActive = False;


      if( avoidRange != NotSpecified && (depthToObsAltitude < _depthCmdOut || _avoidAngleActive) )
      {

	 if( _avoidAngleActive )
	 {
	    _depthCmdOut = depthToObsAltAll;
	    _thetaFF  = _maxPitch;     //Climb as fast as allowed.
	 }
	 else if( _obsRange < avoidRange )
	 {
	    if( !_avoidRangeActive )
	    {
	       LOG_INFO("ObsAvoid::Obstacle detected %.1f meters ahead. "
			"avoidRange activated at t = %15.2f.  Climb!",
			_obsRange, ((float)TimeP::milliseconds())/1000.);
	       _avoidRangeActive = True;
	    }
	    _depthCmdOut = depthToObsAltitude;
	    if( _obsRange > 1.0 && _feedThetaForward )
	    {
	       _thetaFF  = atan2(measuredAltitude - _obsAltitude, _obsRange);
	       if( _thetaFF >  _maxPitch ) _thetaFF =  _maxPitch;
	       if( _thetaFF < -_maxPitch ) _thetaFF = -_maxPitch;
	    }
	 }
	 if( _obsRange >= avoidRange ) _avoidRangeActive = False;

      }
      if( _obsRange >= avoidRange ) _avoidRangeActive = False;

   }
   else  //if( depthToAltitude < _depthCmd )
   {
      _avoidRangeActive = False;
      _avoidAngleActive = False;
   }

   *depthCmdOut = _depthCmdOut;


   oaData.pingtime = aftSonar->pingtime;

   oaData.roll  = vehState->roll;
   oaData.pitch = vehState->pitch;
   oaData.yaw   = vehState->yaw;
   oaData.depthCmdOut = _depthCmdOut;

   _log->setFields( &oaData );
   _log->write();

   return 0;
}