#include <iostream>
#include <cstring>
#include <stdint.h>
#include <array>

#include "stm32h7xx_ll_usart.h"
#include "stm32h7xx_ll_gpio.h"
#include "stm32h7xx_ll_rcc.h"
#include "stm32h7xx_ll_bus.h"
#include "stm32h7xx_ll_dma.h"

#include "UARTBase.hpp"

bool startDMAXfer(UARTConfigStruct* uartConfig, uint8_t txBuff[], uint32_t txBuffSize);
bool startITXfer(UARTConfigStruct* uartConfig, uint8_t txBuff[], uint32_t txBuffSize);

static void MX_USART3_UART_Init(void);
static void MX_UART4_Init(void);

uint16_t UARTBase::getQCount_T(UARTBase::devices deviceNum)
{
	uint16_t qCount = 0;
	switch (deviceNum)
	{
	case UARTBase::gps1:
		qCount = usart1::getQCount_T();
		break;
	}
}

void UARTBase::deQ_T(UARTBase::devices deviceNum, usart1::fifoRecord &deviceRecord)
{
	switch (deviceNum)
	{
	case UARTBase::gps1:
		usart1::deQ_T(deviceRecord);
		break;
	}
}


UARTConfigStruct* getUARTConfig(UARTBase::devices deviceNum)
{
	UARTConfigStruct* uConfig;
	switch (deviceNum)
	{
	case UARTBase::gps1:
		uConfig = usart1::getUARTConfig();
		break;
	}
	return uConfig;
}

void UARTBase::initUARTs()
{
	usart1::init();
}

bool UARTBase::isXferDone(devices deviceNum)
{
	UARTConfigStruct* uartConfig = getUARTConfig(deviceNum);
	return uartConfig->isDataReady;
}

const std::string UARTBase::getRXBuffer(devices deviceNum)
{
	UARTConfigStruct* uartConfig = getUARTConfig(deviceNum);
	
	std::string tempString = (char*)uartConfig->rxBuffer;
	return tempString;
}


bool UARTBase::startXfer(devices deviceNum, uint8_t *txBuff, uint32_t txBuffSize)
{
	bool returnValue = false;
	
	UARTConfigStruct* uartConfig = getUARTConfig(deviceNum);

	if (uartConfig->isXferInProg)
		{
		std::cout << "**ERROR: startXfer Xfer already in progress.  Ignoring request" << std::endl;
		returnValue = false;
	}
	else
	{
		if (uartConfig->useDMA)
		{
			returnValue = startDMAXfer(uartConfig, txBuff, txBuffSize);
		}
		else
		{
			returnValue = startITXfer(uartConfig, txBuff, txBuffSize);
		}
	}
	return returnValue;
}

bool startDMAXfer(UARTConfigStruct* uartConfig, uint8_t *txBuff, uint32_t txBuffSize)
{
	bool returnValue = false;
		
	memset(uartConfig->rxBuffer, 0, uartConfig->rxBufferSize);
	uartConfig->isDataReady = false;
	uartConfig->isXferInProg = true;

	std::cout << "Starting DMA transfer ..." << std::endl;
	//The reference manual page 2011 says "the DMA channel must be disabled before resetting the [USARTN->CR1] UE bit"
	//Setup TX
	LL_DMA_ConfigAddresses(uartConfig->dmaNum,
		uartConfig->txStreamNum,
		(uint32_t)txBuff,
		LL_USART_DMA_GetRegAddr(uartConfig->uartNum, LL_USART_DMA_REG_DATA_TRANSMIT),
		LL_DMA_GetDataTransferDirection(uartConfig->dmaNum, uartConfig->txStreamNum));
	LL_DMA_SetDataLength(uartConfig->dmaNum, uartConfig->txStreamNum, txBuffSize);
	
	//Setup RX
	LL_DMA_ConfigAddresses(uartConfig->dmaNum,
		uartConfig->rxStreamNum,
		LL_USART_DMA_GetRegAddr(uartConfig->uartNum, LL_USART_DMA_REG_DATA_RECEIVE),
		(uint32_t)uartConfig->rxBuffer,
		LL_DMA_GetDataTransferDirection(uartConfig->dmaNum, uartConfig->rxStreamNum));
	LL_DMA_SetDataLength(uartConfig->dmaNum, uartConfig->rxStreamNum, uartConfig->rxBufferSize);
		
	LL_DMA_EnableStream(uartConfig->dmaNum, uartConfig->rxStreamNum);
	LL_USART_EnableDMAReq_RX(uartConfig->uartNum);

	LL_DMA_EnableStream(uartConfig->dmaNum, uartConfig->txStreamNum);
	LL_USART_EnableDMAReq_TX(uartConfig->uartNum);
		
	returnValue = true;
	return returnValue;
}

bool startITXfer(UARTConfigStruct* uartConfig, uint8_t *txBuff, uint32_t txBuffSize)
{
	std::cout << "StartITXfer not implemented yet" << std::endl;
	return false;
}

/**
  * @brief USART2 Initialization Function
  * @param None
  * @retval None
  */
//static 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 */
//
//}

/**
  * @brief USART3 Initialization Function
  * @param None
  * @retval None
  */
static void MX_USART3_UART_Init(void)
{

	/* USER CODE BEGIN USART3_Init 0 */

	/* USER CODE END USART3_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_USART3);

	LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOD);
	/**USART3 GPIO Configuration
	PD8   ------> USART3_TX
	PD9   ------> USART3_RX
	*/
	GPIO_InitStruct.Pin = LL_GPIO_PIN_8 | LL_GPIO_PIN_9;
	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);

	/* USART3 DMA Init */

	/* USART3_RX Init */
	LL_DMA_SetPeriphRequest(DMA1, LL_DMA_STREAM_2, LL_DMAMUX1_REQ_USART3_RX);

	LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_STREAM_2, LL_DMA_DIRECTION_PERIPH_TO_MEMORY);

	LL_DMA_SetStreamPriorityLevel(DMA1, LL_DMA_STREAM_2, LL_DMA_PRIORITY_LOW);

	LL_DMA_SetMode(DMA1, LL_DMA_STREAM_2, LL_DMA_MODE_NORMAL);

	LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_STREAM_2, LL_DMA_PERIPH_NOINCREMENT);

	LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_STREAM_2, LL_DMA_MEMORY_INCREMENT);

	LL_DMA_SetPeriphSize(DMA1, LL_DMA_STREAM_2, LL_DMA_PDATAALIGN_BYTE);

	LL_DMA_SetMemorySize(DMA1, LL_DMA_STREAM_2, LL_DMA_MDATAALIGN_BYTE);

	LL_DMA_DisableFifoMode(DMA1, LL_DMA_STREAM_2);

	/* USART3_TX Init */
	LL_DMA_SetPeriphRequest(DMA1, LL_DMA_STREAM_3, LL_DMAMUX1_REQ_USART3_TX);

	LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_STREAM_3, LL_DMA_DIRECTION_MEMORY_TO_PERIPH);

	LL_DMA_SetStreamPriorityLevel(DMA1, LL_DMA_STREAM_3, LL_DMA_PRIORITY_LOW);

	LL_DMA_SetMode(DMA1, LL_DMA_STREAM_3, LL_DMA_MODE_NORMAL);

	LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_STREAM_3, LL_DMA_PERIPH_NOINCREMENT);

	LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_STREAM_3, LL_DMA_MEMORY_INCREMENT);

	LL_DMA_SetPeriphSize(DMA1, LL_DMA_STREAM_3, LL_DMA_PDATAALIGN_BYTE);

	LL_DMA_SetMemorySize(DMA1, LL_DMA_STREAM_3, LL_DMA_MDATAALIGN_BYTE);

	LL_DMA_DisableFifoMode(DMA1, LL_DMA_STREAM_3);

	/* USART3 interrupt Init */
	NVIC_SetPriority(USART3_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 0, 0));
	NVIC_EnableIRQ(USART3_IRQn);

	/* USER CODE BEGIN USART3_Init 1 */

	/* USER CODE END USART3_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(USART3, &USART_InitStruct);
	LL_USART_SetTXFIFOThreshold(USART3, LL_USART_FIFOTHRESHOLD_1_8);
	LL_USART_SetRXFIFOThreshold(USART3, LL_USART_FIFOTHRESHOLD_1_8);
	LL_USART_DisableFIFO(USART3);
	LL_USART_ConfigAsyncMode(USART3);

	/* USER CODE BEGIN WKUPType USART3 */

	/* USER CODE END WKUPType USART3 */

	LL_USART_Enable(USART3);

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

	/* USER CODE END USART3_Init 2 */

}

/**
  * @brief UART4 Initialization Function
  * @param None
  * @retval None
  */
static void MX_UART4_Init(void)
{

	/* USER CODE BEGIN UART4_Init 0 */

	/* USER CODE END UART4_Init 0 */

	LL_USART_InitTypeDef UART_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_UART4);

	LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOA);
	/**UART4 GPIO Configuration
	PA0   ------> UART4_TX
	PA1   ------> UART4_RX
	*/
	GPIO_InitStruct.Pin = LL_GPIO_PIN_0 | LL_GPIO_PIN_1;
	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_8;
	LL_GPIO_Init(GPIOA, &GPIO_InitStruct);

	/* UART4 interrupt Init */
	NVIC_SetPriority(UART4_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 0, 0));
	NVIC_EnableIRQ(UART4_IRQn);

	/* USER CODE BEGIN UART4_Init 1 */

	/* USER CODE END UART4_Init 1 */
	UART_InitStruct.PrescalerValue = LL_USART_PRESCALER_DIV1;
	UART_InitStruct.BaudRate = 115200;
	UART_InitStruct.DataWidth = LL_USART_DATAWIDTH_8B;
	UART_InitStruct.StopBits = LL_USART_STOPBITS_1;
	UART_InitStruct.Parity = LL_USART_PARITY_NONE;
	UART_InitStruct.TransferDirection = LL_USART_DIRECTION_TX_RX;
	UART_InitStruct.HardwareFlowControl = LL_USART_HWCONTROL_NONE;
	UART_InitStruct.OverSampling = LL_USART_OVERSAMPLING_16;
	LL_USART_Init(UART4, &UART_InitStruct);
	LL_USART_DisableFIFO(UART4);
	LL_USART_SetTXFIFOThreshold(UART4, LL_USART_FIFOTHRESHOLD_1_8);
	LL_USART_SetRXFIFOThreshold(UART4, LL_USART_FIFOTHRESHOLD_1_8);
	LL_USART_ConfigAsyncMode(UART4);

	/* USER CODE BEGIN WKUPType UART4 */

	/* USER CODE END WKUPType UART4 */

	LL_USART_Enable(UART4);

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

	/* USER CODE END UART4_Init 2 */

}