/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : CurrentBuoy.cc                                                */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 10/17/2001                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include "CurrentBuoy.h"
#include "FloatAttribute.h"
#include "AngleAttribute.h"
#include "IntegerAttribute.h"
#include "VerticalModeAttribute.h"
#include "MissionTimeAttribute.h"
#include "MathP.h"
#include "Syslog.h"
#include "BuoyLauncherIF.h"

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

CurrentBuoy::CurrentBuoy()
  : Behavior(CurrentBuoyBehaviorName, NonSequential)
{
  Boolean debug = False;

  try {
    _buoyLauncher = new BuoyLauncherIF("buoyLauncher", 1000);
  }
  catch(Exception e)
  {
    Syslog::write("CurrentBuoy::Cannot create IF to buoyLauncher - %s", e.msg);
    perror("CurrentBuoy::Cannot create IF to buoyLauncher");
  }

  attributes.add(new IntegerAttribute("buoyNumber", "Buoy Number",
				    &_buoyNumber));

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

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

  _legsCompleted = 0;
  _legStartTime = NotSpecified;
  _commandSent = False;
  _waitMessage = True;
}


CurrentBuoy::~CurrentBuoy()
{
   delete (_buoyLauncher);
}


Boolean CurrentBuoy::shouldBehaviorStart( void ) {

     return horizontalSequence();
}

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

     if (_buoyLauncher == NULL)
     {
       Syslog::write(" CurrentBuoy::No IF to buoyLauncher server - nothing to do");
       setState(Finished);
       return;
     }

     BuoyLauncherIF::LC_status l_s;
     DeviceIF::Status status;

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

     }
     
     if( ( _legDuration != NullLegDuration) && 
	 (_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();
     }

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

     //
     // Check to determine if the launcher is alive and well.
     // If it is, send the currentBuoy command.
     //
     if( !_commandSent && _buoyLauncher->ready()) 
     {
       Syslog::write(" CurrentBuoy::Initiating Current Buoy command");
       if ((status = _buoyLauncher->currentBuoy((short)_buoyNumber)) != DeviceIF::Ok)
       {
         Syslog::write(" CurrentBuoy::Error Initiating Current Buoy command");
         setState(Finished);
       }
       else
         _commandSent = True;
     }
     else if(_commandSent)
     {
       // Check to see if the buoy is in position
       //
       _buoyLauncher->getLauncherStatus(&l_s);
       if (_buoyLauncher->ready() && l_s.currentBuoyNumber == (short)_buoyNumber)
       {
         Syslog::write("CurrentBuoy::Buoy %d in data position!",
                       _buoyNumber);   
         setState(Finished);
       }
       else
       {
         int missionTime = (int)(_missionClock->seconds());
         if (missionTime % 5 == 0)
         {
           if (_waitMessage)
             Syslog::write("CurrentBuoy::Waiting for buoy %d in data position",
                           _buoyNumber);
           _waitMessage = False;
         }
         else
           _waitMessage = True;
       }
     }
}

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