#include "WaypointWall.h"
#include "FloatAttribute.h"
#include "AngleAttribute.h"
#include "BooleanAttribute.h"
//#include "StringAttribute.h"
#include "IntegerAttribute.h"
#include "Syslog.h"
#include "WorkSiteIF.h"
#include "DvlSideIF.h"
#include "MultibeamerIF.h"
#include "NavUtils.h"
#include "TimeP.h"

#ifndef PI
#define PI     3.14159265358979323846
#endif
#define R2D(r) (r*180.0/PI)

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

#define NotSpecified -100000.0
#define AngleNotSpecified ((NotSpecified) / Math::RadsPerDeg)
//#define DEFAULT_STRING_ATTR "\"\""
#define DEFAULT_STRING_ATTR "DefaultString"

//allocate statics for logging
int WaypointWall::_logFileRefCnt;
WaypointWallLog *WaypointWall::_log;

WaypointWall::WaypointWall()
   : Behavior(WaypointWallBehaviorName, Sequential), 
     cos30(cos(PI/6.))
{
  attributes.add(new FloatAttribute("northing", 
				    "Northing (or specify latitude/longitude)",
				    &_northing, NotSpecified));

  attributes.add(new FloatAttribute("easting", 
				    "Easting (or specify latitude/longitude)",
				    &_easting, NotSpecified));

  attributes.add(new AngleAttribute("latitude", 
				    "Latitude (or specify northing/easting)",
				    &_latitude, AngleNotSpecified));

  attributes.add(new AngleAttribute("longitude", 
				    "Longitude (or specify northing/easting)",
				    &_longitude, AngleNotSpecified));

  attributes.add(new AngleAttribute("lastLatitude", 
				    "Last Latitude (or specify northing/easting)",
				    &_lastLatitude, AngleNotSpecified));

  attributes.add(new AngleAttribute("lastLongitude", 
				    "Last Longitude (or specify northing/easting)",
				    &_lastLongitude, AngleNotSpecified));

  attributes.add(new FloatAttribute("speed", "Speed", &_speed));

  attributes.add(new FloatAttribute("initialDepth", "Initial Depth", 
				    &_initialDepth));

  attributes.add(new FloatAttribute("finalDepth", "Final Depth", 
				    &_finalDepth));

  attributes.add(new FloatAttribute("standOff", "StandOff", &_standOff, NotSpecified));

  attributes.add(new FloatAttribute("sonarTimeOut", "SonarTimeOut", 
                                     &_sonarTimeOut, NotSpecified));

  attributes.add(new FloatAttribute("maxCrossTrackError",  "Max Cross Track Error", 
				    &_maxXte, NotSpecified));

  attributes.add(new FloatAttribute("starboardMaxCrossTrackError",  "Starbrd Max Cross Track Error", 
				    &_maxXteStar, NotSpecified));

  attributes.add(new BooleanAttribute("abortOnTimeout",
				      "Should mission abort if waypoint "
				      "times out?",
				      &_abortOnTimeout,
				      False));

  attributes.add(new BooleanAttribute("bergToStarboard",
				      "Default is to port",
				      &_starboard,            
				      False));
//
// Type of projection for the range error:
// Waypoint=1; Body=2; Minimum=3.
  attributes.add(new IntegerAttribute("projection",  
				      "Type of range error", 
                                      &_projection, 
				      3));
//
// Dvl=1; Reson=2; DeltaT=3.
  attributes.add(new IntegerAttribute("rangeInst",  "Range Instrument", 
                                      &_rangeInst, 3));
//
  attributes.add(new IntegerAttribute("windowLen",  "Median Filter Length", 
                                      &_windowLen, 1));

  attributes.add(new BooleanAttribute("useWallEstimator",
				      "Otherwise use waypoints.",
				      &_useWallEstimator,            
				      False));

  attributes.add(new BooleanAttribute("wallFeedForward",
				      "Otherwise zero it.",
				      &_wallFeedForward,            
				      False));

  attributes.add(new AngleAttribute("initialBearing",  "initial Bearing", 
				    &_initBearing, AngleNotSpecified));

  attributes.add(new FloatAttribute("maxAqDist",  
				    "Maximum initial acquisition distance", 
				    &_maxAqDist, NotSpecified));

  attributes.add(new FloatAttribute("detectRange",  
				    "Wall range where this behavior terminates.", 
				    &_detectRange, 500.));
//
//Beware that these attributes are not parsed until after this constructor
//finishes - so you can't use it in a calculation here.
//
//  attributes.add(new StringAttribute("rangeInst", "Range Instrument",));


  for( int i=0; i<NUM_BEAMS; i++ )
  {
     _consecGoodHits[i] = 0;
  }

  _first = True;

  _hMode = DynamicControlIF::WaypointWall;
  _sonarIsAlive = True;

  _depth = 0.;

  _beamIndx = 60;

  _xteWall = 0.;

  _deltaPsiFF = 0.;

  _RHT = _LHT = False;

  _minAllowableRange = .1;

  _initializeWallBearing = False;

  _acquire = False;

  _firstAbsence = True;

  _detectRange = 500.;

  //set up logging
  if (_log == NULL) _log = new WaypointWallLog(DataLog::BinaryFormat);
  _logFileRefCnt++;
}

WaypointWall::~WaypointWall()
{
   //
   //Don't delete _dvlSide because subsequent behaviors may use it, assuming
   //this destructor runs when this behavior ends.  Or does it not run until
   //the whole stack is done ??

   //
   //if( _dvlSide ) delete _dvlSide;
   _logFileRefCnt--;
   if (_logFileRefCnt == 0 && _log) {
	    delete _log;
	    _log = NULL;
   }
}

void WaypointWall::execute( void )
{
  double dn, de;
  double dtw, goal;
  double ye;
  Boolean outOfTheLane = False;
  Boolean debug = True;
  Boolean gotReturn = False;
  double now = _missionClock->seconds();

  NavigationIF::Position position;
  NavigationIF::Attitude attitude;
  // Get current position
  _navigation->state(&position, &attitude);

  _heading = attitude.yaw;

  if(_first && _initBearing != NotSpecified )
  {
     _northing = position.x + _maxAqDist*cos(_initBearing);
     _easting  = position.y + _maxAqDist*sin(_initBearing);
     Syslog::write("WaypointWall::Setting the northing and easting for "
		   "acquistion at t=%.2f sec.", now);
     Syslog::write("WaypointWall::_initBearing=%.2f, "
		   "NotSpecified = %.2f", _initBearing, 
		   NotSpecified);
  }
  /* distance to waypoint in N, E coords */
  dn = position.x - _northing;
  de = position.y - _easting;

  if(_first)
  {
     //
     // Determine the minimum depth.  This CANNOT be done in the constructor
     // because the "attributes" aren't initialized yet.
     _minDepth = _finalDepth;
     if( _initialDepth < _finalDepth ) _minDepth = _initialDepth;

     //
     // This is Steve R's psi_ref, that is, _bearing = psi_ref.
     //
     _bearing = PI + Math::modPi( atan2(_easting - position.y,
					_northing - position.x) - PI);
     //
     // If the vehicle has cross-track error because it is either wall
     // following, or pinned up against a corridor boundary, compute the next
     // bearing from the previous waypoint, not the vehicle's current
     // location.  This smoothly connects the waypoints and corridor boundaries.
     //
     if( _lastLatitude != NotSpecified && _lastLongitude != NotSpecified &&
	_initBearing == NotSpecified )
     {
	_bearing = PI + Math::modPi( atan2(_easting  - _lastEasting,
					   _northing - _lastNorthing) - PI);
     }

     dtw = sqrt( dn*dn + de*de );

     if( dtw < 1.0 )
     {
	//
	// This is here to prevent a divide by zero in the slope calcuation
	// that follows.
	Syslog::write("WaypointWall -- ERROR.  The distance from the start\n"
		      "to the next waypoint is less than one meter.");
	//abortMission();
	return;
     }
     _slope = (_initialDepth - _finalDepth)/dtw;

     if( _speed < 0.2 )
     {
	Syslog::write("WaypointWall -- ERROR.  Commanded speed = %7.4f,"
		      " which is too small.", _speed);
	//abortMission();
	return;
     }
     //
     // Compute the climb rate, taking into account the speed commanded for
     // this waypoint. Abort if it's larger than the limit specified by
     // vehicle.cfg.
     //
     double deltah    = _slope*dtw;
     double sRange    = sqrt( dtw*dtw + deltah*deltah );  //dtw >= 10.0 above
     //
     // Let theta be the steady-state flight-path angle.  Then
     // Zdot = U sin( theta )
     //      = U slope dtw / sRange.
     // where dtw is the horizontal distance and sRange is the (slant) dist.
     double climbRate = _speed * _slope * dtw / sRange;
     if( fabs(climbRate) > _vehicleConfig->maxDiveRate())
     {
	Syslog::write("WaypointWall -- ERROR.  Waypoint slope = %7.4f,"
		      " causes a climb rate of %7.4f,\n which is too large. ",
		      _slope, climbRate);
	//abortMission();
	return;
     }
    //
    // Please DO NOT CHANGE the write statement below.  It is automatically
    // read out of syslog by a shell script, and read into the plotting 
    // routines.  Any changes will disrupt the plotting routines.  Contact
    // Rob McEwen if you need to change this.
    //
     Syslog::write( "WaypointWall initialization: \n"
		    "  Begin waypoint control at t= %-15.2f"
		    "                   (wplog)\n"
		    "  The current location (N,E) = %-15.1f, %-15.1f  (wplog)\n"
		    "  The next waypoint          = %-15.1f, %-15.1f  (wplog)\n"
		    "  The bearing to the next w.p. is %.1f Degrees.\n" 
		    "  The distance to the next w.p. is %.1f Meters.\n"
		    "  The slope to the next w.p. is %7.4f.\n" 
		    "  The initial commanded depth is %.1f meters.\n" 
		    "  The final commanded depth is %.1f meters.\n",
		    _missionClock->seconds(),
		    position.x, position.y, 
		    _northing, _easting, R2D(_bearing), dtw, _slope,  
		    _initialDepth, _finalDepth);

    _rangeToWall = _standOff;
    //_first = False;
    Syslog::write( "WaypointWall:: standOff = %5.2f at %.2f", _standOff, now);
    Syslog::write( "WaypointWall:: projection = %d at %.2f", _projection, now);
    Syslog::write( "WaypointWall:: windowLen = %d at %.2f", _windowLen, now);
    if( _initBearing != NotSpecified ) 
       Syslog::write( "WaypointWall:: detectRange = %.2f at %.2f", _detectRange, now);

    _halfWindowLen = (_windowLen-1)/2;


    _phiMF   = 0.;
    _thetaMF = 0.;
    _psiMF   = 0.;

    _deltaT->get(DeltaTIF::Side, &_deltaTSideData);
    _tbeams = _deltaTSideData.nbeams;

    for( int i=0;i<_tbeams;i++)
    {
       _BeamRanges[i] = &(_deltaTSideData.beam_ranges[i]);
    }

    if( _deltaT->enabled(DeltaTIF::Fwd) )
    {
       Syslog::write("WaypointWall::Forward-looking DeltaT IS enabled at "
		     "t=%.2f sec.", now);
       _deltaT->get_mounting_angles(DeltaTIF::Fwd, &_phiMF, &_thetaMF, &_psiMF);
       _deltaT->get(DeltaTIF::Fwd,  &_deltaTFwdData);
       _tbeams += _deltaTFwdData.nbeams;

       int j=0;
       for( int i=_deltaTSideData.nbeams; i<_tbeams; i++)
       {
	  _BeamRanges[i] = &(_deltaTFwdData.beam_ranges[j]);
	  j++;
       }
    }
    else
    {
       Syslog::write("WaypointWall::Forward-looking DeltaT IS NOT enabled.");
    }

    Syslog::write("WaypointWall::Total number of beams is %d, "
		  "t=%.2f sec.", _tbeams, now);

    //
    //Encode switches with bit masking:
    _switches = 0;
    if(_useWallEstimator) _switches |= (1<<0);
    if(_wallFeedForward)  _switches |= (1<<1);

    if( _initBearing != NotSpecified) 
    {
       _acquire = True;
       Syslog::write("WaypointWall - Wall not yet detected.  Fly the "
		     "initial bearing; t=%.2f sec.\n", now);
    }

    _wallPresent = False;

  } //if(_first)

  //check to see if we timed out
  if ( _abortOnTimeout && ( now - startTime() > _timeout)) 
  {
	Syslog::write("WaypointWall::execute() -- timed out without reaching "
		      "goal, t=%.2f sec.\n", now);
	setState(Finished);
	abortMission();
	return;
  }

  /* transform into dtw, xte coordinates */
  dtw = -de*sin(_bearing) - dn*cos(_bearing);

  if( _initBearing != NotSpecified && !_acquire )
  {
     //
     // Setting dtw after _acquire changes to false allows this behavoir to
     // run once with hMode==WaypointWall, which initializes _wallBearing in
     // DynamicControlServer.
     Syslog::write("WaypointWall::execute() -- Terminating  "
		   "acquistion at=%.2f sec.", now);
     dtw = 0.;
  }



  // The final argument is the beam number, where one count equals one degree.

  processDeltaT( &_sonarIsAlive, &_sonarHasNewData, &_rangeToWall, &_deltaPsiFF,
		 &_beamIndx);

  const short  fovMargin  = 15;
  const int    avoidAngle = 150;    // Psi = 0 in B
  const double avoidDist  = 100.;   //meters
  const int    avoidCount = 5;
  const int    detectCount= 30;

  _counter    = 0;

  double xteWay;
  //
  // Compute cross-track error from the line.  This includes a sign flip to
  // make negative feedback.
  xteWay = dn*sin(_bearing) - de*cos(_bearing);

  if( _sonarHasNewData )
  {
     _initializeWallBearing = False;
     _RHT = False;
     _LHT = False;
	
     for( int i=0; i<_tbeams; i++ )  
     {
	if( *_BeamRanges[i] > _minAllowableRange ) _counter++;
     }
     if( _counter <=5 )
     {
	// If the sonar doesn't see the wall skip the rest and fly the
	// left-hand edge.
	_wallPresent = False;
     }
     else
     {
	_wallPresent = True;
	if( _deltaT->enabled(DeltaTIF::Fwd) )
	{
	   _counter = 0;
	   for( int i=avoidAngle; i<_tbeams; i++ )   //avoidAngle = 150 = 0 deg in B.
	   {
	      if( *_BeamRanges[i] > _minAllowableRange && *_BeamRanges[i] < avoidDist ) 
	      {
		 _counter++;
	      }
	   }
	   if( _counter >= avoidCount ) 
	   {
	      //
	      // If the sonar sees a wall ahead or to the right, turn right
	      // and skip the rest:
	      _RHT = True;
	   } 
	   else if( _beamIndx > _tbeams - 135 &&           //240-135 = 105 = -45 deg in B.
		    _beamIndx < _tbeams - 45)              //240-45  = 195 = +45 deg in B.
	   {
	      _RHT = True;         // -45 < PsiMin < 45 deg in B
	   }
	}
	else
	{
	   if( _beamIndx > _tbeams - fovMargin ) _RHT = True;
	}

	if( _beamIndx < fovMargin ) _LHT = True;
     }
     //
     // Make the above true for ngood>=5
     //
     _xteWall = _standOff - _rangeToWall;

     //if( _starboard ) _xteWall *= -1;
  }

  //
  // Now check to see if the vehicle will be *commanded* out of the
  // lane. "Captain, you are out of the lane!"
  //
  //  -y_cmd = - Delta_y + rangeToWall - standoff 
  //         = - Delta_y - xteWall
  //         =   xteWay  - xteWall
  //
  // is the commanded location. xteWay has already been multipled by -1 for
  // negative feedback, hence the -y_cmd.
  //
  // Check to see if commanded out of the port side of the lane:
  //
  //if( xteWay - _xteWall >= _maxXte )
  //if( xteWay >= _maxXte && xteWay - _xteWall >= _maxXte )
  //else if( xteWay - _xteWall <= -_maxXteStar || _rangeToWall == 0. )
  //else if( (xteWay <= -_maxXteStar
  //       && xteWay - _xteWall <= -_maxXteStar)
  //       || _rangeToWall == 0. )

  _hMode = DynamicControlIF::WaypointWall;
  //
  // The sonar has stopped responding. Either abort or return to line.
  if( !_sonarIsAlive ) 
  {
     Syslog::write("WaypointWall - One of the multibeam sonars has not responded "
		   "for too long.\n  Fly down the center of the corridor.");
     _xTrkOffset = 0.;
     _deltaPsiFF = 0.;
     _LHT = False;
     _RHT = False;
     _hMode = DynamicControlIF::WaypointDepth;
  }
  else if( !_wallPresent )
  {
     if( _firstAbsence )
     {
	_dtw0 = dtw;
	_firstAbsence = False;
     }
     _xTrkOffset = xteWay;   //Fly the center. _maxXte is the left edge.
     _deltaPsiFF = 0.;
     _LHT = False;
     _RHT = False;
     //_xTrkOffset = xteWay - 10*sin( (dtw-_dtw0)*2.*PI/50. );
     if( xteWay - _xTrkOffset >= _maxXte )
     {
	_xTrkOffset = xteWay - _maxXte;
	outOfTheLane = True;
     }
     else if( xteWay - _xTrkOffset <= -_maxXteStar )
     {
	_xTrkOffset = xteWay + _maxXteStar;
	outOfTheLane = True;
     }
  }
  else if( xteWay - _xteWall >= _maxXte )
  {
     _xTrkOffset = xteWay - _maxXte;
     outOfTheLane = True;
  }
  else if( xteWay - _xteWall <= -_maxXteStar )
  {
     _xTrkOffset = xteWay + _maxXteStar;
     outOfTheLane = True;
  }
  else
  {
     _xTrkOffset = _xteWall;
  }

  //
  //Since here dtw is the projection along the line of bearing, only
  //require that the vehicle pass the waypoint, which is equivalent to
  //dtw<0.
  //
  goal = 0.;

  /* load outputs */
  if ( dtw <= goal ) 
  {

    setState(Finished);
    //
    // Please to not modify the following write.
    //
    dprintf(" WaypointWall has set the state to finished.\n");

    Syslog::write( "WaypointWall (%.1f, %.1f), \n"
		   "  reached at t = %-15.2f"
		   "                                 (wplog)\n", 
                    _northing, _easting, _missionClock->seconds());
    Syslog::write( "Vehicle Position is error is (%.1f, %.1f)\n",
                    _northing-position.x, _easting-position.y);
  }
  else 
  {
    /* load command vector */

    if (_depth != NotSpecified)
    {
       _depth = _finalDepth + _slope*dtw;
       setVertical(DynamicControlIF::Depth, _depth, 0., 0., 
		   (long) _beamIndx, _rangeToWall);
    }

    setSpeed(DynamicControlIF::Speed, _speed);
    //
    // Set the other switches:
    // 0 useWallEstimator
    // 1 wallFeedForward
    // 2 outOfTheLane
    // 3 LHT
    // 4 RHT
    // 5 initializeWallBearing
    // 6 wallPresent
    //
    if(outOfTheLane) _switches |=  (1<<2);
    else             _switches &= ~(1<<2);

    if(_LHT)         _switches |=  (1<<3);
    else             _switches &= ~(1<<3);

    if(_RHT)         _switches |=  (1<<4);
    else             _switches &= ~(1<<4);

    if(_initializeWallBearing) _switches |=  (1<<5);
    else                       _switches &= ~(1<<5);

    if(_wallPresent) _switches |=  (1<<6);
    else             _switches &= ~(1<<6);
    //
    // Use newEasting to pass the switches.
    double newEasting = _switches;

    double newNorthing = 0.;
    //
    // use newBearing to pass deltaPsiFF.
    double newBearing = _deltaPsiFF;

    setHorizontal(_hMode, _bearing, _northing, _easting,
		       newBearing, newNorthing, newEasting, 
		       _first, _xTrkOffset);
  }

  _log->setFields(this);
  _log->write();
  _first = False;
}


Boolean WaypointWall::shouldBehaviorStart()
{
  return bothSequence();
}

void WaypointWall::processDvl( Boolean *sonarIsAlive, 
                               Boolean *sonarHasNewData, 
                               double *rangeToWall )
{
  //
  // Compute range to the wall from the side-looking Dvl:
  DeviceIF::Status status = _dvlSide->get(&_dvlData, &_dvlHasNewData);
  
  double ctheta, stheta, cphi, sphi;
  int numValidBeams=0;
  double beamRange[NUM_BEAMS], h[NUM_BEAMS];
  double aveAlt = 0., minRange = 500.;
  Boolean useMinBeam = False;
  int minIndx = -1;
  //
  TimeIF::TimeSpec now;
  double nowUnix;
  Time::gettime(&now);
  nowUnix = Time::seconds(&now);
  //
  // Ignore the _dvlHasNewData passed by get() and flag new data here.  Use a !=
  // rather than an < in the "if" below so that a Dvl clock rollover doesn't
  // set _dvlHasNewData to False.
  //
  *sonarHasNewData = False;
  if( _lastPingTime != _dvlData.pingTime )
  {
    *sonarHasNewData  = True;
    *sonarIsAlive     = True;
    _lastPingTime     = _dvlData.pingTime;
    _lastUnixPingTime = nowUnix;
  }
  //
  // Ensure that the Dvl is still working.
  //
  if( nowUnix > _lastUnixPingTime + _sonarTimeOut ) *sonarIsAlive = False;

  if( *sonarIsAlive && *sonarHasNewData )
  {
#if 0
     ctheta = cos(attitude.pitch);
     stheta = sin(attitude.pitch);
     cphi   = cos(attitude.roll);
     sphi   = sin(attitude.roll);
#endif
     //
     // Unfortunately the beams are not declared as an array in the DvlSideIF.idl.

     // keep running count of number of consecutive good hits for filtering
     (_dvlData.beam1 != BAD_BEAM_RANGE) ? _consecGoodHits[0]++:_consecGoodHits[0]=0;
     (_dvlData.beam2 != BAD_BEAM_RANGE) ? _consecGoodHits[1]++:_consecGoodHits[1]=0;
     (_dvlData.beam3 != BAD_BEAM_RANGE) ? _consecGoodHits[2]++:_consecGoodHits[2]=0;
     (_dvlData.beam4 != BAD_BEAM_RANGE) ? _consecGoodHits[3]++:_consecGoodHits[3]=0;

     // Prevent rollover
     if (_consecGoodHits[0] >= MIN_GOOD_HITS_WALL) 
	_consecGoodHits[0] = MIN_GOOD_HITS_WALL;
     if (_consecGoodHits[1] >= MIN_GOOD_HITS_WALL) 
	_consecGoodHits[1] = MIN_GOOD_HITS_WALL;
     if (_consecGoodHits[2] >= MIN_GOOD_HITS_WALL) 
	_consecGoodHits[2] = MIN_GOOD_HITS_WALL;
     if (_consecGoodHits[3] >= MIN_GOOD_HITS_WALL) 
	_consecGoodHits[3] = MIN_GOOD_HITS_WALL;
 
     beamRange[0] = _dvlData.beam1;
     beamRange[1] = _dvlData.beam2;
     beamRange[2] = _dvlData.beam3;
     beamRange[3] = _dvlData.beam4;

     //mark beam range as bad if it hasn't tracked the bottom for more than
     //MIN_GOOD_HITS number of bottom detects
     if (_consecGoodHits[0] < MIN_GOOD_HITS_WALL)  beamRange[0] = BAD_BEAM_RANGE;
     if (_consecGoodHits[1] < MIN_GOOD_HITS_WALL)  beamRange[1] = BAD_BEAM_RANGE;
     if (_consecGoodHits[2] < MIN_GOOD_HITS_WALL)  beamRange[2] = BAD_BEAM_RANGE;
     if (_consecGoodHits[3] < MIN_GOOD_HITS_WALL)  beamRange[3] = BAD_BEAM_RANGE;
     //
     //
     // Beam numbering convention, looking through Dvl from behind.  x and z are
     // the vehicle axes.
     //
     //                x
     //                ^ 
     //                |
     //              1 | 3
     //                |------> z
     //              4   2
     //
     // h[i] is the the z component of each beam's unit vector (in N).
     // Multiply by the measured range to get the altitude of the i^th beam
     // assuming a planar level bottom.
     //
     //h[0] = ( -stheta*c3s2  -  ctheta*sphi*s3s2  +  ctheta*cphi*c2 )*_dvlData.beam1;
     //h[1] = (  stheta*c3s2  +  ctheta*sphi*s3s2  +  ctheta*cphi*c2 )*_dvlData.beam2;
     //h[2] = ( -stheta*c3s2  +  ctheta*sphi*s3s2  +  ctheta*cphi*c2 )*_dvlData.beam3;
     //h[3] = (  stheta*c3s2  -  ctheta*sphi*s3s2  +  ctheta*cphi*c2 )*_dvlData.beam4;
     //
     h[0] = _dvlData.beam1*cos30;
     h[1] = _dvlData.beam2*cos30;
     h[2] = _dvlData.beam3*cos30;
     h[3] = _dvlData.beam4*cos30;

     for( int i=0; i<NUM_BEAMS; i++ ) 
     {
	/*
	** Get range from beam i
	*/
	if (beamRange[i] != (double) BAD_BEAM_RANGE) 
	{
	   numValidBeams++;
	   /*
	   ** Sum up all ranges, or take the minimum range of the four beams
	   */
	   if( useMinBeam ) 
	   {
	      if( beamRange[i] < minRange ) 
	      {
		 minIndx = i;
		 minRange = beamRange[i];
	      }
	   }
	   else 
	   {
	      aveAlt += h[i];
	   }  // if( useMinBeam ) 
	}  // if (beamRange[i] != (double) BAD_BEAM_RANGE) 
     }  // for( i=0; i<NUM_BEAMS; i++ ) 
     //
     // Select either the minimum altitude or the average of the altitudes
     // computed from the good (nonzero) beam ranges. 
     //
     if(numValidBeams)
     {
	//
	// Compute the range to the wall in vehicle coordinates.
	if( useMinBeam )  *rangeToWall =  h[minIndx];
	else              *rangeToWall =  aveAlt / ( (double) numValidBeams );
     }
     else
     {
	//
	// There wasn't a return.  Interpret this as the wall being out of
	// range.  Add code later to determine if instead the wall is too
	// close.  Another cause for no return is the wall is angled >20
	// degrees from the Dvl centerline.

	*rangeToWall = MAX_BEAM_RANGE * cos30;
     }
  }//  if( *sonarIsAlive && *sonarHasNewData )
}

void WaypointWall::processMultibeamer( Boolean *sonarIsAlive, 
                                       Boolean *sonarHasNewData, 
                                       double  *rangeToWall )
{
   TimeIF::TimeSpec updateTime;
   Boolean ready;
   double pingTime;

   _multibeamer->get_status( rangeToWall, &updateTime, sonarIsAlive, &ready);
   
   pingTime = Time::seconds(&updateTime);

   if( _lastPingTime != pingTime )
   {
      *sonarHasNewData   = True;
      _lastPingTime      = pingTime;
   }
   else
   {
      *sonarHasNewData   = False;
   }
}

const short maxWindowLen = 31;

void WaypointWall::processDeltaT( Boolean *sonarIsAlive, 
                                  Boolean *sonarHasNewData, 
                                  double  *rangeToWall,
				  double  *deltaPsiFF,
				  short *beamIndx
 )
{
  short startIndx;
  short nbeams = _deltaTSideData.nbeams;
  short centerIndx = nbeams/2, offsetIndx = 0;
  long medianArray[maxWindowLen];
  short scale = 1000;
  double minBeamRange;
  const double initMinBeamRange = 500.;
  int minIndx;
  int i;
  Boolean medianFilter = False;
  //
  TimeIF::TimeSpec now;
  double nowUnix, pingTime, pingFwdTime;
  Time::gettime(&now);
  nowUnix = Time::seconds(&now);
  //
  // Compute range to the wall:
   _deltaT->get(DeltaTIF::Side, &_deltaTSideData);

   if( _deltaT->enabled(DeltaTIF::Fwd) )
   {
      _deltaT->get(DeltaTIF::Fwd,  &_deltaTFwdData);
   }

  pingTime = Time::seconds(&(_deltaTSideData.update_time));

  *sonarHasNewData = False;
  if( _lastPingTime != pingTime )
  {
    *sonarHasNewData  = True;
    *sonarIsAlive     = True;
    _lastPingTime     = pingTime;
    _lastUnixPingTime = nowUnix;
  }

  Boolean fwdSonarHasNewData = False;
  if( _deltaT->enabled(DeltaTIF::Fwd) )
  {
     pingFwdTime  = Time::seconds(&(_deltaTFwdData.update_time));
     if( _lastFwdPingTime != pingFwdTime )
     {
	 fwdSonarHasNewData  = True;
	*sonarIsAlive        = True;
	_lastFwdPingTime     = pingFwdTime;
	_lastFwdUnixPingTime = nowUnix;
     }
  }
  //
  // Ensure that the side-looking DeltaT is still working.
  //
  if( nowUnix > _lastUnixPingTime + _sonarTimeOut ) *sonarIsAlive = False;
  //
  // Check to see if the forward-looking DeltaT is working.
  //
  if( _deltaT->enabled(DeltaTIF::Fwd) )
  {
     if( nowUnix > _lastFwdUnixPingTime + _sonarTimeOut ) *sonarIsAlive = False;
  }


  if( *sonarHasNewData || fwdSonarHasNewData )
  {
     switch( _projection )
     {
	case Waypoint:
	default:
	   //
	   // Compute the direction perpendicular to the line of bearing,
	   // assuming one beam per degree, and -Fov (beam 0) points aft.
	   // 
	   offsetIndx = (short) ( (Math::modPi(_heading) - Math::modPi(_bearing))*Math::DegsPerRad );
	   *beamIndx = centerIndx - offsetIndx;
	   break;


	case Body:
	   *beamIndx = centerIndx;
	   break;


	case Minimum:
	   //
	   // Locate the minimum range.
	   minBeamRange = initMinBeamRange;
	   *beamIndx = 0;
	   for( i=0; i<_tbeams; i++ )
	   {
	      if( *_BeamRanges[i] < minBeamRange && 
	          *_BeamRanges[i] > _minAllowableRange )
	      {
		 minBeamRange = *_BeamRanges[i];
		 *beamIndx=i;
	      }
	   }
	   break;
     }// switch

     if( *beamIndx >= _tbeams ) *beamIndx = _tbeams-1;
     else if( *beamIndx < 0 ) *beamIndx = 0;

     if( _windowLen > 1 )
     {
	//
	// Median filter.  Select halfWindowLen neighboring beams to either side
	// of the minimum range beam.  Handle the case where *beamIndx is within
	// halfWindowLen of either end of the array.

	if( *beamIndx < _halfWindowLen ) startIndx = 0;

	else if( *beamIndx > _tbeams - _halfWindowLen - 1 ) startIndx = _tbeams - _windowLen;

	else startIndx = *beamIndx - _halfWindowLen;

	//
	// Find the median in the window
	for( i=0; i < _windowLen; i++ )
	{
	   medianArray[i] = (long) (*_BeamRanges[startIndx + i]*scale);
	}

	Math::shellSort( medianArray, _windowLen );

	*rangeToWall = ((double) medianArray[_halfWindowLen+1])/((double) scale);
     }
     else
     {
	*rangeToWall = (double) *_BeamRanges[*beamIndx];
     }

     if(minBeamRange == initMinBeamRange) 
	*deltaPsiFF = 0.;
     else  
	*deltaPsiFF  = (double) (*beamIndx - 60)*PI/180.;

  }//  if( *sonarIsAlive && *sonarHasNewData )
}



Boolean WaypointWall::validInput()
{
  Boolean debug = False;
  Boolean valid = True;
  WorkSiteIF workSite("workSite");

  dprintf("WaypointWall::validInput() - _northing: %.2f, _easting: %.2f",
	  _northing, _easting);

  dprintf("WaypointWall::validInput() - _latitude: %.2f, _longitude: %.2f",
	  _latitude, _longitude);

  Boolean _utmSpecified = False;
  Boolean _geographicSpecified = False;

  if (_northing != NotSpecified || _easting != NotSpecified)
    _utmSpecified = True;

  if (_latitude != NotSpecified || _longitude != NotSpecified)
    _geographicSpecified = True;

  if (_utmSpecified && _geographicSpecified) {
    printError("Can't specify both UTM and geographic coords");
    valid = False;
  }

  Boolean coordsSpecified = False;
  if( _initBearing != NotSpecified )
  {
     // OK, this is an acquisition run.  Waypoints aren't specified.
     if( _maxAqDist == NotSpecified)
     {
	valid = False;
	printError("You must specify both initialBearing "
		   "and maxAqDist");
     }
     else
	coordsSpecified = True;
  }
  else
  {
     //
     // Waypoint location must be specified either in UTM or geographic coords
     if (_northing != NotSpecified && _easting != NotSpecified) 
     {
	coordsSpecified = True;
     }

     if (_latitude != NotSpecified && _longitude != NotSpecified) 
     {
	if (coordsSpecified) 
	{
	   printError("Use either UTM or geographic to specify coords");
	   valid = False;
	}
	else 
	{
	   // Convert lat/lon to UTM
	   NavUtils::geoToUtm(_latitude, _longitude, workSite.utmZone(), 
			      &_northing, &_easting);
	   coordsSpecified = True;
	}
     }
  }


  if (!coordsSpecified) 
  {
    printError("Use either UTM or geographic to specify coords");
    valid = False;
  }

  if (_speed < 0.) 
  {
    printError("Invalid speed");
    valid = False;
  }

//  if (_depth != NotSpecified && _depth < 0.) {
//    printError("Invalid depth");
//    valid = False;
//  }

  if (_abortOnTimeout) {
	_timeout = duration();
	_duration += 10;
  }

  if (_maxXte == NotSpecified || _maxXte < 0.)
  {
    printError("maxCrossTrackError not specified.");
    valid = False;
  }

  if( _maxXteStar != NotSpecified ) 
  {
     if( _maxXteStar < 0.) 
     {
	printError("starboardMaxCrossTrackError must be positive.");
	valid = False;
     }
     else
     {
	Syslog::write("WaypointWall::Setting starboard corridor limit to %5.2f meters.",
		      _maxXteStar);
     }
  }
  else //if not specified:
  {
     _maxXteStar = _maxXte;
  }

  if (_sonarTimeOut == NotSpecified || _sonarTimeOut < 0.)
  {
    printError("sonarTimeOut not specified.");
    valid = False;
  }

  if (_standOff == NotSpecified || _standOff < 0.)
  {
    printError("standOff not specified.");
    valid = False;
  }

  if(_lastLatitude == NotSpecified && _lastLongitude != NotSpecified ||
     _lastLatitude != NotSpecified && _lastLongitude == NotSpecified)
  {
     printError("You must specify both lastLatitude and lastLongitude.");
     valid = False;
  }

  if(_lastLatitude != NotSpecified && _lastLongitude != NotSpecified)
  {
      NavUtils::geoToUtm(_lastLatitude, _lastLongitude, workSite.utmZone(), 
			 &_lastNorthing, &_lastEasting);
      Syslog::write("WaypointWall::Converted lastLatitude = %12.8f degrees to %12.2f m", 
		    _lastLatitude*180/PI, _lastNorthing);
      Syslog::write("WaypointWall::Converted lastLongitude = %12.8f degrees to %12.2f m", 
		    _lastLongitude*180/PI, _lastEasting);
  }


  if( _windowLen < 0 || _windowLen > maxWindowLen || _windowLen % 2 == 0 )
  {
     char errorStr[128];
     sprintf(errorStr,"windowLen must be an odd positive integer <= %d", maxWindowLen);
     printError(errorStr);
     valid = False;
  }

  Syslog::write("WaypointWall::validInput() - Opening IF to rangeInst = %d", 
                _rangeInst);

  Boolean IFopen = False;
  
  switch( _rangeInst )
  {
     case Dvl:
	try 
	{
	   if( !_dvlSide ) _dvlSide = new DvlSideIF("dvlSide");
	   IFopen = True;
	} 
	catch (Exception e) 
	{
	   Syslog::write("WaypointWall--Caught exception on creation "
	   "of DvlIF:%s\n", e.msg);
	   _dvlSide = NULL;
	}
	catch(...)
	{
	   Syslog::write("WaypointWall -- Failed to initialize connection "
	   "to DvlSideIF");
	   _dvlSide = NULL;
	}
	break;
     case Multibeamer:
	try 
	{
	   if( !_multibeamer ) _multibeamer = new MultibeamerIF("Multibeamer");
	   IFopen = True;
	} 
	catch (Exception e) 
	{
	   Syslog::write("WaypointWall--Caught exception on creation "
	   "of MultibeamerIF:%s\n", e.msg);
	   _multibeamer = NULL;
	}
	catch(...)
	{
	   Syslog::write("WaypointWall -- Failed to initialize connection "
	   "to MultibeamerIF");
	   _multibeamer = NULL;
	}
	break;
     case DeltaT:
	try 
	{
	   if( !_deltaT ) _deltaT = new DeltaTIF("DeltaT");
	   IFopen = True;
	} 
	catch (Exception e) 
	{
	   Syslog::write("WaypointWall--Caught exception on creation "
	   "of DeltaTIF:%s\n", e.msg);
	   _deltaT = NULL;
	}
	catch(...)
	{
	   Syslog::write("WaypointWall -- Failed to initialize connection "
	   "to DeltaTIF");
	   _deltaT = NULL;
	}
	break;
     default:
     {
	Syslog::write("WaypointWall:: Error.  You must specify the range sensor.");
	//abortMission();
	valid = False;
     }
     break;
  }

  if( !IFopen )
  {
     Syslog::write("WaypointWall:: Error.  The interface to %d failed to open. "
                   "Aborting.", _rangeInst);
     //abortMission();
     valid = False;
  }
  else
  {
     Syslog::write("Using range finder %d.", _rangeInst);
  }


  return valid;
}