#include "WaypointYoyo.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)

WaypointYoyo::WaypointYoyo()
  : Behavior(WaypointYoyoBehaviorName, Sequential)
{
  _cyclesCompleted = 0.;
  _goingDown = True;

  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("captureRadius", "Capture radius", 
				    &_captureRadius, NotSpecified));

  //attributes.add(new AngleAttribute("bearing", "Bearing", &_bearing));

  attributes.add(new BooleanAttribute("circleMode", "Circle at end?", 
				      &_circleMode, False));

  //yoyo arguments
  attributes.add(new FloatAttribute("minDepth", 
				    "Start down when above this depth",
				    &_minDepth));

  attributes.add(new FloatAttribute("maxDepth", 
				    "Start up when below this depth",
				    &_maxDepth));

  attributes.add(new FloatAttribute("minPitch", 
				    "Commanded pitch for descent yo",
				    &_minPitch, NotSpecified));

  attributes.add(new FloatAttribute("maxPitch", 
				    "Commanded pitch for ascent yo",
				    &_maxPitch, NotSpecified));

  attributes.add(new FloatAttribute("minAltitude", 
				    "Start up when below this altitude",
				    &_minAltitude, 
				    5.));

  attributes.add(new FloatAttribute("maxCycles", 
				    "End after this many cycles",
				    &_maxCycles, 1.));
  attributes.add(new FloatAttribute("abortDepth",
				    "abort if vehicle goes past this depth",
				    &_abortDepth, NotSpecified));
  _first = True;
  _controlMode = Unspecified;		 
}



WaypointYoyo::~WaypointYoyo()
{
}


void WaypointYoyo::execute( void )
{
  double dn, de;
  double dtw, goal;
  Boolean debug = True;
  Boolean abortDepthSpecified = (_abortDepth != NotSpecified);

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

  if(_first)
  {
    _bearing = PI + Math::modPi( atan2(_easting - position.y,
				 _northing - position.x) - 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( "Waypoint 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) );
    _first = False;
  }

  /* 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 = 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 */

    setSpeed(DynamicControlIF::Speed, _speed);
    setHorizontal(DynamicControlIF::Waypoint, _bearing, _northing, _easting);
  }

  //yoyo part of behavior
  debug = False;

  // TEST TEST TEST
  //  const double dummyPad = 1.1;   // This value works
  const double dummyPad = 0.5;   // Value from Odyssey code

  if (_maxCycles - _cyclesCompleted < 0.25) {
    //
    // RSM, 20 Aug 03 Added debug printout.
    //
    Syslog::write( "Behavior waypoint_yoyo terminating on maxCycles.\n",
                   " _maxCycles = %.2f, _cyclesCompleted = %.2f \n",
		   _maxCycles, _cyclesCompleted );
    setState(Finished);
    return;
  }

  double depth = _navigation->depth();
  double targetDepth, targetPitch;
  DynamicControlIF::VerticalMode verticalMode;

  if (_goingDown) {

    // Going down
    targetDepth = _maxDepth;

    if ((depth > _maxDepth - dummyPad) || 
	(_navigation->altitude() < _minAltitude)) {

      // Reached bottom; start heading up
      _goingDown = False;
      targetDepth = _minDepth;

      // Completed 1 "yo", which is 0.5 cycle
      _cyclesCompleted += 0.5;
      verticalMode = DynamicControlIF::DepthImmediate;
    } else {
      verticalMode = DynamicControlIF::Depth;
    }
  }
  else {

    // Going up
    targetDepth = _minDepth;

    if (depth < _minDepth + dummyPad) {

      // Reached top; start heading down
      _goingDown = True;
      targetDepth = _maxDepth;

      // Completed 1 yo, which is 0.5 cycle
      _cyclesCompleted += 0.5;
      verticalMode = DynamicControlIF::DepthImmediate;
    } else {
      verticalMode = DynamicControlIF::Depth;
    }
  }

  if( _controlMode == Depth )  
     setVertical(verticalMode, targetDepth);
  else if( _controlMode == Pitch )  
  {
     if( _goingDown ) setVertical(DynamicControlIF::Pitch, _minPitch);
     else             setVertical(DynamicControlIF::Pitch, _maxPitch);
  }
  else
  {
     Syslog::write("WaypointYoyo -- ERROR: Undefined control mode.  Abort.");
     abortMission();
     return;
  }

  if (abortDepthSpecified && (depth > _abortDepth)) {
       Syslog::write("DepthEnvelope -- Measured depth %lf "
		     "over allowable depth %lf.  Aborting!", depth, _abortDepth );
    abortMission();
  }
    return;

}


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


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

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

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

  Boolean _utmSpecified = False;
  Boolean _geographicSpecified = False;
  //
  // Check to see if one of the pair of UTM coordinates and one of the pair
  // of Geo coordinates are specified.  Declare an error.
  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;
  }
  //
  // Now check to see if both UTM coordinates and both Geo coordinates are
  // specified.  IF so, declare an error.  Waypoint location must be
  // specified either in UTM or geographic coords.  If Geo, then convert to
  // UTM.
  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;
  }
  //
  // Now check to see if the pair (_maxPitch, _minPitch) are specified.
  // Require the user to specifiy both.  If both are specified, use pitch
  // control instead of depth control.
  Boolean validPitch = False;
  if( _minPitch != NotSpecified && _maxPitch != NotSpecified ) 
  {
     validPitch = True;
     Syslog::write("WaypointYoyo -- Using pitch control.");
  }
  //
  // Declare an error if the user has specified only one of the pitch limits.
  if( _minPitch == NotSpecified && _maxPitch != NotSpecified ||
      _minPitch != NotSpecified && _maxPitch == NotSpecified ) 
  {
     valid = False;
     printError("Please specify both maximum and minimum and pitch limits.\n");
  }
  if( validPitch ) _controlMode = Pitch;
  else             _controlMode = Depth;

  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 (_maxDepth < _minDepth) {
    printError("maxDepth is less than minDepth");
    valid = False;
  }

  if (_maxCycles <= 0.) {
    printError("maxCycles must be greater than zero");
    valid = False;
  }
  return valid;
}