#include <libopencm3/cm3/nvic.h>
#include <libopencm3/cm3/systick.h>
#include <libopencm3/cm3/scb.h>
#include <libopencm3/stm32/rcc.h>
#include <stddef.h>
#include <stdio.h>
#include <string.h>

#include "adc.h"
#include "buttons.h"
#include "usb.h"
#include "ringbuffer.h"
#include "uart.h"
#include "encoder.h"
#include "systick.h"

static const uint32_t button_time = 100;

volatile uint32_t tick = 0;
RINGBUFFER_STORAGE(usb_to_uart_buf, 64)
RINGBUFFER_STORAGE(uart_to_usb_buf, 64)
RINGBUFFER_STORAGE(comm_in_buf, 64)
RINGBUFFER_STORAGE(comm_out_buf, 64)

static void sys_tick_setup(void);

#if defined(BOOTLOADER)
extern uint32_t *_board_dfu_dbl_tap;

static void start_bootloader(void);
static void erase_app(void);
#endif

static void fast_reset(void);
static void set_watchdog(int w);
static void set_print_changes(int p);

static void sys_tick_setup() {
  systick_set_reload(rcc_ahb_frequency / SYSTICK_FREQUENCY - 1);
  systick_set_clocksource(STK_CSR_CLKSOURCE_AHB);
  systick_counter_enable();
  systick_interrupt_enable();
}

void sys_tick_handler() {
  tick++;
}

void fast_reset() {
#if defined(BOOTLOADER)
  *_board_dfu_dbl_tap = 0xf02669ef;
#endif
  scb_reset_system();
}

#if defined(BOOTLOADER)
void start_bootloader() {
  *_board_dfu_dbl_tap = 0xf01669ef;
  scb_reset_system();
}
void erase_app() {
  *_board_dfu_dbl_tap = 0xf5e80ab4;
  scb_reset_system();
}
#endif

int watchdog = 0;
int print_changes = 0;

void set_watchdog(int w) {
  watchdog = w;
  printf("watchdog: %d", watchdog);
}

void set_print_changes(int p) {
  print_changes = p;
  printf("pos reporting: %d", print_changes);
}

int main(void) {
#if 0
  rcc_clock_setup_pll(&rcc_hse_25mhz_3v3[RCC_CLOCK_3V3_84MHZ]);
#else
  rcc_clock_setup_pll(&rcc_hse_12mhz_3v3[RCC_CLOCK_3V3_84MHZ]);
#endif

  RINGBUFFER_INIT(uart_to_usb_buf, 64);
  RINGBUFFER_INIT(usb_to_uart_buf, 64);
  RINGBUFFER_INIT(comm_in_buf, 64);
  RINGBUFFER_INIT(comm_out_buf, 64);

  sys_tick_setup();

  adc_setup();
  uart_setup();
  usb_setup();

  buttons_setup();
  encoder_setup();

  nvic_enable_irq(NVIC_USART3_IRQ);

  uint32_t last_tick = tick;
  int last_pos = 0;
  uint32_t watchdog_counter = 0;

  while (tick < 5000 && !usb_connected()) {
    usbd_poll(g_usbd_dev);
  }

  if (!usb_connected()) {
    fast_reset();
  }

  char cmd_buf[64];
  uint8_t cmd_len = 0;
  bool cmd = false;

  while (1) {
    /* Handle control messages through the comms CDC */
    char buf[64];
    unsigned buf_len = 0;

    const bool no_comms = watchdog && ringbuffer_empty(usb_to_uart_buf) && ringbuffer_empty(comm_in_buf);
    if (!no_comms) {
      watchdog_counter = 0;
    }
    while (tick != last_tick) {
      buttons_tick();
      last_tick++;
      watchdog_counter += no_comms;
    }
    if (watchdog_counter >= s_ticks(35)) {
      fast_reset();
    }

    for (char c; ringbuffer_get(comm_in_buf, (void *)&c, 1);) {
      if (cmd) {
        if (c == '\r' || c == '\n') {
          cmd_buf[cmd_len] = 0;

          if (cmd_buf[0] == 'r' && cmd_len == 2 && (cmd_buf[1] == '0' || cmd_buf[1] == '1')) {
            set_print_changes(cmd_buf[1] == '1');
          } else if(cmd_buf[0] == 'w' && cmd_len == 2 && (cmd_buf[1] == '0' || cmd_buf[1] == '1')) {
            set_watchdog(cmd_buf[1] == '1');
          } else if (!strcmp(cmd_buf, "reset")) {
            fast_reset();
          }
#if defined(BOOTLOADER)
          else if (!strcmp(cmd_buf, "flash")) {
            start_bootloader();
          } else if (!strcmp(cmd_buf, "erase")) {
            erase_app();
          }
#endif
          else {
            printf("unknown command: %s", cmd_buf);
          }
          cmd = false;
        } else if (cmd_len < sizeof cmd_buf - 1) {
          cmd_buf[cmd_len++] = c;
        }
      }
      else switch (c) {
        case '>':
          cmd = true;
          cmd_len = 0;
          break;
        case 'B': // Battery
          printf("%lu", (unsigned long)adc_bat_voltage());
          break;
        case 'O': // Open
          buttons_open(button_time);
          break;
        case 'C': // Close
          buttons_close(button_time);
          break;
        case 'R': // Report
          printf("pos: %d %d", encoder_get(), encoder_current_speed());
          break;
        case 'r': // toggle reporting
          set_print_changes(!print_changes);
          break;
        case 'W': // toggle watchdog
          set_watchdog(!watchdog);
          break;
        case 'S': // reStart
          fast_reset();
          break;
#if defined(BOOTLOADER)
        case 'F': // Flash
          start_bootloader();
          break;
        case 'E': // Erase
          erase_app();
          break;
#endif
      }
    }

    int pos = encoder_get();
    if (pos != last_pos) {
      if (ringbuffer_empty(comm_out_buf)) {
        if (print_changes) {
          printf("pos: %d %d", pos, encoder_speed());
        }
        last_pos = pos;
      }
    }

    /* Send replies */
    if ((buf_len = ringbuffer_peek(comm_out_buf, &buf[0], sizeof buf))) {
      if (!usb_connected() || usb_write_cdcacm(ACM_COMM, (void *)buf, buf_len, 1)) {
        ringbuffer_skip(comm_out_buf, buf_len);
      }
    }

    /* Send any available data to the NFC CDC */
    if (!ringbuffer_empty(uart_to_usb_buf)) {
      unsigned len = ringbuffer_peek(uart_to_usb_buf, &buf[0], sizeof buf);
      if (!usb_connected() || usb_write_cdcacm(ACM_NFC, &buf[0], len, 1)) {
        ringbuffer_skip(uart_to_usb_buf, len);
      }
    }
    if (!ringbuffer_empty(usb_to_uart_buf)) {
      usart_enable_tx_interrupt(USART3);
    }

    usbd_poll(g_usbd_dev);
    // __asm__("wfi");
  }
}

void usart3_isr() {
  while (USART_SR(USART3) & USART_SR_RXNE) {
    uint8_t c = usart_recv(USART3);
    ringbuffer_add(uart_to_usb_buf, (void *)&c, 1);
  }
  while (USART_SR(USART3) & USART_SR_TXE) {
    if (ringbuffer_empty(usb_to_uart_buf)) {
      usart_disable_tx_interrupt(USART3);
      break;
    } else {
      uint8_t c;
      ringbuffer_get(usb_to_uart_buf, &c, 1);
      usart_send(USART3, c);
    }
  }
}