/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#pragma once

#include <mrpt/hwdrivers/CGenericSensor.h>
#include <mrpt/obs/CObservationRange.h>
#include <mrpt/poses/CPoint3D.h>
#include <mrpt/system/COutputLogger.h>

namespace mrpt::hwdrivers
{
/** \brief : An interface for the phidget Interface kit board (1018).
 *  \class CPhidgetInterfaceKitProximitySensors
 *  \author Adrien BARRAL - Robopec (aba@robopec.com).
 *
 * An interface for the Phidgets Interface kit board (part number 1018) on wich
 * it could be plugged either an Sharp IR adaptater board
 * (phidget's part number : 1101),or a MaxBotix EZ-1 sonar (phidget's part
 * number : 1118).
 * The configuration file describe what is plugged to this board, and the
 * geometry of the sensors on the robots. See the exemple below.
 * \code
 * [PhidgetInterfaceKitProximitySensors]
 * sensorLabel = FrontProximitySensors
 * process_rate = 100 						// Integer value in Hz (common to
 * all
 * sensors),
 * default value is 50Hz.
 * displayRecapitulativeInformations = true	// default value = false.
 * serialNumber = 12345						// The interface kit serial number
 * (Integer
 * value),
 * default value is -1.
 * sensor1 = SHARP-30cm						// sharp InfraRed sensor 30cm range
 * (string
 * value).
 * capital to convert raw data to range data (in meters).
 * pose1_x = 0								// position on the robot (float
 * value
 * in
 * meters)
 * pose1_y = 0
 * pose1_z = 0.5
 * pose1_yaw = -45.0     					// Angles in degrees (float value).
 * pose1_pitch = 0
 * pose1_roll = 0
 *  //...
 * sensorn = EZ1							// Maxbotix Ultrasound sonar
 * posen_x = 0
 * // ...
 * \endcode
 *
 * The maximum number of sensors on this board is 8. Sensor 1 is the first
 * sensor. If you haven't plugged any sensor on an entry of the board, you
 * haven't to specify
 * anyithing about this sensor in the configuration file.
 * The following table enumerate the different sensors supported by this class.
 * \latexonly
 * \begin{tabular}{|c|c|c}
 * 		\hline
 * 		Part Number & Config file indentifiant & IR or US
 * 		\hline
 * 		MaxBotix EZ-1 Sonar Sensor & EZ1 & US \\
 * 		GP2D12 & SHARP-30cm & IR \\
 * 		GP2Y0A21** & SHARP-80cm & IR \\
 * 		\hline
 * \end{tabular}
 *
 * This isn't an event based implementation of the phidget library. That means
 * that when an instanciation of a CPhidgetInterfaceKitProximitySensors is done,
 * the constructor will block during
 * in the worst case 200ms, if the board isn't found, an exception will be
 * thrown.
 * mrpt::obs::CObservation returned by this class is a CObservationRange.
 * CObservationrange::minSensorDistance will be the minimum of the minimum of
 * the sensor distances, e.g if you plug to the interface
 * kit a GP2D12 (min range 4 cm) and a GP2Y0A21 (min range 8 cm), then
 * CObservationrange::minSensorDistance = min(0.04,0.08) = 0.04. Respectively
 * for the maximal range.
 * \endlatexonly
 * \warning{The Phidget library use udev. By default, udev require to be root to
 * be launched, if you want to be able to run a program wich use a phidget board
 * without be root, you must modify files in /etc/udev/rules.d .}
 * \ingroup mrpt_hwdrivers_grp
 */
enum SensorType
{
	SHARP_30cm,
	SHARP_80cm,
	EZ1,
	UNPLUGGED
};

class CPhidgetInterfaceKitProximitySensors : public mrpt::system::COutputLogger,
											 public CGenericSensor
{
	DEFINE_GENERIC_SENSOR(CPhidgetInterfaceKitProximitySensors)

   public:
	/** Constructor
	 * \param serialNumber The board's serial number. Set -1 to choose the
	 * first available board
	 */
	CPhidgetInterfaceKitProximitySensors();

	/** Destructor
	 */
	~CPhidgetInterfaceKitProximitySensors() override;

	/** This method tries to get a set of range measurements from the IR
	 * sensors.
	 * \param outThereIsObservation Will be true if an observation was
	 * sucessfully received.
	 */
	void getObservation(mrpt::obs::CObservationRange& outObservation);
	/** Initialize the sensor according to the parameters previously read in the
	 * configuration file.
	 * \exception throw an exception if the board could not be found.
	 * \exception throw an exception if the process rate can't be set on one of
	 * the board channel.
	 */
	void initialize() override;

	/** This method should be called periodically. Period depend on the
	 * process_rate in the configuration file.
	 */
	void doProcess() override;

   private:
	/** An 8 dimension vector of boolean value wich store the presence or
	 * abscence of a sensor on the phidget interface kit board.
	 */
	std::vector<bool> m_sensorIsPlugged;
	/** The minimum range in meters, this field is automaticaly filled according
	 * to the sensor part number read in the configuration file.
	 * Size of this vector depend on the number of sensors described in the
	 * configuration file.
	 */
	std::vector<float> m_minRange;

	/** The maximum range in meters, this field is automaticaly filled according
	 * to the sensor part number read in the configuration file.
	 * Size of this vector depend on the number of sensors described in the
	 * configuration file.
	 */
	std::vector<float> m_maxRange;

	/** The sensor type.
	 */
	std::vector<SensorType> m_sensorType;
	/** The poses of the 8 sensors x[m] y[m] z[m] yaw[deg] pitch[deg] roll[deg].
	 * This field is automaticaly filled according to the sensor
	 * described in the configuration file.
	 */
	std::vector<mrpt::poses::CPose3D> m_sensorPoses;

	/** The board serial number read in the configuration file. -1 for any
	 * board.
	 */
	int m_serialNumber{-1};
	float m_minOfMinRanges;
	float m_maxOfMaxRanges;

	void* m_carteInterfaceKit;	// CPhidgetInterfaceKitHandle

	/** See the class documentation at the top for expected parameters */
	void loadConfig_sensorSpecific(
		const mrpt::config::CConfigFileBase& configSource,
		const std::string& iniSection) override;
};	// end class

}  // namespace mrpt::hwdrivers
