/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, 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 which
 * 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 example 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 which 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 which 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
