#include <iostream>
#include <cstdint>

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

#include "USART2.hpp"
#include "UARTBase.hpp"
#include "RTClock.hpp"
#include "FIFOQueue.hpp"
	
constexpr uint16_t msgSize = 256;
constexpr uint16_t qDepth = 32;
FIFOQueue fifoQusart2(qDepth, msgSize, "usart2"); 

void dma1S4RXDoneCallback();
void dma1S5TXDoneCallback();
void usart2DMAErrorCallback();
void usart2CMCallback();
void MX_USART2_UART_Init(void);

//UARTBase ioPort;
UARTConfigStruct uartConfig;

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

bool usart2::startXfer(uint8_t* cmd, uint32_t cmdSize)
{
	//UARTBase::startXfer(&uartConfig, cmd, cmdSize);
}
bool usart2::isXferDone()
{
	return !uartConfig.isXferInProg;
}

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

	LL_USART_DisableOverrunDetect(USART2);
	LL_USART_ClearFlag_ORE(USART2);

	//Stream 0 = RX
	LL_DMA_EnableIT_TC(DMA1, LL_DMA_STREAM_4);
	LL_DMA_EnableIT_TE(DMA1, LL_DMA_STREAM_5);
	
	LL_DMA_EnableIT_TC(DMA1, LL_DMA_STREAM_4);
	LL_DMA_EnableIT_TE(DMA1, LL_DMA_STREAM_5);

	LL_USART_Enable(USART2);

	LL_DMA_EnableBufferableTransfer(DMA1, LL_DMA_STREAM_4);
	LL_USART_EnableDirectionRx(USART2);

	LL_DMA_EnableBufferableTransfer(DMA1, LL_DMA_STREAM_5);
	LL_USART_EnableDirectionTx(USART2);

	uartConfig.rxBufferSize = msgSize;
	uartConfig.rxBuffer = new uint8_t[msgSize];
	uartConfig.name = "need a device";
	uartConfig.uartNum = USART2;
	uartConfig.useDMA = true;
	uartConfig.dmaNum = DMA1;
	uartConfig.rxStreamNum = LL_DMA_STREAM_4;
	uartConfig.txStreamNum = LL_DMA_STREAM_5;
}	

void usart2CMCallback()
{
	std::cout << "Got to usart2CMCallback ..." << std::endl;
	
	fifoQusart2.enqueue(RTClock::getDateTimeStampFast(), uartConfig.rxBuffer);
	uartConfig.isDataReady = true;
	uartConfig.isXferInProg = false;
	LL_USART_ClearFlag_CM(USART2);
}

void USART2_IRQHandler(void)
{
	std::cout << "Got to USART2_IRQHandlerCPF ..." << std::endl;
	if (LL_USART_IsActiveFlag_CM(USART2))
	{
		std::cout << "Got to USART2_IRQHandlerCPF if CM ..." << std::endl;
		usart2CMCallback();
	}
}

void DMA1_Stream4_IRQHandler(void)
{
	std::cout << "Got to DMA_Stream40_IRQHandlerCPF" << std::endl; 
	if (LL_DMA_IsActiveFlag_TC0(DMA1))
	{
		LL_DMA_ClearFlag_TC0(DMA1);
		dma1S4RXDoneCallback();
	}
	else if (LL_DMA_IsActiveFlag_TE0(DMA1))
	{
		LL_DMA_ClearFlag_TE0(DMA1);
		usart2DMAErrorCallback();
	}

	else
	{
		//Do something
	}

}

void DMA1_Stream5_IRQHandler(void)
{
	std::cout << "Got to DMA_Stream5_IRQHandlerCPF" << std::endl; 
	if (LL_DMA_IsActiveFlag_TC1(DMA1))
	{
		LL_DMA_ClearFlag_TC1(DMA1);

		dma1S5TXDoneCallback();
	}
	else if (LL_DMA_IsActiveFlag_TE1(DMA1))
	{
		LL_DMA_ClearFlag_TE1(DMA1);
		usart2DMAErrorCallback();

	}

	else
	{
		//Do something...
	}
}

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

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

void usart2DMAErrorCallback(void)
{
	std::cout << "Got to usart2DMAErrorCallback ..." << 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;
	
	LL_DMA_DisableStream(DMA1, LL_DMA_STREAM_0);
	LL_DMA_DisableStream(DMA1, LL_DMA_STREAM_1);
}

/**
  * @brief USART2 Initialization Function
  * @param None
  * @retval None
  */
void MX_USART2_UART_Init(void)
{

	/* USER CODE BEGIN USART2_Init 0 */

	/* USER CODE END USART2_Init 0 */

	LL_USART_InitTypeDef USART_InitStruct = { 0 };

	LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 };

	LL_RCC_SetUSARTClockSource(LL_RCC_USART234578_CLKSOURCE_PCLK1);

	/* Peripheral clock enable */
	LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_USART2);

	LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOA);
	LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOD);
	/**USART2 GPIO Configuration
	PA2   ------> USART2_TX
	PD6   ------> USART2_RX
	*/
	GPIO_InitStruct.Pin = LL_GPIO_PIN_2;
	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(GPIOA, &GPIO_InitStruct);

	GPIO_InitStruct.Pin = LL_GPIO_PIN_6;
	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(GPIOD, &GPIO_InitStruct);

	/* USART2 DMA Init */

	/* USART2_RX Init */
	LL_DMA_SetPeriphRequest(DMA1, LL_DMA_STREAM_4, LL_DMAMUX1_REQ_USART2_RX);

	LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_STREAM_4, LL_DMA_DIRECTION_PERIPH_TO_MEMORY);

	LL_DMA_SetStreamPriorityLevel(DMA1, LL_DMA_STREAM_4, LL_DMA_PRIORITY_LOW);

	LL_DMA_SetMode(DMA1, LL_DMA_STREAM_4, LL_DMA_MODE_NORMAL);

	LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_STREAM_4, LL_DMA_PERIPH_NOINCREMENT);

	LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_STREAM_4, LL_DMA_MEMORY_INCREMENT);

	LL_DMA_SetPeriphSize(DMA1, LL_DMA_STREAM_4, LL_DMA_PDATAALIGN_BYTE);

	LL_DMA_SetMemorySize(DMA1, LL_DMA_STREAM_4, LL_DMA_MDATAALIGN_BYTE);

	LL_DMA_DisableFifoMode(DMA1, LL_DMA_STREAM_4);

	/* USART2_TX Init */
	LL_DMA_SetPeriphRequest(DMA1, LL_DMA_STREAM_5, LL_DMAMUX1_REQ_USART2_TX);

	LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_STREAM_5, LL_DMA_DIRECTION_MEMORY_TO_PERIPH);

	LL_DMA_SetStreamPriorityLevel(DMA1, LL_DMA_STREAM_5, LL_DMA_PRIORITY_LOW);

	LL_DMA_SetMode(DMA1, LL_DMA_STREAM_5, LL_DMA_MODE_NORMAL);

	LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_STREAM_5, LL_DMA_PERIPH_NOINCREMENT);

	LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_STREAM_5, LL_DMA_MEMORY_INCREMENT);

	LL_DMA_SetPeriphSize(DMA1, LL_DMA_STREAM_5, LL_DMA_PDATAALIGN_BYTE);

	LL_DMA_SetMemorySize(DMA1, LL_DMA_STREAM_5, LL_DMA_MDATAALIGN_BYTE);

	LL_DMA_DisableFifoMode(DMA1, LL_DMA_STREAM_5);

	/* USER CODE BEGIN USART2_Init 1 */

	/* USER CODE END USART2_Init 1 */
	USART_InitStruct.PrescalerValue = LL_USART_PRESCALER_DIV1;
	USART_InitStruct.BaudRate = 115200;
	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(USART2, &USART_InitStruct);
	LL_USART_SetTXFIFOThreshold(USART2, LL_USART_FIFOTHRESHOLD_1_8);
	LL_USART_SetRXFIFOThreshold(USART2, LL_USART_FIFOTHRESHOLD_1_8);
	LL_USART_DisableFIFO(USART2);
	LL_USART_ConfigAsyncMode(USART2);

	/* USER CODE BEGIN WKUPType USART2 */

	/* USER CODE END WKUPType USART2 */

	LL_USART_Enable(USART2);

	/* Polling USART2 initialisation */
	while ((!(LL_USART_IsActiveFlag_TEACK(USART2))) || (!(LL_USART_IsActiveFlag_REACK(USART2))))
	{
	}
	/* USER CODE BEGIN USART2_Init 2 */

	/* USER CODE END USART2_Init 2 */

}