/* +------------------------------------------------------------------------+
   |                     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/comms/CClientTCPSocket.h>
#include <mrpt/hwdrivers/CGPSInterface.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>

#include <chrono>
#include <iostream>
#include <list>
#include <mutex>
#include <thread>

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

IMPLEMENTS_GENERIC_SENSOR(CGPSInterface, mrpt::hwdrivers)

struct TParsersRegistry
{
	std::list<CGPSInterface::ptr_parser_t> all_parsers;

	static const TParsersRegistry& getInstance()
	{
		static TParsersRegistry reg;
		return reg;
	}

   private:
	TParsersRegistry()
	{
		all_parsers.push_back(&CGPSInterface::implement_parser_NMEA);
		all_parsers.push_back(&CGPSInterface::implement_parser_NOVATEL_OEM6);
	}
};

/* -----------------------------------------------------
				Constructor
   ----------------------------------------------------- */
CGPSInterface::CGPSInterface()
	: mrpt::system::COutputLogger("CGPSInterface"),
	  m_customInit(),
	  m_rx_buffer(0x10000),
	  m_raw_dump_file_prefix(),
	  m_COMname(),
	  m_last_timestamp(INVALID_TIMESTAMP),
	  m_JAVAD_rtk_src_port(),
	  m_JAVAD_rtk_format("cmr")
{
	m_sensorLabel = "GPS";
}

/* -----------------------------------------------------
				loadConfig_sensorSpecific
   ----------------------------------------------------- */
void CGPSInterface::loadConfig_sensorSpecific(
	const mrpt::config::CConfigFileBase& configSource,
	const std::string& iniSection)
{
	m_parser = configSource.read_enum<CGPSInterface::PARSERS>(
		iniSection, "parser", m_parser, false /*Allow default values*/);
	m_raw_dump_file_prefix = configSource.read_string(
		iniSection, "raw_dump_file_prefix", m_raw_dump_file_prefix,
		false /*Allow default values*/);

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

	m_COMbauds =
		configSource.read_int(iniSection, "baudRate", m_COMbauds, true);
	m_sensorLabelAppendMsgType = configSource.read_bool(
		iniSection, "sensor_label_append_msg_type", m_sensorLabelAppendMsgType);

	// legacy custom cmds:
	m_customInit =
		configSource.read_string(iniSection, "customInit", m_customInit, false);

	// new custom cmds:
	m_custom_cmds_delay = configSource.read_float(
		iniSection, "custom_cmds_delay", m_custom_cmds_delay);
	m_custom_cmds_append_CRLF = configSource.read_bool(
		iniSection, "custom_cmds_append_CRLF", m_custom_cmds_append_CRLF);
	// Load as many strings as found on the way:
	m_setup_cmds.clear();
	for (int i = 1; true; i++)
	{
		std::string sLine = configSource.read_string(
			iniSection, mrpt::format("setup_cmd%i", i), std::string());
		sLine = mrpt::system::trim(sLine);
		if (sLine.empty()) break;
		m_setup_cmds.push_back(sLine);
	}

	m_shutdown_cmds.clear();
	for (int i = 1; true; i++)
	{
		std::string sLine = configSource.read_string(
			iniSection, mrpt::format("shutdown_cmd%i", i), std::string());
		sLine = mrpt::system::trim(sLine);
		if (sLine.empty()) break;
		m_shutdown_cmds.push_back(sLine);
	}

	m_sensorPose.setFromValues(
		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_JAVAD_rtk_src_port = configSource.read_string(
		iniSection, "JAVAD_rtk_src_port", m_JAVAD_rtk_src_port);
	m_JAVAD_rtk_src_baud = configSource.read_int(
		iniSection, "JAVAD_rtk_src_baud", m_JAVAD_rtk_src_baud);
	m_JAVAD_rtk_format = configSource.read_string(
		iniSection, "JAVAD_rtk_format", m_JAVAD_rtk_format);

	m_topcon_useAIMMode = configSource.read_bool(
		iniSection, "JAVAD_useAIMMode", m_topcon_useAIMMode);
	m_topcon_data_period = 1.0 /
		configSource.read_double(
			iniSection, "outputRate", m_topcon_data_period);
}

CGPSInterface::~CGPSInterface() { OnConnectionShutdown(); }

void CGPSInterface::setParser(CGPSInterface::PARSERS parser)
{
	m_parser = parser;
}
CGPSInterface::PARSERS CGPSInterface::getParser() const { return m_parser; }
void CGPSInterface::bindStream(
	const std::shared_ptr<mrpt::io::CStream>& external_stream,
	const std::shared_ptr<std::mutex>& csOptionalExternalStream)
{
	m_data_stream_is_external = true;
	m_data_stream = external_stream;
	m_data_stream_cs = csOptionalExternalStream ? csOptionalExternalStream
												: m_data_stream_mine_cs;
}
void CGPSInterface::setSetupCommandsDelay(const double delay_secs)
{
	m_custom_cmds_delay = delay_secs;
}
double CGPSInterface::getSetupCommandsDelay() const
{
	return m_custom_cmds_delay;
}
void CGPSInterface::setSetupCommands(const std::vector<std::string>& cmds)
{
	m_setup_cmds = cmds;
}
const std::vector<std::string>& CGPSInterface::getSetupCommands() const
{
	return m_setup_cmds;
}
void CGPSInterface::setShutdownCommands(const std::vector<std::string>& cmds)
{
	m_shutdown_cmds = cmds;
}
const std::vector<std::string>& CGPSInterface::getShutdownCommands() const
{
	return m_shutdown_cmds;
}
void CGPSInterface::enableSetupCommandsAppendCRLF(const bool enable)
{
	m_custom_cmds_append_CRLF = enable;
}
bool CGPSInterface::isEnabledSetupCommandsAppendCRLF() const
{
	return m_custom_cmds_append_CRLF;
}

/* -----------------------------------------------------
				setSerialPortName
----------------------------------------------------- */
void CGPSInterface::setSerialPortName(const std::string& COM_port)
{
	// Dont allow changing the serial port if:
	if (m_data_stream_is_external)
		THROW_EXCEPTION(
			"Cannot change serial port name: an external stream has been "
			"already bound manually.");

	if (m_data_stream)
	{
		std::lock_guard<std::mutex> lock(*m_data_stream_cs);
		auto serial =
			dynamic_cast<mrpt::comms::CSerialPort*>(m_data_stream.get());
		if (serial && serial->isOpen())
			THROW_EXCEPTION(
				"Cannot change serial port name when it is already open");
	}

	// OK:
	m_COMname = COM_port;
}

/* -----------------------------------------------------
				getSerialPortName
----------------------------------------------------- */
std::string CGPSInterface::getSerialPortName() const { return m_COMname; }
/* -----------------------------------------------------
				tryToOpenTheCOM
----------------------------------------------------- */
bool CGPSInterface::tryToOpenTheCOM()
{
	// If this is the first use of the COM port, create it:
	if (!m_data_stream)
	{
		m_data_stream = std::make_shared<mrpt::comms::CSerialPort>();
		m_data_stream_is_external = false;
		m_data_stream_cs = m_data_stream_mine_cs;
	}

	auto serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
	if (serial)
	{
		{
			std::lock_guard<std::mutex> lock(*m_data_stream_cs);
			if (serial->isOpen()) return true;	// Already open

			if (m_verbose)
				cout << "[CGPSInterface] Opening " << m_COMname << " @ "
					 << m_COMbauds << endl;
		}
		try
		{
			serial->open(m_COMname);
			// Config:
			serial->setConfig(m_COMbauds, 0, 8, 1);
			serial->setTimeouts(1, 0, 1, 1, 1);

			// Do extra initialization?
			if (!OnConnectionEstablished())
			{
				serial->close();
				return false;
			}
			return true;  // All OK
		}
		catch (const std::exception& e)
		{
			std::cerr << "[CGPSInterface::tryToOpenTheCOM] Error opening or "
						 "configuring serial port:"
					  << std::endl
					  << e.what();
			serial->close();
			return false;
		}
	}  // end of this is a serial port

	return true;  // All OK
}

/* -----------------------------------------------------
				isGPS_connected
----------------------------------------------------- */
bool CGPSInterface::isGPS_connected() { return m_GPS_comsWork; }
/* -----------------------------------------------------
				doProcess
----------------------------------------------------- */
void CGPSInterface::doProcess()
{
	// Is the COM open?
	if (!tryToOpenTheCOM())
	{
		m_state = ssError;
		THROW_EXCEPTION("Could not open the input stream");
	}
	ASSERT_(m_data_stream != nullptr);
	auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
	auto* stream_tcpip = dynamic_cast<CClientTCPSocket*>(m_data_stream.get());

	// Read as many bytes as available:
	uint8_t buf[0x1000];
	const size_t to_read =
		std::min(m_rx_buffer.available() - 1, sizeof(buf) - 1);
	try
	{
		size_t nRead = 0;
		if (to_read > 0)
		{
			std::lock_guard<std::mutex> lock(*m_data_stream_cs);
			if (stream_tcpip)
			{
				nRead = stream_tcpip->readAsync(buf, to_read, 100, 10);
			}
			else if (stream_serial)
			{
				nRead = stream_serial->Read(buf, to_read);
			}
			else
			{
				nRead = m_data_stream->Read(buf, to_read);
			}
		}

		if (nRead) m_rx_buffer.push_many(buf, nRead);

		// Also dump to raw file:
		if (!m_raw_dump_file_prefix.empty() &&
			!m_raw_output_file.fileOpenCorrectly())
		{
			// 1st time open:
			mrpt::system::TTimeParts parts;
			mrpt::system::timestampToParts(now(), parts, true);
			string sFilePostfix = "_";
			sFilePostfix += format(
				"%04u-%02u-%02u_%02uh%02um%02us", (unsigned int)parts.year,
				(unsigned int)parts.month, (unsigned int)parts.day,
				(unsigned int)parts.hour, (unsigned int)parts.minute,
				(unsigned int)parts.second);
			const string sFileName = m_raw_dump_file_prefix +
				mrpt::system::fileNameStripInvalidChars(sFilePostfix) +
				string(".gps");

			if (m_verbose)
				std::cout << "[CGPSInterface] Creating RAW dump file: `"
						  << sFileName << "`\n";
			m_raw_output_file.open(sFileName, mrpt::io::OpenMode::TRUNCATE);
		}
		if (nRead && m_raw_output_file.fileOpenCorrectly())
		{
			m_raw_output_file.Write(buf, nRead);
		}
	}
	catch (std::exception&)
	{
		// ERROR:
		MRPT_LOG_ERROR(
			"[CGPSInterface::doProcess] Error reading stream of data: Closing "
			"communications\n");
		if (stream_serial)
		{
			std::lock_guard<std::mutex> lock(*m_data_stream_cs);
			stream_serial->close();
		}
		m_GPS_comsWork = false;
		return;
	}

	// Try to parse incomming data as messages:
	parseBuffer();

	// Decide whether to push out a new observation in old legacy mode.
	if (!m_customInit.empty())
	{  // "Advanced" (RTK,mmGPS) device  (kept for backwards-compatibility)
		bool do_append_obs = false;
		// FAMD
		// Append observation if:
		// 0. the timestamp seems to be correct!
		// 1. it contains both synched GGA and RMC data
		// 2. it contains only GGA or RMC but the next one is not synched with
		// it
		if (m_last_timestamp == INVALID_TIMESTAMP)
		{
			if (m_verbose)
				cout << "[CGPSInterface] Initial timestamp: "
					 << mrpt::system::timeToString(
							m_just_parsed_messages.timestamp)
					 << endl;
			// Check if the initial timestamp seems to be OK (not a spurio one)
			TTimeStamp tmNow = mrpt::system::now();
			const double tdif = mrpt::system::timeDifference(
				m_just_parsed_messages.timestamp, tmNow);
			if (tdif >= 0 && tdif < 7500 /*Up to two hours*/)
				m_last_timestamp = m_just_parsed_messages.timestamp;
			else
			{
				if (m_verbose)
					cout << "[CGPSInterface] Warning: The initial timestamp "
							"seems to be wrong! : "
						 << tdif << endl;
			}
		}  // end-if
		else
		{
			const double time_diff = mrpt::system::timeDifference(
				m_last_timestamp, m_just_parsed_messages.timestamp);
			if (time_diff < 0 || time_diff > 300)  // Assert that the current
			// timestamp is after the
			// previous one and not more
			// than 5 minutes later ->
			// remove spurious
			{
				if (m_verbose)
					cout << "[CGPSInterface ] Bad timestamp difference" << endl;
				return;
			}

			if (time_diff - m_topcon_data_period > 0.25 * m_topcon_data_period)
			{
				if (m_verbose)
					cout << "[CGPSInterface] WARNING: According to the "
							"timestamps, we probably skipped one frame!"
						 << endl;
			}

			// a. These GPS data have both synched RMC and GGA data
			// don't append observation until we have both data
			do_append_obs =
				(m_just_parsed_messages.has_GGA_datum() &&
				 m_just_parsed_messages.has_RMC_datum());
		}  // end-else

		if (do_append_obs) flushParsedMessagesNow();
	}
}

/** Queue out now the messages in \a m_just_parsed_messages, leaving it empty */

void CGPSInterface::flushParsedMessagesNow()
{
	// Generic observation data:
	m_just_parsed_messages.sensorPose = m_sensorPose;
	if (m_sensorLabelAppendMsgType)
		m_just_parsed_messages.sensorLabel =
			m_sensorLabel + string("_") + m_just_parsed_messages.sensorLabel;
	else
		m_just_parsed_messages.sensorLabel = m_sensorLabel;
	// Add observation to the output queue:
	CObservationGPS::Ptr newObs = std::make_shared<CObservationGPS>();
	m_just_parsed_messages.swap(*newObs);
	CGenericSensor::appendObservation(newObs);
	m_just_parsed_messages.clear();
	m_last_timestamp = m_just_parsed_messages.timestamp;

	// And this means the comms works:
	m_GPS_comsWork = true;
	m_state = ssWorking;
}

/* -----------------------------------------------------
					parseBuffer
----------------------------------------------------- */
void CGPSInterface::parseBuffer()
{
	if (m_parser == CGPSInterface::NONE) return;  // Dont try to parse data

	// Only one parser selected?
	ptr_parser_t parser_ptr = nullptr;
	switch (m_parser)
	{
		case CGPSInterface::NMEA:
			parser_ptr = &CGPSInterface::implement_parser_NMEA;
			break;
		case CGPSInterface::NOVATEL_OEM6:
			parser_ptr = &CGPSInterface::implement_parser_NOVATEL_OEM6;
			break;
		case CGPSInterface::AUTO: break;  // Leave it as NULL
		default: throw std::runtime_error("[CGPSInterface] Unknown parser!");
	};
	if (parser_ptr)
	{
		// Use only one parser ----------
		size_t min_bytes;
		do
		{
			if (!(*this.*parser_ptr)(min_bytes))
			{
				if (m_rx_buffer.size() != 0)
					m_rx_buffer.pop();	// Not the start of a frame, skip 1 byte
			}
			if (m_customInit.empty() /* If we are not in old legacy mode */ &&
				!m_just_parsed_messages.messages.empty())
				flushParsedMessagesNow();
		} while (m_rx_buffer.size() >= min_bytes);
	}  // end one parser mode ----------
	else
	{
		// AUTO mode --------
		const std::list<CGPSInterface::ptr_parser_t>& all_parsers =
			TParsersRegistry::getInstance().all_parsers;

		size_t global_min_bytes_max = 0;
		do
		{
			bool all_parsers_want_to_skip = true;
			for (auto all_parser : all_parsers)
			{
				parser_ptr = all_parser;
				size_t this_parser_min_bytes;
				if ((*this.*parser_ptr)(this_parser_min_bytes))
					all_parsers_want_to_skip = false;
				mrpt::keep_max(global_min_bytes_max, this_parser_min_bytes);
			}

			if (all_parsers_want_to_skip && m_rx_buffer.size() != 0)
				m_rx_buffer.pop();	// Not the start of a frame, skip 1 byte

			if (m_customInit.empty() /* If we are not in old legacy mode */ &&
				!m_just_parsed_messages.messages.empty())
				flushParsedMessagesNow();
		} while (m_rx_buffer.size() >= global_min_bytes_max);
	}  // end AUTO mode ----
}

/* -----------------------------------------------------
					JAVAD_sendMessage
----------------------------------------------------- */
void CGPSInterface::JAVAD_sendMessage(const char* str, bool waitForAnswer)
{
	if (!str) return;
	const size_t len = strlen(str);
	auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
	if (!stream_serial) return;

	size_t written;

	{
		std::lock_guard<std::mutex> lock(*m_data_stream_cs);
		written = stream_serial->Write(str, len);
	}

	if (m_verbose) std::cout << "[CGPSInterface] TX: " << str;

	if (written != len)
		throw std::runtime_error(
			format("Error sending command: '%s'", str).c_str());
	std::this_thread::sleep_for(5ms);

	if (!waitForAnswer) return;

	std::this_thread::sleep_for(200ms);
	char buf[200];
	buf[0] = '\0';

	int bad_counter = 0;
	while (bad_counter < 10)
	{
		size_t nRead;
		{
			std::lock_guard<std::mutex> lock(*m_data_stream_cs);
			written = stream_serial->Write(str, len);
			nRead = stream_serial->Read(buf, sizeof(buf));
		}

		if (m_verbose) std::cout << "[CGPSInterface] RX: " << buf << std::endl;

		if (nRead < 3)
			throw std::runtime_error(format(
				"ERROR: Invalid response '%s' for command '%s'", buf, str));

		if (nRead >= 3 && buf[0] == 'R' && buf[1] == 'E') return;  // Ok!
		else
			++bad_counter;
	}
	throw std::runtime_error(
		format("ERROR: Invalid response '%s' for command '%s'", buf, str));
}

bool CGPSInterface::OnConnectionShutdown()
{
	auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());

	if (stream_serial && !stream_serial->isOpen()) return false;

	// Send commands:
	for (const auto& m_shutdown_cmd : m_shutdown_cmds)
	{
		if (m_verbose)
			cout << "[CGPSInterface] TX shutdown command: `" << m_shutdown_cmd
				 << "`\n";

		std::string sTx = m_shutdown_cmd;
		if (m_custom_cmds_append_CRLF) sTx += std::string("\r\n");
		try
		{
			std::lock_guard<std::mutex> lock(*m_data_stream_cs);
			m_data_stream->Write(&sTx[0], sTx.size());
		}
		catch (...)
		{
			return false;  // On any I/O error
		}

		std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(
			m_custom_cmds_delay * 1000));
	}
	return true;
}

/* -----------------------------------------------------
					OnConnectionEstablished
----------------------------------------------------- */
bool CGPSInterface::OnConnectionEstablished()
{
	m_last_GGA.clear();	 // On comms reset, empty this cache
	m_just_parsed_messages.clear();

	// Legacy behavior:
	if (!os::_strcmpi(m_customInit.c_str(), "JAVAD") ||
		!os::_strcmpi(m_customInit.c_str(), "TOPCON"))
	{
		return legacy_topcon_setup_commands();
	}

	// Purge input:
	auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
	if (stream_serial)
	{
		std::lock_guard<std::mutex> lock(*m_data_stream_cs);
		stream_serial->purgeBuffers();
	}

	// New behavior: Send generic commands set-up by the user in the config
	// file.

	// Send commands:
	for (const auto& m_setup_cmd : m_setup_cmds)
	{
		if (m_verbose)
			cout << "[CGPSInterface] TX setup command: `" << m_setup_cmd
				 << "`\n";

		std::string sTx = m_setup_cmd;
		if (m_custom_cmds_append_CRLF) sTx += std::string("\r\n");

		try
		{
			std::lock_guard<std::mutex> lock(*m_data_stream_cs);
			m_data_stream->Write(&sTx[0], sTx.size());
		}
		catch (const std::exception& e)
		{
			std::cerr << "[CGPSInterface::OnConnectionEstablished] Error "
						 "sending setup cmds: "
					  << e.what() << std::endl;
			return false;
		}
		std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(
			m_custom_cmds_delay * 1000));
	}
	std::this_thread::sleep_for(
		std::chrono::duration<double, std::milli>(m_custom_cmds_delay * 1000));
	return true;
}

bool CGPSInterface::unsetJAVAD_AIM_mode()
{
	MRPT_START
	if (!os::_strcmpi(m_customInit.c_str(), "JAVAD") ||
		!os::_strcmpi(m_customInit.c_str(), "TOPCON"))
	{
		// Stop messaging:
		JAVAD_sendMessage("%%dm\r\n", false);
		std::this_thread::sleep_for(500ms);
		JAVAD_sendMessage("%%dm\r\n", false);
		std::this_thread::sleep_for(1000ms);

		// Purge input:
		auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
		if (stream_serial)
		{
			std::lock_guard<std::mutex> lock(*m_data_stream_cs);
			stream_serial->purgeBuffers();
		}

		JAVAD_sendMessage("%%set,/par/cur/term/imode,cmd\r\n");	 // set the
		// current port
		// in command
		// mode
		return true;
	}
	else
		return true;
	MRPT_END
}  // end-unsetJAVAD_AIM_mode

bool CGPSInterface::setJAVAD_AIM_mode()
{
	MRPT_START
	if (!os::_strcmpi(m_customInit.c_str(), "JAVAD") ||
		!os::_strcmpi(m_customInit.c_str(), "TOPCON"))
	{
		JAVAD_sendMessage(
			format("%%%%set,/par%s/imode,cmd\r\n", m_JAVAD_rtk_src_port.c_str())
				.c_str());	// set the port in command mode
		JAVAD_sendMessage(
			"%%set,/par/cur/term/jps/0,{nscmd,37,n,\"\"}\r\n");	 // any command
		// starting
		// with % will
		// be treated
		// as normal

		ASSERT_(!m_JAVAD_rtk_format.empty());
		cout << "Formato de correcciones para GR3: " << m_JAVAD_rtk_format
			 << endl;
		if (m_JAVAD_rtk_format == "cmr")
		{
			JAVAD_sendMessage(
				format(
					"%%%%set,/par/cur/term/jps/1,{cmr,-1,y,%s}\r\n",
					m_JAVAD_rtk_src_port.c_str())
					.c_str());	// set corrections type CMR or CMR+
			JAVAD_sendMessage("%%set,/par/cur/term/jps/2,{none,-1,n,\"\"}\r\n");
			JAVAD_sendMessage(format(
								  "%%%%set,/par%s/imode,cmr\r\n",
								  m_JAVAD_rtk_src_port.c_str())
								  .c_str());
		}
		else if (m_JAVAD_rtk_format == "rtcm")
		{
			JAVAD_sendMessage(
				format(
					"%%%%set,/par/cur/term/jps/1,{rtcm,-1,y,%s}\r\n",
					m_JAVAD_rtk_src_port.c_str())
					.c_str());	// set corrections type RTCM
			JAVAD_sendMessage("%%set,/par/cur/term/jps/2,{none,-1,n,\"\"}\r\n");
			JAVAD_sendMessage(format(
								  "%%%%set,/par%s/imode,rtcm\r\n",
								  m_JAVAD_rtk_src_port.c_str())
								  .c_str());
		}
		else if (m_JAVAD_rtk_format == "rtcm3")
		{
			JAVAD_sendMessage(
				format(
					"%%%%set,/par/cur/term/jps/1,{rtcm3,-1,y,%s}\r\n",
					m_JAVAD_rtk_src_port.c_str())
					.c_str());	// set corrections type RTCM 3.x
			JAVAD_sendMessage("%%set,/par/cur/term/jps/2,{none,-1,n,\"\"}\r\n");
			JAVAD_sendMessage(format(
								  "%%%%set,/par%s/imode,rtcm3\r\n",
								  m_JAVAD_rtk_src_port.c_str())
								  .c_str());
		}
		else
		{
			cout << "Unknown RTK corrections format. Only supported: CMR, RTCM "
					"or RTCM3"
				 << endl;
			return false;
		}
		JAVAD_sendMessage("%%set,/par/cur/term/imode,jps\r\n");	 // sets current
		// port into
		// "JPS" mode

		return true;

	}  // end-if
	else
		return true;
	MRPT_END
}  // end-setJAVAD_AIM_mode

std::string CGPSInterface::getLastGGA(bool reset)
{
	std::string ret = m_last_GGA;
	if (reset) m_last_GGA.clear();
	return ret;
}

bool CGPSInterface::legacy_topcon_setup_commands()
{
	// Stop messaging:
	JAVAD_sendMessage("%%dm\r\n", false);
	std::this_thread::sleep_for(500ms);
	JAVAD_sendMessage("%%dm\r\n", false);
	std::this_thread::sleep_for(1000ms);

	// Purge input:
	auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
	if (stream_serial)
	{
		std::lock_guard<std::mutex> lock(*m_data_stream_cs);
		stream_serial->purgeBuffers();
	}

	// Configure RTK mode and source:
	if (m_verbose) cout << "[CGPSInterface] Configure RTK options" << endl;

	if (!m_JAVAD_rtk_src_port.empty())
	{
		const int elevation_mask = 5;  // Degs

		JAVAD_sendMessage(
			format("%%%%set,/par/lock/elm,%i\r\n", elevation_mask)
				.c_str());	// Set elevation mask to track satellites
		JAVAD_sendMessage(
			"%%set,/par/base/mode/,off\r\n");  // Set Base Mode off
		JAVAD_sendMessage("%%set,/par/pos/pd/period,1.0\r\n");	// Differential
		// Correction
		// Interval
		// JAVAD_sendMessage("%%set,hd/mode,off\r\n");  // fixed distance to rtk
		// base: Off
		// JAVAD_sendMessage("%%set,/par/pos/pd/hd/mode,off\r\n");  // fixed
		// distance to rtk base: Off  <-- Not working with TopCon GR3! (option
		// disabled)
		JAVAD_sendMessage(
			"%%set,/par/pos/pd/qcheck,off\r\n");  // Set Quality Checks Off
		JAVAD_sendMessage(
			"%%set,/par/pos/mode/cur,pd\r\n");	// Pos Mode Phase Diff
		JAVAD_sendMessage(
			"%%set,/par/pos/pd/textr,10\r\n");	// RTK Extrapolation Limit
		JAVAD_sendMessage(
			"%%set,/par/pos/pd/inuse,-1\r\n");	// Set Rovers Reference Station
		JAVAD_sendMessage("%%set,/par/pos/pd/nrs/mode,y\r\n");	// Enable
		// Nearest
		// Reference
		// Station Mode
		JAVAD_sendMessage("%%set,/par/pos/pd/mode,extrap\r\n");	 // Enable
		// EXTRAPOLATED
		// mode in RTK
		// corrections

		// Set Differential Correction Source
		JAVAD_sendMessage(
			format(
				"%%%%set,/par/pos/pd/port,%s\r\n", m_JAVAD_rtk_src_port.c_str())
				.c_str());

		// Set port bauds:
		if (!m_topcon_useAIMMode && m_JAVAD_rtk_src_baud != 0 &&
			!mrpt::system::strCmp(m_JAVAD_rtk_src_port, "/dev/usb/a"))
			JAVAD_sendMessage(format(
								  "%%%%set,/par%s/rate,%u\r\n",
								  m_JAVAD_rtk_src_port.c_str(),
								  m_JAVAD_rtk_src_baud)
								  .c_str());

		// Set Input Mode: CMR,RTCM,...
		if (!m_topcon_useAIMMode && !m_JAVAD_rtk_format.empty())
			JAVAD_sendMessage(format(
								  "%%%%set,/par%s/imode,%s\r\n",
								  m_JAVAD_rtk_src_port.c_str(),
								  m_JAVAD_rtk_format.c_str())
								  .c_str());
	}

	// Start NMEA messaging:
	//		JAVAD_sendMessage("%%em,,/msg/nmea/GGA:0.2\r\n");
	//		JAVAD_sendMessage("%%em,,/msg/nmea/RMC:0.2\r\n");
	// JAVAD_sendMessage("%%em,,/msg/jps/PS:0.2\r\n");

	if (m_topcon_useAIMMode)
	{
		if (m_verbose) cout << "[CGPSInterface] Using Advanced Input Mode";
		m_topcon_AIMConfigured = setJAVAD_AIM_mode();
		if (m_verbose) cout << "... done" << endl;
	}
	JAVAD_sendMessage(
		format("%%%%em,,/msg/nmea/GGA:%.1f\r\n", m_topcon_data_period).c_str());
	JAVAD_sendMessage(
		format("%%%%em,,/msg/nmea/RMC:%.1f\r\n", m_topcon_data_period)
			.c_str());	// FAMD: 10 Hz

	if (m_topcon_useAIMMode)
	{
		if (m_verbose)
			cout << "[CGPSInterface::OnConnectionEstablished] JAVAD/TopCon "
					"commands sent successfully with AIM."
				 << endl;
	}
	else
	{
		if (m_verbose)
			cout << "[CGPSInterface::OnConnectionEstablished] JAVAD/TopCon "
					"commands sent successfully."
				 << endl;
	}

	return true;
}

/** Send a custom data block to the GNSS device right now. Can be used to change
 * its behavior online as needed. */
bool CGPSInterface::sendCustomCommand(const void* data, size_t datalen)
{
	try
	{
		std::lock_guard<std::mutex> lock(*m_data_stream_cs);
		m_data_stream->Write(data, datalen);
		return true;
	}
	catch (const std::exception& e)
	{
		std::cerr << "[CGPSInterface::sendCustomCommand] Error sending cmd: "
				  << e.what() << std::endl;
		return false;
	}
}
