#include "WaypointDepth.h"
#include "FloatAttribute.h"
#include "AngleAttribute.h"
#include "BooleanAttribute.h"
#include "Syslog.h"
#include "WorkSiteIF.h"
#include "NavUtils.h"

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

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

WaypointDepth::WaypointDepth()
  : Behavior(WaypointDepthBehaviorName, 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("initialDepth", "Initial Depth", 
				    &_initialDepth));

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

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

  attributes.add(new FloatAttribute("maxCrossTrackError", 
				    "Max Cross Track Error", 
				    &_maxXte, 1000.0));
  attributes.add(new BooleanAttribute("abortOnTimeout",
				      "Should mission abort if waypoint "
				      "times out?",
				      &_abortOnTimeout,
				      False));
  _first = True;
  _xtError = False;
  _timeout = 0.0;

}


WaypointDepth::~WaypointDepth()
{
}


void WaypointDepth::execute( void )
{
  double dn, de;
  double dtw, dt, xt, goal;
  Boolean debug = True;
  double now = _missionClock->seconds();

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

  /* 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;
     //
     // Compute the bearing to the next waypoint:
     _bearing = PI + Math::modPi( atan2(_easting - position.y,
					_northing - position.x) - 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("WaypointDepth -- ERROR.  The distance from the start\n"
//		      "to the next waypoint is less than one meter.\n"
//		      "Aborting.");
	//abortMission();
	return;
     }
     _slope = (_initialDepth - _finalDepth)/dtw;

     if( _speed < 0.2 )
     {
//	Syslog::write("WaypointDepth -- ERROR.  Commanded speed = %7.4f,"
//		      " which is too small.  Aborting!", _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("WaypointDepth -- ERROR.  Waypoint slope = %7.4f,"
//		      " causes a climb rate of %7.4f,\n which is too large. "
//		      " Aborting!", _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( "WaypointDepth 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);
     _first = False;
  }  // if(_first)

  //check to see if we timed out
  if ( _abortOnTimeout &&
	( now - startTime() > _timeout)) {
	Syslog::write("WaypointDepth::execute() -- timed out without reaching goal\n");
	setState(Finished);
	abortMission();
	return;
   }
  /* transform into down-track and cross-track (dt, xt) coordinates */
  dt  = -de*sin(_bearing) - dn*cos(_bearing);
  xt  =  dn*sin(_bearing) - de*cos(_bearing);
  //
  // This behavior does not have the Circle Mode and captureRadius parameter
  // which are present in the original Waypoint.cc.  It tracks the distance
  // to the waypoint projected on to the line of bearing, and simply
  // terminates when the vehicle passes the waypoint, irrespective of the
  // cross-track error.  
  //
  //goal = -_captureRadius;
  goal = 0.;


  /* Check to see if the cross track error is too large.  If so, revert to 
     the minimum depth */
  if( fabs(xt) > fabs(_maxXte) )
  {
     _xtError = True;
    Syslog::write( "WaypointDepth:  Maximum cross-track error exceeded.\n"
		   "Resetting the commanded depth to minDepth.");
  }

  // Check to see dt, distance to the waypoint, is less than zero. If so,
  // terminate.
  if ( dt <= goal ) 
  {
    setState(Finished);
    //
    // Please to not modify the following write.
    //
    dprintf(" WaypointDepth 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 error is (%.1f, %.1f)\n",
                    _northing-position.x, _easting-position.y);
  }
  else {
    /* load command vector */

    if (_depth != NotSpecified)
    {
       if( _xtError )
       {
	  _depth = _minDepth;
	  setVertical(DynamicControlIF::Depth, _depth);
       }
       else
       {
	  _depth = _finalDepth + _slope*dt;
	  setVertical(DynamicControlIF::Depth, _depth);
       }
    }
    setSpeed(DynamicControlIF::Speed, _speed);
    setHorizontal(DynamicControlIF::WaypointDepth, _bearing, 
		  _northing, _easting);
  }
}


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


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

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

  dprintf("WaypointDepth::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.) {
//    printError("Invalid captureRadius");
//    valid = False;
//  }

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

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

  return valid;
}