#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/atomic.h>

#include "uart.h"

#define UART_BUFSIZE 64

typedef volatile struct {
	char data[UART_BUFSIZE];
	uint8_t r;
	uint8_t w;
} buf;

buf __attribute__ ((section (".noinit"))) uart_rxbuf;
buf __attribute__ ((section (".noinit"))) uart_txbuf;

#define BAUD_PRESCALE (((F_CPU/(BAUDRATE*8UL)))-1)

void __attribute__ ((naked)) __attribute__ ((section (".init5"))) uart_init() {
	uart_rxbuf.r = 0;
	uart_rxbuf.w = 0;

	uart_txbuf.r = 0;
	uart_txbuf.w = 0;

	UCSR0A = (1<<U2X0);
	UCSR0B = (1<<RXEN0) | (1<<TXEN0) | (1<<RXCIE0);
	UCSR0C = (1<<UCSZ00) | (1<<UCSZ01); // 8N1
	UBRR0 = BAUD_PRESCALE;
}

void __attribute__ ((naked)) __attribute__ ((section (".fini5"))) uart_deinit() {
	while ((UCSR0B & (1 << UDRIE0))) {};
	while (!(UCSR0A & (1 << TXC0))) {};
}

// RX Facilities
ISR(USART_RX_vect) {
	uint8_t c = UDR0;

	if (((uart_rxbuf.w - uart_rxbuf.r) & (UART_BUFSIZE - 1)) == 1) return;
	uart_rxbuf.data[uart_rxbuf.w++] = c;
	uart_rxbuf.w = uart_rxbuf.w & (UART_BUFSIZE - 1);
}

uint8_t uart_getc() {
	uint8_t c;
	while (uart_rxbuf.w == uart_rxbuf.r) {};

	// Disable interrupt to avoid race condition
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
		c = uart_rxbuf.data[uart_rxbuf.r++];
		uart_rxbuf.r = uart_rxbuf.r & (UART_BUFSIZE - 1);
	}

	return c;
}

// TX Facilities
ISR(USART_UDRE_vect) {
	if ((uart_txbuf.w != uart_txbuf.r) && (UCSR0B & (1<<TXEN0))) {
		UDR0 = uart_txbuf.data[uart_txbuf.r++];
		uart_txbuf.r = uart_txbuf.r & (UART_BUFSIZE - 1);
	} else {
		UCSR0B &= ~(1<<UDRIE0); // UDRE off
	}
}

uint8_t uart_putc(unsigned char c) {
	while (((uart_txbuf.r - uart_txbuf.w) & (UART_BUFSIZE - 1)) == 1) {};

	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
		uart_txbuf.data[uart_txbuf.w++] = c;
		uart_txbuf.w = uart_txbuf.w & (UART_BUFSIZE - 1);
	}

	UCSR0B |= (1<<UDRIE0); // UDRE on

	return 1;
}