#include <stdio.h>
#include <string.h>

#include "lwip/err.h"
#include "lwip/tcp.h"
#include "xtime_l.h"
#include "xparameters.h"
#if defined (__arm__) || defined (__aarch64__)
#include "xil_printf.h"
#endif

// Widi specific includes
#include "frame_mem_config.h"
#include "sleep.h"

#ifndef FRAME_SIZE
#define FRAME_SIZE 921600 * 3
#endif

#define FRAME_BUFFER_SZE 2
#define WIDI_NTWK_DEBUG 0

// Prototype decaration
err_t EthToMem(struct memory_range_s *memPtr, u32 *data, u32 Length);
void test(struct tcp_pcb *tpcb);
void setFrameReady(int curFrameBuffer);
void test_rx(void * ptr, long int len);

extern int FrameReadyFlag;
extern int FrameBufferNum;

//Test variables
u32 testFrame[FRAME_BUFFER_SZE][FRAME_SIZE];


int transfer_data() {
	return 0;
}

void print_app_header()
{
#if (LWIP_IPV6==0)
	xil_printf("\n\r\n\r-----lwIP TCP echo server ------\n\r");
#else
	xil_printf("\n\r\n\r-----lwIPv6 TCP echo server ------\n\r");
#endif
	xil_printf("TCP packets sent to port 6001 will be echoed back\n\r");
}

void setFrameReady(int curFrameBuffer){
	FrameReadyFlag = 1;
	FrameBufferNum = curFrameBuffer;
}

err_t connect_callback(void *arg, struct tcp_pcb *tpcb, err_t err){
	xil_printf("Connection Secured\n");
	return ERR_OK;
}

void error_handler(void * arg, err_t err){

}
XTime old_time = 0, new_time = 0;

err_t recv_callback(void *arg, struct tcp_pcb *tpcb,
                               struct pbuf *p, err_t err)
{
	static int curFrameBuffer = 0;
	static long int sizeAccum = 0;
	//sizeAccum += p->len;

	err_t status = ERR_OK;
	//int frame = getFrameCount();

	if(WIDI_NTWK_DEBUG){
		xil_printf("Packet received: ");
		print_ip(NULL, &tpcb->local_ip);
		xil_printf(" -> ");
		print_ip(NULL, &tpcb->remote_ip);
		xil_printf("\n");
	}

	/* do not read the packet if we are not in ESTABLISHED state */
	if (!p) {
		tcp_close(tpcb);
		tcp_recv(tpcb, NULL);
		return ERR_OK;
	}

	memcpy(&testFrame[curFrameBuffer][sizeAccum], p, p->len);
	/* indicate that the accumulated packets have been received */
	tcp_recved(tpcb, p->len);

	if((sizeAccum += p->len) >= FRAME_SIZE){
		sizeAccum = 0;
		setFrameReady(curFrameBuffer);
#if WIDI_NTWK_DEBUG == 1
		test_rx(testFrame[curFrameBuffer], FRAME_SIZE);
#endif
		if(++curFrameBuffer >= FRAME_BUFFER_SZE){
			curFrameBuffer = 0;
		}
	}

	/* save payload */
	// Input to specific memory frame (may be dependent on gray code value)
	//status = EthToMem(&memory_ranges[0], p->payload, p->len);
	/*for(int i = 0; i < p->len; i++){
		xil_printf("%d, ", *(u32*)(p->payload + i));
	}*/

	if(WIDI_NTWK_DEBUG){
		xil_printf("Size of incoming data: %d\n", p->len);
		xil_printf("Size of accumulated data: %d\n", sizeAccum);
		XTime_GetTime(&new_time);
		xil_printf("Time: %.8f\n", 1.0 * (new_time - old_time) / (COUNTS_PER_SECOND/1000000));
		old_time = new_time;
	}

	/* free the received pbuf */
	pbuf_free(p);

	return status;
}

err_t accept_callback(void *arg, struct tcp_pcb *newpcb, err_t err)
{
	static int connection = 1;

if(WIDI_NTWK_DEBUG){
	xil_printf("Connection accepted: ");
	print_ip(NULL, &newpcb->local_ip);
	xil_printf(" -> ");
	print_ip(NULL, &newpcb->remote_ip);
	xil_printf("\n");
}

	/* set the receive callback for this connection */
	tcp_recv(newpcb, recv_callback);

	/* just use an integer number indicating the connection id as the
	   callback argument */
	tcp_arg(newpcb, (void*)(UINTPTR)connection);

	/* increment for subsequent accepted connections */
	connection++;

	return ERR_OK;
}


int recv_start_application()
{
	struct tcp_pcb *pcb;
	err_t err;
	unsigned port = 7;

	/* create new TCP PCB structure */
	pcb = tcp_new_ip_type(IPADDR_TYPE_ANY);
	if (!pcb) {
		xil_printf("Error creating PCB. Out of Memory\n\r");
		return -1;
	}

	/* bind to specified @port */
	err = tcp_bind(pcb, IP_ANY_TYPE, port);
	if (err != ERR_OK) {
		xil_printf("Unable to bind to port %d: err = %d\n\r", port, err);
		return -2;
	}

	/* we do not need any arguments to callback functions */
	tcp_arg(pcb, NULL);

	/* listen for connections */
	pcb = tcp_listen(pcb);
	if (!pcb) {
		xil_printf("Out of memory while tcp_listen\n\r");
		return -3;
	}

	/* specify callback to use for incoming connections */
	tcp_accept(pcb, accept_callback);

	xil_printf("TCP receive server started @ port %d\n\r", port);

	return 0;
}


err_t EthToMem(struct memory_range_s *memPtr, u32 *data, u32 Length)
{
	u32 *Addr = (u32*)memPtr->base;

	if(Length > memPtr->size){
		xil_printf("Error - Data size inconsistent with frame");
		return ERR_MEM;
	}

	u32 I;
	for (I = 0U; I < Length; I++) {
		*(Addr+I) = *(data + I);
	}

	return ERR_OK;

}

void test_rx(void * ptr, long int len){
	for(int i = 0; i < len; i++){
		xil_printf(" 0x%08x ", *((u32*)ptr + i));
	}
}

err_t MemToEth(struct tcp_pcb *tpcb, struct memory_range_s *memPtr){
	err_t err;

	err = tcp_write(tpcb, (u32*)memPtr->base, memPtr->size, 1);
	if(err != ERR_OK){
		//freak the fuck out
	}
	return err;
}