/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

// This file contains portions of code from sicklms200.cc from the Player/Stage
// project.
#include "hwdrivers-precomp.h"	// Precompiled headers
//

#include <mrpt/hwdrivers/CSickLaserSerial.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/crc.h>
#include <mrpt/system/os.h>

#include <chrono>
#include <thread>

IMPLEMENTS_GENERIC_SENSOR(CSickLaserSerial, mrpt::hwdrivers)

#define RET_ERROR(msg)                                                         \
	{                                                                          \
		cout << "[" << __CURRENT_FUNCTION_NAME__ << "] " << msg << endl;       \
		return false;                                                          \
	}

using namespace std;
using namespace mrpt;
using namespace mrpt::comms;
using namespace mrpt::obs;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace mrpt::hwdrivers;
using namespace mrpt::system;

int CSickLaserSerial::CRC16_GEN_POL = 0x8005;

/*-------------------------------------------------------------
						CSickLaserSerial
-------------------------------------------------------------*/
CSickLaserSerial::CSickLaserSerial() : m_com_port()

{
	m_sensorLabel = "SICKLMS";
	memset(m_received_frame_buffer, 0, sizeof(m_received_frame_buffer));
}

/*-------------------------------------------------------------
						~CSickLaserSerial
-------------------------------------------------------------*/
CSickLaserSerial::~CSickLaserSerial()
{
	if (m_stream)
	{
		try
		{
			if (!m_skip_laser_config) { LMS_endContinuousMode(); }
		}
		catch (...)
		{
		}
	}
}

/*-------------------------------------------------------------
						doProcessSimple
-------------------------------------------------------------*/
void CSickLaserSerial::doProcessSimple(
	bool& outThereIsObservation,
	mrpt::obs::CObservation2DRangeScan& outObservation, bool& hardwareError)
{
	outThereIsObservation = false;
	hardwareError = false;

	if (!tryToOpenComms())
	{
		hardwareError = true;
		return;
	}

	vector<float> ranges;
	unsigned char LMS_stat;
	bool is_mm_mode;

	m_state = ssWorking;

	// Wait for a scan:
	if (!waitContinuousSampleFrame(ranges, LMS_stat, is_mm_mode))
	{
		if (!internal_notifyNoScanReceived()) { hardwareError = true; }
		return;
	}

	// Yes, we have a new scan:

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

	outObservation.sensorLabel = m_sensorLabel;	 // Set label

	// Extract the timestamp of the sensor:

	// And the scan ranges:
	outObservation.rightToLeft = true;
	outObservation.aperture = M_PIf;
	outObservation.maxRange = is_mm_mode ? 32.7 : 81.0;
	outObservation.stdError = 0.003f;
	outObservation.sensorPose = mrpt::poses::CPose3D(m_sensorPose);

	outObservation.resizeScan(ranges.size());

	for (size_t i = 0; i < ranges.size(); i++)
	{
		outObservation.setScanRange(i, ranges[i]);
		outObservation.setScanRangeValidity(
			i, (outObservation.getScanRange(i) <= outObservation.maxRange));
	}

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

	outThereIsObservation = true;
	internal_notifyGoodScanNow();
}

/*-------------------------------------------------------------
						loadConfig_sensorSpecific
-------------------------------------------------------------*/
void CSickLaserSerial::loadConfig_sensorSpecific(
	const mrpt::config::CConfigFileBase& configSource,
	const std::string& iniSection)
{
	m_sensorPose = TPose3D(
		configSource.read_float(iniSection, "pose_x", 0),
		configSource.read_float(iniSection, "pose_y", 0),
		configSource.read_float(iniSection, "pose_z", 0),
		DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
		DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
		DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));

	m_mm_mode = configSource.read_bool(iniSection, "mm_mode", m_mm_mode);

#ifdef _WIN32
	m_com_port =
		configSource.read_string(iniSection, "COM_port_WIN", m_com_port, true);
#else
	m_com_port =
		configSource.read_string(iniSection, "COM_port_LIN", m_com_port, true);
#endif

	m_com_baudRate =
		configSource.read_int(iniSection, "COM_baudRate", m_com_baudRate);
	m_nTries_connect =
		configSource.read_int(iniSection, "nTries_connect", m_nTries_connect);

	m_scans_FOV = configSource.read_int(iniSection, "FOV", m_scans_FOV);
	m_scans_res = configSource.read_int(iniSection, "resolution", m_scans_res);

	m_skip_laser_config = configSource.read_bool(
		iniSection, "skip_laser_config", m_skip_laser_config);

	// Parent options:
	C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
}

/*-------------------------------------------------------------
						turnOn
-------------------------------------------------------------*/
bool CSickLaserSerial::turnOn() { return true; }
/*-------------------------------------------------------------
						turnOff
-------------------------------------------------------------*/
bool CSickLaserSerial::turnOff() { return true; }
/*-------------------------------------------------------------
 Tries to open the com port and setup
 all the LMS protocol. Returns true if OK or already open.
-------------------------------------------------------------*/
bool CSickLaserSerial::tryToOpenComms(std::string* err_msg)
{
	if (err_msg) *err_msg = "";
	try
	{
		if (!m_stream)
		{
			ASSERT_(m_mySerialPort == nullptr);

			// There is no COMMS port open yet...
			if (!m_com_port.empty())
			{
				// Create the port myself:
				m_mySerialPort = std::make_shared<CSerialPort>();
				m_stream = std::shared_ptr<mrpt::io::CStream>(m_mySerialPort);
			}
			else
				throw std::logic_error(
					"ERROR: No serial port attached with bindIO, neither it "
					"set with 'setSerialPort'");
		}

		// We assure now we have a stream... try to open it, if it's not done
		// yet.
		bool just_open = false;
		auto* COM = dynamic_cast<CSerialPort*>(m_stream.get());
		if (COM != nullptr)
		{
			if (!COM->isOpen())
			{
				// Try to open it now:
				COM->setSerialPortName(m_com_port);
				COM->open();  // will raise an exception on error.

				// Set basic params:
				COM->setConfig(9600);
				COM->setTimeouts(100, 0, 10, 0, 50);

				just_open = true;
			}
		}

		// It seems the port was open and working so we are done here.
		if (!just_open) return true;

		// ==================================================================
		// Otherwise, it was just opened now, we must send the
		//  ** initialization commands **
		// and put the laser in continuous measuring mode:
		// ==================================================================
		if (!m_skip_laser_config)
		{
			if (!LMS_setupSerialComms()) RET_ERROR("error");

			bool res;
			for (int nTry = 0; nTry < 4; nTry++)
				if (true == (res = LMS_sendMeasuringMode_cm_mm())) break;

			if (!res) return false;

			for (int nTry = 0; nTry < 4; nTry++)
				if (true == (res = LMS_startContinuousMode())) break;

			return res;
		}
		else
		{
			// Skip setup:
			return true;
		}
	}
	catch (const std::exception& e)
	{
		std::string s = "[CSickLaserSerial] Error trying to open SICK at port ";
		s += e.what();
		if (err_msg) *err_msg = s;
		MRPT_LOG_ERROR(s);
		return false;
	}
}

/*-------------------------------------------------------------
					waitContinuousSampleFrame
-------------------------------------------------------------*/
bool CSickLaserSerial::waitContinuousSampleFrame(
	vector<float>& out_ranges_meters, unsigned char& LMS_status,
	bool& is_mm_mode)
{
	auto* COM = dynamic_cast<CSerialPort*>(m_stream.get());
	ASSERTMSG_(COM != nullptr, "No I/O channel bound to this object");

	size_t nRead, nBytesToRead;
	size_t nFrameBytes = 0;
	size_t lengthField;
	unsigned char buf[2000];
	buf[2] = buf[3] = buf[4] = 0;

	while (nFrameBytes < (lengthField = (6 + (buf[2] | (buf[3] << 8)))))
	{
		if (lengthField > 800)
		{
			cout << "#";
			nFrameBytes = 0;  // No es cabecera de trama correcta
			buf[2] = buf[3] = 0;
		}

		if (nFrameBytes < 4) nBytesToRead = 1;
		else
			nBytesToRead = (lengthField)-nFrameBytes;

		try
		{
			nRead = COM->Read(buf + nFrameBytes, nBytesToRead);
		}
		catch (const std::exception& e)
		{
			// Disconnected?
			MRPT_LOG_ERROR_FMT(
				"[CSickLaserSerial::waitContinuousSampleFrame] Disconnecting "
				"due to comms error: %s\n",
				e.what());
			// m_usbConnection->Close();
			return false;
		}

		if (!nRead && !nFrameBytes) return false;

		if (nRead < nBytesToRead) std::this_thread::sleep_for(1ms);

		// Lectura OK:
		// Era la primera?
		if (nFrameBytes > 1 || (!nFrameBytes && buf[0] == 0x02) ||
			(nFrameBytes == 1 && buf[1] == 0x80))
		{
			nFrameBytes += nRead;
		}
		else
		{
			nFrameBytes = 0;  // No es cabecera de trama correcta
			buf[2] = buf[3] = 0;
			// cerr << "."; //"[CSickLaserSerial] Skipping non-header..." <<
			// endl;
		}
	}

	// Frame received
	// --------------------------------------------------------------------------
	// | STX | ADDR | L1 | L2 | COM | INF1 | INF2 |	DATA	| STA | CRC1 | CRC2
	// |
	// --------------------------------------------------------------------------

	// Trama completa:
	//  Checkear que el byte de comando es 0xB0:
	if (buf[4] != 0xB0) return false;

	// GET FRAME INFO
	int info = buf[5] | (buf[6] << 8);	// Little Endian
	int n_points = info & 0x01FF;
	is_mm_mode = 0 != ((info & 0xC000) >> 14);	// 0x00: cm 0x01: mm

	out_ranges_meters.resize(n_points);

	// Copiar rangos:
	short mask = is_mm_mode ? 0x7FFF : 0x1FFF;
	float meters_scale = is_mm_mode ? 0.001f : 0.01f;

	for (int i = 0; i < n_points; i++)
		out_ranges_meters[i] =
			((buf[7 + i * 2] | (buf[8 + i * 2] << 8)) & mask) * meters_scale;

	// Status
	LMS_status = buf[lengthField - 3];

	// CRC:
	uint16_t CRC =
		mrpt::system::compute_CRC16(buf, lengthField - 2, CRC16_GEN_POL);
	uint16_t CRC_packet = buf[lengthField - 2] | (buf[lengthField - 1] << 8);
	if (CRC_packet != CRC)
	{
		cerr << format(
					"[CSickLaserSerial::waitContinuousSampleFrame] bad CRC "
					"len=%u nptns=%u: %i != %i",
					unsigned(lengthField), unsigned(n_points), CRC_packet, CRC)
			 << endl;
		return false;  // Bad CRC
	}

	// All OK
	return true;
}

/*-------------------------------------------------------------
						initialize
-------------------------------------------------------------*/
void CSickLaserSerial::initialize()
{
	string err_str;
	memset(m_received_frame_buffer, 0, sizeof(m_received_frame_buffer));
	if (!tryToOpenComms(&err_str))
	{
		cerr << err_str << endl;
		throw std::logic_error(err_str);
	}
}

/*-----------------------------------------------------------------
	Assures laser is connected and operating at 38400, in
	 its case returns true.
  -----------------------------------------------------------------*/
bool CSickLaserSerial::LMS_setupSerialComms()
{
	ASSERT_(
		m_com_baudRate == 9600 || m_com_baudRate == 38400 ||
		m_com_baudRate == 500000);

	auto* COM = dynamic_cast<CSerialPort*>(m_stream.get());
	if (COM == nullptr) return true;

	int detected_rate = 0;
	for (size_t reps = 0; !detected_rate && reps < m_nTries_connect; reps++)
	{
		m_nTries_current = reps;

		int rates[] = {0, 9600, 38400, 500000};

		// Try first the desired rate to speed up the process, just in case
		//  the laser is already setup from a previous run:
		rates[0] = m_com_baudRate;

		detected_rate = 0;

		for (size_t i = 0;
			 !detected_rate && i < sizeof(rates) / sizeof(rates[0]); i++)
		{
			// Are we already receiving at 500k?
			// ------------------------------------------------
			COM->setConfig(rates[i]);
			if (m_verbose)
				printf(
					"[CSickLaserSerial] Testing if the scanner is "
					"set to %i bauds...\n",
					rates[i]);

			LMS_endContinuousMode();  // Stop continuous mode.
			std::this_thread::sleep_for(100ms);
			COM->purgeBuffers();

			for (int nTry = 0; nTry < 4 && !detected_rate; nTry++)
			{
				COM->purgeBuffers();
				// Ask for the laser status at the current rate:
				if (LMS_statusQuery())
				{
					detected_rate = rates[i];
					break;
				}
				std::this_thread::sleep_for(20ms);
			}  // for tries
			// There is no link, or the baudrate is wrong...
		}

		// Try again in a while:
		if (!detected_rate && reps != (m_nTries_connect - 1))
			std::this_thread::sleep_for(5000ms);
	}

	// Are we connected at the right rate?
	if (detected_rate == m_com_baudRate) return true;

	// Switch to the desired rate now
	if (!this->LMS_setupBaudrate(m_com_baudRate)) RET_ERROR("error");

	// Check response is OK:
	if (!(m_received_frame_buffer[2] == 0x03 &&
		  m_received_frame_buffer[4] == 0xA0 &&
		  m_received_frame_buffer[6] == 0x10))
		return false;

	COM->setConfig(m_com_baudRate);
	COM->purgeBuffers();

	// Wait...
	std::this_thread::sleep_for(500ms);

	// And check comms at the new baud rate:
	return LMS_statusQuery();
}

/*-----------------------------------------------------------------
 Query to LMS a baudrate change command.
   Returns true if response is read ok.
  -----------------------------------------------------------------*/
bool CSickLaserSerial::LMS_setupBaudrate(int baud)
{
	ASSERT_(m_stream);
	if (m_verbose)
		printf("[CSickLaserSerial::LMS_setupBaudrate] rate=%i\n", baud);

	uint8_t cmd[4];
	cmd[0] = 0x20;
	switch (baud)
	{
		case 9600: cmd[1] = 0x42; break;
		case 19200: cmd[1] = 0x41; break;
		case 38400: cmd[1] = 0x40; break;
		case 500000: cmd[1] = 0x48; break;
		default: THROW_EXCEPTION("Invalid baud rate value");
	}

	uint16_t cmd_len = 2;

	if (!SendCommandToSICK(cmd, cmd_len)) return false;
	return LMS_waitIncomingFrame(500);
}

/*-----------------------------------------------------------------
  Query to LMS a status query.
   Returns true if response is read ok.
  -----------------------------------------------------------------*/
bool CSickLaserSerial::LMS_statusQuery()
{
	ASSERT_(m_stream);

	uint8_t cmd[1];
	cmd[0] = 0x31;
	uint16_t cmd_len = 1;

	if (!SendCommandToSICK(cmd, cmd_len)) return false;
	return LMS_waitIncomingFrame(500);
}

// Returns false if timeout
bool CSickLaserSerial::LMS_waitACK(uint16_t timeout_ms)
{
	auto* COM = dynamic_cast<CSerialPort*>(m_stream.get());
	ASSERT_(COM);

	uint8_t b = 0;
	CTicTac tictac;
	tictac.Tic();

	do
	{
		if (COM->Read(&b, 1))
		{  // Byte rx:
			if (b == 0x06) return true;
		}
	} while (tictac.Tac() < timeout_ms * 1e-3);

	if (b == 0x15) RET_ERROR("NACK received.")
	else if (b != 0)
		RET_ERROR(format("Unexpected code received: 0x%02X", b))
	else
		return false;  // RET_ERROR("Timeout")
}

// Returns false if timeout
bool CSickLaserSerial::LMS_waitIncomingFrame(uint16_t timeout)
{
	auto* COM = dynamic_cast<CSerialPort*>(m_stream.get());
	ASSERT_(COM);

	uint8_t b;
	unsigned int nBytes = 0;

	CTicTac tictac;
	tictac.Tic();
	const double maxTime = timeout * 1e-3;

	while (nBytes < 6 ||
		   (nBytes < (6U + m_received_frame_buffer[2] +
					  (uint16_t)(m_received_frame_buffer[3] << 8))))
	{
		if (COM->Read(&b, 1))
		{
			// First byte must be STX:
			if (nBytes > 1 || (!nBytes && b == 0x02) ||
				(nBytes == 1 && b == 0x80))
			{
				// Store in frame:
				m_received_frame_buffer[nBytes] = b;
				nBytes++;
				if (m_verbose)
				{
					printf("[CSickLaserSerial::Receive] RX: %02X\n", b);
				}
			}
		}
		if (tictac.Tac() >= maxTime) return false;	// Timeout
	}

	const uint16_t lengthField =
		m_received_frame_buffer[2] + (m_received_frame_buffer[3] << 8);
	// Check len:
	if (4U + lengthField + 2U != nBytes)
	{
		printf(
			"[CSickLaserSerial::LMS_waitIncomingFrame] Error: expected %u "
			"bytes, received %u\n",
			4U + lengthField + 2U, nBytes);
		return false;
	}

	// Check CRC
	uint16_t CRC = mrpt::system::compute_CRC16(
		m_received_frame_buffer, 4 + lengthField, CRC16_GEN_POL);
	uint16_t CRC_packet = m_received_frame_buffer[4 + lengthField + 0] |
		(m_received_frame_buffer[4 + lengthField + 1] << 8);
	if (CRC_packet != CRC)
	{
		printf(
			"[CSickLaserSerial::LMS_waitIncomingFrame] Error in CRC: rx: "
			"0x%04X, computed: 0x%04X\n",
			CRC_packet, CRC);
		return false;
	}

#if 0
    printf("RX: ");
    for (unsigned int i=0;i<nBytes;i++)
        printf("%02X ",m_received_frame_buffer[i]);
    printf("\n");
#endif

	// OK
	return true;
}

bool CSickLaserSerial::LMS_sendMeasuringMode_cm_mm()
{
	auto* COM = dynamic_cast<CSerialPort*>(m_stream.get());
	ASSERT_(COM);

	// **************************
	// Send command: Switch to Installation mode
	// **************************
	uint8_t cmd[128];  // =
	// {0x02,0x00,0x0A,0x00,0x20,0x00,0x53,0x49,0x43,0x4B,0x5F,0x4C,0x4D,0x53,0xBE,0xC5};
	cmd[0] = 0x20; /* mode change command */
	cmd[1] = 0x00; /* configuration mode */
	cmd[2] = 0x53;	// S - the password
	cmd[3] = 0x49;	// I
	cmd[4] = 0x43;	// C
	cmd[5] = 0x4B;	// K
	cmd[6] = 0x5F;	// _
	cmd[7] = 0x4C;	// L
	cmd[8] = 0x4D;	// M
	cmd[9] = 0x53;	// S

	uint16_t cmd_len = 10;
	if (!SendCommandToSICK(cmd, cmd_len))
		RET_ERROR("Error waiting ACK to installation mode");
	if (!LMS_waitIncomingFrame(500))
		RET_ERROR("Error in response to installation mode");

	// Check response
	if (!(m_received_frame_buffer[4] == 0xA0 &&
		  m_received_frame_buffer[5] == 0x00))
		RET_ERROR("Wrong response to installation mode");

	// **************************
	// Request LMS Configuration
	// **************************
	cmd[0] = 0x74;
	cmd_len = 1;

	if (!SendCommandToSICK(cmd, cmd_len))
		RET_ERROR("No ACK to 0x74 (req. config)");
	if (!LMS_waitIncomingFrame(500))
		RET_ERROR("No answer to 0x74 (req. config)");

	// 2. Check response
	if (m_received_frame_buffer[4] != 0xF4)
		RET_ERROR("No expected 0xF4 in response to 0x74 (req. config)");

	// ***********************************************************************
	// Configure 1/2: Measuremente Range, Measurement Unit, Master/Slave Role
	// ***********************************************************************
	// 4.a Modify some values...

	// Measuring mode: Measurement range 32m in mm mode, or 80m+reflectance info
	// in cm mode.
	// See page 98 in LMS2xx_list_datagrams.pdf.
	m_received_frame_buffer[10] = this->m_mm_mode ? 0x06 : 0x00;
	m_received_frame_buffer[11] = this->m_mm_mode ? 0x01 : 0x00;

	// 4.2 Build the output command
	m_received_frame_buffer[1] = 0x00;	// Address
	m_received_frame_buffer[2] = 0x23;	// Length (low byte)
	m_received_frame_buffer[3] = 0x00;	// Length (high byte)
	m_received_frame_buffer[4] = 0x77;	// Configure command

	memcpy(cmd, m_received_frame_buffer + 4, 0x23);
	cmd_len = 0x23;

	// 4.4 Send to the LMS
	if (!SendCommandToSICK(cmd, cmd_len))
		RET_ERROR("No ACK for config command (0x77)");
	if (!LMS_waitIncomingFrame(600))
		RET_ERROR("No answer for config command (0x77)");

	if (!(m_received_frame_buffer[4] == 0xF7 &&
		  m_received_frame_buffer[5] == 0x01))
		RET_ERROR("Wrong answer for config command (0x77)");

	// **************************
	// Switch to Monitoring mode
	// **************************
	cmd[0] = 0x20;
	cmd[1] = 0x25;
	cmd_len = 2;
	if (!SendCommandToSICK(cmd, cmd_len))
		RET_ERROR("No ACK for set monitoring mode");
	if (!LMS_waitIncomingFrame(500))
		RET_ERROR("No answer for set monitoring mode");

	if (!(m_received_frame_buffer[4] == 0xA0 &&
		  m_received_frame_buffer[5] == 0x00))
		RET_ERROR("Wrong answer for set monitoring mode");

	// All ok.
	return true;
}

/*-----------------------------------------------------------------
  Start continuous mode measurements.
   Returns true if response is read ok.
  -----------------------------------------------------------------*/
bool CSickLaserSerial::LMS_startContinuousMode()
{
	ASSERT_(m_scans_FOV == 100 || m_scans_FOV == 180);
	ASSERT_(m_scans_res == 25 || m_scans_res == 50 || m_scans_res == 100);

	auto* COM = dynamic_cast<CSerialPort*>(m_stream.get());
	ASSERT_(COM);

	uint8_t cmd[40];

	// Config angle/resolution
	cmd[0] = 0x3B;
	cmd[1] = m_scans_FOV;
	cmd[2] = 0x00;
	cmd[3] = m_scans_res;  // 25,50 or 100 -  1/100th of deg
	cmd[4] = 0x00;
	uint16_t cmd_len = 5;
	if (!SendCommandToSICK(cmd, cmd_len))
		RET_ERROR("Error waiting ack for change angle/resolution");
	if (!LMS_waitIncomingFrame(500))
		RET_ERROR("Error waiting answer for change angle/resolution");

	// Start continuous mode:
	cmd[0] = 0x20;
	cmd[1] = 0x24;
	cmd_len = 2;
	if (!SendCommandToSICK(cmd, cmd_len))
		RET_ERROR("Error waiting ack for start scanning");
	if (!LMS_waitIncomingFrame(500))
		RET_ERROR("Error waiting answer for start scanning");

	return true;
}

bool CSickLaserSerial::LMS_endContinuousMode()
{
	auto* COM = dynamic_cast<CSerialPort*>(m_stream.get());
	ASSERT_(COM);

	uint8_t cmd[40];

	// End continuous mode:
	cmd[0] = 0x20;
	cmd[1] = 0x25;
	uint16_t cmd_len = 2;
	if (!SendCommandToSICK(cmd, cmd_len)) return false;
	return LMS_waitIncomingFrame(50);
}

bool CSickLaserSerial::SendCommandToSICK(
	const uint8_t* cmd, const uint16_t cmd_len)
{
	uint8_t cmd_full[1024];
	ASSERT_(sizeof(cmd_full) > cmd_len + 4U + 2U);

	auto* COM = dynamic_cast<CSerialPort*>(m_stream.get());
	ASSERT_(COM);

	// Create header
	cmd_full[0] = 0x02;	 // STX
	cmd_full[1] = 0;  // ADDR
	cmd_full[2] = cmd_len & 0xFF;
	cmd_full[3] = cmd_len >> 8;

	memcpy(cmd_full + 4, cmd, cmd_len);

	const uint16_t crc =
		mrpt::system::compute_CRC16(cmd_full, 4 + cmd_len, CRC16_GEN_POL);
	cmd_full[4 + cmd_len + 0] = crc & 0xFF;
	cmd_full[4 + cmd_len + 1] = crc >> 8;

	const size_t toWrite = 4 + cmd_len + 2;

	if (m_verbose)
	{
		printf("[CSickLaserSerial::SendCommandToSICK] TX: ");
		for (unsigned int i = 0; i < toWrite; i++)
			printf("%02X ", cmd_full[i]);
		printf("\n");
	}

	const int NTRIES = 3;

	for (int k = 0; k < NTRIES; k++)
	{
		if (toWrite != COM->Write(cmd_full, toWrite))
		{
			cout << "[CSickLaserSerial::SendCommandToSICK] Error writing data "
					"to serial port."
				 << endl;
			return false;
		}
		std::this_thread::sleep_for(15ms);
		if (LMS_waitACK(50)) return true;
		std::this_thread::sleep_for(10ms);
	}

	return false;
}
