/****************************************************************************/
/* 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 "WaypointWallLog.h"
#include "ourTypes.h"
#include "Behavior.h"
#include "MathP.h"

#define BAD_BEAM_RANGE 0
#define MAX_BEAM_RANGE 200.
#define NUM_BEAMS 4
#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 WaypointWallLog log;

protected:

  void processDvl( Boolean*, Boolean*, double* );
  void processMultibeamer( Boolean*, Boolean*, double* );
  void processEchoSounder( Boolean*, Boolean*, double* );

  double _northing;
  double _easting;
  double _latitude;
  double _longitude;
  double _speed;
  double _depth;
  double _bearing;
  double _gain;
  double _standOff;
  double _sonarTimeOut;
  double _lastPingTime, _lastUnixPingTime;
  double _rangeToWall;
  double _xTrkOffset;
  double _maxXte;
  double _wallInter, _wallSlope;
  Boolean _first;
  Boolean _sonarIsAlive;
  Boolean _sonarHasNewData;
  Boolean _abortOnTimeOut;
  enum RangeFinder {Dvl=1, Multibeamer, EchoSounder};
  long _rangeInst;
  DynamicControlIF::HorizontalMode _hMode;
//
  DvlSideIF::Data _dvlData;
  Boolean _dvlHasNewData;
  Boolean _dvlIsAlive;
  unsigned int _consecGoodHits[NUM_BEAMS];
  const double cos30;
};



#endif