#include "LowAltitudeFollowing.h"
#include "FloatAttribute.h"
#include "AngleAttribute.h"
#include "BooleanAttribute.h"
#include "IntegerAttribute.h"
#include "Syslog.h"
#include "WorkSiteIF.h"
#include "NavUtils.h"
#include "MissionTimeAttribute.h"
//#include "dvl/Dvl.h"
#include <time.h>

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

#define NotSpecified -100000
#define AngleNotSpecified ((NotSpecified) / Math::RadsPerDeg)
#define SUCCESSIVE_BAD 3
#define SUCCESSIVE_GOOD 5

const Boolean sim = False;

LowAltitudeFollowing::LowAltitudeFollowing()
   : Behavior(LowAltitudeFollowingBehaviorName, NonSequential)
{
 
   // Should run forever
   Attribute::Input *input;
   input = new Attribute::Input("endTime", NoTimeLimitMnem);
   attributes.parse(input);
   delete input;

   // Elevator angle during deltaDepth shutoff period.
   _elevator = -3.*PI/180.;
   _propOffCntr = 0;
   _propOnCntr = 0;

}



LowAltitudeFollowing::~LowAltitudeFollowing()
{
}


void LowAltitudeFollowing::execute( void )
{

   if( !lowAltitudeFlightEnabled() ) return;

   double altitude = _navigation->altitude();

   //
   // First, this behavior is only active near the bottom, so we can't get
   // bad altitude readings from being too high. Then, for SUCCESSIVE_BAD
   // altitude readings shut off the prop.
   if( altitude == INVALID_ALTITUDE )
   {
      if( !_altitudeStop) 
      {
	 _altitudeStop = True;
	 Syslog::write("LowAltitudeFollowing -- Invalid Altitude. "
	               "Stopping propulsion at t= %.2f.\n", 
	                _missionClock->seconds());
      }
   }
   else
   {
      if( _altitudeStop)
      {
	 Syslog::write("LowAltitudeFollowing -- Altitude is valid at t=%.2f. "
	 "Turning propulsion back on.", _missionClock->seconds() );
	 _altitudeStop = False;
      }
   }

   if (_altitudeStop) 
   { 
      //
      // If the prop has been shut off, we must overide the lower behaviors at
      // every sampling instant until we decide to resume.
      setSpeed(DynamicControlIF::Speed, 0.0);
      setVertical(DynamicControlIF::Elevator, _elevator);
   }

   // If the stack is empty, then pass it through (that is, if all behaviors
   // above are inactive, then be inactive also.
   if( isOutputEmpty() ) 
   {
      //dprintf("LowAltitudeFollowing::execute() - Received empty stack, "
      //"passing it through\n");
      return;
   }


}

Boolean LowAltitudeFollowing::validInput()
{
   Boolean valid = True;
   return valid;
}