#include "WaypointBottom.h"
#include "FloatAttribute.h"
#include "AngleAttribute.h"
#include "BooleanAttribute.h"
#include "StringAttribute.h"
#include "Syslog.h"
#include "WorkSiteIF.h"
#include "NavUtils.h"
#include "TimeP.h"
#include "VehicleConfigurationServer.h"
#include "LowAltitudeFollowing.h"
#include "MissionTimeAttribute.h"

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

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

//allocate statics for logging
int WaypointBottom::_logFileRefCnt;
int WaypointBottom::_terrainAidIFCnt;
int WaypointBottom::_behaviorCnt;
WaypointBottomLog *WaypointBottom::_log;

WaypointBottom::WaypointBottom()
  : Behavior(WaypointBottomBehaviorName, Sequential)
{
  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("depth", "Depth", &_depth, NotSpecified));

  attributes.add(new FloatAttribute("altitude", "Altitude", &_altitude, 
  NotSpecified));

  attributes.add(new FloatAttribute("wavelength", "Wavelength", 
  &_lambda, NotSpecified));

  attributes.add(new FloatAttribute("amplitude", "Amplitude", &_amp, NotSpecified));

  attributes.add(new FloatAttribute("stime", "Settling Time", &_stime, 0.));

  attributes.add(new BooleanAttribute("feedThetaForward", "Use Feedfoward", 
  &_feedThetaForward, False));

  attributes.add(new BooleanAttribute("quarterWave", "Quarter-wave step", 
  &_quarterWave, False));

  attributes.add(new BooleanAttribute("nativeAltitudeControl", "", 
  &_nativeAltitudeControl, False));

  attributes.add(new FloatAttribute("avoidRange", "Range at which vehicle climbs", 
				    &_avoidRange, NotSpecified));

  attributes.add(new FloatAttribute("maxVertError", "Vertical error", 
				    &_maxVertError, NotSpecified));

  attributes.add(new FloatAttribute("tau", "Filter time constant", 
				    &_tau, 0.));

  attributes.add(new StringAttribute("profileFileName", "Altitude or Depth & Theta FF File", 
  &_baseName, DEFAULT_STRING_ATTR));

  attributes.add(new AngleAttribute("maxLowerPitch",
				    "Lower pitch limit when near bottom.",
				    &_maxLowerPitch, 45. ));

  attributes.add(new BooleanAttribute("useThetaB", "", &_useThetaB, False));

  attributes.add(new BooleanAttribute("reinitFilter", "", &_reinitFilter, False));

  attributes.add(new FloatAttribute("deltaNorthing", "", &_deltaNorthing, 0.));

  attributes.add(new FloatAttribute("deltaEasting", "", &_deltaEasting, 0.));

  _first = True;

  _thetaFF  = 0.;

  _latched = False;
  _run     = False;

  _indxTab = 0;

  _altitudeCmd = 50.;

  _lenTab = 0;

  _initProfile = True;

  _stopProp = False;

  _maxDepthHold = False;

  _depthHysteresis = 0.;

  for( int i=0; i< NTAB; i++ )
  {
     _profileTab[i][0] = 0.;   //distance
     _profileTab[i][1] = 0.;   //thetaFF
     _profileTab[i][2] = 50.;  //depth
     _profileTab[i][3] = 0.;   //thetaT
  }
  _avoidRangeActive = False;
  _depth2AltActive  = False;

  _Ts = .2;

  //_driftPitch = PI/36.;
  _driftPitch = PI/180.;

  _firstFilter = True;

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

  _behaviorCnt++;
  Syslog::write("WaypointBottom - Constructed behavior %d.", _behaviorCnt);


  // Read in the attributes:
  // Attribute::Input *input;
  // input = new Attribute::Input("endTime", NoTimeLimitMnem);
  // attributes.parse(input);
  // delete input;
  // Syslog::write("WaypointBottom - reinitFilter = %d.", _reinitFilter);
  //
  // The above didn't work. ???

}

WaypointBottom::~WaypointBottom()
{
   if( _fpAsc) fclose(_fpAsc);

   _logFileRefCnt--;
   if (_logFileRefCnt == 0 && _log) {
	    delete _log;
	    _log = NULL;
   }
   if( Behavior::_terrainAid )
   {
      Syslog::write("WaypointBottom - Destructing Behavior::_terrainAid.  "
		    "%d interfaces open.",_terrainAidIFCnt);
      delete Behavior::_terrainAid;
      Behavior::_terrainAid = NULL;
      _terrainAidIFCnt--;
   }
   if( Behavior::_dynamicControl )
   {
      Syslog::write("WaypointBottom number %d - Destroying "
		    "dynamicControl interface: Behavior::_dynamicControl.",
		    _behaviorCnt);
      delete Behavior::_dynamicControl;
      Behavior::_dynamicControl = NULL;
   }

   Syslog::write("WaypointBottom - Destroying behavior %d.", _behaviorCnt);
   _behaviorCnt--;
}


void WaypointBottom::execute( void )
{
  double dn, de;
  double dtw, goal;
  Boolean debug = True;
  TimeIF::TimeSpec now;
  Boolean depthBasedAltitudeControl = False;

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

  double measuredDepth    = _navigation->depth();
  double measuredAltitude = _navigation->altitude();
  double obstacleAltitude = position.flsVertRange;
  double obstacleRange    = position.flsHorzRange;
  double thetaB           = position.thetaB;
  double obstacleSlopeAlt = position.obstacleSlopeAlt;
  double obstacleSlopeRange = position.obstacleSlopeRange;

  if(_first)
  {
    _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 )
     {
	_bearing = PI + Math::modPi( atan2(_easting  - _lastEasting,
					   _northing - _lastNorthing) - PI);
     }
    //
    // 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( "WaypointBottom 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", 
                   _missionClock->seconds(),
		   position.x, position.y, 
		   _northing, _easting, R2D(_bearing) );

    TimeIF::TimeSpec now;
    Time::gettime(&now);
    _stime0 = Time::seconds(&now);

    _first = False;
    _depthCmd = _depth;
    _altitudeCmd = _altitude;

    if( _depth != NotSpecified && _amp != NotSpecified && 
    _lambda != NotSpecified )
    {
       Syslog::write("WaypointBottom:: _feedThetaForward = %d",_feedThetaForward);
       Syslog::write("WaypointBottom:: _quarterWave = %d",_quarterWave);
    }
    if( _altitude != NotSpecified ) 
    {
       if( _nativeAltitudeControl )
	  Syslog::write("WaypointBottom:: Native Altitude control.");
       else
	  Syslog::write("WaypointBottom:: Depth-Based Altitude control.");
    }
    else
       Syslog::write("WaypointBottom:: Depth control.");
    //
    // Pass the value through to Multibeam.cc

    Syslog::write("WaypointBottom::execute - in 'first', avoidRange = %.2f", _avoidRange);

    if( _avoidRange != NotSpecified ) _navigation->setAvoidRange( _avoidRange);
    
    // Set up first order LP on obstacle altitude: 
    //
    _x = 0.;
    _y = 0.;
    if(_tau) 
    {
       _pole = exp( -_Ts/_tau);
       _gain =  1. - _pole;
       Syslog::write("WaypointBottom: Initializing obstacle altitude  "
		     "filter:\n\tgain = %.2e, tau = %.2f seconds, "
		     "pole=%.2e.", 
		     _gain, _tau, _pole);

    }
    if(_useThetaB)
    {
       Syslog::write("WaypointBottom::execute - using thetaB.");
    }

    Syslog::write("WaypointBottom - reinitFilter = %d.", _reinitFilter);

    try 
    {
       if( _reinitFilter )
       {
	  if( Behavior::_terrainAid )
	  {
	     Behavior::_terrainAid->reinitFilter();
	     Syslog::write("WaypointBottom - Reinitialized Filter at "
			   "t = %.2f.", _missionClock->seconds());
	  }
	  else
	  {
	     Syslog::write("WaypointBottom - Reinitialisation attempted "
			   "but terrainAidIF not present at t = %.2f.",
			   _missionClock->seconds() );
	  }
	  if( Behavior::_dynamicControl )
	  {
	     Behavior::_dynamicControl->resetTrnOffsets(_deltaNorthing, _deltaEasting);
	     Syslog::write("WaypointBottom - Set map offsets and zeroed Trn biases "
			   "in Dynamic Control at t = %.2f.", _missionClock->seconds());
	  }
	  else
	  {
	     Syslog::write("WaypointBottom - Attempted to set map offsets and zero Trn biases"
			   "but DynamicControlIF not present at t = %.2f.",
			   _missionClock->seconds() );
	  }
       }
    }
    catch (Exception e)
    {
       Syslog::write("WaypointBottom - Attempted to reinitialize "
		     "at t = %.2f: %s", _missionClock->seconds(), e.msg);
    }
    catch (...) 
    {
       Behavior::_terrainAid = NULL;
       Syslog::write("WaypointBottom - Exception caught while Attempting "
		     "to reinitialize at t = %.2f.",_missionClock->seconds());
    }

    try 
    {
       if( Behavior::_terrainAid )
       {
	  //if( _deltaNorthing != 0. || _deltaEasting != 0. )
	  //{
	     Behavior::_terrainAid->offset( _deltaNorthing, _deltaEasting );
	     Syslog::write("WaypointBottom - Offset the Trn filter by "
			   "(%.1f, %.1f) at t = %.2f",
			   _deltaNorthing, _deltaEasting,
			   _missionClock->seconds());
         //}
       }
       else //if( _deltaNorthing != 0. || _deltaEasting != 0. )
       {
	  Syslog::write("WaypointBottom - Offset (%.1f, %.1f) attempted "
			"but terrainAidIF not present at t = %.2f.",
			_deltaNorthing, _deltaEasting,
			_missionClock->seconds());
       }
    }
    catch (Exception e)
    {
       Syslog::write("WaypointBottom - Attempted to offset Trn "
		     "at t = %.2f: %s", _missionClock->seconds(),e.msg);
    }
    catch (...) 
    {
       Behavior::_terrainAid = NULL;
       Syslog::write("WaypointBottom - Exception caught while Attempting to "
		     "offset Trn at t = %.2f.",_missionClock->seconds());
    }

  } //end first
    
  Time::gettime(&now);
  if( !_latched && ( Time::seconds(&now) >= (_stime0 + _stime)) )
  {
     _x0 = position.x;
     _y0 = position.y;
     _run = True;
     _latched = True;
  }


  /* distance to waypoint in N, E coords */
  dn = position.x - _northing;
  de = position.y - _easting;

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


  double h_tg;
  double cmdAlt, dist, dn0, de0;
  //
  // Try adding a first order filter with time constant tau:
  //
  if(_tau && obstacleAltitude != INVALID_ALTITUDE)
  {

      if( _firstFilter )
     {
	_x = measuredAltitude * (1. - _gain)/_gain;
	Syslog::write("WaypointBottom: Initializing _x to %.2f at t = "
		      "%.2f.",_missionClock->seconds(), _x);
	_firstFilter = False;
     }


    double u = obstacleAltitude;

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

	  obstacleAltitude = _y;

	 _log->setFields(this);
	 _log->write();
  }
  //
  // Depth control by either a constant, command generator, or table lookup.
  if( _altitude == NotSpecified )
  {
     _depthCmd = _depth;
     _thetaFF  = 0.;

     if( _run && _amp != NotSpecified && _lambda != NotSpecified )
     {

	dn0 = position.x - _x0;
	de0 = position.y - _y0;

	//dist = dn0*sin(_bearing) + de0*cos(_bearing);
	dist = sqrt( dn0*dn0 + de0*de0 );

	if( !_quarterWave || dist<_lambda/2. )
	{
	   _depthCmd = _depth - _amp*(1.-cos(2.*PI*dist/_lambda));
	   if( _feedThetaForward )
	      _thetaFF  = 2.*PI*_amp/_lambda*sin(2.*PI*dist/_lambda);
	   else
	      _thetaFF = 0.;
	}
	else
	{
	   _thetaFF = 0.;
	}
	if( _initProfile )
	{
	   Syslog::write("WaypointBottom::  Latching position and starting "
			 "command generator at t = %.2f.",_missionClock->seconds());
	   _initProfile = False;
	}
     }
     else if( _run && _fpAsc )
     {
	// If a look-up table exists, use it for depth and thetaFF.
	// _x0 and _y0 aren't valid until _run == True.
	//
	// Distance from start:
	dn0 = position.x - _x0;
	de0 = position.y - _y0;
	dist = sqrt( dn0*dn0 + de0*de0 );

	while( _profileTab[_indxTab+1][0] < dist && _indxTab < _lenTab-2 )  
	{
	   _indxTab++;

	   Syslog::write("WaypointBottom: t = %.2f, _indxTab = %d, "
			 "_profileTab[][0]=%7.2f, dist=%7.2f, "
			 "_thetaFF=%7.2f, _depthCmd = %7.2f",
			 _missionClock->seconds(),
			 _indxTab, _profileTab[_indxTab][0], dist, 
			 _profileTab[_indxTab][1],
			 _profileTab[_indxTab][2]);
	}

	_thetaFF     = (double) _profileTab[_indxTab][1];
	_depthCmd    = (double) _profileTab[_indxTab][2];

	if( dist >= _profileTab[_lenTab-1][0] ) 
	{
	   _depthCmd = (double) _profileTab[_lenTab-1][2];
	   _thetaFF  = 0.;
	}

	if( _maxVertError != NotSpecified && 
	    fabs( measuredDepth - _depthCmd ) > _maxVertError )
	{
	   _thetaFF = 0.;
	}

	if( _initProfile )
	{
	   Syslog::write("WaypointBottom::  Latching position and starting "
			 "profile at t = %.2f.",_missionClock->seconds());
	   _initProfile = False;
	}
     }
  }
  //
  // Altitude-control family:
  else
  {
     _thetaFF = 0.;
     _altitudeCmd = _altitude;

     // If a look-up table exists, use it for altitude and thetaFF.
     // _x0 and _y0 aren't valid until _run == True.
     if( _run && _fpAsc )
     {
	//
	// Distance from start:
	dn0 = position.x - _x0;
	de0 = position.y - _y0;
	dist = sqrt( dn0*dn0 + de0*de0 );

	while( _profileTab[_indxTab+1][0] < dist && _indxTab < _lenTab-2 )  
	{
	   _indxTab++;

	   Syslog::write("WaypointBottom: t = %.2f, _indxTab = %d, "
			 "_profileTab[][0]=%7.2f, dist=%7.2f, "
			 "_thetaFF=%7.2f, _altitudeCmd = %7.2f",
			 _missionClock->seconds(),
			 _indxTab, _profileTab[_indxTab][0], dist, 
			 _profileTab[_indxTab][1],
			 _profileTab[_indxTab][2]);
	}

	_thetaFF     = (double) _profileTab[_indxTab][1];
	_altitudeCmd = (double) _profileTab[_indxTab][2];

	if( dist >= _profileTab[_lenTab-1][0] ) 
	{
	   _altitudeCmd = (double) _profileTab[_lenTab-1][2];
	   _thetaFF = 0.;
	}

	if( _maxVertError != NotSpecified && 
	    fabs( measuredAltitude - _altitudeCmd ) > _maxVertError )
	{
	   _thetaFF = 0.;
	}

	if( _initProfile )
	{
	   Syslog::write("WaypointBottom::  Latching position and starting "
			 "profile at t = %.2f.",_missionClock->seconds());
	   _initProfile = False;
	}
     }

     if( _nativeAltitudeControl )
     {
	if( _avoidRange != NotSpecified && obstacleAltitude < _altitudeCmd )
	{
	   if( obstacleRange < _avoidRange )
	   {
	      //
	      // Overwrite _altitudeCmd if obstacle altitude is less:
	      _altitudeCmd = 2.*_altitudeCmd - obstacleAltitude;
	      if( obstacleRange > 1.0 && _feedThetaForward ) 
	      {
		 _thetaFF  = atan2(_altitudeCmd - obstacleAltitude, obstacleRange);
		 if( _thetaFF >  PI/4. ) _thetaFF =  PI/4.;
		 if( _thetaFF < -PI/4. ) _thetaFF = -PI/4.;
	      }
	   }
	}
     }
     else // Depth-based altitude control:
     {
	double depthToAltitude     = measuredDepth + measuredAltitude - _altitudeCmd;
	double depthToObsAltitude  = measuredDepth + obstacleAltitude - _altitudeCmd;

	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 +
	      ( obstacleSlopeAlt - _altitudeCmd ) / cos(thetaB);
	   obstacleRange = obstacleSlopeRange * cos(thetaB);

	   _thetaFF = thetaB;
	}
	else
	   thetaB = 0.;
	
	//
	// Now reset _altitudeCmd to turn off native altitude control and use
	// depth instead:
	_altitudeCmd = NotSpecified;
	depthBasedAltitudeControl = True;
	_depthCmd = _depth;

	if( (depthToAltitude || depthToObsAltitude) < _depth )
	{
	   if( depthToAltitude < _depth )  _depthCmd = depthToAltitude;
	      
	   if( _avoidRange != NotSpecified && depthToObsAltitude < _depthCmd )
	   {

	      if( !_depth2AltActive )
	      {
		 Syslog::write("WaypointBottom::depthToObsAltitude activated at t = %15.2f", 
			       _missionClock->seconds());
		 _depth2AltActive = True;
	      }

	      if( obstacleRange < _avoidRange )
	      {
		 if( !_avoidRangeActive )
		 {
		    Syslog::write("WaypointBottom::avoidRange activated at t = %15.2f",
				  _missionClock->seconds());
		    _avoidRangeActive = True;
		 }
		 _depthCmd = depthToObsAltitude;
		 if( obstacleRange > 1.0 && _feedThetaForward ) 
		 {
		    _thetaFF  = atan2(measuredAltitude - obstacleAltitude, obstacleRange);
		    if( _thetaFF >  PI/4. ) _thetaFF =  PI/4.;
		    if( _thetaFF < -PI/4. ) _thetaFF = -PI/4.;
		 }
	      } 	
	      else _avoidRangeActive = False;

	   }
	   else _depth2AltActive = False;
	} //if( depthToAltitude < _depth )
     } //end depth-based altitude control
  }  // end altitude-control section
  //
  //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(" Waypoint has set the state to finished.\n");

    Syslog::write( "Waypoint (%.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( _navigation->depth() < _depth && _depthHysteresis != 0. ) 
     {
	_depthHysteresis = 0.;
	Syslog::write("WaypointBottom:: Resetting maximum depth to %.2f m "
		      "at t= %.2f sec.", _depth, _missionClock->seconds());
     }

     //
     // Do not let the vehicle go below the specified maximum depth _depth.
     if( _altitudeCmd != NotSpecified && _nativeAltitudeControl && 
	 ( _navigation->depth() > _depth+_depthHysteresis || _maxDepthHold ) )
     {
	_thetaFF = 0.;
	setVertical(DynamicControlIF::DepthImmediate, _depth, _maxLowerPitch, 
		    _thetaFF);
	if( !_maxDepthHold )
	{
	   _maxDepthHold = True;
	   Syslog::write("WaypointBottom:: Vehicle %.2f m below maximum specified depth. "
			 "Switching to depth control at t = %.2f",
			 _navigation->depth() - (_depth+_depthHysteresis),
			 _missionClock->seconds());
	}
	if( _navigation->altitude() < _altitudeCmd )
	{
	   _maxDepthHold = False;
	   Syslog::write("WaypointBottom:: Measured altitude %.2f < commanded altitude %.2f. "
			 "Resuming native altitude control at t = %.2f",
			 _navigation->altitude(), _altitudeCmd,
			 _missionClock->seconds());
	   if( _navigation->depth() > _depth ) 
	   {
	      _depthHysteresis = 1.;
	      Syslog::write("WaypointBottom:: Setting maximum depth to %.2f m "
			    "at t= %.2f sec.", _depth+_depthHysteresis, 
			    _missionClock->seconds());
	   }
	}
     }
     else
     {
	if (_altitudeCmd != NotSpecified )
	   setVertical(DynamicControlIF::Altitude, _altitudeCmd, _maxLowerPitch, 
		       _thetaFF);
	else
	   setVertical(DynamicControlIF::Depth, _depthCmd,  _maxLowerPitch, 
		       _thetaFF);
     }  //end max depth safemode

     //
     // Shut off the prop if under altitude control and the altitude reading
     // is invalid:
     //if( _altitudeCmd != NotSpecified && !_nativeAltitudeControl &&
     if( depthBasedAltitudeControl && _navigation->altitude() == INVALID_ALTITUDE)
     {
	if( !_stopProp )
	{
	   Syslog::write("WaypointBottom:: Stopping the prop due to invalid "
			 "altitude at t=%.2f.", _missionClock->seconds());
	   _stopProp = True;
	   _depthLatch = measuredDepth;
	}
	setSpeed(DynamicControlIF::Speed, 0);
	setVertical(DynamicControlIF::Depth, _depthLatch, _maxLowerPitch, 
		    _thetaFF);
     }
     else
     {
	if( _stopProp )
	{
	   Syslog::write("WaypointBottom:: Altitude now valid. Resuming "
			 "propulsion at t=%.2f.", _missionClock->seconds());
	   _stopProp = False;
	}
	setSpeed(DynamicControlIF::Speed, _speed);
     }
    setHorizontal(DynamicControlIF::Waypoint, _bearing, _northing, _easting);
  }
}


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


Boolean WaypointBottom::validInput()
{
  Boolean debug = False;
  Boolean valid = True;

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

  dprintf("WaypointBottom::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;
  }

  // Waypoint location must be specified either in UTM or geographic coords
  Boolean coordsSpecified = False;
  WorkSiteIF workSite("workSite");
  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(_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("WaypointBottom::Converted lastLatitude = %12.8f degrees to %12.2f m", 
		    _lastLatitude*180/PI, _lastNorthing);
      Syslog::write("WaypointBottom::Converted lastLongitude = %12.8f degrees to %12.2f m", 
		    _lastLongitude*180/PI, _lastEasting);
  }
  
  if (_speed < 0.) {
    printError("Invalid speed");
    valid = False;
  }

  if (_depth == NotSpecified ) {
    printError("Error:You must specify depth.");
    valid = False;
  }

  if (_depth != NotSpecified && _depth < 0.) {
    printError("Error: The specified depth must be greater than zero.");
    valid = False;
  }

  if (_altitude == NotSpecified && _avoidRange != NotSpecified) 
  {
    printError("WaypointBottom::Error - You cannot specify "
    "avoidRange without also specifying altitude.");
    valid = False;
  }

  if (_lambda != NotSpecified && _lambda == 0.) {
    printError("Wavelengh cannot be zero.");
    valid = False;
  }

  char *auvConfigDir = getenv(AuvConfigDirName);
  if (auvConfigDir == 0)
  {
    Syslog::write("WaypointBottom::WaypointBottom() - environment variable %s not set\n",
                  AuvConfigDirName);
  }
  //
  // FullFileName contains the full path and file name.
  //
  sprintf(_fullFileName, "%s/%s", auvConfigDir, _baseName);

  _fpAsc = fopen(_fullFileName,"r");

  // strcmp opposite what you'd expect.
  if( _fpAsc == NULL && !strcmp( _baseName, DEFAULT_STRING_ATTR ) )
  {
     // File not specified; do nothing.
  }
  else if( _fpAsc == NULL && strcmp( _baseName, DEFAULT_STRING_ATTR ) )
  {
     //
     // File specified, but couldn't be opened.
     //
     Syslog::write( " WaypointBottom::Waypointbottom() - "
		    "Error. Could not open %s.\n", _fullFileName );
//                 "Could not open:\n   %s.\n   ThetaFF = 0.\n", _fullFileName );
     valid = False;
  }
  else
  {
     //
     // File found and opened.  Check to see if it's correct.
     // 
     int i, ncols = 0;
     int ncolsRead;
     char buf[128];
     ncols = sscanf(buf, "%f %f %f %f", &_profileTab[i][0], &_profileTab[i][1], 
		    &_profileTab[i][2], &_profileTab[i][3]);
     Syslog::write("WaypointBottom:: %d columns detected in file %s.", 
		   ncols, _fullFileName);

     if( ncols > NCOLS) valid = False;
     
     for( i=0; fgets(buf, 128, _fpAsc) && i<NTAB; i++ )
     {
	ncolsRead =sscanf(buf, "%f %f %f %f", &_profileTab[i][0], 
			  &_profileTab[i][1], &_profileTab[i][2],
			  &_profileTab[i][3]);
	Syslog::write("WaypointBottom:: Distance=%7.2f, ThetaFF=%7.2f, "
	"Profile=%7.2f, ThetaT = %7.2f\n",
		      _profileTab[i][0], _profileTab[i][1], _profileTab[i][2],
		      _profileTab[i][3]);
	if( ncolsRead != ncols )
	{
	   Syslog::write( " WaypointBottom::Waypointbottom() - "
			  "Error. Read %d columns on line %d",
			  ncolsRead, i+1 );
	}
     }
     _lenTab = i;
     Syslog::write("WaypointBottom:: Read %d lines from file %s.",
     _lenTab, _fullFileName);
  }
  //_tergde = new TerrainGuidance(_mapFile, initConditions);
  //Syslog::write("WaypointBottom:: 21 Distance=%7.2f, ThetaFF=%7.2f, "
  //"Altitude=%7.2f\n",
  //_profileTab[21][0], _profileTab[21][1], _profileTab[21][2]);

  //
  // Only try to open the interface if reinitFilter is True in the argument
  // list.  Do it here rather than in the constructor because the
  // "attributes" aren't yet parsed in the constructor.
  if( _reinitFilter && !Behavior::_terrainAid )
  {
     try 
     {
	Behavior::_terrainAid = new TerrainAidIF("TerrainAidIFServer");
	Syslog::write("WaypointBottom - Opened TerrainAidIFServer %d.",
		      _terrainAidIFCnt);
	_terrainAidIFCnt++;
     }
     catch (Exception e)
     {
	Syslog::write("WaypointBottom - %s", e.msg);
	Behavior::_terrainAid = NULL;
     }
     catch (...) 
     {
	Behavior::_terrainAid = NULL;
	Syslog::write("WaypointBottom - Could not open TerrainAidIFServer.");
     }
  }

  if( _reinitFilter && !Behavior::_dynamicControl )
  {
     try 
     {
	Behavior::_dynamicControl = new DynamicControlIF("dynamicControl");
	Syslog::write("WaypointBottom - Opened DynamicControlIF.");
     } 
     catch (Exception e)
     {
	Syslog::write("WaypointBottom - %s", e.msg);
	Behavior::_dynamicControl = NULL;
     }
     catch(...)
     {
	Syslog::write("WaypointBottom: -- Error. Failed to initialize connection "
		      "to DynamicControlIF\n");
	Behavior::_dynamicControl = NULL;
     }
  }

  return valid;
}