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

RotateCarousel::RotateCarousel()
  : Behavior(RotateCarouselBehaviorName, NonSequential)
{
  Boolean debug = False;

  _buoyLauncher = new BuoyLauncherIF("buoyLauncher");

  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);


  attributes.add(new IntegerAttribute("statusCalls", 
				      "Turn status calls on?",
				      &_statusCalls, NullLegDuration));

  _initiateRotateDelayAttr = new FloatAttribute("initiateRotateDelay",
				       "Rotatation initiation delay is",
				        &_initiateRotateDelay, 
				        NotSpecified);

  attributes.add(_initiateRotateDelayAttr);

  _rotateDelayAttr = new FloatAttribute("rotateDelay",
				       "Delay until rotatation is finished ",
				       &_rotateDelay, 
				       NotSpecified);

  attributes.add(_rotateDelayAttr);


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

  attributes.add(_pitchAttribute);

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

  _legsCompleted = 0;
  _legStartTime = NotSpecified;
  //
  // Initialize rotate stuff:
  //
  _initiate = False;
  _statusSet = False;

}


RotateCarousel::~RotateCarousel()
{
}


Boolean RotateCarousel::shouldBehaviorStart( void ) {

     return horizontalSequence();
}

void RotateCarousel::execute()
{
     Boolean debug = True;

     BuoyLauncherIF::LC_status l_s;

     if (!_statusSet) {
       if (_buoyLauncher->statusOn((Boolean)(_statusCalls != 0) == DeviceIF::Ok))
	 _statusSet = True;
       else
	 _statusSet = False;
     }

     if( (state() == Active) && 
	 (_legStartTime == NotSpecified) ) {
	  
	  // First starting...
	  _legStartTime = _missionClock->seconds();

     }
     
     if( ( _legDuration != NullLegDuration) && 
	 (_missionClock->seconds() - _legStartTime >= _legDuration) ) {
	  
//	  dprintf("RotateCarousel::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("RotateCarousel.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);
     //
     // Rotate the buoy when it's time.
     //
     if( _missionClock->seconds() - _legStartTime >= _initiateRotateDelay)
     {
       //
       // Put check here to determine if the launcher is alive.
       //
       // _buoyLauncher->taskIF() == True
       //
       if( !_initiate ) 
       {
	 Syslog::write(" RotateCarousel::Initiating Buoy rotate.\n");
	 if (_buoyLauncher->rotateCarousel() == DeviceIF::Ok) {
	 _initiate = True;
	 } else {
	 _initiate = False;
	 }
       } else if( _missionClock->seconds() - _legStartTime >= _rotateDelay) {
	 Syslog::write(" RotateCarousel::Initiating Buoy rotate.\n");
	 setState(Finished);
       }
     }
     
}


Boolean RotateCarousel::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("RotateCarousel::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 RotateCarousel.  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;
  }

  if( _rotateDelay > _legDuration )
  {
    printError(" The Rotate Delay must be less than the duration.\n");
    valid = False;
  }

  return valid;
}