#include <string.h>
#include <stdint.h>
#include <stdio.h>
#include <stdbool.h>

#include "main.h"

IWDG_HandleTypeDef hiwdg;
UART_HandleTypeDef huart2;

static void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_USART2_UART_Init(void);
static void MX_IWDG_Init(void);

#define SYS_TICK_PRIO   0

class My
{
    public:
        My() {printf("Constructor\r\n");};
        ~My() {printf("Destructor\r\n");};
};

int main(void)
{
    unsigned int i = 1, j = 40;

    SET_BIT(FLASH->ACR, FLASH_ACR_PRFTEN);
    SysTick_Config(SystemCoreClock / 1000U);    // 1kHz
    NVIC_SetPriority(SysTick_IRQn, SYS_TICK_PRIO);

    SET_BIT(RCC->APBENR2, RCC_APBENR2_SYSCFGEN);
    /* Delay after an RCC peripheral clock enabling */
    READ_BIT(RCC->APBENR2, RCC_APBENR2_SYSCFGEN);

    SET_BIT(RCC->APBENR1, RCC_APBENR1_PWREN);
    /* Delay after an RCC peripheral clock enabling */
    READ_BIT(RCC->APBENR1, RCC_APBENR1_PWREN);

    /* Change strobe configuration of GPIO depending on UCPDx dead battery settings */
    MODIFY_REG(SYSCFG->CFGR1, (SYSCFG_CFGR1_UCPD1_STROBE | SYSCFG_CFGR1_UCPD2_STROBE), SYSCFG_CFGR1_UCPD1_STROBE | SYSCFG_CFGR1_UCPD2_STROBE);

    SystemClock_Config();
    MX_GPIO_Init();
    MX_USART2_UART_Init();

    My* my = new My();

    delete(my);

    // MX_IWDG_Init();
    while (1) {
        if (j < 100) {
            j += 10;
        }
        else if (j < 200) {
            j += 20;
        }
        else if (j < 400) {
            j += 40;
        }
        if (j > 800) {
            j = 800;
        }
        printf("%u: Hello World\r\n", i++);
        HAL_GPIO_TogglePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin);
        HAL_Delay(j);
        // HAL_IWDG_Refresh(&hiwdg);
    }
}

void SystemClock_Config(void)
{
    /* Modify voltage scaling range */
    MODIFY_REG(PWR->CR1, PWR_CR1_VOS, PWR_REGULATOR_VOLTAGE_SCALE1);
    /* Wait until VOSF is reset */
    while(HAL_IS_BIT_SET(PWR->SR2, PWR_SR2_VOSF));

    /* HSI clock config */
    MODIFY_REG(RCC->ICSCR, RCC_ICSCR_HSITRIM, RCC_HSICALIBRATION_DEFAULT << RCC_ICSCR_HSITRIM_Pos);
    /* Adjust the HSI16 division factor */
    MODIFY_REG(RCC->CR, RCC_CR_HSIDIV, RCC_HSI_DIV1);
    /* Update the SystemCoreClock global variable with HSISYS value  */
    SystemCoreClock = (HSI_VALUE / (1UL << ((READ_BIT(RCC->CR, RCC_CR_HSIDIV)) >> RCC_CR_HSIDIV_Pos)));

    /* Adapt Systick interrupt period */
    SysTick_Config(SystemCoreClock / 1000U);    // 1kHz
    NVIC_SetPriority(SysTick_IRQn, SYS_TICK_PRIO);


    /* LSI config */
    /* Disable the Internal Low Speed oscillator (LSI). */
    CLEAR_BIT(RCC->CSR, RCC_CSR_LSION);
    /* Wait till LSI is disabled */
    while (READ_BIT(RCC->CSR, RCC_CSR_LSIRDY) != 0U);

    /* PLL config */
    /* Disable the main PLL. */
    CLEAR_BIT(RCC->CR, RCC_CR_PLLON);
    /* Wait till PLL is ready */
    while (READ_BIT(RCC->CR, RCC_CR_PLLRDY) != 0U);
    /* Configure the main PLL clock source, multiplication and division factors. */
    MODIFY_REG(RCC->PLLCFGR, (RCC_PLLCFGR_PLLSRC | RCC_PLLCFGR_PLLM | RCC_PLLCFGR_PLLN | RCC_PLLCFGR_PLLP | RCC_PLLCFGR_PLLQ | RCC_PLLCFGR_PLLR),
               (RCC_PLLSOURCE_HSI | RCC_PLLM_DIV1 | (8 << RCC_PLLCFGR_PLLN_Pos) | RCC_PLLP_DIV2 | RCC_PLLQ_DIV2 | RCC_PLLR_DIV2));
    /* Enable the main PLL. */
    SET_BIT(RCC->CR, RCC_CR_PLLON);
    /* Enable PLLR Clock output. */
    SET_BIT(RCC->PLLCFGR, RCC_PLLRCLK);
    /* Wait till PLL is ready */
    while (READ_BIT(RCC->CR, RCC_CR_PLLRDY) == 0U);

    /* Program the new number of wait states to the LATENCY bits in the FLASH_ACR register */
    MODIFY_REG(FLASH->ACR, FLASH_ACR_LATENCY, FLASH_LATENCY_2);
    /* Check that the new number of wait states is taken into account to access the Flash
       memory by polling the FLASH_ACR register */
    while ((FLASH->ACR & FLASH_ACR_LATENCY) != FLASH_LATENCY_2);

    /* HCLK config */
    /* Set the highest APB divider in order to ensure that we do not go through
       a non-spec phase whatever we decrease or increase HCLK. */
    MODIFY_REG(RCC->CFGR, RCC_CFGR_PPRE, RCC_HCLK_DIV16);
    /* Set the new HCLK clock divider */
    MODIFY_REG(RCC->CFGR, RCC_CFGR_HPRE, RCC_SYSCLK_DIV1);

    /* SYSCLK config */
    MODIFY_REG(RCC->CFGR, RCC_CFGR_SW, RCC_SYSCLKSOURCE_PLLCLK);
    while ((RCC->CFGR & RCC_CFGR_SWS) != (RCC_SYSCLKSOURCE_PLLCLK << RCC_CFGR_SWS_Pos));

    /* PCLK1 Configuration */
    MODIFY_REG(RCC->CFGR, RCC_CFGR_PPRE, RCC_HCLK_DIV1);

    /* TODO: Update the SystemCoreClock global variable */
    SystemCoreClock = 64000000;

    /* Configure the USART2 clock source */
    MODIFY_REG(RCC->CCIPR, RCC_CCIPR_USART2SEL, RCC_USART2CLKSOURCE_PCLK1);
}

static void MX_IWDG_Init(void)
{
    hiwdg.Instance = IWDG;
    hiwdg.Init.Prescaler = IWDG_PRESCALER_4;
    hiwdg.Init.Window = 4095;
    hiwdg.Init.Reload = 4095;
    if (HAL_IWDG_Init(&hiwdg) != HAL_OK) {
        Error_Handler();
    }
}

static void MX_USART2_UART_Init(void)
{
    huart2.Instance = USART2;
    huart2.Init.BaudRate = 115200;
    huart2.Init.WordLength = UART_WORDLENGTH_8B;
    huart2.Init.StopBits = UART_STOPBITS_1;
    huart2.Init.Parity = UART_PARITY_NONE;
    huart2.Init.Mode = UART_MODE_TX_RX;
    huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
    huart2.Init.OverSampling = UART_OVERSAMPLING_16;
    huart2.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
    huart2.Init.ClockPrescaler = UART_PRESCALER_DIV1;
    huart2.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
    if (HAL_UART_Init(&huart2) != HAL_OK) {
        Error_Handler();
    }
}

static void MX_GPIO_Init(void)
{
    /* Enable  PORT C clock*/
    SET_BIT(RCC->IOPENR, RCC_IOPENR_GPIOCEN);
    /* Delay after an RCC peripheral clock enabling */
    READ_BIT(RCC->IOPENR, RCC_IOPENR_GPIOCEN);

    /* Enable  PORT F clock*/
    SET_BIT(RCC->IOPENR, RCC_IOPENR_GPIOFEN);
    /* Delay after an RCC peripheral clock enabling */
    READ_BIT(RCC->IOPENR, RCC_IOPENR_GPIOFEN);

    /* Enable  PORT A clock*/
    SET_BIT(RCC->IOPENR, RCC_IOPENR_GPIOAEN);
    /* Delay after an RCC peripheral clock enabling */
    READ_BIT(RCC->IOPENR, RCC_IOPENR_GPIOAEN);

    GPIOA->BSRR = 1 << 5;   // PORTA PIN5 (green LED)

    /* Configure the IO Speed */
    uint32_t temp = GPIOA->OSPEEDR;
    temp &= ~(GPIO_OSPEEDR_OSPEED0 << (5 * 2u));
    temp |= (GPIO_SPEED_FREQ_HIGH << (5 * 2u));
    GPIOA->OSPEEDR = temp;

    /* Configure the IO Output Type */
    temp = GPIOA->OTYPER;
    temp &= ~(GPIO_OTYPER_OT0 << 5);
    temp |= (((GPIO_MODE_OUTPUT_PP & 0x00000010u) >> 4u) << 5);
    GPIOA->OTYPER = temp;

    /* Activate the Pull-up or Pull down resistor for the current IO */
    temp = GPIOA->PUPDR;
    temp &= ~(GPIO_PUPDR_PUPD0 << (5 * 2u));
    temp |= ((GPIO_NOPULL) << (5 * 2u));
    GPIOA->PUPDR = temp;

    /* Configure IO Direction mode (Input, Output, Alternate or Analog) */
    temp = GPIOA->MODER;
    temp &= ~(GPIO_MODER_MODE0 << (5 * 2u));
    temp |= ((GPIO_MODE_OUTPUT_PP & 0x00000003u) << (5 * 2u));
    GPIOA->MODER = temp;
}

void Error_Handler(void)
{
    __disable_irq();
    while (1) {
    }
}

#ifdef  USE_FULL_ASSERT
void assert_failed(uint8_t *file, uint32_t line)
{
    /* User can add his own implementation to report the file name and line number,
       ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
}
#endif /* USE_FULL_ASSERT */