#include "bsp.hpp"
#include <stm32h7xx_hal.h>
#include <cassert>
#include "_bsp_private.hpp"
#include <STM32ClockControl.hpp>

static uint32_t RtcClockSource = RCC_RTCCLKSOURCE_LSI;

void bsp_private::initializeSystemClocks(void)
{
	RCC_OscInitTypeDef RCC_OscInitStruct{
		.OscillatorType =
			(RCC_OSCILLATORTYPE_HSI48 | RCC_OSCILLATORTYPE_HSI | RCC_OSCILLATORTYPE_LSI),
		.HSEState = 0,
		.LSEState = 0,
		.HSIState = RCC_HSI_DIV1,
		.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT,
		.LSIState = RCC_LSI_ON,
		.HSI48State = RCC_HSI48_ON,
		.CSIState = 0,
		.CSICalibrationValue = 0,
		.PLL = {.PLLState = RCC_PLL_ON,
				.PLLSource = RCC_PLLSOURCE_HSI,
				.PLLM = 32,
				.PLLN = 129,
				.PLLP = 2,
				.PLLQ = 2,
				.PLLR = 2,
				.PLLRGE = RCC_PLL1VCIRANGE_1,
				.PLLVCOSEL = RCC_PLL1VCOWIDE,
				.PLLFRACN = 0},
	};

	RCC_ClkInitTypeDef RCC_ClkInitStruct{
		.ClockType = (RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 |
					  RCC_CLOCKTYPE_PCLK2 | RCC_CLOCKTYPE_D3PCLK1 | RCC_CLOCKTYPE_D1PCLK1),
		.SYSCLKSource = RCC_SYSCLKSOURCE_HSI,
		.SYSCLKDivider = RCC_SYSCLK_DIV1,
		.AHBCLKDivider = RCC_HCLK_DIV1,
		.APB3CLKDivider = RCC_APB3_DIV2,
		.APB1CLKDivider = RCC_APB1_DIV2,
		.APB2CLKDivider = RCC_APB2_DIV2,
		.APB4CLKDivider = RCC_APB4_DIV2,
	};

	// Supply configuration update enable
	HAL_PWREx_ConfigSupply(PWR_DIRECT_SMPS_SUPPLY);
	// Configure the main internal regulator output voltage.
	__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE3);

	while(!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY))
	{
	}

	// Init RCC Oscillators
	int ret = HAL_RCC_OscConfig(&RCC_OscInitStruct);
	assert(ret == HAL_OK);

	// Initializes the CPU, AHB and APB buses clocks
	ret = HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2);
	assert(ret == HAL_OK);
}

static void MX_GPIO_Init()
{
	/* GPIO Ports Clock Enable */
	__HAL_RCC_GPIOA_CLK_ENABLE();
	__HAL_RCC_GPIOB_CLK_ENABLE();
	__HAL_RCC_GPIOC_CLK_ENABLE();
	__HAL_RCC_GPIOD_CLK_ENABLE();
	__HAL_RCC_GPIOE_CLK_ENABLE();
	__HAL_RCC_GPIOG_CLK_ENABLE(); // LEDs are currently here
}

/* USER CODE BEGIN 4 */
/**
  * @brief  Configure the MPU attributes.
  * @note   The Base Address 0x24000000 is the SRAM1 accessible by the SDIO internal DMA.
			the configured region is 512Kb size.
  * @param  None
  * @retval None
  */
static void MPU_Config()
{
	MPU_Region_InitTypeDef MPU_InitStruct;

	/* Disable the MPU */
	HAL_MPU_Disable();

	/* Configure the MPU attributes as Normal Non Cacheable for SRAM1 */

	MPU_InitStruct.Enable = MPU_REGION_ENABLE;
	MPU_InitStruct.BaseAddress = 0x24000000;
	MPU_InitStruct.Size = MPU_REGION_SIZE_512KB;
	MPU_InitStruct.AccessPermission = MPU_REGION_FULL_ACCESS;
	MPU_InitStruct.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE;
	MPU_InitStruct.IsCacheable = MPU_ACCESS_NOT_CACHEABLE;
	MPU_InitStruct.IsShareable = MPU_ACCESS_NOT_SHAREABLE;
	MPU_InitStruct.Number = MPU_REGION_NUMBER1;
	MPU_InitStruct.TypeExtField = MPU_TEX_LEVEL1;
	MPU_InitStruct.SubRegionDisable = 0x00;
	MPU_InitStruct.DisableExec = MPU_INSTRUCTION_ACCESS_ENABLE;

	HAL_MPU_ConfigRegion(&MPU_InitStruct);

	/* Enable the MPU */
	HAL_MPU_Enable(MPU_PRIVILEGED_DEFAULT);
}

void initializeProcessor()
{
	MPU_Config();
	HAL_Init(); // Question: Is the goal to get rid of this eventually?
	bsp_private::initializeSystemClocks();
	bsp_private::initializeRTC();
	MX_GPIO_Init();
	STM32ClockControl::dmaEnable(0); // Enables DMA1 clock
}

/**
 * @brief RTC MSP Initialization
 *        This function configures the hardware resources used in this example
 * @param hrtc: RTC handle pointer
 *
 * @note  Care must be taken when HAL_RCCEx_PeriphCLKConfig() is used to select
 *        the RTC clock source; in this case the Backup domain will be reset in
 *        order to modify the RTC Clock source, as consequence RTC registers (including
 *        the backup registers) and RCC_BDCR register are set to their reset values.
 *
 * @retval None
 */
void HAL_RTC_MspInit(RTC_HandleTypeDef* hrtc)
{
	(void)hrtc;
	RCC_OscInitTypeDef RCC_OscInitStruct{};
	RCC_PeriphCLKInitTypeDef PeriphClkInitStruct{};

	/*##-1- Enables the PWR Clock and Enables access to the backup domain ######*/
	/* To enable access on RTC registers */
	HAL_PWR_EnableBkUpAccess();

	/*##-2- Get RTC clock configuration
	 * #########################################*/
	HAL_RCCEx_GetPeriphCLKConfig(&PeriphClkInitStruct);

	/*In case of RTC clock already enable, make sur it's the good one */
	if(PeriphClkInitStruct.RTCClockSelection == RtcClockSource)
	{
		/* Do nothing */
	}
	else
	{
		/* Reset Backup Domain to reset RTC clock source and LSIRDY bit */
		__HAL_RCC_BACKUPRESET_FORCE();
		__HAL_RCC_BACKUPRESET_RELEASE();
		RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_LSI | RCC_OSCILLATORTYPE_LSE;
		RCC_OscInitStruct.PLL.PLLState = RCC_PLL_NONE;
		RCC_OscInitStruct.LSEState = RCC_LSE_ON;
		RCC_OscInitStruct.LSIState = RCC_LSI_OFF;

		auto status = HAL_RCC_OscConfig(&RCC_OscInitStruct);
		assert(status == HAL_OK);

		PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_RTC;
		PeriphClkInitStruct.RTCClockSelection = RtcClockSource;
		status = HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct);
		assert(status == HAL_OK);
	}

	/*##-3- Enable RTC peripheral Clocks #######################################*/
	/* Enable RTC Clock */
	__HAL_RCC_RTC_ENABLE();

	/*##-4- Configure the NVIC for RTC Alarm and Wakeup ########################*/
    HAL_NVIC_SetPriority(RTC_WKUP_IRQn, 0, 0);
    HAL_NVIC_EnableIRQ(RTC_WKUP_IRQn);
    HAL_NVIC_SetPriority(RTC_Alarm_IRQn, 0, 0);
    HAL_NVIC_EnableIRQ(RTC_Alarm_IRQn);
}

/**
 * @brief RTC MSP De-Initialization
 *        This function frees the hardware resources used in this example:
 *          - Disable the Peripheral's clock
 * @param hrtc: RTC handle pointer
 * @retval None
 */
void HAL_RTC_MspDeInit(RTC_HandleTypeDef* hrtc)
{
	(void)hrtc;
	/*##-1- Reset peripherals ##################################################*/
	__HAL_RCC_RTC_DISABLE();
	HAL_NVIC_DisableIRQ(RTC_WKUP_IRQn);
  HAL_NVIC_DisableIRQ(RTC_Alarm_IRQn);
}
