#include <stdio.h>
#include <string.h>
#include "xparameters.h"
#include "platform_config.h"
#include "xil_printf.h"
#include "lwip/sockets.h"
#include "netif/xadapter.h"
#include "lwipopts.h"
#include "xil_printf.h"
#include "VGA_zonderClock.h"
#include "xil_io.h"

/* FreeRTOS includes */
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"

#define THREAD_STACKSIZE 1024

// receive port for echo packets
u16_t echo_port = 7;

int main_thread();
void print_echo_app_header();
void process_echo_request(void *p);
void echo_application_thread();
void network_thread(void *p);
int main_thread();
void lwip_init();
static void prvRxTask( void*pvParameters);
static TaskHandle_t xRxTask;
static QueueHandle_t xQueue= NULL;
char HWstring[2048] = "\0";
int moveX = 0;

#define THREAD_STACKSIZE 1024

//Generic data structure used for all lwIP network interfaces.
static struct netif server_netif;
struct netif *echo_netif;


void process_echo_request(void *p)
{
    int sd = (int)p;
    int RECV_BUF_SIZE = 2048;
    char recv_buf[RECV_BUF_SIZE];
    int n, nwrote;

    while (1) {
        /* read a max of RECV_BUF_SIZE bytes from socket */
        if ((n = read(sd, recv_buf, RECV_BUF_SIZE)) < 0) {
            xil_printf("%s: error reading from socket %d, closing socket\r\n", __FUNCTION__, sd);
            break;
        }
        else
        {
            strncpy(HWstring, recv_buf, 1);
            xil_printf(HWstring);
            xQueueSend( xQueue,HWstring,0UL );
        }


        /* break if the received message = "quit" */
        if (!strncmp(recv_buf, "quit", 4))
            break;

        /* break if client closed connection */
        if (n <= 0)
            break;

        /* handle request */
        if ((nwrote = write(sd, recv_buf, n)) < 0) {
            xil_printf("%s: ERROR responding to client echo request. received = %d, written = %d\r\n",
                    __FUNCTION__, n, nwrote);
            xil_printf("Closing socket %d\r\n", sd);
            break;
        }


    }

    /* close connection */
    close(sd);
    vTaskDelete(NULL);
}

void echo_application_thread()
{
    xil_printf("-----in echo application thread ------\r\n");
    int sock, new_sd;
    struct sockaddr_in address, remote;
    int size;

    if ((sock = lwip_socket(AF_INET, SOCK_STREAM, 0)) < 0)
        return;

    address.sin_family = AF_INET;
    address.sin_port = htons(echo_port);
    address.sin_addr.s_addr = INADDR_ANY;

    if (lwip_bind(sock, (struct sockaddr *)&address, sizeof (address)) < 0)
        return;

    //  Set a socket into listen mode.
    lwip_listen(sock, 0);

    size = sizeof(remote);

    while (1) {
        if ((new_sd = lwip_accept(sock, (struct sockaddr *)&remote, (socklen_t *)&size)) > 0) {
            /*---------------------------------------------------------------------------*
             * Routine:  sys_thread_new
             *---------------------------------------------------------------------------*
             * Description:
             *      Starts a new thread with priority "prio" that will begin its
             *      execution in the function "thread()". The "arg" argument will be
             *      passed as an argument to the thread() function. The id of the new
             *      thread is returned. Both the id and the priority are system
             *      dependent.
             * Inputs:
             *      char *name              -- Name of thread
             *      void (* thread)(void *arg) -- Pointer to function to run.
             *      void *arg               -- Argument passed into function
             *      int stacksize           -- Required stack amount in bytes
             *      int prio                -- Thread priority
             * Outputs:
             *      sys_thread_t            -- Pointer to per-thread timeouts.
             *---------------------------------------------------------------------------*/
            sys_thread_new("echos", process_echo_request, (void*)new_sd,4096,DEFAULT_THREAD_PRIO);
        }
    }
}
static void prvRxTask( void*pvParameters)
{
char Recdstring[2048] = "";
    for( ;; )
    {
        xQueueReceive( xQueue,Recdstring,portMAX_DELAY);
        xil_printf( "Received string from UDP network: %d\r\n", Recdstring[0]);
        //VGA_AXI_mWriteReg(XPAR_VGA_AXI_0_S00_AXI_BASEADDR, 0, ((int)Recdstring[0]-48)<<28);
        if(strcmp(Recdstring, "o") == 0)
        {
            moveX = moveX + 50;
                xil_printf("%d\n\r", moveX);
            VGA_ZONDERCLOCK_mWriteReg(XPAR_VGA_ZONDERCLOCK_0_S_AXI_BASEADDR, VGA_ZONDERCLOCK_S_AXI_SLV_REG2_OFFSET, moveX);
            VGA_ZONDERCLOCK_mWriteReg(XPAR_VGA_ZONDERCLOCK_0_S_AXI_BASEADDR, 0, 0x7E0);         //nog geen limietenss
        }
        //VGA_ZONDERCLOCK_mWriteReg(XPAR_VGA_ZONDERCLOCK_0_S_AXI_BASEADDR, 0, Recdstring[0]);
        VGA_ZONDERCLOCK_mWriteReg(XPAR_VGA_ZONDERCLOCK_0_S_AXI_BASEADDR, 4, Recdstring[0]);
    }
}

int main()
{
    xil_printf("-----in Main ------\r\n");
    sys_thread_new("main_thrd", (void(*)(void*))main_thread, 0,THREAD_STACKSIZE,DEFAULT_THREAD_PRIO);
    xTaskCreate( prvRxTask, ( const char* ) "Rx", 2048, NULL, tskIDLE_PRIORITY, &xRxTask);
    xQueue= xQueueCreate(1,sizeof( HWstring) );
    configASSERT(xQueue);
    vTaskStartScheduler();
    while(1);
    return 0;
}

void network_thread(void *p)
{
    xil_printf("-----in network_thread ------\r\n");
    struct netif *netif;
    struct ip_addr ipaddr, netmask, gw;
    /* the mac address of the board. this should be unique per board */
    unsigned char mac_ethernet_address[] = { 0x00, 0x0a, 0x35, 0x00, 0x01, 0x02 };

    netif = &server_netif;

    /* initiliaze IP addresses to be used */
    IP4_ADDR(&ipaddr,  192, 168, 1, 10);
    IP4_ADDR(&netmask, 255, 255, 255,  0);
    IP4_ADDR(&gw,      192, 168, 1, 1);

    /* Add network interface to the netif_list, and set it as default */
    if (!xemac_add(netif, &ipaddr, &netmask, &gw, mac_ethernet_address, PLATFORM_EMAC_BASEADDR)) {
        xil_printf("Error adding N/W interface\r\n");
        return;
    }
    // Set a network interface as the default network interface
    netif_set_default(netif);

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

    /* start packet receive thread - required for lwIP operation */
    sys_thread_new("xemacif_input_thread", (void(*)(void*))xemacif_input_thread, netif, THREAD_STACKSIZE, DEFAULT_THREAD_PRIO);
    xil_printf("\r\n");
    sys_thread_new("echod", echo_application_thread, 0,THREAD_STACKSIZE, DEFAULT_THREAD_PRIO);
    vTaskDelete(NULL);

    return;
}

int main_thread()
{
    lwip_init();
    xil_printf("-----in Main_Thread ------\r\n");
    /* initialize lwIP before calling sys_thread_new */
    /* any thread using lwIP should be created using sys_thread_new */
    sys_thread_new("NW_THRD", network_thread, NULL,THREAD_STACKSIZE,DEFAULT_THREAD_PRIO);
    vTaskDelete(NULL);
    return 0;
}