/* +------------------------------------------------------------------------+
   |                     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/comms/CClientTCPSocket.h>
#include <mrpt/comms/CSerialPort.h>
#include <mrpt/hwdrivers/CHokuyoURG.h>
#include <mrpt/opengl/CAxis.h>
#include <mrpt/opengl/CPlanarLaserScan.h>  // in library mrpt-maps
#include <mrpt/system/CTimeLogger.h>
#include <mrpt/system/os.h>

#include <chrono>
#include <thread>

IMPLEMENTS_GENERIC_SENSOR(CHokuyoURG, mrpt::hwdrivers)

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

const int MINIMUM_PACKETS_TO_SET_TIMESTAMP_REFERENCE = 10;

CHokuyoURG::CHokuyoURG() : m_rx_buffer(40000) { m_sensorLabel = "Hokuyo"; }

CHokuyoURG::~CHokuyoURG()
{
  try
  {
    m_win.reset();
    closeStreamConnection();
  }
  catch (const std::exception& e)
  {
    std::cerr << "[~CHokuyoURG] Exception:\n" << mrpt::exception_to_str(e);
  }
}

void CHokuyoURG::closeStreamConnection()
{
  if (m_stream)
  {
    turnOff();
    m_stream.reset();
  }
}

void CHokuyoURG::sendCmd(const char* str)
{
  MRPT_START
  ASSERT_(str != nullptr);
  ASSERT_(m_stream != nullptr);
  const size_t N = strlen(str);
  const size_t nWriten = m_stream->Write(str, N);
  ASSERT_EQUAL_(nWriten, N);

  MRPT_LOG_DEBUG_STREAM("[Hokuyo] sendCmd(): `" << str << "`");

  m_lastSentMeasCmd = std::string(str);  // for echo verification
  MRPT_END
}

void CHokuyoURG::doProcessSimple(
    bool& outThereIsObservation,
    mrpt::obs::CObservation2DRangeScan& outObservation,
    bool& hardwareError)
{
#if 0
	static mrpt::system::CTimeLogger timLogger("CHokuyoURG");
	mrpt::system::CTimeLoggerEntry tle(timLogger, "doProcessSimple");
#endif

  outThereIsObservation = false;
  hardwareError = false;

  // Bound? If not, connect to the device and set it up (again) in
  // continuous read mode:
  if (!ensureStreamIsOpen())
  {
    m_timeStartUI = 0;
    m_timeStartSynchDelay = 0;
    hardwareError = true;
    return;
  }

  // Wait for a message:
  int nRanges = m_lastRange - m_firstRange + 1;
  int expectedSize = nRanges * 3 + 4;
  if (m_intensity) expectedSize += nRanges * 3;

  m_rcv_data.clear();
  m_rcv_data.reserve(expectedSize + 1000);

  m_state = ssWorking;
  if (!parseResponse(false))
  {
    if (!internal_notifyNoScanReceived())
    {
      // It seems the sensor needs to be reset (?), let this know
      // to the caller:
      m_state = ssError;
      hardwareError = true;

      // And on our side, close the connection to ensure initialization
      // is called again to set-up the laser in the next call to
      // ensureStreamIsOpen() above.
      closeStreamConnection();

      return;
    }

    // No new data
    return;
  }

  // DECODE:
  if (m_rcv_status0 != '0' && m_rcv_status0 != '9')
  {
    hardwareError = true;
    return;
  }

  // -----------------------------------------------
  //   Extract the observation:
  // -----------------------------------------------
  outObservation.timestamp = mrpt::Clock::now();

  if ((size_t)expectedSize != m_rcv_data.size())
  {
    MRPT_LOG_ERROR_STREAM(
        "[CHokuyoURG::doProcess] ERROR: Expected " << expectedSize << " data bytes, received "
                                                   << m_rcv_data.size() << "instead!");
    hardwareError = true;
    return;
  }
  // Delay the sync of timestamps due to instability in the constant rate
  // during the first few packets.
  bool do_timestamp_sync = !m_disable_firmware_timestamp;
  if (do_timestamp_sync && m_timeStartSynchDelay < MINIMUM_PACKETS_TO_SET_TIMESTAMP_REFERENCE)
  {
    do_timestamp_sync = false;
    m_timeStartSynchDelay++;
  }

  if (do_timestamp_sync)
  {
    // Extract the timestamp of the sensor:
    uint32_t nowUI = ((m_rcv_data[0] - 0x30) << 18) + ((m_rcv_data[1] - 0x30) << 12) +
                     ((m_rcv_data[2] - 0x30) << 6) + (m_rcv_data[3] - 0x30);

    uint32_t AtUI = 0;
    if (m_timeStartUI == 0)
    {
      m_timeStartUI = nowUI;
      m_timeStartTT = mrpt::Clock::now();
    }
    else
      AtUI = nowUI - m_timeStartUI;

    auto AtDO = std::chrono::milliseconds(AtUI);
    outObservation.timestamp = m_timeStartTT + AtDO;
  }

  // And the scan ranges:
  outObservation.rightToLeft = true;

  outObservation.aperture = nRanges * 2 * M_PI / m_sensor_info.scans_per_360deg;

  outObservation.maxRange = m_sensor_info.d_max;
  outObservation.stdError = 0.010f;
  outObservation.sensorPose = m_sensorPose;
  outObservation.sensorLabel = m_sensorLabel;

  outObservation.resizeScan(nRanges);
  char* ptr = (char*)&m_rcv_data[4];

  if (m_intensity) outObservation.setScanHasIntensity(true);

  for (int i = 0; i < nRanges; i++)
  {
    int b1 = (*ptr++) - 0x30;
    int b2 = (*ptr++) - 0x30;
    int b3 = (*ptr++) - 0x30;

    int range_mm = ((b1 << 12) | (b2 << 6) | b3);

    outObservation.setScanRange(i, range_mm * 0.001f);
    outObservation.setScanRangeValidity(
        i, range_mm >= 20 && (outObservation.getScanRange(i) <= outObservation.maxRange));

    if (m_intensity)
    {
      int b4 = (*ptr++) - 0x30;
      int b5 = (*ptr++) - 0x30;
      int b6 = (*ptr++) - 0x30;
      outObservation.setScanIntensity(i, ((b4 << 12) | (b5 << 6) | b6));
    }
  }

  // Do filter:
  C2DRangeFinderAbstract::filterByExclusionAreas(outObservation);
  C2DRangeFinderAbstract::filterByExclusionAngles(outObservation);
  // Do show preview:
  C2DRangeFinderAbstract::processPreview(outObservation);

  outThereIsObservation = true;
  internal_notifyGoodScanNow();
}

/*-------------------------------------------------------------
            loadConfig_sensorSpecific
-------------------------------------------------------------*/
void CHokuyoURG::loadConfig_sensorSpecific(
    const mrpt::config::CConfigFileBase& c, const std::string& s)
{
  m_reduced_fov = DEG2RAD(c.read_float(s, "reduced_fov", 0)),

  m_motorSpeed_rpm = c.read_int(s, "HOKUYO_motorSpeed_rpm", 0);
  m_sensorPose.setFromValues(
      c.read_float(s, "pose_x", 0), c.read_float(s, "pose_y", 0), c.read_float(s, "pose_z", 0),
      DEG2RAD(c.read_float(s, "pose_yaw", 0)), DEG2RAD(c.read_float(s, "pose_pitch", 0)),
      DEG2RAD(c.read_float(s, "pose_roll", 0)));

  m_highSensMode = c.read_bool(s, "HOKUYO_HS_mode", m_highSensMode);

#ifdef _WIN32
  const char* comVarName = "COM_port_WIN";
#else
  const char* comVarName = "COM_port_LIN";
#endif

  m_com_port = c.read_string(s, comVarName, m_com_port);
  m_ip_dir = c.read_string(s, "IP_DIR", m_ip_dir);
  m_port_dir = c.read_int(s, "PORT_DIR", m_port_dir);

  ASSERTMSG_(
      !m_com_port.empty() || !m_ip_dir.empty(),
      "Either COM_port or IP_DIR must be defined in the configuration file!");
  ASSERTMSG_(
      m_com_port.empty() || m_ip_dir.empty(),
      "Both COM_port and IP_DIR set! Please, define only one of them.");
  if (!m_ip_dir.empty())
  {
    ASSERTMSG_(
        m_port_dir,
        "A TCP/IP port number `PORT_DIR` must be specified for Ethernet "
        "connection");
  }

  m_disable_firmware_timestamp =
      c.read_bool(s, "disable_firmware_timestamp", m_disable_firmware_timestamp);
  m_intensity = c.read_bool(s, "intensity", m_intensity),

  MRPT_LOAD_HERE_CONFIG_VAR(scan_interval, int, m_scan_interval, c, s);
  MRPT_LOAD_HERE_CONFIG_VAR(comms_timeout_ms, int, m_comms_timeout_ms, c, s);
  MRPT_LOAD_HERE_CONFIG_VAR(comms_between_timeout_ms, int, m_comms_between_timeout_ms, c, s);

  // Parent options:
  C2DRangeFinderAbstract::loadCommonParams(c, s);
}

/*-------------------------------------------------------------
            turnOn
-------------------------------------------------------------*/
bool CHokuyoURG::turnOn()
{
  MRPT_START

  // Bound?
  if (!ensureStreamIsOpen()) return false;

  // If we are over a serial link, set it up:
  if (m_ip_dir.empty())
  {
    auto* COM = dynamic_cast<CSerialPort*>(m_stream.get());

    if (COM != nullptr)
    {
      // It is a COM:
      COM->setConfig(19200);
      COM->setTimeouts(m_comms_timeout_ms, 0, m_comms_timeout_ms, 0, 0);

      // Assure the laser is off and quiet:
      switchLaserOff();
      std::this_thread::sleep_for(10ms);

      COM->purgeBuffers();
      std::this_thread::sleep_for(10ms);

      COM->setConfig(115200);
      switchLaserOff();
      std::this_thread::sleep_for(10ms);
      COM->purgeBuffers();
      std::this_thread::sleep_for(10ms);
      COM->setConfig(19200);
    }

    if (COM != nullptr)
    {
      // Set 115200 baud rate:
      setHighBaudrate();
      enableSCIP20();
      COM->setConfig(115200);
    }
  }
  else
  {
    auto* COM = dynamic_cast<CClientTCPSocket*>(m_stream.get());

    if (COM != nullptr)
    {
      // Assure the laser is off and quiet:
      switchLaserOff();
      std::this_thread::sleep_for(10ms);

      purgeBuffers();
      std::this_thread::sleep_for(10ms);

      switchLaserOff();
      std::this_thread::sleep_for(10ms);
      purgeBuffers();
    }
  }

  if (!enableSCIP20()) return false;

  // Turn on the laser:
  if (!switchLaserOn()) return false;

  // Set the motor speed:
  if (m_motorSpeed_rpm)
    if (!setMotorSpeed(m_motorSpeed_rpm)) return false;

  // Set HS mode:
  setHighSensitivityMode(m_highSensMode);

  // Display sensor information:
  if (!displaySensorInfo(&m_sensor_info)) return false;

  // Set for scanning angles:
  m_firstRange = m_sensor_info.scan_first;
  m_lastRange = m_sensor_info.scan_last;

  // Artificially reduced FOV?
  if (m_reduced_fov > 0 && m_reduced_fov < 2 * M_PI)
  {
    int center = (m_lastRange + m_firstRange) >> 1;
    const int half_range =
        static_cast<int>((m_sensor_info.scans_per_360deg / 360.0) * RAD2DEG(m_reduced_fov)) >> 1;
    m_firstRange = center - half_range;
    m_lastRange = center + half_range;
    MRPT_LOG_INFO_STREAM(
        "[HOKUYO::turnOn] Using reduced FOV: ranges ["
        << m_firstRange << "-" << m_lastRange << "] for " << RAD2DEG(m_reduced_fov) << " deg. FOV");
  }

  if (!displayVersionInfo())
  {
    // return false; // It's not SO important
  }

  // Start!
  if (!startScanningMode()) return false;

  return true;

  MRPT_END
}

/*-------------------------------------------------------------
            turnOff
-------------------------------------------------------------*/
bool CHokuyoURG::turnOff()
{
  // Turn off the laser:
  if (!switchLaserOff()) return false;

  return true;
}

bool CHokuyoURG::setHighBaudrate()
{
  if (!ensureStreamIsOpen()) return false;

  MRPT_LOG_DEBUG("[CHokuyoURG::setHighBaudrate] Changing baudrate to 115200...");

  // Send command:
  sendCmd("SS115200\x0A");

  // Receive response:
  if (!parseResponse())
  {
    MRPT_LOG_ERROR("[CHokuyoURG::setHighBaudrate] Error waiting for response");
    return false;
  }

  MRPT_LOG_DEBUG("OK\n");
  return true;
}

/*-------------------------------------------------------------
            ensureBufferHasBytes
-------------------------------------------------------------*/
bool CHokuyoURG::ensureBufferHasBytes(size_t nDesiredBytes, bool additionalWaitForData)
{
  ASSERT_LT_(nDesiredBytes, m_rx_buffer.capacity());

  if (m_rx_buffer.size() >= nDesiredBytes) return true;

  // Try to read more bytes:
  std::array<uint8_t, 512> buf;
  const size_t to_read = std::min(m_rx_buffer.available(), buf.size());

  try
  {
    const size_t nRead = [this, &buf, additionalWaitForData, to_read]()
    {
      if (auto sock = dynamic_cast<CClientTCPSocket*>(m_stream.get()); sock)
      {
        // socket:
        auto timeout = m_comms_timeout_ms;
        if (additionalWaitForData) mrpt::keep_max(timeout, 100);

        return sock->readAsync(&buf[0], to_read, timeout, m_comms_between_timeout_ms);
      }
      else
      {
        // serial port:
        return m_stream->Read(&buf[0], to_read);
      }
    }();

    m_rx_buffer.push_many(&buf[0], nRead);
  }
  catch (std::exception&)
  {
    // 0 bytes read
  }

  return (m_rx_buffer.size() >= nDesiredBytes);
}

bool CHokuyoURG::parseResponse(bool additionalWaitForData)
{
  m_rcv_status0 = '\0';
  m_rcv_status1 = '\0';
  m_rcv_data.clear();

  if (!ensureStreamIsOpen()) return false;
  ASSERT_(!m_lastSentMeasCmd.empty());

  try
  {
    // Process response:
    // ---------------------------------
    char tmp_rcv_status0 = '\0', tmp_rcv_status1 = '\0';

    // COMMAND ECHO ---------
    size_t peekIdx = 0;
    const unsigned int verifLen = m_lastSentMeasCmd.size();

    if (verifLen)
    {
      unsigned int i = 0;
      do
      {
        if (!ensureBufferHasBytes(verifLen - i, additionalWaitForData)) return false;

        // If matches the echo, go on:
        if (m_rx_buffer.peek(peekIdx++) == m_lastSentMeasCmd[i])
        {
          // Match is ok:
          i++;
        }
        else
        {
          // Skip one byte and keep trying to match:
          m_rx_buffer.pop();
          i = 0;
          peekIdx = 0;
        }
      } while (i < verifLen);
    }

    // Now, the status bytes:
    if (!ensureBufferHasBytes(peekIdx + 2, additionalWaitForData)) return false;

    tmp_rcv_status0 = m_rx_buffer.peek(peekIdx++);
    tmp_rcv_status1 = m_rx_buffer.peek(peekIdx++);

    // In SCIP2.0, there is an additional sum char:
    if (tmp_rcv_status1 != 0x0A)
    {
      // Yes, it is SCIP2.0
      if (!ensureBufferHasBytes(peekIdx + 1, additionalWaitForData)) return false;
      // Ignore this byte: sumStatus
      peekIdx++;
    }
    else
    {
      // Continue, it seems a SCIP1.1 response...
    }

    // After the status bytes, there must be a LF:
    if (!ensureBufferHasBytes(peekIdx + 1, additionalWaitForData)) return false;
    char nextChar = m_rx_buffer.peek(peekIdx++);
    if (nextChar != 0x0A) return false;

    // -----------------------------------------------------------------------------
    // Now the data:
    // There's a problem here, we don't know in advance how many bytes to
    // read,
    //  so rely on the serial port class implemented buffer and call many
    //  times
    //  the read method with only 1 byte each time:
    // -----------------------------------------------------------------------------
    bool lastWasLF = false;
    std::string tmp_rx;
    for (;;)
    {
      if (!ensureBufferHasBytes(peekIdx + 1, additionalWaitForData))
      {
        // Do not empty the queue, it seems we need to wait for more
        // data:
        return false;
      }
      tmp_rx.push_back(m_rx_buffer.peek(peekIdx++));

      // No data?
      if (tmp_rx.size() == 1 && tmp_rx[0] == 0x0A)
      {
        tmp_rx.clear();
        m_rcv_status0 = tmp_rcv_status0;
        m_rcv_status1 = tmp_rcv_status1;
        // Empty read bytes so far:
        for (size_t k = 0; k < peekIdx; k++) m_rx_buffer.pop();
        return true;
      }

      // Is it a LF?
      if (*tmp_rx.rbegin() != 0x0A)
      {
        lastWasLF = false;
        continue;
      }

      // This was a single LF:
      if (!lastWasLF)
      {
        // Discard SUM+LF
        ASSERT_(tmp_rx.size() >= 2);
        tmp_rx.resize(tmp_rx.size() - 2);
      }
      else
      {
        // This was a double LF.

        // Discard this last LF:
        tmp_rx.resize(tmp_rx.size() - 1);

        // Done!
        m_rcv_data = tmp_rx;
        m_rcv_status0 = tmp_rcv_status0;
        m_rcv_status1 = tmp_rcv_status1;

        // Empty read bytes so far:
        for (size_t k = 0; k < peekIdx; k++) m_rx_buffer.pop();

        MRPT_LOG_DEBUG_STREAM("[Hokuyo] parseResponse(): RX `" << m_rcv_data << "`");

        if (m_rcv_status0 != '0' && (m_rcv_status0 != '9' && m_rcv_status1 != '9'))
        {
          MRPT_LOG_ERROR_STREAM(
              "[Hokuyo] Error LIDAR status: " << (int)m_rcv_status0 << " after command: `"
                                              << m_lastSentMeasCmd << "`");
          return false;
        }
        return true;
      }
      lastWasLF = true;
    }
  }
  catch (const std::exception& e)
  {
    MRPT_LOG_ERROR_FMT("[Hokuyo] parseResponse() Exception: %s", e.what());
    return false;
  }
  catch (...)
  {
    return false;  // Serial port timeout,...
  }
}

bool CHokuyoURG::enableSCIP20()
{
  if (!ensureStreamIsOpen()) return false;

  MRPT_LOG_DEBUG("[CHokuyoURG::enableSCIP20] Changing protocol to SCIP2.0...");

  // Send command:
  sendCmd("SCIP2.0\x0A");

  // Receive response:
  if (!parseResponse())
  {
    MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
    return false;
  }

  MRPT_LOG_DEBUG("OK\n");
  return true;
}

bool CHokuyoURG::switchLaserOn()
{
  if (!ensureStreamIsOpen()) return false;

  MRPT_LOG_DEBUG("[CHokuyoURG::switchLaserOn] Switching laser ON...");

  // Send command:
  sendCmd("BM\x0A");

  // Receive response:
  if (!parseResponse())
  {
    MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
    return false;
  }

  MRPT_LOG_DEBUG("OK\n");
  return true;
}

bool CHokuyoURG::switchLaserOff()
{
  if (!ensureStreamIsOpen()) return false;

  MRPT_LOG_DEBUG("[CHokuyoURG::switchLaserOff] Switching laser OFF...");

  // Send command:
  sendCmd("QT\x0A");

  // Receive response:
  if (!parseResponse())
  {
    MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
    return false;
  }

  MRPT_LOG_DEBUG("OK\n");
  return true;
}

void CHokuyoURG::setScanInterval(unsigned int skipScanCount) { m_scan_interval = skipScanCount; }
unsigned int CHokuyoURG::getScanInterval() const { return m_scan_interval; }
bool CHokuyoURG::setMotorSpeed(int motoSpeed_rpm)
{
  if (!ensureStreamIsOpen()) return false;

  MRPT_LOG_DEBUG_FMT("[CHokuyoURG::setMotorSpeed] Setting to %i rpm...", motoSpeed_rpm);

  // Send command:
  int motorSpeedCode = (600 - motoSpeed_rpm) / 6;
  if (motorSpeedCode < 0 || motorSpeedCode > 10)
  {
    MRPT_LOG_ERROR_STREAM(
        __CURRENT_FUNCTION_NAME__ << " Motorspeed must be in the range 540-600 rpm");
    return false;
  }

  char cmd[20];
  os::sprintf(cmd, 20, "CR%02i\x0A", motorSpeedCode);
  sendCmd(cmd);

  // Receive response:
  if (!parseResponse())
  {
    MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
    return false;
  }

  MRPT_LOG_DEBUG("OK\n");
  return true;
}

/*-------------------------------------------------------------
            setHighSensitivityMode
-------------------------------------------------------------*/
bool CHokuyoURG::setHighSensitivityMode(bool enabled)
{
  if (!ensureStreamIsOpen()) return false;

  MRPT_LOG_DEBUG_FMT(
      "[CHokuyoURG::setHighSensitivityMode] Setting HS mode to: %s...", enabled ? "true" : "false");

  // Send command:
  char cmd[20];
  os::sprintf(cmd, 20, "HS%i\x0A", enabled ? 1 : 0);
  sendCmd(cmd);

  // Receive response:
  if (!parseResponse())
  {
    MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
    return false;
  }

  MRPT_LOG_DEBUG("OK\n");
  return true;
}

/*-------------------------------------------------------------
                        setIntensityMode
-------------------------------------------------------------*/
bool CHokuyoURG::setIntensityMode(bool enabled)
{
  m_intensity = enabled;
  return true;
}

bool CHokuyoURG::displayVersionInfo()
{
  if (!ensureStreamIsOpen()) return false;

  MRPT_LOG_DEBUG("[CHokuyoURG::displayVersionInfo] Asking info...");

  // Send command:
  sendCmd("VV\x0A");

  // Receive response:
  if (!parseResponse())
  {
    MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
    return false;
  }

  MRPT_LOG_DEBUG("OK\n");

  // PRINT:
  for (auto& c : m_rcv_data)
    if (c == ';') c = '\n';
  m_rcv_data[m_rcv_data.size()] = '\0';

  if (!m_rcv_data.empty())
  {
    MRPT_LOG_INFO_STREAM(
        "\n------------- HOKUYO Scanner: Version Information ------\n"
        << &m_rcv_data[0]
        << "\n"
           "-------------------------------------------------------\n\n");
  }
  return true;
}

/*-------------------------------------------------------------
            displaySensorInfo
-------------------------------------------------------------*/
bool CHokuyoURG::displaySensorInfo(TSensorInfo* out_data)
{
  if (!ensureStreamIsOpen()) return false;

  MRPT_LOG_DEBUG("[CHokuyoURG::displaySensorInfo] Asking for info...");

  // Send command:
  sendCmd("PP\x0A");

  // Receive response:
  if (!parseResponse())
  {
    MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
    return false;
  }
  MRPT_LOG_DEBUG("OK\n");

  // PRINT:
  for (auto& c : m_rcv_data)
    if (c == ';') c = '\n';
  m_rcv_data[m_rcv_data.size()] = '\0';

  if (!m_rcv_data.empty())
  {
    MRPT_LOG_INFO_STREAM(
        "\n------------- HOKUYO Scanner: Product Information ------\n"
        << &m_rcv_data[0]
        << "\n"
           "-------------------------------------------------------\n\n");
  }

  // Parse the data:
  if (out_data)
  {
    const char* ptr;

    if (nullptr != (ptr = strstr(&m_rcv_data[0], "DMAX:")))
      out_data->d_max = 0.001 * atoi(ptr + 5);
    else
      MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");

    if (nullptr != (ptr = strstr(&m_rcv_data[0], "DMIN:")))
      out_data->d_min = 0.001 * atoi(ptr + 5);
    else
      MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");

    if (nullptr != (ptr = strstr(&m_rcv_data[0], "ARES:")))
      out_data->scans_per_360deg = atoi(ptr + 5);
    else
      MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");

    if (nullptr != (ptr = strstr(&m_rcv_data[0], "SCAN:")))
      out_data->motor_speed_rpm = atoi(ptr + 5);
    else
      MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");

    if (nullptr != (ptr = strstr(&m_rcv_data[0], "AMIN:")))
      out_data->scan_first = atoi(ptr + 5);
    else
      MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");

    if (nullptr != (ptr = strstr(&m_rcv_data[0], "AMAX:")))
      out_data->scan_last = atoi(ptr + 5);
    else
      MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");

    if (nullptr != (ptr = strstr(&m_rcv_data[0], "AFRT:")))
      out_data->scan_front = atoi(ptr + 5);
    else
      MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");

    if (nullptr != (ptr = strstr(&m_rcv_data[0], "MODL:")))
    {
      char aux[30];
      memcpy(aux, ptr + 5, 8);
      aux[8] = '\0';
      out_data->model = aux;
    }
    else
      MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
  }

  return true;
}

bool CHokuyoURG::startScanningMode()
{
  if (!ensureStreamIsOpen()) return false;

  MRPT_LOG_DEBUG("[CHokuyoURG::startScanningMode] Starting scanning mode...");

  // Send command:
  // 'M' 'D'
  // 'XXXX' (starting step)
  // 'XXXX' (end step)
  // 'XX' (cluster count)
  // 'X' (scan interval)
  // 'XX' (number of scans)
  char cmd[50];
  unsigned int scan_interval = m_scan_interval;
  if (scan_interval > 9) scan_interval = 9;
  os::sprintf(
      cmd, 50, "M%c%04u%04u01%u00\x0A", m_intensity ? 'E' : 'D', m_firstRange, m_lastRange,
      scan_interval);

  sendCmd(cmd);

  // Receive response:
  if (!parseResponse())
  {
    MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
    return false;
  }

  MRPT_LOG_DEBUG("OK\n");
  return true;
}

bool CHokuyoURG::ensureStreamIsOpen()
{
  MRPT_START

  if (m_stream)
  {
    // Socket or USB connection?
    if (!m_ip_dir.empty() && m_port_dir)
    {
      // Has the port been disconected (USB serial ports)??
      auto* COM = dynamic_cast<CClientTCPSocket*>(m_stream.get());

      if (COM != nullptr)
      {
        if (COM->isConnected()) return true;

        // It has been disconnected... try to reconnect:
        MRPT_LOG_ERROR(
            "[CHokuyoURG] Socket connection lost! trying to "
            "reconnect...");

        try
        {
          COM->connect(m_ip_dir, m_port_dir);
          // OK, reconfigure the laser:
          turnOn();
          return true;
        }
        catch (...)
        {
          // Not yet..
          return false;
        }
      }
      else
      {
        return true;  // Assume OK
      }
    }
    else
    {
      // Has the port been disconected (USB serial ports)??
      auto* COM = dynamic_cast<CSerialPort*>(m_stream.get());
      if (COM != nullptr)
      {
        if (COM->isOpen()) return true;

        // It has been disconnected... try to reconnect:
        MRPT_LOG_ERROR_STREAM(
            __CURRENT_FUNCTION_NAME__ << ": Serial port connection lost! Trying to reconnect...");

        try
        {
          COM->open();
          // OK, reconfigure the laser:
          turnOn();
          return true;
        }
        catch (...)
        {
          // Not yet..
          return false;
        }
      }
      else
      {
        return true;  // Assume OK
      }
    }
  }
  else
  {
    if (m_com_port.empty() && (m_ip_dir.empty() || !m_port_dir))
    {
      THROW_EXCEPTION(
          "No stream bound to the laser nor COM serial port or ip and "
          "port provided in 'm_com_port','m_ip_dir' and 'm_port_dir'");
    }

    if (!m_ip_dir.empty())
    {
      // Connect to the TCP/IP port:
      auto theCOM = std::make_shared<CClientTCPSocket>();

      MRPT_LOG_INFO_STREAM(
          __CURRENT_FUNCTION_NAME__ << " Connecting to " << m_ip_dir << ":" << m_port_dir);
      theCOM->connect(m_ip_dir, m_port_dir);

      if (!theCOM->isConnected())
      {
        MRPT_LOG_ERROR_STREAM(
            __CURRENT_FUNCTION_NAME__ << " Cannot connect with the server '" << m_com_port << "'");
        return false;
      }

      // Bind:
      bindIO(theCOM);
    }
    else
    {
      // Try to open the serial port:
      auto theCOM = std::make_shared<CSerialPort>(m_com_port, true);

      if (!theCOM->isOpen())
      {
        MRPT_LOG_ERROR_STREAM(
            __CURRENT_FUNCTION_NAME__ << " Cannot open serial port '" << m_com_port << "'");
        return false;
      }

      // Bind:
      bindIO(theCOM);
    }

    // (re)connected to the sensor. Configure the laser:
    turnOn();

    return true;
  }
  MRPT_END
}

void CHokuyoURG::initialize()
{
  if (m_verbose) this->setMinLoggingLevel(mrpt::system::LVL_DEBUG);

  if (!ensureStreamIsOpen() || !turnOn())
  {
    m_state = ssError;

    MRPT_LOG_ERROR("[Hokuyo] Error initializing HOKUYO scanner");
    return;
  }
}

void CHokuyoURG::purgeBuffers()
{
  if (!ensureStreamIsOpen()) return;

  if (m_ip_dir.empty())
  {
    auto* COM = dynamic_cast<CSerialPort*>(m_stream.get());
    if (COM != nullptr)
    {
      COM->purgeBuffers();
    }
  }
  else  // Socket connection
  {
    auto* COM = dynamic_cast<CClientTCPSocket*>(m_stream.get());

    size_t to_read = COM->getReadPendingBytes();

    if (to_read)
    {
      void* buf = malloc(sizeof(uint8_t) * to_read);

      size_t nRead = m_stream->Read(buf, to_read);

      if (nRead != to_read)
        THROW_EXCEPTION(
            "Error in purge buffers: read and expected number of bytes "
            "are different.");

      free(buf);
    }
  }
}
