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

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

using namespace std;
using namespace mrpt::hwdrivers;

IMPLEMENTS_GENERIC_SENSOR(CGillAnemometer, mrpt::hwdrivers)

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

  com_bauds = configSource.read_int(iniSection, "COM_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 CGillAnemometer::tryToOpenTheCOM()
{
  if (COM.isOpen()) return true;  // Already open

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

  try
  {
    COM.open(com_port);
    COM.setConfig(com_bauds, 0, 8, 1);
    COM.setTimeouts(50, 1, 100, 1, 20);
    COM.purgeBuffers();

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

/* -----------------------------------------------------
        doProcess
----------------------------------------------------- */
void CGillAnemometer::doProcess()
{
  // Is the COM open?
  if (!tryToOpenTheCOM())
  {
    m_state = ssError;
    printf("ERROR: No observation received from the Anemometer!\n");
    THROW_EXCEPTION("Cannot open the serial port");
  }

  mrpt::obs::CObservationWindSensor::Ptr obsPtr = mrpt::obs::CObservationWindSensor::Create();
  bool have_reading = false;
  std::string wind_reading;
  bool time_out = false;

  try
  {
    while (!have_reading)
    {
      // Read info into string: (default is Polar continuous)
      // format = <STX>Q, dir, speed, units, status, <ETX>
      // Q      -> Sensor identifier
      // dir    -> 3 digits (0-359)
      // speed  -> %3.2f
      // units  -> M (m/s), K (km/h)
      // status -> 00 = ok, else is an Error Code

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

    // parse format = <STX>Q, dir, speed, units, status, <ETX>
    std::deque<std::string> list;
    mrpt::system::tokenize(wind_reading, ",", list);
    if (list.size() == 6)
    {
      // Status
      int status = atoi(list.at(4).c_str());
      if (status == 0)
      {
        // Units
        std::string s_units = list.at(3);
        // Module
        std::string s_speed = list.at(2);
        if (s_units == "M")
          obsPtr->speed = atof(s_speed.c_str());
        else if (s_units == "K")
          obsPtr->speed = atof(s_speed.c_str()) * 1000 / 3600;
        else
        {
          printf(
              "ERROR: WindSonic measurement units not supported: "
              "%s\n",
              s_units.c_str());
          obsPtr->speed = 0.0;
        }
        // angle
        std::string s_direction = list.at(1);
        obsPtr->direction = atof(s_direction.c_str());

        // Prepare observation
        obsPtr->sensorLabel = m_sensorLabel;
        obsPtr->timestamp = mrpt::Clock::now();
        obsPtr->sensorPoseOnRobot =
            mrpt::poses::CPose3D(pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll);
        appendObservation(obsPtr);
      }
      else
        printf("ERROR: WindSonic error code %u\n", status);
    }
    else if (list.size() == 5)  // when there is no wind, the sensor does
    // not provide a wind direction
    {
      // Status
      int status = atoi(list.at(3).c_str());
      if (status == 0)
      {
        // Units
        std::string s_units = list.at(2);
        // module
        std::string s_speed = list.at(1);
        if (s_units == "M")
          obsPtr->speed = atof(s_speed.c_str());
        else if (s_units == "K")
          obsPtr->speed = atof(s_speed.c_str()) * 1000 / 3600;
        else
        {
          printf(
              "ERROR: WindSonic measurement units not supported: "
              "%s\n",
              s_units.c_str());
          obsPtr->speed = 0.0;
        }
        // Angle
        obsPtr->direction = 0.0;

        // Prepare observation
        obsPtr->sensorLabel = m_sensorLabel;
        obsPtr->timestamp = mrpt::Clock::now();
        obsPtr->sensorPoseOnRobot =
            mrpt::poses::CPose3D(pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll);
        appendObservation(obsPtr);
      }
      else
        printf("ERROR: WindSonic error code %u\n", status);
    }
    else
    {
      printf(
          "ERROR: Windsonic reading incorrect format: %s [%u]\n", wind_reading.c_str(),
          static_cast<unsigned int>(list.size()));
    }
  }
  catch (const std::exception& e)
  {
    std::cerr << "[CGillAnemometer::doProcess] Error:" << std::endl << e.what();
  }
  catch (...)
  {
    std::cerr << "[CGillAnemometer::doProcess] Unknown Error" << std::endl;
  }
}
