#include "uart.h"
#include "cbuffer.h"
#include "leds.h"

static CircularBuffer fromPC, toPC;

// Indicates the end of a transmission
#define TRANSMIT_COMMAND   255u

// Escape codes
#define DATA_ESCAPE        254u
#define ESC_TRANSMIT_COMMAND 1u
#define ESC_DATA_ESCAPE    0u

#define MAX_PACKET_LENGTH 20u

/**
 * Returns the amount of room in the UART send buffer.
 */
uint8_t uart_send_space_left() {
	return BUFSIZE - 1 - circular_buffer_size(&toPC);
}

/**
 * Initializes the UART to 9600 baud.
 */
void uart_init() {
	UCA1CTL1 |= UCSWRST;                // **Put state machine in reset**
	UCA1CTL1 |= UCSSEL_2;				// SMCLK
	UCA1BR0 = 105;						// Baud rate of 9600 (can change later)
	UCA1BR1 = 3;						// (UCA0BR0 + UCA0BR1 * 256 = UCBR)
	UCA1MCTL = UCBRS_7 + UCBRF_0;		// Modulation UCBRSx = 1
	UCA1CTL1 &= ~UCSWRST;				// **Initialize USCI state machine**
	UCA1IE |= UCRXIE;                   // Enable USCI_A0 RX interrupt	
	
	circular_buffer_init(&fromPC);
	circular_buffer_init(&toPC);
}

/**
 * Sends data to the UART.  If there is no room, the data is dropped.
 * If the UART is not transmitting, transmission begins.
 */
void uart_write_raw(uint8_t byte) {
	circular_buffer_push(&toPC, byte);
	
	// Start transmission if it has stopped.
	if (!(UCA1IE & UCTXIE)) {
		UCA1IE |= UCTXIE;
		UCA1TXBUF = circular_buffer_pop(&toPC);
	}
}

/**
 * Tells the wireless interface to transmit.
 */
void uart_finish() {
	uart_write_raw(TRANSMIT_COMMAND);
}

/**
 * Sends an encoded data byte.
 */
void uart_write(uint8_t byte) {
	switch (byte) {
		case TRANSMIT_COMMAND:
			uart_write_raw(DATA_ESCAPE);
			uart_write_raw(ESC_TRANSMIT_COMMAND);
		break;
		
		case DATA_ESCAPE:
			uart_write_raw(DATA_ESCAPE);
			uart_write_raw(ESC_DATA_ESCAPE);
		break;
		
		default:
			uart_write_raw(byte);
		break;
	}
}

/**
 * Returns true if the UART has data to receive.
 */
bool uart_has_data() {
	// Skip over transmit commands
	while (!circular_buffer_empty(&fromPC) &&
	       circular_buffer_peek(&fromPC, 0) == TRANSMIT_COMMAND) {
		circular_buffer_pop(&fromPC);
	}
	uint8_t length = circular_buffer_size(&fromPC);
	if (!length) { return false; }
	return length >= ((circular_buffer_peek(&fromPC, 0) == DATA_ESCAPE) ? 2 : 1);
}

/**
 * Reads one byte of data from the UART.
 * Returns 0 if there is no data.
 */
uint8_t uart_read() {
	uint8_t byte = circular_buffer_peek(&fromPC, 0);
	if (byte == DATA_ESCAPE) {
		//led_toggle(LED_GREEN);
		if (circular_buffer_size(&fromPC) < 2) {
			return 0;
		}
		circular_buffer_pop(&fromPC);
		switch (circular_buffer_pop(&fromPC)) {
			case ESC_DATA_ESCAPE:
				return DATA_ESCAPE;
			case ESC_TRANSMIT_COMMAND:
				return TRANSMIT_COMMAND;
			default:
				// Should never get here...
				return 0;
		}
	} else {
		//led_toggle(LED_RED);
		return circular_buffer_pop(&fromPC);
	}
}

/**
 * USCI A1 Transmit/Receive ISR
 * 
 * Handles both sending and receiving.
 */
#pragma vector=USCI_A1_VECTOR
__interrupt void USCI_A1_ISR(void)
{
	if (UCA1IV == BIT1) {
		// Received something from the UART so just store
		// in the buffer
		uint8_t data = UCA1RXBUF;
		if (!circular_buffer_full(&fromPC)) {
			circular_buffer_push(&fromPC, data);
		}
	} else {
		// Transmit data or turn off transmitter
		if (!circular_buffer_empty(&toPC)) {
			UCA1TXBUF = circular_buffer_pop(&toPC);
		} else {
			// Turn interrupt off to signal end of transmission
			// TODO: is there a better way?
			UCA1IFG &= ~UCTXIFG;
			UCA1IE &= ~UCTXIE;
		}
	}
}