/* +------------------------------------------------------------------------+
   |                     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/CGPSInterface.h>
#include <mrpt/io/CMemoryStream.h>
#include <mrpt/system/crc.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>

#include <iostream>

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

bool CGPSInterface::implement_parser_NOVATEL_OEM6(
	size_t& out_minimum_rx_buf_to_decide)
{
	// to be grabbed from the last Message_NV_OEM6_IONUTC msg
	static uint32_t num_leap_seconds =
		getenv("MRPT_HWDRIVERS_DEFAULT_LEAP_SECONDS") == nullptr
		? 18
		: atoi(getenv("MRPT_HWDRIVERS_DEFAULT_LEAP_SECONDS"));

	using namespace mrpt::obs::gnss;

	out_minimum_rx_buf_to_decide = sizeof(nv_oem6_short_header_t);

	const size_t nBytesAval = m_rx_buffer.size();  // Available for read
	if (nBytesAval < out_minimum_rx_buf_to_decide)
		return true;  // no need to skip 1 byte

	// If the synch bytes do not match, it is not a valid frame:
	uint8_t peek_buffer[3];
	m_rx_buffer.peek_many(&peek_buffer[0], 3);

	// Short header?
	const bool is_short_hdr =
		peek_buffer[0] == nv_oem6_short_header_t::SYNCH0 &&
		peek_buffer[1] == nv_oem6_short_header_t::SYNCH1 &&
		peek_buffer[2] == nv_oem6_short_header_t::SYNCH2;

	const bool is_regular_hdr = peek_buffer[0] == nv_oem6_header_t::SYNCH0 &&
		peek_buffer[1] == nv_oem6_header_t::SYNCH1 &&
		peek_buffer[2] == nv_oem6_header_t::SYNCH2;

	if (!is_short_hdr && !is_regular_hdr)
		return false;  // skip 1 byte, we dont recognize this format

	if (is_short_hdr)
	{
		if (nBytesAval < sizeof(nv_oem6_short_header_t))
		{
			out_minimum_rx_buf_to_decide = sizeof(nv_oem6_short_header_t);
			return true;  // we must wait for more data in the buffer
		}
		nv_oem6_short_header_t hdr;
		m_rx_buffer.peek_many(reinterpret_cast<uint8_t*>(&hdr), sizeof(hdr));
		hdr.fixEndianness();
		const uint32_t expected_total_msg_len =
			sizeof(hdr) + hdr.msg_len + 4 /*crc*/;
		if (nBytesAval < expected_total_msg_len)
		{
			out_minimum_rx_buf_to_decide = expected_total_msg_len;
			return true;  // we must wait for more data in the buffer
		}

		std::vector<uint8_t> buf(expected_total_msg_len);
		m_rx_buffer.pop_many(reinterpret_cast<uint8_t*>(&buf[0]), sizeof(hdr));
		m_rx_buffer.pop_many(
			reinterpret_cast<uint8_t*>(&buf[sizeof(hdr)]),
			hdr.msg_len + 4 /*crc*/);

		// Check CRC:
		const uint32_t crc_computed =
			mrpt::system::compute_CRC32(&buf[0], expected_total_msg_len - 4);
		const uint32_t crc_read = (buf[expected_total_msg_len - 1] << 24) |
			(buf[expected_total_msg_len - 2] << 16) |
			(buf[expected_total_msg_len - 3] << 8) |
			(buf[expected_total_msg_len - 4] << 0);
		if (crc_read != crc_computed)
			return false;  // skip 1 byte, we dont recognize this format

		// Deserialize the message:
		// 1st, test if we have a specific data structure for this msg_id:
		const bool use_generic_container = !gnss_message::FactoryKnowsMsgType(
			(gnss_message_type_t)(NV_OEM6_MSG2ENUM + hdr.msg_id));
		// ------ Serialization format:
		// const int32_t msg_id = message_type;
		// out << msg_id;
		// this->internal_writeToStream(out);  == >  out <<
		// static_cast<uint32_t>(DATA_LEN); out.WriteBuffer(DATA_PTR,DATA_LEN);
		// }
		// ------
		mrpt::io::CMemoryStream tmpStream;
		auto arch = mrpt::serialization::archiveFrom(tmpStream);
		const uint32_t msg_id = use_generic_container
			? (uint32_t)(NV_OEM6_GENERIC_SHORT_FRAME)
			: (uint32_t)hdr.msg_id + NV_OEM6_MSG2ENUM;
		arch << (uint32_t)(msg_id);
		// This len = hdr + hdr.msg_len + 4 (crc);
		arch << (uint32_t)(expected_total_msg_len);
		arch.WriteBuffer(&buf[0], buf.size());

		tmpStream.Seek(0);
		gnss_message_ptr msg(gnss_message::readAndBuildFromStream(arch));
		if (!msg.get())
		{
			std::cerr << "[CGPSInterface::implement_parser_NOVATEL_OEM6] Error "
						 "parsing binary packet msg_id="
					  << hdr.msg_id << "\n";
			return true;
		}
		m_just_parsed_messages.messages[msg->message_type] = msg;
		m_just_parsed_messages.originalReceivedTimestamp = mrpt::system::now();
		if (!CObservationGPS::GPS_time_to_UTC(
				hdr.week, hdr.ms_in_week * 1e-3, num_leap_seconds,
				m_just_parsed_messages.timestamp))
			m_just_parsed_messages.timestamp = mrpt::system::now();
		else
			m_just_parsed_messages.has_satellite_timestamp = true;

		m_just_parsed_messages.sensorLabel = msg->getMessageTypeAsString();

		flushParsedMessagesNow();
		return true;
	}  // end short hdr

	if (is_regular_hdr)
	{
		if (nBytesAval < sizeof(nv_oem6_header_t))
		{
			out_minimum_rx_buf_to_decide = sizeof(nv_oem6_header_t);
			return true;  // we must wait for more data in the buffer
		}
		nv_oem6_header_t hdr;
		m_rx_buffer.peek_many(reinterpret_cast<uint8_t*>(&hdr), sizeof(hdr));
		hdr.fixEndianness();
		const uint32_t expected_total_msg_len =
			sizeof(hdr) + hdr.msg_len + 4 /*crc*/;
		if (nBytesAval < expected_total_msg_len)
		{
			out_minimum_rx_buf_to_decide = expected_total_msg_len;
			return true;  // we must wait for more data in the buffer
		}

		std::vector<uint8_t> buf(expected_total_msg_len);
		m_rx_buffer.pop_many(reinterpret_cast<uint8_t*>(&buf[0]), sizeof(hdr));
		m_rx_buffer.pop_many(
			reinterpret_cast<uint8_t*>(&buf[sizeof(hdr)]),
			hdr.msg_len + 4 /*crc*/);

		// Check CRC:
		const uint32_t crc_computed =
			mrpt::system::compute_CRC32(&buf[0], expected_total_msg_len - 4);
		const uint32_t crc_read = (buf[expected_total_msg_len - 1] << 24) |
			(buf[expected_total_msg_len - 2] << 16) |
			(buf[expected_total_msg_len - 3] << 8) |
			(buf[expected_total_msg_len - 4] << 0);
		if (crc_read != crc_computed)
			return false;  // skip 1 byte, we dont recognize this format

		// Deserialize the message:
		// 1st, test if we have a specific data structure for this msg_id:
		const bool use_generic_container = !gnss_message::FactoryKnowsMsgType(
			(gnss_message_type_t)(NV_OEM6_MSG2ENUM + hdr.msg_id));
		// ------ Serialization format:
		// const int32_t msg_id = message_type;
		// out << msg_id;
		// this->internal_writeToStream(out);  == >  out <<
		// static_cast<uint32_t>(DATA_LEN); out.WriteBuffer(DATA_PTR,DATA_LEN);
		// }
		// ------
		mrpt::io::CMemoryStream tmpStream;
		auto arch = mrpt::serialization::archiveFrom(tmpStream);
		const int32_t msg_id = use_generic_container
			? (uint32_t)(NV_OEM6_GENERIC_FRAME)
			: (uint32_t)hdr.msg_id + NV_OEM6_MSG2ENUM;
		arch << msg_id;
		arch << (uint32_t)(expected_total_msg_len);
		arch.WriteBuffer(&buf[0], buf.size());

		tmpStream.Seek(0);
		gnss_message_ptr msg(gnss_message::readAndBuildFromStream(arch));
		if (!msg.get())
		{
			std::cerr << "[CGPSInterface::implement_parser_NOVATEL_OEM6] Error "
						 "parsing binary packet msg_id="
					  << hdr.msg_id << "\n";
			return true;
		}
		m_just_parsed_messages.messages[msg->message_type] = msg;
		m_just_parsed_messages.originalReceivedTimestamp = mrpt::system::now();
		{
			// Detect NV_OEM6_IONUTC msgs to learn about the current leap
			// seconds:
			const auto* ionutc =
				dynamic_cast<const gnss::Message_NV_OEM6_IONUTC*>(msg.get());
			if (ionutc) num_leap_seconds = ionutc->fields.deltat_ls;
		}
		if (!CObservationGPS::GPS_time_to_UTC(
				hdr.week, hdr.ms_in_week * 1e-3, num_leap_seconds,
				m_just_parsed_messages.timestamp))
			m_just_parsed_messages.timestamp = mrpt::system::now();
		else
			m_just_parsed_messages.has_satellite_timestamp = true;

		m_just_parsed_messages.sensorLabel = msg->getMessageTypeAsString();
		flushParsedMessagesNow();
		return true;
	}  // end regular hdr

	// Shouldnt arrive here, but MSVC complies anyway:
	return false;
}
