/* +------------------------------------------------------------------------+
   |                     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          |
   +------------------------------------------------------------------------+ */

#include "hwdrivers-precomp.h"  // Precompiled headers
//
#include <mrpt/config/CConfigFileBase.h>
#include <mrpt/hwdrivers/CPhidgetInterfaceKitProximitySensors.h>
#include <mrpt/system/os.h>

#include <algorithm>
#include <iostream>

#if MRPT_HAS_PHIDGET
#include <phidget21.h>
#endif

using namespace mrpt::hwdrivers;
using namespace mrpt::obs;
using namespace mrpt::system;
using namespace std;

IMPLEMENTS_GENERIC_SENSOR(CPhidgetInterfaceKitProximitySensors, mrpt::hwdrivers)

/* -----------------------------------------------------
        Constructor
   ----------------------------------------------------- */
CPhidgetInterfaceKitProximitySensors::CPhidgetInterfaceKitProximitySensors() :
    mrpt::system::COutputLogger("CPhidgetInterfaceKitProximitySensors")

{
#if MRPT_HAS_PHIDGET
  m_carteInterfaceKit = new CPhidgetInterfaceKitHandle;
  *((CPhidgetInterfaceKitHandle*)m_carteInterfaceKit) = 0;
  m_sensorLabel = "PhidgetInterfaceKit";

  m_sensorIsPlugged.assign(8, false);
  m_minRange.assign(8, 0.1f);
  m_maxRange.assign(8, 0.8f);
  m_sensorPoses.resize(8);
  m_sensorType.assign(8, UNPLUGGED);

#else
  THROW_EXCEPTION(
      "MRPT Was compiled without the CPhidget support. Recompile MRPT to use "
      "this class");
#endif
}

/* -----------------------------------------------------
        loadConfig_sensorSpecific
   ----------------------------------------------------- */
void CPhidgetInterfaceKitProximitySensors::loadConfig_sensorSpecific(
    [[maybe_unused]] const mrpt::config::CConfigFileBase& configSource,
    [[maybe_unused]] const std::string& iniSection)
{
#if MRPT_HAS_PHIDGET
  if (!configSource.sectionExists(iniSection))
    THROW_EXCEPTION("Can't find section in configuration file");
  // looking for the board parameters.
  // process_rate = 100 						// Hz (common to all sensors)
  // serialNumber = 12345						// The interface kit serial
  // number.
  m_process_rate = configSource.read_int(iniSection, string("process_rate"), 50);
  m_serialNumber = configSource.read_int(iniSection, string("serialNumber"), -1);
  bool display =
      configSource.read_bool(iniSection, string("displayRecapitulativeInformations"), false);

  // Looking for each sensor.

  for (int i = 1; i <= 8; i++)
  {
    string sensorNKeyName = format("sensor%d", i);
    string sensorType = configSource.read_string(iniSection, sensorNKeyName, string("UNPLUGGED"));
    if (sensorType != string("UNPLUGGED"))
    {
      // the sensor is plugged :
      // // check if the sensor type is supported.
      if (sensorType == string("EZ1"))
      {
        m_sensorType[i - 1] = EZ1;
        m_minRange[i - 1] = 0.15;  // meters
        m_maxRange[i - 1] = 6.45;  // meters
      }
      else if (sensorType == string("SHARP-30cm"))
      {
        m_sensorType[i - 1] = SHARP_30cm;
        m_minRange[i - 1] = 0.04;  // meters
        m_maxRange[i - 1] = 0.3;   // meters
      }
      else if (sensorType == string("SHARP-80cm"))
      {
        m_sensorType[i - 1] = SHARP_80cm;
        m_minRange[i - 1] = 0.06;  // meters
        m_maxRange[i - 1] = 0.8;   // meters
      }
      else
      {
        string err = format("Type of sensor %d is not supported", i);
        m_state = CGenericSensor::ssError;
        THROW_EXCEPTION(err);
      }
      m_sensorIsPlugged[i - 1] = true;
      // reading the sensor pose.
      string sensorNPoseX = format("pose%d_x", i);
      string sensorNPoseY = format("pose%d_y", i);
      string sensorNPoseZ = format("pose%d_z", i);
      string sensorNPoseYaw = format("pose%d_yaw", i);
      string sensorNPosePitch = format("pose%d_pitch", i);
      string sensorNPoseRoll = format("pose%d_roll", i);

      float x = configSource.read_float(iniSection, sensorNPoseX, 0.0);
      float y = configSource.read_float(iniSection, sensorNPoseY, 0.0);
      float z = configSource.read_float(iniSection, sensorNPoseZ, 0.0);
      float yaw = configSource.read_float(iniSection, sensorNPoseYaw, 0.0);
      float pitch = configSource.read_float(iniSection, sensorNPosePitch, 0.0);
      float roll = configSource.read_float(iniSection, sensorNPoseRoll, 0.0);

      m_sensorPoses[i - 1] = mrpt::poses::CPose3D(x, y, z, yaw, pitch, roll);
    }
  }
  if (display)
  {  // width = 80;
    cout.fill(' ');
    cout << "+-------------------------------------------------------------"
            "-----------------+"
         << endl;
    cout.width(79);
    cout << "|  Phidget interfaceKit board number : " << m_serialNumber;
    cout << "|" << endl;
    cout << "| Process rate : " << m_process_rate;
    cout << "|" << endl;
    cout << "+---------+---------------------+-----------------------------"
            "-----------------+"
         << endl;
    cout << "|    #    + Sensor type         | Sensor 3D pose              "
            "                 |"
         << endl;
    cout << "+---------+---------------------+-----------------------------"
            "-----------------+"
         << endl;
    for (int i = 0; i < 8; i++)
    {
      cout << "|";
      cout.width(9);
      cout << i + 1;
      cout << " |";
      cout.width(19);
      switch (m_sensorType[i])
      {
        case EZ1:
          cout << "EZ1 |";
          break;
        case SHARP_30cm:
          cout << "SHARP_30cm |";
          break;
        case SHARP_80cm:
          cout << "SHARP_80cm |";
          break;
        case UNPLUGGED:
          cout << "UNPLUGGED |";
          break;
      }
      cout.width(43);
      cout << m_sensorPoses[i];
      cout << "|" << endl;
    }
    cout << "+-------------------------------------------------------------"
            "-----------------+"
         << endl;
  }
#endif
}

/* -----------------------------------------------------
        Initialize
   ----------------------------------------------------- */
void CPhidgetInterfaceKitProximitySensors::initialize()
{
#if MRPT_HAS_PHIDGET
  /*Try to connect to the interface kit board*/
  CPhidgetInterfaceKit_create((CPhidgetInterfaceKitHandle*)m_carteInterfaceKit);
  CPhidget_open(*((CPhidgetHandle*)(m_carteInterfaceKit)), m_serialNumber);
  int err = CPhidget_waitForAttachment(
      *((CPhidgetHandle*)(m_carteInterfaceKit)),
      200);  // wait 200ms for board attachment.
  // if an error occur, "err" will be a > 0 value.
  if (err > 0)
  {
    m_state = CGenericSensor::ssError;
    THROW_EXCEPTION("Can't find Phidget IK card, please check your serial number.");
  }
  // set frame rate
  /*int miliseconds =
  static_cast<int>(1000./d2f(m_process_rate)); for(int i = 0 ;
  i < 8 ; i++)
  {
    if(m_sensorIsPlugged[i])
    {
      int err =
  CPhidgetInterfaceKit_setDataRate(*((CPhidgetInterfaceKitHandle*)(m_carteInterfaceKit)),
  i, miliseconds); if(err > 0)
      {
        string error = format("Can't set process rate to %d ms on
  channel %d of the Phidget IK Board.", miliseconds, i); m_state =
  CGenericSensor::ssError; THROW_EXCEPTION(error);
      }
    }
  }*/  // seems to be used only in the event based programming
						 // way.
  // compute (min/max) of (min/max) ranges.
  m_minOfMinRanges = *min_element(m_minRange.begin(), m_minRange.end());
  m_maxOfMaxRanges = *max_element(m_maxRange.begin(), m_maxRange.end());
  // driver is ready.
  m_state = CGenericSensor::ssWorking;
#endif
}

/* -----------------------------------------------------
        Destructor
   ----------------------------------------------------- */
CPhidgetInterfaceKitProximitySensors::~CPhidgetInterfaceKitProximitySensors()
{
#if MRPT_HAS_PHIDGET
  if (*((CPhidgetHandle*)m_carteInterfaceKit))
  {
    CPhidget_close(*((CPhidgetHandle*)(m_carteInterfaceKit)));
    CPhidget_delete(*((CPhidgetHandle*)(m_carteInterfaceKit)));
  }
#endif
}

/*-------------------------------------------------------------
          doProcess
-------------------------------------------------------------*/
void CPhidgetInterfaceKitProximitySensors::doProcess()
{
  CObservationRange::Ptr obs = std::make_shared<CObservationRange>();

  try
  {
    getObservation(*obs);
    m_state = ssWorking;
    // if at least one data have been sensed :
    if (obs->sensedData.size() > 0)
    {
      appendObservation(obs);
    }
  }
  catch (...)
  {
    m_state = ssError;
    THROW_EXCEPTION("No observation received from the Phidget board!");
  }
}

/*-------------------------------------------------------------
          getObservation
-------------------------------------------------------------*/
void CPhidgetInterfaceKitProximitySensors::getObservation(
    [[maybe_unused]] mrpt::obs::CObservationRange& obs)
{
#if MRPT_HAS_PHIDGET
  obs.timestamp = mrpt::Clock::now();
  obs.sensorLabel = m_sensorLabel;
  obs.minSensorDistance = m_minOfMinRanges;
  obs.maxSensorDistance = m_maxOfMaxRanges;
  obs.sensorConeAperture = DEG2RAD(2.0f);  // TODO : Adapt to real sensor cone apperture.
  obs.sensedData.clear();

  int sensorValue;
  for (int i = 0; i < 8; i++)
  {
    if (m_sensorIsPlugged[i])
    {
      mrpt::obs::CObservationRange::TMeasurement obsRange;
      int err = CPhidgetInterfaceKit_getSensorValue(
          *((CPhidgetInterfaceKitHandle*)(m_carteInterfaceKit)), i, &sensorValue);
      if (err > 0)
      {
        string error("Error durring acquiering sensor value on channel : %d", i);
        THROW_EXCEPTION(error);
      }
      switch (m_sensorType[i])
      {
        case EZ1:
          // TODO : find the conversion formula.
          obsRange.sensedDistance = 1.0;
          break;
        case SHARP_30cm:
          obsRange.sensedDistance = 2076. / (d2f(sensorValue) - 11.);
          break;
        case SHARP_80cm:
          obsRange.sensedDistance = 4800. / (d2f(sensorValue) - 16.92);
          break;
        default:
          obsRange.sensedDistance = -1;
          break;
      }

      obsRange.sensorID = i;
      obsRange.sensorPose = m_sensorPoses[i].asTPose();  // The pose of the IR sensor on the robot
      obs.sensedData.push_back(obsRange);
    }
  }
#endif
}
