#include "arch.h"
#include "core/clock.h"
#include "core/dma.h"
#include "core/gpio.h"
#include "core/system.h"
#include "core/usart.h"
#include "core/nvic.h"

#include "delay.h"
#include "mem.h"
#include "spin.h"
#include "string.h"

/** Overrides the default systick irq handler. */
void on_systick()
{
  static int is_on = 0;

  __IO gpio_port_t* port_b = enable_gpio(GPIO_PORT_B);
  gpio_output_pin_t pin3 = set_gpio_pin_output(port_b, PIN_3);

  if (is_on) {
    pin_off(pin3);
  } else {
    pin_on(pin3);
  }

  is_on = ! is_on;
}

void setup_usart2(uint32_t baud_rate)
{
  __IO gpio_port_t* port_a = enable_gpio(GPIO_PORT_A);
  enable_hsi(&RCC, true);

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

  set_gpio_pin_mode(port_a, PIN_2, MODE_ALTERNATE);
  set_gpio_pin_mode(port_a, PIN_15, MODE_ALTERNATE);
  set_gpio_alternate_function(port_a, PIN_2, AFN_7);
  set_gpio_alternate_function(port_a, PIN_15, AFN_3);

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

#ifdef ARCH_STM32L4

/* Main function. This gets executed from the interrupt vector defined above. */
int main()
{
  /* Enable a higher clock frequency. */
  set_system_clock_MHz(80);

  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);
  USART2.td_r = (uint8_t) 0x61;

  __IO gpio_port_t* port_b = enable_gpio(GPIO_PORT_B);
  gpio_output_pin_t pin3 = set_gpio_pin_output(port_b, PIN_3);
  pin_on(pin3);

  usart_printf(&USART2, "\nUSART2.c_r1: %p\n", USART2.c_r1);
  usart_printf(&USART2, "NVIC intlinesnum: %d\n",
      regget(NVIC.ict_r, nvic_intlinesnum));

  int off = 1;
  int last;
  for(;;) {
    int next = USART2.is_r & usart_rxne;
    volatile int y = USART2.rd_r;
    if (next)
      USART2.td_r = y;
    if (last != next) {
      if (off) {
        pin_on(pin3);
      } else {
        pin_off(pin3);
      }
      off = !off;
    }
  }

  // usart_printf(&USART2, "Start Configuring Countdown!\n");

  /* Set the countdown to start from 1,000,0000. */
  // SCB.strv_r = 10000000;

  /* Enable interrupts. */
  // regset(SCB.stcs_r, scb_tickint, 1);

  /* Start the systick. */
  // regset(SCB.stcs_r, scb_enable, 1);

  // usart_printf(&USART2, "Start Countdown Started!\n");
}

#endif