#include "netif/xadapter.h"
#include "lwipopts.h"
#include "lwip/init.h"
#include "lwip/inet.h"
#include "lwip/tcp.h"
#include "lwip/ip_addr.h"
#include "lwip/sockets.h"
#include "lwip/sys.h"

#include "project.h"
#include "radar_manager_icd.h"
#include "novatel.h"

#define DEFAULT_IP_ADDRESS "192.168.1.200"
#define DEFAULT_GW_ADDRESS "192.168.1.1"
#define DEFAULT_IP_MASK "255.255.255.0"
#define TCP_CONN_PORT 5001

static struct netif server_netif;
int curr_sd;
int new_sd;
static SemaphoreHandle_t xMutex;
static QueueHandle_t valid_client;
static unsigned char msgBuffer[MAX_MSG_LENGTH];
#define RECV_BUF_SIZE 1024
unsigned char recv_buf[RECV_BUF_SIZE+2];


void print_ip(char *msg, ip_addr_t *ip)
{
	DEBUG_PRINT(msg);
	DEBUG_PRINT("%d.%d.%d.%d\n\r", ip4_addr1(ip), ip4_addr2(ip),
			ip4_addr3(ip), ip4_addr4(ip));
}

void print_ip_settings(ip_addr_t *ip, ip_addr_t *mask, ip_addr_t *gw, u16 port)
{

	print_ip("Board IP: ", ip);
	print_ip("Netmask : ", mask);
	print_ip("Gateway : ", gw);
	DEBUG_PRINT("Port    : %d\n\r", htons(port));
}

static void assign_default_ip(ip_addr_t *ip, ip_addr_t *mask, ip_addr_t *gw)
{
	int err;

	DEBUG_PRINT("Configuring default IP %s \r\n", DEFAULT_IP_ADDRESS);

	err = inet_aton(DEFAULT_IP_ADDRESS, ip);
	if(!err)
		DEBUG_PRINT("Invalid default IP address: %d\r\n", err);

	err = inet_aton(DEFAULT_IP_MASK, mask);
	if(!err)
		DEBUG_PRINT("Invalid default IP MASK: %d\r\n", err);

	err = inet_aton(DEFAULT_GW_ADDRESS, gw);
	if(!err)
		DEBUG_PRINT("Invalid default gateway address: %d\r\n", err);
}

void eth_sendMessage(u8 * data, int num_bytes)
{
	// Need a mutex to make this safe to call from any task, currently this is
	// setup to block forever until the mutex is available
    if( xSemaphoreTake( xMutex, portMAX_DELAY ) )
    {
    	int sent_count = 0;

		while (sent_count < num_bytes)
		{
			sent_count += send(curr_sd, data, num_bytes, 0);
		}

    	// Release the mutex
    	xSemaphoreGive( xMutex );
    }
    else
    {
    	// Could not get mutex
    }
}

void eth_rx_task()
{
	int n;
	int header_found = 0;
	int msg_ind = 0;

	while (1)
	{
		// Need to wait until we have a valid socket to interact with
		DEBUG_PRINT("RX Thread - Waiting for client\r\n");
		xQueueReceive( valid_client, &curr_sd, portMAX_DELAY);
		DEBUG_PRINT("RX Thread - Got Client %d\r\n", curr_sd);

		while (1)
		{

			struct pollfd poll_fd;
			poll_fd.fd = curr_sd;
			poll_fd.events = POLLIN;
			int poll_ret = lwip_poll(&poll_fd, 1, 1000);

			if (poll_ret == 0)
			{
				// This was a timeout, see if there is a new client available
				int temp_sd;
				if (xQueueReceive( valid_client, &temp_sd, 0) == pdTRUE)
				{
					// Close the old socket
					close(curr_sd);
					header_found = 0;
					msg_ind = 0;
					// Get the new socket
					curr_sd = temp_sd;

					DEBUG_PRINT("RX Thread - Got Client Poll %d\r\n", curr_sd);
				}

			}
			else if (poll_ret > 0)
			{
				// Socket is ready
				n = read(curr_sd, recv_buf, RECV_BUF_SIZE);

				/* break if client closed connection or error */
				if (n == 0)
				{
					DEBUG_PRINT("RX Thread - Client Closed Socket, closing socket %d\r\n", curr_sd);
					close(curr_sd);
					header_found = 0;
					msg_ind = 0;
					break;
				}
				else if (n < 0)
				{
					// Some other error
					DEBUG_PRINT("RX Thread - Socket Error %d, closing socket %d\r\n", n, curr_sd);
					close(curr_sd);
					break;
				}

                // Send data to radar manager (radar_manager_get_message)
				radar_manager_get_message(recv_buf, n, msgBuffer, &msg_ind, &header_found);
			}
		}
	}
}


void tcp_server_task()
{
	valid_client = xQueueCreate( 1, 4 );

	// Mutex for thread saftey for send task
	xMutex = xSemaphoreCreateMutex();

	/* initialize lwIP before calling sys_thread_new */
    lwip_init();

	/* the mac address of the board. this should be unique per board */
	u16 ip_port = htons(TCP_CONN_PORT);
	u8_t mac_ethernet_address[] = { 0x00, 0x0a, 0x35, 0x00, 0x01, 0x02 };

	DEBUG_PRINT("\n\r\n\r");
	DEBUG_PRINT("-----lwIP Socket Mode TCP Server Application------\r\n");
	DEBUG_PRINT("Ethernet MAC: %02X:%02X:%02X:%02X:%02X:%02X\n\r",
			mac_ethernet_address[0],
			mac_ethernet_address[1],
			mac_ethernet_address[2],
			mac_ethernet_address[3],
			mac_ethernet_address[4],
			mac_ethernet_address[5]);

	/* Add network interface to the netif_list, and set it as default */
	if (!xemac_add(&server_netif, NULL, NULL, NULL, mac_ethernet_address,
			XPAR_AXI_ETHERNET_0_BASEADDR)) {
		DEBUG_PRINT("Error adding N/W interface\r\n");
		return;
	}

	netif_set_default(&server_netif);

	/* specify that the network if is up */
	netif_set_up(&server_netif);

	/* start packet receive thread - required for lwIP operation */
	sys_thread_new("xemacif_input_thread",
			(void(*)(void*))xemacif_input_thread, &server_netif,
			1024, DEFAULT_THREAD_PRIO);

	assign_default_ip(&(server_netif.ip_addr), &(server_netif.netmask),	&(server_netif.gw));
	print_ip_settings(&(server_netif.ip_addr), &(server_netif.netmask), &(server_netif.gw), ip_port);


	int sock;
	int size;

	struct sockaddr_in address, remote;

	memset(&address, 0, sizeof(address));

	if ((sock = lwip_socket(AF_INET, SOCK_STREAM, 0)) < 0) {
		DEBUG_PRINT("TCP server: Error creating Socket\r\n");
		return;
	}
	address.sin_family = AF_INET;
	address.sin_port = ip_port;
	address.sin_addr.s_addr = INADDR_ANY;

	if (bind(sock, (struct sockaddr *)&address, sizeof (address)) < 0) {
		DEBUG_PRINT("TCP server: Unable to bind to port %d\r\n",
				htons(ip_port));
		close(sock);
		return;
	}

	if (listen(sock, 1) < 0) {
		DEBUG_PRINT("TCP server: tcp_listen failed\r\n");
		close(sock);
		return;
	}

	size = sizeof(remote);

	sys_thread_new("eth_rx", eth_rx_task,
		NULL,
		4096,
		TASK_PRIORITY_ETH_RX);

	sys_thread_new("novatel", novatel_task,
		NULL,
		4096,
		TASK_PRIORITY_ETH_RX);

	while (1)
	{
		// Wait for a client to connect, currently only support a single connected client.  When a new client connects
		// the old connection is dropped
		DEBUG_PRINT("Server Listening\r\n");
		new_sd = lwip_accept(sock, (struct sockaddr *)&remote, (socklen_t *)&size);
		DEBUG_PRINT("Server Accept %d from %s:%d\r\n", new_sd, inet_ntoa(remote.sin_addr), htons(remote.sin_port));
		if (new_sd > 0)
		{
			// Set socket options
			int flags = 1;
			int ret = setsockopt(new_sd, IPPROTO_TCP, TCP_NODELAY, (void *)&flags, sizeof(flags));

			// Notify the RX task a new connection is available
			xQueueSend( valid_client, &new_sd, 0 );
			DEBUG_PRINT("Client Connected! %d, %d\r\n", new_sd, ret);
		}
	}
}
