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

#define NoEndDepth -1000.0
#define NoMinAltitude -1000.0

#define NoValue;


Descend::Descend()
  : Behavior(DescendBehaviorName, 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("maxDepth", 
				    "End behavior when below this depth",
				    &_maxDepth,
				    NoEndDepth ));

  attributes.add(new FloatAttribute("minAltitude", 
				    "End behavior when below this altitude",
				    &_minAltitude,
				    NoMinAltitude ));

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

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


Descend::~Descend()
{

}

Boolean Descend::shouldBehaviorStart( void )
{
     return verticalSequence();
}

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

     Boolean endOnDepth = (_maxDepth != NoEndDepth );
     Boolean endOnAltitude = ( _minAltitude != NoMinAltitude );
     
     Boolean finished = False;

     if( (endOnDepth) &&
	 (_navigation->depth() > _maxDepth) ) {
       Syslog::write("Descend::execute() - Descend terminating on depth %lf > %lf at time %lf\n", _navigation->depth(), _maxDepth, _missionClock->seconds() );
	  finished = True;
     }

     if( endOnAltitude &&
	 (_navigation->altitude() < _minAltitude) ) {
       Syslog::write("Descend::execute() - Descend terminating on altitude %lf < %lf at time %lf\n", _navigation->altitude(), _minAltitude, _missionClock->seconds() );
	  finished = True;
     }

     if ( finished ) {

	  dprintf("Descend::execute() - depth=%.4f, alt=%.4f\n",
		  _navigation->depth(), _navigation->altitude());
	  
	  if (_dropDescentWeight) {
	       // Drop weight
	  }
	  
	  if (_dropAscentWeight) {
	       // Drop weight
	  }
	  
	  setState(Finished);
       
	  return;
     }
     
     dprintf("Descend::execute() - horizontal=%.2f, pitch=%.2f, speed=%.2f\n",
	     _horizontal, _pitch, _speed);
     
     if( _horizontalMode != DynamicControlIF::HmInitial ) {
	  dprintf("Descend::execute() -- Setting horizontal to %lf, mode: ", _horizontal);
	  switch( _horizontalMode ) {
	  case DynamicControlIF::Heading:
	       dprintf(" heading\n");
	       break;
		
	  case DynamicControlIF::Rudder:
	       dprintf(" rudder\n");
	       break;
	       
	  case DynamicControlIF::Waypoint:
	       dprintf(" waypoint\n");
	       break;
	  default:
	       dprintf(" not sure\n");
	       break;
	  }

	  setHorizontal(_horizontalMode, _horizontal);
     }

     setVertical(DynamicControlIF::Pitch, _pitch);
     setSpeed(DynamicControlIF::Speed, _speed);
     
     return;
}


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

  if (_speed < 0.) {
    printError("Negative speed");
    valid = False;
  }

  if( ( _maxDepth != NoEndDepth ) &&
      (_maxDepth < 0.) )
  {
    printError("Negative maxDepth");
    valid = False;
  }

  if( ( _minAltitude != NoMinAltitude ) &&
      (_minAltitude < 0.) )
  {
    printError("Negative minAltitude");
    valid = False;
  }


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

  return valid;
}