/*

ARIA header files for use with ARNL 1.7.1

Copyright(C) 2004, 2005 ActivMedia Robotics, LLC. 
Copyright(C) 2006, 2007, 2008, 2009 MobileRobots Inc.
All rights reserved.

This copy of Aria was relicensed for use with Arnl and the Arnl
license by MobileRobots Inc.  If you wish to download a seperate
distribution of Aria licensed under the GPL or a commercial license go to
http://www.mobilerobots.com/SOFTWARE/aria.html or contact MobileRobots
Inc, at robots@mobilerobots.com or MobileRobots Inc,
10 Columbia Drive, Amherst, NH 03031; 800-639-9481

MobileRobots Inc hereby grants to other individuals or
organizations permission to use this software with Arnl and in
compliance with the Arnl license.  This software may not be
distributed to others except by MobileRobots Inc.

MobileRobots Inc does not make any representations about the
suitability of this software for any purpose.  It is provided "as is"
without express or implied warranty.

*/
#ifndef ARROBOT_H
#define ARROBOT_H

#include "ariaTypedefs.h"
#include "ArRobotPacketSender.h"
#include "ArRobotPacketReceiver.h"
#include "ArFunctor.h"
#include "ArFunctor.h"
#include "ArSyncTask.h"
#include "ArSensorReading.h"
#include "ArMutex.h"
#include "ArCondition.h"
#include "ArSyncLoop.h"
#include "ArRobotParams.h"
#include "ArActionDesired.h"
#include "ArResolver.h"
#include "ArTransform.h"
#include "ArInterpolation.h"
#include "ArKeyHandler.h"
#include <list>

class ArAction;
class ArRobotConfigPacketReader;
class ArDeviceConnection;
class ArRangeDevice;
class ArRobotPacket;
class ArPTZ;
class ArLaser;

/// Central class for communicating with and operating the robot
/** 
    This is the most important class. It is used to communicate with
    the robot by sending commands and retrieving data (including
    wheel odometry, digital and analog inputs, sonar data, and more). 
    It is also used
    to provide access to objects for controlling attached accessories, 
    ArRangeDevice objects, ArAction objects, and others.  For details
    on usage, and how the task cycle and obot state synchronization works,
    see the @ref robot "ArRobot section" and the
    @ref ClientCommands "Commands and Actions section" of the ARIA overview.

    @note In Windows you cannot make an ArRobot object a global variable, 
    it will crash because the compiler initializes the constructors in
    the wrong order. You can, however, make a pointer to an ArRobot and then
    allocate it with 'new' at program start.

    @see ArSimpleConnector
**/
class ArRobot
{
public:

  typedef enum {
    WAIT_CONNECTED, ///< The robot has connected
    WAIT_FAILED_CONN, ///< The robot failed to connect
    WAIT_RUN_EXIT, ///< The run loop has exited
    WAIT_TIMEDOUT, ///< The wait reached the timeout specified
    WAIT_INTR, ///< The wait was interupted by a signal
    WAIT_FAIL ///< The wait failed due to an error
  } WaitState;

  enum ChargeState {
    CHARGING_UNKNOWN = -1,
    CHARGING_NOT = 0,
    CHARGING_BULK = 1,
    CHARGING_OVERCHARGE = 2,
    CHARGING_FLOAT = 3
  };
  /// Constructor
  AREXPORT ArRobot(const char * name = NULL, bool ignored = true,
		   bool doSigHandle=true, 
		   bool normalInit = true, bool addAriaExitCallback = true);
		   
  /// Destructor
  AREXPORT ~ArRobot();

  /// Starts the instance to do processing in this thread
  AREXPORT void run(bool stopRunIfNotConnected);
  /// Starts the instance to do processing in its own new thread
  AREXPORT void runAsync(bool stopRunIfNotConnected);

  /// Returns whether the robot is currently running or not
  AREXPORT bool isRunning(void) const;

  /// Stops the robot from doing any more processing
  AREXPORT void stopRunning(bool doDisconnect=true); 

  /// Sets the connection this instance uses
  AREXPORT void setDeviceConnection(ArDeviceConnection *connection);
  /// Gets the connection this instance uses
  AREXPORT ArDeviceConnection *getDeviceConnection(void) const;
  
  /// Questions whether the robot is connected or not
  /** 
      @return true if connected to a robot, false if not
  */
  AREXPORT bool isConnected(void) const { return myIsConnected; }
  /// Connects to a robot, not returning until connection made or failed
  AREXPORT bool blockingConnect(void);
  /// Connects to a robot, from the robots own thread
  AREXPORT bool asyncConnect(void);
  /// Disconnects from a robot
  AREXPORT bool disconnect(void);

  /// Clears what direct motion commands have been given, so actions work
  AREXPORT void clearDirectMotion(void);
  /// Returns true if direct motion commands are blocking actions
  AREXPORT bool isDirectMotion(void) const;

  /// Sets the state reflection to be inactive (until motion or clearDirectMotion)
  /// @see clearDirectMotion
  AREXPORT void stopStateReflection(void);
  

  /// Enables the motors on the robot
  AREXPORT void enableMotors();
  /// Disables the motors on the robot
  AREXPORT void disableMotors();

  /// Enables the sonar on the robot
  AREXPORT void enableSonar();
  /// Disables the sonar on the robot
  AREXPORT void disableSonar();

  /// Stops the robot
  /// @see clearDirectMotion
  AREXPORT void stop(void);
  /// Sets the velocity
  /// @see clearDirectMotion
  AREXPORT void setVel(double velocity);
  /// Sets the velocity of the wheels independently
  AREXPORT void setVel2(double leftVelocity, double rightVelocity);
  /// Move the given distance forward/backwards
  AREXPORT void move(double distance);
  /// Sees if the robot is done moving the previously given move
  AREXPORT bool isMoveDone(double delta = 0.0);
  /// Sets the difference required for being done with a move
  AREXPORT void setMoveDoneDist(double dist) { myMoveDoneDist = dist; }
  /// Gets the difference required for being done with a move
  AREXPORT double getMoveDoneDist(void) { return myMoveDoneDist; }
  /// Sets the heading
  AREXPORT void setHeading(double heading);
  /// Sets the rotational velocity
  AREXPORT void setRotVel(double velocity);
  /// Sets the delta heading
  AREXPORT void setDeltaHeading(double deltaHeading);
  /// Sees if the robot is done changing to the previously given setHeading
  AREXPORT bool isHeadingDone(double delta = 0.0) const;
  /// sets the difference required for being done with a heading change (e.g. used in isHeadingDone())
  AREXPORT void setHeadingDoneDiff(double degrees) 
    { myHeadingDoneDiff = degrees; }
  /// Gets the difference required for being done with a heading change (e.g. used in isHeadingDone())
  AREXPORT double getHeadingDoneDiff(void) const { return myHeadingDoneDiff; }
  /// Sets the lateral velocity
  /// @see clearDirectMotion
  AREXPORT void setLatVel(double latVelocity);

  /// Sets the length of time a direct motion command will take precedence 
  /// over actions, in milliseconds
  AREXPORT void setDirectMotionPrecedenceTime(int mSec);

  /// Gets the length of time a direct motion command will take precedence 
  /// over actions, in milliseconds
  AREXPORT unsigned int getDirectMotionPrecedenceTime(void) const;

  /// Sends a command to the robot with no arguments
  AREXPORT bool com(unsigned char command);
  /// Sends a command to the robot with an int for argument
  AREXPORT bool comInt(unsigned char command, short int argument);
  /// Sends a command to the robot with two bytes for argument
  AREXPORT bool com2Bytes(unsigned char command, char high, char low);
  /// Sends a command to the robot with a length-prefixed string for argument
  AREXPORT bool comStr(unsigned char command, const char *argument);
  /// Sends a command to the robot with a length-prefixed string for argument
  AREXPORT bool comStrN(unsigned char command, const char *str, int size);
  /// Sends a command containing exactly the data in the given buffer as argument
  AREXPORT bool comDataN(unsigned char command, const char *data, int size);

  /// Returns the robot's name that is set in its onboard firmware configuration
  AREXPORT const char * getRobotName(void) const { return myRobotName.c_str();}
  /// Returns the type of the robot we are currently connected to
  AREXPORT const char * getRobotType(void) const { return myRobotType.c_str();}
  /// Returns the subtype of the robot we are currently connected to
  const char * getRobotSubType(void) const 
    { return myRobotSubType.c_str(); }

  /// Gets the robot's absolute maximum translational velocity
  double getAbsoluteMaxTransVel(void) const 
    { return myAbsoluteMaxTransVel; }
  /// Sets the robot's absolute maximum translational velocity
  AREXPORT bool setAbsoluteMaxTransVel(double maxVel);

  /// Gets the robot's absolute maximum translational acceleration
  AREXPORT double getAbsoluteMaxTransAccel(void) const 
    { return myAbsoluteMaxTransAccel; }
  /// Sets the robot's absolute maximum translational acceleration
  AREXPORT bool setAbsoluteMaxTransAccel(double maxAccel);

  /// Gets the robot's absolute maximum translational deceleration
  double getAbsoluteMaxTransDecel(void) const 
    { return myAbsoluteMaxTransDecel; }
  /// Sets the robot's absolute maximum translational deceleration
  AREXPORT bool setAbsoluteMaxTransDecel(double maxDecel);

  /// Gets the robot's absolute maximum rotational velocity
  double getAbsoluteMaxRotVel(void) const 
    { return myAbsoluteMaxRotVel; }
  /// Sets the robot's absolute maximum rotational velocity
  AREXPORT bool setAbsoluteMaxRotVel(double maxVel);
  
  /// Gets the robot's absolute maximum rotational acceleration
  double getAbsoluteMaxRotAccel(void) const 
    { return myAbsoluteMaxRotAccel; }
  /// Sets the robot's absolute maximum rotational acceleration
  AREXPORT bool setAbsoluteMaxRotAccel(double maxAccel);

  /// Gets the robot's absolute maximum rotational deceleration
  double getAbsoluteMaxRotDecel(void) const 
    { return myAbsoluteMaxRotDecel; }
  /// Sets the robot's absolute maximum rotational deceleration
  AREXPORT bool setAbsoluteMaxRotDecel(double maxDecel);

  /// Gets the robot's absolute maximum lateral velocity
  double getAbsoluteMaxLatVel(void) const 
    { return myAbsoluteMaxLatVel; }
  /// Sets the robot's absolute maximum lateral velocity
  AREXPORT bool setAbsoluteMaxLatVel(double maxVel);

  /// Gets the robot's absolute maximum lateral acceleration
  double getAbsoluteMaxLatAccel(void) const 
    { return myAbsoluteMaxLatAccel; }
  /// Sets the robot's absolute maximum lateral acceleration
  AREXPORT bool setAbsoluteMaxLatAccel(double maxAccel);

  /// Gets the robot's absolute maximum lateral deceleration
  double getAbsoluteMaxLatDecel(void) const 
    { return myAbsoluteMaxLatDecel; }
  /// Sets the robot's absolute maximum lateral deceleration
  AREXPORT bool setAbsoluteMaxLatDecel(double maxDecel);


  // Accessors

  /** @brief Get the current stored global position of the robot.
   *
   *  This position is updated by data reported by the robot as it
   *  moves, and may also be changed by other program components,
   *  such as a localization process (see moveTo()).
   *
   *  This position is also referred to as the robot's 
   *  "odometry" or "odometric" pose, since the robot
   *  uses its odometry data to determine this pose; but it
   *  may also incorporate additional data sources such as 
   *  an onboard gyro.  The term "odometric pose" also 
   *  distinguishes this position by the fact that its
   *  coordinate system may be arbitrary, and seperate
   *  from any external coordinate system.
   *
   *  @sa getEncoderPose()
   *  @sa moveTo()
   */
  ArPose getPose(void) const { return myGlobalPose; }
  /// Gets the global X position of the robot
  /** @sa getPose() */
  double getX(void) const { return  myGlobalPose.getX(); }
  /// Gets the global Y position of the robot
  /** @sa getPose() */
  double getY(void) const { return myGlobalPose.getY(); }
  /// Gets the global angular position ("theta") of the robot
  /** @sa getPose() */
  double getTh(void) const { return myGlobalPose.getTh(); }
  /// Gets the distance to a point from the robot's current position
  double findDistanceTo(const ArPose pose) 
    { return myGlobalPose.findDistanceTo(pose); }
  /// Gets the angle to a point from the robot's current position and orientation
  double findAngleTo(const ArPose pose) 
    { return myGlobalPose.findAngleTo(pose); }
  /// Gets the difference between the angle to a point from the robot's current heading
  double findDeltaHeadingTo(const ArPose pose) 
    { return ArMath::subAngle(myGlobalPose.findAngleTo(pose),
			      myGlobalPose.getTh()); }


  /// Gets the current translational velocity of the robot
  double getVel(void) const { return myVel; }
  /// Gets the current rotational velocity of the robot
  /**
     Note that with new firmware versions (ARCOS as of April 2006 or so)
     this is the velocity reported by the robot.  With older firmware
     this number is calculated using the difference between the robot's reported wheel
     velocities multiplied by diffConvFactor from the .p (robot parameter) files.
  **/
  double getRotVel(void) const { return myRotVel; }
  /// Gets the current lateral velocity of the robot
  /**
     Note that this will only be valid if hasLatVel() returns true
   **/
  double getLatVel(void) const { return myLatVel; }
  /// Sees if the robot supports lateral velocities (e.g. Seekur(TM))
  bool hasLatVel(void) const { return myParams->hasLatVel(); }
  /// Gets the robot radius (in mm)
  double getRobotRadius(void) const { return myParams->getRobotRadius(); }
  /// Gets the robot width (in mm)
  double getRobotWidth(void) const { return myParams->getRobotWidth(); }
  /// Gets the robot length (in mm)
  double getRobotLength(void) const { return myParams->getRobotLength(); }
  /// Gets the robot length to the front (in mm)
  double getRobotLengthFront(void) const { return myRobotLengthFront; }
  /// Gets the robot length to the front (in mm)
  double getRobotLengthRear(void) const { return myRobotLengthRear; }
  /// Gets the robot diagonal (half-height to diagonal of octagon) (in mm)
  double getRobotDiagonal(void) const { return myParams->getRobotDiagonal(); }
  /// Gets the battery voltage of the robot (normalized to 12 volt system)
  /**
     This value is averaged over a number of readings, use getBatteryVoltageNow() to
     get the value most recently received.  (Access the number of readings
     used in the running average with getBatteryVoltageAverageOfNum() and
     setBatteryVoltageAverageOfNum().)

     This is a value normalized to 12 volts, if you want what the
     actual voltage of the robot is use getRealBatteryVoltage().

     @sa getRealBatteryVoltage()
     @sa getBatteryVoltageNow()
   **/
  double getBatteryVoltage(void) const {return myBatteryAverager.getAverage();}
  /// Gets the instaneous battery voltage
  /**
     This is a value normalized to 12 volts, if you want what the
     actual voltage of the robot is use getRealBatteryVoltage().
     
     @sa getBatteryVoltage()
     @sa getRealBatteryVoltage()
   **/
  double getBatteryVoltageNow(void) const { return myBatteryVoltage; }
  /// Gets the real battery voltage of the robot 
  /**
     This value is averaged over a number of readings, use getRealBatteryVoltageNow() to
     get the value most recently received.  (Access the number of readings
     used in the running average with getRealBatteryVoltageAverageOfNum() and
     setRealBatteryVoltageAverageOfNum().)

     This is whatever the actual voltage of the robot is, if you want
     a value normalized to common 12 volts for all robot types use getBatteryVoltage(). 

     If the robot doesn't support a "real" battery voltage, then
     this method will just return the normal battery voltage (normalized to 12
     volt scale). (Most older robots that don't support a real battery voltage
     have 12 volts batteries anyway.)
   **/
  double getRealBatteryVoltage(void) const 
    { return myRealBatteryAverager.getAverage(); }
  /// Gets the instaneous battery voltage
  /**
     This is whatever the actual voltage of the robot is, if you want
     a value normalized to a common 12 volts for all robot types use getBatteryVoltage().  If the
     robot doesn't support this number the voltage will be less than 0
     and you should use getBatteryVoltageNow().
  **/
  double getRealBatteryVoltageNow(void) const { return myRealBatteryVoltage; }
  /// Gets if the state of charge value is in use
  bool haveStateOfCharge(void) const { return myHaveStateOfCharge; }
  /// Gets the state of charge (percent of charge, as number between 0 and 100)
  double getStateOfCharge(void) const 
    { if (!myHaveStateOfCharge) return 0; else return myStateOfCharge; }
  /// Gets the last time the state of charge was set
  ArTime getStateOfChargeSetTime(void) const { return myStateOfChargeSetTime; }
  /// Gets the state of charge that is considered low
  double getStateOfChargeLow(void) const 
    { return myStateOfChargeLow; }
  /// Gets the state of charge (percent of charge, as number between 0 and 100)
  double getStateOfChargeShutdown(void) const 
    { return myStateOfChargeShutdown; }
  /// Gets the velocity of the left wheel
  double getLeftVel(void) const { return myLeftVel; }
  /// Gets the velocity of the right wheel
  double getRightVel(void) const { return myRightVel; }
  /// Gets the 2 bytes of stall and bumper flags from the robot 
  /** See robot operations manual SIP packet documentation for details.
   *  @sa isLeftMotorStalled()
   *  @sa isRightMotorStalled()
   */
  int getStallValue(void) const { return myStallValue; }
  /// Returns true if the left motor is stalled
  bool isLeftMotorStalled(void) const 
    { return (myStallValue & 0xff) & ArUtil::BIT0; }
  /// Returns true if the left motor is stalled
  bool isRightMotorStalled(void) const
    { return ((myStallValue & 0xff00) >> 8) & ArUtil::BIT0; }
  /// Gets the control heading
  /**
    Gets the control heading as an offset from the current heading.
    @see getTh
  */
  double getControl(void) const { return myControl; }
  /// Gets the flags values
  int getFlags(void) const { return myFlags; }
  /// Gets the fault flags values
  int getFaultFlags(void) const { return myFaultFlags; }
  /// Gets whether or not we're getting the fault flags values
  bool hasFaultFlags(void) const { return myHasFaultFlags; }
  /// returns true if the motors are enabled
  bool areMotorsEnabled(void) const { return (myFlags & ArUtil::BIT0); }
  /// returns true if the sonars are enabled (note that if the robot has no sonars at all, this will return false)
  bool areSonarsEnabled(void) const { 
    return (myFlags & 
	    (ArUtil::BIT1 | ArUtil::BIT2 | ArUtil::BIT3 | ArUtil::BIT4)); }
  /// returns true if the estop is pressed (or unrelieved)
  bool isEStopPressed(void) const { return (myFlags & ArUtil::BIT5); }
  /// Gets the compass heading from the robot
  double getCompass(void) const { return myCompass; }
  /// Gets which analog port is selected
  int getAnalogPortSelected(void) const { return myAnalogPortSelected; }
  /// Gets the analog value
  unsigned char getAnalog(void) const { return myAnalog; }
  /// Gets the byte representing digital input status
  unsigned char getDigIn(void) const { return myDigIn; }
  /// Gets the byte representing digital output status
  unsigned char getDigOut(void) const { return myDigOut; }
  /// Gets the charge state of the robot (see long docs)
  AREXPORT ChargeState getChargeState(void) const;

  /// Gets the number of bytes in the analog IO buffer
  int getIOAnalogSize(void) const { return myIOAnalogSize; }
  /// Gets the number of bytes in the digital input IO buffer
  int getIODigInSize(void) const { return myIODigInSize; }
  /// Gets the number of bytes in the digital output IO buffer
  int getIODigOutSize(void) const { return myIODigOutSize; }

  /// Gets the n'th byte from the analog input data from the IO packet
  AREXPORT int getIOAnalog(int num) const;
  /// Gets the n'th byte from the analog input data from the IO packet
  AREXPORT double getIOAnalogVoltage(int num) const;
  /// Gets the n'th byte from the digital input data from the IO packet
  AREXPORT unsigned char getIODigIn(int num) const;
  /// Gets the n'th byte from the digital output data from the IO packet
  AREXPORT unsigned char getIODigOut(int num) const;

  /// Gets whether the robot has table sensing IR or not (see params in docs)
  bool hasTableSensingIR(void) const { return myParams->haveTableSensingIR(); }
  /// Returns true if the left table sensing IR is triggered
  AREXPORT bool isLeftTableSensingIRTriggered(void) const;
  /// Returns true if the right table sensing IR is triggered
  AREXPORT bool isRightTableSensingIRTriggered(void) const;
  /// Returns true if the left break beam IR is triggered
  AREXPORT bool isLeftBreakBeamTriggered(void) const;
  /// Returns true if the right break beam IR is triggered
  AREXPORT bool isRightBreakBeamTriggered(void) const;
  /// Returns the time received of the last IO packet
  ArTime getIOPacketTime(void) const { return myLastIOPacketReceivedTime; }

  /// Returns true if the E-Stop button is pressed
  AREXPORT bool getEstop(void) { return (myFlags & ArUtil::BIT5); }

  /// Gets whether the robot has front bumpers (see ARCOS parameters in the robot manual)
  bool hasFrontBumpers(void) const { return myParams->haveFrontBumpers(); }
  /// Get the number of the front bumper switches
  unsigned int getNumFrontBumpers(void) const 
    { return myParams->numFrontBumpers(); }
  /// Gets whether the robot has rear bumpers (see ARCOS parameters in the robot manual)
  bool hasRearBumpers(void) const { return myParams->haveRearBumpers(); }
  /// Gets the number of  rear bumper switches
  unsigned int getNumRearBumpers(void) const 
    { return myParams->numRearBumpers(); }

  /** @brief Get the position of the robot according to the last robot SIP,
   * possibly with gyro correction if installed and enabled, but without any
   * transformations applied.
   * @sa getPose()
   * @sa getRawEncoderPose()
   */
  ArPose getEncoderPose(void) const { return myEncoderPose; }

  /// Gets if the robot is trying to move or not 
  /**
     "Trying" to move means that some action or other command has
     requested motion, but another action or factor has cancelled
     that request. 
   
     This is so that if the robot is trying to move, but is prevented
     (mainly by an action) there'll still be some indication that the
     robot is trying to move (e.g. we can prevent the sonar from automatically being
     turned off). Note that this flag
     doesn't have anything to do with if the robot is really moving or
     not, to check that, check the current velocities.
     @sa forceTryingToMove() to force this state on
   **/
  bool isTryingToMove(void) { return myTryingToMove; }

  /// Manually sets the flag that says the robot is trying to move for one cycle
  /**
     This is so that things that might move the robot at any time
     can can make the robot look
     like it is otherwise in the TryingToMove state, even if no motion
     is currently being requested.  For example, ArNetworking's teleoperation
     mode forces TryingToMove at all times while active, not just when
     requesting motion.  This method must be called in each task cycle,
     it is reset at the end of the task cycle (in state reflection stage)
     to its natural (non-forced) value.

     @sa isTryingToMove()
   **/
  void forceTryingToMove(void) { myTryingToMove = true; }
  

  /// Gets the number of motor packets received in the last second
  AREXPORT int getMotorPacCount(void) const;
  /// Gets the number of sonar returns received in the last second
  AREXPORT int getSonarPacCount(void) const;

  /// Gets the range of the last sonar reading for the given sonar
  AREXPORT int getSonarRange(int num) const;
  /// Find out if the given sonar reading was newly refreshed by the last incoming SIP received.
  AREXPORT bool isSonarNew(int num) const;
  /// Find the number of sonar sensors (that the robot has yet returned values for)
  int getNumSonar(void) const { return myNumSonar; }
  /// Returns the sonar reading for the given sonar
  AREXPORT ArSensorReading *getSonarReading(int num) const;
  /// Returns the closest of the current sonar reading in the given range
  AREXPORT int getClosestSonarRange(double startAngle, double endAngle) const;
  /// Returns the number of the sonar that has the closest current reading in the given range
  AREXPORT int getClosestSonarNumber(double startAngle, double endAngle) const;

  /// Gets the robots name in ARIAs list
  AREXPORT const char *getName(void) const;
  /// Sets the robots name in ARIAs list
  AREXPORT void setName(const char *name);

  /// Change stored pose (i.e. the value returned by getPose())
  AREXPORT void moveTo(ArPose pose, bool doCumulative = true);
  /// Change stored pose (i.e. the value returned by getPose()) 
  AREXPORT void moveTo(ArPose to, ArPose from, bool doCumulative = true);
  /// Manually sets the current percentage that the robot is charged (argument is percentage, as a number between 0 and 100)
  AREXPORT void setStateOfCharge(double stateOfCharge);
  /// Sets the state of charge (percentage) that is considered to be low
  void setStateOfChargeLow(double stateOfChargeLow) 
    { myStateOfChargeLow = stateOfChargeLow; }
  /// Sets the state of charge that will cause a shutdown
  void setStateOfChargeShutdown(double stateOfChargeShutdown) 
    { myStateOfChargeShutdown = stateOfChargeShutdown; }
  /// Sets the charge state (for use with setting the state of charge)
  AREXPORT void setChargeState(ArRobot::ChargeState chargeState);

  /// Gets the number of readings the battery voltage is the average of
  size_t getBatteryVoltageAverageOfNum(void) 
    { return myBatteryAverager.getNumToAverage(); }

  /// Sets the number of readings the battery voltage is the average of (default 20)
  void setBatteryVoltageAverageOfNum(size_t numToAverage)
    { myBatteryAverager.setNumToAverage(numToAverage); }

  /// Gets the number of readings the battery voltage is the average of
  size_t getRealBatteryVoltageAverageOfNum(void) 
    { return myRealBatteryAverager.getNumToAverage(); }

  /// Sets the number of readings the real battery voltage is the average of (default 20)
  void setRealBatteryVoltageAverageOfNum(size_t numToAverage)
    { myRealBatteryAverager.setNumToAverage(numToAverage); }

  /// Returns true if we have a temperature, false otherwise
  bool hasTemperature(void) 
    { if (myTemperature != -128) return true; else return false; }
  
  /// Gets the temperature of the robot, -128 if not available, -127 to 127 otherwise
  int getTemperature(void) { return myTemperature; }

  /// Starts a continuous stream of encoder packets
  AREXPORT void requestEncoderPackets(void);

  /// Starts a continuous stream of IO packets
  AREXPORT void requestIOPackets(void);

  /// Stops a continuous stream of encoder packets
  AREXPORT void stopEncoderPackets(void);

  /// Stops a continuous stream of IO packets
  AREXPORT void stopIOPackets(void);

  /// Gets packet data from the left encoder
  AREXPORT long int getLeftEncoder(void);

  /// Gets packet data from the right encoder
  AREXPORT long int getRightEncoder(void);

  /// Changes the transform
  AREXPORT void setEncoderTransform(ArPose deadReconPos,
				    ArPose globalPos);

  /// Changes the transform directly
  AREXPORT void setEncoderTransform(ArPose transformPos);

  /// Gets the encoder transform
  AREXPORT ArTransform getEncoderTransform(void) const;

  /// This gets the transform from local coords to global coords
  AREXPORT ArTransform getToGlobalTransform(void) const;

  /// This gets the transform for going from global coords to local coords
  AREXPORT ArTransform getToLocalTransform(void) const;

  /// This applies a transform to all the robot range devices and to the sonar
  AREXPORT void applyTransform(ArTransform trans, bool doCumulative = true);

  /// Sets the dead recon position of the robot
  AREXPORT void setDeadReconPose(ArPose pose);

  /// This gets the distance the robot has travelled since the last reset (mm)
  AREXPORT double getTripOdometerDistance(void) 
    { return myTripOdometerDistance; }

  /// This gets the number of degrees the robot has turned since the last reset (deg)
  AREXPORT double getTripOdometerDegrees(void) 
    { return myTripOdometerDegrees; }

  /// This gets the time since the odometer was reset (sec)
  AREXPORT double getTripOdometerTime(void) 
    { return myTripOdometerStart.secSince(); }

  /// Resets the odometer
  AREXPORT void resetTripOdometer(void);

  /// This gets the distance the robot has travelled (mm)
  AREXPORT double getOdometerDistance(void) 
    { return myOdometerDistance; }

  /// This gets the number of degrees the robot has turned (deg)
  AREXPORT double getOdometerDegrees(void) 
    { return myOdometerDegrees; }

  /// This gets the time since the robot started (sec)
  AREXPORT double getOdometerTime(void) 
    { return myOdometerStart.secSince(); }


  /// Adds a rangeDevice to the robot's list of them, and set the device's robot pointer
  AREXPORT void addRangeDevice(ArRangeDevice *device);
  /// Remove a range device from the robot's list, by name
  AREXPORT void remRangeDevice(const char *name);
  /// Remove a range device from the robot's list, by instance
  AREXPORT void remRangeDevice(ArRangeDevice *device);

  /// Finds a rangeDevice in the robot's list
  AREXPORT const ArRangeDevice *findRangeDevice(const char *name, 
						bool ignoreCase = false) const;

  /// Finds a rangeDevice in the robot's list
  AREXPORT ArRangeDevice *findRangeDevice(const char *name, 
					  bool ignoreCase = false);

  /// Gets the range device list
  AREXPORT std::list<ArRangeDevice *> *getRangeDeviceList(void);

  /// Finds whether a particular range device is attached to this robot or not
  AREXPORT bool hasRangeDevice(ArRangeDevice *device) const;

  /// Goes through all the range devices and checks them
  AREXPORT double checkRangeDevicesCurrentPolar(
	  double startAngle, double endAngle, double *angle = NULL,
	  const ArRangeDevice **rangeDevice = NULL,
	  bool useLocationDependentDevices = true) const;

  /// Goes through all the range devices and checks them
  AREXPORT double checkRangeDevicesCumulativePolar(
	  double startAngle, double endAngle, double *angle = NULL,
	  const ArRangeDevice **rangeDevice = NULL,
	  bool useLocationDependentDevices = true) const;


  // Goes through all the range devices and checks them
  AREXPORT double checkRangeDevicesCurrentBox(
	  double x1, double y1, double x2, double y2,
	  ArPose *readingPos = NULL,
	  const ArRangeDevice **rangeDevice = NULL,
	  bool useLocationDependentDevices = true) const;

  // Goes through all the range devices and checks them
  AREXPORT double checkRangeDevicesCumulativeBox(
	  double x1, double y1, double x2, double y2,
	  ArPose *readingPos = NULL, 
	  const ArRangeDevice **rangeDevice = NULL,
	  bool useLocationDependentDevices = true) const;

  /// Adds a laser to the robot's map of them
  AREXPORT bool addLaser(ArLaser *laser, int laserNumber, 
			 bool addAsRangeDevice = true);
  /// Remove a range device from the robot's list, by instance
  AREXPORT bool remLaser(ArLaser *laser, bool removeAsRangeDevice = true);
  /// Remove a range device from the robot's list, by number
  AREXPORT bool remLaser(int laserNumber, bool removeAsRangeDevice = true);

  /// Finds a laser in the robot's list
  AREXPORT const ArLaser *findLaser(int laserNumber) const;

  /// Finds a laser in the robot's list
  AREXPORT ArLaser *findLaser(int laserNumber);

  /// Gets the range device list
  AREXPORT const std::map<int, ArLaser *> *getLaserMap(void) const;

  /// Gets the range device list
  AREXPORT std::map<int, ArLaser *> *getLaserMap(void);

  /// Finds whether a particular range device is attached to this robot or not
  AREXPORT bool hasLaser(ArLaser *device) const;

  /// Sets the camera this robot is using
  void setPTZ(ArPTZ *ptz) { myPtz = ptz; }
  /// Sets the camera this robot is using
  ArPTZ *getPTZ(void) { return myPtz; }

  /// Sets the number of milliseconds between state reflection refreshes
  /// if the state has not changed 
  AREXPORT void setStateReflectionRefreshTime(int msec);

  /// Sets the number of milliseconds between state reflection refreshes
  /// if the state has not changed 
  AREXPORT int getStateReflectionRefreshTime(void) const;

  /// Adds a packet handler to the list of packet handlers
  AREXPORT void addPacketHandler(
	  ArRetFunctor1<bool, ArRobotPacket *> *functor, 
	  ArListPos::Pos position = ArListPos::LAST);
  
  /// Removes a packet handler from the list of packet handlers
  AREXPORT void remPacketHandler(
	  ArRetFunctor1<bool, ArRobotPacket *> *functor);

  /// Adds a connect callback
  AREXPORT void addConnectCB(ArFunctor *functor, 
			     ArListPos::Pos position = ArListPos::LAST);
  /// Removes a connect callback
  AREXPORT void remConnectCB(ArFunctor *functor);

  /// Adds a callback for when a connection to the robot is failed
  AREXPORT void addFailedConnectCB(ArFunctor *functor, 
				   ArListPos::Pos position = ArListPos::LAST);
  /// Removes a callback for when a connection to the robot is failed
  AREXPORT void remFailedConnectCB(ArFunctor *functor);

  /// Adds a callback for when disconnect is called while connected
  AREXPORT void addDisconnectNormallyCB(ArFunctor *functor, 
				ArListPos::Pos position = ArListPos::LAST);
  /// Removes a callback for when disconnect is called while connected
  AREXPORT void remDisconnectNormallyCB(ArFunctor *functor);
  
  /// Adds a callback for when disconnection happens because of an error
  AREXPORT void addDisconnectOnErrorCB(ArFunctor *functor, 
				   ArListPos::Pos position = ArListPos::LAST);
  /// Removes a callback for when disconnection happens because of an error
  AREXPORT void remDisconnectOnErrorCB(ArFunctor *functor);

  /// Adds a callback for when the run loop exits for what ever reason
  AREXPORT void addRunExitCB(ArFunctor *functor, 
			     ArListPos::Pos position = ArListPos::LAST);
  /// Removes a callback for when the run loop exits for what ever reason
  AREXPORT void remRunExitCB(ArFunctor *functor);

  /// Suspend calling thread until the ArRobot is connected
  AREXPORT WaitState waitForConnect(unsigned int msecs=0);
  /// Suspend calling thread until the ArRobot is connected or fails to connect
  AREXPORT WaitState waitForConnectOrConnFail(unsigned int msecs=0);
  /// Suspend calling thread until the ArRobot run loop has exited
  AREXPORT WaitState waitForRunExit(unsigned int msecs=0);

  /// Wake up all threads waiting on this robot
  AREXPORT void wakeAllWaitingThreads();
  /// Wake up all threads waiting for connection
  AREXPORT void wakeAllConnWaitingThreads();
  /// Wake up all threads waiting for connection or connection failure
  AREXPORT void wakeAllConnOrFailWaitingThreads();
  /// Wake up all threads waiting for the run loop to exit
  AREXPORT void wakeAllRunExitWaitingThreads();

  /// Adds a user task to the list of synchronous taskes
  AREXPORT bool addUserTask(const char *name, int position, 
			       ArFunctor *functor,
			       ArTaskState::State *state = NULL);
  /// Removes a user task from the list of synchronous taskes by name
  AREXPORT void remUserTask(const char *name);
  /// Removes a user task from the list of synchronous taskes by functor
  AREXPORT void remUserTask(ArFunctor *functor);

  /// Finds a user task by name
  AREXPORT ArSyncTask *findUserTask(const char *name);
  /// Finds a user task by functor
  AREXPORT ArSyncTask *findUserTask(ArFunctor *functor);
  
  /// Logs the list of user tasks, strictly for your viewing pleasure
  AREXPORT void logUserTasks(void) const;
  /// Logs the list of all tasks, strictly for your viewing pleasure
  AREXPORT void logAllTasks(void) const;

  /// Adds a task under the sensor interp part of the syncronous tasks
  AREXPORT bool addSensorInterpTask(const char *name, int position, 
				       ArFunctor *functor,
	       			       ArTaskState::State *state = NULL);
  /// Removes a sensor interp tasks by name
  AREXPORT void remSensorInterpTask(const char *name);
  /// Removes a sensor interp tasks by functor
  AREXPORT void remSensorInterpTask(ArFunctor *functor);

  /// Finds a task by name
  AREXPORT ArSyncTask *findTask(const char *name);
  /// Finds a task by functor
  AREXPORT ArSyncTask *findTask(ArFunctor *functor);

  /// Adds an action to the list with the given priority 
  AREXPORT bool addAction(ArAction *action, int priority);
  /// Removes an action from the list, by pointer
  AREXPORT bool remAction(ArAction *action);
  /// Removes an action from the list, by name
  AREXPORT bool remAction(const char *actionName);
  /// Returns the first (highest priority) action with the given name (or NULL)
  AREXPORT ArAction *findAction(const char *actionName);
  /// Returns the map of actions... don't do this unless you really
  /// know what you're doing
  AREXPORT ArResolver::ActionMap *getActionMap(void);
  /// Deactivates all the actions
  AREXPORT void deactivateActions(void);

  /// Logs out the actions and their priorities
  AREXPORT void logActions(bool logDeactivated = false) const;

  /// Gets the resolver the robot is using
  AREXPORT ArResolver *getResolver(void);

  /// Sets the resolver the robot is using
  AREXPORT void setResolver(ArResolver *resolver);

  /// Sets the encoderCorrectionCallback
  AREXPORT void setEncoderCorrectionCallback(
	  ArRetFunctor1<double, ArPoseWithTime> *functor);
  /// Gets the encoderCorrectionCallback
  AREXPORT ArRetFunctor1<double, ArPoseWithTime> *
          getEncoderCorrectionCallback(void) const;

  // set up some of the internals of how the ArRobot class works
  /// Sets the number of ms between cycles
  AREXPORT void setCycleTime(unsigned int ms);
  /// Gets the number of ms between cycles
  AREXPORT unsigned int getCycleTime(void) const;
  /// Sets the number of ms between cycles to warn over
  AREXPORT void setCycleWarningTime(unsigned int ms);
  /// Gets the number of ms between cycles to warn over
  AREXPORT unsigned int getCycleWarningTime(void) const;
  /// Gets the number of ms between cycles to warn over
  AREXPORT unsigned int getCycleWarningTime(void);
  /// Sets the multiplier for how many cycles ArRobot waits when connecting
  AREXPORT void setConnectionCycleMultiplier(unsigned int multiplier);
  /// Gets the multiplier for how many cycles ArRobot waits when connecting
  AREXPORT unsigned int getConnectionCycleMultiplier(void) const;
  
  /// Sets whether to chain the robot cycle to when we get in SIP packets
  void setCycleChained(bool cycleChained) { myCycleChained = cycleChained; }
  /// Gets whether we chain the robot cycle to when we get in SIP packets
  bool isCycleChained(void) const { return myCycleChained; }
  /// Sets the time without a response until connection assumed lost
  AREXPORT void setConnectionTimeoutTime(int mSecs);
  /// Gets the time without a response until connection assumed lost
  AREXPORT int getConnectionTimeoutTime(void) const;
  /// Gets the time the last packet was received
  AREXPORT ArTime getLastPacketTime(void) const;
  /// Gets the time the last odometry was received
  AREXPORT ArTime getLastOdometryTime(void) const;

  /// Sets the number of packets back in time the ArInterpolation goes
  AREXPORT void setPoseInterpNumReadings(size_t numReadings) 
    { myInterpolation.setNumberOfReadings(numReadings); }

  /// Sets the number of packets back in time the position interpol goes
  AREXPORT size_t getPoseInterpNumReadings(void) const
    { return myInterpolation.getNumberOfReadings(); }

  /// Gets the position the robot was at at the given timestamp
  /** @see ArInterpolation::getPose 
   */
  AREXPORT int getPoseInterpPosition(ArTime timeStamp, ArPose *position)
    { return myInterpolation.getPose(timeStamp, position); }

  /// Sets the number of packets back in time the ArInterpolation goes for encoder readings
  AREXPORT void setEncoderPoseInterpNumReadings(size_t numReadings) 
    { myEncoderInterpolation.setNumberOfReadings(numReadings); }

  /// Sets the number of packets back in time the encoder position interpolation goes
  AREXPORT size_t getEncoderPoseInterpNumReadings(void) const
    { return myEncoderInterpolation.getNumberOfReadings(); }

  /// Gets the encoder position the robot was at at the given timestamp
  /** @see ArInterpolation::getPose 
   */
  AREXPORT int getEncoderPoseInterpPosition(ArTime timeStamp, ArPose *position)
    { return myEncoderInterpolation.getPose(timeStamp, position); }

  /// Gets the Counter for the time through the loop
  unsigned int getCounter(void) const { return myCounter; }

  /// Gets the parameters the robot is using
  AREXPORT const ArRobotParams *getRobotParams(void) const;

  /// Gets the original robot config packet information
  AREXPORT const ArRobotConfigPacketReader *getOrigRobotConfig(void) const;

  /// Sets the maximum translational velocity
  AREXPORT void setTransVelMax(double vel);
  /// Sets the translational acceleration
  AREXPORT void setTransAccel(double acc);
  /// Sets the translational acceleration
  AREXPORT void setTransDecel(double decel);
  /// Sets the maximum rotational velocity
  AREXPORT void setRotVelMax(double vel);
  /// Sets the rotational acceleration
  AREXPORT void setRotAccel(double acc);
  /// Sets the rotational acceleration
  AREXPORT void setRotDecel(double decel);
  /// Sets the maximum lateral velocity
  AREXPORT void setLatVelMax(double vel);
  /// Sets the lateral acceleration
  AREXPORT void setLatAccel(double acc);
  /// Sets the lateral acceleration
  AREXPORT void setLatDecel(double decel);

  /// If the robot has settable maximum velocities
  bool hasSettableVelMaxes(void) const 
    { return myParams->hasSettableVelMaxes(); }
  /// Gets the maximum translational velocity
  AREXPORT double getTransVelMax(void) const;
  /// Gets the maximum rotational velocity
  AREXPORT double getRotVelMax(void) const;
  /// If the robot has settable accels and decels
  bool hasSettableAccsDecs(void)
      const { return myParams->hasSettableAccsDecs(); }
  /// Gets the translational acceleration
  AREXPORT double getTransAccel(void) const;
  /// Gets the translational acceleration
  AREXPORT double getTransDecel(void) const;
  /// Gets the rotational acceleration
  AREXPORT double getRotAccel(void) const;
  /// Gets the rotational acceleration
  AREXPORT double getRotDecel(void) const;

  /// Gets the maximum lateral velocity
  AREXPORT double getLatVelMax(void) const;
  /// Gets the lateral acceleration
  AREXPORT double getLatAccel(void) const;
  /// Gets the lateral acceleration
  AREXPORT double getLatDecel(void) const;

  /// Loads a parameter file (replacing all other params)
  AREXPORT bool loadParamFile(const char *file);
  /// Sets the robot to use a passed in set of params (passes ownership)
  AREXPORT void setRobotParams(ArRobotParams *params);

  /// Attachs a key handler
  AREXPORT void attachKeyHandler(ArKeyHandler *keyHandler,
				 bool exitOnEscape = true,
				 bool useExitNotShutdown = true);
  /// Gets the key handler attached to this robot
  AREXPORT ArKeyHandler *getKeyHandler(void) const;

  /// Lock the robot instance
  int lock() {return(myMutex.lock());}
  /// Try to lock the robot instance without blocking
  int tryLock() {return(myMutex.tryLock());}
  /// Unlock the robot instance
  int unlock() {return(myMutex.unlock());}
  /// Turn on verbose locking of robot mutex
  void setMutexLogging(bool v) { myMutex.setLog(v); }
  /// Set robot lock warning time (see ArMutex::setLockWarningTime())
  void setMutexLockWarningTime(double sec) { myMutex.setLockWarningTime(sec); }
  /// Set robot lock-unlock warning time (see ArMutex::setUnlockWarningTime())
  void setMutexUnlockWarningTime(double sec) { myMutex.setUnlockWarningTime(sec); }

  /// This tells us if we're in the preconnection state
  AREXPORT bool isStabilizing(void) { return myIsStabilizing; }

  /// How long we should stabilize for in ms (0 disables stabilizing)
  AREXPORT void setStabilizingTime(int mSecs);

  /// How long we stabilize for in ms (0 means no stabilizng)
  AREXPORT int getStabilizingTime(void) const;


  /// Adds a callback called when the robot starts stabilizing before declaring connection
  AREXPORT void addStabilizingCB(ArFunctor *functor, 
			     ArListPos::Pos position = ArListPos::LAST);
  /// Removes stabilizing callback
  AREXPORT void remStabilizingCB(ArFunctor *functor);
  
  /// This gets the root of the syncronous task tree, only serious 
  /// developers should use it
  AREXPORT ArSyncTask *getSyncTaskRoot(void);

  /// This function loops once...  only serious developers should use it
  AREXPORT void loopOnce(void);

  /// Sets the delay in the odometry readings
  /**
     Note that this doesn't cause a delay, its informational so that
     the delay can be adjusted for and causes nothing to happen in
     this class.
  **/
  AREXPORT void setOdometryDelay(int msec) { myOdometryDelay = msec; }

  /// Gets the delay in odometry readings
  /**
     This gets the odometry delay, not that this is just information
     about what the delay is it doesn't cause anything to happen in
     this class.
  **/
  AREXPORT int getOdometryDelay(void) { return myOdometryDelay; }

  /// Gets if we're logging all the movement commands sent down
  AREXPORT bool getLogMovementSent(void) { return myLogMovementSent; }
  /// Sets if we're logging all the movement commands sent down
  AREXPORT void setLogMovementSent(bool logMovementSent)
    { myLogMovementSent = logMovementSent; }

  /// Gets if we're logging all the positions received from the robot
  AREXPORT bool getLogMovementReceived(void) { return myLogMovementReceived; }
  /// Sets if we're logging all the positions received from the robot
  AREXPORT void setLogMovementReceived(bool logMovementReceived)
    { myLogMovementReceived = logMovementReceived; }

  /// Gets if we're logging all the velocities (and heading) received
  AREXPORT bool getLogVelocitiesReceived(void) 
    { return myLogVelocitiesReceived; }
  /// Sets if we're logging all the velocities (and heading) received
  AREXPORT void setLogVelocitiesReceived(bool logVelocitiesReceived) 
    { myLogVelocitiesReceived = logVelocitiesReceived; }

  /// Gets if we're logging all the packets received (just times and types)
  AREXPORT bool getPacketsReceivedTracking(void)
    { return myPacketsReceivedTracking; }
  /// Sets if we're logging all the packets received (just times and types)
  AREXPORT void setPacketsReceivedTracking(bool packetsReceivedTracking);

  /// Gets if we're logging all the packets sent and their payload
  AREXPORT bool getPacketsSentTracking(void)
    { return myPacketsSentTracking; }
  /// Sets if we're logging all the packets sent and their payloads
  AREXPORT void setPacketsSentTracking(bool packetsSentTracking)
    { myPacketsSentTracking = packetsSentTracking; }

  /// Gets if we're logging all the actions as they happen
  AREXPORT bool getLogActions(void) { return myLogActions; }
  /// Sets if we're logging all the actions as they happen
  AREXPORT void setLogActions(bool logActions)
  { myLogActions = logActions; }

  /// Sets the flag that controls if the robot will switch baud rates
  AREXPORT void setDoNotSwitchBaud(bool doNotSwitchBaud)
    { myDoNotSwitchBaud = true; } 

  /// Gets the flag that controls if the robot won't switch baud rates
  AREXPORT bool getDoNotSwitchBaud(void) { return myDoNotSwitchBaud; }
  
  /// This is only for use by syncLoop
  AREXPORT void incCounter(void) { myCounter++; }

  /// Packet Handler, internal 
  AREXPORT void packetHandler(void);
  /// Action Handler, internal
  AREXPORT void actionHandler(void);
  /// State Reflector, internal
  AREXPORT void stateReflector(void);
  /// Robot locker, internal
  AREXPORT void robotLocker(void);
  /// Robot unlocker, internal
  AREXPORT void robotUnlocker(void);

  /// For the key handler, escape calls this to exit, internal
  AREXPORT void keyHandlerExit(void);

  /// Processes a motor packet, internal  
  AREXPORT bool processMotorPacket(ArRobotPacket *packet);
  /// Processes a new sonar reading, internal
  AREXPORT void processNewSonar(char number, int range, ArTime timeReceived);
  /// Processes a new encoder packet, internal
  AREXPORT bool processEncoderPacket(ArRobotPacket *packet);
  /// Processes a new IO packet, internal
  AREXPORT bool processIOPacket(ArRobotPacket *packet);
  
  /// Internal function, shouldn't be used
  AREXPORT void init(void);

  /// Internal function, shouldn't be used, sets up the default sync list
  AREXPORT void setUpSyncList(void);
  /// Internal function, shouldn't be used, sets up the default packet handlers
  AREXPORT void setUpPacketHandlers(void);

  ArRetFunctor1C<bool, ArRobot, ArRobotPacket *> myMotorPacketCB;
  ArRetFunctor1C<bool, ArRobot, ArRobotPacket *> myEncoderPacketCB;
  ArRetFunctor1C<bool, ArRobot, ArRobotPacket *> myIOPacketCB;
  ArFunctorC<ArRobot> myPacketHandlerCB;
  ArFunctorC<ArRobot> myActionHandlerCB;
  ArFunctorC<ArRobot> myStateReflectorCB;
  ArFunctorC<ArRobot> myRobotLockerCB;
  ArFunctorC<ArRobot> myRobotUnlockerCB;
  ArFunctorC<ArRobot> myKeyHandlerExitCB;
  ArFunctorC<ArKeyHandler> *myKeyHandlerCB;

  // These four are internal... only people monkeying deeply should mess
  // with them, so they aren't documented... these process the cblists
  // and such
  /// Internal function, shouldn't be used, does a single run of connecting
  AREXPORT int asyncConnectHandler(bool tryHarderToConnect);

  /// Internal function, shouldn't be used, drops the conn because of error
  AREXPORT void dropConnection(void);
  /// Internal function, shouldn't be used, denotes the conn failed
  AREXPORT void failedConnect(void);
  /// Internal function, shouldn't be used, does the initial conn stuff
  AREXPORT bool madeConnection(void);
  /// Internal function, shouldn't be used, calls the preconnected stuff
  AREXPORT void startStabilization(void);
  /// Internal function, shouldn't be used, does the after conn stuff
  AREXPORT void finishedConnection(void);
  /// Internal function, shouldn't be used, cancels the connection quietly
  AREXPORT void cancelConnection(void);

  /// Internal function, takes a packet and passes it to the packet handlers,
  /// returns true if handled, false otherwise
  AREXPORT bool handlePacket(ArRobotPacket *packet);
  /// Internal function, shouldn't be used, does what its name says
  AREXPORT std::list<ArFunctor *> * getRunExitListCopy();
  /// Internal function, processes a parameter file
  AREXPORT void processParamFile(void);

  /** @brief Get the position of the robot according to the last robot SIP only,
   *  with no correction by the gyro, other devices or software proceses.
   *
   * @note For the most accurate pose, use getPose() or getEncoderPose(); 
   * only use this method if you must have raw encoder pose with no correction.
   * @sa getPose()
   * @sa getEncoderPose()
   */
  AREXPORT ArPose getRawEncoderPose(void) const { return myRawEncoderPose; }

  /// Internal function for sync loop and sync task to see if we should warn this cycle or not
  AREXPORT bool getNoTimeWarningThisCycle(void)  
    { return myNoTimeWarningThisCycle; }
  /// Internal function for sync loop and sync task to say if we should warn this cycle or not
  AREXPORT void setNoTimeWarningThisCycle(bool noTimeWarningThisCycle)  
    { myNoTimeWarningThisCycle = noTimeWarningThisCycle; }
  // callbacks for warning time and if we should warn now to pass to sync tasks
  ArRetFunctorC<unsigned int, ArRobot> myGetCycleWarningTimeCB;
  ArRetFunctorC<bool, ArRobot> myGetNoTimeWarningThisCycleCB;
  /// internal function called when Aria::exit is called
  AREXPORT void ariaExitCallback(void);
  /// internal call that will let the robot connect even if it can't find params
  void setConnectWithNoParams(bool connectWithNoParams) 
    { myConnectWithNoParams = connectWithNoParams; }
  /// Internal call that will get the thread the robot is running in
  ArThread::ThreadType getOSThread(void) { return mySyncLoop.getOSThread(); }
protected:
  enum RotDesired { 
    ROT_NONE,
    ROT_IGNORE,
    ROT_HEADING,
    ROT_VEL
  };
  enum TransDesired {
    TRANS_NONE,
    TRANS_IGNORE,
    TRANS_VEL,
    TRANS_VEL2,
    TRANS_DIST,
    TRANS_DIST_NEW
  };
  enum LatDesired {
    LAT_NONE,
    LAT_IGNORE,
    LAT_VEL
  };
  void reset(void);
 

  // the config the robot had at connection
  ArRobotConfigPacketReader *myOrigRobotConfig;
  // the values we'll maintain for the different motion parameters
  double myRotVelMax;
  double myRotAccel;
  double myRotDecel;
  double myTransVelMax;
  double myTransAccel;
  double myTransDecel;
  double myLatVelMax;
  double myLatAccel;
  double myLatDecel;

  ArPTZ *myPtz;
  bool myNoTimeWarningThisCycle;

  long int myLeftEncoder;
  long int myRightEncoder;
  bool myFirstEncoderPose;
  ArPoseWithTime myRawEncoderPose;

  ArTransform myEncoderTransform;

  bool myLogMovementSent;
  bool myLogMovementReceived;
  bool myPacketsReceivedTracking;
  bool myLogActions;
  bool myLogVelocitiesReceived;
  double myLastVel;
  double myLastRotVel;
  double myLastHeading;
  double myLastCalculatedRotVel;
  double myLastLatVel;

  bool myTryingToMove;

  long myPacketsReceivedTrackingCount;
  ArTime myPacketsReceivedTrackingStarted;
  bool myPacketsSentTracking;
  ArMutex myMutex;
  ArSyncTask *mySyncTaskRoot;
  std::list<ArRetFunctor1<bool, ArRobotPacket *> *> myPacketHandlerList;

  ArSyncLoop mySyncLoop;

  std::list<ArFunctor *> myStabilizingCBList;
  std::list<ArFunctor *> myConnectCBList;
  std::list<ArFunctor *> myFailedConnectCBList;
  std::list<ArFunctor *> myDisconnectNormallyCBList;
  std::list<ArFunctor *> myDisconnectOnErrorCBList;
  std::list<ArFunctor *> myRunExitCBList;

  ArRetFunctor1<double, ArPoseWithTime> *myEncoderCorrectionCB;
  std::list<ArRangeDevice *> myRangeDeviceList;
  std::map<int, ArLaser *> myLaserMap;

  ArCondition myConnectCond;
  ArCondition myConnOrFailCond;
  ArCondition myRunExitCond;

  ArResolver::ActionMap myActions;
  bool myOwnTheResolver;
  ArResolver *myResolver;

  std::map<int, ArSensorReading *> mySonars;
  int myNumSonar;
  
  unsigned int myCounter;
  bool myIsConnected;
  bool myIsStabilizing;

  bool myBlockingConnectRun;
  bool myAsyncConnectFlag;
  int myAsyncConnectState;
  int myAsyncConnectNoPacketCount;
  int myAsyncConnectTimesTried;
  ArTime myAsyncStartedConnection;
  int myAsyncConnectStartBaud;
  ArTime myAsyncConnectStartedChangeBaud;
  bool myAsyncConnectSentChangeBaud;
  ArTime myStartedStabilizing;

  int myStabilizingTime;

  bool mySentPulse;

  double myTransVal;
  double myTransVal2;
  int myLastTransVal;
  int myLastTransVal2;
  TransDesired myTransType;
  TransDesired myLastTransType;
  ArTime myTransSetTime;
  ArTime myLastTransSent;
  int myLastActionTransVal;
  bool myActionTransSet;
  ArPose myTransDistStart;
  double myMoveDoneDist;

  double myRotVal;
  int myLastRotVal;
  RotDesired myRotType;
  RotDesired myLastRotType;
  ArTime myRotSetTime;
  ArTime myLastRotSent;
  int myLastActionRotVal;
  bool myLastActionRotHeading;
  bool myLastActionRotStopped;
  bool myActionRotSet;
  double myHeadingDoneDiff;

  double myLatVal;
  int myLastLatVal;
  LatDesired myLatType;
  LatDesired myLastLatType;
  ArTime myLatSetTime;
  ArTime myLastLatSent;
  int myLastActionLatVal;
  bool myActionLatSet;

  double myLastSentTransVelMax;
  double myLastSentTransAccel;
  double myLastSentTransDecel;
  double myLastSentRotVelMax;
  double myLastSentRotAccel;
  double myLastSentRotDecel;
  double myLastSentLatVelMax;
  double myLastSentLatAccel;
  double myLastSentLatDecel;

  ArTime myLastPulseSent;

  int myDirectPrecedenceTime;
  
  int myStateReflectionRefreshTime;

  ArActionDesired myActionDesired;

  std::string myName;
  std::string myRobotName;
  std::string myRobotType;
  std::string myRobotSubType;

  double myAbsoluteMaxTransVel;
  double myAbsoluteMaxTransAccel;
  double myAbsoluteMaxTransDecel;
  double myAbsoluteMaxRotVel;
  double myAbsoluteMaxRotAccel;
  double myAbsoluteMaxRotDecel;
  double myAbsoluteMaxLatVel;
  double myAbsoluteMaxLatAccel;
  double myAbsoluteMaxLatDecel;

  ArDeviceConnection *myConn;

  ArRobotPacketSender mySender;
  ArRobotPacketReceiver myReceiver;

  ArRobotParams *myParams;
  double myRobotLengthFront;
  double myRobotLengthRear;

  ArInterpolation myInterpolation;
  ArInterpolation myEncoderInterpolation;

  ArKeyHandler *myKeyHandler;
  bool myKeyHandlerUseExitNotShutdown;

  bool myConnectWithNoParams;
  bool myWarnedAboutExtraSonar;

  // variables for tracking the data stream
  time_t myTimeLastMotorPacket;
  int myMotorPacCurrentCount;
  int myMotorPacCount;
  time_t myTimeLastSonarPacket;
  int mySonarPacCurrentCount;
  int mySonarPacCount;
  unsigned int myCycleTime;
  unsigned int myCycleWarningTime;
  unsigned int myConnectionCycleMultiplier;
  bool myCycleChained;
  ArTime myLastPacketReceivedTime;
  ArTime myLastOdometryReceivedTime;
  int myTimeoutTime;

  bool myRequestedIOPackets;
  bool myRequestedEncoderPackets;

  // all the state reflecing variables
  ArPoseWithTime myEncoderPose;
  ArTime myEncoderPoseTaken;
  ArPose myGlobalPose;
  // um, this myEncoderGlobalTrans doesn't do anything
  ArTransform myEncoderGlobalTrans;
  double myLeftVel;
  double myRightVel;
  double myBatteryVoltage;
  ArRunningAverage myBatteryAverager;
  double myRealBatteryVoltage;
  ArRunningAverage myRealBatteryAverager;
  bool myHaveStateOfCharge;
  double myStateOfCharge;
  ArTime myStateOfChargeSetTime;
  double myStateOfChargeLow;
  double myStateOfChargeShutdown;

  int myStallValue;
  double myControl;
  int myFlags;
  int myFaultFlags;
  bool myHasFaultFlags;
  double myCompass;
  int myAnalogPortSelected;
  unsigned char myAnalog;
  unsigned char myDigIn;
  unsigned char myDigOut;
  int myIOAnalog[128];
  unsigned char myIODigIn[255];
  unsigned char myIODigOut[255];
  int myIOAnalogSize;
  int myIODigInSize;
  int myIODigOutSize;
  ArTime myLastIOPacketReceivedTime;
  double myVel;
  double myRotVel;
  double myLatVel;
  int myLastX;
  int myLastY;
  int myLastTh;
  ChargeState myChargeState;
  bool myOverriddenChargeState;
  
  char myTemperature;

  double myTripOdometerDistance;
  double myTripOdometerDegrees;
  ArTime myTripOdometerStart;

  double myOdometerDistance;
  double myOdometerDegrees;
  ArTime myOdometerStart;

  int myOdometryDelay;

  bool myDoNotSwitchBaud;
  
  bool myAddedAriaExitCB;
  ArFunctorC<ArRobot> myAriaExitCB;
};


#endif // ARROBOT_H