#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"
#include "queue.h"
#include <errno.h>

#include "common_config.h"

#include "lwip/sockets.h"
#include "lwip/inet.h"

#include "settings_api.h"
#include "portgw.h"
#include "tinystdio.h"
#include "usart.h"
#ifdef PRINTF_STDLIB
#include <stdio.h>
#endif
#ifdef PRINTF_CUSTOM
#include "tinystdio.h"
#endif

#include <string.h>

#ifdef PORTGW_ENABLE


#define PORTGW_PORT_NUM         1001
#define PORTGW_DATA_CHUNK_SIZE  512

/* Set option to drop old connection if the new one is accepted */
#define TCP_DROP_PREV_CONNECTION    1

static int listen_sd;
static struct sockaddr_in sa;
static struct fd_set master_set, read_set, write_set;
static int max_sd;

static uint8_t data_buffer[PORTGW_DATA_CHUNK_SIZE];

#undef DBG
#define DBG if(0)


/* (Re)init serial port */ 
void serial_reinit()
{
    uint16_t parity, stop, wordlen;

    /* Parity set */
    switch (sSettings.sPortGw.parity) {
        case GW_EVEN_PAR:
            parity = USART_Parity_Even;
            break;
        case GW_ODD_PAR:
            parity = USART_Parity_Odd;
            break;
        case GW_NO_PAR:
        default:
            parity = USART_Parity_No;
            break;
    }
    /* Stop bits set */
    switch (sSettings.sPortGw.stopbits) {
        case 2:
            stop = USART_StopBits_2;
            break;
        case 1:
        default:
            stop = USART_StopBits_1;
            break;
    }
    /* Word length set */
    switch (sSettings.sPortGw.databits) {
        case 8:
            if ((sSettings.sPortGw.parity == GW_EVEN_PAR) ||
                (sSettings.sPortGw.parity == GW_ODD_PAR)) {
                /* 8-bit data + Parity */
                wordlen = 9;
            } else {
                wordlen = 8;
            }
            break;
        case 7:
        default:
            /* 7-bit data + Parity */
            wordlen = 8;
            break;
    }
    uart_config_reinit(RS485_USART, sSettings.sPortGw.baud, wordlen, parity, stop);
}

/* Stop server */
static void stop_server(void) {
    /* Clean up all of the sockets that are open */
    for (int i = 0; i <= max_sd; ++i)
    {
        if (FD_ISSET(i, &master_set)) {
            DBG printf("Close sock %d\n", i);
            closesocket(i);
            FD_CLR(i, &master_set);
        }
    }
    DBG printf("Portgw stopped\n");
}

/* Start TCP server */
static bool start_tcp_server(uint16_t port)
{
    int res;

    listen_sd = socket(PF_INET, SOCK_STREAM, 0);

    if (listen_sd < 0) {
        DBG printf("Socket create failed\r\n");
        return false;
    }

    res = fcntl(listen_sd, F_SETFL, O_NONBLOCK);
    if (res < 0) {
        DBG printf("fcntl() failed");
        closesocket(listen_sd);
        return false;
    }

#if 0
    /* Not used */
    lwip_setsockopt(listen_sd, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));
    if (res < 0) {
        printf("setsockopt() failed");
        closesocket(listen_sd);
        return false;
    }

    /* Not used (SO_REUSE==1 should be enabled) */
    int on = 1;
    res = setsockopt(listen_sd, SOL_SOCKET,  SO_REUSEADDR, (char *)&on, sizeof(on));
    if (res < 0) {
        printf("setsockopt() failed");
        closesocket(listen_sd);
        return false;
    }
#endif

    memset(&sa, 0, sizeof(struct sockaddr_in));
    sa.sin_family = AF_INET;
    sa.sin_addr.s_addr = IPADDR_ANY;
    sa.sin_port = htons(port);

    if (bind(listen_sd, (struct sockaddr *)&sa, sizeof(sa)) == -1)
    {
        DBG printf("Bind to port %d failed\n", port);
        closesocket(listen_sd);
        return false;
    }

    res = listen(listen_sd, 1);
    if (res < 0) {
        DBG printf("Listen failed failed\r\n");
        closesocket(listen_sd);
        return false;
    }

    FD_ZERO(&master_set);
    max_sd = listen_sd;
    FD_SET(listen_sd, &master_set);

    printf("Port %d opened\n", port);

    return true;
}

/* Start UDP server */
static bool start_udp_server(uint16_t port)
{
    int res;

    listen_sd = socket(PF_INET, SOCK_DGRAM, 0);

    if (listen_sd < 0) {
        printf("Socket create failed\r\n");
        return false;
    }
    else {
        printf("Socket create OK\r\n");
    }

    res = fcntl(listen_sd, F_SETFL, O_NONBLOCK);
    if (res < 0) {
        DBG printf("fcntl() failed");
        closesocket(listen_sd);
        return false;
    }

#if 0
    /* Not used */
    lwip_setsockopt(listen_sd, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));
    if (res < 0) {
        printf("setsockopt() failed");
        closesocket(listen_sd);
        return false;
    }

    /* Not used (SO_REUSE==1 should be enabled) */
    int on = 1;
    res = setsockopt(listen_sd, SOL_SOCKET,  SO_REUSEADDR, (char *)&on, sizeof(on));
    if (res < 0) {
        printf("setsockopt() failed");
        closesocket(listen_sd);
        return false;
    }
#endif

    memset(&sa, 0, sizeof(struct sockaddr_in));
    sa.sin_family = AF_INET;
    sa.sin_addr.s_addr = IPADDR_ANY;
    sa.sin_port = htons(port);

    if (bind(listen_sd, (struct sockaddr *)&sa, sizeof(sa)) == -1)
    {
        DBG printf("Bind to port %d failed\n", port);
        closesocket(listen_sd);
        return false;
    }

    FD_ZERO(&master_set);
    max_sd = listen_sd;
    FD_SET(listen_sd, &master_set);

    printf("Port %d opened\n", port);

    return true;
}

/* Start server */
static bool start_server(gwtrans_t transtype, uint16_t port)
{
    switch (transtype) {
        case GW_TCP:
            return start_tcp_server(port);
        case GW_UDP:
            return start_udp_server(port);
        default:
            return false;
    }
}

/* TCP server main loop */
static void tcp_server(void)
{
    int recvd, sent;
    uint8_t c;
    int new_sd;
    static int active_sd = -1;
    int desc_ready, rc;
    struct timeval timeout;
    bool close_conn;

    timeout.tv_sec = 5;
    timeout.tv_usec = 0;

    memcpy(&read_set, &master_set, sizeof(master_set));
    memcpy(&write_set, &master_set, sizeof(master_set));

    DBG printf("Waiting on select()...\n");
    rc = select(max_sd + 1, &read_set, &write_set, NULL, &timeout);

    DBG printf("  select() returned %d\n", rc);

    if (rc < 0) {
        DBG printf("  select() failed\n");
    }

    if (rc == 0) {
        DBG printf("  select() timed out.\n");
    }

    /* One or more descriptors are readable.  Need to         \
        * determine which ones they are.                         */
    desc_ready = rc;
    for (int i=0; i <= max_sd  &&  desc_ready > 0; ++i) {
        /*******************************************************/
        /* Check to see if this descriptor is ready            */
        /*******************************************************/
        if (FD_ISSET(i, &read_set)) {
            /* A descriptor was found that was readable - one  \
            * less has to be looked for.  This is being done   \
            * so that we can stop looking at the working set   \
            * once we have found all of the descriptors that   \
            * were ready.                                      */
            desc_ready -= 1;

            /* Check to see if this is the listening socket     */
            if (i == listen_sd) {
                DBG printf("  Listening socket is readable\n");

                /* Accept all incoming connections that are      */
                /* queued up on the listening socket before we   */
                /* loop back and call select again.              */
                do {

                    /* Accept each incoming connection.  If       */
                    /* accept fails with EWOULDBLOCK, then we     */
                    /* have accepted all of them.  Any other      */
                    /* failure on accept will cause us to end the */
                    /* server.                                    */
                    new_sd = accept(listen_sd, NULL, NULL);
                    if (new_sd < 0 && errno != 0) {
                        if (errno != EWOULDBLOCK) {
                            DBG printf("  accept() failed: %d\n", errno);
                        }
                        break;
                    }

                    /* Add the new incoming connection to the     */
                    /* master read set                            */
                    DBG printf("  New incoming connection - %d\n", new_sd);
                    FD_SET(new_sd, &master_set);
                    if (new_sd > max_sd) {
                        max_sd = new_sd;
                    }

#if TCP_DROP_PREV_CONNECTION
                    /* Close previous active connection */
                    if (active_sd != -1 && active_sd != new_sd) {
                        DBG printf("  Close prev active connection %d\n", active_sd);
                        close(active_sd);
                        FD_CLR(active_sd, &master_set);
                        if (active_sd == max_sd) {
                            while (FD_ISSET(max_sd, &master_set) == false) {
                                max_sd -= 1;
                            }
                        }
                    }
                    /* Mark new connection as active */
                    active_sd = new_sd;
                    DBG printf("  New active connection %d\n", active_sd);
#endif

                    /* Loop back up and accept another incoming   */
                    /* connection                                 */
                } while (new_sd != -1);
            }

            /* This is not the listening socket, therefore an   */
            /* existing connection must be readable             */
            else {
                DBG printf("  Descriptor %d is readable\n", i);
                close_conn = false;

                /* Receive data on this connection until the  */
                /* recv fails with EWOULDBLOCK.  If any other */
                /* failure occurs, we will close the          */
                /* connection.                                */
                if ((recvd = recv(i, data_buffer, sizeof(data_buffer), 0)) > 0) {
                    DBG {
                        printf("received %d bytes:", recvd);
                        for (uint32_t i = 0; i < 3; i++) {
                            printf(" 0x%X", data_buffer[i]);
                        }
                        printf("...\r\n");
                    }
                    /* Put RX data to the queue */
                    for (uint32_t i = 0; i < (uint32_t)recvd; i++) {
                        if (xQueueSend(rs485TxQ, &data_buffer[i], 1000) != pdTRUE) {
                            printf("[portgw] Rx data lost\r\n");
                        }
                    }
                    /* Start RS485 transmission */
                    rs485_enable_tx();
                }

                if (recvd < 0) {
                    if (errno != EWOULDBLOCK){
                        DBG printf("  recv() failed: %d\n", errno);
                        close_conn = true;
                    }
                }

                /* Check to see if the connection has been    */
                /* closed by the client                       */
                if (recvd == 0) {
                    DBG printf("  Connection closed\n");
                    close_conn = true;
                }

                /* If the close_conn flag was turned on, we need */
                /* to clean up this active connection.  This     */
                /* clean up process includes removing the        */
                /* descriptor from the master set and            */
                /* determining the new maximum descriptor value  */
                /* based on the bits that are still turned on in */
                /* the master set.                               */
                if (close_conn) {
                    closesocket(i);
                    FD_CLR(i, &master_set);
                    if (i == max_sd) {
                        while (FD_ISSET(max_sd, &master_set) == false) {
                            max_sd -= 1;
                        }
                    }
                }
            }
        }

        /* Socket is ready to write */
        else if (FD_ISSET(i, &write_set)){
            /* Put RX data to the queue */
            uint32_t len = 0;
            while (len < sizeof(data_buffer) && xQueueReceive(rs485RxQ, &c, 5) == pdTRUE) {
                data_buffer[len++] = c;
            }
            if (len > 0) {
                DBG printf("%d bytes to send\r\n", len);
                sent = send(i, data_buffer, len, 0);
                if (sent <= 0) {
                    DBG printf("send failed (%d)\n", sent);
                }
            }
        }
    }
}

/* UDP server main loop */
static void udp_server(void)
{
    struct timeval timeout;
    int recvd, sent;
    uint8_t c;
    int rc;
    bool close_conn;

    timeout.tv_sec = 5;
    timeout.tv_usec = 0;

    memcpy(&read_set, &master_set, sizeof(master_set));
    memcpy(&write_set, &master_set, sizeof(master_set));

    /* TODO Remove select for UDP */
    DBG printf("Waiting on select()...\n");
    rc = select(max_sd + 1, &read_set, &write_set, NULL, &timeout);

    DBG printf("  select() returned %d\n", rc);

    if (rc < 0) {
        DBG printf("  select() failed\n");
    }

    if (rc == 0) {
        DBG printf("  select() timed out.\n");
    }
    else{
        /*******************************************************/
        /* Check to see if this descriptor is ready            */
        /*******************************************************/
        if (FD_ISSET(listen_sd, &read_set)) {
            DBG printf("  Descriptor %d is readable\n", listen_sd);
            close_conn = false;

            /* Receive data on this connection until the  */
            /* recv fails with EWOULDBLOCK.  If any other */
            /* failure occurs, we will close the          */
            /* connection.
                */
            socklen_t from_len;
            /* Save addr & port where data (requests) come from */
            if ((recvd = recvfrom(listen_sd, data_buffer, sizeof(data_buffer), 0, 
                (struct sockaddr*)&sa, &from_len)) > 0) {
                DBG {
                    printf("From %s:%d received %d bytes:", 
                        inet_ntoa(sa.sin_addr.s_addr), sa.sin_port, recvd);
                    for (uint32_t i = 0; i < 3; i++) {
                        printf(" 0x%X", data_buffer[i]);
                    }
                    printf("...\r\n");
                }
                /* Put RX data to the queue */
                for (uint32_t i = 0; i < (uint32_t)recvd; i++) {
                    if (xQueueSend(rs485TxQ, &data_buffer[i], 1000) != pdTRUE) {
                        printf("[portgw] Rx data lost\r\n");
                    }
                }
                /* Start RS485 transmission */
                rs485_enable_tx();
            }

            if (recvd < 0) {
                if (errno != EWOULDBLOCK){
                    DBG printf("  recv() failed\n");
                    close_conn = true;
                }
            }

            /* Check to see if the connection has been    */
            /* closed by the client                       */
            if (recvd == 0) {
                DBG printf("  Connection closed\n");
                close_conn = true;
            }

            /* If the close_conn flag was turned on, we need */
            /* to clean up this active connection.  This     */
            /* clean up process includes removing the        */
            /* descriptor from the master set and            */
            /* determining the new maximum descriptor value  */
            /* based on the bits that are still turned on in */
            /* the master set.                               */
            if (close_conn) {
                closesocket(listen_sd);
                FD_CLR(listen_sd, &master_set);
            }
        }

        /* Socket is ready to write */
        else if (FD_ISSET(listen_sd, &write_set)) {
            uint32_t len = 0;
            while (len < sizeof(data_buffer) && xQueueReceive(rs485RxQ, &c, 5) == pdTRUE) {
                data_buffer[len++] = c;
            }
            if (len > 0) {
                DBG printf("%d bytes to send\r\n", len);
                /* Send data (responce) to the latest saved addr & port */
                sent = sendto(listen_sd, data_buffer, len, 0, (struct sockaddr*)&sa, sizeof(sa));
                if (sent <= 0) {
                    DBG printf("send failed (%d)\n", sent);
                }
            }
        } else {
            /* Nothing to do */
            taskYIELD();
        }
    }
}

/* Main task */
void portgw_thread(void *arg)
{
    uint16_t port;
    gwtrans_t transtype;
    gwmode_t mode;
    bool enabled;
    gwparity_t parity;
    uint8_t stopbits;
    uint8_t databits;
    uint32_t baud;
    bool firstrun = true;
    (void)arg;

    FD_ZERO(&master_set);

    enabled = sSettings.sPortGw.enabled;
    port = sSettings.sPortGw.port;
    transtype = sSettings.sPortGw.transtype;
    mode = sSettings.sPortGw.mode;

    parity = sSettings.sPortGw.parity;
    stopbits = sSettings.sPortGw.stopbits;
    databits = sSettings.sPortGw.databits;
    baud = sSettings.sPortGw.baud;

    (void)mode;

    while (1) {
        /* Check if network settings was changed */
        if ((sSettings.sPortGw.port != port) ||
            (sSettings.sPortGw.enabled != enabled) ||
            (sSettings.sPortGw.transtype != transtype) ||
            (firstrun))
        {
            port = sSettings.sPortGw.port;
            enabled = sSettings.sPortGw.enabled;
            transtype = sSettings.sPortGw.transtype;

            if (!firstrun) {
                /* Stop server */
                stop_server();
            }

            if (enabled) {
                /* (Re)start server */
                if (!start_server(transtype, port)) {
                    DBG printf("Server start error\n");
                    vTaskDelay(5000);
                    continue;
                }
            }
            else {
                /* Clear buffers */
                if (uxQueueMessagesWaiting(rs485TxQ) > 0) {
                    xQueueReset(rs485TxQ);
                }
                if (uxQueueMessagesWaiting(rs485RxQ) > 0) {
                    xQueueReset(rs485RxQ);
                }
                vTaskDelay(5000);
                continue;
            }
        }

        /* Check if serial settings was changed */
        if ((sSettings.sPortGw.parity != parity) ||
            (sSettings.sPortGw.stopbits != stopbits) ||
            (sSettings.sPortGw.databits != databits) ||
            (sSettings.sPortGw.baud != baud) ||
            (firstrun))
        {
            parity = sSettings.sPortGw.parity;
            stopbits = sSettings.sPortGw.stopbits;
            databits = sSettings.sPortGw.databits;
            baud = sSettings.sPortGw.baud;

            serial_reinit();
            DBG printf("Serial reinitialized\r\n");
        }

        if (!enabled) {
            vTaskDelay(5000);
            continue;
        }

        switch (transtype) {
            case GW_UDP:
                udp_server();
                break;
            case GW_TCP:
                tcp_server();
                break;
            default:
                vTaskDelay(5000);
                break;
        }
        firstrun = false;
    }
}

/* Main init */
void portgw_init(void)
{
    xTaskCreate(portgw_thread, ( char * ) "portgw_thr", configMINIMAL_STACK_SIZE * 2,
                NULL, tskIDLE_PRIORITY, NULL);
}

#endif