#include <stdint.h>
#include <stdarg.h>
#include "vsrv.h"

static volatile uint8_t *uart;


#ifdef UART_16550
//
// 16550 uart
//
void uart2_init() {
  uart = (uint8_t *)(void *)NS16550A_UART2_CTRL_ADDR;
  uint32_t uart_freq = UART2_CLOCK_FREQ;
  uint32_t baud_rate = UART2_BAUD_RATE;
  uint32_t divisor   = uart_freq / (16 * baud_rate);
  uart[UART_LCR] = UART_LCR_DLAB;
  uart[UART_DLL] = divisor & 0xff;
  uart[UART_DLM] = (divisor >> 8) & 0xff;
  uart[UART_LCR] = UART_LCR_PODD | UART_LCR_8BIT;
}

static int ns16550a_putchar(int ch) {
  while ((uart[UART_LSR] & UART_LSR_RI) == 0)
    ;
  return uart[UART_THR] = ch & 0xff;
}

void uart_send(char c) { ns16550a_putchar(c); }

void print_s(const char *s) {
  while (*s != '\0') {
    /* convert newline to carrige return + newline */
    if (*s == '\n') uart_send('\r');
    uart_send(*s++);
  }
}

void print_c(char c) { uart_send(c); }

void print_i(unsigned long int x) {
  if (x < 0) {
    print_c('-');
    x = -x;
  }
  if (x >= 10) print_i(x / 10);
  print_c(x % 10 + '0');
}

void print_h(unsigned long int d) {
  unsigned int n;
  int c;
  for (c = 28; c >= 0; c -= 4) {
    // get highest tetrad
    n = (d >> c) & 0xF;
    // 0-9 => '0'-'9', 10-15 => 'A'-'F'
    n += n > 9 ? 0x37 : 0x30;
    uart_send(n);
  }
}

int uart_putc(char ch) {
  // wait until UART transmit FIFO is empty
  while ((uart[UART_LSR] & UART_LSR_RI) == 0)
    ;
  uart[UART_THR] = ch & 0xff;
  return 0;
}



#else


//
// USE ULITE (uart lite) uart
//
void ulite_init() {
  uart = (uint8_t *)(void *)ULITE_BASE_ADDR;
  uint32_t uart_freq  = ULITE_CLOCK_FREQ;
  uint32_t baud_rate  = ULITE_BAUD_RATE;
  uint32_t divisor    = (uart_freq / baud_rate) -1;
  uart[ULITE_DIV]     = divisor;
}


  
static int ulite_putchar(int ch) {
  while ( uart[ULITE_ST] & ULITE_ST_TXFULL_B)
    ;
  return uart[ULITE_TX] = ch & 0xff;
}

void uart_send(char c) { ulite_putchar(c); }

void print_s(const char *s) {
  while (*s != '\0') {
    /* convert newline to carrige return + newline */
    if (*s == '\n') uart_send('\r');
    uart_send(*s++);
  }
}

void print_c(char c) { uart_send(c); }

void print_i(unsigned long int x) {
  if (x < 0) {
    print_c('-');
    x = -x;
  }
  if (x >= 10) print_i(x / 10);
  print_c(x % 10 + '0');
}

void print_h(unsigned long int d) {
  unsigned int n;
  int c;
  for (c = 28; c >= 0; c -= 4) {
    // get highest tetrad
    n = (d >> c) & 0xF;
    // 0-9 => '0'-'9', 10-15 => 'A'-'F'
    n += n > 9 ? 0x37 : 0x30;
    uart_send(n);
  }
}

int uart_putc(char ch) {
  // // wait until UART transmit FIFO is empty
  //  // write the character to the UART data register (TX)
  while ( uart[ULITE_ST] & ULITE_ST_TXFULL_B)
    ;
  uart[ULITE_TX] = ch & 0xFF;
  return 0;
}


// Get one character from UART
char uart_getc(void) {
  // wait until data is available
  while (! (uart[ULITE_ST] & ULITE_ST_RXFULL_B) );
  return (char)(ULITE_RX & 0xFF);   // read the character from the data reg
}


#endif


// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// Helper functions for printf

static void print_str(const char *s)
{
    while (*s) {
        if (*s == '\n') uart_putc('\r'); // optional CRLF
        uart_putc(*s++);
    }
}

static void print_dec(int value)
{
    char buf[16];
    int i = 0;

    if (value == 0) {
        uart_putc('0');
        return;
    }

    if (value < 0) {
        uart_putc('-');
        value = -value;
    }

    while (value > 0) {
        buf[i++] = '0' + (value % 10);
        value /= 10;
    }

    while (i--) uart_putc(buf[i]);
}


static void print_hex(uint32_t value)
{
    char hex[] = "0123456789ABCDEF";
    for (int i = 7; i >= 0; i--) {
        uart_putc(hex[(value >> (i * 4)) & 0xF]);
    }
}


// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

void printf(const char *fmt, ...)
{
    va_list args;
    va_start(args, fmt);

    while (*fmt) {
        if (*fmt == '%') {
            fmt++;

            switch (*fmt) {
                case 'd': {
                    int v = va_arg(args, int);
                    print_dec(v);
                    break;
                }
                case 'x': {
                    uint32_t v = va_arg(args, uint32_t);
                    print_hex(v);
                    break;
                }
                case 's': {
                    char *s = va_arg(args, char *);
                    print_str(s);
                    break;
                }
                case 'c': {
                    char c = (char)va_arg(args, int);
                    uart_putc(c);
                    break;
                }
                case '%': {
                    uart_putc('%');
                    break;
                }

	        default:
                    uart_putc('?');
            }
        } else {
            if (*fmt == '\n') uart_putc('\r');
            uart_putc(*fmt);
        }
        fmt++;
    }

    va_end(args);
}


// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
