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

Yoyo::Yoyo()
  : Behavior(YoyoBehaviorName, NonSequential)
{
  _cyclesCompleted = 0.;
  _goingDown = True;

  attributes.add(new FloatAttribute("minDepth", 
				    "Start down when above this depth",
				    &_minDepth));

  attributes.add(new FloatAttribute("maxDepth", 
				    "Start up when below this depth",
				    &_maxDepth));

  attributes.add(new FloatAttribute("minAltitude", 
				    "Start up when below this altitude",
				    &_minAltitude, 
				    5.));

  attributes.add(new FloatAttribute("maxCycles", 
				    "End after this many cycles",
				    &_maxCycles, 1.));
}


Yoyo::~Yoyo()
{
}

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


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

  // TEST TEST TEST
  //  const double dummyPad = 1.1;   // This value works
  const double dummyPad = 0.5;   // Value from Odyssey code

  if (_maxCycles - _cyclesCompleted < 0.25) {
    setState(Finished);
    return;
  }

  double depth = _navigation->depth();
  double targetDepth;

  if (_goingDown) {

    // Going down
    targetDepth = _maxDepth;

    if ((depth > _maxDepth - dummyPad) || 
	(_navigation->altitude() < _minAltitude)) {

      // Reached bottom; start heading up
      _goingDown = False;
      targetDepth = _minDepth;

      // Completed 1 "yo", which is 0.5 cycle
      _cyclesCompleted += 0.5;
    }
  }
  else {

    // Going up
    targetDepth = _minDepth;

    if (depth < _minDepth + dummyPad) {

      // Reached top; start heading down
      _goingDown = True;
      targetDepth = _maxDepth;

      // Completed 1 yo, which is 0.5 cycle
      _cyclesCompleted += 0.5;
    }
  }

  setVertical(DynamicControlIF::Depth, targetDepth);
}


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

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

  if (_maxCycles <= 0.) {
    printError("maxCycles must be greater than zero");
    valid = False;
  }

  return valid;
}