#include "communication.h"

#define INTC		XScuGic
#define COMM_UART_DEVICE_ID		XPAR_PS7_UART_0_DEVICE_ID
#define COMM_INTC_DEVICE_ID		XPAR_SCUGIC_SINGLE_DEVICE_ID
#define COMM_UART_INT_IRQ_ID		XPAR_PS7_UART_0_INTR//XPAR_XUARTPS_0_INTR

#define BAUD_RATE 921600
// Maximum number of bytes to be received within our loop time,
// plus the maximum packet size that might be carried over from the last call,
// plus 128 for a little buffer
// (bit/s) * (seconds) / (10 bits / byte for UART)
#define MAX_PACKET_SIZE 256
#define UART_BUF_SIZE (((BAUD_RATE * (DESIRED_USEC_PER_LOOP / 1000) / 1000) / 10) + MAX_PACKET_SIZE + 128)

// Declaration of interrupt handler
void Handler(void *CallBackRef, u32 Event, unsigned int EventData);

// Pointer to the UART driver instance
static XUartPs* uartInstPtr;

static u8 recvBuf[UART_BUF_SIZE];
static volatile size_t recv_buf_begin = 0; // Index of start of valid packets in buffer
static volatile size_t recv_buf_end = 0; // One past end of valid packets in buffer


int initUartComms() {
	uartInstPtr = uart0_init(COMM_UART_DEVICE_ID, BAUD_RATE);
	// Initialize UART0 (Bluetooth/WiFi)
	if(!uartInstPtr) {
		return -1;
	}

	uart0_clearFIFOs();

	if (uart0_int_init(COMM_UART_INT_IRQ_ID, (Xil_ExceptionHandler) uart_interrupt_handler) != XST_SUCCESS) {
		return -1;
	}


	/*
	 * Setup the handlers for the UART that will be called from the
	 * interrupt context when data has been sent and received, specify
	 * a pointer to the UART driver instance as the callback reference
	 * so the handlers are able to access the instance data
	 */
	//XUartPs_SetHandler(uartInstPtr, (XUartPs_Handler)Handler, uartInstPtr);

	u32 IntrMask = XUARTPS_IXR_RXFULL | XUARTPS_IXR_RXOVR | XUARTPS_IXR_TOUT;

	XUartPs_SetInterruptMask(uartInstPtr, IntrMask);


	/*
	 * Set the receiver timeout. If it is not set, and the last few bytes
	 * of data do not trigger the over-water or full interrupt, the bytes
	 * will not be received. By default it is disabled.
	 * Timeout duration = RecvTimeout x 4 x Bit Period. 0 disables the
	 *		timeout function.
	 *
	 * The setting of 8 will timeout after 8 x 4 = 32 character times.
	 * Increase the time out value if baud rate is high, decrease it if
	 * baud rate is low.
	 */
	XUartPs_SetRecvTimeout(uartInstPtr, 8);

	// Second argument is the number of bytes to trigger an interrupt at
	XUartPs_SetFifoThreshold(uartInstPtr, 48);

	return 0;
}

int process_packet(unsigned char* packet, modular_structs_t *structs) {
	metadata_t meta_data;
	// parse metadata
	meta_data.begin_char = packet[0];
	meta_data.msg_type = packet[1];
	meta_data.msg_subtype = packet[2];
	meta_data.msg_id = (packet[4] << 8) | (packet[3]);
	meta_data.data_len = (packet[6] << 8) | (packet[5]);
	unsigned char packet_checksum = packet[7+meta_data.data_len];
	unsigned char* packet_data = packet + sizeof(metadata_t);

	// Compute checksum
	int i;
	unsigned char calculated_checksum = 0;
	for(i = 0; i < meta_data.data_len + 7; i++){
		calculated_checksum ^= packet[i];
	}
	// Discard if checksum didn't match
	if(packet_checksum != calculated_checksum) {
		printf("Checksums did not match: 0x%02x\t0x%02x\n", packet_checksum, calculated_checksum);
		return -1;
	}

	// Call appropriate function for packet
	(* (MessageTypes[meta_data.msg_type].subtypes[meta_data.msg_subtype].functionPtr))(packet_data, meta_data.data_len, structs);

	return 0;

}

void parse_available(modular_structs_t *structs) {
	// Minimum size of a packet (header + checksum)
	int min_packet_size = 8;
	// TODO: Loop limits?
	while (recv_buf_end - recv_buf_begin >= min_packet_size) {
		// Discard all data before packet begin character
		while (recv_buf_begin < recv_buf_end && recvBuf[recv_buf_begin] != BEGIN_CHAR) {
			recv_buf_begin++;
		}
		// If, after discarding everything before header, not enough is left, don't parse
		if (recv_buf_end - recv_buf_begin < min_packet_size) {
			break;
		}
		unsigned char* packet_header = recvBuf + recv_buf_begin;

		// Read out the payload size information from the header
		size_t packet_len = (packet_header[6] << 8) | (packet_header[5]);
		// Determine if we have received the entire packet. If so, process it.
		if (recv_buf_end - recv_buf_begin >= min_packet_size + packet_len) {
			process_packet(recvBuf + recv_buf_begin, structs);
			recv_buf_begin += min_packet_size + packet_len;
		}
		else {
			// Not enough data for a packet
			break;
		}
	}
}

/*
 * Temporarily disables interrupts and returns the interrupt state, for restoring them
 */
u32 disable_interrupts() {
	u32 ImrRegister;
	ImrRegister = XUartPs_ReadReg(uartInstPtr->Config.BaseAddress, XUARTPS_IMR_OFFSET);
	XUartPs_WriteReg(uartInstPtr->Config.BaseAddress, XUARTPS_IDR_OFFSET, XUARTPS_IXR_MASK);
	return ImrRegister;
}

/*
 * Restores the interrupt state, saved from disable_interrupts
 */
void restore_interrupts(u32 intr_state) {
	XUartPs_WriteReg(uartInstPtr->Config.BaseAddress, XUARTPS_IER_OFFSET, intr_state);
}

void process_received(modular_structs_t *structs) {
	// Parse as many packets as possible
	parse_available(structs);

	// Disable interrupts while moving data around
	u32 intr_state = disable_interrupts();

	// Move unprocessed bytes to front of secondary buffer
	size_t unprocessed_size = recv_buf_end - recv_buf_begin;
	memmove(recvBuf, recvBuf + recv_buf_begin, unprocessed_size);
	recv_buf_begin = 0;
	recv_buf_end = unprocessed_size;

	restore_interrupts(intr_state);
	//XUartPs_SetRecvTimeout(uartInstPtr, 8);
	//unsigned char in_fifo = XUartPs_ReadReg(uartInstPtr->Config.BaseAddress, XUARTPS_FIFO_OFFSET);
	return;
}

void uart_interrupt_handler(XUartPs *InstancePtr) {
	u32 IsrStatus;

	/*
	 * Read the interrupt ID register to determine which
	 * interrupt is active
	 */
	IsrStatus = XUartPs_ReadReg(InstancePtr->Config.BaseAddress,
				   XUARTPS_IMR_OFFSET);

	IsrStatus &= XUartPs_ReadReg(InstancePtr->Config.BaseAddress,
				   XUARTPS_ISR_OFFSET);

	/*
	* Read the Channel Status Register to determine if there is any data in
	 * the RX FIFO
	 */

	u32 CsrRegister = XUartPs_ReadReg(InstancePtr->Config.BaseAddress,
				XUARTPS_SR_OFFSET);

	while (0 == (CsrRegister & XUARTPS_SR_RXEMPTY) && recv_buf_end <= UART_BUF_SIZE) {
		recvBuf[recv_buf_end] = XUartPs_ReadReg(InstancePtr->Config.BaseAddress, XUARTPS_FIFO_OFFSET);
		recv_buf_end += 1;
		CsrRegister = XUartPs_ReadReg(InstancePtr->Config.BaseAddress, XUARTPS_SR_OFFSET);
	}

	// Clear the interrupt status.
	XUartPs_WriteReg(InstancePtr->Config.BaseAddress, XUARTPS_ISR_OFFSET,
			IsrStatus);
}

int send_data(u16 type_id, u16 subtype_id, u16 msg_id, char* data, size_t size) {
	//----------------------------------------------------------------------------------------------
	//	   index||	   0	|	  1	   |	  2		 |	3 & 4 |		 5 & 6		 |	7+	|	end	   |
	//---------------------------------------------------------------------------------------------|
	// msg param|| beg char | msg type | msg subtype | msg id | data len (bytes) | data | checksum |
	//-------------------------------------------------------------------------------------------- |
	//	   bytes||	   1	|	  1	   |	  1		 |	  2	  |		   2		 | var	|	 1	   |
	//----------------------------------------------------------------------------------------------

	char formattedHeader[7];

	// Begin Char:
	formattedHeader[0] = BEGIN_CHAR;
	// Msg type:
	formattedHeader[1] = type_id;
	// Msg subtype
	formattedHeader[2] = subtype_id;
	//Msg id 2 bytes
	formattedHeader[3] = msg_id & 0x000000ff;
	formattedHeader[4] = (msg_id >> 8) & 0x000000ff;
	// Data length and data - bytes 5&6 for len, 7+ for data
	formattedHeader[5] = size & 0x000000ff;
	formattedHeader[6] = (size >> 8) & 0x000000ff;

	// Compute checksum while sending
	unsigned char packet_checksum = 0;

	int i;
	// TODO: Look into uart0_sendBytes and see if it would be better to use
	// Send header
	//uart0_sendBytes(formattedHeader, 7);

	for(i = 0; i < 7; i++) {
		packet_checksum ^= formattedHeader[i];
		uart0_sendByte(formattedHeader[i]);
	}
	// Send data
	for (i = 0; i < size; i++) {
		packet_checksum ^= data[i];
		uart0_sendByte(data[i]);
	}
	//uart0_sendBytes(data, size);
	// Send checksum
	uart0_sendByte(packet_checksum);

	return 0;
}