#include "string.h"
#include "mem.h"
#include "dma.h"
#include "arch.h"
#include "clock.h"
#include "delay.h"
#include "gpio.h"
#include "spin.h"
#include "usart.h"

volatile uint32_t delay_amt = 20000000 / 4;

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

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

#ifdef ARCH_STM32L4

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

  setup_usart2(115200);


  char* into = (char*) (SRAM1_BASE + 50);
  const char* hello = "Hello, Chester Cheeto!\r\n";
  int len = strlen(hello);
  memcpy(into, hello, len + 1);

  usart_enable_dma(&USART2, USART_ENABLE_TX);
  usart_set_enabled(&USART2, USART_ENABLE_TX | USART_ENABLE_RX);

  pin_on(pin3);

  RCC.ahb1en_bf.dma1en = 1; /* Enable DMA1 Clock. */
  DMA1.csel_bf.c7s = 0x2; /* Select USART2_TX for the sel. */
  DMA1.channel_config[6].cc_r = 0;
  DMA1.channel_config[6].cc_bf.dir = READ_FROM_MEMORY;
  DMA1.channel_config[6].cc_bf.minc = 1;
  DMA1.channel_config[6].cc_bf.pl = DMA_PRIORITY_LEVEL_VERY_HIGH;
  DMA1.channel_config[6].cndt_bf.ndt = len;
  DMA1.channel_config[6].cpa_r = ptr2reg(&USART2.td_r);
  DMA1.channel_config[6].cma_r = ptr2reg(into);

  USART2.ic_bf.tccf = 1;
  DMA1.channel_config[6].cc_bf.en = 1;

  pin_on(pin3);
  // usart_transmit_str_sync(&USART2, into);
  for(;;);
}

#endif