/*
 * Copyright © 2008-2014 Stéphane Raimbault <stephane.raimbault@gmail.com>
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <stdlib.h>
#include <errno.h>
#include <signal.h>

#include <sys/select.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

#include "dido-modbus.h"

#include <getopt.h>

static volatile int mb_looping;
static void close_sigint(int dummy)
{
    mb_looping = 0;
}

static void * gpio_working_thread(void * _dido)
{
    struct dido_modbus * dido;
    modbus_mapping_t * mbmap;
    struct lnxall_gpio * gpios;
    static int count_array[WOOLINK_DI_MAX] = {0};                   /* 16 GPIO input Pins: DI */
    static int last_gpio_val_array[WOOLINK_DI_MAX] = {0};           /* 16 GPIO input Pins: DI */
    static int start_continue_sample_array[WOOLINK_DI_MAX] = {0};   /* 16 GPIO input Pins: DI */

    dido = (struct dido_modbus *) _dido;
    mbmap = dido->dm_map;
    gpios = dido->dm_gpios;
    while (mb_looping) {
        int rval, idx;
        struct timespec tspec;
        struct lnxall_gpio * gpio;

        rval = 0;
        tspec.tv_sec = 0;
        tspec.tv_nsec = 100 * 1000000;
        nanosleep(&tspec, NULL);

        rval = pthread_mutex_lock(&dido->dm_lock);
        if (rval != 0) {
            fprintf(stderr, "Error, failed to lock mutex: %d\n", rval);
            fflush(stderr);
            break;
        }

        for (idx = 0; idx < dido->dm_num_input; ++idx) {
            int offs;
            gpio = &gpios[idx + dido->dm_num_output];
            lnxall_gpio_getval(gpio, 1);
            if (dido->dm_verbose != 0) {
                fprintf(stdout, "GPIO input at index %d has changed: %d\n",
                    idx + 1, gpio->gpio_val);
                fflush(stdout);
            }

            offs = idx + WOOLINK_DI_OFFSET;
            /* update input pins */
            if (last_gpio_val_array[idx] != gpio->gpio_val) {
                last_gpio_val_array[idx] = gpio->gpio_val;
                start_continue_sample_array[idx] = 1;
                count_array[idx] = 0;
            } else {
                if (start_continue_sample_array[idx]) {
                    count_array[idx]++;
                    if(count_array[idx] >= 5) {
                        mbmap->tab_bits[offs] = gpio->gpio_val ? 0x1 : 0x0;
                        mbmap->tab_registers[offs] = gpio->gpio_val ? 0x1 : 0x0;
                        last_gpio_val_array[idx] = gpio->gpio_val;
                        count_array[idx] = 0;
                        start_continue_sample_array[idx] = 0;
                    }
                }
            }
        }
        rval = pthread_mutex_unlock(&dido->dm_lock);
        if (rval != 0) {
            fprintf(stderr, "Error, failed to unlock mutex: %d\n", rval);
            fflush(stderr);
            break;
        }
    }

    fputs("GPIO working thread will now exit!\n", stdout);
    fflush(stdout);
    return NULL;
}

static void dump_hexmsg(const void * msgp, size_t msglen, size_t elsize)
{
    size_t idx, jdx;
    char hexline[128];
    const size_t mask_val = 0x0f;

    if (elsize == 1) {
        const unsigned char * pmsg;
        jdx = 0;
        pmsg = (const unsigned char *) msgp;
        for (idx = 0; idx < msglen; ++idx) {
            int endc;

            if ((idx & mask_val) == 0)
                fprintf(stdout, "[%#08x]: ", (unsigned int) idx);
            endc = ((idx + 1) & mask_val) ? ' ' : '\n';
            if ((idx + 1) >= msglen)
                endc = '\n';
            jdx += snprintf(&hexline[jdx], sizeof(hexline) - jdx, "%02x%c",
                (unsigned int) *pmsg++, endc);
            if (endc == '\n') {
                jdx = 0;
                fputs(hexline, stdout);
            }
        }
    } else if (elsize == 2) {
        const unsigned short * pmsg;
        jdx = 0;
        pmsg = (const unsigned short *) msgp;
        for (idx = 0; idx < msglen; ++idx) {
            int endc;
            unsigned short pval = *pmsg++;

            /* pval = __builtin_bswap16(pval); */
            if ((idx & mask_val) == 0)
                fprintf(stdout, "[%#08x]: ", (unsigned int) idx);
            endc = ((idx + 1) & mask_val) ? ' ' : '\n';
            if ((idx + 1) >= msglen)
                endc = '\n';
            jdx += snprintf(&hexline[jdx], sizeof(hexline) - jdx, "%04x%c",
                (unsigned int) pval, endc);
            if (endc == '\n') {
                jdx = 0;
                fputs(hexline, stdout);
            }
        }
    }
    fflush(stdout);
}

static void dump_mappings(const modbus_mapping_t * mbmap)
{
    fprintf(stdout, "dumping mappings: =>\n"
        "\tnb_bits:                 %d\n"
        "\tstart_bits:              %d\n"
        "\tnb_input_bits:           %d\n"
        "\tstart_input_bits:        %d\n"
        "\tnb_input_registers:      %d\n"
        "\tstart_input_registers:   %d\n"
        "\tnb_registers:            %d\n"
        "\tstart_registers:         %d\n",
        mbmap->nb_bits,
        mbmap->start_bits,
        mbmap->nb_input_bits,
        mbmap->start_input_bits,
        mbmap->nb_input_registers,
        mbmap->start_input_registers,
        mbmap->nb_registers,
        mbmap->start_registers);

    if (mbmap->tab_bits != NULL) {
        fprintf(stdout, "tab_bits: %d, %d =>\n",
            mbmap->nb_bits, mbmap->start_bits);
        dump_hexmsg(mbmap->tab_bits, mbmap->nb_bits, 0x1);
    } else {
        fputs("tab_bits: NULL\n", stdout);
    }

    if (mbmap->tab_input_bits != NULL) {
        fprintf(stdout, "tab_input_bits: %d, %d =>\n",
            mbmap->nb_input_bits, mbmap->start_input_bits);
        dump_hexmsg(mbmap->tab_input_bits, mbmap->nb_input_bits, 0x1);
    } else {
        fputs("tab_input_bits: NULL\n", stdout);
    }

    if (mbmap->tab_input_registers != NULL) {
        fprintf(stdout, "tab_input_registers: %d, %d =>\n",
            mbmap->nb_input_registers, mbmap->start_input_registers);
        dump_hexmsg(mbmap->tab_input_registers,
            mbmap->nb_input_registers, sizeof(uint16_t));
    } else {
        fputs("tab_input_registers: NULL\n", stdout);
    }

    if (mbmap->tab_registers != NULL) {
        fprintf(stdout, "tab_registers: %d, %d =>\n",
            mbmap->nb_registers, mbmap->start_registers);
        dump_hexmsg(mbmap->tab_registers,
            mbmap->nb_registers, sizeof(uint16_t));
    } else {
        fputs("tab_registers: NULL\n", stdout);
    }
    fflush(stdout);
}

static int mbmsg_get_offset(const uint8_t * pqmsg, int headlen, int pgsize)
{
    unsigned int rval;

    if (pgsize < (headlen + 0x3)) {
        fprintf(stderr, "Error, invalid ModBus headlen: %d, total: %d\n",
            headlen, pgsize);
        fflush(stderr);
        return -1;
    }

    rval = (unsigned int) pqmsg[headlen + 1];
    rval = (rval << 0x8) | ((unsigned int) pqmsg[headlen + 2]);
    return (int) rval;
}

static int mbmsg_get_datalen(const uint8_t * pqmsg, int headlen, int pgsize)
{
    unsigned int rval;

    if (pgsize < (headlen + 0x5)) {
        fprintf(stderr, "Error, invalid ModBus datalen: %d, total: %d\n",
            headlen, pgsize);
        fflush(stderr);
        return -1;
    }

    rval = (unsigned int) pqmsg[headlen + 3];
    rval = (rval << 0x8) | ((unsigned int) pqmsg[headlen + 4]);
    return (int) rval;
}

static int mbmsg_get_single_coil(const uint8_t * pqmsg, int headlen, int pgsize)
{
    int rval;

    rval = mbmsg_get_datalen(pqmsg, headlen, pgsize);
    if (rval < 0)
        return -1;
    if (rval == 0xff00)
        return 1;
    return 0;
}

static void handle_mbmsg(struct dido_modbus * dido, uint8_t * pquery, int qsize)
{
    int ret;
    int hlen, offs, dlen;
    unsigned char optcmd;

    modbus_t * mbctx;
    modbus_mapping_t * mbmap;
    struct lnxall_gpio * gpios;

    mbctx = dido->dm_ctx;
    mbmap = dido->dm_map;
    gpios = dido->dm_gpios;
    hlen = modbus_get_header_length(mbctx);
    if (hlen < 0 || hlen >= qsize) {
        fprintf(stderr, "Error, invalid modbus header length: %d (%d)\n",
            hlen, qsize);
        fflush(stderr);
        return;
    }

    offs = mbmsg_get_offset(pquery, hlen, qsize);
    dlen = mbmsg_get_datalen(pquery, hlen, qsize);
    if (offs < 0 || dlen < 0)
        return;

    ret = pthread_mutex_lock(&dido->dm_lock);
    if (ret != 0) {
        fprintf(stderr, "Error, failed to lock mutex: %d\n", ret);
        fflush(stderr);
        return;
    }

    optcmd = pquery[hlen];
    switch (optcmd) {
    case MODBUS_FC_READ_COILS:
        break;

    case MODBUS_FC_READ_DISCRETE_INPUTS:
        break;

    case MODBUS_FC_READ_HOLDING_REGISTERS:
        break;

    case MODBUS_FC_READ_INPUT_REGISTERS:
        break;

    case MODBUS_FC_WRITE_SINGLE_COIL:
    {
        int coil;
        coil = mbmsg_get_single_coil(pquery, hlen, qsize);
        if (coil < 0)
            break;
        if (offs >= WOOLINK_DO_MAX || mbmap->nb_bits == 0) {
            fprintf(stderr, "Error, invalid offset for single coil write: (%d, %d, %d)\n",
                offs, mbmap->start_bits, mbmap->nb_bits);
            fflush(stderr);
            break;
        }
        mbmap->tab_bits[offs] = coil;
        mbmap->tab_registers[offs] = coil;
        if (offs < dido->dm_num_output)
            lnxall_gpio_setval(&gpios[offs], coil);
        break;
    }

    case MODBUS_FC_WRITE_SINGLE_REGISTER:
        break;

    case MODBUS_FC_READ_EXCEPTION_STATUS:
        break;

    case MODBUS_FC_WRITE_MULTIPLE_COILS:
    {
        unsigned char coilval;
        int num_bits, idx, jdx, nbit;
        num_bits = dlen;
        if (num_bits == 0 || (offs + num_bits) > WOOLINK_DO_MAX) {
            fprintf(stderr, "Error, invalid number of coils: %d\n", num_bits);
            fflush(stderr);
            break;
        }

        nbit = 0;
        jdx = hlen + 0x6;
        while (nbit < num_bits) {
            int nbits = num_bits - nbit;
            if (nbits > 0x8)
                nbits = 0x8;
            coilval = pquery[jdx];
            for (idx = 0; idx < nbits; ++idx) {
                int gval = (coilval & (0x1 << idx)) != 0;
                int noffs = offs + idx;
                mbmap->tab_bits[noffs] = gval;
                mbmap->tab_registers[noffs] = gval;
                if (noffs < dido->dm_num_output)
                    lnxall_gpio_setval(&gpios[noffs], gval);
            }
            jdx++;
            offs += 0x8;
            nbit += 0x8;
        }
    }

    case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
        break;

    case MODBUS_FC_REPORT_SLAVE_ID:
        break;

    case MODBUS_FC_MASK_WRITE_REGISTER:
        break;

    case MODBUS_FC_WRITE_AND_READ_REGISTERS:
        break;

    default:
        break;
    }

    ret = pthread_mutex_unlock(&dido->dm_lock);
    if (ret != 0) {
        fprintf(stderr, "Error, failed to unlock mutex: %d\n", ret);
        fflush(stderr);
    }
}

int main(int argc, char *argv[])
{
    fd_set rdset;
    fd_set refset;
    int gpio_out, gpio_in;
    int master_socket, retval;
    struct dido_modbus * g_dido;
    /* Maximum file descriptor number */
    int fdmax, header_length, idx, l_sock;

    g_dido = NULL;
    mb_looping = -1;

    int c;
    Reversal_flag = 0;
    while ((c = getopt(argc, argv, "r")) != -1) {
        switch (c) {
            case 'r':
                Reversal_flag = 1;
                break;
            default:
                break;
        }
    }

    if (signal(SIGINT, close_sigint) == SIG_ERR ||
        signal(SIGTERM, close_sigint) == SIG_ERR) {
        fputs("Error, failed to register signal handler!\n", stderr);
        fflush(stderr);
        return 1;
    }

    gpio_out = gpio_in = 0;
    if (dido_modbus_gpio_get(&gpio_out, &gpio_in) < 0) {
        struct timespec tspec;
        tspec.tv_sec = 60;
        tspec.tv_nsec = 0;
        nanosleep(&tspec, NULL);
        return 2;
    }

    g_dido = dido_modbus_create(gpio_out, gpio_in);
    if (g_dido == NULL)
        goto err0;

    retval = dido_modbus_opengpio(g_dido);
    if (retval < 0)
        goto err0;

    retval = dido_modbus_opentcp(g_dido, "0.0.0.0", 1502);
    if (retval < 0)
        goto err0;

    /* update GPIO values for output */
    for (idx = 0; idx < g_dido->dm_num_output; ++idx) {
        int gval = g_dido->dm_gpios[idx + WOOLINK_DO_OFFSET].gpio_val;
        int offs = WOOLINK_DO_OFFSET + idx;
        g_dido->dm_map->tab_bits[offs] = (uint8_t) gval;
        g_dido->dm_map->tab_registers[offs] = (uint16_t) gval;
    }
    /* update GPIO values for input */
    for (idx = 0; idx < g_dido->dm_num_input; ++idx) {
        int offs = idx + g_dido->dm_num_output;
        int gval = g_dido->dm_gpios[offs].gpio_val;
        offs = idx + WOOLINK_DI_OFFSET;
        g_dido->dm_map->tab_bits[offs] = (uint8_t) gval;
        g_dido->dm_map->tab_registers[offs] = (uint16_t) gval;
    }

    /* Clear the reference set of socket */
    FD_ZERO(&refset);
    /* Add the server socket */
    l_sock = g_dido->dm_sock;
    FD_SET(l_sock, &refset);

    /* Keep track of the max file descriptor */
    fdmax = g_dido->dm_sock;
    header_length = modbus_get_header_length(g_dido->dm_ctx);
    fprintf(stdout, "MB header_length: %d\n", header_length);

    /* create GPIO monitoring thread */
    retval = pthread_create(&g_dido->dm_thread, NULL, gpio_working_thread, g_dido);
    if (retval != 0) {
        mb_looping = 0;
        fprintf(stderr, "Error, failed to start working thread: %d\n", retval);
        fflush(stderr);
    }

    while (mb_looping) {
        int errn;
        struct timeval tval;
        rdset = refset;
        tval.tv_sec = 30;
        tval.tv_usec = 0;
        retval = select(fdmax + 1, &rdset, NULL, NULL, &tval);
        if (retval == -1) {
            errn = errno;
            if (errn == EINTR)
                continue;
            fprintf(stderr, "Server select() failure: %s\n", strerror(errn));
            fflush(stderr);
            break;
        }

        /* Run through the existing connections looking for data to be
         * read */
        for (master_socket = 0; master_socket <= fdmax; master_socket++) {

            if (!FD_ISSET(master_socket, &rdset)) {
                continue;
            }

            if (master_socket == l_sock) {
                /* A client is asking a new connection */
                socklen_t addrlen;
                struct sockaddr_in clientaddr;
                int newfd;

                /* Handle new connections */
                addrlen = sizeof(clientaddr);
                memset(&clientaddr, 0, sizeof(clientaddr));
                newfd = accept(l_sock, (struct sockaddr *)&clientaddr, &addrlen);
                if (newfd == -1) {
                    perror("Server accept() error");
                } else {
                    FD_SET(newfd, &refset);

                    if (newfd > fdmax) {
                        /* Keep track of the maximum */
                        fdmax = newfd;
                    }
                    printf("New connection from %s:%d on socket %d\n",
                           inet_ntoa(clientaddr.sin_addr), clientaddr.sin_port, newfd);
                }
            } else {
                modbus_set_socket(g_dido->dm_ctx, master_socket);
                memset(g_dido->dm_query, 0, MODBUS_TCP_MAX_ADU_LENGTH);
                retval = modbus_receive(g_dido->dm_ctx, g_dido->dm_query);
                if (retval > 0) {
                    if (g_dido->dm_verbose != 0) {
                        fprintf(stdout, "----------------- MSG LEN: %u ---------------\n",
                            (unsigned int) retval);
                        dump_hexmsg(g_dido->dm_query, retval, 0x1);
                    }
                    handle_mbmsg(g_dido, g_dido->dm_query, retval);
                    if (g_dido->dm_verbose != 0)
                        dump_mappings(g_dido->dm_map);
                    modbus_reply(g_dido->dm_ctx, g_dido->dm_query, retval, g_dido->dm_map);
                } else if (retval == -1) {
                    /* This example server in ended on connection closing or
                     * any errors. */
                    printf("Connection closed on socket %d\n", master_socket);
                    close(master_socket);

                    /* Remove from reference set */
                    FD_CLR(master_socket, &refset);

                    if (master_socket == fdmax) {
                        fdmax--;
                    }
                }
            }
        }
    }

err0:
    mb_looping = 0; /* assert mb_looping is set to 0 */
    if (g_dido != NULL) {
        dido_modbus_free(g_dido);
        g_dido = NULL;
    }
    return 3;
}