/*---------------------------------------------------------------
	FILE: CHokuyoURG.h
	USE: See doxygen doc.

   Part of the MRPT Library
   ISA - Universidad de Malaga - http://www.isa.uma.es
  ---------------------------------------------------------------*/
#ifndef CHokuyoURG_H
#define CHokuyoURG_H

#include <MRPT/MRML/CPose3D.h>
#include <MRPT/HWDRIVERS/C2DRangeFinderAbstract.h>

namespace HWDRIVERS
{
	/** This "software drivers" implements the protocol SCIP-2.0 for interfacing HOKUYO URG laser scanners.
	  *
	  *
	  *  \code
	  *  PARAMETERS IN THE ".INI"-LIKE CONFIGURATION STRINGS:
	  * -------------------------------------------------------
	  *   [supplied_section_name]
	  *	   HOKUYO_firstRange=44
	  *    HOKUYO_lastRange=725
	  *    HOKUYO_motorSpeed_rpm=600
	  *    COM_port=3
	  *    pose_x=0.21	; Laser range scaner 3D position in the robot (meters)
	  *    pose_y=0
	  *    pose_z=0.34
	  *    pose_yaw=0	; Angles in degrees
	  *    pose_pitch=0
	  *    pose_roll=0
	  *
	  *  \endcode
	  */
	class CHokuyoURG : public C2DRangeFinderAbstract
	{
	private:
		/** The first and last ranges to consider from the scan.
		  */
		int		m_firstRange,m_lastRange;

		/** The motor speed (default=600rpm)
		  */
		int		m_motorSpeed_rpm;

		/** The sensor 6D pose:
		  */
		MRML::CPose3D	m_sensorPose;

		/** Enables the SCIP2.0 protocol (this must be called at the very begining!).
		  * \return false on any error
		  */
		bool  enableSCIP20();

		/** Passes to 115200bps bitrate.
		  * \return false on any error
		  */
		bool  setHighBaudrate();

		/** Switchs the laser on.
		  * \return false on any error
		  */
		bool  switchLaserOn();

		/** Switchs the laser off
		  * \return false on any error
		  */
		bool  switchLaserOff();

		/** Changes the motor speed in rpm's (default 600rpm)
		  * \return false on any error
		  */
		bool  setMotorSpeed(int motoSpeed_rpm);

		/** Ask to the device, and print to the debug stream, details about the firmware version,serial number,...
		  * \return false on any error
		  */
		bool  displayVersionInfo();

		/** Ask to the device, and print to the debug stream, details about the sensor model.
		  * \return false on any error
		  */
		bool  displaySensorInfo();

		/** Start the scanning mode, using parameters stored in the object (loaded from the .ini file)
		  * After this command the device will start to send scans until "switchLaserOff" is called.
		  * \return false on any error
		  */
		bool  startScanningMode();

		/** Waits for a response from the device.
		  * \return false on any error
		  */
		bool  receiveResponse(
				char	*sentCmd_forEchoVerification,
				char	&rcv_status0,
				char	&rcv_status1,
				char	*rcv_data,
				int		&rcv_dataLength);


	public:
		/** Constructor
		  */
		CHokuyoURG();

		/** Specific laser scanner "software drivers" must process here new data from the I/O stream, and, if a whole scan has arrived, return it.
		  *  This method will be typically called in a different thread than other methods, and will be called in a timely fashion.
		  */
		void  doProcess(
			bool							&outThereIsObservation,
			MRML::CObservation2DRangeScan	&outObservation,
			bool							&hardwareError );

		/** Loads specific configuration for the device from a given source of configuration parameters, for example, an ".ini" file, loading from the section "[iniSection]" (see UTILS::CConfigFileBase and derived classes)
		  *  See HWDRIVERS::CHokuyoURG for the possible parameters
		  */
		void  loadConfig(
			const CConfigFileBase *configSource,
			const std::string	  &iniSection );

		/** Enables the scanning mode (which may depend on the specific laser device); this must be called before asking for observations to assure that the protocol has been initializated.
		  * \return If everything works "true", or "false" if there is any error.
		  */
		bool  turnOn();

		/** Disables the scanning mode (this can be used to turn the device in low energy mode, if available)
		  * \return If everything works "true", or "false" if there is any error.
		  */
		bool  turnOff();

	};	// End of class

} // End of namespace


#endif
