#include "config.h"
#include "uart.h"
#include "util.h"

void uart_init(int uart, int32_t rate)
{
	int32_t brg;

	brg = ((FCY + (8 * rate - 1)) / (16 * rate)) - 1;
	if (brg < 1) brg = 1;
	if (brg > 65535) brg = 65535;

	if (uart == 1) {
		U1MODE = 0;
		U1MODEbits.BRGH = 0; // errata: BRGH=1 is broken
		U1BRG = brg;
		U1STA = 0;
		U1MODEbits.UARTEN = 1;
		U1STAbits.UTXEN = 1;
	} else if (uart == 2) {
		U2MODE = 0;
		U2MODEbits.BRGH = 0;
		U2BRG = brg;
		U2STA = 0;
		U2MODEbits.UARTEN = 1;
		U2STAbits.UTXEN = 1;
	}
}

void uart_put(int uart, uint8_t x)
{
	if (uart == 1) {
		while (U1STAbits.UTXBF) continue;
		U1TXREG = x;
	} else if (uart == 2) {
		while (U2STAbits.UTXBF) continue;
		U2TXREG = x;
	}
}

int uart_can_get(int uart)
{
	if (uart == 1)
		return U1STAbits.URXDA;
	else if (uart == 2)
		return U2STAbits.URXDA;
	else
		return 0;
}

uint8_t uart_get(int uart)
{
	uint8_t data = 0;
	if (uart == 1) {
		while (!U1STAbits.URXDA) continue;
		data = U1RXREG;
		if (U1STAbits.OERR)
			U1STAbits.OERR = 0;
	} else if (uart == 2) {
		while (!U2STAbits.URXDA) continue;
		data = U2RXREG;
		if (U2STAbits.OERR)
			U2STAbits.OERR = 0;
	}
	return data;
}

void uart_put_string(int uart, const char *s)
{
	while(s && *s)
		uart_put(uart, *s++);
}

void uart_put_bin(int uart, uint8_t x)
{
	int i;
	for(i=0;i<8;i++) {
		uart_put(uart, (x & 0x80) ? '1' : '0');
		x <<= 1;
	}
}

void uart_put_hex(int uart, uint8_t x)
{
	uart_put(uart, hex[x >> 4]);
	uart_put(uart, hex[x & 15]);
}

void uart_put_hex16(int uart, uint16_t x)
{
	uart_put_hex(uart, (x >> 8) & 0xFF);
	uart_put_hex(uart, (x) & 0xFF);
}

void uart_put_hex32(int uart, uint32_t x)
{
	uart_put_hex(uart, (x >> 24) & 0xFF);
	uart_put_hex(uart, (x >> 16) & 0xFF);
	uart_put_hex(uart, (x >> 8) & 0xFF);
	uart_put_hex(uart, (x) & 0xFF);
}

void uart_put_dec(int uart, int32_t x)
{
	uint32_t val;
	uint32_t place = 1;

	if (x >= 0) {
		val = x;
	} else {
		uart_put(uart, '-');
		val = -x;
	}

	while (val / place > 9)
		place *= 10;
	while (place > 0) {
		uart_put(uart, val / place + '0');
		val %= place;
		place /= 10;
	}
}

void uart_put_float(int uart, float x)
{
	int i;
	int32_t v;

	v = (int32_t) x;
	uart_put_dec(uart, v);
	uart_put(uart, '.');
	for (i = 0; i < 6; i++) {
		x -= v;
		x *= 10;
		v = (int32_t) x;
		uart_put(uart, v + '0');
	}
}

void uart_crlf(int uart)
{
	uart_put(uart, '\r');
	uart_put(uart, '\n');
}