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

SetElevator::SetElevator()
  : Behavior(SetElevatorBehaviorName, NonSequential)
{
  attributes.add(new AngleAttribute("elevator", "Commanded elevator angle", 
				    &_angle ));

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

  attributes.add(new AngleAttribute("heading", "Commanded heading",
				    &_heading));

  attributes.add(new FloatAttribute("minDepth", "End if above this depth",
				    &_minDepth, -1.0 ));

  attributes.add(new FloatAttribute("maxDepth", "End if below this depth",
				    &_maxDepth, 10000.0 ));

  attributes.add(new AngleAttribute("maxPitch", "End if abs value of "
				    "this pitch is exceeded",
				    &_maxPitch, 90.0 ));
}


SetElevator::~SetElevator()
{
}

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

void SetElevator::execute()
{
  NavigationIF::Position position;
  NavigationIF::Attitude attitude;
  
  _navigation->state(&position, &attitude);
  
  if (position.z < _minDepth || position.z > _maxDepth ||
      fabs(attitude.pitch) > _maxPitch) {
    setState(Finished);
    return;
  }

  setHorizontal(DynamicControlIF::Heading, _heading);
  setVertical(DynamicControlIF::Elevator, _angle);

  if( _speed >= 0 ) 
       setSpeed(DynamicControlIF::Speed, _speed);

  return;
}


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

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

  return valid;
}