#include <iostream>
#include <chrono>
#include <thread>
#include <functional>
#include <modbus.h>

#define NUM_SLAVES 10
#define NUM_REGISTERS 240

modbus_mapping_t* mapping[NUM_SLAVES];
modbus_t* rtusrv;
uint8_t req[MODBUS_RTU_MAX_ADU_LENGTH];// request buffer
int len;// length of the request/response


void close_connections()
{
    for (int i = 0; i < NUM_SLAVES; i++)
    {
        modbus_mapping_free(mapping[i]);
    }

    modbus_close(rtusrv);
    modbus_free(rtusrv);
}

int setup_connections()
{
    for (int i = 0; i < NUM_SLAVES; i++)
    {
        mapping[i] = modbus_mapping_new(0, 0, NUM_REGISTERS, 0);
        if (!mapping[i]) {
            fprintf(stderr, "Failed to allocate the mapping: %s\n", modbus_strerror(errno));
            return -1;
        }
    }

    rtusrv = modbus_new_rtu("\COM8", 115200, 'N', 8, 1);
    if (!rtusrv) {
        fprintf(stderr, "Failed to create the context: %s\n", modbus_strerror(errno));
        return -1;
    }

    if (modbus_connect(rtusrv) == -1) {
        fprintf(stderr, "Unable to connect: %s\n", modbus_strerror(errno));
        modbus_free(rtusrv);
        return -1;
    }

    //set registers of server
    for (int i = 0; i < NUM_SLAVES; i++)
    {
        for (int j = 0; j < NUM_REGISTERS; j++)
        {
            mapping[i]->tab_registers[j] = (i+1)*1000 + (j+1);
        }
    }
    modbus_set_slave(rtusrv, 1);
    modbus_set_debug(rtusrv,TRUE);
    return 0;
}

int main()
{
    //int slaveid = 1;
    printf("Modbus RTU Server\n");

    if (setup_connections()) return -1;

    while (1)
    {
        len = modbus_receive(rtusrv, req);
        if (len == -1) break;
        else
        {
            printf("len %d req[0]=%d\n", len, req[0]);
            if (len == 0)
            {
                slaveid = req[0];
                modbus_set_slave(rtusrv, slaveid);
                len = 8;
            }

            len = modbus_reply(rtusrv, req, len, mapping[slaveid-1]);
            if (len == -1) break;
        }
    }
    printf("Exit the loop: %s\n", modbus_strerror(errno));

    close_connections();
}