/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : Setpoint.cc                                                   */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include "Setpoint.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)

Setpoint::Setpoint()
  : Behavior(SetpointBehaviorName, NonSequential)
{
  Boolean debug = False;

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

  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("Setpoint::Setpoint() - pitch=%.3f", _pitch);

  attributes.add(new IntegerAttribute("legDuration", 
				      "Duration of each leg",
				      &_legDuration, NullLegDuration));

  attributes.add(new IntegerAttribute("maxLegs", 
				      "Maximum number of legs",
				      &_maxLegs, 1));

  attributes.add(new AngleAttribute("headingInc", 
				    "Increment heading by this amount "
				    "after each leg",
				    &_headingIncrement, 
				    0.));

  dprintf("Setpoint::Setpoint() - #2: pitch=%.3f", _pitch);

  _legsCompleted = 0;
  _legStartTime = NotSpecified;
}


Setpoint::~Setpoint()
{
}


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

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

//     return value;

     return horizontalSequence();
}

void Setpoint::execute()
{

     Boolean debug = True;

     if( (state() == Active) && 
	 (_legStartTime == NotSpecified) ) {
	  
	  // First starting...
	  _legStartTime = _missionClock->seconds();
     }
     
     if( ( _legDuration != NullLegDuration) && 
	 (_missionClock->seconds() - _legStartTime >= _legDuration) ) {
	  
//	  dprintf("Setpoint::execute - New leg, time = %lf, startTime = %lf, duration = %ld\n", _missionClock->seconds(), _legStartTime, _legDuration );

	  if (++_legsCompleted > _maxLegs) {
	       setState(Finished);
	       return;
	  }
	  
	  // Turn by specified increment and start next leg
	  _heading += _headingIncrement;
	  Math::zeroToTwoPi(&_heading);
	  _legStartTime = _missionClock->seconds();
     }

//     dprintf("Setpoint.cc -- Setting heading = %lf, legs complete = %d\n",
//	     _heading, _legsCompleted );

     setHorizontal(DynamicControlIF::Heading, _heading);
     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 Setpoint::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("Setpoint::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 setpoint.  Is this correct?");
       valid = True;
  }

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

  if( (_maxLegs > 1) && (_legDuration <= 0.0) ) {
       printError("maxLegs is greater than 1, but legDuration is "
		  "less than zero");
       valid = False;
  }

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

  return valid;
}