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

DropWeightSafeMode::DropWeightSafeMode()
   : Behavior(DropWeightSafeModeBehaviorName, NonSequential)
{
   Attribute::Input *input;
   // Default run forever
   input = new Attribute::Input("endTime", NoTimeLimitMnem);
   attributes.parse(input);
   delete input;
     
   attributes.add(new FloatAttribute("dropWeightDepth", 
				     "Drop Weight Depth", 
				     &_dropWeightDepth ));
     
   attributes.add(new IntegerAttribute("nConsecutive", 
				       "Required number of consecutive "
				       "measurements", 
				       &_nConsecutive ));
   _counter = 0;
   _dropped = False;

   try 
   {
      _dropWeight = new DropWeightIF("dropWeight");
   } 
   catch(...)
   {
      Syslog::write("DropTheWeight:: -- Failed to initialize connection "
		    "to DropWeightIF");
   }

}


DropWeightSafeMode::~DropWeightSafeMode()
{
   delete _dropWeight;
}


Boolean DropWeightSafeMode::validInput()
{
   if( _dropWeightDepth < 0. ) 
   {
      printError("Drop Weight Depth must be > 0.");
      return False;
   }
   if( _nConsecutive < 1 ) 
   {
      printError("nConsecutive must be greater than one.");
      return False;
   }
   return True;
}


void DropWeightSafeMode::execute()
{
   //
   // If the vehicle goes below the dropWeightDepth, drop the weight.
   //
   double depth = _navigation->depth();
   if (depth > _dropWeightDepth && !_dropped )
   {
      _counter++;
      if(_counter > _nConsecutive)
      {
	 Syslog::write("DropWeightSafeMode:  The measured depth %lf, "
		       "is greater than the drop weight depth %lf.  \n"
		       "Drop the weight.", 
		       depth, _dropWeightDepth );

	 _dropWeight->release();
	 _dropped = True;
	 //
	 // It's most likely that the mission has already been aborted by
	 // DepthEnvelope, presuming it's present on the behavior stack. Call
	 // abortMission() again anyway to be sure.
	 //
	 abortMission();
      }
   }
   else
      //
      // Reset the counter since we are counting only consecutive occurances.
      //
      _counter = 0;

   //
   // Add humidity sensor code here.
   //

   //
   // (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() ) 
   {
      Syslog::write("DropWeightSafeMode::execute() - "
		    "Received empty stack, passing it through.");
      return;
   }

}