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

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

#define RECV_BUF_LEN 1024
#define SEND_BUF_LEN 1024

static volatile int recv_buf1_head1, recv_buf1_tail1, recv_buf1_size1;
static volatile char recv_buf1[RECV_BUF_LEN];

static volatile int send_buf1_head1, send_buf1_tail1, send_buf1_size1;
static volatile char send_buf1[SEND_BUF_LEN];

void init_uart1(void) {
  /* Set clock rate divisor to have 38400 bps */
  UBRR1H = (unsigned char)(MYUBRR >> 8);
  UBRR1L = (unsigned char)MYUBRR;

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

  /* Enable receiver and tranceiver and associated interrupts */
  UCSR1B = _BV(RXCIE1) | _BV(TXEN1) | _BV(RXEN1);

  //initialize things needed for our buffers
  recv_buf1_head1 = 0;
  recv_buf1_tail1 = 0;
  recv_buf1_size1 = 0;
  send_buf1_head1 = 0;
  send_buf1_tail1 = 0;
  send_buf1_size1 = 0;

  /* Uncomment to allow redirection of stdin and stdout through uart1 */
    fdevopen(uart1_put, uart1_get);

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

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

/* Receive complete on USART0 */
ISR(USART1_RX_vect) {
  char tmp =  UDR1;

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

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

/* Data register is empty on USART0 */
ISR(USART1_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_buf1_size1 <= 0) {
    UCSR1B &= ~_BV(UDRIE1);
  } else {
    // put data into UDR0 to be sent

    char temp = send_buf1[send_buf1_head1];
    
    send_buf1_size1--;
    send_buf1_head1++;
    if (send_buf1_head1 >= SEND_BUF_LEN) {
      send_buf1_head1 = 0;
    }

    UDR1 = temp;
  }
}

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

  cli();

  send_buf1[send_buf1_tail1] =  c;
  send_buf1_tail1++;
  if (send_buf1_tail1 >= SEND_BUF_LEN) {
    send_buf1_tail1 = 0;
  }
  send_buf1_size1++;

  sei();

  UCSR1B |= _BV(UDRIE1);

  return 0;
}

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

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

  cli();

  char tmp = recv_buf1[recv_buf1_head1];

  recv_buf1_head1++;
  if (recv_buf1_head1 >= RECV_BUF_LEN) {
    recv_buf1_head1 = 0;
  } 
  recv_buf1_size1--;

  sei();

  return tmp;
}