/*
 * Copyright (C) 2022 EXOLIGENT (www.exoligent.com)
 * Author: Maxime Coroyer <maxime.coroyer@exoligent.com>
 */

#include <stdlib.h>     // malloc(), free()
#include <string.h>     // memset()
#include <unistd.h>     // close()
#include <pthread.h>    // pthread()
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <arpa/inet.h>  // inet_ntoa

#include "gw.h"

void* mbus_task(void *arg);

int mbus_srv_start(struct gateway *gw)
{
    int i, start_addr;
    pthread_attr_t thr_attr;
    struct sched_param thr_param;

    if (!gw)
        return -1;

    if (gw->mbus.task_run)
        return -1;

    /* init modbus variable context */
    /* => input registers */
    for (i = 0, start_addr = 0; i < gw->fip.cons_varcnt; ++i) {
        int bsz = gw->fip.cons_varlist[i]->bsz;
        int reg16_nb = (bsz / 2) + !!(bsz % 2);
        gw->mbus.cons_varlist[i].addr = start_addr;
        gw->mbus.cons_varlist[i].nb = reg16_nb;
        start_addr += reg16_nb;
    }
    gw->mbus.cons_varcnt = gw->fip.cons_varcnt;

    /* => holding registers */
    for (i = 0, start_addr = 0; i < gw->fip.prod_varcnt; ++i) {
        int bsz = gw->fip.prod_varlist[i]->bsz;
        int reg16_nb = (bsz / 2) + !!(bsz % 2);
        gw->mbus.prod_varlist[i].addr = start_addr;
        gw->mbus.prod_varlist[i].nb = reg16_nb;
        start_addr += reg16_nb;
    }
    gw->mbus.prod_varcnt = gw->fip.prod_varcnt;

    // printf("gw->mbus.cons_varcnt = %d\n", gw->mbus.cons_varcnt);
    // for (i = 0; i < gw->mbus.cons_varcnt; ++i) {
    //     printf("[%d] input var\n", i + i);
    //     printf("  addr = %d\n", gw->mbus.cons_varlist[i].addr);
    //     printf("  nb   = %d\n", gw->mbus.cons_varlist[i].nb);
    // }
    // printf("gw->mbus.prod_varcnt = %d\n", gw->mbus.prod_varcnt);
    // for (i = 0; i < gw->mbus.prod_varcnt; ++i) {
    //     printf("[%d] output var\n", i + i);
    //     printf("  addr = %d\n", gw->mbus.prod_varlist[i].addr);
    //     printf("  nb   = %d\n", gw->mbus.prod_varlist[i].nb);
    // }

    /* init modbus connection */
    gw->mbus.ctx = modbus_new_tcp(gw->mbus.ip_str, gw->mbus.port);
    gw->mbus.query = malloc(MODBUS_TCP_MAX_ADU_LENGTH);

    /* TODO: taille à gérer dynamiquement en fonction de la taille totale
     * des variables input et output fip
     * max: 65535 16-bit, soit 131070B par zones:
     * zones:
     *  - input registers: registres 16-bit en lecture
     *  - holding registers: registres 16-bit en lecture/écriture
     */
    gw->mbus.mapping = modbus_mapping_new(0, 0, 1024, 1024);
    if (!gw->mbus.mapping) {
        modbus_free(gw->mbus.ctx);
        _print(mbus, err, "[failed] mb-fip i/o map [automatically]\n");
        return -1;
    }
    _print(mbus, info, "[ok] mb-fip i/o map [automatically]\n");

    /* app - thread creation */
    if (pthread_attr_init(&thr_attr)) {
        _print(mbus, err, "[failed] pthread_attr_init()\n");
        return -1; // errno is already set
    }

    pthread_attr_setinheritsched(&thr_attr, PTHREAD_EXPLICIT_SCHED);
    pthread_attr_setschedparam(&thr_attr, &thr_param);

    /* thread: loop task */
    if (pthread_create(&gw->mbus.thr_task, &thr_attr, mbus_task, (void*) gw)) {
        pthread_attr_destroy(&thr_attr);
        _print(mbus, err, "[failed] pthread_create()\n");
        return -1; // errno is already set
    }

    pthread_attr_destroy(&thr_attr);
    gw->mbus.task_run = 1;
    return 0;
}

int mbus_srv_stop(struct gateway *gw)
{
    if (!gw)
        return -1;

    if (!gw->mbus.task_run)
        return -1;

    gw->mbus.task_run = 0;

    pthread_cancel(gw->mbus.thr_task);
    pthread_join(gw->mbus.thr_task, NULL);

    if (gw->mbus.socket != -1)
        close(gw->mbus.socket);

    modbus_mapping_free(gw->mbus.mapping);
    free(gw->mbus.query);
    /* for rtu */
    modbus_close(gw->mbus.ctx);
    modbus_free(gw->mbus.ctx);

    return 0;
}

void mbus_display_update(struct gateway *gw)
{
    printf("Modbus: TCP server (%s:%d)\n",
        gw->mbus.ip_str, gw->mbus.port);
    printf("  Connections: %d\n", gw->mbus.nb_co);
    printf("  RX request : ok=%d [failed=%d]\n",
        gw->mbus.req_ok, gw->mbus.req_nok);
    printf("  TX response: ok=%d [failed=%d]\n",
        gw->mbus.res_ok, gw->mbus.res_nok);
}

void* mbus_task(void *arg)
{
    int i, skt_max, rc = 0;
    struct gateway *gw = (struct gateway *) arg;
    fd_set ref_set, rd_set;

    gw->mbus.socket = modbus_tcp_listen(gw->mbus.ctx, MBUS_CLIENT_COUNT_MAX);
    if (gw->mbus.socket == -1) {
        _print(mbus, err, "[failed] unable to listen tcp connection\n");
        return NULL;
    }

    /* clear the reference set of socket */
    FD_ZERO(&ref_set);
    /* add the server socket */
    FD_SET(gw->mbus.socket, &ref_set);

    _print(mbus, info, "[ok] modbus server start (%s:%d) \n",
        gw->mbus.ip_str, gw->mbus.port);

    skt_max = gw->mbus.socket; /* keep track the max socket (here server) */
    for (;;) {

        rd_set = ref_set;
        if (select(skt_max + 1, &rd_set, NULL, NULL, NULL) == -1) {
            _print(mbus, err, "[failed] server select()\n");
            break; /* quit */
        }

        for (i = 0; i <= skt_max; ++i) {

            if (!FD_ISSET(i, &rd_set))
                continue;

            if (i == gw->mbus.socket)
            {
                /* a client is asking a new connection */
                socklen_t addr_len;
                struct sockaddr_in addr_in;
                int skt_new;

                /* handle new connections */
                addr_len = sizeof(addr_in);
                memset(&addr_in, 0, sizeof(addr_in));

                skt_new = accept(gw->mbus.socket,
                            (struct sockaddr *) &addr_in, &addr_len);
                if (skt_new != -1) {
                    FD_SET(skt_new, &ref_set);

                    if (skt_new > skt_max)
                        skt_max = skt_new; /* keep track the max socket */

                    gw->mbus.nb_co++;

                    _print(mbus, info,
                        "[ok] new connection from %s:%d on socket %d\n",
                        inet_ntoa(addr_in.sin_addr), addr_in.sin_port, skt_new);
                } else
                    _print(mbus, err, "[failed] server accept()\n");
            }
            else
            {
                modbus_set_socket(gw->mbus.ctx, i);
                rc = modbus_receive(gw->mbus.ctx, gw->mbus.query);

                if (rc > 0)
                {
                    gw->mbus.req_ok++;
                    modbus_reply(gw->mbus.ctx, gw->mbus.query, rc,
                        gw->mbus.mapping);
                    gw->mbus.res_ok++;
                }
                else if (rc == -1)
                {
                    gw->mbus.nb_co--;

                    _print(mbus, info,
                        "[ok] connection closed on socket %d\n", i);

                    /* we close the client connection in error case */
                    close(i);

                    /* remove from reference set */
                    FD_CLR(i, &ref_set);

                    if (i == skt_max)
                        skt_max--;
                }
            }
        }

        if (!gw->mbus.task_run)
            break; /* quit */
    }
    return NULL;
}