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

/* ============================================ */
/* KONFIGURACJA PROGRAMU - EDYTUJ TU           */
/* ============================================ */
#define READ_LOOP true              /* false = jeden odczyt, true = pętla co 5s */
#define VOLTAGE_BUFFER_SIZE 12
#define VOLTAGE_FLUCTUATION_THRESHOLD 0.05 // 5%
#define HIGH_FREQ_MODE_DURATION 300 // 300 seconds
#define READ_ORNO true               /* Czy odczytywać ORNO */
#define READ_SUN2K true              /* Czy odczytywać SUN2K */

/* Adresy urządzeń Modbus RTU */
#define SUN2000_SLAVE_ADR 3
#define ORNO_SLAVE_ADR 2

/* Port szeregowy */
#define USB_DEV_DEFAULT "/dev/ttyUSB0"

/* Parametry timing dla ORNO (mikrosekund) */
#define ORNO_RTS_DELAY 5000          /* RTS delay: 5ms */
#define ORNO_BYTE_TIMEOUT 2500       /* Byte timeout: 2.5ms */

/* Parametry timing dla SUN2K (mikrosekund) */
#define SUN2K_RTS_DELAY 5000         /* RTS delay: 5ms */
#define SUN2K_BYTE_TIMEOUT 2500      /* Byte timeout: 2.5ms */

/* Przerwa między ORNO a SUN2K (mikrosekund) */
#define DELAY_BETWEEN_DEVICES 10000 /* 10ms */

/* MQTT - ustaw 1 aby wyłączyć MQTT */
#define DISABLE_MQTT 0               /* 0 = MQTT włączony, 1 = wyłączony */

/* Broker MQTT */
#define MQTT_BROKER "10.1.1.1"
#define MQTT_PORT 1883

/* InfluxDB Configuration */
#define DISABLE_INFLUX 0             /* 0 = InfluxDB włączony, 1 = wyłączony */
#define INFLUX_HOST "10.1.1.1"
#define INFLUX_PORT 5086
#define INFLUX_ORG "sic"          /* Zmień na swoją organizację */
#define INFLUX_BUCKET "energydb"       /* Zmień na swój bucket */
#define INFLUX_TOKEN "BCIZ6kcCAVgpcwSfU0PBS7m0Zb6an93kuOtizbEtlXub-uaoYp4dmIQBQYaJCv8_KE4QYYZ08jxtpaZ3TUWP-Q=="      /* Zmień na swój token */
/* ============================================ */

/*
'Model' : 'ID'
'addr' : '30000', 
'registers' : 15, 
'name' : 'Model', 
'scale' : 1, 
'type' : 'str', 
'units' : '' , 
'use' : 'info', 
'method' : 'hold'
*/
enum regtype
{
    rgStr,
    rgU16,
    rgU32,
    rgI16,
    rgI32,
    rgB16,
    rgB32,
    rgData,
    rgFloat
};
typedef struct s_mb_reg
{
    const char *reg_id;
    int ireg;
    int num_reg;
    char *desc;
    int scale;
    enum regtype type;
    char *units;
    char *use;
    char *method;
    char buf_last_val[30];
} t_mb_reg;

t_mb_reg mbReg[] = {

    {"Start", 32091, 2, "Startup time", 1, rgData, "s", "info", "hold", ""},
    {"Shutdown", 32093, 2, "Shutdown time", 1, rgData, "s", "info", "hold", ""},
    {"Time", 40000, 2, "Current time", 1, rgData, "s", "info", "hold", ""},
    {"State1", 32000, 1, "Status 1", 1, rgB16, "", "stat", "hold", ""},
    {"Alarm1", 32008, 1, "Alarm 1", 1, rgB16, "", "stat", "hold", ""},
    {"Status", 32089, 1, "Device status", 1, rgU16, "", "stat", "hold", ""},
    {"Fault", 32090, 1, "Fault code", 1, rgU16, "", "stat", "hold", ""},
    {"PV_P", 32064, 2, "Input power", 1000, rgI32, "kW", "data", "hold", ""},
    {"U_A", 32069, 1, "Phase Voltage A", 10, rgU16, "V", "data", "hold", ""},
    {"U_B", 32070, 1, "Phase Voltage B", 10, rgU16, "V", "data", "hold", ""},
    {"U_C", 32071, 1, "Phase Voltage C", 10, rgU16, "V", "ext", "hold", ""},
    {"P_peak", 32078, 2, "Peak Power", 1000, rgI32, "kW", "data", "hold", ""},
    {"P_active", 32080, 2, "Active power", 1000, rgI32, "kW", "data", "hold", ""},
    {"P_reactive", 32082, 2, "Reactive power", 1000, rgI32, "kVar", "data", "hold", ""},
    {"Frequency", 32085, 1, "Grid frequency", 100, rgU16, "Hz", "data", "hold", ""},
    {"Temp", 32087, 1, "Internal temperature", 10, rgI16, "°C", "data", "hold", ""},
    {"P_accum", 32106, 2, "Accumulated energy yield", 100, rgU32, "kWh", "data", "hold", ""},
    {"P_daily", 32114, 2, "Daily energy yield", 100, rgU32, "kWh", "data", "hold", ""}

};

t_mb_reg mbReg_Orno[] = {

    {"L1 Volt", 0x000e, 2, "L1 Volt", 1, rgU32, "V", "info", "hold", ""},
    {"L2 Volt", 0x0010, 2, "L2 Volt", 1, rgU32, "V", "info", "hold", ""},
    {"L3 Volt", 0x0012, 2, "L3 Volt", 10, rgU32, "V", "info", "hold", ""},
    {"Tot Power", 0x001c, 2, "Tot Power", 1, rgU32, "kW", "info", "hold", ""},
    {"L1 Tot Power", 0x001e, 2, "L1 Tot Power", 1, rgU32, "kW", "info", "hold", ""},
    {"L2 Tot Power", 0x0020, 2, "L2 Tot Power", 1, rgU32, "kW", "info", "hold", ""},
    {"L3 Tot Power", 0x0022, 2, "L3 Tot Power", 1, rgU32, "kW", "info", "hold", ""},
    {"Tot Energ", 0x0100, 2, "Tot Energ", 1, rgU32, "kWh", "info", "hold", ""},
    {"L1 Tot Energ", 0x0102, 2, "L1 Tot Energ", 1, rgU32, "kWh", "info", "hold", ""},
    {"L2 Tot Energ", 0x0104, 2, "L2 Tot Energ", 1, rgU32, "kWh", "info", "hold", ""},
    {"L3 Tot Energ", 0x0106, 2, "L3 Tot Energ", 1, rgU32, "kWh", "info", "hold", ""}};

void mosq_log_callback(struct mosquitto *mosq, void *userdata, int level, const char *str)
{
    /* Pring all log messages regardless of level. */

    switch (level)
    {
    //case MOSQ_LOG_DEBUG:
    //case MOSQ_LOG_INFO:
    //case MOSQ_LOG_NOTICE:
    case MOSQ_LOG_WARNING:
    case MOSQ_LOG_ERR:
    {
        printf("MQTT: %i:%s\n", level, str);
    }
    }
}

struct mosquitto *mosq = NULL;
void mqtt_setup()
{
    if (DISABLE_MQTT) {
        printf("MQTT: disabled by configuration\n");
        return;
    }

    char *host = MQTT_BROKER;
    int port = MQTT_PORT;
    int keepalive = 60;
    bool clean_session = true;

    mosquitto_lib_init();
    mosq = mosquitto_new(NULL, clean_session, NULL);
    if (!mosq)
    {
        fprintf(stderr, "MQTT: Error: Out of memory.\n");
        exit(1);
    }

    mosquitto_log_callback_set(mosq, mosq_log_callback);

    if (mosquitto_connect(mosq, host, port, keepalive))
    {
        fprintf(stderr, "MQTT: Unable to connect.\n");
        exit(1);
    }
    int loop = mosquitto_loop_start(mosq);
    if (loop != MOSQ_ERR_SUCCESS)
    {
        fprintf(stderr, "MQTT: Unable to start loop: %i\n", loop);
        exit(1);
    }
}
int mqtt_send(char *topic, char *buf)
{
    if (DISABLE_MQTT) return 0;
    return mosquitto_publish(mosq, NULL, topic, strlen(buf), buf, 0, 0);
}

int mqtt_send_SUN2K(t_mb_reg *reg_to_send, char *_str_buf)
{
    if (DISABLE_MQTT) return 0;

    /* Don't send if str_buf is empty or NULL - means read failed */
    if (!_str_buf || strlen(_str_buf) == 0) {
        printf("MQTT: Skipping %s - no valid data\n", reg_to_send->reg_id);
        return 0;
    }

    char topic[128];
    char buf[128];
    snprintf(topic, sizeof(topic), "/energy/SUN2K");
    snprintf(buf, sizeof(buf), "{\"%s\":%s}", reg_to_send->reg_id, _str_buf);

    /* Save last valid value */
    strncpy(reg_to_send->buf_last_val, _str_buf, sizeof(reg_to_send->buf_last_val)-1);
    reg_to_send->buf_last_val[sizeof(reg_to_send->buf_last_val)-1] = '\0';

    printf("MQTT: Publishing %s\n", buf);
    return mosquitto_publish(mosq, NULL, topic, strlen(buf), buf, 0, 0);
}

int mqtt_send_U(float U1, float U2, float U3)
{
    if (DISABLE_MQTT) return 0;
    char buf[128];
    sprintf(buf, "{\"U1\":%f,\"U2\":%f,\"U3\":%f}", U1, U2, U3);
    return mosquitto_publish(mosq, NULL, "/energy/orno/U", strlen(buf), buf, 0, 0);
}

int mqtt_send_P(float P_Tot, float P1, float P2, float P3)
{
    if (DISABLE_MQTT) return 0;
    char buf[128];
    sprintf(buf, "{\"P_Tot\":%f,\"P1\":%f,\"P2\":%f,\"P3\":%f}", P_Tot, P1, P2, P3);
    return mosquitto_publish(mosq, NULL, "/energy/orno/P", strlen(buf), buf, 0, 0);
}

int mqtt_send_W(float W_Tot, float W1, float W2, float W3)
{
    if (DISABLE_MQTT) return 0;
    char buf[128];
    sprintf(buf, "{\"W_Tot\":%f,\"W1\":%f,\"W2\":%f,\"W3\":%f}", W_Tot, W1, W2, W3);
    return mosquitto_publish(mosq, NULL, "/energy/orno/W", strlen(buf), buf, 0, 0);
}

int mqtt_send_I(float I1, float I2, float I3)
{
    if (DISABLE_MQTT) return 0;
    char buf[128];
    sprintf(buf, "{\"I1\":%f,\"I2\":%f,\"I3\":%f}", I1, I2, I3);
    return mosquitto_publish(mosq, NULL, "/energy/orno/I", strlen(buf), buf, 0, 0);
}

int mqtt_send_Hz(float Hz)
{
    if (DISABLE_MQTT) return 0;
    char buf[64];
    sprintf(buf, "%f", Hz);
    return mosquitto_publish(mosq, NULL, "/energy/orno/Hz", strlen(buf), buf, 0, 0);
}

/* InfluxDB Functions */
int influx_send_post(char *data)
{
    if (DISABLE_INFLUX) return 0;

    int sock;
    struct sockaddr_in server;

    sock = socket(AF_INET, SOCK_STREAM, 0);
    if (sock == -1) {
        perror("INFLUX: Could not create socket for InfluxDB");
        return -1;
    }

    server.sin_addr.s_addr = inet_addr(INFLUX_HOST);
    server.sin_family = AF_INET;
    server.sin_port = htons(INFLUX_PORT);

    /* Set timeout for socket */
    struct timeval tv;
    tv.tv_sec = 2;
    tv.tv_usec = 0;
    setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof tv);
    setsockopt(sock, SOL_SOCKET, SO_SNDTIMEO, (const char*)&tv, sizeof tv);

    if (connect(sock, (struct sockaddr *)&server, sizeof(server)) < 0) {
        perror("INFLUX: Connect to InfluxDB failed");
        close(sock);
        return -1;
    }

    char header[1024];
    /* Construct HTTP POST request for InfluxDB v2 Write API */
    snprintf(header, sizeof(header), 
             "POST /api/v2/write?org=%s&bucket=%s&precision=s HTTP/1.1\r\n"
             "Host: %s:%d\r\n"
             "Authorization: Token %s\r\n"
             "Content-Length: %ld\r\n"
             "Content-Type: text/plain; charset=utf-8\r\n"
             "Connection: close\r\n\r\n",
             INFLUX_ORG, INFLUX_BUCKET, INFLUX_HOST, INFLUX_PORT, INFLUX_TOKEN, strlen(data));

    if (send(sock, header, strlen(header), 0) < 0) {
        perror("INFLUX: Send header to InfluxDB failed");
        close(sock);
        return -1;
    }

    if (send(sock, data, strlen(data), 0) < 0) {
        perror("INFLUX: Send body to InfluxDB failed");
        close(sock);
        return -1;
    }

    close(sock);
    return 0;
}

int influx_send_U(float U1, float U2, float U3) {
    char line[256];
    snprintf(line, sizeof(line), "orno,device=orno,pomiar=voltage, L1=%.2f,L2=%.2f,L3=%.2f", U1, U2, U3);
    return influx_send_post(line);
}

int influx_send_I(float I1, float I2, float I3) {
    char line[256];
    snprintf(line, sizeof(line), "orno,device=orno,pomiar=current L1=%.2f,L2=%.2f,L3=%.2f", I1, I2, I3);
    return influx_send_post(line);
}

int influx_send_P(float P_Tot, float P1, float P2, float P3) {
    char line[256];
    snprintf(line, sizeof(line), "orno,device=orno,pomiar=power total=%.3f,L1=%.3f,L2=%.3f,L3=%.3f", P_Tot, P1, P2, P3);
    return influx_send_post(line);
}

int influx_send_W(float W_Tot, float W1, float W2, float W3) {
    char line[256];
    snprintf(line, sizeof(line), "orno,device=orno,pomiar=energy total=%.3f,L1=%.3f,L2=%.3f,L3=%.3f", W_Tot, W1, W2, W3);
    return influx_send_post(line);
}

int influx_send_Hz(float Hz) {
    char line[256];
    snprintf(line, sizeof(line), "orno,device=orno,pomiar=frequency value=%.4f", Hz);
    return influx_send_post(line);
}

int influx_send_SUN2K(t_mb_reg *reg_to_send, char *_str_buf) {
    if (!_str_buf || strlen(_str_buf) == 0) return 0;
    char line[256];
    /* reg_id used as tag, value as field */
    snprintf(line, sizeof(line), "sun2k,device=sun2k,pomiar=%s value=%s", reg_to_send->reg_id, _str_buf);
    return influx_send_post(line);
}

/* Validate float value - check if not NaN, Inf, or out of reasonable range */
int is_valid_float(float value, float min_val, float max_val)
{
    if (isnan(value)) {
        printf("VALIDATE: WARNING: Value is NaN\n");
        return 0;
    }
    if (isinf(value)) {
        printf("VALIDATE: WARNING: Value is Inf\n");
        return 0;
    }
    if (value < min_val || value > max_val) {
        printf("VALIDATE: WARNING: Value %.2f out of range [%.2f, %.2f]\n", value, min_val, max_val);
        return 0;
    }
    return 1;
}

/* Timed wrapper for modbus_read_registers - logs start/stop and duration */
int modbus_read_timed(modbus_t *ctx, int addr, int nb, uint16_t *dest)
{
    struct timeval t0, t1;
    gettimeofday(&t0, NULL);
    int res = modbus_read_registers(ctx, addr, nb, dest);
    gettimeofday(&t1, NULL);
    long ms = (t1.tv_sec - t0.tv_sec) * 1000 + (t1.tv_usec - t0.tv_usec) / 1000;
    if (res < 0) {
        printf("MBUS: modbus_read_registers addr=0x%X nb=%d -> ERR (%s) elapsed=%ldms\n", addr, nb, modbus_strerror(errno), ms);
    } else {
        printf("MBUS: modbus_read_registers addr=0x%X nb=%d -> OK (%d) elapsed=%ldms\n", addr, nb, res, ms);
    }
    return res;
}

int mosq_test()
{
    mqtt_setup();
    int i = -1000;
    int k = 10;
    char *buf = malloc(64);
    while (k-- > 0)
    {
        sprintf(buf, "i=%i", i++);
        int snd = mqtt_send("/testtopic", buf);
        if (snd != 0)
            printf("TEST: mqtt_send error=%i\n", snd);
        usleep(5000000);
    }
}

int main(int argc, char *argv[])
{
    /* Użyj wartości z #define */
    const char *USB_DEV = USB_DEV_DEFAULT;
    int ORNO_SLAVE = ORNO_SLAVE_ADR;
    int SUN2000_SLAVE = SUN2000_SLAVE_ADR;
    int do_orno = READ_ORNO;
    int do_sun2k = READ_SUN2K;

    float voltage_buffer_L1[VOLTAGE_BUFFER_SIZE] = {0};
    float voltage_buffer_L2[VOLTAGE_BUFFER_SIZE] = {0};
    float voltage_buffer_L3[VOLTAGE_BUFFER_SIZE] = {0};
    int voltage_buffer_index = 0;
    int voltage_buffer_items = 0;
    time_t high_frequency_mode_end_time = 0;

    time_t rawtime;
    struct tm *timeinfo;

    time(&rawtime);
    timeinfo = localtime(&rawtime);
    printf("==============================================\n");
    printf("Energy Meter Reader - ORNO & SUN2K\n");
    printf("Start: %s", asctime(timeinfo));
    printf("==============================================\n");
    printf("CFG: Configuration:\n");
    printf("CFG:   Device: %s\n", USB_DEV);
    printf("CFG:   ORNO slave address: %d\n", ORNO_SLAVE);
    printf("CFG:   SUN2K slave address: %d\n", SUN2000_SLAVE);
    printf("CFG:   MQTT: %s\n", DISABLE_MQTT ? "DISABLED" : "ENABLED");
    printf("CFG:   MQTT Broker: %s:%d\n", MQTT_BROKER, MQTT_PORT);
    printf("CFG:   InfluxDB: %s (%s:%d)\n", DISABLE_INFLUX ? "DISABLED" : "ENABLED", INFLUX_HOST, INFLUX_PORT);
    printf("==============================================\n\n");

    mqtt_setup();

    do
    {
        if (do_orno)
        {
            //Create a new RTU context with proper serial parameters (in this example,
            //device name /dev/ttyS0, baud rate 9600, no parity bit, 8 data bits, 1 stop bit)
            modbus_t *ctx = modbus_new_rtu(USB_DEV, 9600, 'E', 8, 1);
            //Set the Modbus address of the remote slave (to 2)
            modbus_set_slave(ctx, ORNO_SLAVE);
            if (!ctx)
            {
                fprintf(stderr, "ORNO: Failed to create the context: %s\n", modbus_strerror(errno));
                exit(1);
            }

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

            /* Enable libmodbus debug to print raw frames */
                /* modbus_set_debug(ctx, TRUE); */ /* Disabled - causes timing issues */
            /* Configure RTS delay and timeouts from #define */
            modbus_rtu_set_rts_delay(ctx, ORNO_RTS_DELAY);
            modbus_set_response_timeout(ctx, 0, 900000); /* 0.9s */
            modbus_set_byte_timeout(ctx, 0, ORNO_BYTE_TIMEOUT);

            /* Display current configuration */
            int rts_delay = modbus_rtu_get_rts_delay(ctx);
            uint32_t response_timeout_sec, response_timeout_usec;
            uint32_t byte_timeout_sec, byte_timeout_usec;
            modbus_get_response_timeout(ctx, &response_timeout_sec, &response_timeout_usec);
            modbus_get_byte_timeout(ctx, &byte_timeout_sec, &byte_timeout_usec);
            printf("ORNO: RTS Delay        %u us\n", rts_delay);
            printf("ORNO: Response Timeout %u,%06u s\n", response_timeout_sec, response_timeout_usec);
            printf("ORNO: Byte     Timeout %u,%06u s\n", byte_timeout_sec, byte_timeout_usec);

            uint16_t reg[32]; // will store read registers values
            uint16_t reg2[2]; // will store read registers values
            float d = 0.0;

            int num = 0;
            int proba = 0;
            float U1, U2, U3;
            float I1, I2, I3;
            float P_Tot, P1, P2, P3;
            float W_Tot, W1, W2, W3;
            float Freq;

            long l = 0;

            /* Flush serial buffer and wait for device to be ready */
            modbus_flush(ctx);
            usleep(100000); /* 100ms delay before first read */

            memset((char *)reg, 0, sizeof(reg));
            //NAPIECIA
            num = modbus_read_timed(ctx, 0xe, 6, reg); // ORNO
            //        num = modbus_read_registers(ctx, 32072, 2, reg); // SUN2000
            if (num != 6)
            { // number of read registers is not the one expected
                printf("ORNO: %x \t\n", 0xe);
                printf("ORNO: Failed to read: %s\n", modbus_strerror(errno));
            }
            else
            {
                U1 = modbus_get_float_abcd(&reg[0]);
                U2 = modbus_get_float_abcd(&reg[2]);
                U3 = modbus_get_float_abcd(&reg[4]);
                printf("ORNO: Voltages: L1=%.1f V, L2=%.1f V, L3=%.1f V\n", U1, U2, U3);

                // Add to circular buffer
                voltage_buffer_L1[voltage_buffer_index] = U1;
                voltage_buffer_L2[voltage_buffer_index] = U2;
                voltage_buffer_L3[voltage_buffer_index] = U3;
                voltage_buffer_index = (voltage_buffer_index + 1) % VOLTAGE_BUFFER_SIZE;
                if (voltage_buffer_items < VOLTAGE_BUFFER_SIZE) {
                    voltage_buffer_items++;
                }

                float avg_L1 = 0, avg_L2 = 0, avg_L3 = 0;
                for (int i = 0; i < voltage_buffer_items; i++) {
                    avg_L1 += voltage_buffer_L1[i];
                    avg_L2 += voltage_buffer_L2[i];
                    avg_L3 += voltage_buffer_L3[i];
                }
                avg_L1 /= voltage_buffer_items;
                avg_L2 /= voltage_buffer_items;
                avg_L3 /= voltage_buffer_items;

                if (fabs(U1 - avg_L1) > avg_L1 * VOLTAGE_FLUCTUATION_THRESHOLD ||
                    fabs(U2 - avg_L2) > avg_L2 * VOLTAGE_FLUCTUATION_THRESHOLD ||
                    fabs(U3 - avg_L3) > avg_L3 * VOLTAGE_FLUCTUATION_THRESHOLD) {
                    printf("ORNO: Voltage fluctuation detected! Switching to high frequency polling for %d seconds.\n", HIGH_FREQ_MODE_DURATION);
                    high_frequency_mode_end_time = time(NULL) + HIGH_FREQ_MODE_DURATION;
                }

                /* Validate voltages (150V - 280V is reasonable range for EU grid) */
                if (is_valid_float(U1, 150.0, 280.0) &&
                    is_valid_float(U2, 150.0, 280.0) &&
                    is_valid_float(U3, 150.0, 280.0)) {
                    mqtt_send_U(U1, U2, U3);
                    influx_send_U(U1, U2, U3);
                    printf("ORNO: MQTT: Published voltages\n");
                } else {
                    printf("ORNO: MQTT: Skipping voltages - invalid values\n");
                }
            }

            usleep(100000); /* 100ms delay between queries */

            // PRADY (Currents)
            memset((char *)reg, 0, sizeof(reg));
            num = modbus_read_timed(ctx, 0x16, 6, reg); // ORNO
            if (num != 6)
            {
                printf("ORNO: %x \t\n", 0x16);
                printf("ORNO: Failed to read currents: %s\n", modbus_strerror(errno));
            }
            else
            {
                I1 = modbus_get_float_abcd(&reg[0]);
                I2 = modbus_get_float_abcd(&reg[2]);
                I3 = modbus_get_float_abcd(&reg[4]);
                printf("ORNO: Currents: L1=%.2f A, L2=%.2f A, L3=%.2f A\n", I1, I2, I3);

                /* Validate currents (0-100A is reasonable range for home use) */
                if (is_valid_float(I1, 0.0, 100.0) &&
                    is_valid_float(I2, 0.0, 100.0) &&
                    is_valid_float(I3, 0.0, 100.0)) {
                    mqtt_send_I(I1, I2, I3);
                    influx_send_I(I1, I2, I3);
                    printf("ORNO: MQTT: Published currents\n");
                } else {
                    printf("ORNO: MQTT: Skipping currents - invalid values\n");
                }
            }

            usleep(100000); /* 100ms delay between queries */

            memset((char *)reg, 0, sizeof(reg));
            num = modbus_read_timed(ctx, 0x1c, 8, reg);
            if (num != 8)
            { // number of read registers is not the one expected
                printf("ORNO: %x \t\n", 0x1c);
                printf("ORNO: Failed to read: %s\n", modbus_strerror(errno));
            }
            else
            {
                P_Tot = modbus_get_float_abcd(&reg[0]);
                P1 = modbus_get_float_abcd(&reg[2]);
                P2 = modbus_get_float_abcd(&reg[4]);
                P3 = modbus_get_float_abcd(&reg[6]);
                printf("ORNO: Power: Total=%.3f W, L1=%.3f W, L2=%.3f W, L3=%.3f W\n", P_Tot, P1, P2, P3);

                /* Validate power (-25000W to +25000W, allows negative for generation) */
                if (is_valid_float(P_Tot, -25000.0, 25000.0) &&
                    is_valid_float(P1, -10000.0, 10000.0) &&
                    is_valid_float(P2, -10000.0, 10000.0) &&
                    is_valid_float(P3, -10000.0, 10000.0)) {
                    mqtt_send_P(P_Tot, P1, P2, P3);
                    influx_send_P(P_Tot, P1, P2, P3);
                    printf("ORNO: MQTT: Published power\n");
                } else {
                    printf("ORNO: MQTT: Skipping power - invalid values\n");
                }
            }
            usleep(100000); /* 100ms delay between queries */

            memset((char *)reg, 0, sizeof(reg));
            num = modbus_read_timed(ctx, 0x100, 8, reg);
            if (num != 8)
            { // number of read registers is not the one expected
                printf("ORNO: %x \t\n", 0x100);
                printf("ORNO: Failed to read: %s\n", modbus_strerror(errno));
            }
            else
            {
                W_Tot = modbus_get_float_abcd(&reg[0]);
                W1 = modbus_get_float_abcd(&reg[2]);
                W2 = modbus_get_float_abcd(&reg[4]);
                W3 = modbus_get_float_abcd(&reg[6]);
                printf("ORNO: Energy: Total=%.3f kWh, L1=%.3f kWh, L2=%.3f kWh, L3=%.3f kWh\n", W_Tot, W1, W2, W3);

                /* Validate energy (0-1000000 kWh is reasonable range) */
                if (is_valid_float(W_Tot, 0.0, 1000000.0) &&
                    is_valid_float(W1, 0.0, 1000000.0) &&
                    is_valid_float(W2, 0.0, 1000000.0) &&
                    is_valid_float(W3, 0.0, 1000000.0)) {
                    mqtt_send_W(W_Tot, W1, W2, W3);
                    influx_send_W(W_Tot, W1, W2, W3);
                    printf("ORNO: MQTT: Published energy\n");
                } else {
                    printf("ORNO: MQTT: Skipping energy - invalid values\n");
                }
            }

            usleep(10000); /* 10ms delay between queries */

            // CZESTOTLIWOSC (Frequency)
            memset((char *)reg, 0, sizeof(reg));
            num = modbus_read_timed(ctx, 0x14, 2, reg); // ORNO
            if (num != 2)
            {
                printf("ORNO: %x \t\n", 0x14);
                printf("ORNO: Failed to read frequency: %s\n", modbus_strerror(errno));
            }
            else
            {
                Freq = modbus_get_float_abcd(&reg[0]);
                printf("ORNO: Frequency: %.4f Hz\n", Freq);

                /* Validate frequency (47-53 Hz is reasonable range for EU/US grid) */
                if (is_valid_float(Freq, 47.0, 53.0)) {
                    mqtt_send_Hz(Freq);
                    influx_send_Hz(Freq);
                    printf("ORNO: MQTT: Published frequency\n");
                } else {
                    printf("ORNO: MQTT: Skipping frequency - invalid value\n");
                }
            }

            modbus_close(ctx);
            modbus_free(ctx);
        }
        if (do_sun2k)
        {
            /* Delay between ORNO and SUN2K as configured */
            if (do_orno) {
                printf("\nSUN2K: Waiting %d ms before SUN2K...\n\n", DELAY_BETWEEN_DEVICES/1000);
                usleep(DELAY_BETWEEN_DEVICES);
            }

            time_t rawtime;
            struct tm *timeinfo;

            time(&rawtime);
            timeinfo = localtime(&rawtime);
            printf("SUN2K: === SUN2K Reading Started: %s", asctime(timeinfo));
            //Create a new RTU context with proper serial parameters (in this example,
            //device name /dev/ttyS0, baud rate 9600, no parity bit, 8 data bits, 1 stop bit)
            modbus_t *ctx = modbus_new_rtu(USB_DEV, 9600, 'N', 8, 1);
            if (!ctx)
            {
                fprintf(stderr, "SUN2K: Failed to create the context: %s\n", modbus_strerror(errno));
                exit(1);
            }
            //Set the Modbus address of the remote slave (to 3)
            modbus_set_slave(ctx, SUN2000_SLAVE);

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

            /* Configure timeouts for SUN2K from #define */
            modbus_rtu_set_rts_delay(ctx, SUN2K_RTS_DELAY);
            modbus_set_response_timeout(ctx, 0, 900000); /* 0.9s */
            modbus_set_byte_timeout(ctx, 0, SUN2K_BYTE_TIMEOUT);

            /* Display current configuration */
            int rts_delay = modbus_rtu_get_rts_delay(ctx);
            uint32_t response_timeout_sec, response_timeout_usec;
            uint32_t byte_timeout_sec, byte_timeout_usec;
            modbus_get_response_timeout(ctx, &response_timeout_sec, &response_timeout_usec);
            modbus_get_byte_timeout(ctx, &byte_timeout_sec, &byte_timeout_usec);
            printf("SUN2K: RTS Delay        %u us\n", rts_delay);
            printf("SUN2K: Response Timeout %u,%06u s\n", response_timeout_sec, response_timeout_usec);
            printf("SUN2K: Byte     Timeout %u,%06u s\n", byte_timeout_sec, byte_timeout_usec);

            /* Flush serial buffer and wait for device to be ready */
            modbus_flush(ctx);
            usleep(100000); /* 100ms delay before first read */

            /* Enable libmodbus debug to print raw frames */
                /* libmodbus debug disabled for normal runs */
                /* modbus_set_debug(ctx, TRUE); */

            uint16_t reg[32]; // will store read registers values
            int num = 0;
            int proba = 0;
            char str_buf[32];

            for (int i = 0; i < sizeof(mbReg) / sizeof(t_mb_reg); i++)
            {
                memset(str_buf, 0, sizeof(str_buf));
                proba = 0;
                do
                {
                    num = modbus_read_timed(ctx, mbReg[i].ireg, mbReg[i].num_reg, reg);
                } while ((num != mbReg[i].num_reg) && (proba++ < 10));

                printf("SUN2K: %10i\t", mbReg[i].ireg);
                printf("%u\t", reg[0]);
                if (mbReg[i].num_reg == 2)
                    printf("%u\t", reg[1]);
                else
                    printf("\t");
                if (num != mbReg[i].num_reg)
                { // number of read registers is not the one expected
                    printf("%20.20s \t", mbReg[i].reg_id);
                    printf("SUN2K: Failed to %i read: %s\n", proba, modbus_strerror(errno));
		    continue;
                }
                else
                {
                    printf("SUN2K: %20.20s \t", mbReg[i].reg_id);
                    switch (mbReg[i].type)
                    {
                    case rgStr:
                    {
                        for (int j = 0; j < mbReg[i].num_reg; j++)
                        {
                            printf("%c%c", (reg[j] / 256), (reg[j] % 256));
                        }
                        printf("\n");
                        break;
                    }
                    case rgData:
                    {
                        time_t td = reg[0] * 65536 + reg[1];
                        timeinfo = localtime(&td);
                        printf("%s", asctime(timeinfo));
                        sprintf(str_buf, "\"%s\"", asctime(timeinfo));
                        str_buf[strlen(str_buf) - 2] = '"';
                        str_buf[strlen(str_buf) - 1] = '\0';
                        break;
                    }
                    case rgI32:
                    {
                        int dh = reg[0];
                        int dl = reg[1];
                        int d = (dh << 16) + dl;
                        if (mbReg[i].scale == 1)
                        {
                            printf("%i", d);
                            sprintf(str_buf, "%i", d);
                        }
                        else
                        {
                            double f = d;
                            f /= mbReg[i].scale;
                            printf("%lf", f);
                            sprintf(str_buf, "%lf", f);
                        }
                        break;
                    }
                    case rgI16:
                    {
                        int d = reg[0];
                        if (mbReg[i].scale == 1)
                        {
                            printf("%i", d);
                            sprintf(str_buf, "%i", d);
                        }
                        else
                        {
                            double f = d;
                            f /= mbReg[i].scale;
                            printf("%lf", f);
                            sprintf(str_buf, "%lf", f);
                        }
                        break;
                    }
                    case rgU32:
                    {
                        unsigned int dh = reg[0];
                        unsigned int dl = reg[1];
                        unsigned int d = (dh << 16) + dl;

                        if (mbReg[i].scale == 1)
                        {
                            printf("%i", d);
                            sprintf(str_buf, "%i", d);
                        }
                        else
                        {
                            double f = d;
                            f /= mbReg[i].scale;
                            printf("%lf", f);
                            sprintf(str_buf, "%lf", f);
                        }
                        break;
                    }
                    case rgU16:
                    {
                        unsigned int d = reg[0];
                        if (mbReg[i].scale == 1)
                        {
                            printf("%i", d);
                            sprintf(str_buf, "%i", d);
                        }
                        else
                        {
                            double f = d;
                            f /= mbReg[i].scale;
                            printf("%lf", f);
                            sprintf(str_buf, "%lf", f);
                        }
                        break;
                    }
                    default:
                    {
                        for (int j = 0; j < mbReg[i].num_reg; j++)
                        {
                            printf("%i:", reg[j]);
                            sprintf(str_buf, "%i", reg[j]);
                        }
                        break;
                    }
                    }
                    if (mbReg[i].type != rgData)
                        printf(" %s\n", mbReg[i].units);
                }
                mqtt_send_SUN2K(&mbReg[i], str_buf);
                influx_send_SUN2K(&mbReg[i], str_buf);
                usleep(10000); /* 10ms delay between queries */
            }
            modbus_close(ctx);
            modbus_free(ctx);
        }
        if (READ_LOOP) {
            if (time(NULL) < high_frequency_mode_end_time) {
                usleep(10000); // 10ms
            } else {
                sleep(5); // 5s
            }
        }
    } while (READ_LOOP);
    //mosq_test();
    return 0;
}