/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : DepthEnvelope.cc                                              */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include "MissionTimeAttribute.h"
#include "DepthEnvelope.h"
#include "FloatAttribute.h"
#include "AcousticModemIF.h"
#include "AngleAttribute.h"
#include "BooleanAttribute.h"
#include "Syslog.h"
#include "TimeP.h"
#include "LowAltitudeFollowing.h"

//const double PI=3.1415926535898;

DepthEnvelope::DepthEnvelope()
   : Behavior(DepthEnvelopeBehaviorName, NonSequential)
{
   Attribute::Input *input;

   attributes.add(new FloatAttribute("minDepth", "Minimum depth", 
				     &_minDepth ));
     
   attributes.add(new FloatAttribute("maxDepth", "Maximum depth", 
				     &_maxDepth ));

   attributes.add(new FloatAttribute("abortLockoutDepth",
				     "Altitude aborts are ignored when shallower than this depth",
				     &_hardDeck, NoHardDeck));    
 
   attributes.add(new FloatAttribute("minAltitude", "Minimum altitude", 
				     &_minAltitude, NoMinimumAltitude ));
     
   attributes.add(new FloatAttribute("abortDepth", 
				     "Abort when below this depth", 
				     &_abortDepth, NoAbortDepth ));
     
   attributes.add(new FloatAttribute("abortAltitude",
				     "Abort when below this altitude",
				     &_abortAltitude, NoAbortAltitude ));
     
   attributes.add(new FloatAttribute("deltaDepthRestart",
				     "Restart when depth has decreased this much",
				     &_deltaDepth, NoDeltaDepth ));

   attributes.add(new AngleAttribute("maxLowerPitch",
				     "Lower pitch limit when near bottom.",
				     &_maxLowerPitch, NoMaxLowerPitch ));
     
   attributes.add(new FloatAttribute("pitchLimitAltitude",
				       "Altitude below which lower pitch limits",
                                     &_pitchLimitAlt, NoPitchLimitAlt ));
     
   attributes.add(new FloatAttribute("minObstacleRange",
				       "Abort when an obstacle is too close",
				       &_minFlsHorzRange, 50. ));
     
   attributes.add(new BooleanAttribute("altSuspendDepthControl",
				       "Use depth control while altitude is invalid",
				       &_altSuspendDepthControl, False ));
     
   attributes.add(new AngleAttribute("altSuspendPitch",
				       "Use pitch control while altitude is invalid",
				       &_altSuspendPitch, NoAltSuspendPitch ));
     
   attributes.add(new FloatAttribute("altTimeOut",
				       "Suspend prop if altitude invalid too long",
				       &_altTimeOut, NoAltTimeOut ));

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

   // Elevator angle during deltaDepth shutoff period.
   _elevator = -3.*Math::RadsPerDeg;
   _lastDepth = 5.;
   _latchData = True;
   _indirectAltCntrl = False;
}


DepthEnvelope::~DepthEnvelope()
{

}


Boolean DepthEnvelope::validInput()
{
   Boolean valid = True;

   if (_maxDepth < _minDepth) {
      printError("maxDepth is less than minDepth");
      valid = False;
   }

   if( _maxLowerPitch < 0. && (_maxLowerPitch != NoMaxLowerPitch*Math::RadsPerDeg))
   {
      Syslog::write("DepthEnvelope - Error. maxLowerPitch must be a postive "
		    "number.\n"
	            "Flipping the sign now.");
      _maxLowerPitch *= -1;
      Syslog::write("maxLowerPitch = %.2f", _maxLowerPitch);
   }

   if( _maxLowerPitch == NoMaxLowerPitch*Math::RadsPerDeg && 
       _pitchLimitAlt != NoPitchLimitAlt )
   {
      Syslog::write("DepthEnvelope - Error. Since pitchLimitAlt is specified, \n"
		    "you must also specify maxLowerPitch.");
      valid = False;
   }

   if( _maxLowerPitch != NoMaxLowerPitch*Math::RadsPerDeg && 
       _pitchLimitAlt == NoPitchLimitAlt )
   {
      Syslog::write("DepthEnvelope - Error. Since maxLowerPitch is specified, \n"
		    "you must also specify pitchLimitAlt.");
      valid = False;
   }

   if( _altSuspendDepthControl && _altSuspendPitch != NoAltSuspendPitch*Math::RadsPerDeg )
   {
      Syslog::write("DepthEnvelope - Error. You cannot specify both \n"
		    "altSuspendDepthControl and altSuspendPitch.");
      valid = False;
   }

   if( _altTimeOut == NoAltTimeOut && 
       (_altSuspendDepthControl || _altSuspendPitch != NoAltSuspendPitch*Math::RadsPerDeg ))
   {
      Syslog::write("DepthEnvelope - Error. You must specify altTimeOut.");
      valid = False;
   }

   return valid;
}


void DepthEnvelope::execute()
{
   Boolean debug = False;
   Boolean abortDepthSpecified = (_abortDepth != NoAbortDepth);
   Boolean abortAltitudeSpecified = (_abortAltitude != NoAbortAltitude );
   Boolean minAltitudeSpecified = (_minAltitude != NoMinimumAltitude);
   Boolean hardDeckSpecified = (_hardDeck != NoHardDeck);
   Boolean deltaDepthSpecified = (_deltaDepth != NoDeltaDepth);
   Boolean maxLowerPitchSpecified = (_maxLowerPitch != NoMaxLowerPitch*Math::RadsPerDeg);
   Boolean pitchLimitAltSpecified = (_pitchLimitAlt != NoPitchLimitAlt);

   double maxDepth = _maxDepth, minDepth = _minDepth, hardDeck = _hardDeck;
   double depth = _navigation->depth();
   double altitude = _navigation->altitude();

   double flsVertRange, flsHorzRange;
   Boolean useFls;

   _navigation->getFlsData( &flsVertRange, &flsHorzRange, &useFls );


   //make sure minAltitude is reasonable, since the prop is stopped at
   //the abort altitude and restarted at the minimum altitude
   if (!minAltitudeSpecified) _minAltitude = _abortAltitude;

   //** First, confirm vehicle is not outside of abort depth
   if (abortDepthSpecified && (depth > _abortDepth)) {
      Syslog::write("DepthEnvelope -- Measured depth %lf over allowable "
		    "depth %lf at t= %.2f.  Stopping!", 
		    depth, _abortDepth, _missionClock->seconds() );
      _depthStop = True;
   }

   //if we've floated shallower than the max depth, then clear the depth stop flag
   if (depth <= maxDepth) {
     if (_depthStop) Syslog::write("DepthEnvelope -- Measured depth %lf shallower than %lf. Restarting!", depth, maxDepth);
     _depthStop = False;
   }

   // check the altitude,stop or restart the prop as appropriate
   if (abortAltitudeSpecified && altitude != INVALID_ALTITUDE) {
     //Too close to the bottom. Latch the prop off.
     if (altitude < _abortAltitude && !useFls) {
       if( !_altitudeStop ) 
       {
	  Syslog::write("DepthEnvelope -- Measured altitude = %lf. Allowable "
	                "altitude = %lf. t= %.2f. Stopping!",
	                altitude, _abortAltitude, _missionClock->seconds());
	  _restartDepth = depth - _deltaDepth;
       }
       _altitudeStop = True;
     }
     //The FLS has detected something in our path ahead. Latch the prop off.
     if (altitude < _abortAltitude && useFls && flsHorzRange<_minFlsHorzRange ) {
       if( !_altitudeStop ) 
       {
	  Syslog::write("DepthEnvelope -- Obstacle detected at altitude = %lf "
                        "and range %lf.\n Allowable "
		        "altitude = %lf. t= %.2f. Stopping!",
                        altitude, flsHorzRange, _abortAltitude, 
                        _missionClock->seconds());
	  _restartDepth = depth - _deltaDepth;
       }
       _altitudeStop = True;
     }
     //restart prop when we are higher than the minimum altitude
     if (!deltaDepthSpecified && _altitudeStop &&  (altitude>_minAltitude)) {
       Syslog::write("DepthEnvelope -- Measured altitude = %lf. Abort altitude "
		     "= %lf. t= %.2f, starting prop!",
		     altitude, _abortAltitude, _missionClock->seconds());
       _altitudeStop = False;
      }
      if (deltaDepthSpecified && (altitude>_minAltitude) && _altitudeStop && 
	  (depth < _restartDepth)) {
	Syslog::write("DepthEnvelope -- Depth %lf shallower than restart "
		      "depth %lf \nand altitude %lf greater than abort altitude "
		      "%lf at t= %.2f. Starting prop!",
		      depth, _restartDepth, altitude, _abortAltitude, 
		      _missionClock->seconds());
	_altitudeStop = False;
      }
   }

   if ((altitude==INVALID_ALTITUDE) && 
       (hardDeckSpecified && (depth < hardDeck)) && 
       _altitudeStop) {
     Syslog::write("DepthEnvelope -- Depth %lf shallow enough to restart "
		   "prop at t= %.2f!",
		   depth, _missionClock->seconds());
     _altitudeStop = False;
   }

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

   if ( altitude==INVALID_ALTITUDE )
   {
      if (getVerticalMode() == DynamicControlIF::Altitude )
      {
	 if( _latchData )
	 {
	    Syslog::write("DepthEnvelope -- Direct Altitude Control:  "
			  "Invalid Altitude at t= %.2f!. "
			  "Latching depth %lf.", _missionClock->seconds(), depth );
	    _lastDepth = depth;
	    if( deltaDepthSpecified ) _restartDepth = depth - _deltaDepth;
	    _lastTime = _missionClock->seconds();
	    _latchData = False;
	    if( _altSuspendDepthControl )
	    {
	       Syslog::write("DepthEnvelope -- Direct Altitude Control:  "
			     "Maintaining constant depth.");
	    }	 
	    if( _altSuspendPitch != NoAltSuspendPitch*Math::RadsPerDeg )
	    {
	       Syslog::write("DepthEnvelope -- Direct Altitude Control:  "
			     "Maintaining constant pitch.");
	    }	 
	 }
	 if( _altSuspendDepthControl )
	 {
	    setVertical(DynamicControlIF::DepthImmediate, _lastDepth);
	 }
	 else if( _altSuspendPitch != NoAltSuspendPitch*Math::RadsPerDeg )
	 {
	    setVertical(DynamicControlIF::Pitch, _altSuspendPitch);
	 }
	 else
	 {
	    // The vehicle won't restart unless both the abort and minimum
	    // altitudes are specified.
	    if( !_altitudeStop )
	    {
	       Syslog::write("DepthEnvelope -- Direct Altitude Control:  "
			     "Altitude invalid at t= %.2f!. "
			     "Suspending propulsion.", _missionClock->seconds());
	    }
	    _altitudeStop = True;
	 }
	 if( _missionClock->seconds() - _lastTime > _altTimeOut )
	 {
	    if( !_altitudeStop )
	    {
	       Syslog::write("DepthEnvelope -- Direct Altitude Control:  "
			     "Altitude still invalid at t= %.2f!. "
			     "Suspending propulsion.", _missionClock->seconds());
	    }
	    _altitudeStop = True;
	    //abortMission();
	 }
      }
   }
   else if( !_latchData )
   {
      Syslog::write("DepthEnvelope -- Obtained valid altitude at %.2f.",
		    _missionClock->seconds());

      if( _missionClock->seconds() - _lastTime < _altTimeOut &&
	  getVerticalMode() == DynamicControlIF::Altitude )
      {
	 Syslog::write("DepthEnvelope -- Direct Altitude Control: %.2f seconds left until timeout.",
		       _altTimeOut- (_missionClock->seconds() - _lastTime));
      }
      _latchData = True;
   }

   if( minAltitudeSpecified )
   {
      double depthToMinAltitude = depth + altitude - _minAltitude;
      //
      // 
      _maxLowerPitchDepth = depth + altitude - _pitchLimitAlt;

      if (depthToMinAltitude < _maxDepth)
	 // "Clip" maximum depth
	 maxDepth = depthToMinAltitude;
   }

   if (_altitudeStop || _depthStop) { //if _altitudeStop flag is set, then turn off the prop
     setSpeed(DynamicControlIF::Speed, 0.0);
     if( deltaDepthSpecified ) 
       setVertical(DynamicControlIF::Elevator, _elevator);
   }

   // Commanded too deep
   if( (  (getVerticalMode()==DynamicControlIF::Depth) ||
          (getVerticalMode()==DynamicControlIF::DepthImmediate)) &&
       (getVertical() > maxDepth ) ) 
   {
      dprintf("DepthEnvelope::execute() - commanded depth = %.3f, too deep\n",
	      getVertical() );
      setVertical(getVerticalMode(), maxDepth);

      if( pitchLimitAltSpecified &&
          maxLowerPitchSpecified && (depth > _maxLowerPitchDepth ))
      {
	 setVertical(DynamicControlIF::Depth, maxDepth, _maxLowerPitch);
	 //Syslog::write("DepthEnvelope - maxLowerPitch = %.2f",
	 //	       _maxLowerPitch);
      }
      return;
   } 

   // Too shallow?
   // Commanded shallow?
   if( ((getVerticalMode() == DynamicControlIF::Depth) ||
	(getVerticalMode() == DynamicControlIF::DepthImmediate)) &&
       (getVertical() < minDepth ) ) {
     dprintf("DepthEnvelope::execute() - commanded depth = %.3f, too shallow\n",
	     getVertical() );
     setVertical(getVerticalMode(), minDepth);
     return;
   }
}