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

#include "main.h"

#include "platform/stm32g0xx/Gpio.h"
#include "platform/stm32g0xx/Uart.h"
#include "platform/stm32g0xx/IndependentWatchdog.h"

static void SystemClock_Config(void);

#define SYS_TICK_PRIO   0

// NOTE! The independent watchdog is clocked by a separate clock. this one
//       isn't controlled by JTAG. That's why the independent watchdog needs to
//       be disabled during JTAG debug sessions.
#define JTAG_DEBUG

using namespace perinet::platform::stm32g0xx;

Gpio green_led(Gpio::Port::PORT_A, 5, Gpio::Mode::MODE_OUTPUT_PP);
Uart uart(Uart::UartDevice::UART_2, 115200);

#ifndef JTAG_DEBUG
IndependentWatchdog watchdog(4095, 4095);
#endif

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();

    char tx_buf[80];

#ifndef JTAG_DEBUG
    watchdog.enable();
#endif

    while (1) {
        if (j < 100) {
            j += 10;
        }
        else if (j < 200) {
            j += 20;
        }
        else if (j < 400) {
            j += 40;
        }
        if (j > 800) {
            j = 800;
        }
        sprintf(tx_buf, "%u: Hello World\r\n", i++);
        uart.sync_send((const uint8_t *)tx_buf, strlen(tx_buf));
        green_led.toggle();
        HAL_Delay(j);

#ifndef JTAG_DEBUG
        watchdog.trigger();
#endif
    }
}

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);
}

#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 */