#include <stdio.h>
#include <unistd.h>
#include <modbus.h>
#include <string.h>
#include <errno.h>
#include <stdlib.h>
#include <math.h>
#include <sys/select.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <time.h>

#define NB_REGS 120
#define NB_CONNECTION 5
#define IEM3155_ADDRESS 1 // Modbus RTU slave address for iEM3155
#define METER_OR_GATEWAY_PORT "502"
#define TRANSLATOR_IP_ADDRESS "10.0.1.2"
#define SYMO_IP_ADDRESS "10.0.1.3"

#define MODBUS_SET_INT32_TO_INT16_FRONIUS(tab_int16, index, value)              \
    do {                                                                \
        ((int16_t *) (tab_int16))[(index) + 1] = (int16_t) ((value) >> 16); \
        ((int16_t *) (tab_int16))[(index)] = (int16_t) (value);     \
    } while (0)

int stringModbusCopy(char * destination,char * source, int n){ // Custom method to write string in the right format for Modbus
    // Copy even characters
    for(int i = 0; i < n; i+=2){
        destination[i + 1] = source[i];
    }
    // Copy odd characters
    for(int i = 1; i < n; i+=2){
        destination[i - 1] = source[i];
    }
}

static int server_socket = -1;
static modbus_mapping_t *mb_mapping_sunspec_snap;

static modbus_t *ctx; // Context for requests to Symo
static modbus_t *ctx2; // Context for requests from GEN24


static void close_sigint(int dummy)
{
    if (server_socket != -1) {
        close(server_socket);
    }
    modbus_free(ctx);
    modbus_free(ctx2);
    //modbus_free(ctx4);
    modbus_mapping_free(mb_mapping_sunspec_snap);

    exit(dummy);
}

int snapUpdate(modbus_mapping_t *mapping, modbus_t *ctxbis){
    int ret;
    uint16_t dest [NB_REGS];
    struct timespec ts;
    float v1, v2, v3;

    ts.tv_sec = 0;
    ts.tv_nsec =  500000000;
    //printf("Debut update symo\n");
    ret = modbus_read_registers(ctxbis, 40069, 40, dest); // Request sunspec model
    if(ret < 0){
        perror("Unable to update\n");
        return -1;
    }
    //memcpy(&mapping->tab_registers[71], &dest[2], 2); // AC Total Current
    /*printf("Test float abcd : %f\n",modbus_get_float_abcd(&dest[2]));
    printf("Test float badc : %f\n",modbus_get_float_badc(&dest[2]));
    printf("Test float cdab : %f\n",modbus_get_float_cdab(&dest[2]));
    printf("Test float dcba : %f\n",modbus_get_float_dcba(&dest[2]));*/
    modbus_set_float_badc(-1.0 * modbus_get_float_abcd(&dest[2]), &mapping->tab_registers[71]); // AC Total Current
    modbus_set_float_badc(-1.0 * modbus_get_float_abcd(&dest[4]), &mapping->tab_registers[73]); // AC Ph1 Current
    modbus_set_float_badc(-1.0 * modbus_get_float_abcd(&dest[6]), &mapping->tab_registers[75]); // AC Ph2 Current
    modbus_set_float_badc(-1.0 * modbus_get_float_abcd(&dest[8]), &mapping->tab_registers[77]); // AC Ph3 Current
    v1 = modbus_get_float_abcd(&dest[16]); // L1-N Voltage
    modbus_set_float_badc(v1, &mapping->tab_registers[81]);
    v2 = modbus_get_float_abcd(&dest[18]); // L2-N Voltage
    modbus_set_float_badc(v2, &mapping->tab_registers[83]);
    v3 = modbus_get_float_abcd(&dest[20]); // L3-N Voltage
    modbus_set_float_badc(v3, &mapping->tab_registers[85]);
    modbus_set_float_badc((v1 + v2 + v3)/3.0, &mapping->tab_registers[79]); // L-N Voltage avg
    v1 = modbus_get_float_abcd(&dest[10]); // L1-L2 Voltage
    modbus_set_float_badc(v1, &mapping->tab_registers[89]);
    v2 = modbus_get_float_abcd(&dest[12]); // L2-L3 Voltage
    modbus_set_float_badc(v2, &mapping->tab_registers[91]);
    v3 = modbus_get_float_abcd(&dest[14]); // L3-L1 Voltage
    modbus_set_float_badc(v3, &mapping->tab_registers[93]);
    modbus_set_float_badc((v1 + v2 + v3)/3.0, &mapping->tab_registers[87]); // L-L Voltage avg
    memcpy(&mapping->tab_registers[95], &dest[24], 2); // Frequency
    modbus_set_float_badc(-1.0 * modbus_get_float_abcd(&dest[22]), &mapping->tab_registers[97]); // AC Real power
    modbus_set_float_badc(-1.0 * modbus_get_float_abcd(&dest[26]), &mapping->tab_registers[105]); // AC Apparent power
    modbus_set_float_badc(-1.0 * modbus_get_float_abcd(&dest[28]), &mapping->tab_registers[113]); // AC Reactive power
    modbus_set_float_badc(-1.0 * modbus_get_float_abcd(&dest[30]), &mapping->tab_registers[121]); // Power factor
    memcpy(&mapping->tab_registers[129], &dest[32], 2); // Energy exported
    return 0;
}

int main()
{
    printf("Begin\n");
    int ret;
    int rc;
    int header_length;
    uint8_t *query;
    uint8_t nb_retry = 0;
    uint8_t slaveAddress;
    uint16_t dest [NB_REGS];
    uint16_t reg_address;
    uint16_t nb_registers;
    uint16_t tempTab [2];
    fd_set refset;
    fd_set rdset;
    /* Maximum file descriptor number */
    int fdmax;
    int master_socket;
    time_t update_snap;

    //Reader preparation - Symo
    ctx = modbus_new_tcp_pi(SYMO_IP_ADDRESS, "502");
    if (ctx == NULL) {
        perror("Unable to create the libmodbus context\n");
        return -1;
    }

    ret = modbus_set_slave(ctx, 1);
    if(ret < 0){
        perror("modbus_set_slave error\n");
        return -1;
    }
    modbus_set_response_timeout(ctx, 10, 0);
    modbus_set_error_recovery(ctx, MODBUS_ERROR_RECOVERY_LINK);

    ret = modbus_connect(ctx);
    if(ret < 0){
        perror("modbus_connect error\n");
        //return -1;
    }

    // Receiver preparation
    ctx2 = modbus_new_tcp_pi(TRANSLATOR_IP_ADDRESS, "502"); // You have to use port TCP/502 here, because the GEN24 doesn't appreciate otherwise
    if (ctx2 == NULL) {
        perror("Unable to create the libmodbus context\n");
        return -1;
    }
    query = malloc(MODBUS_TCP_MAX_ADU_LENGTH);


    server_socket = modbus_tcp_pi_listen(ctx2, NB_CONNECTION);
    if (server_socket  == -1) {
        perror("Unable to listen\n");
        return -1;
    }
    signal(SIGINT, close_sigint);

    /* Clear the reference set of socket */
    FD_ZERO(&refset);
    /* Add the server socket */
    FD_SET(server_socket, &refset);

    /* Keep track of the max file descriptor */
    fdmax = server_socket;

    mb_mapping_sunspec_snap = modbus_mapping_new_start_address(0,0,0,0,40000,197,0,0);

    // Static Sunspec Values - Symo
    mb_mapping_sunspec_snap->tab_registers[0] = 0x5375; // Sunspec ID
    mb_mapping_sunspec_snap->tab_registers[1] = 0x6e53; // Sunspec ID
    mb_mapping_sunspec_snap->tab_registers[2] = 1; // Sunspec ID
    mb_mapping_sunspec_snap->tab_registers[3] = 65; // Length of Sunspec model meter common
    stringModbusCopy((char *) &mb_mapping_sunspec_snap->tab_registers[4], "Fronius", 7); // Manufacturer name
    stringModbusCopy((char *) &mb_mapping_sunspec_snap->tab_registers[20], "Symo 6", 6); // Meter product name
    stringModbusCopy((char *) &mb_mapping_sunspec_snap->tab_registers[36], "custom", 6); // Meter name (test)
    stringModbusCopy((char *) &mb_mapping_sunspec_snap->tab_registers[44], "1.0", 3); // SW version
    stringModbusCopy((char *) &mb_mapping_sunspec_snap->tab_registers[52], "0123456789", 10); // Serial number
    mb_mapping_sunspec_snap->tab_registers[68] = 5; // Modbus address
    mb_mapping_sunspec_snap->tab_registers[69] = 213; // Meter type (213 = Three phases + Neutral float)
    mb_mapping_sunspec_snap->tab_registers[70] = 124; // Length of Sunspec model ac_meter


    mb_mapping_sunspec_snap->tab_registers[195] = 0xFFFF; // End of model ac_meter
    mb_mapping_sunspec_snap->tab_registers[196] = 0; // End of model ac_meter

    // Not implemented registers - Symo
    mb_mapping_sunspec_snap->tab_registers[99] = 0x7FC0; // Active power phase 1
    mb_mapping_sunspec_snap->tab_registers[101] = 0x7FC0; // Active power phase 2
    mb_mapping_sunspec_snap->tab_registers[103] = 0x7FC0; // Active power phase 3
    mb_mapping_sunspec_snap->tab_registers[107] = 0x7FC0; // Apparent power phase 1
    mb_mapping_sunspec_snap->tab_registers[109] = 0x7FC0; // Apparent power phase 2
    mb_mapping_sunspec_snap->tab_registers[111] = 0x7FC0; // Apparent power phase 3
    mb_mapping_sunspec_snap->tab_registers[115] = 0x7FC0; // Reactive power phase 1
    mb_mapping_sunspec_snap->tab_registers[117] = 0x7FC0; // Reactive power phase 2
    mb_mapping_sunspec_snap->tab_registers[119] = 0x7FC0; // Reactive power phase 3
    mb_mapping_sunspec_snap->tab_registers[123] = 0x7FC0; // Power factor phase 1
    mb_mapping_sunspec_snap->tab_registers[125] = 0x7FC0; // Power factor phase 2
    mb_mapping_sunspec_snap->tab_registers[127] = 0x7FC0; // Power factor phase 3
    mb_mapping_sunspec_snap->tab_registers[131] = 0x7FC0; // Active energy exported phase 1
    mb_mapping_sunspec_snap->tab_registers[133] = 0x7FC0; // Active energy exported phase 2
    mb_mapping_sunspec_snap->tab_registers[135] = 0x7FC0; // Active energy exported phase 3
    mb_mapping_sunspec_snap->tab_registers[139] = 0x7FC0; // Active energy imported phase 1
    mb_mapping_sunspec_snap->tab_registers[141] = 0x7FC0; // Active energy imported phase 2
    mb_mapping_sunspec_snap->tab_registers[143] = 0x7FC0; // Active energy imported phase 3
    mb_mapping_sunspec_snap->tab_registers[145] = 0x7FC0; // Apparent energy exported total
    mb_mapping_sunspec_snap->tab_registers[147] = 0x7FC0; // Apparent energy exported phase 1
    mb_mapping_sunspec_snap->tab_registers[149] = 0x7FC0; // Apparent energy exported phase 2
    mb_mapping_sunspec_snap->tab_registers[151] = 0x7FC0; // Apparent energy exported phase 3
    mb_mapping_sunspec_snap->tab_registers[153] = 0x7FC0; // Apparent energy imported total
    mb_mapping_sunspec_snap->tab_registers[155] = 0x7FC0; // Apparent energy imported phase 1
    mb_mapping_sunspec_snap->tab_registers[157] = 0x7FC0; // Apparent energy imported phase 2
    mb_mapping_sunspec_snap->tab_registers[159] = 0x7FC0; // Apparent energy imported phase 3
    mb_mapping_sunspec_snap->tab_registers[161] = 0x7FC0; // Reactive energy imported Q1 total
    mb_mapping_sunspec_snap->tab_registers[163] = 0x7FC0; // Reactive energy imported Q1 phase 1
    mb_mapping_sunspec_snap->tab_registers[165] = 0x7FC0; // Reactive energy imported Q1 phase 2
    mb_mapping_sunspec_snap->tab_registers[167] = 0x7FC0; // Reactive energy imported Q1 phase 3
    mb_mapping_sunspec_snap->tab_registers[169] = 0x7FC0; // Reactive energy imported Q2 total
    mb_mapping_sunspec_snap->tab_registers[171] = 0x7FC0; // Reactive energy imported Q2 phase 1
    mb_mapping_sunspec_snap->tab_registers[173] = 0x7FC0; // Reactive energy imported Q2 phase 2
    mb_mapping_sunspec_snap->tab_registers[175] = 0x7FC0; // Reactive energy imported Q2 phase 3
    mb_mapping_sunspec_snap->tab_registers[177] = 0x7FC0; // Reactive energy exported Q3 total
    mb_mapping_sunspec_snap->tab_registers[179] = 0x7FC0; // Reactive energy exported Q3 phase 1
    mb_mapping_sunspec_snap->tab_registers[181] = 0x7FC0; // Reactive energy exported Q3 phase 2
    mb_mapping_sunspec_snap->tab_registers[183] = 0x7FC0; // Reactive energy exported Q3 phase 3
    mb_mapping_sunspec_snap->tab_registers[185] = 0x7FC0; // Reactive energy exported Q4 total
    mb_mapping_sunspec_snap->tab_registers[187] = 0x7FC0; // Reactive energy exported Q4 phase 1
    mb_mapping_sunspec_snap->tab_registers[189] = 0x7FC0; // Reactive energy exported Q4 phase 2
    mb_mapping_sunspec_snap->tab_registers[191] = 0x7FC0; // Reactive energy exported Q4 phase 3

    // Test data for Symo
    modbus_set_float_badc(0.0, &mb_mapping_sunspec_snap->tab_registers[71]); // AC Total Current
    modbus_set_float_badc(0.0, &mb_mapping_sunspec_snap->tab_registers[73]); // AC Ph1 Current
    modbus_set_float_badc(0.0, &mb_mapping_sunspec_snap->tab_registers[75]); // AC Ph2 Current
    modbus_set_float_badc(0.0, &mb_mapping_sunspec_snap->tab_registers[77]); // AC Ph3 Current
    modbus_set_float_badc(230.0, &mb_mapping_sunspec_snap->tab_registers[81]);
    modbus_set_float_badc(230.0, &mb_mapping_sunspec_snap->tab_registers[83]);
    modbus_set_float_badc(230.0, &mb_mapping_sunspec_snap->tab_registers[85]);
    modbus_set_float_badc(230.0, &mb_mapping_sunspec_snap->tab_registers[79]); // L-N Voltage avg
    modbus_set_float_badc(400.0, &mb_mapping_sunspec_snap->tab_registers[89]);
    modbus_set_float_badc(400.0, &mb_mapping_sunspec_snap->tab_registers[91]);
    modbus_set_float_badc(400.0, &mb_mapping_sunspec_snap->tab_registers[93]);
    modbus_set_float_badc(400.0, &mb_mapping_sunspec_snap->tab_registers[87]); // L-L Voltage avg
    modbus_set_float_badc(50.0, &mb_mapping_sunspec_snap->tab_registers[95]); // Frequency
    modbus_set_float_badc(0.0, &mb_mapping_sunspec_snap->tab_registers[97]); // AC Real power
    modbus_set_float_badc(0.0, &mb_mapping_sunspec_snap->tab_registers[105]); // AC Apparent power
    modbus_set_float_badc(0.0, &mb_mapping_sunspec_snap->tab_registers[113]); // AC Reactive power
    modbus_set_float_badc(100.0, &mb_mapping_sunspec_snap->tab_registers[121]); // Power factor
    modbus_set_float_badc(0.0, &mb_mapping_sunspec_snap->tab_registers[129]); // Energy exported

    if(snapUpdate(mb_mapping_sunspec_snap, ctx) == 0){
        update_snap = time( NULL );
    } else {
        update_snap = 0;
    }

    header_length = modbus_get_header_length(ctx2);

    for (;;) {
        rdset = refset;
        if (select(fdmax + 1, &rdset, NULL, NULL, NULL) == -1) {
            perror("Server select() failure.");
            close_sigint(1);
        }

        /* 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 == server_socket) {
                /* 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(server_socket, (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(ctx2, master_socket);
                rc = modbus_receive(ctx2, query);
                if (rc > 0) {
                    if (query[header_length] == 0x03) {
                        //Read holding registers
                        reg_address = (uint16_t) MODBUS_GET_INT16_FROM_INT8(query, header_length + 1);
                        nb_registers = (uint16_t) MODBUS_GET_INT16_FROM_INT8(query, header_length + 3);
                        slaveAddress = query[header_length - 1];
                        //printf("Request received. Slave address : %d, register address : %d, nb registers : %d\n", slaveAddress, reg_address, nb_registers);
                        if(slaveAddress == 5) { // Symo snap meter
                            if(difftime(time(NULL), update_snap) > 9){
                                if(snapUpdate(mb_mapping_sunspec_snap, ctx) == 0){
                                    update_snap = time( NULL);
                                    //printf("Update symo\n");
                                }
                            }
                            //printf("Reply Symo\n");
                            rc = modbus_reply(ctx2, query, rc, mb_mapping_sunspec_snap);
                            if (rc == -1) {
                                perror("Error during reply");
                            }
                        } else {
                            printf("Slave not found : %d\n", slaveAddress);
                        }
                    }
                } else if (rc == -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--;
                    }
                }
            }
        }

    }

    //modbus_close(ctx);
    //modbus_free(ctx);
    free(query);
    modbus_close(ctx2);
    modbus_free(ctx2);