#include "Undock.h"
#include "FloatAttribute.h"
#include "AngleAttribute.h"
#include "BooleanAttribute.h"
#include "Syslog.h"
#include "WorkSiteIF.h"
#include "NavUtils.h"
#include "dvl/Dvl.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)

#define NoEndDepth -100.0

int    pingTimeOut  = 150;      //milliseconds until the dock ping time out.

Undock::Undock()
  : Behavior(UndockBehaviorName, Sequential)
{

  attributes.add(new FloatAttribute("speed", "Speed", &_speed));

  attributes.add(new FloatAttribute("endDepth", 
				    "End behavior when shallower than this", 
				    &_endDepth, NoEndDepth ));

  attributes.add(new FloatAttribute("abortDepth", 
				    "abort behavior when deeper", 
				    &_abortDepth, NoEndDepth ));

  attributes.add(new FloatAttribute("thrustDuration", 
				    "Duration of thrust", 
				    &_thrustDuration, 0. ));

  attributes.add(new AngleAttribute("elevator", "Commanded elevator angle", 
				    &_elevator, 0. ));

  attributes.add(new BooleanAttribute("abortOnTimeout",
									  "Do not continue upon time-out. ",
				      &_abortOnTimeout,
				      True ) );
  _first = True;
  _firstValid = True;
  _consecutiveGood = 0;
  _probeState = Up;    //This behavior assumes the probe is initially up.
  _networkStatus = Absent;  
  try 
  {
    if( !_dvl ) _dvl = new DvlIF("dvl");
  } 
  catch(...)
  {
    Syslog::write("Undock -- Failed to initialize connection "
		  "to Dvl IF");
    _dvl = NULL;
  }

  try 
  {
    if( !_dock ) _dock = new DockIF("Dock");
  } 
  catch (Exception e) 
  {
	  Syslog::write("Undock--Caught exception on creation "
					"of DockIF:%s\n", e.msg);
	  _dock = NULL;
  }
  catch(...)
  {
    Syslog::write("Undock -- Failed to initialize connection "
		  "to DockIF");
    _dock = NULL;
  }
  
  _startTimeSecs = 0.;
  _netCtr = 0;
  _consecAbsent = False;
  //
  // Start reading the dock network, resolver, and probe:
  //
  if( _dock )  _dock->set_continuous_query( True, pingTimeOut );

}

Undock::~Undock()
{
   //
   // Don't delete - subsequent behaviors can use them
   //delete _dvl;
   //delete _dock;
}

//
// Constants: move these to vehicle.cfg if we decide to keep them, and
// change the initialization of usblSlantRange in Navigation.cc.
//
short  maxGood      = 3;        //Number of consecutive good Usbl hits
double dockAltitude = 2.72;     //Altitude of the lower edge of the tube.
double minSpeed     = 0.1;      //Dvl speed below which the vehicle is stopped

void Undock::execute( void )
{
  Boolean debug = True;
  double updateTimeSecs = 0.;
  double nowTimeSecs = 0.;
  Boolean networkPresent = False;
  DockIF::String32 probeString;
  DockIF::String32 resolverString;
  //
  NavigationIF::Position position;
  NavigationIF::Attitude attitude;
  TimeIF::TimeSpec now;
  // Get current position
  _navigation->state(&position, &attitude);

  if(_first)
  {
	  Syslog::write("Undock duration is %f\n", _duration);
    _first = False;
  }

  _dock->read_continuous_query( &networkPresent,
								probeString,
								resolverString,
								&updateTimeSecs);

  if( !networkPresent )
  {
	 if( _networkStatus == Present ) 
	 {
		Syslog::write("Undock::ERROR Network connection terminated!\n");
	 }
	 _networkStatus = Absent;
  }
  else
  {
	 if( _networkStatus == Absent && _probeState != Down ) 
		Syslog::write("Undock:: Network present.");
	 if( _probeState != Down ) _networkStatus = Present;
  }

  const int maxNoPing = 5;
  
  if( _networkStatus = Absent )
	  {
		  if( _netCtr < maxNoPing ) _netCtr++;
	  }
	  else
		  _netCtr = 0;
  
  if( _netCtr == maxNoPing ) _consecAbsent = True;

  //
  // Prevent a spurious bad ping from starting the prop.  Require
  // consecutive no-pings.
  //

  if( !_consecAbsent )
  {
	 setVertical(DynamicControlIF::Elevator, 0.);
	 setHorizontal(DynamicControlIF::Rudder, 0.);
	 setSpeed(DynamicControlIF::Speed, 0.);
	 if( (    !strncmp( probeString, "up", 2 ) 
			  || !strncmp( probeString, "unk", 3 )   ) 
		 && _probeState == Up )
	 {
		Syslog::write("Undock: Lowering the Probe:");
		//_dock->set_dock_state("docked-nocharge");
		_dock->move_probe( "down" );
		_probeState = Lowering;
	 }
	 else if( !strncmp( probeString, "down", 3 ) )
	 {
		Syslog::write("Undock: The probe is down!\n");
		_probeState = Down;
		_dock->set_continuous_query( False, pingTimeOut );
		Syslog::write("Undock: Closing the TCP port.!\n");
		_dock->close_port();
		//_vehicle->setstate(mission)  This turns off the resolver.
		_networkStatus = Absent;
	 }
  }
  else  
  {
	 if( _firstValid )
	 {
		Time::gettime( &now );
		_startTimeSecs = Time::seconds( &now );
		_firstValid    = False;
	 }
	 Time::gettime( &now );
	 nowTimeSecs = Time::seconds( &now );
	 //
	 // The network is shut off and the probe is down.  Now back out.
	 setVertical(DynamicControlIF::Elevator, _elevator);
	 setHorizontal(DynamicControlIF::Rudder, 0.);

	 double onDuration;
	 onDuration = nowTimeSecs-_startTimeSecs;
	 if( onDuration < _thrustDuration )
	 {
		setSpeed(DynamicControlIF::Speed, -_speed);
		Syslog::write("Undock:: Time = %.1f, Prop ON.", onDuration);
		Syslog::write("Undock:: Thrust duration = %.1f.", _thrustDuration);
	 }
	 else
	 {
		setSpeed(DynamicControlIF::Speed, 0.);
		Syslog::write("Undock:: Time = %.1f, Prop OFF.", onDuration);
	 }
	 //
	 // Terminate the behavior based on depth.
	 //
	 //if( (_endDepth >= 0) &&
     if( 
	 (_navigation->depth() < _endDepth) ) 
	 {
		setSpeed(DynamicControlIF::Speed, 0.);
		Syslog::write("Undock::execute() - depth = %.1f; above endDepth "
					  "- all finished.\n", _navigation->depth());
		setState(Finished);
		return;
     }
	 //
	 // Abort the mission if the abort depth is exceeded.
	 //
     if( (_abortDepth >= 0) &&
	 (_navigation->depth() > _abortDepth) ) 
	 {
		 Syslog::write("Undock::execute() - depth = %.1f; below abortDepth "
					   "- ABORTING THE MISSION.\n", _navigation->depth());
		 abortMission();
		 return;
     }
	 //
	 // Abort the mission if the duration is exceeded, and neither the
	 // endDepth nor the abortDepth have been reached.  The vehicle
	 // may be stuck in the dock.
	 //
	 // HACK!  I'm going to subtract 5 seconds from the mission clock
	 // here to ensure that this "if" activates before the one in the
	 // base class does.  The base class merely sets the state to
	 // "finished" when the behavior exceeds its duration.
	 //
     if( (_duration >= 0) && 
		 ( _missionClock->seconds() -5.0 >= _duration ) &&
		 (_abortOnTimeout ) )
	 {
		 Syslog::write("Undock::execute() - depth = %.1f; duration exceeded "
					   "- ABORTING THE MISSION.\n", _navigation->depth());
		 abortMission();
		 return;
     }
  }
}


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


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

  if (_speed < 0.) 
  {
    printError("Invalid speed");
    valid = False;
  }
#if 0     
     if( ( _endDepth != NoEndDepth ) &&
	 ( _endDepth < 0.0 ) ) 
     {
	  printError("Negative depth; must be positive");
	  valid = False;
     }
#endif
  return valid;
}