/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : Ascend.h                                                      */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#ifndef _WAYPOINTWALL_H
#define _WAYPOINTWALL_H

#include "ourTypes.h"
#include "Behavior.h"
#include "MathP.h"
#include "WaypointWallLog.h"

#define BAD_BEAM_RANGE 0
#define MAX_BEAM_RANGE 200.
#define NUM_BEAMS 4
#define MAX_BEAMS_DT 240
#define MIN_GOOD_HITS_WALL (4)

#define WaypointWallBehaviorName "waypoint_wall"


/*
CLASS 
WaypointWall

DESCRIPTION Computes a (constant) bearing to a waypoint, and then attempts to
fly that line.  If the side-looking sonar detects a wall, the vehicle will
deviate from the line as necessary to maintain a constant stand-off range.


AUTHOR
Rob McEwen, Tom O'Reilly
*/
class WaypointWall : public Behavior {

friend class WaypointWallLog;

public:

  WaypointWall();
  virtual ~WaypointWall();

  virtual void execute( void );
  virtual Boolean validInput( void );

  virtual Boolean shouldBehaviorStart( void );

  static Boolean AbortRequested;
  
protected:

  void processDvl( Boolean*, Boolean*, double* );
  void processMultibeamer( Boolean*, Boolean*, double* );
  void processDeltaT( Boolean*, Boolean*, double*, double*, short* );
  void calcRangeToWall(int filterType, double *rangeToWall, double *psiWallwrtoVehicle, short *beamindx);

  double _northing, _lastNorthing;
  double _easting, _lastEasting;
  double _latitude, _lastLatitude;
  double _longitude, _lastLongitude;
  double _speed;
  double _depth, _initialDepth, _finalDepth, _minDepth;
  double _bearing;
  double _gain;
  double _standOff;
  double _sonarTimeOut;
  double _lastPingTime, _lastUnixPingTime;
  double _lastFwdPingTime, _lastFwdUnixPingTime;
  double _rangeToWall, _rangeLS, _rangeLSMedian;
  double _xTrkOffset;
  double _maxXte, _maxXteStar;
  double _slope;
  double _timeout;
  double _heading;
  double _phiMF, _thetaMF, _psiMF;
  double _minAllowableRange;
  double _minDist;
  float *_BeamRanges[2*DeltaTIF::MaxBeams];
  float _BeamRangesLast[MAX_BEAMS_DT];
  float _BeamRangesLastLast[MAX_BEAMS_DT];
  float _x[MAX_BEAMS_DT];
  float _y[MAX_BEAMS_DT];
  short _tbeams;
  Boolean _first, _second;
  Boolean _sonarIsAlive, _sonarAliveFlipFlop;
  Boolean _sonarHasNewData;
  Boolean _abortOnTimeout;
  Boolean _useWallEstimator;
  Boolean _wallFeedForward;
  Boolean _LHT, _RHT;
  Boolean _firstAbsence;
  Boolean _wallControlOff;
  unsigned int     _switches;
  double _xteWall, _dtw0;  
  double _psiWallwrtoVehicle;
  double _initBearing, _maxAqDist, _detectRange;
  enum RangeFinder {Dvl=1, Multibeamer, DeltaT};
  long _rangeInst;
  double _minBeamRange;
//enum Projection {Waypoint=1, Body, Minimum, LeastSquares};
  enum FilterType {MinBeam=1, LeastSquares, LeastSquaresMedian};
  double _psiWall_B, _yInterWall_B;
  long _projection, _filterType;
  short _beamIndx, _beamIndxMin, _beamIndxLS, _beamIndxLSMedian;
  long _windowLen, _halfWindowLen;
  long _waypointPriority;
  double _nowUnixDT, _wallAbsentTime, _lastWallTime;
  double _wallBearing;
  DynamicControlIF::HorizontalMode _hMode;
  DynamicControlIF *_dynamicControlIF;

  NavigationIF::Position _position;
  NavigationIF::Attitude _attitude;
//
  DvlSideIF::Data _dvlData;
  DeltaTIF ::Data _deltaTSideData, _deltaTFwdData;
  Boolean _dvlHasNewData, _deltaTHasNewData;
  Boolean _dvlIsAlive, _deltaTIsAlive;
  Boolean _starboard;
  Boolean _initializeWallBearing, _wallPresent, _wallPresentFlipFlop;
  unsigned int _consecGoodHits[NUM_BEAMS];
  const double cos30;
  int _counter;
  short _fwdCutover, _beamOverlap;

  //variables to support logging of behavior state
  static int _logFileRefCnt;
  static WaypointWallLog *_log;
};



#endif