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

Launch::Launch()
  : Behavior(LaunchBehaviorName, NonSequential)
{
  attributes.add(new AngleAttribute("elevator", "Commanded elevator", 
				    &_elevator));

  attributes.add(new FloatAttribute("finalDepth", 
				    "End when below this depth",
				    &_finalDepth));

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

  attributes.add(new BooleanAttribute("abortOnTimeout",
				      "Should mission abort if not at depth?",
				      &_abortOnTimeout,
				      False ) );

  _timeout = 0.0;
}


Launch::~Launch()
{
}


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

void Launch::execute()
{
  double depth = _navigation->depth();
  double now = _missionClock->seconds();

  // Reached final depth?
  if (depth > _finalDepth) {
    setState(Finished);
    return;
  }

  // Check for launch timeout, but only if you're going to abort
  if( _abortOnTimeout && 
      ( now - startTime() > _timeout ) ) {
       setState(Finished);
       abortMission();
       return;
  }

  double cmdSpeed;

//  const double minInterval = 0.4;

//  if (depth > (_finalDepth) || 
//      (endTime() - _missionClock->seconds()) < minInterval) {
//
    // Almost done; stop thruster
//    cmdSpeed = 0.;
//  }
//  else

  cmdSpeed = _speed;

  setHorizontal(DynamicControlIF::Rudder, 0.);
  setVertical(DynamicControlIF::Elevator, _elevator);
  setSpeed(DynamicControlIF::Speed, cmdSpeed);
}


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

  if (_finalDepth < 0.) {
    printError("Negative finalDepth");
    valid = False;
  }

  // Take over the timeout function if abortOnTimeout is set
  if( _abortOnTimeout ) {
       _timeout = duration();
       _duration += 10;
  }

  return valid;
}