/* +---------------------------------------------------------------------------+
   |          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 CBoardSonars_H
#define CBoardSonars_H

#include <lib_hwdrivers/CInterfaceFTDIMessages.h>
#include <MRPT/UTILS/SYNCH.h>
#include <MRPT/UTILS/CDebugOutputCapable.h>
#include <MRPT/UTILS/CConfigFileBase.h>
#include <MRPT/MRML/CObservationRange.h>


namespace HWDRIVERS
{
	/**   This "software driver" implements the communication protocol for interfacing a Ultrasonic range finder SRF10 through a custom USB board.
	  *
      *   In this class the "bind" is ignored since it is designed for USB connections only, thus it internally generate the required object for simplicity of use.
	  *   The serial number of the USB device is used to open it on the first call to "doProcess", thus you must call "loadConfig" before this, or manually
	  *     call "setDeviceSerialNumber". The default serial number is "SONAR001"
	  *
	  * Warning: Avoid defining an object of this class in a global scope if you want to catch all potential
	  *      exceptions during the constructors (like USB interface DLL not found, etc...)
	  *
	  *  \code
	  *  PARAMETERS IN THE ".INI"-LIKE CONFIGURATION STRINGS:
	  * -------------------------------------------------------
	  *   [supplied_section_name]
	  *   USB_serialNumber=SONAR001
	  *   gain=6			; Value between 0 and 16, for analog gains between 40 and 700.
	  *   maxRange=4.0		; In meters, used for device internal timer.
	  *   minTimeBetweenPings=0.3  ; In seconds
	  *
	  *   ; The order in which sonars will be fired, indexed by their I2C addresses [0,15]
	  *   ;  Up to 16 devices, but you can put any number of devices (from 1 to 16).
	  *   firingOrder=0 1 2 3
	  *
	  *
	  *		¡¡¡FALTA PONER UNA ESTRUCTURILLA PARA ALMACENAR LA POSE3D DE LOS 16 SONARES SOBRE EL ROBOT!!!
	  *
	  *  \endcode
	  *
	  */
	class CBoardSonars : public HWDRIVERS::CInterfaceFTDIMessages
	{
		protected:
			/** A copy of the device serial number (to open the USB FTDI chip)
			  */
			std::string		m_usbSerialNumber;

			/** A value between 0 and 16, for gains between 40 and 700 (not linear).
			  */
			uint8_t			m_gain;

			/** The maximum range in meters, used for the internal device timer (value between 4cm and 11m).
			  */
			float			m_maxRange;

			/** The order in which sonars will be fired, indexed by their I2C addresses [0,15].
			  *  Up to 16 devices, but you can put any number of devices (from 1 to 16).
			  */
			std::vector<int>		m_firingOrder;

			/** The minimum time between sonar pings (in seconds).
			  */
			float			m_minTimeBetweenPings;

			/** Tries to connect to the USB device (if disconnected).
			  * \return True on connection OK, false on error.
			  */
			bool	checkConnectionAndConnect();

			/** Sends the configuration (max range, gain,...) to the USB board. Used internally after a successfull connection.
			  * \return true on success, false on communications errors or device not found.
			  */
			bool	sendConfigCommands();

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

			/** Destructor
			  */
			~CBoardSonars(){}

			/** 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::CBoardSonars for the possible parameters
		      */
			void  loadConfig(	const CConfigFileBase *configSource,
								const std::string	  &iniSection );

			/** Query the firmware version on the device (can be used to test communications).
			  * \return true on success, false on communications errors or device not found.
			  */
			bool queryFirmwareVersion( std::string &out_firmwareVersion );

			/** Request the latest range measurements.
			  * \return true on success, false on communications errors or device not found.
			  */
			bool getObservation( MRML::CObservationRange &obs );

			/** Requests a command of "change address" for a given SRF10 device.
			  *  currentAddress and newAddress are the I2C addresses in the range 0 to 15 (mapped to 0xE0 to 0xFE internally).
			  * \return true on success, false on communications errors or device not found.
			  */
			bool programI2CAddress( uint8_t currentAddress, uint8_t newAddress );

	};	// End of class

} // End of namespace


#endif
