/*

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 ARLMS2XX_H
#define ARLMS2XX_H


#include "ariaTypedefs.h"
#include "ArLMS2xxPacket.h"
#include "ArLMS2xxPacketReceiver.h"
#include "ArRobotPacket.h"
#include "ArLaser.h"   
#include "ArFunctor.h"
#include "ArCondition.h"

/// Interface to a SICK LMS-200 laser range device
/**
 * This class processes incoming data from a SICK LMS-200
 * laser rangefinding device in a background thread, and provides
 * it through the standard ArRangeDevice API, to be used via ArRobot
 * (see ArRobot::addRangeDevice()), used by an ArAction, or used directly.
 *
 * An ArSick instance must be connected to the laser through a serial port
 * (or simulator): the typical procedure is to allow your ArSimpleConnector
 * to configure the laser based on the robot connection type and command
 * line parameters; then initiate the ArSick background thread; and finally
 * connect ArSick to the laser device.
 * For example:
 * @code
 *  ArRobot robot;
 *  ArSick laser;
 *  ArSimpleConnector connector(...);
 *  ...
 *   Setup the simple connector and connect to the robot --
 *   see the example programs.
 *  ...
 *  connector.setupLaser(&laser);
 *  laser.runAsync();
 *  if(!laser.blockingConnect())
 *  {
 *    // Error...
 *    ...
 *  }
 *  ...
 * @endcode
 *
 * The most important methods in this class are the constructor, runAsync(), 
 * blockingConnect(), getSensorPosition(), isConnected(), addConnectCB(),
 * asyncConnect(), configure(), in addition to the ArRangeDevice interface. 
 *
 * @note The "extra int" on the raw readings returned by
 * ArRangeDevice::getRawReadings() is like other laser
 * devices and is the reflectance value, if enabled, ranging between 0 and 255.
**/
class ArLMS2xx : public ArLaser
{
public:
  /// Constructor
  AREXPORT ArLMS2xx(int laserNumber,
		    const char *name = "lms2xx",
		    bool appendLaserNumberToName = true);

  /// Destructor
  AREXPORT virtual ~ArLMS2xx();

  /// Connect to the laser while blocking
  AREXPORT virtual bool blockingConnect(void);
  /// Connect to the laser asyncronously
  AREXPORT bool asyncConnect(void);
  /// Disconnect from the laser
  AREXPORT virtual bool disconnect(void);
  /// Sees if this is connected to the laser
  AREXPORT virtual bool isConnected(void) 
    { if (myState == STATE_CONNECTED) return true; else return false; }
  AREXPORT virtual bool isTryingToConnect(void) 
    { 
      if (myState != STATE_CONNECTED && myState != STATE_NONE) 
	return true; 
      else if (myStartConnect)
	return true;
      else 
	return false; 
    }

  /// Sets the device connection
  AREXPORT virtual void setDeviceConnection(ArDeviceConnection *conn);

  /** The internal function used by the ArRangeDeviceThreaded
   *  @internal
   */
  AREXPORT virtual void * runThread(void *arg);
  AREXPORT virtual void setRobot(ArRobot *robot);
protected:
  // The packet handler for when connected to the simulator
  AREXPORT bool simPacketHandler(ArRobotPacket * packet);
  // The function called if the laser isn't running in its own thread and isn't simulated
  AREXPORT void sensorInterpCallback(void);
  // An internal function for connecting to the sim
  AREXPORT bool internalConnectSim(void);
  /// An internal function, single loop event to connect to laser
  AREXPORT int internalConnectHandler(void);
  // The internal function which processes the sickPackets
  AREXPORT void processPacket(ArLMS2xxPacket *packet, ArPose pose, 
			      ArPose encoderPose, unsigned int counter,
			      bool deinterlace, ArPose deinterlaceDelta);
  // The internal function that gets does the work
  AREXPORT void runOnce(bool lockRobot);
  // 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 after conn stuff
  AREXPORT void madeConnection(void);

  /// Internal function that gets whether the laser is simulated or not (just for the old ArSick)
  AREXPORT bool sickGetIsUsingSim(void);

  /// Internal function that sets whether the laser is simulated or not (just for the old ArSick)
  AREXPORT void sickSetIsUsingSim(bool usingSim);

  /// internal function to runOnRobot so that ArSick can do that while this class won't
  AREXPORT bool internalRunOnRobot(void);

  /// Finishes getting the unset parameters from the robot then
  /// setting some internal variables that need it
  bool finishParams(void);

  AREXPORT virtual bool laserCheckParams(void);

  AREXPORT virtual void laserSetName(const char *name);

  enum State {
    STATE_NONE, ///< Nothing, haven't tried to connect or anything
    STATE_INIT, ///< Initializing the laser
    STATE_WAIT_FOR_POWER_ON, ///< Waiting for power on
    STATE_CHANGE_BAUD, ///< Change the baud, no confirm here
    STATE_CONFIGURE, ///< Send the width and increment to the laser
    STATE_WAIT_FOR_CONFIGURE_ACK, ///< Wait for the configuration Ack
    STATE_INSTALL_MODE, ///< Switch to install mode
    STATE_WAIT_FOR_INSTALL_MODE_ACK, ///< Wait until its switched to install mode
    STATE_SET_MODE, ///< Set the mode (mm/cm) and extra field bits
    STATE_WAIT_FOR_SET_MODE_ACK, ///< Waiting for set-mode ack
    STATE_START_READINGS, ///< Switch to monitoring mode
    STATE_WAIT_FOR_START_ACK, ///< Waiting for the switch-mode ack
    STATE_CONNECTED ///< We're connected and getting readings
  };
  /// Internal function for switching states
  AREXPORT void switchState(State state);
  State myState;
  ArTime myStateStart;
  ArFunctorC<ArLMS2xx> myRobotConnectCB;
  ArRetFunctor1C<bool, ArLMS2xx, ArRobotPacket *> mySimPacketHandler;
  ArFunctorC<ArLMS2xx> mySensorInterpCB;
  std::list<ArSensorReading *>::iterator myIter;
  bool myStartConnect;
  bool myRunningOnRobot;

  // range buffers to hold current range set and assembling range set
  std::list<ArSensorReading *> *myAssembleReadings;
  std::list<ArSensorReading *> *myCurrentReadings;

  bool myProcessImmediately;
  bool myInterpolation;
  // list of packets, so we can process them from the sensor callback
  std::list<ArLMS2xxPacket *> myPackets;

  // these two are just for the sim packets
  unsigned int myWhichReading;
  unsigned int myTotalNumReadings;

  // some variables so we don't have to do a tedios if every time
  double myOffsetAmount;
  double myIncrementAmount;

  // packet stuff
  ArLMS2xxPacket myPacket;
  bool myRunInOwnThread;
  bool myUseSim;
  
  int myNumReflectorBits;
  bool myInterlaced;

  // stuff for the sim packet
  ArPose mySimPacketStart;
  ArTransform mySimPacketTrans;
  ArTransform mySimPacketEncoderTrans;
  unsigned int mySimPacketCounter;

  // connection
  ArLMS2xxPacketReceiver myLMS2xxPacketReceiver;

  ArMutex myStateMutex;
  ArRetFunctorC<bool, ArLMS2xx> myAriaExitCB;
};


#endif 
