#ifndef OBSAVOID_H
#define OBSAVOID_H

#define OA_MAX_BEAMS 120
#define INVALID_ALTITUDE 1000.

#ifndef PI
#define PI 3.1415926535898
#endif

#ifndef TWOPI
#define TWOPI (2.*PI)
#endif

#ifndef PIUPON2
#define PIUPON2 (PI/2.)
#endif

#ifndef PIUPON4
#define PIUPON4 (PI/4.)
#endif

const double NotSpecified = -10000;

#include <unistd.h>
#include <stdint.h>
#include "Idt83pParser.h"
#include "ObsAvoidLog.h"

struct OaVehState
{
   //
   // Vehicle location in an inertially-fixed North-East-Down (NED) frame.
   double northings, eastings, depth;
   unsigned int utm_zone;
   //
   // Velocity of the vehicle's body frame origin, with respect to an
   // inertial frame, coordinatized in the body frame.
   double v_x, v_y, v_z;
   //
   // 3-2-1 Euler angles of body wrto NED frame.
   double roll, pitch, yaw;
   //
   // Angular velocity of the vehicle's body fixed frame with respect to an
   // inertial frame, coordinatized in the body frame.
   double omega_x, omega_y, omega_z;
   //
   // Altitude from LR nav:
   double altitude;
};

struct  OaSonarData
{
   uint16_t nbeams;
   double*  ranges;
};




class ObsAvoid
{
  public:
   ObsAvoid();
   ~ObsAvoid();


   ///////////////////////////////////////////////////////////////////
   // Mounting angle in x-z plane in radians.
   double xzMountAngle();

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

  protected:

   double _vehAltitude;  //Vehicle altitude
   double _depthCmdOut;
   double _altitudeCmd;
   int    _nadirCnts;
   int    _nGood;
   int    _medianBeamNo;
   double _obsAltitude;     //Obstacle altitude
   double _obsRange;
   int    _obsCnts;
   double _obsSlopeAlt;
   double _obsSlopeRange;
   int    _obsSlopeCnts;
   double _obsAltAll;
   double _obsRangeAll;
   bool _feedThetaForward;
   double _thetaFF;
   double _flightPathAngle, _avoidAngle;
   double _stime;
   double _x0, _y0, _stime0;
   int _xzMountAngleCnts;
   int _sectorOffset;
   double _maxRange;
   double _rangeEng;
   bool _echoReceived, _first;
   double _depth, _depthCmd, _depthLast;
   double _xLast, _yLast;
   unsigned long _time, _timeLast;

   double _altitudeArray[OA_MAX_BEAMS], _altitudeSlopeArray[OA_MAX_BEAMS];
   double _hRangeArray[OA_MAX_BEAMS], _hRangeSlopeArray[OA_MAX_BEAMS];;
   double _minAltitude, _minSlopeAlt, _minAltAll;

   double _altitudeArrayLast[OA_MAX_BEAMS];
   double _altitudeArrayLastLast[OA_MAX_BEAMS];

   timespec _lastSampleTime, _lastSampleTimeFwd;
   double _pingTime, _lastPingTime, _fwdPingTime, _lastFwdPingTime;

   timespec _localEpoch;
   long _epochOffset;

   short _tindxMinAltitude, _tindxLast, _numberOfPings;
   short _tindxMinSlopeAlt, _tindxMinAltAll ;

   unsigned short _nbeams;
   const bool _useFwd;

   float _BeamRanges[OA_MAX_BEAMS];

   long _noResponseAft, _noResponseFwd;
   bool _firstSurfaceDetect;
   double _minBeamRange;
   int    _minBeamCnts;
   double _thetaB, _lsAlt;
   short _beamOverlap, _fwdCutover;
   bool _valid, _avoidAngleActive, _avoidRangeActive;
   double _maxPitch;

   bool _useThetaB, _firstFilter;
   double _x, _xNext, _pole, _gain, _tau, _Ts, _y;

   ObsAvoidLog::oaLog oaData;
   ObsAvoidLog *_log;

};

#endif