#include <uart.h>
#include <kmalloc.h>
#include <interrupt.h>
#include <logger.h>
#include <errcode.h>
#include <utils.h>
#include <string.h>
#include <gpio.h>
#include <stddef.h>
#include <ringbuffer.h>

/* Auxilary mini UART registers */
#define AUX_ENABLE      ((volatile uint32_t*)(MMIO_BASE + 0x00215004))
#define AUX_MU_IO       ((volatile uint32_t*)(MMIO_BASE + 0x00215040))
#define AUX_MU_IER      ((volatile uint32_t*)(MMIO_BASE + 0x00215044))
#define AUX_MU_IIR      ((volatile uint32_t*)(MMIO_BASE + 0x00215048))
#define AUX_MU_LCR      ((volatile uint32_t*)(MMIO_BASE + 0x0021504C))
#define AUX_MU_MCR      ((volatile uint32_t*)(MMIO_BASE + 0x00215050))
#define AUX_MU_LSR      ((volatile uint32_t*)(MMIO_BASE + 0x00215054))
#define AUX_MU_MSR      ((volatile uint32_t*)(MMIO_BASE + 0x00215058))
#define AUX_MU_SCRATCH  ((volatile uint32_t*)(MMIO_BASE + 0x0021505C))
#define AUX_MU_CNTL     ((volatile uint32_t*)(MMIO_BASE + 0x00215060))
#define AUX_MU_STAT     ((volatile uint32_t*)(MMIO_BASE + 0x00215064))
#define AUX_MU_BAUD     ((volatile uint32_t*)(MMIO_BASE + 0x00215068))

#define ARMINT_En_IRQs1 ((volatile uint32_t*)(MMIO_BASE + 0x0000b210))

size_t (*uart_putb)(const uint8_t *bytes, size_t len) = uart_putb_sync;
size_t (*uart_getb)(uint8_t *bytes, size_t len)       = uart_getb_sync;
int is_uart_inited = false;

int is_uart_in_interrupt_queue = false;

ringbuffer_t *uart_readbuf;
ringbuffer_t *uart_writebuf;

MMIO_W_HELPER(_uart_enable_receive_interrupt,  AUX_MU_IER, 0x1, 0);
MMIO_W_HELPER(_uart_enable_transmit_interrupt, AUX_MU_IER, 0x1, 1);
MMIO_W_HELPER(_uart_clear_receive_fifo,  AUX_MU_IIR, 0x1, 1);
MMIO_W_HELPER(_uart_clear_transmit_fifo, AUX_MU_IIR, 0x1, 2);
MMIO_W_HELPER(_uart_write_data, AUX_MU_IO, 0xff, 0);
MMIO_W_HELPER(_uart_interrupt_controller, ARMINT_En_IRQs1, 0x1, 29);

MMIO_R_HELPER(_uart_interrupt_pending,  AUX_MU_IIR, 0x1, 0);
MMIO_R_HELPER(_uart_transmit_interrupt, AUX_MU_IIR, 0x1, 1);
MMIO_R_HELPER(_uart_receive_interrupt,  AUX_MU_IIR, 0x1, 2);
MMIO_R_HELPER(_uart_transmitter_idle, AUX_MU_LSR, 0x1, 5);
MMIO_R_HELPER(_uart_data_ready,       AUX_MU_LSR, 0x1, 0);
MMIO_R_HELPER(_uart_read_data, AUX_MU_IO, 0xff, 0);

void uart_init(void)
{
  register uint32_t r;

  /* initialize UART */
  *AUX_ENABLE |= 1;      // enable UART1, AUX mini uart
  *AUX_MU_CNTL = 0;      // disable Tx, Rx
  *AUX_MU_LCR  = 3;      // set 8 bits
  *AUX_MU_MCR  = 0;
  *AUX_MU_IER  = 0;      // disable interrupt by default
  *AUX_MU_IIR  = 0x6;    // clear tx, rx FIFO
  *AUX_MU_BAUD = 270;    // 115200 baud
  /* map UART1 to GPIO pins */
  r = *GPFSEL1;
  r &= ~((7 << 12) | (7 << 15)); // gpio14, gpio15
  r |= (2 << 12) | (2 << 15);    // alt5
  *GPFSEL1 = r;
  *GPPUD = 0;            // enable pins 14 and 15
  r = 150; while (r--) { asm volatile("nop"); }
  *GPPUDCLK0 = (1 << 14) | (1 << 15);
  r = 150; while (r--) { asm volatile("nop"); }
  *GPPUDCLK0 = 0;        // flush GPIO setup
  *AUX_MU_CNTL = 3;      // enable Tx, Rx
  _uart_interrupt_controller(true);

  uart_readbuf = make_ringbuffer(UART_BUFLEN);
  uart_writebuf = make_ringbuffer(UART_BUFLEN);

  _uart_enable_receive_interrupt(true);
  //uint64_t ier = *AUX_MU_IER;

  uart_getb = uart_getb_async;
  uart_putb = uart_putb_async;

  is_uart_inited = true;
}

static inline
void _enable_uart_interrupt()
{
  _uart_enable_receive_interrupt(true);
  if (uart_writebuf->size > 0)
    _uart_enable_transmit_interrupt(true);
}

static inline
void _uart_transmit_interrupt_callback(uint64_t)
{
  _uart_write_data(ringbuffer_bump(uart_writebuf));

  is_uart_in_interrupt_queue = false;
  _enable_uart_interrupt();
}

static inline
void _uart_transmit_interrupt_handler()
{
  if (uart_writebuf->size > 0) {
    is_uart_in_interrupt_queue = true;
    add_interrupt_task(11, _uart_transmit_interrupt_callback, 0x0);
  }
}

static inline
void _uart_receive_interrupt_callback(uint64_t)
{
  ringbuffer_push(uart_readbuf, _uart_read_data());

  is_uart_in_interrupt_queue = false;
  _enable_uart_interrupt();
}

static inline
void _uart_receive_interrupt_handler()
{
  if (uart_readbuf->size < uart_readbuf->cap) {
    is_uart_in_interrupt_queue = true;
    add_interrupt_task(12, _uart_receive_interrupt_callback, 0x0);
  }
}

void uart_irq_handler(void)
{
  _uart_enable_receive_interrupt(false);
  _uart_enable_transmit_interrupt(false);

  if (_uart_receive_interrupt())
    _uart_receive_interrupt_handler();
  if (_uart_transmit_interrupt())
    _uart_transmit_interrupt_handler();
}

size_t uart_putb_async(const uint8_t *bytes, size_t len)
{
  size_t sentlen = 0;
  for (; sentlen < len; ++bytes, ++sentlen)
    ringbuffer_push(uart_writebuf, *bytes);
  
  if (!is_uart_in_interrupt_queue && uart_writebuf)
    _enable_uart_interrupt();

  return sentlen;
}

size_t uart_putb_sync(const uint8_t *bytes, size_t len)
{
  size_t sentlen = 0;
  for (; sentlen < len; ++bytes, ++sentlen) {
    do asm volatile("nop"); // busy waiting
    while (!_uart_transmitter_idle());

    _uart_write_data(*bytes);
  }

  return sentlen;
}

size_t uart_getb_async(uint8_t *bytes, size_t len)
{
  size_t recvlen = 0;
  for (; recvlen < len; ++bytes, ++recvlen) {
    while (!uart_readbuf->size)
      wfe(); // wait for interrupt
  
    *bytes = ringbuffer_bump(uart_readbuf);
  }

  return recvlen;
}

size_t uart_getb_sync(uint8_t *bytes, size_t len)
{
  size_t recvlen = 0;
  for (; recvlen < len; ++bytes, ++recvlen) {
    do asm volatile("nop");
    while (!_uart_data_ready());

    *bytes = _uart_read_data();
  }

  return recvlen;
}

char uart_getc(void)
{
  char r;
  uart_getb((uint8_t *)&r, 1);
  return r == '\r' ? '\n' : r;
}

void uart_puts(const char *s)
{
  size_t len = strlen(s);
  size_t ret = uart_putb((const uint8_t *)s, len);
  if (ret != len)
    exit(ERR_IO_FAILED);
}

void uart_hex(uint64_t d)
{
  char buf[0x11], *p = buf;
  for (int c = 28, n; c >= 0; c -= 4) {
    n = (d >> c) & 0xf;
    n += (n > 9) ? 0x37 : 0x30;
    *p++ = (char)n;
  }
  *p = '\0';
  uart_puts(buf);
}