/*
Adept MobileRobots Robotics Interface for Applications (ARIA)
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
Copyright (C) 2006, 2007, 2008, 2009, 2010 MobileRobots Inc.
Copyright (C) 2011, 2012, 2013 Adept Technology

     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation; either version 2 of the License, or
     (at your option) any later version.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program; if not, write to the Free Software
     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

If you wish to redistribute ARIA under different terms, contact 
Adept MobileRobots for information about a commercial version of ARIA at 
robots@mobilerobots.com or 
Adept MobileRobots, 10 Columbia Drive, Amherst, NH 03031; +1-603-881-7960
*/
#ifndef ARACTIONDESIRED_H
#define ARACTIONDESIRED_H

#include "ariaTypedefs.h"
#include "ariaUtil.h"

/// Class used by ArActionDesired for each channel, internal
class ArActionDesiredChannel
{
public:
  AREXPORT static const double NO_STRENGTH;
  AREXPORT static const double MIN_STRENGTH;
  AREXPORT static const double MAX_STRENGTH;
  
  ArActionDesiredChannel() { reset(); myOverrideDoesLessThan = true; }
  ~ArActionDesiredChannel() {}
  void setOverrideDoesLessThan(bool overrideDoesLessThan) 
    { myOverrideDoesLessThan = overrideDoesLessThan; }
  void setDesired(double desired, double desiredStrength, 
		  bool allowOverride = false) 
    {
      myDesired = desired;
      myStrength = desiredStrength; 
      myAllowOverride = allowOverride;
      if (myStrength > MAX_STRENGTH)
	myStrength = MAX_STRENGTH;
      if (myStrength < MIN_STRENGTH)
	myStrength = NO_STRENGTH;
    }
  double getDesired(void) const { return myDesired; }
  double getStrength(void) const { return myStrength; }
  double getAllowOverride(void) const { return myAllowOverride; }
  void reset(void) 
    { myDesired = 0; myStrength = NO_STRENGTH; myAllowOverride = true; }
  void merge(ArActionDesiredChannel *desiredChannel)
    {
      double otherStrength = desiredChannel->getStrength();
      double oldStrength = myStrength;
      if (myStrength + otherStrength > MAX_STRENGTH)
	otherStrength = MAX_STRENGTH - myStrength;
      myStrength = myStrength + otherStrength;
      myAllowOverride = myAllowOverride && desiredChannel->getAllowOverride();
      // if we're allowing override just set myDesired to the least
      // (or greatest) value
      if (myAllowOverride && myStrength >= MIN_STRENGTH)
      {
	// if both have strength get the min/max
	if (oldStrength >= MIN_STRENGTH && 
	    desiredChannel->getStrength() >= MIN_STRENGTH)
	{
	  if (myOverrideDoesLessThan)
	    myDesired = ArUtil::findMin(myDesired, 
					desiredChannel->getDesired());
	  else if (!myOverrideDoesLessThan)
	    myDesired = ArUtil::findMax(myDesired, 
					desiredChannel->getDesired());
	}
	// if only it has strength use it
	else if (desiredChannel->getStrength() >= MIN_STRENGTH)
	{
	  myDesired = desiredChannel->getDesired();
	}
	// if only this has strength then we don't need to do anything
      }
      else if (myStrength >= MIN_STRENGTH)
	myDesired = (((oldStrength * myDesired) + 
		      (desiredChannel->getDesired() * otherStrength)) 
		     / (myStrength));
    }
  void startAverage(void)
    {
      myDesiredTotal = myDesired * myStrength;
      myStrengthTotal = myStrength;
    }
  void addAverage(ArActionDesiredChannel *desiredChannel)
    {
      myAllowOverride = myAllowOverride && desiredChannel->getAllowOverride();
      // if we're allowing override then myDesired is just the least
      // of the values thats going to come through... still compute
      // the old way in case something doesn't want to override it
      if (myAllowOverride)
      {
	// if both have strength get the min/max
	if (myStrength >= MIN_STRENGTH && 
	    desiredChannel->getStrength() >= MIN_STRENGTH)
	{
	  if (myOverrideDoesLessThan)
	    myDesired = ArUtil::findMin(myDesired, 
					desiredChannel->getDesired());
	  else if (!myOverrideDoesLessThan)
	    myDesired = ArUtil::findMax(myDesired, 
					desiredChannel->getDesired());
	}
	// if only it has strength use it
	else if (desiredChannel->getStrength() >= MIN_STRENGTH)
	{
	  myDesired = desiredChannel->getDesired();
	}
	// if only this has strength then we don't need to do anything
      }
      myDesiredTotal += (desiredChannel->getDesired() * 
			 desiredChannel->getStrength());
      myStrengthTotal += desiredChannel->getStrength();
    }
  void endAverage(void)
    {
      if (myStrengthTotal < MIN_STRENGTH)
      {
	myStrength = NO_STRENGTH;
	return;
      }
      // if we're overriding we just use what myDesired already is
      if (!myAllowOverride)
	myDesired = (myDesiredTotal / myStrengthTotal);
      myStrength = myStrengthTotal;
      if (myStrength > MAX_STRENGTH)
	myStrength = MAX_STRENGTH;
    }
  /// do some bounds checking
  void checkLowerBound(const char *actionName, const char *typeName, 
				int lowerBound)
    {
      // if it has no strength, just return
      if (myStrength < MIN_STRENGTH)
	return;
      
      if (ArMath::roundInt(myDesired) < lowerBound)
      {
	ArLog::log(ArLog::Terse, 
		   "ActionSanityChecking: '%s' tried to set %s to %g (which wound wind up less than %d and will be set to %d)",
		   actionName, typeName, myDesired, lowerBound, lowerBound);
	myDesired = lowerBound;
	return;
      }
    }
  /// do some bounds checking
  void checkUpperBound(const char *actionName, const char *typeName, 
				int upperBound)
    {
      // if it has no strength, just return
      if (myStrength < MIN_STRENGTH)
	return;
      
      if (ArMath::roundInt(myDesired) > upperBound)
      {
	ArLog::log(ArLog::Terse, 
		   "ActionSanityChecking: '%s' tried to set %s to %g (which would wind up greater than %d and will be set to %d)",
		   actionName, typeName, myDesired, upperBound, upperBound);
	myDesired = upperBound;
	return;
      }
    }


protected:
  double myDesired;
  double myStrength;
  bool myAllowOverride;
  double myDesiredTotal;
  double myStrengthTotal;
  bool myOverrideDoesLessThan;
};

/// Contains values returned by ArAction objects expressing desired motion commands to resolver
/**
   This class is use by actions to report what want movement commands they want. 
   The action resolver combines the ArActionDesired objects returned by different actions.

   A brief summary follows. For a fuller explanation of actions, see @ref actions.

   Different values are organized into different "channels". 
   Translational (front/back) and rotational (right/left) movements are separate 
   channels.  Translational movement uses velocity, while rotational movement uses 
   change in heading from current heading. 
   Each channel has a strength value.  
   Both translational and rotational movement have maximum velocities as well,
   that also have their own strengths.

   The strength value reflects how strongly an action wants to do the chosen 
   movement command, the resolver (ArResolver) will combine these strengths 
   and figure out what to do based on them.

   For all strength values there is a total of 1.0 combined strength avaliable.
   The range for strength is from 0 to 1.  This is simply a convention that 
   ARIA uses by default, if you don't like it, you could override this
   class the ArResolver class.

   Note that for the different maximum/accel/decel values they take an
   additional argument of whether just to use the slowest speed,
   slowest accel, or fastest decel.  By default these will just use
   safer values (slowest speed, slowest accel, fastest decel)... you
   can specify false on these for the old behavior.  Note that if
   you're safest values then the strength is largely ignored though it
   is still tracked and must still be greater than MIN_STRENGTH to
   work and it is still capped at MAX_STRENGTH).

   @sa @ref actions

*/
class ArActionDesired
{
public:
  AREXPORT static const double NO_STRENGTH;
  AREXPORT static const double MIN_STRENGTH;
  AREXPORT static const double MAX_STRENGTH;
  /// Constructor
  ArActionDesired() 
    { 
      myHeadingSet = false; 
      myTransDecelDes.setOverrideDoesLessThan(false); 
      myRotDecelDes.setOverrideDoesLessThan(false);
      myMaxNegVelDes.setOverrideDoesLessThan(false); 
    }
  /// Destructor
  virtual ~ArActionDesired() {}
  
  /// Sets the velocity (mm/sec) and strength
  /**
     @param vel desired vel (mm/sec)
     @param strength strength given to this, defaults to MAX_STRENGTH (1.0)
  */
  virtual void setVel(double vel, double strength = MAX_STRENGTH)
    { myVelDes.setDesired(vel, strength); }
  /// Sets the delta heading (deg) and strength
  /**
     If there's already a rotVel set this WILL NOT work.
     @param deltaHeading desired change in heading (deg)
     @param strength strength given to this, defaults to MAX_STRENGTH (1.0)
  */
  virtual void setDeltaHeading(double deltaHeading, 
			       double strength = MAX_STRENGTH)
    { myDeltaHeadingDes.setDesired(deltaHeading, strength); }
  /// Sets the absolute heading (deg) 
  /**
     If there's already a rotVel set this WILL NOT work.
     This is a way to set the heading instead of using a delta, there is no
     get for this, because accountForRobotHeading MUST be called (this should
     be called by all resolvers, but if you want to call it you can,
     thats fine).
     @param heading desired heading (deg)
     @param strength strength given to this, defaults to MAX_STRENGTH (1.0)
  */
  virtual void setHeading(double heading, double strength = MAX_STRENGTH)
    { myHeading = heading; myHeadingStrength = strength; myHeadingSet = true; }

  /// Sets the rotational velocity
  /**
     If there's already a delta heading or heading this WILL NOT work.
     @param rotVel desired rotational velocity (deg/sec)
     @param strength strength given to this, defaults to MAX_STRENGTH (1.0)
  **/
  virtual void setRotVel(double rotVel, double strength = MAX_STRENGTH)
    { myRotVelDes.setDesired(rotVel, strength); }

  /// Sets the maximum velocity (+mm/sec) and strength
  /**
     This sets the maximum positive velocity for this cycle.  Check
     the ArRobot class notes for more details.
     
     @param maxVel desired maximum velocity (+mm/sec)
     @param strength strength given to this, defaults to MAX_STRENGTH
     (1.0)
     @param useSlowest if this is true (the default) everywhere
     then the slowest maximum vel is what will be selected
  **/
  virtual void setMaxVel(double maxVel, double strength = MAX_STRENGTH,
			 bool useSlowest = true)
    { myMaxVelDes.setDesired(maxVel, strength, useSlowest); }
  /// Sets the maximum velocity for going backwards (-mm/sec) and strength
  /**
     This sets the maximum negative velocity for this cycle.  Check
     the ArRobot class notes for more details.

     @param maxVel desired maximum velocity for going backwards (-mm/sec)
     @param strength strength given to this, defaults to MAX_STRENGTH (1.0)
     @param useSlowest if this is true (the default) everywhere
     then the slowest max neg vel is what will be selected
  **/
  virtual void setMaxNegVel(double maxVel, double strength = MAX_STRENGTH,
			    bool useSlowest = true)
    { myMaxNegVelDes.setDesired(maxVel, strength, useSlowest); }

  /// Sets the translation acceleration (deg/sec/sec) and strength
  /**
     This sets the translation acceleration for this cycle (this is
     sent down to the robot).  Check the ArRobot class notes for more
     details.

     @param transAccel desired translation acceleration (deg/sec/sec)
     @param strength strength given to this, defaults to MAX_STRENGTH (1.0) 
     @param useSlowest if this is true (the default) everywhere then
     the slowest accel is what will be selected
  **/
  virtual void setTransAccel(double transAccel, 
			     double strength = MAX_STRENGTH,
			     bool useSlowest = true)
    { myTransAccelDes.setDesired(transAccel, strength, useSlowest);  }

  /// Sets the translation deceleration (deg/sec/sec) and strength
  /**
     This sets the translation deceleration for this cycle (this is
     sent down to the robot).  Check the ArRobot class notes for more
     details.

     @param transDecel desired translation deceleration (deg/sec/sec)
     @param strength strength given to this, defaults to MAX_STRENGTH (1.0) 
     @param useFastestDecel if this is true (the default) everywhere
     then the fastest decel is what will be selected
  **/
  virtual void setTransDecel(double transDecel, double strength = MAX_STRENGTH,
			     bool useFastestDecel = true)
    { myTransDecelDes.setDesired(transDecel, strength, useFastestDecel);  }

  /// Sets the maximum rotational velocity (deg/sec) and strength
  /**
     This sets the maximum rotational velocity for this cycle (this is
     sent down to the robot).  Check the ArRobot class notes for more
     details.

     @param maxVel desired maximum rotational velocity (deg/sec)
     @param strength strength given to this, defaults to MAX_STRENGTH (1.0) 
     @param useSlowest if this is true (the default) everywhere
     then the slowest rot vel is what will be selected
  **/
  virtual void setMaxRotVel(double maxVel, double strength = MAX_STRENGTH,
			    bool useSlowest = true)
    { myMaxRotVelDes.setDesired(maxVel, strength, useSlowest); }

  /// Sets the maximum rotational velocity (deg/sec) in the positive direction and strength
  /**
     This sets the maximum rotational velocity for this cycle (this is
     sent down to the robot) in the positive direction.  If the
     setMaxRotVel is set to less than this that will be used instead.
     Check the ArRobot class notes for more details.

     @param maxVel desired maximum rotational velocity in the positive
     direction (deg/sec)

     @param strength strength given to this, defaults to MAX_STRENGTH (1.0) 

     @param useSlowest if this is true (the default) everywhere
     then the slowest rot vel is what will be selected
  **/
  virtual void setMaxRotVelPos(double maxVel, double strength = MAX_STRENGTH,
			       bool useSlowest = true)
    { myMaxRotVelPosDes.setDesired(maxVel, strength, useSlowest); }

  /// Sets the maximum rotational velocity (deg/sec) in the negative direction and strength
  /**
     This sets the maximum rotational velocity for this cycle (this is
     sent down to the robot) in the negative direction.  If the
     setMaxRotVel is set to less than this that will be used instead.
     Check the ArRobot class notes for more details.

     @param maxVel desired maximum rotational velocity in the negative
     direction (deg/sec)

     @param strength strength given to this, defaults to MAX_STRENGTH (1.0) 

     @param useSlowest if this is true (the default) everywhere
     then the slowest rot vel is what will be selected
  **/
  virtual void setMaxRotVelNeg(double maxVel, double strength = MAX_STRENGTH,
			       bool useSlowest = true)
    { myMaxRotVelNegDes.setDesired(maxVel, strength, useSlowest); }

  /// Sets the rotational acceleration (deg/sec/sec) and strength
  /**
     This sets the rotational acceleration for this cycle (this is
     sent down to the robot).  Check the ArRobot class notes for more
     details.

     @param rotAccel desired rotational acceleration (deg/sec/sec)
     @param strength strength given to this, defaults to MAX_STRENGTH (1.0) 
     @param useSlowest if this is true (the default) everywhere
     then the slowest rot accel is what will be selected
  **/
  virtual void setRotAccel(double rotAccel, double strength = MAX_STRENGTH,
			   bool useSlowest = true)
    { myRotAccelDes.setDesired(rotAccel, strength, useSlowest);  }

  /// Sets the rotational deceleration (deg/sec/sec) and strength
  /**
     This sets the rotational deceleration for this cycle (this is
     sent down to the robot).  Check the ArRobot class notes for more
     details.

     @param rotDecel desired rotational deceleration (deg/sec/sec)
     @param strength strength given to this, defaults to MAX_STRENGTH (1.0) 
     @param useFastest if this is true (the default) everywhere
     then the fastest rot decel is what will be selected
  **/
  virtual void setRotDecel(double rotDecel, double strength = MAX_STRENGTH,
			   bool useFastest = true)
    { myRotDecelDes.setDesired(rotDecel, strength, useFastest);  }

  /// Sets the left lateral velocity (mm/sec) and strength
  /**
     Note that there is only one actual velocity for lat vel, but
     instead of making people remember which way is left and right
     there are two functions, setLeftLatVel and setRightLatVel... all
     setRightLatVel does is flip the direction on the vel.  You can
     set a negative left lat vel and thats the same as setting a
     positive right vel.  You can do the same with setting a negative
     right vel to get a positive left vel.

     @param latVel desired vel (mm/sec)
     @param strength strength given to this, defaults to MAX_STRENGTH (1.0)
  */
  virtual void setLeftLatVel(double latVel, double strength = MAX_STRENGTH)
    { myLatVelDes.setDesired(latVel, strength); }
  /// Sets the right lateral velocity (mm/sec) and strength
  /**
     Note that there is only one actual velocity for lat vel, but
     instead of making people remember which way is left and right
     there are two functions, setLeftLatVel and setRightLatVel... all
     setRightLatVel does is flip the direction on the vel.  You can
     set a negative left lat vel and thats the same as setting a
     positive right vel.  You can do the same with setting a negative
     right vel to get a positive left vel.

     @param latVel desired vel (mm/sec)
     @param strength strength given to this, defaults to MAX_STRENGTH (1.0)
  */
  virtual void setRightLatVel(double latVel, double strength = MAX_STRENGTH)
    { myLatVelDes.setDesired(-latVel, strength); }
  /// Sets the maximum lateral velocity (deg/sec) and strength
  /**
     This sets the maximum lateral velocity for this cycle.  Check
     the ArRobot class notes for more details.

     @param maxVel desired maximum lateral velocity (deg/sec)
     @param strength strength given to this, defaults to MAX_STRENGTH (1.0) 
     @param useSlowest if this is true (the default) everywhere
     then the slowest lat vel is what will be selected
  **/
  virtual void setMaxLeftLatVel(double maxVel, double strength = MAX_STRENGTH,
			    bool useSlowest = true)
    { myMaxLeftLatVelDes.setDesired(maxVel, strength, useSlowest); }
  /// Sets the maximum lateral velocity (deg/sec) and strength
  /**
     This sets the maximum lateral velocity for this cycle.  Check
     the ArRobot class notes for more details.

     @param maxVel desired maximum lateral velocity (deg/sec)
     @param strength strength given to this, defaults to MAX_STRENGTH (1.0) 
     @param useSlowest if this is true (the default) everywhere
     then the slowest lat vel is what will be selected
  **/
  virtual void setMaxRightLatVel(double maxVel, double strength = MAX_STRENGTH,
			    bool useSlowest = true)
    { myMaxRightLatVelDes.setDesired(maxVel, strength, useSlowest); }

  /// Sets the lateral acceleration (deg/sec/sec) and strength
  /**
     This sets the lateral acceleration for this cycle (this is
     sent down to the robot).  Check the ArRobot class notes for more
     details.

     @param latAccel desired lateral acceleration (deg/sec/sec)
     @param strength strength given to this, defaults to MAX_STRENGTH (1.0) 
     @param useSlowest if this is true (the default) everywhere
     then the slowest lat accel is what will be selected
  **/
  virtual void setLatAccel(double latAccel, double strength = MAX_STRENGTH,
			   bool useSlowest = true)
    { myLatAccelDes.setDesired(latAccel, strength, useSlowest);  }

  /// Sets the lateral deceleration (deg/sec/sec) and strength
  /**
     This sets the lateral deceleration for this cycle (this is
     sent down to the robot).  Check the ArRobot class notes for more
     details.

     @param latDecel desired lateral deceleration (deg/sec/sec)
     @param strength strength given to this, defaults to MAX_STRENGTH (1.0) 
     @param useFastest if this is true (the default) everywhere
     then the fastest lat decel is what will be selected
  **/
  virtual void setLatDecel(double latDecel, double strength = MAX_STRENGTH,
			   bool useFastest = true)
    { myLatDecelDes.setDesired(latDecel, strength, useFastest);  }

  /// Resets the strengths to 0
  virtual void reset(void) 
    {
      myVelDes.reset(); 
      myMaxVelDes.reset(); myMaxNegVelDes.reset(); 
      myTransAccelDes.reset(); myTransDecelDes.reset();

      myRotVelDes.reset(); myDeltaHeadingDes.reset(); 
      myMaxRotVelDes.reset(); 
      myMaxRotVelPosDes.reset(); myMaxRotVelNegDes.reset(); 
      myRotAccelDes.reset(); myRotDecelDes.reset();
      myHeadingStrength = 0;
      myHeadingSet = false;

      myLatVelDes.reset();
      myMaxLeftLatVelDes.reset();  myMaxRightLatVelDes.reset(); 
      myLatAccelDes.reset(); myLatDecelDes.reset();
    }

  /// Gets the translational velocity desired (mm/sec)
  virtual double getVel(void) const
    { return myVelDes.getDesired(); }
  /// Gets the strength of the translational velocity desired
  virtual double getVelStrength(void) const
    { return myVelDes.getStrength(); }
  /// Gets the heading desired (deg)
  virtual double getHeading(void) const
    { return myHeading; }
  /// Gets the strength of the heading desired
  virtual double getHeadingStrength(void) const
    { return myHeadingStrength; }
  /// Gets the delta heading desired (deg)
  virtual double getDeltaHeading(void) const
    { return myDeltaHeadingDes.getDesired(); }
  /// Gets the strength of the delta heading desired
  virtual double getDeltaHeadingStrength(void) const
    { return myDeltaHeadingDes.getStrength(); }
  /// Gets the rot vel that was set
  virtual double getRotVel(void) const { return myRotVelDes.getDesired(); }
  /// Gets the rot vel des (deg/sec)
  virtual double getRotVelStrength(void) const 
    { return myRotVelDes.getStrength(); }

  /// Gets the desired maximum velocity (mm/sec)
  virtual double getMaxVel(void) const
    { return myMaxVelDes.getDesired(); }
  /// Gets the maximum velocity strength
  virtual double getMaxVelStrength(void) const
    { return myMaxVelDes.getStrength(); }
  /// Gets whether the slowest is being used or not
  virtual double getMaxVelSlowestUsed(void) const
    { return myMaxVelDes.getAllowOverride(); }
  /// Gets the desired maximum negative velocity (-mm/sec)
  virtual double getMaxNegVel(void) const
    { return myMaxNegVelDes.getDesired(); }
  /// Gets the desired maximum negative velocity strength
  virtual double getMaxNegVelStrength(void) const
    { return myMaxNegVelDes.getStrength(); }
  /// Gets whether the slowest is being used or not
  virtual double getMaxNegVelSlowestUsed(void) const
    { return myMaxNegVelDes.getAllowOverride(); }
  /// Gets the desired trans acceleration (mm/sec)
  virtual double getTransAccel(void) const
    { return myTransAccelDes.getDesired(); }
  /// Gets the desired trans acceleration strength
  virtual double getTransAccelStrength(void) const
    { return myTransAccelDes.getStrength(); }
  /// Gets whether the slowest accel is being used or not
  virtual double getTransAccelSlowestUsed(void) const
    { return myTransAccelDes.getAllowOverride(); }
  /// Gets the desired trans deceleration (-mm/sec/sec)
  virtual double getTransDecel(void) const
    { return myTransDecelDes.getDesired(); }
  /// Gets the desired trans deceleration strength
  virtual double getTransDecelStrength(void) const
    { return myTransDecelDes.getStrength(); }
  /// Gets whether the fastest decel is being used or not
  virtual double getTransDecelFastestUsed(void) const
    { return myTransDecelDes.getAllowOverride(); }

  /// Gets the maximum rotational velocity
  virtual double getMaxRotVel(void) const
    { return myMaxRotVelDes.getDesired(); }
  /// Gets the maximum rotational velocity strength
  virtual double getMaxRotVelStrength(void) const
    { return myMaxRotVelDes.getStrength(); }
  /// Gets whether the slowest rot vel is being used or not
  virtual double getMaxRotVelSlowestUsed(void) const
    { return myMaxRotVelDes.getAllowOverride(); }

  /// Gets the maximum rotational velocity in the positive direction
  virtual double getMaxRotVelPos(void) const
    { return myMaxRotVelPosDes.getDesired(); }
  /// Gets the maximum rotational velocity in the positive direction strength
  virtual double getMaxRotVelPosStrength(void) const
    { return myMaxRotVelPosDes.getStrength(); }
  /// Gets whether the slowest rot vel in the positive direction is being used or not
  virtual double getMaxRotVelPosSlowestUsed(void) const
    { return myMaxRotVelPosDes.getAllowOverride(); }

  /// Gets the maximum rotational velocity in the negative direction
  virtual double getMaxRotVelNeg(void) const
    { return myMaxRotVelNegDes.getDesired(); }
  /// Gets the maximum rotational velocity in the negative direction strength
  virtual double getMaxRotVelNegStrength(void) const
    { return myMaxRotVelNegDes.getStrength(); }
  /// Gets whether the slowest rot vel in the negative direction is being used or not
  virtual double getMaxRotVelNegSlowestUsed(void) const
    { return myMaxRotVelNegDes.getAllowOverride(); }

  /// Gets the desired rotational acceleration (mm/sec)
  virtual double getRotAccel(void) const
    { return myRotAccelDes.getDesired(); }
  /// Gets the desired rotational acceleration strength
  virtual double getRotAccelStrength(void) const
    { return myRotAccelDes.getStrength(); }
  /// Gets whether the slowest rot accel is being used or not
  virtual double getRotAccelSlowestUsed(void) const
    { return myRotAccelDes.getAllowOverride(); }
  /// Gets the desired rotational deceleration (-mm/sec/sec)
  virtual double getRotDecel(void) const
    { return myRotDecelDes.getDesired(); }
  /// Gets the desired rotational deceleration strength
  virtual double getRotDecelStrength(void) const
    { return myRotDecelDes.getStrength(); }
  /// Gets whether the fastest rot decel is being used or not
  virtual double getRotDecelFastestUsed(void) const
    { return myRotDecelDes.getAllowOverride(); }

  /// Gets the lat vel that was set
  virtual double getLatVel(void) const { return myLatVelDes.getDesired(); }
  /// Gets the lat vel des (deg/sec)
  virtual double getLatVelStrength(void) const 
    { return myLatVelDes.getStrength(); }
  /// Gets the maximum lateral velocity
  virtual double getMaxLeftLatVel(void) const
    { return myMaxLeftLatVelDes.getDesired(); }
  /// Gets the maximum lateral velocity strength
  virtual double getMaxLeftLatVelStrength(void) const
    { return myMaxLeftLatVelDes.getStrength(); }
  /// Gets whether the slowest lat vel is being used or not
  virtual double getMaxLeftLatVelSlowestUsed(void) const
    { return myMaxLeftLatVelDes.getAllowOverride(); }
  /// Gets the maximum lateral velocity
  virtual double getMaxRightLatVel(void) const
    { return myMaxRightLatVelDes.getDesired(); }
  /// Gets the maximum lateral velocity strength
  virtual double getMaxRightLatVelStrength(void) const
    { return myMaxRightLatVelDes.getStrength(); }
  /// Gets whether the slowest lat vel is being used or not
  virtual double getMaxRightLatVelSlowestUsed(void) const
    { return myMaxRightLatVelDes.getAllowOverride(); }
  /// Gets the desired lateral acceleration (mm/sec)
  virtual double getLatAccel(void) const
    { return myLatAccelDes.getDesired(); }
  /// Gets the desired lateral acceleration strength
  virtual double getLatAccelStrength(void) const
    { return myLatAccelDes.getStrength(); }
  /// Gets whether the slowest lat accel is being used or not
  virtual double getLatAccelSlowestUsed(void) const
    { return myLatAccelDes.getAllowOverride(); }
  /// Gets the desired lateral deceleration (-mm/sec/sec)
  virtual double getLatDecel(void) const
    { return myLatDecelDes.getDesired(); }
  /// Gets the desired lateral deceleration strength
  virtual double getLatDecelStrength(void) const
    { return myLatDecelDes.getStrength(); }
  /// Gets whether the fastest lat decel is being used or not
  virtual double getLatDecelFastestUsed(void) const
    { return myLatDecelDes.getAllowOverride(); }


  /// Merges the given ArActionDesired into this one (this one has precedence),
  /// internal
  /** 
      This merges in the two different action values, accountForRobotHeading
      MUST be done before this is called (on both actions), since this merges
      their delta headings, and the deltas can't be known unless the account
      for angle is done.
      @param actDesired the actionDesired to merge with this one
  */
  virtual void merge(ArActionDesired *actDesired)
    {
      if (actDesired == NULL)
	return;
      myVelDes.merge(&actDesired->myVelDes);
      // if we're already using rot or delt use that, otherwise use what it wants
      if (myDeltaHeadingDes.getStrength() > NO_STRENGTH)
      {
	myDeltaHeadingDes.merge(&actDesired->myDeltaHeadingDes);
      }
      else if (myRotVelDes.getStrength() > NO_STRENGTH)
      {
	myRotVelDes.merge(&actDesired->myRotVelDes);
      }
      else
      {
	myDeltaHeadingDes.merge(&actDesired->myDeltaHeadingDes);
	myRotVelDes.merge(&actDesired->myRotVelDes);
      }
      myMaxVelDes.merge(&actDesired->myMaxVelDes);
      myMaxNegVelDes.merge(&actDesired->myMaxNegVelDes);
      myMaxRotVelDes.merge(&actDesired->myMaxRotVelDes);
      myMaxRotVelPosDes.merge(&actDesired->myMaxRotVelPosDes);
      myMaxRotVelNegDes.merge(&actDesired->myMaxRotVelNegDes);
      myTransAccelDes.merge(&actDesired->myTransAccelDes);
      myTransDecelDes.merge(&actDesired->myTransDecelDes);
      myRotAccelDes.merge(&actDesired->myRotAccelDes);
      myRotDecelDes.merge(&actDesired->myRotDecelDes);

      myLatVelDes.merge(&actDesired->myLatVelDes);
      myMaxLeftLatVelDes.merge(&actDesired->myMaxLeftLatVelDes);
      myMaxRightLatVelDes.merge(&actDesired->myMaxRightLatVelDes);
      myLatAccelDes.merge(&actDesired->myLatAccelDes);
      myLatDecelDes.merge(&actDesired->myLatDecelDes);
    }
  /// Starts the process of avereraging together different desireds
  /**
     There is a three step process for averaging actionDesireds together,
     first startAverage must be done to set up the process, then addAverage
     must be done with each average that is desired, then finally endAverage
     should be used, after that is done then the normal process of getting
     the results out should be done.
  */
  virtual void startAverage(void)
    {
      myVelDes.startAverage();
      myMaxVelDes.startAverage();
      myMaxNegVelDes.startAverage();
      myTransAccelDes.startAverage();
      myTransDecelDes.startAverage();


      myRotVelDes.startAverage();
      myDeltaHeadingDes.startAverage();
      myMaxRotVelDes.startAverage();
      myMaxRotVelPosDes.startAverage();
      myMaxRotVelNegDes.startAverage();
      myRotAccelDes.startAverage();
      myRotDecelDes.startAverage();

      myLatVelDes.startAverage();
      myMaxLeftLatVelDes.startAverage();
      myMaxRightLatVelDes.startAverage();
      myLatAccelDes.startAverage();
      myLatDecelDes.startAverage();
    }
  /// Adds another actionDesired into the mix to average
  /**
     For a description of how to use this, see startAverage.
     @param actDesired the actionDesired to add into the average
  */
  virtual void addAverage(ArActionDesired *actDesired)
    {
      if (actDesired == NULL)
	return;
      myVelDes.addAverage(&actDesired->myVelDes);

      myMaxVelDes.addAverage(&actDesired->myMaxVelDes);
      myMaxNegVelDes.addAverage(&actDesired->myMaxNegVelDes);
      myTransAccelDes.addAverage(&actDesired->myTransAccelDes);
      myTransDecelDes.addAverage(&actDesired->myTransDecelDes);

      // if we're using one of rot or delta heading use that,
      // otherwise use whatever they're using
      if (myRotVelDes.getStrength() > NO_STRENGTH)
      {
	myRotVelDes.addAverage(
		&actDesired->myRotVelDes);
      }
      else if (myDeltaHeadingDes.getStrength() > NO_STRENGTH)
      {
	myDeltaHeadingDes.addAverage(
		&actDesired->myDeltaHeadingDes);
      }
      else
      {
	myRotVelDes.addAverage(
		&actDesired->myRotVelDes);
	myDeltaHeadingDes.addAverage(
		&actDesired->myDeltaHeadingDes);
      }
      myMaxRotVelDes.addAverage(&actDesired->myMaxRotVelDes);
      myMaxRotVelPosDes.addAverage(&actDesired->myMaxRotVelPosDes);
      myMaxRotVelNegDes.addAverage(&actDesired->myMaxRotVelNegDes);
      myRotAccelDes.addAverage(&actDesired->myRotAccelDes);
      myRotDecelDes.addAverage(&actDesired->myRotDecelDes);

      myLatVelDes.addAverage(&actDesired->myLatVelDes);
      myMaxLeftLatVelDes.addAverage(&actDesired->myMaxLeftLatVelDes);
      myMaxRightLatVelDes.addAverage(&actDesired->myMaxRightLatVelDes);
      myLatAccelDes.addAverage(&actDesired->myLatAccelDes);
      myLatDecelDes.addAverage(&actDesired->myLatDecelDes);
    }
  /// Ends the process of avereraging together different desireds
  /**
     For a description of how to use this, see startAverage.
  */
  virtual void endAverage(void)
    {
      myVelDes.endAverage();
      myMaxVelDes.endAverage();
      myMaxNegVelDes.endAverage();
      myTransAccelDes.endAverage();
      myTransDecelDes.endAverage();

      myRotVelDes.endAverage();
      myDeltaHeadingDes.endAverage();
      myMaxRotVelDes.endAverage();
      myMaxRotVelPosDes.endAverage();
      myMaxRotVelNegDes.endAverage();
      myRotAccelDes.endAverage();
      myRotDecelDes.endAverage();

      myLatVelDes.endAverage();
      myMaxLeftLatVelDes.endAverage();
      myMaxRightLatVelDes.endAverage();
      myLatAccelDes.endAverage();
      myLatDecelDes.endAverage();
    }
  /// Accounts for robot heading, mostly internal
  /**
     This accounts for the robots heading, and transforms the set heading
     on this actionDesired into a delta heading so it can be merged and 
     averaged and the like
     @param robotHeading the heading the real actual robot is at now
   */
  virtual void accountForRobotHeading(double robotHeading)
    {
      if (myHeadingSet)
	setDeltaHeading(ArMath::subAngle(myHeading, robotHeading), 
			myHeadingStrength);
      myHeadingSet = false;
    }
  /// Logs what is desired
  AREXPORT virtual void log(void) const;
  /// Gets whether anything is desired (should only really be used in relation to logging)
  AREXPORT virtual bool isAnythingDesired(void) const;
  /// Do a sanity check on the action (this is set up this way so the
  /// action name can be passed in)
  AREXPORT virtual void sanityCheck(const char *actionName);

protected:
  double myHeading;
  double myHeadingStrength;
  bool myHeadingSet;

  ArActionDesiredChannel myVelDes;
  ArActionDesiredChannel myMaxVelDes;
  ArActionDesiredChannel myMaxNegVelDes;
  ArActionDesiredChannel myTransAccelDes;
  ArActionDesiredChannel myTransDecelDes;

  ArActionDesiredChannel myRotVelDes;
  ArActionDesiredChannel myDeltaHeadingDes;
  ArActionDesiredChannel myMaxRotVelDes;
  ArActionDesiredChannel myMaxRotVelPosDes;
  ArActionDesiredChannel myMaxRotVelNegDes;
  ArActionDesiredChannel myRotAccelDes;
  ArActionDesiredChannel myRotDecelDes;

  ArActionDesiredChannel myLatVelDes;
  ArActionDesiredChannel myMaxLeftLatVelDes;
  ArActionDesiredChannel myMaxRightLatVelDes;
  ArActionDesiredChannel myLatAccelDes;
  ArActionDesiredChannel myLatDecelDes;
};


#endif