#include "common.h"
#include "serial.h"

#include <stdarg.h>
#include <stdio.h>
#include <avr/io.h>
#include <avr/interrupt.h>

#define USART_BUFSIZE  512
#define USART_BAUDRATE 9600
#define USART_RXBUF_MASK (USART_BUFSIZE - 1)
#define USART_BAUD_PRESCALE ((((F_CPU / 16) + \
	(USART_BAUDRATE / 2)) / (USART_BAUDRATE)) - 1)

static unsigned char rxbuf[USART_BUFSIZE];
static unsigned char rxhead, rxtail;

void USART_Init(void)
{
	rxhead = 0;
	rxtail = 0;

	// Handle RXC interrupt
	UCSRB |= (1 << RXCIE);
	sei();

	UCSRB |= BIT(RXEN) | BIT(TXEN); // Enable rx and tx circuitry
	UCSRC |= BIT(URSEL) | BIT(UCSZ0) | BIT(UCSZ1); // 8-bit chars
	UBRRH = (USART_BAUD_PRESCALE >> 8); // Baud Rate upper byte
	UBRRL = USART_BAUD_PRESCALE; // Baud rate lower byte
}

char USART_GetChar(void)
{
	unsigned char newtail;

	if (!USART_IsDataAvailable()) {
		return '\0'; // Non-blocking
	}

	newtail = (rxtail + 1) & USART_RXBUF_MASK;
	rxtail = newtail;

	return rxbuf[newtail];
}

void USART_PutChar(const char ch)
{
	// Wait until UDR can receive data
	while ((UCSRA & BIT(UDRE)) == 0);
	UDR = ch;
}

void USART_Printf(const char *fmt, ...)
{
	char buf[256], *ptr;
	va_list args;

	va_start(args, fmt);
	vsprintf(buf, fmt, args);
	va_end(args);

	ptr = buf;
	while (*ptr != '\0') {
		USART_PutChar(*ptr++);
	}
}

bool USART_IsDataAvailable(void)
{
	return rxhead != rxtail;
}

// Interrupt Handler
ISR(USART_RXC_vect)
{
	unsigned char newhead;

	newhead = (rxhead + 1) & USART_RXBUF_MASK;
	if (newhead == rxtail) {
		// TODO: Handle overflow
	} else {
		rxhead = newhead;
		rxbuf[newhead] = UDR;
	}
}