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

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 reseted (?), 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::system::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::system::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(
	const 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);
		}
	}
}
