#include "kern/log.h"

#include "arch/stm32l4xxx/peripherals/clock.h"
#include "arch/stm32l4xxx/peripherals/usart.h"
#include "kern/common.h"
#include "kern/gpio/gpio_manager.h"
#include "kern/init.h"
#include "kern/delay.h"

void setup_usart2(uint32_t baud_rate);

/** This module requires an initialization routine. This is a level2 routine,
 * so anything running at level3 or lower is guaranteed to have access
 * to the klong. */
init2()
{
  setup_usart2(115200);
  regset(USART2.c_r1, usart_txeie, 1);
  regset(USART2.c_r1, usart_rxneie, 1);
  usart_set_enabled(&USART2, USART_ENABLE_TX | USART_ENABLE_RX);
}

void klogf(const char* fmt, ...)
{
  va_list l;
  va_start(l, fmt);

  kvlogf(fmt, l);
}

void kvlogf(const char* fmt, va_list l)
{
  usart_vprintf(&USART2, fmt, l);
}

void kerr_vlogf(const char* fmt, va_list l)
{
  klogf("\x1b[01;31m[ERROR] ");
  kvlogf(fmt, l);
  klogf("\x1b[00m");
}

void kerr_logf(const char* fmt, ...)
{
  va_list l;
  va_start(l, fmt);

  kerr_vlogf(fmt, l);
}

void setup_usart2(uint32_t baud_rate)
{
  enable_hsi(&RCC, true);

  int ec = 0;
  gpio_enable_alternate_function(
      GPIO_ALTERNATE_FUNCTION_USART2_TX, GPIO_PIN_PA2, &ec);

  gpio_enable_alternate_function(
      GPIO_ALTERNATE_FUNCTION_USART2_RX, GPIO_PIN_PA15, &ec);

  set_usart2_clock_src(&RCC, USART_CLK_SRC_HSI16);
  set_usart2_clock_enabled(&RCC, USART_CLK_SRC_HSI16);

  /* De-assert reset of USART2 */
  regset(RCC.apb1rst1_r, rcc_usart2rst, 0);

  USART2.c_r1 = 0;
  USART2.c_r2 = 0;
  USART2.c_r3 = 0;

  usart_set_divisor(&USART2, 16000000 / baud_rate);
}