#include <vector>
#include <iostream>
#include <cmath>
#include <cstdint>
#include <cstring>

#include "stm32h7xx_ll_utils.h"

#include "GPS1.hpp"
#include "CPFUtilities.hpp"
#include "Logger.hpp"
#include "RTClock.hpp"
#include "UARTBase.hpp"

struct GPSFix
{
	double latDD;
	double lonDD;
	std::string latNMEA;
	std::string lonNMEA;
	std::string tofNMEA;
	tm tofCPF;
	unsigned int numSats;
	float hdop;
	std::string navStat;
	uint8_t msg[GPS_MAX_MESSAGE_SIZE];

	void reset()
	{
		latDD = static_cast<double>(NAN);
		lonDD = static_cast<double>(NAN);
		latNMEA = "NaN";
		lonNMEA = "NaN";
		numSats = static_cast<int>(NAN);
		hdop = NAN;
		navStat = "NaN";
		tofNMEA = "NaN";
		tofCPF.tm_year = 0;
		tofCPF.tm_mon = 0;
		tofCPF.tm_mday = 0;
		tofCPF.tm_hour = 0;
		tofCPF.tm_min = 0;
		tofCPF.tm_sec = 0.0;
		memset(msg, 0, sizeof(msg));
	}
};
GPSFix currentFix;

void updateFixPUBX04(uint8_t pubx04msg[]);

alignas(32) constexpr uint8_t pubx04[] = "$PUBX,04*37\r\n";

ALIGN_32BYTES(std::uint8_t  pubx00Cmd[]) = "$PUBX,00*33\r\n";
constexpr uint32_t pubx00CmdSize = sizeof(pubx00Cmd) - 1;
ALIGN_32BYTES(uint8_t  pubx04Cmd[]) = "$PUBX,04*37\r\n";
static uint32_t pubx04CmdSize = sizeof(pubx04Cmd) - 1;

void GPS1::logEngFileHeader()
{
	Logger::logEngLine(false, {gpsID, "dateTime_Z", "state", "comment", "latDD", "lonDD",
					   "latNMEA", "lonNMEA", "ttff", "numSats", "hdop", "navStat", "msg"});
	Logger::logEngLine(false, {gpsID, "text", "text", "text", "0.0", "0.0", "text", "text",
					   "text", "0.0", "0.0", "text", "text"});
}

void GPS1::synchRTCtoGPS(bool logResults)
{
	getTimePUBX04();
	while (!isXferDone())
	{
		std::cout << "Waiting for PUBX04 XferDone" << std::endl;
		LL_mDelay(100); //It's OK to block here, this only happens during BSP::init and once per profile during surface ops
	}

	std::string timeMsg = getRXBufferString(); //TODO need to check for no or bad message
	std::cout << "GPS::synchRTCtoGPS timeMsg: " << timeMsg << std::endl;
	
	std::string gpsTime = CPFUtils::getField(timeMsg, ",", 2);
	std::string gpsDate = CPFUtils::getField(timeMsg, ",", 3);
	RTClock::synchToGPS(gpsTime, gpsDate);
	if (logResults)
	{
		//log results to engFile
	}
}

void GPS1::init()
{
	std::cout << "GPS1::init()" << std::endl;
}

int8_t GPS1::getPositionPUBX00()
{
	RTClock::CPFTimestamp timestamp;
	uint8_t* message;
	uint16_t messageSize;
	int8_t returnValue = 0;
	
	usart1::fifoRecord myRecord;
	
	if (xferInProgress)
	{
		if (IOPORT::getQCount_T(IOPORT::gps1) >= 1)
		{
			IOPORT::deQ_T(IOPORT::gps1, myRecord);

			std::cout << "FIFO Record timestamp: " << myRecord.timestamp.data() << "   msg: " << myRecord.message.data() << std::endl;
			xferInProgress = false;
			returnValue = 1;
		}
	}
	else
	{
		IOPORT::startXfer(IOPORT::gps1, pubx00Cmd, pubx00CmdSize);
		xferInProgress = true;	
		returnValue = 0;
	}
	
	return returnValue;
}

void GPS1::getTimePUBX04()
{
	IOPORT::startXfer(IOPORT::gps1, pubx04Cmd, pubx04CmdSize);
}

bool GPS1::isXferDone()
{
	return IOPORT::isXferDone(IOPORT::gps1);
}

const std::string GPS1::getRXBufferString()
{
	std::string tempString = IOPORT::getRXBuffer(IOPORT::gps1);	
	return tempString;
}

tm GPS1::get_tofCPF(void)
{
	return currentFix.tofCPF;
}

void updateFixPUBX04(uint8_t pubx04msg[])
{
	uint16_t year = 1;
	float fracSecond = -0.1;

	year = static_cast<uint16_t>((pubx04msg[23] - 48) * 10 + (pubx04msg[24] - 48) + 2000);
	// TODO: temp if(year > 2018)
	{
		currentFix.tofCPF.tm_hour = static_cast<uint8_t>((pubx04msg[9] - 48) * 10 + (pubx04msg[10] - 48));
		currentFix.tofCPF.tm_min = static_cast<uint8_t>((pubx04msg[11] - 48) * 10 + (pubx04msg[12] - 48));
		currentFix.tofCPF.tm_sec = static_cast<float>((pubx04msg[13] - 48) * 10 + (pubx04msg[14] - 48));
		fracSecond = static_cast<float>(((pubx04msg[16] - 48) * 0.1) + ((pubx04msg[17] - 48) * 0.01));
		//currentFix.tofCPF.second = currentFix.tofCPF.second + fracSecond;
		
		currentFix.tofCPF.tm_year = year - 1900;
		currentFix.tofCPF.tm_mon = static_cast<uint8_t>(((pubx04msg[21] - 48) * 10 + (pubx04msg[22] - 48)));
		currentFix.tofCPF.tm_mday = static_cast<uint8_t>(((pubx04msg[19] - 48) * 10 + (pubx04msg[20] - 48)));
	}
}