/* +------------------------------------------------------------------------+
   |                     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/hwdrivers/CRaePID.h>
#include <mrpt/system/datetime.h>

#include <chrono>
#include <iostream>
#include <iterator>
#include <memory>
#include <sstream>
#include <thread>

using namespace std;
using namespace mrpt::hwdrivers;

IMPLEMENTS_GENERIC_SENSOR(CRaePID, mrpt::hwdrivers)

/* -----------------------------------------------------
        Constructor
   ----------------------------------------------------- */
CRaePID::CRaePID() { m_sensorLabel = "RAE_PID"; }
/* -----------------------------------------------------
        loadConfig_sensorSpecific
   ----------------------------------------------------- */
void CRaePID::loadConfig_sensorSpecific(
    const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
{
#ifdef _WIN32
  com_port = configSource.read_string(iniSection, "COM_port_PID", "COM1", true);
#else
  com_port = configSource.read_string(iniSection, "COM_port_PID", "/dev/tty0", true);
#endif

  com_bauds = configSource.read_int(iniSection, "baudRate", 9600, false);

  pose_x = configSource.read_float(iniSection, "pose_x", 0, true);
  pose_y = configSource.read_float(iniSection, "pose_y", 0, true);
  pose_z = configSource.read_float(iniSection, "pose_z", 0, true);
  pose_roll = configSource.read_float(iniSection, "pose_roll", 0, true);
  pose_pitch = configSource.read_float(iniSection, "pose_pitch", 0, true);
  pose_yaw = configSource.read_float(iniSection, "pose_yaw", 0, true);
}

/* -----------------------------------------------------
        tryToOpenTheCOM
----------------------------------------------------- */
bool CRaePID::tryToOpenTheCOM()
{
  if (COM.isOpen()) return true;  // Already open

  if (m_verbose) cout << "[CRaePID] Opening " << com_port << " @ " << com_bauds << endl;

  try
  {
    COM.open(com_port);
    // Config:
    COM.setConfig(com_bauds, 0, 8, 1);
    // COM.setTimeouts( 1, 0, 1, 1, 1 );
    COM.setTimeouts(50, 1, 100, 1, 20);
    // std::this_thread::sleep_for(10ms);
    COM.purgeBuffers();
    // std::this_thread::sleep_for(10ms);

    return true;  // All OK!
  }
  catch (const std::exception& e)
  {
    std::cerr << "[CRaePID::tryToOpenTheCOM] Error opening or configuring "
                 "the serial port:"
              << std::endl
              << e.what();
    COM.close();
    return false;
  }
  catch (...)
  {
    std::cerr << "[CRaePID::tryToOpenTheCOM] Error opening or configuring "
                 "the serial port."
              << std::endl;
    COM.close();
    return false;
  }
}

/* -----------------------------------------------------
        doProcess
----------------------------------------------------- */
void CRaePID::doProcess()
{
  // Is the COM open?
  if (!tryToOpenTheCOM())
  {
    m_state = ssError;
    THROW_EXCEPTION("Cannot open the serial port");
  }

  bool have_reading = false;
  std::string power_reading;
  bool time_out = false;

  while (!have_reading)
  {
    // Send command to PID to request a measurement
    COM.purgeBuffers();
    COM.Write("R", 1);

    // Read PID response
    power_reading = COM.ReadString(500, &time_out);
    if (time_out)
    {
      // cout << "[CRaePID] " << com_port << " @ " <<com_bauds << " -
      // measurement Timed-Out" << endl;
      std::this_thread::sleep_for(10ms);
    }
    else
      have_reading = true;
  }

  // cout << "[CRaePID] " << com_port << " @ " <<com_bauds << " - measurement
  // -> " << power_reading << endl;

  // Convert the text to a number (ppm)
  const float readnum = atof(power_reading.c_str());
  const float val_ppm = readnum / 1000;

  // Fill the observation
  mrpt::obs::CObservationGasSensors::TObservationENose obs;
  obs.readingsVoltage.push_back(val_ppm);
  obs.sensorTypes.push_back(0x0001);

  mrpt::obs::CObservationGasSensors obsG;
  obsG.sensorLabel = this->getSensorLabel();
  obsG.m_readings.push_back(obs);
  obsG.timestamp = mrpt::Clock::now();

  appendObservation(mrpt::obs::CObservationGasSensors::Create(obsG));
}

std::string CRaePID::getFirmware()
{
  // Send the command
  cout << "Firmware version: " << endl;
  COM.purgeBuffers();
  size_t B_written = COM.Write("F", 1);
  if (!B_written) return std::string("COMMS.ERROR");

  // Read the returned text
  bool time_out = false;
  std::string s_read = COM.ReadString(2000, &time_out);
  if (time_out) s_read = "Time_out";
  return s_read;
}

std::string CRaePID::getModel()
{
  // Send the command
  COM.purgeBuffers();
  COM.Write("M", 1);

  // Read the returned text
  return COM.ReadString();
}

std::string CRaePID::getSerialNumber()
{
  // Send the command
  COM.purgeBuffers();
  COM.Write("S", 1);

  // Read the returned text
  return COM.ReadString();
}

std::string CRaePID::getName()
{
  // Send the command
  COM.purgeBuffers();
  COM.Write("N", 1);

  // Read the returned text
  return COM.ReadString();
}

bool CRaePID::switchPower()
{
  // Send the command
  COM.purgeBuffers();
  COM.Write("P", 1);

  // Read the returned text
  std::string reading;
  reading = COM.ReadString();

  if (strcmp(reading.c_str(), "Sleep...") == 0)
    return true;
  else
    return false;
}

mrpt::obs::CObservationGasSensors CRaePID::getFullInfo()
{
  // Send the command
  COM.purgeBuffers();
  COM.Write("C", 1);

  // Read the returned text
  std::string reading;
  reading = COM.ReadString();

  // Iterate over each information component (tokenize first)
  // construct a stream from the string (as seen in
  // http://stackoverflow.com/a/53921)
  std::stringstream readings_str(reading);

  // use stream iterators to copy the stream to the vector as whitespace
  // separated strings
  std::istream_iterator<std::string> it(readings_str);
  std::istream_iterator<std::string> endit;
  std::vector<std::string> measurements_text(it, endit);

  // Convert the text to a number (ppm)
  mrpt::obs::CObservationGasSensors::TObservationENose obs;
  mrpt::obs::CObservationGasSensors obsG;

  for (auto& k : measurements_text)
  {
    const float readnum = atof(k.c_str());
    const float val_ppm = readnum / 1000.f;

    // Fill the observation
    obs.readingsVoltage.push_back(val_ppm);
    obsG.m_readings.push_back(obs);
  }

  obsG.sensorLabel = this->getSensorLabel();
  obsG.timestamp = mrpt::Clock::now();

  return obsG;
}

bool CRaePID::errorStatus(std::string& errorString)
{
  // Send the command
  COM.purgeBuffers();
  COM.Write("E", 1);

  // Read the returned text
  std::string reading;
  reading = COM.ReadString();

  // Tokenize to separate the two components:
  // construct a stream from the string (as seen in
  // http://stackoverflow.com/a/53921)
  std::stringstream readings_str(reading);

  // use stream iterators to copy the stream to the vector as whitespace
  // separated strings
  std::istream_iterator<std::string> it(readings_str);
  std::istream_iterator<std::string> endit;
  std::vector<std::string> errors_text(it, endit);

  // Take the first part and check the possible error condition
  if ((strcmp(errors_text[0].c_str(), "0") == 0) &&
      (strcmp(errors_text[1].c_str(), "0") == 0))  // no error
  {
    return false;
  }
  else
  {
    // By the moment, return the raw error; note that if necessary a
    // detailed description of the error can be obtained analyzing the two
    // error strings separately
    errorString = reading;
    return true;
  }
}

void CRaePID::getLimits(float& min, float& max)
{
  // Send the command
  COM.purgeBuffers();
  COM.Write("L", 1);

  // Read the returned text
  std::string reading;
  reading = COM.ReadString();

  // Tokenize to separate the two components:
  // construct a stream from the string (as seen in
  // http://stackoverflow.com/a/53921)
  std::stringstream readings_str(reading);

  // use stream iterators to copy the stream to the vector as whitespace
  // separated strings
  std::istream_iterator<std::string> it(readings_str);
  std::istream_iterator<std::string> endit;
  std::vector<std::string> readings_text(it, endit);

  // read min and max
  max = atof(readings_text[0].c_str());
  min = atof(readings_text[1].c_str());
}
