/* +------------------------------------------------------------------------+
   |                     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
//
// This file contains portions of code from sicklms200.cc from the Player/Stage
// project.

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

#include <cstdio>  // printf
#include <cstring>	// memset
#include <iostream>
#include <thread>

IMPLEMENTS_GENERIC_SENSOR(CCANBusReader, mrpt::hwdrivers)

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

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

char hexCharToInt(char n)
{
	if (n >= '0' && n <= '9') return (n - '0');
	else

		if (n >= 'A' && n <= 'F')
		return (n - 'A' + 10);
	else
		return 0;
}

/*-------------------------------------------------------------
						CCANBusReader
-------------------------------------------------------------*/
CCANBusReader::CCANBusReader()
	: mrpt::system::COutputLogger("CCANBusReader"), m_com_port()

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

/*-------------------------------------------------------------
						~CCANBusReader
-------------------------------------------------------------*/
CCANBusReader::~CCANBusReader()
{
	if (m_CANBusChannel_isOpen)
	{
		try
		{
			CANBusCloseChannel();
		}
		catch (...)
		{
		}
	}

	if (m_mySerialPort)
	{
		delete m_mySerialPort;
		m_mySerialPort = nullptr;
	}
}

void CCANBusReader::doProcess()
{
	mrpt::obs::CObservationCANBusJ1939::Ptr obs =
		mrpt::obs::CObservationCANBusJ1939::Create();
	bool thereIsObservation;
	bool hardwareError;

	doProcessSimple(thereIsObservation, *obs, hardwareError);
	if (thereIsObservation) appendObservation(obs);
	else
		cout << "No frame received" << endl;
}

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

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

	m_state = ssWorking;

	// Wait for a scan:
	uint8_t out_prio, out_pdu_format, out_pdu_spec, out_src_address,
		out_data_length;
	uint16_t out_pgn;
	vector<uint8_t> out_data;
	vector<char> out_raw_frame;
	if (!waitContinuousSampleFrame(
			out_prio, out_pdu_format, out_pdu_spec, out_src_address,
			out_data_length, out_pgn, out_data, out_raw_frame))
		return;

	// Yes, we have a new scan:
	//    cout << "we've got a frame" << endl;
	// -----------------------------------------------
	//   Extract the observation:
	// -----------------------------------------------
	outObservation.timestamp = mrpt::system::now();
	outObservation.sensorLabel = m_sensorLabel;	 // Set label

	// And the scan ranges:
	outObservation.m_priority = out_prio;
	outObservation.m_pdu_spec = out_pdu_spec;
	outObservation.m_pdu_format = out_pdu_format;
	outObservation.m_src_address = out_src_address;
	outObservation.m_pgn = out_pgn;
	outObservation.m_data_length = out_data_length;
	outObservation.m_data.resize(out_data.size());
	for (size_t k = 0; k < out_data.size(); ++k)
		outObservation.m_data[k] = out_data[k];
	outObservation.m_raw_frame.resize(out_raw_frame.size());
	for (size_t k = 0; k < out_raw_frame.size(); ++k)
		outObservation.m_raw_frame[k] = out_raw_frame[k];

	// we've got a new observation
	outThereIsObservation = true;
}

/*-------------------------------------------------------------
						loadConfig_sensorSpecific
-------------------------------------------------------------*/
void CCANBusReader::loadConfig_sensorSpecific(
	const mrpt::config::CConfigFileBase& configSource,
	const std::string& iniSection)
{
	m_canbus_speed =
		configSource.read_int(iniSection, "CANBusSpeed", m_canbus_speed);
	m_canreader_timestamp = configSource.read_bool(
		iniSection, "useCANReaderTimestamp", m_canreader_timestamp);

#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);
}

/*-------------------------------------------------------------
 Tries to open the com port and setup
 all the LMS protocol. Returns true if OK or already open.
-------------------------------------------------------------*/
bool CCANBusReader::tryToOpenComms(std::string* err_msg)
{
	if (err_msg) *err_msg = "";
	try
	{
		if (!m_mySerialPort)
		{
			// There is no COMMS port open yet...
			if (!m_com_port.empty())
			{
				//			    cout << "Creating port" << endl;
				m_mySerialPort =
					new mrpt::comms::CSerialPort();	 // Create the port myself:
			}
			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;
		if (m_mySerialPort != nullptr)
		{
			if (!m_mySerialPort->isOpen())
			{
				// Try to open it now:
				m_mySerialPort->setSerialPortName(m_com_port);
				m_mySerialPort->open();	 // will raise an exception on error.

				// Set basic params:
				m_mySerialPort->setConfig(9600);
				m_mySerialPort->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 CAN Converter in recording mode:
		// ==================================================================
		cout << "Setting up serial comms in port " << m_com_port;
		if (!setupSerialComms()) RET_ERROR("error");
		cout << " ... done" << endl;

		// initialize
		// set CAN Bus speed
		/**/
		bool res;
		cout << "Setting up CAN BUS Speed at: " << m_canbus_speed << endl;
		for (int nTry = 0; nTry < 250000 /*4*/; nTry++)
			if (true == (res = sendCANBusReaderSpeed())) break;
		if (!res) return false;
		cout << " ... done" << endl;

		// open the CAN channel. If true, at this point, frames should be poping
		// out the CAN Bus
		cout << "Opening CAN BUS and starting to receive." << endl;
		for (int nTry = 0; nTry < 250000 /*4*/; nTry++)
			if (true == (res = CANBusOpenChannel())) break;
		if (!res) return false;
		cout << " ... done" << endl;

		//        cout << "Autopoll" << endl;
		//        for (int nTry=0;nTry<250000/*4*/;nTry++)
		//            if (true==(res=CANBusAutoPoll()))
		//                break;
		//        if(!res) return false;
		//        cout << " ... done" << endl;

		return res;
		/**/
	}
	catch (const std::exception& e)
	{
		std::string s =
			"[CCANBusReader] Error trying to open CANBusReader at port ";
		s += e.what();
		if (err_msg) *err_msg = s;
		MRPT_LOG_ERROR_STREAM(s);
		return false;
	}
}

/*-------------------------------------------------------------
 Tries to send the command to set up the speed of the
 CAN Bus reader -> tractor link
-------------------------------------------------------------*/
bool CCANBusReader::sendCANBusReaderSpeed()
{
	// command: S0 --> S8 according to the selected speed
	unsigned char cmd[2];

	cmd[0] = 'S';
	switch (m_canbus_speed)
	{
		case 10000: cmd[1] = '0'; break;
		case 20000: cmd[1] = '1'; break;
		case 50000: cmd[1] = '2'; break;
		case 100000: cmd[1] = '3'; break;
		case 125000: cmd[1] = '4'; break;
		case 250000: cmd[1] = '5'; break;
		case 500000: cmd[1] = '6'; break;
		case 800000: cmd[1] = '7'; break;
		case 1000000: cmd[1] = '8'; break;
		default: RET_ERROR("Incorrect CAN Bus speed"); break;
	}
	sendCommandToCANReader(cmd, 2);
	return waitACK(50);
}

bool CCANBusReader::CANBusOpenChannel()
{
	unsigned char cmd[1];
	cmd[0] = 'O';
	sendCommandToCANReader(cmd, 1);
	m_CANBusChannel_isOpen = waitACK(50);
	return m_CANBusChannel_isOpen;
}

bool CCANBusReader::CANBusCloseChannel()
{
	unsigned char cmd[1];
	cmd[0] = 'C';
	sendCommandToCANReader(cmd, 1);
	//    m_CANBusChannel_isOpen = !waitACK(50);
	m_CANBusChannel_isOpen = false;
	//    return !m_CANBusChannel_isOpen;
	return true;
}

bool CCANBusReader::CANBusAutoPoll()
{
	unsigned char cmd[1];
	cmd[0] = 'A';
	sendCommandToCANReader(cmd, 1);
	return waitACK(50);
}

bool CCANBusReader::CANBusX1()
{
	unsigned char cmd[2];
	cmd[0] = 'X';
	cmd[1] = '1';
	sendCommandToCANReader(cmd, 2);
	return waitACK(50);
}

bool CCANBusReader::CANBusPoll()
{
	unsigned char cmd[1];
	cmd[0] = 'P';
	sendCommandToCANReader(cmd, 1);
	return waitACK(50);
}

/*-------------------------------------------------------------
					waitContinuousSampleFrame
-------------------------------------------------------------*/
bool CCANBusReader::waitContinuousSampleFrame(
	uint8_t& out_prio, uint8_t& out_pdu_format, uint8_t& out_pdu_spec,
	uint8_t& out_src_address, uint8_t& out_data_length, uint16_t& out_pgn,
	vector<uint8_t>& out_data, vector<char>& out_raw_frame)
{
	size_t nRead, nBytesToRead;
	size_t nFrameBytes = 0;
	size_t lengthField;
	unsigned char buf[40];

	// clear buffer
	for (unsigned char& k : buf)
		k = 0;

	uint8_t dlc = 0;
	while (nFrameBytes < (lengthField = (10U + dlc + 1U)))
	{
		//	    cout << "trying to receive" << endl;
		if (lengthField > 30)
		{
			cout << "#" << int(dlc) << " ";
			nFrameBytes = 0;  // No es cabecera de trama correcta
			for (unsigned char& k : buf)
				k = 0;
			dlc = 0;
		}

		if (nFrameBytes < 10) nBytesToRead = 1;
		else
		{
			dlc = 2 * uint8_t(hexCharToInt(buf[9]));
			//		    cout << "dlc: " << int(dlc) << endl;
			nBytesToRead = (lengthField)-nFrameBytes;
		}

		try
		{
			nRead = m_mySerialPort->Read(buf + nFrameBytes, nBytesToRead);
			//			cout << "to read: " << nBytesToRead << " received: " <<
			// nRead << " -> ";
			//			for( uint8_t k = 0; k < nRead; ++k )
			//                cout << int(buf[k+nFrameBytes]);
			//            cout << endl;
		}
		catch (const std::exception& e)
		{
			// Disconnected?
			MRPT_LOG_ERROR_STREAM(
				"[waitContinuousSampleFrame] Disconnecting due to comms error: "
				<< e.what());
			return false;
		}

		if (!nRead) return false;

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

		// Reading OK:
		// Was it the first one?
		if (nFrameBytes > 0 || (nFrameBytes == 0 && buf[0] == 0x54 /*T*/))
			nFrameBytes += nRead;
		else
		{
			nFrameBytes = 0;  // No es cabecera de trama correcta
			for (unsigned char& k : buf)
				k = 0;
		}
	}  // end while

	// Process frame
	// convert ASCII text into integer
	vector<uint8_t> aux;
	out_raw_frame.resize(nFrameBytes);
	for (uint8_t k = 0; k < nFrameBytes; ++k)
	{
		aux.push_back(hexCharToInt(buf[k]));
		out_raw_frame[k] = buf[k];
	}

	out_prio = (aux[1] << 2) | (aux[2] >> 2);
	out_pdu_format = (aux[3] << 4) | aux[4];
	out_pdu_spec = (aux[5] << 4) | aux[6];
	out_src_address = (aux[7] << 4) | aux[8];
	out_data_length = aux[9];
	out_pgn = uint16_t(out_pdu_format) << 8 | uint16_t(out_pdu_spec);
	out_data.resize(out_data_length);
	for (uint8_t k = 0, k2 = 0; k < 2 * out_data_length; k += 2, k2++)
		out_data[k2] = (aux[10 + k] << 4) | aux[11 + k];

	if (buf[nFrameBytes - 1] != 0x0D)
	{
		cout << format(
					"[CCANBusReader::waitContinuousSampleFrame] expected 0x0D "
					"ending flag, 0x%X found instead",
					buf[nFrameBytes])
			 << endl;
		return false;  // Bad ending flag
	}

	// All OK
	return true;
}

/*-------------------------------------------------------------
						initialize
-------------------------------------------------------------*/
void CCANBusReader::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 CCANBusReader::setupSerialComms()
{
	ASSERT_(
		m_com_baudRate == 9600 || m_com_baudRate == 38400 ||
		m_com_baudRate == 57600 || m_com_baudRate == 500000);

	if (m_mySerialPort == 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, 57600, 500000};

		// Try first the desired rate to speed up the process, just in case
		//  the converter 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?
			// ------------------------------------------------
			m_mySerialPort->setConfig(rates[i]);
			std::this_thread::sleep_for(100ms);
			m_mySerialPort->purgeBuffers();

			// close the connection
			/**/
			cout << endl << "Closing CAN Channel " << endl;
			for (int nTry = 0; nTry < 250000 /*4*/; nTry++)
				if (true == CANBusCloseChannel()) break;
			cout << " ... done" << endl;
			/**/

			std::this_thread::sleep_for(100ms);
			m_mySerialPort->purgeBuffers();

			for (int nTry = 0; nTry < 250000 /*4*/ && !detected_rate; nTry++)
			{
				m_mySerialPort->purgeBuffers();

				// Ask for the laser version at the current rate:
				if (queryVersion(true))
				{
					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);
	}

	// Switch "this" serial port to the detected baudrate
	setBaudRate(detected_rate);

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

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

	// And check comms at the new baud rate:
	return true;
}

/*-----------------------------------------------------------------
  Query to LMS a status query.
   Returns true if response is read ok.
  -----------------------------------------------------------------*/
bool CCANBusReader::queryVersion(bool printOutVersion)
{
	ASSERT_(m_mySerialPort);

	uint8_t cmd[1];
	cmd[0] = 'V';
	uint16_t cmd_len = 1;

	if (!sendCommandToCANReader(cmd, cmd_len, false)) return false;
	return waitForVersion(500, printOutVersion);
}

// Returns false if timeout
bool CCANBusReader::waitACK(uint16_t timeout_ms)
{
	uint8_t b = 0;
	CTicTac tictac;
	tictac.Tic();

	do
	{
		if (m_mySerialPort->Read(&b, 1))
		{
			// Byte rx:
			if (b == 0x0D /*0x30*/)
			{
				cout << int(b) << endl;
				return true;  // [CR]
			}
		}
	} while (tictac.Tac() < timeout_ms * 1e-3);

	if (b == 0x07)	// [BELL]
		RET_ERROR("ERROR received.")
	else if (b != 0)
		RET_ERROR(format("Unexpected code received: 0x%02X", b))
	else
		return false;  // RET_ERROR("Timeout")
}

bool CCANBusReader::waitForVersion(uint16_t timeout, bool printOutVersion)
{
	uint8_t b;
	unsigned int nBytes = 0;

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

	while (nBytes < 6)
	{
		if (m_mySerialPort->Read(&b, 1))
		{
			//		    cout << "received " << nBytes << " bytes: " << char(b)
			//<<
			// endl;
			// First byte must be STX:
			if (nBytes > 0 || (nBytes == 0 && b == 'V'))
			{
				// Store in frame:
				m_received_frame_buffer[nBytes] = b;
				nBytes++;
			}
		}
		if (tictac.Tac() >= maxTime)
		{
			cout << "Version timeout" << endl;
			return false;  // Timeout
		}
	}

	// Check len:
	if (m_received_frame_buffer[nBytes - 1] != 0x0D)
	{
		printf(
			"[CCANBusReader::waitForVersion] Error: expected 0x0D final byte, "
			"received %x\n",
			m_received_frame_buffer[nBytes - 1]);
		return false;
	}

	if (printOutVersion)
	{
		cout << "Version: ";
		for (uint8_t k = 0; k < nBytes; ++k)
			cout << char(m_received_frame_buffer[k]);
		cout << endl;
	}
	return true;
}
// Returns false if timeout
bool CCANBusReader::waitIncomingFrame(uint16_t timeout)
{
	uint8_t b;
	unsigned int nBytes = 0;

	CTicTac tictac;
	tictac.Tic();
	const double maxTime = timeout * 1e-3;
	uint8_t dlc = 0;
	while (nBytes < 10 || (nBytes < (10U + dlc + 1U /*CR*/)))
	{
		if (m_mySerialPort->Read(&b, 1))
		{
			// First byte must be STX:
			if (nBytes > 1 || (!nBytes && b == 0x54 /*'T'*/))
			{
				// Store in frame:
				m_received_frame_buffer[nBytes] = b;
				nBytes++;
			}
			if (nBytes == 10)
				dlc = 2 *
					uint8_t(hexCharToInt(
						m_received_frame_buffer[9]));  // here is the
			// number of
			// BYTES of
			// data -> 2
			// hex values
			// for byte
		}
		if (tictac.Tac() >= maxTime) return false;	// Timeout
	}
	// Check final flag
	if (m_received_frame_buffer[10U + dlc] != 0x0D)
	{
		printf(
			"[CCANBusReader::waitIncomingFrame] Error: expected 0x0D as final "
			"flag, received %x\n",
			m_received_frame_buffer[10U + dlc]);
		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 CCANBusReader::sendCommandToCANReader(
	const uint8_t* cmd, const uint16_t cmd_len, [[maybe_unused]] bool wait)
{
	uint8_t cmd_full[1024];
	ASSERT_(sizeof(cmd_full) > cmd_len);

	// command is just plain text so no frame header nor CRC is needed
	memcpy(cmd_full, cmd, cmd_len);
	cmd_full[cmd_len] = 0x0D;  // [CR] at the end

	const size_t toWrite = cmd_len + 1;

#if 1
	printf("TX: ");
	for (unsigned int i = 0; i < toWrite; i++)
		printf("%02X ", cmd_full[i]);
	printf("\n");
#endif

	//    const int NTRIES = 3;

	//    for (int k=0;k<NTRIES;k++)
	//    {
	if (toWrite != m_mySerialPort->Write(cmd_full, toWrite))
	{
		cout << "[CCANBusReader::SendCommandToCANReader] Error writing data to "
				"serial port."
			 << endl;
		return false;
	}
	return true;
	//        std::this_thread::sleep_for(15ms);
	//        if(wait)
	//        {
	//            if(waitACK(5000))
	//                return true;
	//            std::this_thread::sleep_for(10ms);
	//        }
	//        else
	//            return true; // perform special wait outside this method
	//    }

	return false;
}
