#include <avr/io.h>
#include <avr/interrupt.h>
#include "uart0.h"
#include "Robostix.h"
#include "debug.h"

#define FOSC 16000000 // Clock Speed
#define BAUD 38400
#define MYUBRR (FOSC / 16 / BAUD - 1)

#define RECV_BUF_LEN 1024
#define SEND_BUF_LEN 1024

static volatile int recv_buf_head, recv_buf_tail, recv_buf_size;
static volatile char recv_buf[RECV_BUF_LEN];

static volatile int send_buf_head, send_buf_tail, send_buf_size;
static volatile char send_buf[SEND_BUF_LEN];

void init_uart0(void) {
  /* Set clock rate divisor to have 38400 bps */
  UBRR0H = (unsigned char)(MYUBRR >> 8);
  UBRR0L = (unsigned char)MYUBRR;

  /* Set mode of operation to 8N1 */
  UCSR0C = _BV(UCSZ01) | _BV(UCSZ00);

  /* Enable receiver and tranceiver and associated interrupts */
  UCSR0B = _BV(RXCIE0) | _BV(TXEN0) | _BV(RXEN0);

  //initialize things needed for our buffers
  recv_buf_head = 0;
  recv_buf_tail = 0;
  recv_buf_size = 0;
  send_buf_head = 0;
  send_buf_tail = 0;
  send_buf_size = 0;

  /* Uncomment to allow redirection of stdin and stdout through uart0 */
  //  fdevopen(uart0_put, uart0_get);

  //  debug("\n/****************/\n");
  //  debug("      UART0       \n");
  //  debug("/****************/\n");
}

/* Transmit complete on USART0 */
ISR(USART0_TX_vect) {
  //not needed
}

/* Receive complete on USART0 */
ISR(USART0_RX_vect) {
  char tmp =  UDR0;

  //add the received byte to our buffer assuming our buffer isn't full
  if (recv_buf_size < RECV_BUF_LEN) {
    recv_buf[recv_buf_tail] = tmp;
    recv_buf_size++;

    recv_buf_tail++;
    if (recv_buf_tail >= RECV_BUF_LEN) {
      recv_buf_tail = 0;
    }
  }
}

/* Data register is empty on USART0 */
ISR(USART0_UDRE_vect) {

  //if there is no more data to send, disable the firing of UDRE 
  // interrupts until there is more data to send
  if (send_buf_size <= 0) {
    UCSR0B &= ~_BV(UDRIE0);
  } else {
    // put data into UDR0 to be sent

    char temp = send_buf[send_buf_head];
    
    send_buf_size--;
    send_buf_head++;
    if (send_buf_head >= SEND_BUF_LEN) {
      send_buf_head = 0;
    }

    UDR0 = temp;
  }
}

/**
 * Send a character through the uart
 * return with success or not
 **/
int uart0_put(char c, FILE *f) {
  if (send_buf_size >= SEND_BUF_LEN) {
    return -1;
  }

  cli();

  send_buf[send_buf_tail] =  c;
  send_buf_tail++;
  if (send_buf_tail >= SEND_BUF_LEN) {
    send_buf_tail = 0;
  }
  send_buf_size++;

  sei();

  UCSR0B |= _BV(UDRIE0);

  return 0;
}

/**
 * Get a character from the uart and return it
 * This function will block until a character is available
 **/
int uart0_get(FILE *f) {

  //wait until something is available in the receive buffer
  while (recv_buf_size == 0);

  cli();

  char tmp = recv_buf[recv_buf_head];

  recv_buf_head++;
  if (recv_buf_head >= RECV_BUF_LEN) {
    recv_buf_head = 0;
  } 
  recv_buf_size--;

  sei();

  return tmp;
}