/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : Zigzag.cc                                                     */
/* Author   : Rob McEwen, based on the original setpont code.               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 2001/6/22                                                     */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include "Zigzag.h"
#include "FloatAttribute.h"
#include "AngleAttribute.h"
#include "IntegerAttribute.h"
#include "VerticalModeAttribute.h"
#include "MissionTimeAttribute.h"
#include "MathP.h"
#include "Syslog.h"

#define NotSpecified -100000.0
#define AngleNotSpecified (-1000. / Math::RadsPerDeg)

Zigzag::Zigzag()
  : Behavior(ZigzagBehaviorName, NonSequential)
{
  Boolean debug = False;

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

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

  attributes.add(new VerticalModeAttribute("verticalMode", 
					   "Either \"depth\" or \"pitch\"",
					   &_verticalMode,
					   DynamicControlIF::VmInitial));

  _depthAttribute = new FloatAttribute("depth",
				       "Commanded depth if verticalMode "
				       "is \"depth\"",
				       &_depth, 
				       NotSpecified);

  attributes.add(_depthAttribute);

  _pitchAttribute = new AngleAttribute("pitch", 
				       "Commanded pitch if verticalMode "
				       "is \"pitch\"",
				       &_pitch, 
				       AngleNotSpecified);

  attributes.add(_pitchAttribute);

  dprintf("Zigzag::Zigzag() - pitch=%.3f", _pitch);


  attributes.add(new IntegerAttribute("cycles", 
				      "Number of cycles",
				      &_maxLegs, 1));

  attributes.add(new AngleAttribute("headingDeviation", 
				    "Increment heading by this amount "
				    "after each zig",
				    &_headingDeviation, 
				    5.));

  attributes.add(new AngleAttribute("rudder", 
				    "Rudder angle ",
				    &_rudder,
				    5.));

  dprintf("Zigzag - rudder  =%.3f", _rudder);
  dprintf("Zigzag - pitch   =%.3f", _pitch);
  dprintf("Zigzag - Constructed at %.1f\n", _missionClock->seconds());

  _legsCompleted = 0;
  _legStartTime = NotSpecified;
  _first = True;
}


Zigzag::~Zigzag()
{
}


Boolean Zigzag::shouldBehaviorStart( void ) {
     Boolean debug = True;
     Boolean value;

     value = horizontalSequence();
     
//    dprintf("Zigzag::shouldBehaviorStart() 
//                           -- Evaluating if stack is empty -- ");
//     if( value )   dprintf("it is\n");
//     else          dprintf("it isn't\n");
//     return value;

     return horizontalSequence();
}

void Zigzag::execute()
{

  Boolean debug = True;

  double heading;
  // Get current position

  _navigation->heading( &heading );
  //
  // End if the heading is wrong
  //     
  if( fabs( heading - _headingCmd ) >= _headingDeviation && _first )
  {
    Syslog::write(" Zigzag - Initial heading exceeds deviation.  Exit.\n");
    setHorizontal(DynamicControlIF::Rudder, 0.);
    setState(Finished);
    return;
  }
  _first = False;

  if( (state() == Active) && 
      (_legStartTime == NotSpecified) ) {

       // First starting...
       _legStartTime = _missionClock->seconds();
  }
  //
  // End if the time-out has expired.
  //     
  if( ( _duration != NullLegDuration) && 
      (_missionClock->seconds() - _legStartTime >= _duration) ) 
  {
    setHorizontal(DynamicControlIF::Rudder, 0.);
    setState(Finished);
    return;
  }
  //
  // Account for a heading command near zero:
  //
  if( ( _headingCmd - _headingDeviation < 0.)    && 
      ( heading > _headingCmd + PI )                )
    heading -= 2*PI;
      
  if( ( _headingCmd + _headingDeviation > 2*PI)  && 
      ( heading < _headingCmd - PI )                )
    heading += 2*PI;

  //
  // Switch rudder angle if the heading is greater than the commanded deviation:
  //
  if( 
      ( 
        ( _rudder > 0. ) 
      && 
        ( ( _headingCmd - heading ) >= _headingDeviation ) 
      ) 
    ||
      ( 
        ( _rudder < 0. ) 
      && 
        ( ( _headingCmd - heading ) <= -_headingDeviation ) 
      ) 
    )
  {

    if (++_legsCompleted > _maxLegs) 
    {
      setHorizontal(DynamicControlIF::Rudder, 0.);
      setState(Finished);
      return;
    }

    _rudder *= -1.;

  }

  setHorizontal(DynamicControlIF::Rudder, _rudder);
  setSpeed(DynamicControlIF::Speed, _speed);


  //** Set vertical mode 
  double vertical = NotSpecified;

  switch (_verticalMode) {

  case DynamicControlIF::Depth:
       vertical = _depth;
       break;

  case DynamicControlIF::Pitch:
       vertical = _pitch;
       break;

  }

  if( vertical != NotSpecified )
       setVertical(_verticalMode, vertical);
}


Boolean Zigzag::validInput()
{
  Boolean valid = True;
  Boolean debug = True;

  switch (_verticalMode) {
    
  case DynamicControlIF::Depth:
       if (_depth == NotSpecified || _depth < 0.) {
	    printError("Invalid depth or not specified");
	    valid = False;
       }
       if (_pitchAttribute->valueHasBeenSet()) {
	    dprintf("Zigzag::validInput() - pitch=%.3f, NotSpecified=%.3f", 
		    _pitch, NotSpecified);
	    
	    printError("Pitch specified in depth mode?");
	    valid = False;
       }
       break;
       
  case DynamicControlIF::Pitch:
    //       if (_pitch == NotSpecified || _pitch < 0.) {
       if (_pitch == NotSpecified ) {
	    printError("Invalid pitch or not specified");
	    valid = False;
       }
       if (_depthAttribute->valueHasBeenSet()) {
	    printError("Depth specified in pitch mode?");
	    valid = False;
       }
       break;
       
  case DynamicControlIF::Elevator:
       printError("Elevator mode not implemented");
       valid = False;
       break;
       
  default:
       printError("Note!  Depth not set in Zigzag.  Is this correct?");
       valid = True;
  }

  if (_speed < 0.) {
    printError("Speed is less than zero");
    valid = False;
  }

  if (_maxLegs < 0) {
       printError("maxLegs is less than zero");
       valid = False;
  }

  if (_headingCmd <= 0.) {
       printError("The heading must be greater than zero.");
       valid = False;
  }

  if (_headingDeviation <= 0.) {
       printError("The heading deviation must be greater than zero.");
       valid = False;
  }

  if (_rudder <= 0.) {
       printError("The commanded rudder angle must be greater than zero.");
       valid = False;
  }

  return valid;
}