/****************************************************************************/
/* 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 "Syslog.h"

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("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 ));

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


DepthEnvelope::~DepthEnvelope()
{
}


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

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

  return valid;
}


void DepthEnvelope::execute()
{
  Boolean debug = False;

  Boolean abortDepthSpecified = (_abortDepth != NoAbortDepth);
  Boolean abortAltitudeSpecified = (_abortAltitude != NoAbortAltitude );
  Boolean minAltitudeSpecified = (_minAltitude != NoMinimumAltitude);

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


  //** First, confirm vehicle is not outside of abort depth
  if (abortDepthSpecified && (depth > _abortDepth)) {
       Syslog::write("DepthEnvelope -- Measured depth %lf over allowable depth %lf.  Aborting!", depth, _abortDepth );
    abortMission();
    return;
  }

  // Or abort altitude
  if (abortAltitudeSpecified &&
      (altitude > 0.0) &&
      (altitude < _abortAltitude) ) {
       Syslog::write("DepthEnvelope -- Measured altitude %lf over allowable altitude %lf.  Aborting!", altitude, _abortAltitude );
       abortMission();
       return;
  }

  // (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;
  }

  // Depth to minimum altitude
  if( minAltitudeSpecified &&
       (altitude > 0.0) ) {
       double depthToMinAltitude = depth + altitude - _minAltitude;

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

  }


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

  // Too deep?
//  if (depth > maxDepth) {
//       dprintf("DepthEnvelope::execute() - depth=%.3f, too deep", depth);
//       setVertical(DynamicControlIF::Depth, maxDepth);
//       return;
//  }



  // Too shallow?
//  if (depth < minDepth) {
//       dprintf("DepthEnvelope::execute() - depth=%.3f, too shallow", depth);
//       setVertical(DynamicControlIF::Depth, minDepth);
//       return;
//  }

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