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

const Boolean sim = False;

Avoid::Avoid()
   : Behavior(AvoidBehaviorName, NonSequential)
{
   
   attributes.add(new IntegerAttribute("maxbad", 
				     "Maximum number of consec. sonar errors",
				     &_maxBad, 10));
   attributes.add(new IntegerAttribute("nsamples", 
				     "Number of previous ranges to keep",
				     &_nSamples, MEDIANSIZE));
   attributes.add(new FloatAttribute("minrange", 
				     "Minimum allowable range to an obstacle",
				     &_minRange, NotSpecified));
   attributes.add(new IntegerAttribute("maxtooclose", 
				     "Maximum number of ranges less than minrange",
				     &_maxTooClose, NotSpecified));
   attributes.add(new IntegerAttribute("mintooclose", 
				     "Condition to resume propulsion",
				     &_minTooClose, NotSpecified));
   attributes.add(new FloatAttribute("depthoffset", 
				     "depth to get over obstacle",
				     &_depthOffset, NotSpecified));
   attributes.add(new FloatAttribute("bottomlockout", 
				     "dist below vehicle to ignore bottom returns",
				     &_bottomLockOut, NotSpecified));


   // Depth envelope should be default run forever
   Attribute::Input *input;
   input = new Attribute::Input("endTime", NoTimeLimitMnem);
   attributes.parse(input);
   delete input;

   if( _bottomLockOut == NotSpecified ) _bottomLockOut = _minRange;

   _first = True;
   _firstValid = True;
   _avoidMode = False;
   _consecutiveGood = 0;
   try 
   {
      _echo = new EchoSounderIF("echo");
   } 
   catch(...)
   {
      Syslog::write("Avoid -- Failed to initialize connection "
		    "to EchoSounderIF");
      _echo = NULL;
      //
      // DON'T start a mission with a dead OA sonar.
      abortMission();
   }
   _badStatusCntr = 0;
   _nRange = 0;
   _stopProp = False;
   _lastSeconds = 0;
   _lastNanoSeconds = 0;
   for( int i=0; i<NRANGES; i++ ) _rangeList[i] = 200.;
}



Avoid::~Avoid()
{
   if( !_echo )  delete _echo;
}


void Avoid::execute( void )
{
   double dn, de;
   double dtw, goal;
   Boolean debug = True;
   Boolean nearDock = False, lastNearDock = False;
   double updateTimeSecs = 0.;
   NavigationIF::Position position;
   NavigationIF::Attitude attitude;
   EchoSounderIF::Data data;
   DeviceIF::Status status;
   int median, i;
   //
   // If the prop has been shut off, we must overide the lower behaviors at
   // every sampling instant until we decide to resume.
   if( _stopProp )  setSpeed(DynamicControlIF::Speed, 0.0);


   // Get current position
   _navigation->state(&position, &attitude);

   if( _avoidMode )
   {

      if( (getVerticalMode() == DynamicControlIF::Depth) )
	 setVertical(DynamicControlIF::Depth, _avoidDepth);
      //else
      // Abort if elevator or pitch mode??

      double dx = position.x - _lastNorthing;
      double dy = position.y - _lastEasting;
      _arclen += sqrt( dx*dx + dy*dy);
      
      if( _arclen >= _minRange ) 
      {
	 _avoidMode = False;
	 Syslog::write("Avoid:: _arclen = %.2f; end avoid mode.", _arclen);
      }

      _lastNorthing = position.x;
      _lastEasting  = position.y;
      
   }


   status = _echo->get(&data);
   //Syslog::write("data.range = %.2f", data.range );
   //Syslog::write("data.updateTime.seconds = %ld", data.updateTime.seconds );
   //fflush(stdout);


   if( data.updateTime.seconds     != _lastSeconds ||
       data.updateTime.nanoSeconds != _lastNanoSeconds )
   {
      //
      // We've got new data.  _badStatusCntr is the number of consecutive bad
      // measurements.  updateTime changes when the instrument returns with
      // data, when the readNChars times out (2.5 seconds), or if
      // EchoSounderDriver::parseData fails.

      _lastSeconds     = data.updateTime.seconds;
      _lastNanoSeconds = data.updateTime.nanoSeconds;

      if( status != DeviceIF::Ok ) 
      {
	 _badStatusCntr++;
	 Syslog::write("Avoid:: Sonar has not responded in %d tries.", 
		       _badStatusCntr);
	 return;
      }
      else _badStatusCntr = 0;

   }
   else return;

   //
   // This requires _maxBad consecutive tries by the driver to get sonar data.
   if( _badStatusCntr >= _maxBad ) 
   {
      Syslog::write("Avoid::execute() -- No response from sonar in last %d tries. "
		    "Aborting.", _maxBad);
      setState(Finished);
      abortMission();
      return;
   }

   if(_first)
   {
      //_bearing = PI + Math::modPi( atan2(_easting - position.y,
      //				 _northing - position.x) - PI);
      for( int i=0; i<MEDIANSIZE; i++ )
      {
	 _nList[i] = 0.;
	 _eList[i] = 0.;
      }
      _ngood = 0;
      //
      //
      Syslog::write( "Obstacle Avoidance Initialization "
		     "at t= %-15.2f \n"
		     "The current location (N,E) = %-15.1f, %-15.1f )\n",
		     _missionClock->seconds(),
		     position.x, position.y);
      _first = False;
   }
   //
   // Load the two queues.  Check for bad (too close) ranges.
   double r = data.range;
   double theta = attitude.pitch;
   if( _nRange < _nSamples )
   {
      _rangeList[_nRange] = data.range;
      _pitchList[_nRange] = attitude.pitch;
      if( ( _bottomLockOut != NotSpecified     ) &&
	  ( r*sin(theta) >  - _bottomLockOut ) &&
	  ( r*cos(theta) <=   _minRange      )    )  
	    _hitList[_nRange] = True;
      else  _hitList[_nRange] = False;
      _nRange++;
   }
   else
   {
      for( i=0; i<_nSamples-1; i++ )
      {
	 _rangeList[i] = _rangeList[i+1];
	 _pitchList[i] = _pitchList[i+1];
	 _hitList[i]   = _hitList[i+1];
      }
      _rangeList[_nSamples - 1] = data.range;
      _pitchList[_nSamples - 1] = attitude.pitch;
      if( ( _bottomLockOut != NotSpecified     ) &&
          ( r*sin(theta) >  - _bottomLockOut ) &&
	  ( r*cos(theta) <=   _minRange      )    )  
	    _hitList[_nSamples - 1] = True;
      else  _hitList[_nSamples - 1] = False;
   }      
   //
   // Now count up the bad ones:
   short nbad = 0;
   short nbadList = 0;
   dprintf("_minRange = %.2f", _minRange);
   for( i=0; i<_nRange; i++ )
   {
      dprintf("range, pitch, hit [%d] = %.2f  %.2f  %d", 
	      i, _rangeList[i], _pitchList[i], _hitList[i]);
      if( _rangeList[i] <= _minRange ) nbad++;
      nbadList += (short) _hitList[i];
   }
   dprintf("nbad = %d, nbadList = %d, _maxTooClose = %d", 
	   nbad, nbadList, _maxTooClose);
   if(debug) fflush(stdout);
   //
   // This is a hysteresis loop.  Stop the prop if there are too many hits.
   // Restart when there are few enough.
   if( ( (nbad     >= _maxTooClose) && (_bottomLockOut == NotSpecified) ) ||
       ( (nbadList >= _maxTooClose) && (_bottomLockOut != NotSpecified) )    )
   {
      _stopProp = True;
      if( !_avoidMode && (_depthOffset != NotSpecified) ) 
      {
	 _avoidDepth = position.z - _depthOffset;
	 //
	 // Avoid the possibility that multiple occurances don't create an
	 // _avoidDepth < 0, which means the propulsion will never resume.
	 if( _avoidDepth <= 5.0 ) _avoidDepth = 5.0;
	 _avoidMode  = True;
	 _arclen     = 0.;
	 _lastNorthing = position.x;
	 _lastEasting  = position.y;
	 Syslog::write("Avoid:: Begin avoid mode.");
      }
      if( _avoidMode ) dprintf("Avoid:: _arclen = %.2f", _arclen);
   }
   if( ( (nbad <= _minTooClose) && (_depthOffset == NotSpecified) ) ||
       ( (nbad <= _minTooClose) && (position.z < _avoidDepth) )        )
   {
      _stopProp = False;
   }
   if( _stopProp ) 
   {
      setSpeed(DynamicControlIF::Speed, 0.0);
      Syslog::write("Avoid:: Obstacle ahead; %d hits < %.2f meters.  "
		    "Shut down propulsion.", nbad, _minRange);
   }


}

Boolean Avoid::validInput()
{
   Boolean debug = False;
   Boolean valid = True;
   char errorStr[128];

   if( _nSamples == NotSpecified || _nSamples > NRANGES ||
      _nSamples < 1 )
   {
      valid = False;
      sprintf( errorStr, "nsamples must be >1 and <%d.", NRANGES);
      printError(errorStr);
   }

   if( _minRange == NotSpecified || _minRange > 200 ||
       _minRange < 5 )
   {
      valid = False;
      sprintf( errorStr, "minrange must be >5 and <200 m.");
      printError(errorStr);
   }

   if( _minTooClose == NotSpecified ) _minTooClose = _maxTooClose;
   else if (_minTooClose < 0 || _minTooClose > _nSamples )
   {
      valid = False;
      sprintf( errorStr, "mintooclose must be >=0 and <=%d.", _nSamples );
      printError(errorStr);
   }

   if( _maxTooClose == NotSpecified || _maxTooClose < _minTooClose ||
      _maxTooClose < 1 )
   {
      valid = False;
      sprintf( errorStr, "maxtooclose must be >0 and > mintooClose.");
      printError(errorStr);
   }

   return valid;
}