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


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

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

//This is copied from xuart driver
/***************************************************/

#define XUARTPS_MAX_BAUD_ERROR_RATE		 3	/* max % error allowed */
int XUartPs_SetBaudRate_ours(XUartPs *InstancePtr, u32 BaudRate)
{
	u8 IterBAUDDIV;		/* Iterator for available baud divisor values */
	u32 BRGR_Value;		/* Calculated value for baud rate generator */
	u32 CalcBaudRate;	/* Calculated baud rate */
	u32 BaudError;		/* Diff between calculated and requested baud rate */
	u32 Best_BRGR = 0;	/* Best value for baud rate generator */
	u8 Best_BAUDDIV = 0;	/* Best value for baud divisor */
	u32 Best_Error = 0xFFFFFFFF;
	u32 PercentError;
	u32 ModeReg;
	u32 InputClk;

	/*
	 * Asserts validate the input arguments
	 */
	Xil_AssertNonvoid(InstancePtr != NULL);
	Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY);
	//Xil_AssertNonvoid(BaudRate <= XUARTPS_MAX_RATE);
	Xil_AssertNonvoid(BaudRate >= XUARTPS_MIN_RATE);

	/*
	 * Make sure the baud rate is not impossilby large.
	 * Fastest possible baud rate is Input Clock / 2.
	 */
	if ((BaudRate * 2) > InstancePtr->Config.InputClockHz) {
		return XST_UART_BAUD_ERROR;
	}
	/*
	 * Check whether the input clock is divided by 8
	 */
	ModeReg = XUartPs_ReadReg( InstancePtr->Config.BaseAddress,
				 XUARTPS_MR_OFFSET);

	InputClk = InstancePtr->Config.InputClockHz;
	if(ModeReg & XUARTPS_MR_CLKSEL) {
		InputClk = InstancePtr->Config.InputClockHz / 8;
	}

	/*
	 * Determine the Baud divider. It can be 4to 254.
	 * Loop through all possible combinations
	 */
	for (IterBAUDDIV = 4; IterBAUDDIV < 255; IterBAUDDIV++) {

		/*
		 * Calculate the value for BRGR register
		 */
		BRGR_Value = InputClk / (BaudRate * (IterBAUDDIV + 1));

		/*
		 * Calculate the baud rate from the BRGR value
		 */
		CalcBaudRate = InputClk/ (BRGR_Value * (IterBAUDDIV + 1));

		/*
		 * Avoid unsigned integer underflow
		 */
		if (BaudRate > CalcBaudRate) {
			BaudError = BaudRate - CalcBaudRate;
		}
		else {
			BaudError = CalcBaudRate - BaudRate;
		}

		/*
		 * Find the calculated baud rate closest to requested baud rate.
		 */
		if (Best_Error > BaudError) {

			Best_BRGR = BRGR_Value;
			Best_BAUDDIV = IterBAUDDIV;
			Best_Error = BaudError;
		}
	}

	/*
	 * Make sure the best error is not too large.
	 */
	PercentError = (Best_Error * 100) / BaudRate;
	if (XUARTPS_MAX_BAUD_ERROR_RATE < PercentError) {
		return XST_UART_BAUD_ERROR;
	}

	/*
	 * Disable TX and RX to avoid glitches when setting the baud rate.
	 */
	XUartPs_DisableUart(InstancePtr);

	XUartPs_WriteReg(InstancePtr->Config.BaseAddress,
			   XUARTPS_BAUDGEN_OFFSET, Best_BRGR);
	XUartPs_WriteReg(InstancePtr->Config.BaseAddress,
			   XUARTPS_BAUDDIV_OFFSET, Best_BAUDDIV);

	/*
	 * Enable device
	 */
	XUartPs_EnableUart(InstancePtr);

	InstancePtr->BaudRate = BaudRate;

	return XST_SUCCESS;

}

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

/************************************************/
/************** 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);
	XUartPs_SetBaudRate_ours(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;
	*/
}