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

#define NoEndDepth -100.0


Ascend::Ascend()
  : Behavior(AscendBehaviorName, Sequential)
{
  attributes.add(new HorizontalModeAttribute("horizontalMode", 
					     "Either \"heading\", "
					     "\"rudder\", or \"waypoint\"",
					     &_horizontalMode,
					     DynamicControlIF::HmInitial ));

  attributes.add(new FloatAttribute("horizontal", 
				    "Commanded horizontal value",
				    &_horizontal, 0.0 ));

  attributes.add(new AngleAttribute("pitch", "Commanded pitch",
				    &_pitch ));

  attributes.add(new FloatAttribute("speed", "Commanded speed",
				    &_speed ));

  attributes.add(new FloatAttribute("endDepth", 
				    "End behavior when shallower than this", 
				    &_endDepth, NoEndDepth ));

  attributes.add(new BooleanAttribute("dropDescentWeight", 
				      "Drop descent weight",
				      &_dropDescentWeight, 
				      False));

  attributes.add(new BooleanAttribute("dropAscentWeight", 
				      "Drop ascent weight",
				      &_dropAscentWeight,
				      False));
}


Ascend::~Ascend()
{
}


Boolean Ascend::shouldBehaviorStart( )
{
     return verticalSequence();
}

void Ascend::execute()
{
     Boolean debug = False;
     
     if (_dropAscentWeight) {
	  // Drop ascent weight, if not yet dropped
     }
     
     if (_dropDescentWeight) {
	  // Drop descent weight, if not yet dropped
     }
     
     dprintf("Ascend::execute() - _navigation->depth=%.2f, _endDepth=%.2f\n",
	     _navigation->depth(), _endDepth);
     
     
     if( (_endDepth >= 0) &&
	 (_navigation->depth() < _endDepth) ) {
	  
	  dprintf("Ascend::execute() - above endDepth - all finished.\n");
	  setState(Finished);
	  
	  return;
     }
     
     
     setVertical(DynamicControlIF::Pitch, _pitch);

     
     if( _horizontalMode != DynamicControlIF::HmInitial ) {
	  setHorizontal(_horizontalMode, _horizontal);
     }

     setSpeed(DynamicControlIF::Speed, _speed);
}


Boolean Ascend::validInput()
{
     Boolean valid = True;
     
     if (_pitch < 0.) {
	  printError("Negative pitch; must be positive for ascent");
	  valid = False;
     }
     
     if( ( _endDepth != NoEndDepth ) &&
	 ( _endDepth < 0.0 ) ) 
     {
	  printError("Negative depth; must be positive");
	  valid = False;
     }

     // Convert input values as necessary
     if( (_horizontalMode == DynamicControlIF::Heading ) ||
	 (_horizontalMode == DynamicControlIF::Rudder ) )
	  _horizontal = Math::degToRad( _horizontal );

     return valid;
}