/* +------------------------------------------------------------------------+
   |                     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/hwdrivers/CBoardENoses.h>
#include <mrpt/math/ops_vectors.h>
#include <mrpt/serialization/CMessage.h>
#include <mrpt/system/os.h>

#include <iostream>
#include <memory>
#include <thread>

using namespace mrpt::math;
using namespace mrpt::obs;
using namespace mrpt::poses;
using namespace mrpt::hwdrivers;
using namespace mrpt::io;
using namespace std;

IMPLEMENTS_GENERIC_SENSOR(CBoardENoses, mrpt::hwdrivers)

/*-------------------------------------------------------------
					CBoardENoses
-------------------------------------------------------------*/
CBoardENoses::CBoardENoses() : m_usbSerialNumber("ENOSE001"), m_COM_port()
{
	m_sensorLabel = "ENOSE";
	first_reading = true;
}

/*-------------------------------------------------------------
					loadConfig_sensorSpecific
-------------------------------------------------------------*/
void CBoardENoses::loadConfig_sensorSpecific(
	const mrpt::config::CConfigFileBase& configSource,
	const std::string& iniSection)
{
	MRPT_START

	m_usbSerialNumber =
		configSource.read_string(iniSection, "USB_serialname", "", false);

#ifdef _WIN32
	m_COM_port =
		configSource.read_string(iniSection, "COM_port_WIN", m_COM_port);
#else
	m_COM_port =
		configSource.read_string(iniSection, "COM_port_LIN", m_COM_port);
#endif
	m_COM_baud =
		configSource.read_uint64_t(iniSection, "COM_baudRate", m_COM_baud);

	configSource.read_vector(
		iniSection, "enose_poses_x", vector<float>(0), enose_poses_x, true);
	configSource.read_vector(
		iniSection, "enose_poses_y", vector<float>(0), enose_poses_y, true);
	configSource.read_vector(
		iniSection, "enose_poses_z", vector<float>(0), enose_poses_z, true);

	configSource.read_vector(
		iniSection, "enose_poses_yaw", vector<float>(0), enose_poses_yaw, true);
	configSource.read_vector(
		iniSection, "enose_poses_pitch", vector<float>(0), enose_poses_pitch,
		true);
	configSource.read_vector(
		iniSection, "enose_poses_roll", vector<float>(0), enose_poses_roll,
		true);

	ASSERT_(enose_poses_x.size() == enose_poses_y.size());
	ASSERT_(enose_poses_x.size() == enose_poses_z.size());
	ASSERT_(enose_poses_x.size() == enose_poses_yaw.size());
	ASSERT_(enose_poses_x.size() == enose_poses_pitch.size());
	ASSERT_(enose_poses_x.size() == enose_poses_roll.size());

	// Pass angles to radians:
	enose_poses_yaw *= M_PIf / 180.0f;
	enose_poses_pitch *= M_PIf / 180.0f;
	enose_poses_roll *= M_PIf / 180.0f;

	MRPT_END
}

/*-------------------------------------------------------------
					queryFirmwareVersion
-------------------------------------------------------------*/
bool CBoardENoses::queryFirmwareVersion(string& out_firmwareVersion)
{
	try
	{
		mrpt::serialization::CMessage msg, msgRx;

		// Try to connect to the device:
		CStream* comms = checkConnectionAndConnect();
		if (!comms) return false;
		auto arch = mrpt::serialization::archiveFrom(*comms);

		msg.type = 0x10;
		arch.sendMessage(msg);

		if (arch.receiveMessage(msgRx))
		{
			msgRx.getContentAsString(out_firmwareVersion);
			return true;
		}
		else
			return false;
	}
	catch (...)
	{
		// Close everything and start again:
		m_stream_SERIAL.reset();
		m_stream_FTDI.reset();
		return false;
	}
}

/*-------------------------------------------------------------
					checkConnectionAndConnect
-------------------------------------------------------------*/
CStream* CBoardENoses::checkConnectionAndConnect()
{
	// Make sure one of the two possible pipes is open:
	if (!m_stream_FTDI && !m_stream_SERIAL)
	{
		if (!m_COM_port.empty())
			m_stream_SERIAL = std::make_unique<mrpt::comms::CSerialPort>();
		else
			m_stream_FTDI = std::make_unique<mrpt::comms::CInterfaceFTDI>();
	}

	if (m_stream_FTDI)
	{  // FTDI pipe ==================
		if (m_stream_FTDI->isOpen()) return m_stream_FTDI.get();
		try
		{
			m_stream_FTDI->OpenBySerialNumber(m_usbSerialNumber);
			std::this_thread::sleep_for(10ms);
			m_stream_FTDI->Purge();
			std::this_thread::sleep_for(10ms);
			m_stream_FTDI->SetLatencyTimer(1);
			m_stream_FTDI->SetTimeouts(10, 100);
			return m_stream_FTDI.get();
		}
		catch (...)
		{  // Error opening device:
			m_stream_FTDI->Close();
			return nullptr;
		}
	}
	else
	{  // Serial pipe ==================
		ASSERT_(m_stream_SERIAL);
		if (m_stream_SERIAL->isOpen()) return m_stream_SERIAL.get();
		try
		{
			m_stream_SERIAL->open(m_COM_port);
			m_stream_SERIAL->setConfig(m_COM_baud);
			std::this_thread::sleep_for(10ms);
			m_stream_SERIAL->purgeBuffers();
			std::this_thread::sleep_for(10ms);
			// m_stream_SERIAL->setTimeouts(25,1,100, 1,20);
			m_stream_SERIAL->setTimeouts(50, 1, 100, 1, 20);
			return m_stream_SERIAL.get();
		}
		catch (...)
		{  // Error opening device:
			m_stream_SERIAL->close();
			return nullptr;
		}
	}
}

/*-------------------------------------------------------------
					getObservation
-------------------------------------------------------------*/
bool CBoardENoses::getObservation(mrpt::obs::CObservationGasSensors& obs)
{
	try
	{
		// Connected?
		CStream* comms = checkConnectionAndConnect();

		if (!comms) return false;

		mrpt::serialization::CMessage msg;
		CObservationGasSensors::TObservationENose newRead;

		obs.m_readings.clear();

		//// Send request:
		// msg.type = 0x11;
		// msg.content.clear();
		// comms->sendMessage( msg );

		//----------------------------MCE-nose
		// FRAME--------------------------------------------------
		// Wait for e-nose frame:	<0x69><0x91><lenght><body><0x96> "Bytes"
		// Where <body> = [Numchamber, Activechamber, N sensors*M chambers*2, 2
		// timestamp] of uint16_t
		// MCE-nose provides a 136B body lenght which makes 140B total frame
		// lenght

		auto arch = mrpt::serialization::archiveFrom(*comms);
		if (!arch.receiveMessage(msg)) { return false; }

		// m_state = ssWorking;

		// Copy to "uint16_t":
		ASSERT_((msg.content.size() % 2) == 0);

		vector<uint16_t> readings(
			msg.content.size() /
			2);	 // divide by 2 to pass from byte to word. 136B/2 = 68 Words

		if (msg.content.size() > 0)
		{
			// Copy to a vector of 16bit integers:
			memcpy(
				&readings[0], &msg.content[0],
				msg.content.size() * sizeof(msg.content[0]));

			// HEADER Frame [ Nº of chambers/enoses (16b) , Active Chamber
			// (16b)]
			auto NumberOfChambers = (size_t)readings[0];
			auto ActiveChamber = (size_t)readings[1];

			// Sensors readings info
			ASSERT_(((readings.size() - 4) % NumberOfChambers) == 0);
			size_t wordsPereNose = (readings.size() - 4) / NumberOfChambers;

			// Process each chamber
			for (size_t i = 0; i < NumberOfChambers; i++)
			{
				// ----------------------------------------------------------------------
				// Each "i" comprises a complete Enose reading: Gas sensors +
				// temperature
				// ----------------------------------------------------------------------

				// Do we have the sensor position?
				if (i < enose_poses_x.size())
				{
					newRead.eNosePoseOnTheRobot = TPose3D(
						enose_poses_x[i], enose_poses_y[i], enose_poses_z[i],
						enose_poses_yaw[i], enose_poses_pitch[i],
						enose_poses_roll[i]);
				}
				else
					newRead.eNosePoseOnTheRobot = TPose3D(0, 0, 0, 0, 0, 0);

				// Process the sensor codes:
				newRead.sensorTypes.clear();
				newRead.readingsVoltage.clear();
				newRead.hasTemperature = false;
				newRead.isActive = false;

				// check if active chamber
				if (i == (ActiveChamber)) newRead.isActive = true;

				// process each sensor on this chamber "i"
				for (size_t idx = 0; idx < wordsPereNose / 2; idx++)
				{
					if (readings[i * wordsPereNose + 2 * idx + 2] !=
						0x0000)	 // not empty slot
					{
						// Is temperature?
						if (readings[i * wordsPereNose + 2 * idx + 2] == 0xFFFF)
						{
							newRead.hasTemperature = true;
							newRead.temperature =
								((int16_t)readings
									 [i * wordsPereNose + 2 * idx + 3]) /
								32.0f;
						}
						else  // Is a gas sensors
						{
							// It is not a null code: There is a valid measure:
							newRead.sensorTypes.push_back(
								readings[i * wordsPereNose + 2 * idx + 2]);

							// Pass from ADC value[12bits] to [0-2.5] volt
							// range:
							newRead.readingsVoltage.push_back(
								(readings[i * wordsPereNose + 2 * idx + 3] *
								 5.0f) /
								4096.0f);
						}
					}
				}  // end for each sensor on this eNose

				// Add to observations:
				if (!newRead.sensorTypes.empty())
					obs.m_readings.push_back(newRead);

			}  // end for each i'th eNose

			obs.sensorLabel = m_sensorLabel;

			// Set Timestamp
			auto* p =
				(uint16_t*)&readings[readings.size() - 2];	// Get readings time
			// from frame
			// (always last 2
			// words)
			obs.timestamp =
				mrpt::system::time_tToTimestamp(((double)*p) / 1000);

			if (first_reading)
			{
				initial_timestamp =
					mrpt::system::getCurrentTime() - obs.timestamp;
				first_reading = false;
			}
			obs.timestamp = obs.timestamp + initial_timestamp;

		}  // end if message has data

		// CONTROL
		bool correct = true;

		if (obs.m_readings.size() != 4) correct = false;
		else
		{
			for (auto& m_reading : obs.m_readings)
			{
				if ((m_reading.sensorTypes.size() != 7) ||
					(m_reading.readingsVoltage.size() != 7))
					correct = false;
				else
				{
				}
			}
		}

		if (!correct) printf("Error en la observacion");  // For debug

		return !obs.m_readings.empty();	 // Done OK!
	}
	catch (exception& e)
	{
		cerr << "[CBoardENoses::getObservation] Returning false due to "
				"exception: "
			 << endl;
		cerr << e.what() << endl;
		return false;
	}
	catch (...)
	{
		return false;
	}
}

/*-------------------------------------------------------------
					doProcess
-------------------------------------------------------------*/
/** This method should be called periodically (at least at 1Hz to capture ALL
 * the real-time data)
 *  It is thread safe, i.e. you can call this from one thread, then to other
 * methods from other threads.
 */
void CBoardENoses::doProcess()
{
	CObservationGasSensors::Ptr obs =
		std::make_shared<CObservationGasSensors>();

	if (getObservation(*obs))
	{
		m_state = ssWorking;
		appendObservation(obs);
	}
	else
	{
		m_state = ssError;
		// THROW_EXCEPTION("No observation received from the USB board!");
	}
}

/*-------------------------------------------------------------
					initialize
-------------------------------------------------------------*/
/** Tries to open the camera, after setting all the parameters with a call to
 * loadConfig.
 *  \exception This method must throw an exception with a descriptive message
 * if some critical error is found.
 */
void CBoardENoses::initialize()
{
	// We'll rather try it in doProcess() since it's quite usual that it fails
	//  on a first try, then works on the next ones.
	/*
	if (!checkConnectionAndConnect())
		THROW_EXCEPTION("Couldn't connect to the eNose board");
	*/
}

/*-------------------------------------------------------------
					setActiveChamber
-------------------------------------------------------------*/
/** Send to the MCE-nose the next Active Chamber */

bool CBoardENoses::setActiveChamber(unsigned char chamber)
{
	try
	{
		// Try to connect to the device:
		CStream* comms = checkConnectionAndConnect();
		if (!comms) return false;

		// Send a byte to set the Active chamber on device.
		// by default:  Byte_to_send = 10_ _ _ _10
		unsigned char buf[1];
		buf[0] = ((chamber & 15) << 2) | 130;  // 130= 10 0000 10

		comms->Write(buf, 1);  // Exceptions will be raised on errors here
		return true;
	}
	catch (...)
	{
		// Close everything and start again:
		m_stream_SERIAL.reset();
		m_stream_FTDI.reset();
		return false;
	}
}
