/*
 * uart.c
 *
 *  Created on: Nov 10, 2014
 *      Author: ucart
 */


#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include "uart.h"

// Global PS's
XUartPs* _Uart0PS;
XUartPs* _Uart1PS;


/************************************************/
/************** Main UART Interface *************/

XUartPs* uart_init(XUartPs* uartps_ptr, u16 deviceID, int baudRate) {
	XUartPs_Config* config = XUartPs_LookupConfig(deviceID);

	//Configure XUartPs instance
	int Status = XUartPs_CfgInitialize(uartps_ptr, config, config->BaseAddress);
	if (Status != XST_SUCCESS){
		return NULL;
	}

	//Set Baudrate for BT
	XUartPs_SetBaudRate(uartps_ptr, baudRate);

	return uartps_ptr;
}

void uart_clearFIFOs(XUartPs* uartps_ptr) {
	//Get UART0 Control Register and clear the TX and RX Fifos
	int* uart_ctrl_reg = (int*) uartps_ptr->Config.BaseAddress;
	*uart_ctrl_reg |= 0x00000003; // clear TX & RX
}

void uart_sendByte(XUartPs* uartps_ptr, char data) {
	XUartPs_SendByte(uartps_ptr->Config.BaseAddress, data);
}

void uart_sendStr(XUartPs* uartps_ptr, char* str) {
	uart_sendBytes(uartps_ptr, str, strlen(str));
}

void uart_sendBytes(XUartPs* uartps_ptr, char* data, int numBytes) {
	while (uart_isSending(uartps_ptr))
		;

	int done = 0;
	while (done < numBytes) {
		done += XUartPs_Send(uartps_ptr, (unsigned char*) (&data[done]), numBytes - done);
	}
}

int uart_isSending(XUartPs* uartps_ptr) {
	return XUartPs_IsSending(uartps_ptr);
}

int uart_hasData(XUartPs* uartps_ptr) {
	return XUartPs_IsReceiveData(uartps_ptr->Config.BaseAddress);
}

void uart_recvBytes(XUartPs* uartps_ptr, char* buffer, int numBytes) {
	int received = 0;
	while (received < numBytes) {
		received += XUartPs_Recv(uartps_ptr, (unsigned char*) &buffer[received], (numBytes - received));
	}
}

char uart_recvByte(XUartPs* uartps_ptr) {
	return XUartPs_RecvByte(uartps_ptr->Config.BaseAddress);
	//char buffer[1];
	//XUartPs_Recv(uartps_ptr, (unsigned char*) &buffer[0], 1);

//	return buffer[0];
}

/************************************************/
/************************************************/





/************************************************/
/********** UART 0 convenience methods **********/

XUartPs* uart0_init(u16 deviceID, int baudRate){
	if (_Uart0PS) {
		free(_Uart0PS);
	}
	_Uart0PS = malloc(sizeof(XUartPs));
	return uart_init(_Uart0PS, deviceID, baudRate);
}

void uart0_clearFIFOs(){
	uart_clearFIFOs(_Uart0PS);
}

void uart0_sendByte(u8 data){
	uart_sendByte(_Uart0PS, data);
}

void uart0_sendStr(char* str) {
	uart_sendStr(_Uart0PS, str);
}

void uart0_sendMetaData(metadata_t md)
{
	uart0_sendByte(md.begin_char);
	uart0_sendByte(md.msg_type);
	uart0_sendByte(md.msg_subtype);
	uart0_sendByte(md.msg_id & 0x00ff);
	uart0_sendByte((md.msg_id >> 8) & 0x00ff);
	uart0_sendByte(md.data_len & 0x00ff);
	uart0_sendByte((md.data_len >> 8) & 0x00ff);
}

void uart0_sendBytes(char* data, int numBytes){
	uart_sendBytes(_Uart0PS, data, numBytes);
}

int uart0_isSending(){
	return uart_isSending(_Uart0PS);
}

int uart0_hasData(){
	return uart_hasData(_Uart0PS);
}

void uart0_recvBytes(char* buffer, int numBytes) {
	uart_recvBytes(_Uart0PS, buffer, numBytes);
}

char uart0_recvByte() {
	return uart_recvByte(_Uart0PS);
}

/************************************************/
/************************************************/






/************************************************/
/********** UART 1 convenience methods **********/

XUartPs* uart1_init(u16 deviceID, int baudRate){
	if (_Uart1PS) {
		free(_Uart1PS);
	}
	_Uart1PS = malloc(sizeof(XUartPs));
	return uart_init(_Uart1PS, deviceID, baudRate);
}

void uart1_clearFIFOs(){
	uart_clearFIFOs(_Uart1PS);
}

void uart1_sendByte(char data){
	uart_sendByte(_Uart1PS, data);
}

void uart1_sendStr(char* str) {
	uart_sendStr(_Uart1PS, str);
}

void uart1_sendBytes(char* data, int numBytes){
	uart_sendBytes(_Uart1PS, data, numBytes);
}

int uart1_isSending(){
	return uart_isSending(_Uart1PS);
}

int uart1_hasData(){
	return uart_hasData(_Uart1PS);
}

void uart1_recvBytes(char* buffer, int numBytes) {
	uart_recvBytes(_Uart1PS, buffer, numBytes);
}

char uart1_recvByte() {
	return uart_recvByte(_Uart1PS);
}

/************************************************/
/************************************************/

int tryReceivePacket(stringBuilder_t* sb, int echo) {
	while(uart0_hasData()) {
		char c = uart0_recvByte(); // begin char
		if(c == 0xBE) {
//			printf("Beginning to read packet from UART.\n");

			sb->addChar(sb, 0xBE);
			char type = uart0_recvByte();
			sb->addChar(sb, type); // type
			sb->addChar(sb, uart0_recvByte()); // subtype
			sb->addChar(sb, uart0_recvByte()); // id
			sb->addChar(sb, uart0_recvByte()); // id

			unsigned char byte5 = uart0_recvByte(); // data length
			unsigned char byte6 = uart0_recvByte(); // data length

			int datalen = (byte6 << 8) | byte5;
//			printf("Received packet with data length: %d", datalen);

			sb->addChar(sb, byte5);
			sb->addChar(sb, byte6);

			// Read all the data and the checksum byte
			int i;
			for(i=0; i < datalen + 1; i++)
			{
				sb->addChar(sb, uart0_recvByte());
			}

//			printf("Done reading packet from UART.\n");
			return type;
		} else {
//			printf("The first byte was not the begin char: %x\n", c);
//			return -1;
		}
	}
	return -1;
/*

	while (uart0_hasData()) {
		char c = uart0_recvByte();
		if (c == PACKET_START_CHAR) {
#if DEBUG
			uart0_sendStr("Start ");
#endif
			c = uart0_recvByte();
			char type = c;
//			sb->addChar(sb, type);
			int count = 0;

			//uart0_sendByte(c);

			// if it's a 'C' packet (a command for the quad), then
			// wait for the packet end char. Else, if it's a 'U' packet
			// which is an update from the camera system, then we read
			// a fixed number of bytes (UPDATE_SIZE # of bytes)
			if(type == 'C')
			{
				while (c != PACKET_END_CHAR)
				{
					c = uart0_recvByte();
					count++;
					if(c == PACKET_END_CHAR)
						break;
					if ((c != 0 && c != '\r' && c != '\n'))
					{
						sb->addChar(sb, c);
						if (echo) {
							uart0_sendByte(c);
						}
					}
				}
				return type;
			}

			else if(type == 'U')
			{
				while(count < UPDATE_SIZE)
				{
					c = uart0_recvByte();
					count++;

					sb->addChar(sb, c);
					if (echo) {
						uart0_sendByte(c);
					}
				}
				return type;
			}
#if DEBUG
			uart0_sendStr("End:[");
			uart0_sendStr(sb->buf);
			uart0_sendStr("] ");
#endif
			return 0;
		}
	}

	return 0;
	*/
}