#include <iostream>

#include "stm32h7xx_ll_system.h"
#include "stm32h7xx_ll_pwr.h"
#include "stm32h7xx_ll_rcc.h"
#include "stm32h7xx_ll_utils.h"
#include "stm32h7xx_ll_gpio.h"

#include "BSP.hpp"
#include "UARTBase.hpp"
#include "RTClock.hpp"
#include "FileSys.hpp"
#include "Logger.hpp"
#include "StateVariables.hpp"

#define PushButton_Pin LL_GPIO_PIN_13
#define PushButton_GPIO_Port GPIOC

#define uSD_Detect_Pin LL_GPIO_PIN_3
#define uSD_Detect_GPIO_Port GPIOA

#define LEDGreenPin LL_GPIO_PIN_0
#define LEDGreenPort GPIOB
#define LEDYellowPin LL_GPIO_PIN_1
#define LEDYellowPort GPIOE
#define LEDRedPin LL_GPIO_PIN_14
#define LEDRedPort GPIOB

void SysClockErrorHandler(void);
void SysClockConfig(void);
static void MX_GPIO_Init();
static void MX_DMA_Init();

void BSP::init()
{
	std::cout << "BSP::init HAL_init" << std::endl;
	HAL_Init();
	
	std::cout << "BSP::init SysClockConfig" << std::endl;
	SysClockConfig();

	std::cout << "BSP::init MX_GPIO_Init" << std::endl;
	MX_GPIO_Init();

	std::cout << "BSP::init MX_DMA_Init" << std::endl;
	MX_DMA_Init();
		
	std::cout << "BSP::init initRTC" << std::endl;
	RTClock::initRTC();
	
	std::cout << "BSP::init initFileSys" << std::endl;
	FileSys::initFileSys();
	
	std::cout << "BSP::init UARTs" << std::endl;
	UARTBase::initUARTs();
	
	std::cout << "BSP::init gps1.init" << std::endl;
	gps1.init();
	
	std::cout << "BSP::init gps1.synchRTCtoGPS " << std::endl;
	
	//Do this here in order to get RTC initialized for file naming in Logger::init
	gps1.synchRTCtoGPS(false);
	usart1::clearQ_T();

	if (StateVariables::getProfileNum() == 0)
		StateVariables::setMissionStartTime();

	std::cout << "BSP::init logger1.init" << std::endl;
	Logger::init();
	
	//Blink LED just to make sure basics are working
	std::cout << "Starting blinking ..." << std::endl;
	for (int i = 0; i < 10; i++)
	{
		LL_GPIO_SetOutputPin(LEDGreenPort, LEDGreenPin);
		LL_mDelay(50);
		LL_GPIO_ResetOutputPin(LEDGreenPort, LEDGreenPin);
		LL_mDelay(50);
	}
}

/**
  * Enable DMA controller clock
  */
static void MX_DMA_Init(void)
{

	/* Init with LL driver */
	/* DMA controller clock enable */
	LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_DMA1);
	LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_DMA2);

	/* DMA interrupt init */
	/* DMA1_Stream0_IRQn interrupt configuration */
	NVIC_SetPriority(DMA1_Stream0_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 0, 0));
	NVIC_EnableIRQ(DMA1_Stream0_IRQn);
	/* DMA1_Stream1_IRQn interrupt configuration */
	NVIC_SetPriority(DMA1_Stream1_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 0, 0));
	NVIC_EnableIRQ(DMA1_Stream1_IRQn);
	/* DMA1_Stream2_IRQn interrupt configuration */
	NVIC_SetPriority(DMA1_Stream2_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 0, 0));
	NVIC_EnableIRQ(DMA1_Stream2_IRQn);
	/* DMA1_Stream3_IRQn interrupt configuration */
	NVIC_SetPriority(DMA1_Stream3_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 0, 0));
	NVIC_EnableIRQ(DMA1_Stream3_IRQn);
	/* DMA1_Stream4_IRQn interrupt configuration */
	NVIC_SetPriority(DMA1_Stream4_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 0, 0));
	NVIC_EnableIRQ(DMA1_Stream4_IRQn);
	/* DMA1_Stream5_IRQn interrupt configuration */
	NVIC_SetPriority(DMA1_Stream5_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 0, 0));
	NVIC_EnableIRQ(DMA1_Stream5_IRQn);
	/* DMA1_Stream6_IRQn interrupt configuration */
	NVIC_SetPriority(DMA1_Stream6_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 0, 0));
	NVIC_EnableIRQ(DMA1_Stream6_IRQn);
	/* DMA1_Stream7_IRQn interrupt configuration */
	NVIC_SetPriority(DMA1_Stream7_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 0, 0));
	NVIC_EnableIRQ(DMA1_Stream7_IRQn);
	/* DMA2_Stream0_IRQn interrupt configuration */
	NVIC_SetPriority(DMA2_Stream0_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 0, 0));
	NVIC_EnableIRQ(DMA2_Stream0_IRQn);
	/* DMA2_Stream1_IRQn interrupt configuration */
	NVIC_SetPriority(DMA2_Stream1_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 0, 0));
	NVIC_EnableIRQ(DMA2_Stream1_IRQn);
}

/**
  * @brief GPIO Initialization Function
  * @param None
  * @retval None
  */
static void MX_GPIO_Init(void)
{
	LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 };

	/* GPIO Ports Clock Enable */
	LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOC);
	LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOA);
	LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOB);
	LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOD);
	LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOE);

	/**/
	LL_GPIO_ResetOutputPin(LEDGreenPort, LEDGreenPin);

	/**/
	LL_GPIO_ResetOutputPin(LEDYellowPort, LEDYellowPin);
	//LL_GPIO_ResetOutputPin(LEDRedPort, LEDRedPin);

	/**/
	GPIO_InitStruct.Pin = PushButton_Pin;
	GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT;
	GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
	LL_GPIO_Init(PushButton_GPIO_Port, &GPIO_InitStruct);

	/**/
	GPIO_InitStruct.Pin = uSD_Detect_Pin;
	GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT;
	GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
	LL_GPIO_Init(uSD_Detect_GPIO_Port, &GPIO_InitStruct);

	/**/
	GPIO_InitStruct.Pin = LEDGreenPin;
	GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
	GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW;
	GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
	GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
	LL_GPIO_Init(LEDGreenPort, &GPIO_InitStruct);

	/**/
	GPIO_InitStruct.Pin = LEDYellowPin;
	GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
	GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW;
	GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
	GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
	LL_GPIO_Init(LEDYellowPort, &GPIO_InitStruct);
//
//	GPIO_InitStruct.Pin = LEDRedPin;
//	GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
//	GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW;
//	GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
//	GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
//	LL_GPIO_Init(LEDRedPort, &GPIO_InitStruct);
}

void BSP::ledToggle(BSP::LED color)
{
	switch (color)
	{
	case BSP::green:
		LL_GPIO_TogglePin(LEDGreenPort, LEDGreenPin);  
		break;
	case BSP::yellow:
		LL_GPIO_TogglePin(LEDYellowPort, LEDYellowPin);  
		break;
	case BSP::red:
		LL_GPIO_TogglePin(LEDRedPort, LEDRedPin);  
		break;
	}
}
/**
  * @brief System Clock Configuration
  * @retval None
  */
void SysClockConfig(void)
{
	LL_FLASH_SetLatency(LL_FLASH_LATENCY_2);
	while (LL_FLASH_GetLatency() != LL_FLASH_LATENCY_2)
	{
	}
	LL_PWR_ConfigSupply(LL_PWR_DIRECT_SMPS_SUPPLY);
	LL_PWR_SetRegulVoltageScaling(LL_PWR_REGU_VOLTAGE_SCALE3);
	LL_RCC_HSI_Enable();

	/* Wait till HSI is ready */
	while (LL_RCC_HSI_IsReady() != 1)
	{

	}
	LL_RCC_HSI_SetCalibTrimming(32);
	LL_RCC_HSI_SetDivider(LL_RCC_HSI_DIV1);
	LL_RCC_LSI_Enable();

	/* Wait till LSI is ready */
	while (LL_RCC_LSI_IsReady() != 1)
	{

	}
	LL_PWR_EnableBkUpAccess();
	LL_RCC_PLL_SetSource(LL_RCC_PLLSOURCE_HSI);
	LL_RCC_PLL1Q_Enable();
	LL_RCC_PLL1_SetVCOInputRange(LL_RCC_PLLINPUTRANGE_8_16);
	LL_RCC_PLL1_SetVCOOutputRange(LL_RCC_PLLVCORANGE_WIDE);
	LL_RCC_PLL1_SetM(4);
	LL_RCC_PLL1_SetN(8);
	LL_RCC_PLL1_SetP(2);
	LL_RCC_PLL1_SetQ(1);
	LL_RCC_PLL1_SetR(2);
	LL_RCC_PLL1_Enable();

	/* Wait till PLL is ready */
	while (LL_RCC_PLL1_IsReady() != 1)
	{
	}

	LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_HSI);

	/* Wait till System clock is ready */
	while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_HSI)
	{

	}
	LL_RCC_SetAHBPrescaler(LL_RCC_AHB_DIV_1);
	LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_2);
	LL_RCC_SetAPB2Prescaler(LL_RCC_APB2_DIV_2);
	LL_RCC_SetAPB3Prescaler(LL_RCC_APB3_DIV_2);
	LL_RCC_SetAPB4Prescaler(LL_RCC_APB4_DIV_2);
	LL_SetSystemCoreClock(64000000);

	/* Update the time base */
	if (HAL_InitTick(TICK_INT_PRIORITY) != HAL_OK)
	{
		SysClockErrorHandler();
	}
}

void SysClockErrorHandler(void)
{
	/* USER CODE BEGIN Error_Handler_Debug */
	/* User can add his own implementation to report the HAL error return state */
	__disable_irq();
	while (1)
	{
	}
	/* USER CODE END Error_Handler_Debug */
}