#include <iostream>
#include <cstdint>
#include <array>
#include <cstring>

#include <stm32h7xx_ll_usart.h>
#include <stm32h7xx_ll_gpio.h>
#include <stm32h7xx_ll_rcc.h>
#include <stm32h7xx_ll_bus.h>

#include "USART1.hpp"
#include "RTClock.hpp"

FIFO_T<usart1::fifoRecord, 32> fifoQ_T;

static UARTConfigStruct usart1Config;
	
UARTConfigStruct* usart1::getUARTConfig()
{
	return &usart1Config;
}
	

//TODO: it would be nice if the device could set msgSize and qDepth
#define msgSize (uint16_t)256

void dma1S0RXDoneCallback();
void dma1S1TXDoneCallback();
void usart1DMAErrorCallback();
void usart1CMCallback();
void MX_USART1_UART_Init(void);

void usart1CMCallback()
{
	std::cout << "Got to usart1CMCallback ..." << std::endl;
	std::cout << "rxBuffer: " << usart1Config.rxBuffer << std::endl;

	usart1Config.isDataReady = true;
	usart1Config.isXferInProg = false;
	
	usart1::fifoRecord record_T;
	
	record_T.timestamp = RTClock::getDateTimeStampFast();
	memcpy(record_T.message.data(), usart1Config.rxBuffer, sizeof(record_T.message));
	fifoQ_T.enqueue(record_T);
}

uint16_t usart1::getQCount_T()
{
	return fifoQ_T.getCount();
}

void usart1::deQ_T(fifoRecord &qRecord)
{
	fifoQ_T.dequeue(qRecord);
}

void usart1::clearQ_T()
{
	fifoQ_T.clearFIFO();
}

std::string usart1::getRXBuffer()
{
	std::string tempString = (char*)usart1Config.rxBuffer;
	return tempString;
}

//bool usart1::startXfer(uint8_t* cmd, uint32_t cmdSize)
//{
//	UARTBase::startXfer(&usart1Config, cmd, cmdSize);
//}

bool usart1::isThisXferDone()
{
	return !usart1Config.isXferInProg;
}

void usart1::init()
{
	std::cout << "usart1::init()" << std::endl;
	
	MX_USART1_UART_Init();
	   
	//USART must be disabled to set character match character
	LL_USART_Disable(USART1);
	LL_USART_EnableIT_CM(USART1);
	//Set character match character to LF (decimal 10);
	USART1->CR2 |= (10 << 24);
	LL_USART_Enable(USART1);
	
	LL_USART_Disable(USART1);

	LL_USART_DisableOverrunDetect(USART1);
	LL_USART_ClearFlag_ORE(USART1);

	//Stream 0 = RX
	LL_DMA_EnableIT_TC(DMA1, LL_DMA_STREAM_0);
	LL_DMA_EnableIT_TE(DMA1, LL_DMA_STREAM_0);
	
	LL_DMA_EnableIT_TC(DMA1, LL_DMA_STREAM_1);
	LL_DMA_EnableIT_TE(DMA1, LL_DMA_STREAM_1);

	LL_USART_Enable(USART1);

	LL_DMA_EnableBufferableTransfer(DMA1, LL_DMA_STREAM_0);
	LL_USART_EnableDirectionRx(USART1);

	LL_DMA_EnableBufferableTransfer(DMA1, LL_DMA_STREAM_1);
	LL_USART_EnableDirectionTx(USART1);

	usart1Config.rxBufferSize = msgSize;
	usart1Config.rxBuffer = new uint8_t[msgSize];
	usart1Config.name = "gps1";
	usart1Config.uartNum = USART1;
	usart1Config.useDMA = true;
	usart1Config.dmaNum = DMA1;
	usart1Config.rxStreamNum = LL_DMA_STREAM_0;
	usart1Config.txStreamNum = LL_DMA_STREAM_1;
}	

void USART1_IRQHandler(void)
{
	std::cout << "Got to USART1_IRQHandlerCPF ..." << std::endl;
	if (LL_USART_IsActiveFlag_CM(USART1))
	{
		std::cout << "Got to USART1_IRQHandlerCPF if CM ..." << std::endl;
		LL_USART_ClearFlag_CM(USART1);
		usart1CMCallback();
	}
}

void DMA1_Stream0_IRQHandler(void)
{
	std::cout << "Got to DMA_Stream0_IRQHandlerCPF" << std::endl; 
	if (LL_DMA_IsActiveFlag_TC0(DMA1))
	{
		LL_DMA_ClearFlag_TC0(DMA1);
		dma1S0RXDoneCallback();
	}
	else if (LL_DMA_IsActiveFlag_TE0(DMA1))
	{
		usart1DMAErrorCallback();
		LL_DMA_ClearFlag_TE0(DMA1);
	}

	else
	{
		//Do something
	}

}

void DMA1_Stream1_IRQHandler(void)
{
	std::cout << "Got to DMA_Stream1_IRQHandlerCPF" << std::endl; 
	if (LL_DMA_IsActiveFlag_TC1(DMA1))
	{
		LL_DMA_ClearFlag_TC1(DMA1);
		dma1S1TXDoneCallback();
	}

	if (LL_DMA_IsActiveFlag_HT1(DMA1))
	{
		LL_DMA_ClearFlag_HT1(DMA1);
	}
	
	if (LL_DMA_IsActiveFlag_HT1(DMA1))
	{
		LL_DMA_ClearFlag_HT1(DMA1);
	}
	
	if (LL_DMA_IsActiveFlag_TE1(DMA1))
	{
		usart1DMAErrorCallback();
	}

}

void dma1S0RXDoneCallback(void)
{
	std::cout << "Got to dma1S0RXDoneCallback ..." << std::endl;
	uint8_t rx_cmplt = 1;
}

void dma1S1TXDoneCallback(void)
{
	std::cout << "Got to dma1S1TXDoneCallback ..." << std::endl;
	uint8_t tx_cmplt = 1;
	//gps1XferInProcess = false;
}

void usart1DMAErrorCallback(void)
{
	std::cout << "Got to usart1DMAErrorCallback ..." << std::endl;
	uint32_t regValue = READ_REG(DMA1->LISR);
	std::cout << "DMA1->LISR (hex): " << std::hex << regValue << std::endl;
	regValue = READ_REG(DMA1->HISR);
	std::cout << "DMA1->HISR (hex): " << std::hex << regValue << std::endl;
}

void MX_USART1_UART_Init(void)
{

	/* USER CODE BEGIN USART1_Init 0 */
	/* USER CODE END USART1_Init 0 */

	LL_USART_InitTypeDef USART_InitStruct = { 0 };
	LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 };
	LL_RCC_SetUSARTClockSource(LL_RCC_USART16_CLKSOURCE_PCLK2);

	/* Peripheral clock enable */
	LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_USART1);
	LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOB);

	/**USART1 GPIO Configuration
	PB6   ------> USART1_TX
	PB7   ------> USART1_RX
	*/
	GPIO_InitStruct.Pin = LL_GPIO_PIN_6 | LL_GPIO_PIN_7;
	GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE;
	GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW;
	GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
	GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
	GPIO_InitStruct.Alternate = LL_GPIO_AF_7;
	LL_GPIO_Init(GPIOB, &GPIO_InitStruct);

	/* USART1 DMA Init */
	/* USART1_RX Init */
	LL_DMA_SetPeriphRequest(DMA1, LL_DMA_STREAM_0, LL_DMAMUX1_REQ_USART1_RX);
	LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_STREAM_0, LL_DMA_DIRECTION_PERIPH_TO_MEMORY);
	LL_DMA_SetStreamPriorityLevel(DMA1, LL_DMA_STREAM_0, LL_DMA_PRIORITY_LOW);
	LL_DMA_SetMode(DMA1, LL_DMA_STREAM_0, LL_DMA_MODE_NORMAL);
	//LL_DMA_SetMode(DMA1, LL_DMA_STREAM_0, LL_DMA_MODE_CIRCULAR);
	LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_STREAM_0, LL_DMA_PERIPH_NOINCREMENT);
	LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_STREAM_0, LL_DMA_MEMORY_INCREMENT);
	LL_DMA_SetPeriphSize(DMA1, LL_DMA_STREAM_0, LL_DMA_PDATAALIGN_BYTE);
	LL_DMA_SetMemorySize(DMA1, LL_DMA_STREAM_0, LL_DMA_MDATAALIGN_BYTE);
	LL_DMA_DisableFifoMode(DMA1, LL_DMA_STREAM_0);

	/* USART1_TX Init */
	LL_DMA_SetPeriphRequest(DMA1, LL_DMA_STREAM_1, LL_DMAMUX1_REQ_USART1_TX);
	LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_STREAM_1, LL_DMA_DIRECTION_MEMORY_TO_PERIPH);
	LL_DMA_SetStreamPriorityLevel(DMA1, LL_DMA_STREAM_1, LL_DMA_PRIORITY_LOW);
	LL_DMA_SetMode(DMA1, LL_DMA_STREAM_1, LL_DMA_MODE_NORMAL);
	LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_STREAM_1, LL_DMA_PERIPH_NOINCREMENT);
	LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_STREAM_1, LL_DMA_MEMORY_INCREMENT);
	LL_DMA_SetPeriphSize(DMA1, LL_DMA_STREAM_1, LL_DMA_PDATAALIGN_BYTE);
	LL_DMA_SetMemorySize(DMA1, LL_DMA_STREAM_1, LL_DMA_MDATAALIGN_BYTE);
	LL_DMA_DisableFifoMode(DMA1, LL_DMA_STREAM_1);

	/* USART1 interrupt Init */
	NVIC_SetPriority(USART1_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 0, 0));
	NVIC_EnableIRQ(USART1_IRQn);

	/* USER CODE BEGIN USART1_Init 1 */
	/* USER CODE END USART1_Init 1 */
	USART_InitStruct.PrescalerValue = LL_USART_PRESCALER_DIV1;
	//USART_InitStruct.BaudRate = 115200;
	//USART_InitStruct.BaudRate = 122000;
	USART_InitStruct.BaudRate = 10148;
	USART_InitStruct.DataWidth = LL_USART_DATAWIDTH_8B;
	USART_InitStruct.StopBits = LL_USART_STOPBITS_1;
	USART_InitStruct.Parity = LL_USART_PARITY_NONE;
	USART_InitStruct.TransferDirection = LL_USART_DIRECTION_TX_RX;
	USART_InitStruct.HardwareFlowControl = LL_USART_HWCONTROL_NONE;
	USART_InitStruct.OverSampling = LL_USART_OVERSAMPLING_16;
	LL_USART_Init(USART1, &USART_InitStruct);
	LL_USART_SetTXFIFOThreshold(USART1, LL_USART_FIFOTHRESHOLD_1_8);
	LL_USART_SetRXFIFOThreshold(USART1, LL_USART_FIFOTHRESHOLD_1_8);
	LL_USART_DisableFIFO(USART1);
	LL_USART_ConfigAsyncMode(USART1);

	/* USER CODE BEGIN WKUPType USART1 */
	/* USER CODE END WKUPType USART1 */

	LL_USART_Enable(USART1);

	/* Polling USART1 initialisation */
	while ((!(LL_USART_IsActiveFlag_TEACK(USART1))) || (!(LL_USART_IsActiveFlag_REACK(USART1))))
	{
	}
	/* USER CODE BEGIN USART1_Init 2 */
 
	/* USER CODE END USART1_Init 2 */
}