#include "clock.h"
#include "delay.h"
#include "gpio.h"
#include "spin.h"
#include "usart.h"

volatile uint32_t delay_amt = 20000000 / 4;

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

  // Turn on the clock for the USART2 peripheral
  set_usart2_clock_src(&RCC, USART_CLK_SRC_HSI16);
  set_usart2_clock_enabled(&RCC, true);

  // Configure the I/O pins.  Will use PA2 as TX and PA15 as RX so setup for
  // alternate function
  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
  RCC.apb1rst1_r &= ~BIT(17);

  // Configure the USART
  // disable USART first to allow setting of other control bits
  // This also disables parity checking and enables 16 times oversampling

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

  usart_set_divisor(&USART2, 16000000 / baud_rate);
  usart_set_enabled(&USART2, USART_ENABLE_TX | USART_ENABLE_RX);
}

int enable_usart1(uint32_t baud_rate)
{
  /* Enable the GPIO bus. */
  __IO gpio_port_t* port_b = enable_gpio(GPIO_PORT_B);

  /* Enable the USART clock. */
  RCC.apb2en_r |= BIT(14);

  /* == Configure the IO Pins. == */

  /* GPIO D5 (Port B pin 6) is USART1 Tx,
   * GPIO D6 (Port B pin 7) is USART1 Rx. */
  set_gpio_pin_mode(port_b, PIN_6, MODE_ALTERNATE);
  set_gpio_pin_mode(port_b, PIN_7, MODE_ALTERNATE);

  /* Set the GPIO pins to use the USART alternate function. */
  set_gpio_alternate_function(port_b, PIN_6, AFN_7);
  set_gpio_alternate_function(port_b, PIN_7, AFN_7);

  RCC.apb2rst_r &= ~BIT(14); /* De-assert reset of USART1 */

  uint32_t baud_rate_div = 80000000 / baud_rate;
  USART1.c_r1 = 0;
  USART1.c_r2 = 0;
  USART1.c_r3 = 0;
  USART1.br_r = baud_rate_div;

  USART1.c_r1 |= BIT(3) | BIT(2);
  USART1.c_r1 |= BIT(0);

  /* Enable the transmitter and the receiver. */
  usart_set_enabled(&USART1, USART_ENABLE_TX);
  asm volatile(" cpsie i ");
}

/* Main function. This gets executed from the interrupt vector defined above. */
int main()
{
  /* Enable the GPIO port B. */

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

  /* Enable a higher clock frequency. */
  set_system_clock_MHz(80);

  enable_usart2(115200);

  pin_on(pin3);
  usart_transmit_str(&USART2, "Hello, World\n");
  for(;;);
}