/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : SetControlGains.cc					    */
/* Author   : rsm, using FireTheGulper template.                            */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 18/9/2014                                                     */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include "MissionTimeAttribute.h"
#include "FloatAttribute.h"
#include "BooleanAttribute.h"
#include "SetControlGains.h"
#include "Syslog.h"

#define NotSpecified -100000.0

SetControlGains::SetControlGains()
   : Behavior(SetControlGainsBehaviorName, Sequential)
{
   Attribute::Input *input;

   attributes.add(new FloatAttribute("kwpHeading", "kwpHeading", 
				     &_gains.kwpHeading, NotSpecified));
   attributes.add(new FloatAttribute("kiwp", "kiwp", 
				     &_gains.kiwp, NotSpecified));
   attributes.add(new FloatAttribute("kpHeading", "kpHeading", 
				     &_gains.kpHeading, NotSpecified));
   attributes.add(new FloatAttribute("kiHeading", "kiHeading", 
				     &_gains.kiHeading, NotSpecified));
   attributes.add(new FloatAttribute("kdHeading", "kdHeading", 
				     &_gains.kdHeading, NotSpecified));
   attributes.add(new FloatAttribute("kpDepth", "kpDepth", 
				     &_gains.kpDepth, NotSpecified));
   attributes.add(new FloatAttribute("kiDepth", "kiDepth", 
				     &_gains.kiDepth, NotSpecified));
   attributes.add(new FloatAttribute("kdDepth", "kdDepth", 
				     &_gains.kdDepth, NotSpecified));
   attributes.add(new FloatAttribute("kpPitch", "kpPitch", 
				     &_gains.kpPitch, NotSpecified));
   attributes.add(new FloatAttribute("kiPitch", "kiPitch", 
				     &_gains.kiPitch, NotSpecified));
   attributes.add(new FloatAttribute("kdPitch", "kdPitch", 
				     &_gains.kdPitch, NotSpecified));
   attributes.add(new BooleanAttribute("revert", "revert", 
				       &_revert, False));
   try 
   {
      _dynamicControlIF = new DynamicControlIF("dynamicControl");
   } 
   catch(...)
   {
      Syslog::write("SetControlGains:: -- Failed to initialize connection "
		    "to DynamicControlIF\n");
   }
}


SetControlGains::~SetControlGains()
{
   delete _dynamicControlIF;
}

Boolean SetControlGains::validInput()
{
  return True;
}

Boolean SetControlGains::shouldBehaviorStart( )
{
     return bothSequence();
}

void SetControlGains::execute()
{
   //
   // Revert to the original or pass them through:
   //
   if( _revert ) _dynamicControlIF->revertGains();
   else          _dynamicControlIF->setGains(&_gains);

   Syslog::write("The SetControlGains Behavior has "
		 "executed at t = %-15.2f.\n",
		 _missionClock->seconds());
   setState(Finished);
   return;
}