#include "WaypointWallEscape.h"
#include "WaypointWall.h"
#include "FloatAttribute.h"
#include "IntegerAttribute.h"
#include "AngleAttribute.h"
#include "BooleanAttribute.h"
#include "Syslog.h"
#include "WorkSiteIF.h"
#include "NavUtils.h"
#include "TimeP.h"

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

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

WaypointWallEscape::WaypointWallEscape()
  : Behavior(WaypointWallEscapeBehaviorName, Sequential)
{
  attributes.add(new FloatAttribute("distance", "Distance", &_distance, NotSpecified));
  attributes.add(new FloatAttribute("speed", "Speed", &_speed, NotSpecified));
  attributes.add(new FloatAttribute("depth", "Depth", &_depth, NotSpecified));
  attributes.add(new FloatAttribute("altitude", "Altitude", &_altitude, NotSpecified));
  attributes.add(new IntegerAttribute("missionQueue", "In the mission queue.",
				      &_missionQueue, -1));
  attributes.add(new FloatAttribute("wallAbsentTime", "Time since wall valid.",
				    &_wallAbsentTime, 12.));
  _first = True;

  try 
  {
     _dynamicControlIF = new DynamicControlIF("dynamicControl");
  } 
  catch(...)
  {
     Syslog::write("WaypointWallEscape:: -- Failed to initialize connection "
		    "to DynamicControlIF\n");
     abortMission();
   }
}


WaypointWallEscape::~WaypointWallEscape()
{
}


void WaypointWallEscape::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 0
  Boolean run = WaypointWall::AbortRequested || _missionQueue == 1;

  if( !run )
  {
     setState(Finished);
     Syslog::write(" WaypointWallEscape: WaypointWall did not request an abort. Exit.\n");
     Syslog::write( "WaypointWallEscape (%.1f, %.1f), \n"
		    "  reached at t = %-15.2f"
		    "                                 (wplog)\n", 
                    _northing, _easting, _missionClock->seconds());
     return;
  }
#endif

  if(_first)
  {
     struct timespec timeSpec;
     Time::gettime(&timeSpec);

     double now = (timeSpec.tv_sec + timeSpec.tv_nsec/1.e9);
     
     _dynamicControlIF->getWallData( &_lastWallTime, &_wallBearing );

     Syslog::write( "WaypointWallEscape:: wallAbsentTime = %.1f, ",
		    _wallAbsentTime);
     Syslog::write( "WaypointWallEscape:: _lastWallTime = %.1f, "
		    "mission time = %.2f, _wallBearing = %.1f\n",
		    _lastWallTime, _missionClock->seconds(), R2D(_wallBearing));

     if( _lastWallTime + _wallAbsentTime < now )
     {
	setState(Finished);
	Syslog::write(" WaypointWallEscape: Wall absent for "
		      "%.1f seconds at %-15.2f.\n",
		      now-_lastWallTime, now);
	Syslog::write( "WaypointWallEscape current position (%.1f, %.1f), \n"
		       " reached at t = %-15.2f\n",
		       position.x, position.y, _missionClock->seconds());
	return;
     }
     //
     // Turn until perpendicular to the estimated wall bearing.
     Syslog::write("WaypointWallEscape::The last good wall bearing was %.2f.",
		   _wallBearing*180/PI);
     if( WaypointWall::TurnLeft )
     {
	Syslog::write("WaypointWallEscape::The wall is to our right, so turn left.");
	_bearing = _wallBearing - PI/2.;
     }
     else
     {
	Syslog::write("WaypointWallEscape::The wall is to our left, so turn right.");
	_bearing = _wallBearing + PI/2.;
     }
     //
     // Compute waypoint:
     _northing = position.x + _distance * cos(_bearing);
     _easting  = position.y + _distance * sin(_bearing);
    //
    // 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( "WaypointWallEscape 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;
  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 = 0.;

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

    setState(Finished);
    //
    // Please to not modify the following write.
    //
    dprintf(" WaypointWallEscape has set the state to finished.\n");

    Syslog::write( "WaypointWallEscape (%.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);
    else if (_altitude != NotSpecified)
      setVertical(DynamicControlIF::Altitude, _altitude);

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


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


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

  if (_speed < 0.) {
    printError("Invalid speed");
    valid = False;
  }

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

  if (_distance == NotSpecified || _distance < 0.) {
     printError("WaypointWallEscape::Error - distance must be > 0. ");
    valid = False;
  }

  if (_depth == NotSpecified && _altitude == NotSpecified ) 
  {
    printError("WaypointWallEscape::Error - You must specifiy either "
    "depth or altitude.");
    valid = False;
  }

  if (_depth != NotSpecified && _altitude != NotSpecified ) 
  {
    printError("WaypointWallEscape::Error - You cannot specifiy both "
    "depth and altitude.");
    valid = False;
  }

  return valid;
}