#include "stm32h7xx_hal.h"
#include <memory.h>

#include "GPS1.hpp"
FIFO msgFIFO(16, 256, "gpsFIFO");

//uint8_t rxBuffer[128];
ALIGN_32BYTES(uint8_t rxBuffer[128]);

GPS1::GPS1()
{
	cout << "\r\nGPS1 constructor" << endl;

	Logger engLogger = Logger::getInstance();
	
	initUART4();
	defineCommands();
}

extern FIFO gpsFIFO;

bool GPS1::isGPSMsgReady()
{
	return false;	
}

void GPS1::processRX(UART_HandleTypeDef *uartHandle)  //TODO Make this part of GPS1 class
{
	if (rxBuffer[0] != 0)
	{
		gpsMessage[gpsMsgCount] = rxBuffer[0];
		gpsMsgCount++;
		if (rxBuffer[0] == '\n')
		{   
			gpsFIFO.enqueue((uint64_t)1000, gpsMessage);
			gpsMsgReceived = true;
			cout << "processGPSRXComplete got LF terminated msg: " << gpsMessage << endl;
		}
		else
			HAL_UART_Receive_IT(uartHandle, rxBuffer, 1);
	}
	else
	{
		HAL_UART_Receive_IT(uartHandle, rxBuffer, 1);
	}

}

mcuRealTimeClock::DateTimeStruct GPS1::parsePUBX04Time(uint8_t inMsg[])
{
	mcuRealTimeClock::DateTimeStruct gpsDT;
	
	uint64_t tickstamp = 0;
	uint8_t qBytes[256];
	
	gpsFIFO.dequeue(tickstamp, qBytes);
	
	gpsDT.year = (qBytes[23] - 48) * 10 + (qBytes[24] - 48) + 2000;
	if (gpsDT.year > 18)
	{
		gpsDT.hour = (qBytes[9] - 48) * 10 + (qBytes[10] - 48);
		gpsDT.minute = (qBytes[11] - 48) * 10 + (qBytes[12] - 48);
		gpsDT.second = (qBytes[13] - 48) * 10 + (qBytes[14] - 48);
		
		gpsDT.date = (qBytes[19] - 48) * 10 + (qBytes[20] - 48);
		gpsDT.month = (qBytes[21] - 48) * 10 + (qBytes[22] - 48);
	}
	
	return gpsDT;
}
		
void GPS1::logPosition(string stateID, GPS1Position gps1Position)
{
	string comment;
	
	//TODO P1 it would probably be better to convert the FIFO tick count to CPF timestamp and log it
	comment = engMsgID + "\t" + mcuRTC.getTimestamp().data() + "\t" + stateID + "\tGPS Fix: " + "\t\t\t\t\t\t\t\t" + gps1Position.message + "\r\n";
	logger.writeLineEng(comment, false, true);
}

GPS1::GPS1Position GPS1::parsePUBX00Position()
{
	GPS1::GPS1Position gps1Position;
	
	uint64_t tickstamp = 0;
	uint8_t qBytes[256];
	
	gpsFIFO.dequeue(tickstamp, qBytes);
	gps1Position.message = (char*)qBytes;
	//TODO parse qBytes into the rest of the GPS1Position fields
	
	return gps1Position;
}

int8_t GPS1::txrxDMA(CommandNum cmdNum)
{
	HAL_StatusTypeDef returnResult;
    

	gpsMsgReceived = false;
//	memset(rxBuffer, 0, sizeof(rxBuffer));
//	memset(gpsMessage, 0, sizeof(gpsMessage));
	gpsMsgReceived = false;
	gpsMsgCount = 0;
    
	returnResult = HAL_UART_Receive_DMA(&uart4Handle, rxBuffer, gpsCmd[cmdNum].replyLength);    
//	returnResult = HAL_UART_Receive_DMA(&uart4Handle, rxBuffer, 1024);    //TODO need to figure out how to make character match interrupt work and set buffer size to some large number

	if (returnResult != HAL_OK)
	{
		//TODO process error
		return - 1;
	}
            
	returnResult = HAL_UART_Transmit_DMA(&uart4Handle, gpsCmd[cmdNum].commandPtr, gpsCmd[cmdNum].commandLength);
	if (returnResult != HAL_OK)
	{
		//TODO process error
		return - 2;
	}

	HAL_Delay(1000);
	
	return 0;
}

int8_t GPS1::txrxIT(CommandNum cmdNum)
{
	HAL_StatusTypeDef returnResult;
    

	gpsMsgReceived = false;
	memset(rxBuffer, 0, sizeof(rxBuffer));
	memset(gpsMessage, 0, sizeof(gpsMessage));
	gpsMsgReceived = false;
	gpsMsgCount = 0;
    
	returnResult = HAL_UART_Receive_IT(&uart4Handle, rxBuffer, gpsCmd[cmdNum].replyLength);
	if (returnResult != HAL_OK)
	{
		//TODO process error
		return - 1;
	}
            
	returnResult = HAL_UART_Transmit_IT(&uart4Handle, gpsCmd[cmdNum].commandPtr, gpsCmd[cmdNum].commandLength);
	if (returnResult != HAL_OK)
	{
		//TODO process error
		return - 2;
	}

	return 0;
}

static DMA_HandleTypeDef hdma_tx;
static DMA_HandleTypeDef hdma_rx;

uint8_t GPS1::initUART4()
{
	HAL_StatusTypeDef halReturn;
	
	GPIO_InitTypeDef GPIO_InitStructure;
	
	__USART4_CLK_ENABLE();
	__GPIOH_CLK_ENABLE();
	__HAL_RCC_DMA1_CLK_ENABLE();
    
	GPIO_InitStructure.Pin = GPIO_PIN_13;
	GPIO_InitStructure.Mode = GPIO_MODE_AF_PP;
	GPIO_InitStructure.Alternate = GPIO_AF8_UART4;
	GPIO_InitStructure.Speed = GPIO_SPEED_HIGH;
	GPIO_InitStructure.Pull = GPIO_NOPULL;
	HAL_GPIO_Init(GPIOH, &GPIO_InitStructure);
    
	GPIO_InitStructure.Pin = GPIO_PIN_14;
	GPIO_InitStructure.Mode = GPIO_MODE_AF_OD;
	HAL_GPIO_Init(GPIOH, &GPIO_InitStructure);
 
	uart4Handle.Instance        = UART4;
	uart4Handle.Init.BaudRate   = 9600;
	uart4Handle.Init.WordLength = UART_WORDLENGTH_8B;
	uart4Handle.Init.StopBits   = UART_STOPBITS_1;
	uart4Handle.Init.Parity     = UART_PARITY_NONE;
	uart4Handle.Init.HwFlowCtl  = UART_HWCONTROL_NONE;
	uart4Handle.Init.Mode       = UART_MODE_TX_RX;
	if (HAL_UART_Init(&uart4Handle) != HAL_OK)
		asm("bkpt 255");

	hdma_tx.Instance                 = DMA1_Stream1;
	hdma_tx.Init.Direction           = DMA_MEMORY_TO_PERIPH;
	hdma_tx.Init.PeriphInc           = DMA_PINC_DISABLE;
	hdma_tx.Init.MemInc              = DMA_MINC_ENABLE;
	hdma_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
	hdma_tx.Init.MemDataAlignment    = DMA_MDATAALIGN_BYTE;
	hdma_tx.Init.Mode                = DMA_NORMAL;
	hdma_tx.Init.Priority            = DMA_PRIORITY_LOW;
	hdma_tx.Init.FIFOMode            = DMA_FIFOMODE_DISABLE;
	hdma_tx.Init.FIFOThreshold       = DMA_FIFO_THRESHOLD_FULL;
	hdma_tx.Init.MemBurst            = DMA_MBURST_INC4;
	hdma_tx.Init.PeriphBurst         = DMA_PBURST_INC4;
	hdma_tx.Init.Request             = DMA_REQUEST_UART4_TX;
	halReturn = HAL_DMA_Init(&hdma_tx);

	/* Associate the initialized DMA handle to the UART handle */
	__HAL_LINKDMA(&uart4Handle, hdmatx, hdma_tx);

	/* Configure the DMA handler for reception process */
	hdma_rx.Instance                 = DMA1_Stream0;
	hdma_rx.Init.Direction           = DMA_PERIPH_TO_MEMORY;
	hdma_rx.Init.PeriphInc           = DMA_PINC_DISABLE;
	hdma_rx.Init.MemInc              = DMA_MINC_ENABLE;
	hdma_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
	hdma_rx.Init.MemDataAlignment    = DMA_MDATAALIGN_BYTE;
	hdma_rx.Init.Mode                = DMA_NORMAL;
	hdma_rx.Init.Priority            = DMA_PRIORITY_HIGH;
	hdma_rx.Init.FIFOMode            = DMA_FIFOMODE_DISABLE;
	hdma_rx.Init.FIFOThreshold       = DMA_FIFO_THRESHOLD_FULL;
	hdma_rx.Init.MemBurst            = DMA_MBURST_INC4;
	hdma_rx.Init.PeriphBurst         = DMA_PBURST_INC4;
	hdma_rx.Init.Request             = DMA_REQUEST_UART4_RX;
	halReturn = HAL_DMA_Init(&hdma_rx);

	/* Associate the initialized DMA handle to the the UART handle */
	__HAL_LINKDMA(&uart4Handle, hdmarx, hdma_rx);
    
	/*##-4- Configure the NVIC for DMA #########################################*/
	/* NVIC configuration for DMA transfer complete interrupt (USART4_TX) */
	HAL_NVIC_SetPriority(DMA1_Stream1_IRQn, 0, 1);
	HAL_NVIC_EnableIRQ(DMA1_Stream1_IRQn);
    
	/* NVIC configuration for DMA transfer complete interrupt (USART4_RX) */
	HAL_NVIC_SetPriority(DMA1_Stream0_IRQn, 0, 0);
	HAL_NVIC_EnableIRQ(DMA1_Stream0_IRQn);
 	
	HAL_NVIC_SetPriority(UART4_IRQn, 0, 1);
	NVIC_EnableIRQ(UART4_IRQn);

	//Enable charcter match interrupt and set character to match in UART control register 2
	__HAL_UART_ENABLE_IT(&uart4Handle, UART_IT_CM);		
	uint32_t cr2Value = 0;
	cr2Value = UART4->CR2;
	uint32_t clearMask =	0xFF000000;
	uint32_t setMask =		0x0D000000;
	MODIFY_REG(UART4->CR2, clearMask, setMask);
	cr2Value = UART4->CR2;
	UART4->CR2 |= 0x0D000000;
	cr2Value = UART4->CR2;
	
	
	return 0;		//todo return some kind of error code
}

extern "C" void DMA1_Stream1_IRQHandler()
{
	HAL_DMA_IRQHandler(&hdma_tx);
}

extern "C" void DMA1_Stream0_IRQHandler()
{
	HAL_DMA_IRQHandler(&hdma_rx);
}

uint8_t GPS1::getPosition()
{
	//txrxIT(cmdGetPosition);
	txrxDMA(cmdGetPosition);
	return 0; //TODO return some kind of error code
}

uint8_t GPS1::getTime()
{
	
	txrxDMA(cmdGetTime);
	return 0;	//TODO return some kind of error code
}

void GPS1::defineCommands()
{
	gpsCmd[cmdGetTime].commandPtr = (uint8_t*)"$PUBX,04*37\r\n"; 
	gpsCmd[cmdGetTime].commandLength = sizeof("$PUBX,04*37\r\n"); 
	gpsCmd[cmdGetTime].replyLength = 1;			//TODO need to get character match working so this can be a large number

	gpsCmd[cmdGetPosition].commandPtr = (uint8_t*)"$PUBX,00*33\r\n"; 
	gpsCmd[cmdGetPosition].commandLength = sizeof("$PUBX,00*33\r\n"); 
	gpsCmd[cmdGetPosition].replyLength = 1; 
}

void GPS1::cancelReceiveIT()
{
	//TODO finish cancelReturnIT() stub
}

void GPS1::logRX_ITError()
{
	//TODO finish logRX_ITError() stub
}
	
void GPS1::logTX_ITError()
{
	//TODO finish logRX_ITError() stub
}