#include "Circle.h"
#include "FloatAttribute.h"
#include "AngleAttribute.h"
#include "BooleanAttribute.h"
#include "Syslog.h"
#include "WorkSiteIF.h"
#include "NavUtils.h"
#include "SupervisorOutput.h"

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

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

Circle::Circle()
  : Behavior(CircleBehaviorName, 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("radius", "Radius", &_radius, 
				    NotSpecified));

  attributes.add(new BooleanAttribute("abortOnTimeout",
				      "Should mission abort if insufficient "
                                      "# hits at end of behavior?",
				      &_abortOnTimeout,
				      False ) );
  _timeout = 0.0;
  _first = True;

  _cmdRec = AcousticModemIF::INVALID_CMD;

  try 
     {
	//
	// Declared in Behavior.h
	if( !_modem ) _modem = new AcousticModemIF(AcousticModemIFServerName);
     } 
     catch(...)
     {
	Syslog::write("Circle:: -- Failed to initialize connection "
		      "to AcousticModemIF\n");
	_modem = NULL;
     }

}



Circle::~Circle()
{
//
// Does this destructor get called when this behavior ends, or when the
// mission is over?  
//
// if(!_modem) delete _modem;
}


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

  NavigationIF::Position position;
  NavigationIF::Attitude attitude;

  // Check for timeout.
  if( _abortOnTimeout && 
      ( _missionClock->seconds() - startTime() > _timeout ) ) 
  {
     Syslog::write("Circle::execute() -- Timed out.  Aborting.");
     setState(Finished);
     abortMission();
     return;
  }

  // Get current position
  _navigation->state(&position, &attitude);
  //
  if( _first )
  {
     Syslog::write( "Circle Initialization: \n"
		    "  Begin circle control at t= %-15.2f\n",
		    _missionClock->seconds());
     _first = False;
  }
  //
  // Because the simulation doesn't open an IF to the modem.
  if( _modem ) _modem->getSMACCmd(&_cmdRec, &newdata);
  if( newdata && (_cmdRec == AcousticModemIF::START_SURVEY) )
  {
     Syslog::write("Circle::Received the GO code at t= %.2f", 
		   _missionClock->seconds());
     setState(Finished);
  }

  long cntr = 0;
  double dbearing = atan2(_easting - position.y, _northing - position.x);
  //if( (cntr % 20) == 0 ) printf("dbearing = %.2f\n", dbearing);
  _bearing = dbearing - PI/2.;
  

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

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


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


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

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

  dprintf("Circle::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;
  }

  // Circle 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 (_depth != NotSpecified && _depth < 0.) {
    printError("Invalid depth");
    valid = False;
  }

  if (_radius == NotSpecified ) 
  {
    printError("Circle:radius not specified.");
    valid = False;
  }

  // Take over the timeout function if abortOnTimeout is set
  if( _abortOnTimeout ) {
       _timeout = duration();
       _duration += 10;
  }

  return valid;
}