/* +------------------------------------------------------------------------+
   |                     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          |
   +------------------------------------------------------------------------+ */

#include "hwdrivers-precomp.h"	// Precompiled headers
//
#include <mrpt/hwdrivers/CSickLaserUSB.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/crc.h>
#include <mrpt/system/os.h>

#include <chrono>
#include <thread>

IMPLEMENTS_GENERIC_SENSOR(CSickLaserUSB, mrpt::hwdrivers)

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

/*-------------------------------------------------------------
						CSickLaserUSB
-------------------------------------------------------------*/
CSickLaserUSB::CSickLaserUSB() : m_serialNumber("LASER001")
{
	MRPT_START
	m_sensorLabel = "SICKLMS";
	m_usbConnection = std::make_unique<CInterfaceFTDI>();
	MRPT_END
}

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

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

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

	m_state = ssWorking;

	// Wait for a scan:
	if (!waitContinuousSampleFrame(
			ranges, LMS_stat, board_timestamp, is_mm_mode))
		return;

	// Yes, we have a new scan:

	// -----------------------------------------------
	//   Extract the observation:
	// -----------------------------------------------
	uint32_t AtUI = 0;
	if (m_timeStartUI == 0)
	{
		m_timeStartUI = board_timestamp;
		m_timeStartTT = mrpt::system::now();
	}
	else
		AtUI = board_timestamp - m_timeStartUI;

	/* Board time is ms, -50ms aprox delay for scan sending from the scanner*/
	auto AtDO = std::chrono::milliseconds(AtUI - 50);
	outObservation.timestamp = m_timeStartTT + AtDO;

	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 = m_sensorPose;

	outObservation.resizeScan(ranges.size());

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

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

	outThereIsObservation = true;
}

/*-------------------------------------------------------------
						loadConfig_sensorSpecific
-------------------------------------------------------------*/
void CSickLaserUSB::loadConfig_sensorSpecific(
	const mrpt::config::CConfigFileBase& configSource,
	const std::string& iniSection)
{
	m_serialNumber = configSource.read_string(
		iniSection, "SICKUSB_serialNumber", m_serialNumber);
	m_sensorPose = CPose3D(
		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)));
	// Parent options:
	C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
}

/*-------------------------------------------------------------
						turnOn
-------------------------------------------------------------*/
bool CSickLaserUSB::turnOn() { return true; }
/*-------------------------------------------------------------
						turnOff
-------------------------------------------------------------*/
bool CSickLaserUSB::turnOff() { return true; }
/*-------------------------------------------------------------
					checkControllerIsConnected
-------------------------------------------------------------*/
bool CSickLaserUSB::checkControllerIsConnected()
{
	// If device is already open, thats ok:
	if (m_usbConnection->isOpen()) return true;

	// If it isn't, try to open it now:
	try
	{
		m_usbConnection->OpenBySerialNumber(m_serialNumber);
		m_usbConnection->ResetDevice();
		std::this_thread::sleep_for(10ms);
		m_usbConnection->SetTimeouts(10, 20);  // read, write, in milliseconds
		std::this_thread::sleep_for(10ms);
		m_usbConnection->SetLatencyTimer(1);  // 1ms, the minimum
		std::this_thread::sleep_for(10ms);

		MRPT_LOG_INFO_FMT(
			"[CSickLaserUSB] USB DEVICE S/N:'%s' OPEN SUCCESSFULLY!!!\n",
			m_serialNumber.c_str());
		return true;
	}
	catch (const std::exception& e)
	{
		MRPT_LOG_ERROR_FMT(
			"[CSickLaserUSB] ERROR TRYING TO OPEN USB DEVICE S/N:'%s'\n%s",
			m_serialNumber.c_str(), e.what());
		return false;
	}
}

/*-------------------------------------------------------------
					waitContinuousSampleFrame
-------------------------------------------------------------*/
bool CSickLaserUSB::waitContinuousSampleFrame(
	vector<float>& out_ranges_meters, unsigned char& LMS_status,
	uint32_t& out_board_timestamp, bool& is_mm_mode)
{
	size_t nRead, nBytesToRead;
	size_t nFrameBytes = 0;
	size_t lenghtField;
	unsigned char buf[2000];
	buf[2] = buf[3] = 0;

	while (nFrameBytes < (lenghtField = (6 + (buf[2] | (buf[3] << 8)))) +
			   5 /* for 32bit timestamp + end-flag */)
	{
		if (lenghtField > 800)
		{
			cout << "#";
			nFrameBytes = 0;  // No es cabecera de trama correcta
			buf[2] = buf[3] = 0;
		}

		if (nFrameBytes < 4) nBytesToRead = 1;
		else
			nBytesToRead =
				(5 /* for 32bit timestamp + end-flag */ + lenghtField) -
				nFrameBytes;

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

		if (nRead == 0 && nFrameBytes == 0) return false;

		if (nRead > 0)
		{
			// 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;
			}
		}
	}

	// 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[lenghtField - 3];

	// End frame:
	if (buf[nFrameBytes - 1] != 0x55)
	{
		MRPT_LOG_ERROR(
			"[CSickLaserUSB::waitContinuousSampleFrame] bad end flag");
		return false;  // Bad CRC
	}

	// CRC:
	const uint16_t CRC = mrpt::system::compute_CRC16(buf, lenghtField - 2);
	const uint16_t CRC_packet =
		buf[lenghtField - 2] | (buf[lenghtField - 1] << 8);
	if (CRC_packet != CRC)
	{
		MRPT_LOG_ERROR_FMT(
			"[CSickLaserUSB::waitContinuousSampleFrame] bad CRC len=%u "
			"nptns=%u: %i != %i\n",
			unsigned(lenghtField), unsigned(n_points), CRC_packet, CRC);
		return false;  // Bad CRC
	}

	// Get USB board timestamp:
	out_board_timestamp = (uint32_t(buf[nFrameBytes - 5]) << 24) |
		(uint32_t(buf[nFrameBytes - 4]) << 16) |
		(uint32_t(buf[nFrameBytes - 3]) << 8) | uint32_t(buf[nFrameBytes - 2]);

	// All OK
	return true;
}
