#include "FreeRTOS.h"
#include "task.h"

#include "xil_io.h"
#include "xstatus.h"
#include "xllfifo.h"

#include "pl_udp_eth.h"
#include "project.h"

static XLlFifo FifoInstance;
static unsigned char msg[MAX_MSG_LENGTH];

uint32_t pl_udp_init()
{

	XLlFifo_Config *Config;
	int Status;
	Status = XST_SUCCESS;

	/* Initialize the Device Configuration Interface driver */
	Config = XLlFfio_LookupConfig(XPAR_AXI_FIFO_0_DEVICE_ID);
	if (!Config) {
		xil_printf("No config found for %d\r\n", XPAR_AXI_FIFO_0_DEVICE_ID);
		return XST_FAILURE;
	}

	/*
	 * This is where the virtual address would be used, this example
	 * uses physical address.
	 */
	Status = XLlFifo_CfgInitialize(&FifoInstance, Config, Config->BaseAddress);
	if (Status != XST_SUCCESS) {
		xil_printf("Initialization failed\n\r");
		return Status;
	}

	/* Check for the Reset value */
	Status = XLlFifo_Status(&FifoInstance);
	XLlFifo_IntClear(&FifoInstance, 0xffffffff);
	Status = XLlFifo_Status(&FifoInstance);
	if(Status != 0x0) {
		xil_printf("\n ERROR : Reset value of ISR0 : 0x%x\t"
			    "Expected : 0x0\n\r",
			    XLlFifo_Status(&FifoInstance));
		return XST_FAILURE;
	}

	return Status;
}

uint32_t pl_udp_getMsg(unsigned char *msg, int *length)
{
	int i;
	int ret;
	u32 RxWord;
	static u32 ReceiveLength;

	u32 * msg_as_int = (u32*)msg;
	u32 ind = 0;

	ret = XST_NO_DATA;

	while(XLlFifo_iRxOccupancy(&FifoInstance)) {
		/* Read Receive Length */
		ReceiveLength = XLlFifo_iRxGetLen(&FifoInstance) / 4;
		xil_printf("PL UDP RX, %d, %d \n\r", XLlFifo_iRxOccupancy(&FifoInstance), ReceiveLength);
		for (i=0; i < ReceiveLength; i++) {
			RxWord = XLlFifo_RxGetWord(&FifoInstance);
			msg_as_int[ind] = RxWord;
			ind++;
		}
		ret = XST_SUCCESS;
	}

	*length = ind * 4;


	return ret;

}


uint32_t pl_udp_sendMsg(unsigned char *msg, int length)
{

	u32 udp_packet_size = 512;
	u32 length_int = udp_packet_size >> 2;
	u32 * msg_as_int = (u32*)msg;

	u32 num_packets = length / udp_packet_size;
	if (length % udp_packet_size) {
		num_packets++;
	}

//	xil_printf(" Transmitting Data, Num Bytes - %d, Num Packets - %d\r\n", length, num_packets);
	u32 ind = 0;
	for (int i=0 ; i < num_packets ; i++){
		/* Writing into the FIFO Transmit Port Buffer */
		for (int j=0 ; j < length_int ; j++){
			if( XLlFifo_iTxVacancy(&FifoInstance) ){
				XLlFifo_TxPutWord(&FifoInstance, msg_as_int[ind]);
				ind++;
			}
		}

		/* Start Transmission by writing transmission length into the TLR */
		XLlFifo_iTxSetLen(&FifoInstance, udp_packet_size);

		/* Check for Transmission completion */
		while( !(XLlFifo_IsTxDone(&FifoInstance)) ){

		}
	}
//	xil_printf(" Done \r\n");

	return XST_SUCCESS;
}

void pl_udp_task( void *pvParameters ) {

	// Reset Ethernet
	setBit(0x40050008, 15);
	vTaskDelay(1);
	clearBit(0x40050008, 15);

	pl_udp_init();

	int length;

	while (1) {
		if (pl_udp_getMsg(msg, &length) == XST_SUCCESS) {
			// Looping back so hping will work, can do something more interesting
			// with this later
			pl_udp_sendMsg(msg, length);
		} else {
			vTaskDelay(0);
		}
	}


}
