/*
 * Copyright (C) 2021-2025 Roger Clark, VK3KYY / G4KYF
 *                         Colin Durbridge, G4EML
 *                         Daniel Caujolle-Bert, F1RMB
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer
 *    in the documentation and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * 4. Use of this source code or binary releases for commercial purposes is strictly forbidden. This includes, without limitation,
 *    incorporation in a commercial product or incorporation into a product or project which allows commercial use.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
 * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */
#include "main.h"
#include "user_interface/uiGlobals.h"
#include "user_interface/uiUtilities.h"
#include "interfaces/gps.h"
#include "user_interface/uiLocalisation.h"
#include "usb/usb_com.h"
#if defined(PLATFORM_MD9600)
#include "interfaces/remoteHead.h"
#endif

#if defined(HAS_GPS)

#define GPS_RX_BUFFERS_MAX                  3U

#if defined(LOG_GPS_DATA)
#define LOG_RAM_BUF_SIZE                 4096U

static
#if ! (CPU_MK22FN512VLL12) // Doesn't fit on DM1801
  __attribute__((section(".data.$RAM2")))
#endif
  uint8_t NMEARecordingBuffer[LOG_RAM_BUF_SIZE];

#define LOG_FLASH_16MB_START_ADDRESS  (14 * 1024 * 1024) // Last 2MB
#define LOG_FLASH_16MB_MEM_SIZE        (2 * 1024 * 1024)
#if defined(CPU_MK22FN512VLL12)
#define LOG_FLASH_2MB_START_ADDRESS    (1 * 1024 * 1024) // Last 1MB
#define LOG_FLASH_2MB_MEM_SIZE         (1 * 1024 * 1024)
#define LOG_FLASH_1MB_START_ADDRESS    DMRID_MEMORY_LOCATION_2 // DMRId section 2
#define LOG_FLASH_1MB_MEM_SIZE        ((1 * 1024 * 1024) - DMRID_MEMORY_LOCATION_2) // Last 288k
#endif // CPU_MK22FN512VLL12

static uint32_t gpsLogFlashStartAddress = 0U;
static uint32_t gpsLogFlashMemSize = 0U;
static uint32_t gpsLogMemOffset = 0U;
static bool gpsIsLogging = false;
#endif // LOG_GPS_DATA

typedef struct
{
	uint8_t data[GPS_LINE_LENGTH];
	uint8_t length;
} gpsReceiveBuffer_t;

typedef struct
{
	gpsReceiveBuffer_t rxBuffers[GPS_RX_BUFFERS_MAX];
	uint8_t linesCount;
	uint8_t bufferIndex;
	uint8_t charPosition;
} gpsReceiveData_t;

gpsData_t gpsData =
{
		.Status = (GPS_STATUS_FIX_UPDATED | GPS_STATUS_FIXTYPE_UPDATED),
		.Latitude = 0U,
		.LatitudeHiRes = 0.0,
		.Longitude = 0U,
		.LongitudeHiRes = 0.0,
		.SatsInViewGP = 0U,
		.SatsInViewBD = 0U,
		.currentGPSIndex = 0U,
		.currentBDIndex = 0U,
		.Time = 0U,
		.AccuracyInCm = 0U,
		.HeightInM = 0U
#if defined(STM32F405xx)
		,
		.baudRateIndex = 0U
#endif
};


#if defined(CPU_MK22FN512VLL12)
static volatile gpsReceiveData_t gpsRxData;
#else
static gpsReceiveData_t gpsRxData;
static bool detectingBaudRate = true;
static uint32_t gpsBaudDetectionDelayCounter = 0U;
#endif


static uint8_t gpsBufferIndexProcessing = 0U;
#if defined(STM32F405xx)
static uint8_t gpsDMABuf[GPS_DMA_BUFFER_SIZE]; // double buffer (two halves) for GPS UART DMA receive
#endif

//
// Grace number of GSV invalid frames (in GSV) before setting the fix lost in the Status.
// On the MD-9600, from time to time a bunch if invalid frames ('V') are send by the GPS module.
// This prevents to get some "Fix lost" -> "Fix acquired" sequences.
#define GPS_FIX_GRACE_MAX        15U
static uint8_t gpsFixGraceCount = 0U;

//#define GNSS_MULTI_GSV 1 // Uncomment this to support GB, GA and GL GSVs sentences (not with genuine GPS modules).

//#define USE_DUMMY_GPS_DATA
#ifdef USE_DUMMY_GPS_DATA
const char *DUMMY_GPS_DATA[] = {
		"$GNZDA,074101.000,20,09,2022,,*42",
		"$GNGGA,074102.000,3858.1,N,14602.1,E,1,03,4.38,51.4,M,-1.5,M,,*4F",
		"$GPGSA,A,2,29,25,12,,,,,,,,,,4.49,4.38,1.00,1*16",
		"$BDGSA,A,2,,,,,,,,,,,,,4.49,4.38,1.00,4*0D",
		"$GPGSV,2,1,07,29,63,203,24,2,62,148,,25,61,339,24,20,33,129,*48",
		"$GPGSV,2,2,07,12,32,18,25,26,7,220,,23,1,340,*76",
		"$BDGSV,1,1,04,1,47,0,,4,44,22,,3,34,313,,2,12,289,*5D",
		"$GNRMC,074102.000,A,3758.10000,N,14502.10000,E,5.000,275.00,200922,,,A*53",
		"$GNZDA,074101.000,20,09,2022,,*42",
		"$GNGGA,074102.000,3858.10000,S,14502.10000,E,1,03,4.38,51.4,M,-1.5,M,,*4F",
		"$GPGSA,A,2,29,25,12,,,,,,,,,,4.49,4.38,1.00,1*16",
		"$BDGSA,A,2,,,,,,,,,,,,,4.49,4.38,1.00,4*0D",
		"$GPGSV,2,1,07,29,63,203,24,2,62,148,,25,61,339,24,20,33,129,*48",
		"$GPGSV,2,2,07,12,32,18,25,26,7,220,,23,1,340,*76",
		"$BDGSV,1,1,04,1,47,0,,4,44,22,,3,34,313,,2,12,289,*5D",
		"$GNRMC,074102.000,A,3758.00000,N,14502.00000,E,5.000,275.00,200922,,,A*53"
};
uint32_t dummyGpsDataIndex = 0;
#endif

#if defined(CPU_MK22FN512VLL12)
#include "fsl_uart.h"

static bool gpsIrqIsEnabled = false;

extern double round(double); // implementation in aprs.c
#endif

#if defined(LOG_GPS_DATA)
static void gpsLogNMEAData(const char *nmea, uint8_t length);
#endif


void gpsInit(void)
{
	memset((gpsReceiveData_t *)&gpsRxData, 0, sizeof(gpsReceiveData_t));
	gpsBufferIndexProcessing = 0U;

#if defined(LOG_GPS_DATA)

#if defined(STM32F405xx)
	gpsLogFlashStartAddress = LOG_FLASH_16MB_START_ADDRESS;
	gpsLogFlashMemSize = LOG_FLASH_16MB_MEM_SIZE;
#else
	switch (flashChipPartNumber)
	{
		case 0x4015: // 4015 25Q16   16M-bits  2M-bytes, used in the Baofeng DM-1801 ?
		case 0x4017: // 4017 25Q64   64M-bits  8M-bytes, used in Roger's special GD-77 radios modified on the TYT production line.
			gpsLogFlashStartAddress = LOG_FLASH_2MB_START_ADDRESS;
			gpsLogFlashMemSize = LOG_FLASH_2MB_MEM_SIZE;
			break;

		case 0x4018: // 4018 25Q128 128M-bits 16M-bytes, used in MD9600 and Daniel's modified GD-77.
		case 0x7018: // 7018 25Q128JV 128M-bits 16M-bytes, KI5GZK's modified GD-77.
			gpsLogFlashStartAddress = LOG_FLASH_16MB_START_ADDRESS;
			gpsLogFlashMemSize = LOG_FLASH_16MB_MEM_SIZE;
			break;

		case 0x4014: // 4014 25Q80    8M-bits  1M-bytes, used in the GD-77.
		default:
			if (voicePromptDataIsLoaded)
			{
				gpsLogFlashStartAddress = LOG_FLASH_1MB_START_ADDRESS;
				gpsLogFlashMemSize = LOG_FLASH_1MB_MEM_SIZE;
			}
			else
			{
				gpsLogFlashStartAddress = DMRID_MEMORY_LOCATION_1;
				gpsLogFlashMemSize = ((1 * 1024 * 1024) - DMRID_MEMORY_LOCATION_1); // last 832k
			}
			break;
	}
#endif

#endif // LOG_GPS_DATA

#if defined(CPU_MK22FN512VLL12)
	uart_config_t config;

	UART_GetDefaultConfig(&config);
	config.baudRate_Bps = 9600;
	config.enableTx     = false;
	config.enableRx     = true;
	UART_Init(UART0, &config, CLOCK_GetFreq(SYS_CLK));
	UART_EnableRxFIFO(UART0, true);
	UART_EnableInterrupts(UART0, kUART_RxDataRegFullInterruptEnable | kUART_RxOverrunInterruptEnable);
	gpsOff();
#endif
}

#if defined(CPU_MK22FN512VLL12)
void UART0_RX_TX_IRQHandler (void)
{
	uint32_t uartFlags = UART_GetStatusFlags(UART0);

	while ((kUART_RxDataRegFullFlag | kUART_RxOverrunFlag) & uartFlags)
	{
		gpsProcessChar(UART_ReadByte(UART0));
		uartFlags = UART_GetStatusFlags(UART0);
	};
	SDK_ISR_EXIT_BARRIER;
}
#endif

// Converts the GPS data from the format DDDmm.mmmm into our custom format Int<<23 + frac*1000
static uint32_t gpsLatLongConvert(const char *input, double *dValue)
{
	double value = strtod(input, NULL);
	double degrees = (int32_t)(value * 1E-2); // get degrees

	value -= (degrees * 1E2);
	degrees += (value / 60.0);

	*dValue = degrees;

	return ((((uint32_t)degrees) << 23) + ((uint32_t)(round(((degrees - (uint32_t)degrees) * 1E5)))));
}

static time_t_custom gpsTimeConvert(const char *time, const char *date)
{
	struct tm gpsDateTime;
	uint8_t Date;
	uint8_t Month;
	uint8_t Year;
	uint8_t Hours;
	uint8_t Minutes;
	uint8_t Seconds;

	if (strlen(date) == 6)
	{
		Year = ((date[4] - '0') * 10) + (date[5] - '0');
		Month = ((date[2] - '0') * 10) + (date[3] - '0');
		Date = ((date[0] - '0') * 10) + (date[1] - '0');
	}
	else
	{
		Year = 21;
		Month = 12;
		Date = 25;
	}

	if (Year < 70)
	{
		Year = Year + 100;
	}

	if (strlen(time) >= 6)
	{
		Seconds = ((time[4] - '0') * 10) + (time[5] - '0');
		Minutes = ((time[2] - '0') * 10) + (time[3] - '0');
		Hours = ((time[0] - '0') * 10) + (time[1] - '0');
	}
	else
	{
		Seconds = 0;
		Minutes = 0;
		Hours = 0;
	}

	memset(&gpsDateTime, 0x00, sizeof(struct tm)); // clear entire struct
	gpsDateTime.tm_mday = Date;          /* day of the month, 1 to 31 */
	gpsDateTime.tm_mon = Month - 1;      /* months since January, 0 to 11 */
	gpsDateTime.tm_year = Year;          /* years since 1900 */
	gpsDateTime.tm_hour = Hours;
	gpsDateTime.tm_min = Minutes;
	gpsDateTime.tm_sec = Seconds;

	return mktime_custom(&gpsDateTime);
}

static void getParam(const char *line, char *result, int entryno, int reslen)
{
	int count = 0;
	int respoint = 0;

	for (size_t i = 0; i < strlen(line); i++)
	{
		if (line[i] == ',')
		{
			count++;
			if (count == entryno)
			{
				result[respoint] = 0;
				return;
			}
			else
			{
				respoint = 0;
			}
		}
		else
		{
			result[respoint++] = line[i];
			if (respoint > (reslen - 1))
			{
				result[0] = 0;
				return;
			}
		}
	}

	result[0] = 0;
}

#if !(defined(PLATFORM_MD9600) || defined(CPU_MK22FN512VLL12))
static
#endif
void gpsProcessChar(uint8_t rxchar)
{
	if ((rxchar != '\r') && (gpsRxData.charPosition < GPS_LINE_LENGTH))
	{
		if (rxchar >= '!') // Ignore '\n'
		{
			gpsRxData.rxBuffers[gpsRxData.bufferIndex].data[gpsRxData.charPosition++] = rxchar;
		}
	}
	else
	{
		gpsRxData.rxBuffers[gpsRxData.bufferIndex].data[gpsRxData.charPosition] = 0;
		gpsRxData.rxBuffers[gpsRxData.bufferIndex].length = gpsRxData.charPosition;
		gpsRxData.linesCount++;
		gpsRxData.bufferIndex = (gpsRxData.bufferIndex + 1) % GPS_RX_BUFFERS_MAX;
		gpsRxData.rxBuffers[gpsRxData.bufferIndex].length = 0U;
		gpsRxData.charPosition = 0U;
	}
}

void gpsDataInputStartStop(bool enable)
{
#if defined(STM32F405xx)
	if (enable)
	{
		HAL_UART_Receive_DMA(
#if defined(PLATFORM_MD9600) || defined(PLATFORM_MDUV380) || defined(PLATFORM_RT84_DM1701) || defined(PLATFORM_MD2017)
				&huart1
#elif defined(PLATFORM_MD380)
				&huart3
#endif
				, gpsDMABuf, GPS_DMA_BUFFER_SIZE);

	}
	else
	{
		HAL_UART_DMAStop(
#if defined(PLATFORM_MD9600) || defined(PLATFORM_MDUV380) || defined(PLATFORM_RT84_DM1701) || defined(PLATFORM_MD2017)
				&huart1
#elif defined(PLATFORM_MD380)
				&huart3
#endif
		);
	}
#elif defined(CPU_MK22FN512VLL12)
	if (enable)
	{
		EnableIRQ(UART0_RX_TX_IRQn);
	}
	else
	{
		DisableIRQ(UART0_RX_TX_IRQn);
	}

	gpsIrqIsEnabled = enable;
#endif // STM32F405xx

	memset((gpsReceiveData_t *)&gpsRxData, 0, sizeof(gpsReceiveData_t));
	gpsBufferIndexProcessing = 0U;
}

void gpsOn(void)
{
	memset((uint8_t *)&gpsData, 0x00, sizeof(gpsData_t));// reset everything
	gpsFixGraceCount = 0U;
	gpsData.Status |= (GPS_STATUS_FIX_UPDATED | GPS_STATUS_FIXTYPE_UPDATED);
#if ! defined(PLATFORM_MD9600)
	gpsPower(true);			//Turn on the power to the GPS module, except on MD9600 where it also powers the mic so is turned on all the time in application main
#endif

#if defined(PLATFORM_MD9600)
	if(remoteHeadActive)
	{
		remoteHeadGpsDataInputStartStop(true);
	}
	else
#endif
	{
		gpsDataInputStartStop(true);
	}
}

void gpsOff(void)
{
#if ! defined(PLATFORM_MD9600)
	gpsPower(false);		//Turn off the power to the GPS module except on MD9600 where the mic and GPS are powered at the same time.
#endif

#if defined(PLATFORM_MD9600)
	if(remoteHeadActive)
	{
		remoteHeadGpsDataInputStartStop(false);
	}
	else
#endif
	{
		gpsDataInputStartStop(false);
	}
	memset((uint8_t *)&gpsData, 0x00, sizeof(gpsData_t));// reset everything
	gpsFixGraceCount = 0U;
	gpsData.Status |= (GPS_STATUS_FIX_UPDATED | GPS_STATUS_FIXTYPE_UPDATED);
}

#if defined(STM32F405xx)
void HAL_UART_RxHalfCpltCallback(UART_HandleTypeDef *huart)
{
#if defined(PLATFORM_MD9600)
	if(huart == &huart3)
	{
		remoteHead_UART_RxHalfCpltCallback();
	}
	else
#endif
	{
#if defined(PLATFORM_MD9600)
		// GPS module power is never turned off on this platform, so we must ignore any data from it.
		if (SETTINGS_GPS_MODE_GET(nonVolatileSettings) == GPS_MODE_OFF)
		{
			return;
		}
#endif

		for (size_t i = 0; i < 32; i++)
		{
			gpsProcessChar(gpsDMABuf[i]);
		}
	}
}
#endif // STM32F405xx

void gpsOnUsingQuickKey(bool on)
{
	uiEvent_t e = { .buttons = BUTTON_NONE, .keys = NO_KEYCODE, .rotary = 0, .events = FUNCTION_EVENT, .hasEvent = true, .time = ticksGetMillis() };
	bool sendEvent = false;

	// Use QuickKey to change GPS power status
	if (on && (SETTINGS_GPS_MODE_GET(nonVolatileSettings) > GPS_NOT_DETECTED) && (SETTINGS_GPS_MODE_GET(nonVolatileSettings) < (NUM_GPS_MODES - 1)))
	{
		e.function = QUICKKEY_MENUVALUE(MENU_GENERAL, MENU_GENERAL_OPTIONS_GPS_ENTRY_NUMBER, FUNC_RIGHT);
		sendEvent = true;
	}
	else if((on == false) && (SETTINGS_GPS_MODE_GET(nonVolatileSettings) > GPS_MODE_OFF))
	{
		e.function = QUICKKEY_MENUVALUE(MENU_GENERAL, MENU_GENERAL_OPTIONS_GPS_ENTRY_NUMBER, FUNC_LEFT);
		sendEvent = true;
	}

	if (sendEvent)
	{
		menuSystemPushNewMenu(MENU_GENERAL);
		menuSystemCallCurrentMenuTick(&e);
	}

}

#if defined(STM32F405xx)
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
#if defined(PLATFORM_MD9600)
	if(huart == &huart3)
	{
		remoteHead_UART_RxCpltCallback();
	}
	else
#endif
	{
		/* Debugging only
		 HAL_GPIO_TogglePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin);
		 */

#if defined(PLATFORM_MD9600)
		// GPS module power is never turned off on this platform, so we must ignore any data from it.
		if (SETTINGS_GPS_MODE_GET(nonVolatileSettings) == GPS_MODE_OFF)
		{
			return;
		}
#endif

		for (size_t i = 0; i < 32; i++)
		{
			gpsProcessChar(gpsDMABuf[(GPS_DMA_BUFFER_SIZE / 2) + i]);
		}
	}
}
#endif // STM32F405xx


#if defined(PLATFORM_MD9600)
//turn the GPS on and off. Note this also turns the Microphone power on and off, so the GPS must always be On when the radio is in use.
void gpsAndMicPower(bool on)
{
	if (on)
	{
		GPIOA->MODER = (GPIOA->MODER & ~GPIO_MODER_MODER9_Msk) | GPIO_MODER_MODER9_0;     //Set the GPIOA Pin 9 to GPIO mode
		HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_SET);                               //set it high
	}
	else
	{
		GPIOA->MODER = (GPIOA->MODER & ~GPIO_MODER_MODER9_Msk) | GPIO_MODER_MODER9_0;     // Set the GPIOA Pin 9 to GPIO mode
		HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_RESET);                             //set it Low
	}
}
#endif

void gpsPower(bool on)
{
#if defined(STM32F405xx)
	if (on)
	{
#if defined(PLATFORM_MD380)
		GPIOD->MODER = (GPIOD->MODER & ~GPIO_MODER_MODER8_Msk) | GPIO_MODER_MODER8_0;     //Set the GPIOD Pin 8 to GPIO mode
		HAL_GPIO_WritePin(GPIOD, GPIO_PIN_8, GPIO_PIN_SET);                               //set it high
#else // PLATFORM_MD380

#if defined(PLATFORM_MD9600)
		if(remoteHeadActive)
		{
			remoteHeadGpsPower(true);                   //G4EML... if remote head is fitted then send command to power on the GPS/Mic
		}                                               //G4EML... always also use the hardware power control as the mic might be locally connected.
#endif // PLATFORM_MD9600

		GPIOA->MODER = (GPIOA->MODER & ~GPIO_MODER_MODER9_Msk) | GPIO_MODER_MODER9_0;     //Set the GPIOA Pin 9 to GPIO mode
		HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_SET);                               //set it high
#endif // PLATFORM_MD380
	}
	else
	{
#if defined(PLATFORM_MD380)
		GPIOD->MODER = (GPIOD->MODER & ~GPIO_MODER_MODER8_Msk) | GPIO_MODER_MODER8_0;     // Set the GPIOD Pin 8 to GPIO mode
		HAL_GPIO_WritePin(GPIOD, GPIO_PIN_8, GPIO_PIN_RESET);                             //set it Low
#else // PLATFORM_MD380

#if defined(PLATFORM_MD9600)
		if(remoteHeadActive)
		{
			remoteHeadGpsPower(false);              //G4EML... if remote head is fitted then send command to power on the GPS/Mic
		}                                           //G4EML... always also use the hardware power control as the mic might be locally connected.
#endif // PLATFORM_MD9600

		GPIOA->MODER = (GPIOA->MODER & ~GPIO_MODER_MODER9_Msk) | GPIO_MODER_MODER9_0;     // Set the GPIOA Pin 9 to GPIO mode
		HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_RESET);                             //set it Low
#endif // PLATFORM_MD380
	}
#elif defined(CPU_MK22FN512VLL12) && ! defined(PLATFORM_RD5R)
	// On RD5R, this pin is connected to the flash light LED
	GPIO_PinWrite(GPIO_GPS_Power, Pin_GPS_Power, (on ? 1U : 0U));
#endif // STM32F405xx
}

static bool jumpToNextField(char **pField, uint8_t *curLength)
{
	// Searching end of the current field.
	while (**pField != '\0' && (*curLength > 0))
	{
		(*pField)++;
		(*curLength)--;
	}

	// Another field exists, position *pField on it.
	if (*curLength > 0)
	{
		(*pField)++;
		(*curLength)--;

		return true;
	}

	return false;
}

static void convertCommasToDelimiters(char *line, uint8_t len)
{
	for (uint8_t i = 0; i < len ; i++)
	{
		if ((line[i] == ',') || (line[i] == '*'))
		{
			line[i] = '\0';
		}
	}
}

static int getNmeaInt(const char *str)
{
	if (*str != '\0')
	{
		return atoi(str);
	}

	return -1;
}

#if defined(GNSS_MULTI_GSV)
static bool populateSatelliteData(char **NMEA, uint8_t *curLength, gpsSatellitesData_t *sat, uint8_t *counter, bool *isDifferent, int8_t sub)
#else
static bool populateSatelliteData(char **NMEA, uint8_t *curLength, gpsSatellitesData_t *sat, uint8_t *counter, bool *isDifferent)
#endif
{
	gpsSatellitesData_t pSat;
	int prn;

	memcpy(&pSat, sat, sizeof(gpsSatellitesData_t));

	prn = getNmeaInt(*NMEA);
	sat->Number =
#if defined(GNSS_MULTI_GSV)
			((sub && (prn > sub)) ? (prn - sub) : prn);
#else
			prn;
#endif
	jumpToNextField(NMEA, curLength);

	sat->El = getNmeaInt(*NMEA);
	jumpToNextField(NMEA, curLength);

	sat->Az = getNmeaInt(*NMEA);
	jumpToNextField(NMEA, curLength);

	sat->RSSI = getNmeaInt(*NMEA);

	(*counter)++;

#if defined(USE_DUMMY_GPS_DATA)
	*isDifferent = true;
#else
	*isDifferent = (memcmp(&pSat, sat, sizeof(gpsSatellitesData_t)) != 0);
#endif

	return ((*counter < GPS_STORAGE_MAX) && jumpToNextField(NMEA, curLength));
}

#if defined(GNSS_MULTI_GSV)
static uint16_t processGSV(char *line, uint8_t lineLength, gpsSatellitesData_t *satsStorage, uint8_t *counter, bool *satsAreDifferents, int8_t sub)
#else
static uint16_t processGSV(char *line, uint8_t lineLength, gpsSatellitesData_t *satsStorage, uint8_t *counter, bool *satsAreDifferents)
#endif
{
	uint16_t totalSatsInView = 0;

	if (lineLength > 7)
	{
		int msgNumber;
		bool satIsDifferent;
		char *pos = &line[7]; // skip message header

		convertCommasToDelimiters(line, lineLength);

		lineLength -= 7; // take care of header skipping

		// Skip total messages
		jumpToNextField(&pos, &lineLength);
		msgNumber = atoi(pos);

		if (msgNumber == 1)
		{
			// Reset storage counter
			*counter = 0;
		}

		jumpToNextField(&pos, &lineLength);
		totalSatsInView = atoi(pos);

		jumpToNextField(&pos, &lineLength);

		while(*counter < GPS_STORAGE_MAX)
		{
#if defined(GNSS_MULTI_GSV)
			bool res = (populateSatelliteData(&pos, &lineLength, (satsStorage + *counter), counter, &satIsDifferent, sub) && (lineLength > 4));
#else
			bool res = (populateSatelliteData(&pos, &lineLength, (satsStorage + *counter), counter, &satIsDifferent) && (lineLength > 4));
#endif

			*satsAreDifferents |= satIsDifferent;

			if (!res) // we have to stop the parsing here
			{
				break;
			}

		}
	}

	return totalSatsInView;
}

#if 0
#if USE_CHECKSUM
// https://en.wikipedia.org/wiki/Fletcher%27s_checksum
// Slightly modified, as length will never be > GPS_LINE_LENGTH
static uint16_t fletcher16(const uint8_t *data, uint8_t len)
{
	uint32_t c0 = 0;
	uint32_t c1 = 0;

	do
	{
		c0 = c0 + *data++;
		c1 = c1 + c0;
	} while (--len);

	c0 = c0 % 255;
	c1 = c1 % 255;

   return (c1 << 8 | c0);
}
#endif
#endif

#if defined(STM32F405xx)
void gpsStartBaudRateAutodetection(void)
{
	gpsData.baudRateIndex = 0U;
	detectingBaudRate = true;
	gpsBaudDetectionDelayCounter = 0U;
}

void gpsStopBaudRateAutodetection(void)
{
	detectingBaudRate = false;
	gpsBaudDetectionDelayCounter = 0U;
}

void gpsSetBaudRateByIndex(uint8_t baudRateIndex)
{
	const uint32_t baudRateArray[] = { 9600, 19200, 38400, 57600, 115200, 230400, 460800 };

	huart1.Instance = USART1;
	huart1.Init.BaudRate = baudRateArray[(baudRateIndex % GPS_BAUDRATES_ARRAY_MAX)];
	huart1.Init.WordLength = UART_WORDLENGTH_8B;
	huart1.Init.StopBits = UART_STOPBITS_1;
	huart1.Init.Parity = UART_PARITY_NONE;
	huart1.Init.Mode = UART_MODE_RX;
	huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
	huart1.Init.OverSampling = UART_OVERSAMPLING_16;

	if (HAL_UART_Init(&huart1) != HAL_OK)
	{
		Error_Handler();
	}
}
#endif

void gpsTick(void)
{
	char gpsLine[GPS_LINE_LENGTH] = { 0 };
	char param[6][20];
	char line[20];
	char *p;
	uint8_t lineLength = 0U;

	if ((menuSystemGetCurrentMenuNumber() != UI_TX_SCREEN) &&
			(SETTINGS_GPS_MODE_GET(nonVolatileSettings) >= GPS_MODE_OFF) &&
			((ticksGetMillis() % 500) == 0)
#if defined(STM32F405xx)
			&& (HAL_DMA_GetState(&hdma_usart1_rx) != HAL_DMA_STATE_BUSY)
#elif defined(CPU_MK22FN512VLL12)
			&& (gpsIrqIsEnabled == false)
#endif
	)
	{
		gpsDataInputStartStop(true);
	}

#if defined(STM32F405xx)
	if (detectingBaudRate)
	{
		gpsBaudDetectionDelayCounter++;

		if ((SETTINGS_GPS_MODE_GET(nonVolatileSettings) != GPS_MODE_OFF) &&
				(gpsRxData.linesCount == 0U) &&
				((gpsBaudDetectionDelayCounter % 500) == 0U))
		{
			gpsData.baudRateIndex = (gpsData.baudRateIndex + 1U) % GPS_BAUDRATES_ARRAY_MAX;
			gpsSetBaudRateByIndex(gpsData.baudRateIndex);
			gpsDataInputStartStop(true);
		}
	}
#endif

#ifdef USE_DUMMY_GPS_DATA
	if (true)
	{
		strcpy(gpsLine, DUMMY_GPS_DATA[dummyGpsDataIndex % (sizeof(DUMMY_GPS_DATA) / sizeof(DUMMY_GPS_DATA[0]))]);
		dummyGpsDataIndex++;
		lineLength = strlen(gpsLine);
#else // USE_DUMMY_GPS_DATA

	if (gpsRxData.linesCount > 0U)
	{
#if defined(STM32F405xx)
		if (detectingBaudRate)
		{
			settingsSet(nonVolatileSettings.gpsModeAndBaudsIndex, SETTINGS_GPS_BAUDRATE_SET(nonVolatileSettings, (gpsData.baudRateIndex + 1U)));
			detectingBaudRate = false;
		}
#endif

		if (SETTINGS_GPS_MODE_GET(nonVolatileSettings) == GPS_NOT_DETECTED)
		{
			settingsSet(nonVolatileSettings.gpsModeAndBaudsIndex, SETTINGS_GPS_MODE_SET(nonVolatileSettings, GPS_MODE_OFF));
			gpsOff();
			return;
		}

		lineLength = gpsRxData.rxBuffers[gpsBufferIndexProcessing].length;
		memcpy(gpsLine, (uint8_t *)&gpsRxData.rxBuffers[gpsBufferIndexProcessing].data[0], (lineLength + 1));
		gpsRxData.linesCount--;
		gpsBufferIndexProcessing = (gpsBufferIndexProcessing + 1) % GPS_RX_BUFFERS_MAX;

#endif // USE_DUMMY_GPS_DATA

		if (gpsLine[0] == '$')
		{
			if (SETTINGS_GPS_MODE_GET(nonVolatileSettings) >= GPS_MODE_ON_NMEA)
			{
				USB_DEBUG_printf("%s\r\n", gpsLine);// Note. NMEA protocol requires CR LF

#if defined(LOG_GPS_DATA)
				// log everything once per minute
				if ((gpsData.Time % 60) == 0)
				{
					gpsLogNMEAData(gpsLine, lineLength);
				}
#endif
			}

			if (memcmp(&gpsLine[3], "GGA", 3) == 0)// message that contains accuracy (HDOP) and altitude
			{
				getParam(gpsLine, line, 9, 20);// get accuracy (HDOP)

				uint16_t hdop = (uint16_t)((strtod(line, NULL)) * 1E2);
				if (hdop != gpsData.AccuracyInCm)
				{
					gpsData.AccuracyInCm = hdop;
					gpsData.Status |= (GPS_STATUS_HDOP_UPDATED | GPS_STATUS_HAS_HDOP);
				}

				getParam(gpsLine, line, 10, 20);// get height
				p = strchr(line, '.');
				if (p != NULL)
				{
					*p = '\0';
				}

				int16_t height = atoi(line);
				if (height != gpsData.HeightInM)
				{
					gpsData.HeightInM = height;
					gpsData.Status |= (GPS_STATUS_HEIGHT_UPDATED | GPS_STATUS_HAS_HEIGHT);
				}
			}
			else if (memcmp(&gpsLine[3], "RMC", 3) == 0)			//is this the LAT Long and Time Message?
			{
				char statusLetter[20];
				int currentMenu = menuSystemGetCurrentMenuNumber();

#if defined(LOG_GPS_DATA)
				if ((gpsData.Time % 60) != 0)
				{
					gpsLogNMEAData(gpsLine, lineLength);
				}
#endif
				// check if it has the date and time.
				getParam(gpsLine, param[0], 2, 20);			//get parameter 2 which is GMT Time as hhmmss.sss
				getParam(gpsLine, statusLetter, 3, 20);
				getParam(gpsLine, param[5], 10, 20);	    	//get parameter 10 which is Date as ddmmyy

				if ((param[0][0] != 0) && (param[5][0] != 0))
				{
					gpsData.Time = gpsTimeConvert(param[0], param[5]);

					// Clock skew ?
					if (((gpsData.Status & (GPS_STATUS_HAS_FIX | GPS_STATUS_3D_FIX)) == (GPS_STATUS_HAS_FIX | GPS_STATUS_3D_FIX)) &&
							(abs(uiDataGlobal.dateTimeSecs - gpsData.Time) > 5))
					{
						uiSetUTCDateTimeInSecs(gpsData.Time);
#if defined(STM32F405xx)
						setRtc_custom(uiDataGlobal.dateTimeSecs);
#endif
						// Update Satellite screen (re-enter)
						bool restartSatMenu = (currentMenu == MENU_SATELLITE);
						if (restartSatMenu)
						{
							menuDataGlobal.currentItemIndex = 0; // will restart in prediction list
							menuSystemPopPreviousMenu();
							menuSatelliteSetFullReload();
						}

						menuSatelliteScreenClearPredictions(false);

						if (restartSatMenu)
						{
							menuSystemPushNewMenu(MENU_SATELLITE);
						}
					}

					gpsData.Status |= (GPS_STATUS_TIME_UPDATED | GPS_STATUS_HAS_TIME);
				}

				// Have a fix
				//
				if (statusLetter[0] == 'A')
				{
					gpsFixGraceCount = GPS_FIX_GRACE_MAX;

					if ((gpsData.Status & GPS_STATUS_HAS_FIX) == 0)
					{
						gpsData.Status |= (GPS_STATUS_HAS_FIX | GPS_STATUS_FIX_UPDATED);
					}
				}
				else // Have no fix
				{
					if (gpsFixGraceCount > 0U)
					{
						gpsFixGraceCount--;
					}
					else
					{
						// Clear fix type status
						if (gpsData.Status & (GPS_STATUS_2D_FIX | GPS_STATUS_3D_FIX))
						{
							gpsData.Status &= ~(GPS_STATUS_2D_FIX | GPS_STATUS_3D_FIX);
							gpsData.Status |= GPS_STATUS_FIXTYPE_UPDATED;
						}

						// Loosing fix status
						if (gpsData.Status & GPS_STATUS_HAS_FIX)
						{
							gpsData.Status &= ~(GPS_STATUS_HAS_FIX | GPS_STATUS_HAS_POSITION | GPS_STATUS_HAS_HDOP | GPS_STATUS_HAS_COURSE | GPS_STATUS_HAS_SPEED | GPS_STATUS_HAS_HEIGHT | GPS_STATUS_HAS_TIME);
							gpsData.Status |= GPS_STATUS_FIX_UPDATED;
						}
					}

					return;
				}

				getParam(gpsLine, param[1] , 4, 20);			//get parameter 4 which is Latitude a ddmm.mmmm
				getParam(gpsLine, param[2] , 5, 20);			//get parameter 5 which is N/S
				getParam(gpsLine, param[3] , 6, 20);			//get parameter 6 which is Longitude a dddmm.mmmm
				getParam(gpsLine, param[4] , 7, 20);			//get parameter 7 which is E/W

				gpsData.Latitude = gpsLatLongConvert(param[1], &gpsData.LatitudeHiRes);

				if (param[2][0] == 'S')
				{
					gpsData.Latitude = gpsData.Latitude | 0x80000000;
					gpsData.LatitudeHiRes = -gpsData.LatitudeHiRes;
				}

				gpsData.Longitude = gpsLatLongConvert(param[3], &gpsData.LongitudeHiRes);

				if (param[4][0] == 'W')
				{
					gpsData.Longitude = gpsData.Longitude | 0x80000000;
					gpsData.LongitudeHiRes = -gpsData.LongitudeHiRes;
				}

				if (((currentMenu != UI_TX_SCREEN) && (currentMenu != MENU_SATELLITE)) &&
						((nonVolatileSettings.location.lat != gpsData.Latitude) || (nonVolatileSettings.location.lon != gpsData.Longitude)))
				{
					settingsSet(nonVolatileSettings.location.lat, gpsData.Latitude);
					settingsSet(nonVolatileSettings.location.lon, gpsData.Longitude);

					menuSatelliteScreenClearPredictions(false);

					gpsData.Status |= (GPS_STATUS_POSITION_UPDATED | GPS_STATUS_HAS_POSITION);
				}

				getParam(gpsLine, line, 8, 20);
				if (strchr(line, '.') != NULL) // There is a value
				{
					uint16_t v = (uint16_t)((strtod(line, NULL)) * 1E2);
					if (v != gpsData.SpeedInHundredthKn)
					{
						gpsData.SpeedInHundredthKn = v;
						gpsData.Status |= (GPS_STATUS_SPEED_UPDATED | GPS_STATUS_HAS_SPEED);
					}
				}
				else if (gpsData.Status & GPS_STATUS_HAS_SPEED) // Value cleared
				{
					gpsData.SpeedInHundredthKn = 0U;
					gpsData.Status &= ~GPS_STATUS_HAS_SPEED;
					gpsData.Status |= GPS_STATUS_SPEED_UPDATED;
				}

				getParam(gpsLine, line, 9, 20);
				if (strchr(line, '.') != NULL) // There is a value
				{
					uint16_t v = (uint16_t)((strtod(line, NULL)) * 1E2);
					if (v != gpsData.CourseInHundredthDeg)
					{
						gpsData.CourseInHundredthDeg = v;
						gpsData.Status |= (GPS_STATUS_COURSE_UPDATED | GPS_STATUS_HAS_COURSE);
					}
				}
				else if (gpsData.Status & GPS_STATUS_HAS_COURSE) // Value cleared
				{
					gpsData.CourseInHundredthDeg = 0U;
					gpsData.Status &= ~GPS_STATUS_HAS_COURSE;
					gpsData.Status |= GPS_STATUS_COURSE_UPDATED;
				}
			}
			else if (memcmp(&gpsLine[3], "GSA", 3) == 0) // DOP and active satellites
			{
				getParam(gpsLine, param[0], 2, 20);	// get parameter 2 which is mode 'A' or 'M'
				getParam(gpsLine, param[1], 3, 20);	// get parameter 3 which is Fix type

				if (param[0][0] == 'A')
				{
					if (gpsData.Status & GPS_STATUS_HAS_FIX)
					{
						// We just got a 3D fix
						if ((param[1][0] == '3') && ((gpsData.Status & GPS_STATUS_3D_FIX) == 0))
						{
							gpsData.Status &= ~GPS_STATUS_2D_FIX;
							gpsData.Status |= (GPS_STATUS_3D_FIX | GPS_STATUS_FIXTYPE_UPDATED);
						} // We just got a 2D fix
						else if ((param[1][0] == '2') && ((gpsData.Status & GPS_STATUS_2D_FIX) == 0))
						{
							gpsData.Status &= ~GPS_STATUS_3D_FIX;
							gpsData.Status |= (GPS_STATUS_2D_FIX | GPS_STATUS_FIXTYPE_UPDATED);
						}
					}
				}
				else
				{
					// Clear 2D and 3D fix, if any sets
					if (gpsData.Status & (GPS_STATUS_2D_FIX | GPS_STATUS_3D_FIX))
					{
						gpsData.Status &= ~(GPS_STATUS_2D_FIX | GPS_STATUS_3D_FIX);
						gpsData.Status |= GPS_STATUS_FIXTYPE_UPDATED;
					}
				}
			}
			else if (memcmp(&gpsLine[3], "GSV", 3) == 0)
			{
				uint16_t            *pSatsInView = NULL;
				gpsSatellitesData_t *pSats = NULL;
				uint8_t             *pCurrentGPSIndex = NULL;
				uint16_t             prevSatsInView = 0;
				uint32_t             gpsStatus;
#if defined(GNSS_MULTI_GSV)
				int8_t               prnSub = 0;
				bool                 gbSatSub = false;
				bool                 glSatSub = false;
#endif

				if ((gpsLine[1] == 'G') && (gpsLine[2] == 'P')) // GPS GSV
				{
					pSatsInView      = &gpsData.SatsInViewGP;
					pSats            = &gpsData.GPSatellites[0];
					pCurrentGPSIndex = &gpsData.currentGPSIndex;
					gpsStatus        = GPS_STATUS_GPS_SATS_UPDATED;
				}
				else if((gpsLine[1] == 'B') // (BD) BeiDou GSV`
#if defined(GNSS_MULTI_GSV)
						|| ((gpsLine[1] == 'G') &&
								((gbSatSub = (gpsLine[2] == 'B'))  // (GB) BeiDou GSV (100 should be subtracted to the PRN number to determine the BeiDou PRN number)
										|| (gpsLine[2] == 'A') // (GA) Galileo GSV
										|| ((glSatSub = (gpsLine[2] == 'L'))) // (GL) GLONASS GSV (64 should be subtracted to the PRN number to determine the GLONASS PRN number)
								)
						)
#endif
				)
				{
					pSatsInView      = &gpsData.SatsInViewBD;
					pSats            = &gpsData.BDSatellites[0];
					pCurrentGPSIndex = &gpsData.currentBDIndex;
					gpsStatus        = GPS_STATUS_BD_SATS_UPDATED;
#if defined(GNSS_MULTI_GSV)
					prnSub           = (gbSatSub ? 100 : (glSatSub ? 64 : 0));
#endif
				}

				if (pSatsInView && pSats && pCurrentGPSIndex)
				{
					bool satsAreDifferents = false;

					prevSatsInView = *pSatsInView;

#if defined(GNSS_MULTI_GSV)
					*pSatsInView = processGSV(gpsLine, lineLength, pSats, pCurrentGPSIndex, &satsAreDifferents, prnSub);
#else
					*pSatsInView = processGSV(gpsLine, lineLength, pSats, pCurrentGPSIndex, &satsAreDifferents);
#endif
					if ((*pSatsInView != prevSatsInView) || satsAreDifferents)
					{
						gpsData.Status |= gpsStatus;
					}
				}
			}
		}
	}
#if defined(STM32F405xx)
	else
	{
		// Stops trying to detect the GPS baudrate (no GPS available), after
		// 10 complete runs.
		if (detectingBaudRate && (gpsBaudDetectionDelayCounter % ((500 * GPS_BAUDRATES_ARRAY_MAX) * 10U) == 0U))
		{
			detectingBaudRate = false;
		}
	}
#endif
}

#if defined(LOG_GPS_DATA)
static void gpsLogByte(char data)
{
	NMEARecordingBuffer[gpsLogMemOffset % LOG_RAM_BUF_SIZE] = data;
	gpsLogMemOffset++;

	if ((gpsLogMemOffset % LOG_RAM_BUF_SIZE) == 0)
	{
		SPI_Flash_write((gpsLogFlashStartAddress + (((gpsLogMemOffset / LOG_RAM_BUF_SIZE) - 1) * LOG_RAM_BUF_SIZE)), NMEARecordingBuffer, LOG_RAM_BUF_SIZE);
		gpsLogMemOffset %= gpsLogFlashMemSize;
	}
}

static void gpsLogNMEAData(const char *nmea, uint8_t length)
{
	if ((SETTINGS_GPS_MODE_GET(nonVolatileSettings) == GPS_MODE_ON_LOG) && gpsIsLogging && ((gpsData.Status & GPS_STATUS_HAS_FIX) != 0))
	{
		for(uint8_t i = 0; i < length; i++)
		{
			gpsLogByte(nmea[i]);
		}
	}
}

void gpsLoggingStart(void)
{
	if (SETTINGS_GPS_MODE_GET(nonVolatileSettings) == GPS_MODE_ON_LOG)
	{
		if (gpsIsLogging == false)
		{
#if defined(CPU_MK22FN512VLL12)
			// Invalidate the whole DMRIDs, if exist, on MK22.
			// Except the ones with 16Mb flash chip
			if ((gpsLogFlashStartAddress != LOG_FLASH_16MB_START_ADDRESS) && (dmrIDCacheGetCount() > 0))
			{
				dmrIDCacheClear(); // Ensure dmrIDLookup() fails

				memset(NMEARecordingBuffer, 0x00, DMRID_HEADER_LENGTH);
				SPI_Flash_write(DMRID_MEMORY_LOCATION_1, NMEARecordingBuffer, DMRID_HEADER_LENGTH);
			}
#endif
			// Load the last stored flash block
			gpsLogMemOffset = nonVolatileSettings.gpsLogMemOffset;
			SPI_Flash_read((((gpsLogFlashStartAddress + gpsLogMemOffset) / LOG_RAM_BUF_SIZE) * LOG_RAM_BUF_SIZE), NMEARecordingBuffer, LOG_RAM_BUF_SIZE);
			// write start marker that can be read as text
			gpsLogByte('A');
			gpsLogByte('A');
			gpsLogByte('A');
			gpsLogByte('A');

			gpsIsLogging = true;
		}
	}
}

void gpsLoggingStop(void)
{
	if (gpsIsLogging)
	{
		gpsIsLogging = false;

		// write end marker which can be read as text
		gpsLogByte('Z');
		gpsLogByte('Z');

		if (gpsLogMemOffset % LOG_RAM_BUF_SIZE)
		{
			SPI_Flash_write((gpsLogFlashStartAddress + ((gpsLogMemOffset / LOG_RAM_BUF_SIZE) * LOG_RAM_BUF_SIZE)), NMEARecordingBuffer, (gpsLogMemOffset % LOG_RAM_BUF_SIZE));
		}

		settingsSet(nonVolatileSettings.gpsLogMemOffset, gpsLogMemOffset);
	}
}

void gpsLoggingClear(void)
{
	uint32_t numFlashPages = gpsLogFlashMemSize / LOG_RAM_BUF_SIZE;
	uint32_t addr;

	watchdogRun(false);
	for(uint32_t i = 0; i < numFlashPages; i++)
	{
		addr = gpsLogFlashStartAddress + (i * LOG_RAM_BUF_SIZE);

		SPI_Flash_eraseSector(addr);
	}
	watchdogRun(true);
}
#endif // LOG_GPS_DATA
#endif // HAS_GPS
