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

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

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

#define FRAME_SIZE_U32 921600
#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);

//Test variables
extern u32 testframe[FRAME_SIZE_U32];
int send_buffer_size = 0;

int frame_send_index = 0;
u32* frame_send_ptr;

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");
}

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){

}

err_t recv_callback(void *arg, struct tcp_pcb *tpcb,
                               struct pbuf *p, err_t err)
{
	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;
	}

	/* indicate that the packet has been received */
	tcp_recved(tpcb, p->len);

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

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

	return status;
}

err_t sent_tx(void* arg, struct tcp_pcb *tpcb, u16_t len)
{
	//static int current_sent = send_buffer_size;
	int next_buffer_size;
	int buffer_size;
	err_t err;

	if(WIDI_NTWK_DEBUG){
		xil_printf("Current unsent val:%d/n frm index: %d\n", tpcb->snd_buf,frame_send_index);
	}

	if(tpcb->snd_buf == send_buffer_size && frame_send_index < FRAME_SIZE_U32){
		if(WIDI_NTWK_DEBUG){
			xil_printf("Inside sent_tx\n");
		}

		buffer_size = ((frame_send_index + send_buffer_size) > FRAME_SIZE_U32 * 3) ? FRAME_SIZE_U32 - frame_send_index : send_buffer_size;

		err = tcp_write(tpcb, (u8*)frame_send_ptr + frame_send_index, buffer_size, 0x01);
		if(err != ERR_OK){
			xil_printf("Test failed. Connection State: %s\n", tcp_debug_state_str(tpcb->state));
			xil_printf("Non-Specified Write Error: %d\n", err);
			xil_printf("Current accum val: %d\n", frame_send_index);
		}
		tcp_output(tpcb);
		frame_send_index += buffer_size;
	}

}

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;
}

void err_tx(void* arg, err_t err)
{
	xil_printf("Error Occured, likely during connection\n");
	//xil_printf("Connection State: %s\n", tcp_debug_state_str(newpcb->state));
	xil_printf("Error: %d\n", (int)err);
}

err_t poll_tx(void *arg, struct tcp_pcb *newpcb)
{
	test(newpcb);

	//xil_printf("Polling Connection State: %d\n", newpcb->state);
}

int trans_start_application(ip_addr_t* ipaddr_rx, ip_addr_t* ipaddr_ap)
{
	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);

	tcp_poll(pcb, poll_tx, 20);

	tcp_err(pcb, err_tx);

	tcp_sent(pcb, sent_tx);

	send_buffer_size = tcp_sndbuf(pcb);

	if(WIDI_NTWK_DEBUG){
		xil_printf("Attempting Connection\n");
	}

	err = tcp_connect(pcb, ipaddr_rx, 7, connect_callback);

	if(err == ERR_RTE){
		while(err == ERR_RTE){
			usleep(250);
			xil_printf("Attempting to connect ERR_RTE returned... \n");
			err = tcp_connect(pcb, ipaddr_rx, 7, connect_callback);
		}
	}
	else if (err != ERR_OK){
		xil_printf("Non-Specified Connection Error: %d\n", err);
		return err;
	}

	if(WIDI_NTWK_DEBUG){
		xil_printf("Connection State: %d\n", pcb->state);

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

		xil_printf("Connection State: %s\n", tcp_debug_state_str(pcb->state));
	}

	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(struct tcp_pcb *tpcb){
	err_t err;
	static int flag = 0;
	long int i = 0;

	err = tcp_write(tpcb, (u8*)testframe, send_buffer_size, 0x01);
	if(err != ERR_OK){
		xil_printf("Test failed. Connection State: %s\n", tcp_debug_state_str(tpcb->state));
		xil_printf("Non-Specified Write Error: %d\n", err);
		xil_printf("Current accum val: %d\n", i);
	}
	else{
		tcp_output(tpcb);
	}

	frame_send_ptr = testframe;
	frame_send_index = send_buffer_size;
}

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;
}