#include "WaypointHoming.h"
#include "FloatAttribute.h"
#include "AngleAttribute.h"
#include "BooleanAttribute.h"
#include "StringAttribute.h"
#include "Syslog.h"
#include "WorkSiteIF.h"
#include "NavUtils.h"
#include "dvl/Dvl.h"
#include <time.h>

#define DEFAULT_STRING_ATTR "\"\""
//#define DEFAULT_STRING_ATTR '\0'
#ifndef PI
#define PI     3.14159265358979323846
#endif
#define R2D(r) (r*180.0/PI)

#define NotSpecified -100000.0
#define AngleNotSpecified ((NotSpecified) / Math::RadsPerDeg)
//
// The dock server isn't present for non-docking applications.  Also, a
// simulated dock server doesn't yet exist, so we havet to turn it off for
// docking simulations.
//
const Boolean dockServerExists = False;

WaypointHoming::WaypointHoming()
   : Behavior(WaypointHomingBehaviorName, 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 FloatAttribute("speed", "Speed", &_speed));
   attributes.add(new FloatAttribute("depth", "Depth", &_depth, NotSpecified));

   attributes.add(new FloatAttribute("captureRadius", "Capture radius", 
				     &_captureRadius, 20.));

   attributes.add(new AngleAttribute("dockBearing", "Dock Bearing", 
				     &_dockBearing, AngleNotSpecified));

   attributes.add(new BooleanAttribute("circleMode", "Circle at end?", 
				       &_circleMode, False));
   attributes.add(new FloatAttribute("inTrkOffset", "Along-track waypoint offset", 
				     &_inTrkOffset, -10.0));

   attributes.add(new FloatAttribute("xTrkOffset", "Cross-track waypoint offset", 
				     &_xTrkOffset, 0.));

   attributes.add(new FloatAttribute("clusterRadius", "Maximum allowable cluster size", 
				     &_clusterRadius, 10.));

   attributes.add(new FloatAttribute("finalApproach", "Distance to turn onto "
				     "final approach", 
				     &_finalApproach, 300.));
   attributes.add(new StringAttribute("beaconAddr",
				      "Beacon interrogation/response address",
				      &_beaconAddr, DEFAULT_STRING_ATTR));
   attributes.add(new FloatAttribute("navReset",
				     "=1 turns on navigation reset",
				     &_navReset, NotSpecified ));

   _first = True;
   _firstValid = True;
   _consecutiveGood = 0;
   _northingOffset = 0.;
   _eastingOffset  = 0.;
   _beaconInFov = _beaconInFovLast = False;
   _dtw = 10000.;         //Initialize distance-to-waypoint to be larger than
                          //possible with Usbl measurements.
   _probeState = Down;    //This behavior assumes the probe is initially down.
   _networkStatus = Absent;  
   try 
   {
      if( !_dvl ) _dvl = new DvlIF("dvl");
   } 
   catch(...)
   {
      Syslog::write("WaypointHoming -- Failed to initialize connection "
		    "to Dvl IF");
      _dvl = NULL;
   }

   try 
   {
      if( !_usbl ) _usbl = new UsblIF("usbl");
   } 
   catch(...)
   {
      Syslog::write("WaypointHoming -- Failed to initialize connection "
		    "to Sonardane Usbl IF");
      _usbl = NULL;
   }

   if( dockServerExists )
   {
      try 
      {
	 if( !_dock ) _dock = new DockIF("Dock");
      } 
      catch (Exception e) 
      {
	 Syslog::write("WaypointHoming--Caught exception on creation "
		       "of DockIF:%s\n", e.msg);
	 _dock = NULL;
      }
      catch(...)
      {
	 Syslog::write("WaypointHoming -- Failed to initialize connection "
		       "to DockIF");
	 _dock = NULL;
      }
   }
   //
   // Start polling for the network.  Hardcode this on for tank tests.
   // For operations, shut this off unless the vehicle is near the dock.
   //
   int    pingTimeOut  = 150;      //milliseconds until the dock ping time out.
   if( _dock && dockServerExists )  _dock->set_continuous_query( True, pingTimeOut );

}



WaypointHoming::~WaypointHoming()
{
   //
   // Turn off continuous query here?
   //
   //if( !_dvl )  delete _dvl;
   if( !_dock && dockServerExists ) delete _dock;
   Syslog::write("WaypointHoming:: Destructor executing now.");
}


void WaypointHoming::execute( void )
{
   double dn, de;
   double goal;
   Boolean debug = True;
   Boolean nearDock = False, lastNearDock = False;
   double updateTimeSecs = 0.;
   //
   // Constants: move these to vehicle.cfg if we decide to keep them, and
   // change the initialization of usblSlantRange in Navigation.cc.
   //
   double maxBearing   = PI/4.;    //Max bearing to Usbl wrto vehicle frame.
   double maxYawRate   = 0.10;     //Max yaw rate to accept Usbl data.
   short  maxGood      = 1;        //Number of consecutive good Usbl hits
   double minRange     = 800.;     //Shut off Usbl checks above this range, m.
   double maxDockRange = 20.;      //Enable dock deceleration sensing below this.
   double dockAltitude = 5.;       //Max altitude of the cone, m.
   double minSpeed     = 0.1;      //Dvl speed below which the vehicle is stopped
   Boolean networkPresent = False;
   DockIF::String32 probeString;
   DockIF::String32 resolverString;
   //
   NavigationIF::Position position;
   NavigationIF::Attitude attitude;
   int median;
   double maxRadius = 0.;
   // Get current position
   _navigation->state(&position, &attitude);

   if(_first)
   {
      _bearing = PI + Math::modPi( atan2(_easting - position.y,
					 _northing - position.x) - PI);
      for( int i=0; i<MEDIANSIZE; i++ )
      {
	 _nList[i] = 0.;
	 _eList[i] = 0.;
      }
      _ngood = 0;
      //
      // 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( "WaypointHoming 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) );
      _beaconNorthing = _northing;
      _beaconEasting  = _easting;
      _first = False;

      if( _usbl && strcmp(_beaconAddr, DEFAULT_STRING_ATTR) )
	 _usbl->set_beacon(_beaconAddr);

      if( _usbl ) _usbl->start();
   }
   //
   // If the Usbl is getting good returns, ignore the waypoint and home to
   // the transponder.  BUT, ignore the Usbl once the Auv has passed the
   // transponder because the Usbl gives incorrect x-distance measurements
   // when the transponder is behind it.
   //
   if( (fabs(Math::modPi(_bearing) - Math::modPi(attitude.yaw)) < PI/2.) &&
       (_dtw > 0. ))
   {
      _beaconInFov = True;
   }
   else
   {
      _beaconInFov = False;
   }
   if( _beaconInFov != _beaconInFovLast )
   {
      _beaconInFovLast = _beaconInFov;
      if( _beaconInFov )
      {
	 Syslog::write("WaypointHoming:: Beacon is in the Usbl Fov "
		       "at t = %.1f with dtw = %.2f", 
		       _missionClock->seconds(), _dtw);
      }
      else
      {
	 Syslog::write("WaypointHoming:: Beacon has exited the "
		       "Usbl Fov at t = %.1f with dtw = %.2f", 
		       _missionClock->seconds(), _dtw);
      }
   }
   
   //if( position.usblValid && _dtw > 0.)
   if( position.usblValid && _beaconInFov && (_dtw > 0.) )
   {
      if( _consecutiveGood < maxGood && position.usblNewData ) _consecutiveGood++;
      if( _firstValid )
      {
	 Syslog::write( "WaypointHoming: "
			"The Usbl has aquired the transponder\n"
			"  at t = %-15.2f Seconds.", _missionClock->seconds());
	 Syslog::write( "  The vehicle position is (%.1f, %.1f)\n",
			position.x, position.y);
      }
      _firstValid = False;
      //
      // Latch the direction of the transponder in case the usbl drops out.
      // Only compute the new location if the usbl has new data.
      // PI/100. = 1.8 degrees.
      if(    position.usblNewData 
	     && (    ( fabs(position.usblBearing) <  maxBearing )
		     && ( fabs(attitude.omega_B_z)   <  maxYawRate ) 
		     && (_consecutiveGood            >= maxGood    )  
		     || ( position.usblSlantRange    >  minRange   )  
		)  
        )
      {
	 //
	 // Account for the Usbl offset here.
	 //
	 // Now compute the measured beacon location using the propagated
	 // (dead-reckoned) navigation solution.
	 //
	 _newBearing  = position.usblBearing + attitude.yaw;
	 _newNorthing = position.usblHorizRange*cos(_newBearing) + position.x;
	 _newEasting  = position.usblHorizRange*sin(_newBearing) + position.y;
	 //
	 // Pass the northings and eastings through a median filter.
	 //
	 int dim, i;
	 double tempNorthing, tempEasting, tempBearing;
	 if( _ngood < MEDIANSIZE )
	 {
	    _nList[(int)_ngood] = _newNorthing;
	    _eList[(int)_ngood] = _newEasting;
	    dim = _ngood+1;
	 }
	 else
	 {
	    for( i=0; i<MEDIANSIZE-1; i++ )
	    {
	       _nList[i] = _nList[i+1];
	       _eList[i] = _eList[i+1];
	    }
	    _nList[MEDIANSIZE-1] = _newNorthing;
	    _eList[MEDIANSIZE-1] = _newEasting;
	    dim = MEDIANSIZE;
	 }
	 
	 Boolean useFilter = True;
	 //if( position.usblHorizRange < 40. ) useFilter = False;

	 if( dim >= 5 && useFilter )
	 {
	    for( i=0; i<dim; i++ )
	    {
	       _nSorted[i] = (long) (100.*_nList[i]);
	       _eSorted[i] = (long) (100.*_eList[i]);
	    }
	    Math::shellSort( _nSorted, dim );
	    Math::shellSort( _eSorted, dim );

	    median = dim/2;

	    tempNorthing= (double) (_nSorted[median]/100.);
	    tempEasting = (double) (_eSorted[median]/100.);

	    tempBearing = PI + Math::modPi( atan2(_easting - position.y,
					       _northing - position.x) - PI);
	    //
	    // Compute the radius about the median of the furthest of 
	    // the last dim hits:
	    double radius;
	    for( i=0; i<dim; i++ )
	    {
	       radius = sqrt( pow( (_nList[i] - tempNorthing), 2. ) +
			      pow( (_eList[i] - tempEasting ), 2. ) );
	       if( radius > maxRadius ) maxRadius = radius;
	    }
	    if( maxRadius < _clusterRadius )
	    {
	       _northing = tempNorthing;
	       _easting  = tempEasting;
	       _bearing  = tempBearing;
	       //
	       // Compute the position offset if we have a tight cluster, and
	       // we're close to the beacon.  Rather than resetting the
	       // navigation at this moment, we'll wait until this behavior
	       // ends to avoid confusing the filter memory.  
	       //
	       if( (dim == MEDIANSIZE) && 
		   (position.usblHorizRange < _finalApproach) )
	       {
		  double dn, de;
		  dn = _beaconNorthing - _northing;
		  de = _beaconEasting  - _easting;
		  if( dn != _northingOffset || de != _eastingOffset )
		  {
		     Syslog::write("WaypointHoming - Offset "
				   "(dn de ) = (%.1f %.1f), t = %.2f "
				   "r = %.1fm.",
				   _northingOffset, _eastingOffset, 
				   _missionClock->seconds(),
				   position.usblHorizRange);
		     _northingOffset = dn;
		     _eastingOffset  = de;
		  }
	       }
	    }
	 } //END: if dim >=5
	 // Commented out; 30 Oct 2009 rsm.
// 	 else
// 	 {
// 	    _bearing = _newBearing;
// 	    _northing= _newNorthing;
// 	    _easting = _newEasting;
// 	 }
	 _ngood++;
	 //Syslog::write(" median = %d", median);
	 //for( i=0; i<dim; i++ )	 Syslog::write(" nL = %.2f eL=%.2f "
	 //				       "nS = %d eS = %d", 
	 //				       _nList[i], _eList[i],
	 //				       _nSorted[i], _eSorted[i]);
      }  //END: if Usbl has new data and meets constraints.
      /* distance to waypoint in N, E coords */
      dn = position.x - _northing;
      de = position.y - _easting;
      //
      //Set terminal constraint on heading for docking:
      if( (_dockBearing != NotSpecified) && 
	  (position.usblHorizRange < _finalApproach ) ) _bearing = _dockBearing;
      //
      _dtw = -de*sin(_bearing) - dn*cos(_bearing);
      //
      //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 = _inTrkOffset;
      //
      // Terminate the behavior "goal" meters in front of the transponder:
      //_dtw = position.usblRange;
      //goal = fabs(_captureRadius);
   }     //  END: if Usbl got valid data (may not meet constraints)
   else  //  Usbl does not have data:
   {  
      if( !_firstValid )
      {
	 Syslog::write( "WaypointHoming: "
			"  The Usbl does NOT detect the transponder\n"
			"  at t = %-15.2f Seconds.", _missionClock->seconds());
	 Syslog::write( "  The vehicle position is is (%.1f, %.1f)\n",
			position.x, position.y);
      }
      _firstValid = True;
      _consecutiveGood = 0;
      /* distance to waypoint in N, E coords */
      dn = position.x - _northing;
      de = position.y - _easting;

      /* transform into _dtw, xte coordinates */
      if (_circleMode) 
      {
	 _dtw = sqrt( dn*dn + de*de );
	 goal = fabs(_captureRadius);
      }
      else 
      {
	 _dtw = -de*sin(_bearing) - dn*cos(_bearing);
	 //
	 //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 = -_captureRadius;
	 goal = _inTrkOffset;
      }
   }  // if( position.usblValid )
   //
   //if(    (fabs(position.usblSlantRange) < maxDockRange)
   //    && (     position.altitude        < dockAltitude)  ) nearDock = True;
   //


   //turn the resolver on.


   if( dockServerExists )
   {
      _dock->read_continuous_query( &networkPresent,
				    probeString,
				    resolverString,
				    &updateTimeSecs);

      if( !networkPresent )
      {
	 if( _networkStatus == Present ) 
	 {
	    Syslog::write("WaypointHoming::ERROR "
			  "Network connection terminated!\n");
	 }
	 _networkStatus = Absent;
      }
      else
      {
	 if( _networkStatus == Absent ) 
	    Syslog::write("WaypointHoming:: Network present.");
	 _networkStatus = Present;
      }

      if( _networkStatus == Present )
      {
	 if( (    !strncmp( probeString, "down", 3 ) 
		  || !strncmp( probeString, "unk", 3 )   ) 
	     && _probeState == Down )
	 {
	    if( !strncmp( resolverString, "good", 3 ) )
	    {
	       Syslog::write("WaypointHoming: Raising the Probe:");
	       _dock->set_dock_state("docked-nocharge");
	       _dock->move_probe( "up" );
	       _probeState = Raising;
	    }
	 }
	 else if( !strncmp( probeString, "up", 2 ) )
	 {
	    // HOORAY!  The vehicle has docked.
	    Syslog::write("WaypointHoming: "
			  "The vehicle has docked successfully!\n");
	    _probeState = Up;
	    setState(Finished);
	    abortMission();
	    //
	    // Please to not modify the following write.
	    //
	    dprintf(" WaypointHoming has set the state to finished.\n");

	    Syslog::write( "WaypointHoming (%.1f, %.1f), \n"
			   "  reached at t = %-15.2f"
			   "                                 (wplog)\n", 
			   _northing, _easting, _missionClock->seconds());
	    Syslog::write( "Vehicle Position error is (%.1f, %.1f)\n",
			   _northing-position.x, _easting-position.y);
	    Syslog::write( "The distance to the waypoint is %.1f.\n", _dtw );
	    Syslog::write( "The probe state is %s.\n", probeString );
	    Syslog::write( "The resolver state is %s.\n", resolverString );
	 }
      }
   }
			
//    if(networkPresent) Syslog::write( " The network is present.\n");
//    else               Syslog::write( " The network is absent.\n");
//    Syslog::write( " Resolver:%s\n", resolverString );
//    Syslog::write( " Probe:   %s\n", probeString );
//    Syslog::write( " Update time:   %.2f\n", updateTimeSecs-startTimeSecs );
   //
   // Rely on the behavior time-out to kill the mission if the vehicle gets
   // stuck.  We'll set "goal" to -15 meters, so that if the vehicle gets stuck
   // in the dock without a network connection it can't achieve its waypoint.
   // Then, for this behavior only, we'll do an abort-on-timeout.
#if 0
   // else
   // {
   //
   // Detect the case where the vehicle is stuck and didn't get network:
   //
   // Detect deceleration when the vehicle is near the dock and has (or had)
   // Usbl contact.  Both variables usblSlantRange and altitude will latch
   // their last good value if the respective sensor starts giving invalid
   // data.  This is what we want, since both the Dvl and Usbl are likely to
   // get bad data when the Auv is in the dock.
   //
   if(    (fabs(position.usblSlantRange) < maxDockRange)
	  && (     position.altitude        < dockAltitude)  )
   {
     
      //
      // Now check the Dvl to see if the vehicle has stopped, or the Dvl has
      // bad data, which we'll assume means the vehicle has stopped in this
      // case.
      DeviceIF::Status status = _dvl->get(&_dvlData, &_dvlNewData);
      Boolean velocityValid = !(_dvlData.dataStatus & BAD_BOTTOM_TRACK_VELOCITY);

      if( (position.xRate < minSpeed) || !velocityValid )
      {
	
      }
   }
#endif     

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

      setState(Finished);
      if( _usbl ) _usbl->stop();
      //
      // Please to not modify the following write.
      //
      dprintf(" WaypointHoming has set the state to finished.\n");

      Syslog::write( "WaypointHoming (%.1f, %.1f), \n"
		     "  reached at t = %-15.2f"
		     "                                 (wplog)\n", 
		     _northing, _easting, _missionClock->seconds());
      Syslog::write( "Vehicle Position error is (%.1f, %.1f)\n",
		     _northing-position.x, _easting-position.y);
      Syslog::write( "The projected distance to the waypoint is %.1f.\n", _dtw );
      Syslog::write( "The usbl range to the waypoint is %.1f.\n", 
		     position.usblHorizRange );
      if( (_navReset != NotSpecified) &&
	   _northingOffset && _eastingOffset )
      {
	 Syslog::write("WaypoingHoming:: Requesting nav offset "
		       "(%.1f, %.1f) at t = %.2f",
		       _northingOffset, _eastingOffset, 
		       _missionClock->seconds());
	 _navigation->resetUTM(_northingOffset, _eastingOffset);
      }
   }
   else {
      /* load command vector */

      if (_depth != NotSpecified)
	 setVertical(DynamicControlIF::Depth, _depth);

      setSpeed(DynamicControlIF::Speed, _speed);

      if( _dockBearing == NotSpecified )
	 //
	 // This is a homing run:
	 //
	 if( position.usblValid )   
	    setHorizontal(DynamicControlIF::Homing, 
			  _bearing, _northing, _easting);
	 else
	    setHorizontal(DynamicControlIF::Waypoint, 
			  _bearing, _northing, _easting);
      else
      {
	 //
	 // Docking run.
	 //
	 setHorizontal(DynamicControlIF::Waypoint, 
		       _bearing, _northing, _easting, 
		       _newBearing, _newNorthing, _newEasting, 
		       _first, _xTrkOffset);
      }
   }
}


Boolean WaypointHoming::shouldBehaviorStart()
{
   return horizontalSequence();
}


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

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

   dprintf("WaypointHoming::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;
   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
	 WorkSiteIF workSite("workSite");
	 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 (_captureRadius < 0. && _circleMode) 
   {
      if(_captureRadius == NotSpecified)
	 printError( "You must specify captureRadius for Circle Mode.");
      else
	 printError("captureRadius must be positive.");
      valid = False;
   }

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

   return valid;
}