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

Waypoint::Waypoint()
  : Behavior(WaypointBehaviorName, 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));

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

  attributes.add(new BooleanAttribute("circleMode", "Circle at end?", 
				      &_circleMode, True));
  _first = True;
}



Waypoint::~Waypoint()
{
}


void Waypoint::execute( void )
{
  double dn, de;
  double dtw, goal;
  Boolean debug = True;

  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);
    goal = -_captureRadius;
  }

  /* 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 (_depth != NotSpecified)
      setVertical(DynamicControlIF::Depth, _depth);

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


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


Boolean Waypoint::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;

  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;
  }

  return valid;
}