/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                   http://mrpt.sourceforge.net/                            |
   |                                                                           |
   |   Copyright (C) 2005-2008  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Perception and Robotics               |
   |      research group, University of Malaga (Spain).                        |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT 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 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT 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 MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */
#ifndef CHokuyoURG_H
#define CHokuyoURG_H

#include <MRPT/MRML/CPose3D.h>
#include <lib_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;

		std::vector<char>   m_rx_buffer; //!< Temporal incomming buffer.
		int                 m_rx_idx,m_wr_idx; //!< Read pointers within m_rx_buffer

		std::string     m_lastSentMeasCmd; //!< The last sent measurement command (MDXXX), including the last 0x0A.

		/** 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(
				const char	*sentCmd_forEchoVerification,
				char	&rcv_status0,
				char	&rcv_status1,
				char	*rcv_data,
				int		&rcv_dataLength);


        /** Assures a minimum number of bytes in the input buffer, reading from the serial port only if required.
          * \return false if the number of bytes are not available, even after trying to fetch more data from the serial port.
          */
        bool assureBufferHasBytes(const int &nDesiredBytes);

	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
