#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <modbus/modbus-rtu.h>
#include <errno.h>
#include <time.h>
#include <unistd.h>
#include <sys/time.h>
#include <math.h>

/* Konfiguracja */
#define USB_DEV_DEFAULT "/dev/ttyUSB0"
#define ORNO_SLAVE_ADR 2
#define SUN2000_SLAVE_ADR 3
#define NUM_READINGS 10  /* Liczba odczytów do testu */

/* Parametry timing dla ORNO */
#define ORNO_RTS_DELAY 5000
#define ORNO_BYTE_TIMEOUT 2500

/* Parametry timing dla SUN2K */
#define SUN2K_RTS_DELAY 5000
#define SUN2K_BYTE_TIMEOUT 2500

/* Struktura do przechowywania wyników testu */
typedef struct {
    long total_time_us;
    long min_time_us;
    long max_time_us;
    double avg_time_ms;
    int successful_reads;
    int total_reads;
} test_results_t;

/* Timed wrapper for modbus_read_registers - logs start/stop and duration */
long 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 elapsed_us = (t1.tv_sec - t0.tv_sec) * 1000000 + (t1.tv_usec - t0.tv_usec);

    if (res < 0) {
        printf("MBUS: modbus_read_registers addr=0x%X nb=%d -> ERR (%s) elapsed=%ldus\n",
               addr, nb, modbus_strerror(errno), elapsed_us);
    } else {
        printf("MBUS: modbus_read_registers addr=0x%X nb=%d -> OK (%d) elapsed=%ldus\n",
               addr, nb, res, elapsed_us);
    }
    return elapsed_us;
}

/* Test function for ORNO voltage reading */
int test_orno_voltage(modbus_t *ctx, test_results_t *results) {
    uint16_t reg[6];
    long elapsed_time;

    results->total_time_us = 0;
    results->min_time_us = LONG_MAX;
    results->max_time_us = 0;
    results->successful_reads = 0;
    results->total_reads = NUM_READINGS;

    printf("\n=== TESTING ORNO VOLTAGE READING ===\n");
    printf("Performing %d voltage readings...\n", NUM_READINGS);

    for (int i = 0; i < NUM_READINGS; i++) {
        printf("Reading %d: ", i+1);
        elapsed_time = modbus_read_timed(ctx, 0x0e, 6, reg);

        if (elapsed_time > 0) {
            results->total_time_us += elapsed_time;
            if (elapsed_time < results->min_time_us) results->min_time_us = elapsed_time;
            if (elapsed_time > results->max_time_us) results->max_time_us = elapsed_time;
            results->successful_reads++;

            float U1 = modbus_get_float_abcd(&reg[0]);
            float U2 = modbus_get_float_abcd(&reg[2]);
            float U3 = modbus_get_float_abcd(&reg[4]);
            printf("Voltages: L1=%.1fV L2=%.1fV L3=%.1fV\n", U1, U2, U3);
        }

        usleep(10000); // 10ms delay between reads
    }

    if (results->successful_reads > 0) {
        results->avg_time_ms = (double)results->total_time_us / results->successful_reads / 1000.0;
    } else {
        results->avg_time_ms = 0;
        results->min_time_us = 0;
        results->max_time_us = 0;
    }

    return 0;
}

/* Test function for SUN2K voltage reading */
int test_sun2k_voltage(modbus_t *ctx, test_results_t *results) {
    uint16_t reg[3];
    long elapsed_time;

    results->total_time_us = 0;
    results->min_time_us = LONG_MAX;
    results->max_time_us = 0;
    results->successful_reads = 0;
    results->total_reads = NUM_READINGS;

    printf("\n=== TESTING SUN2K VOLTAGE READING ===\n");
    printf("Performing %d voltage readings...\n", NUM_READINGS);

    for (int i = 0; i < NUM_READINGS; i++) {
        printf("Reading %d: ", i+1);
        elapsed_time = modbus_read_timed(ctx, 32069, 3, reg);

        if (elapsed_time > 0) {
            results->total_time_us += elapsed_time;
            if (elapsed_time < results->min_time_us) results->min_time_us = elapsed_time;
            if (elapsed_time > results->max_time_us) results->max_time_us = elapsed_time;
            results->successful_reads++;

            float U_A = (float)reg[0] / 10.0;
            float U_B = (float)reg[1] / 10.0;
            float U_C = (float)reg[2] / 10.0;
            printf("Voltages: U_A=%.1fV U_B=%.1fV U_C=%.1fV\n", U_A, U_B, U_C);
        }

        usleep(10000); // 10ms delay between reads
    }

    if (results->successful_reads > 0) {
        results->avg_time_ms = (double)results->total_time_us / results->successful_reads / 1000.0;
    } else {
        results->avg_time_ms = 0;
        results->min_time_us = 0;
        results->max_time_us = 0;
    }

    return 0;
}

/* Initialize ORNO modbus context */
modbus_t* init_orno_context(const char *device, int slave_addr) {
    modbus_t *ctx = modbus_new_rtu(device, 9600, 'E', 8, 1);
    if (!ctx) {
        fprintf(stderr, "ORNO: Failed to create the context: %s\n", modbus_strerror(errno));
        return NULL;
    }

    modbus_set_slave(ctx, slave_addr);

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

    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);

    printf("ORNO: Context initialized successfully\n");
    return ctx;
}

/* Initialize SUN2K modbus context */
modbus_t* init_sun2k_context(const char *device, int slave_addr) {
    modbus_t *ctx = modbus_new_rtu(device, 9600, 'N', 8, 1);
    if (!ctx) {
        fprintf(stderr, "SUN2K: Failed to create the context: %s\n", modbus_strerror(errno));
        return NULL;
    }

    modbus_set_slave(ctx, slave_addr);

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

    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);

    printf("SUN2K: Context initialized successfully\n");
    return ctx;
}

/* Print test results */
void print_results(const char *device_name, test_results_t *results) {
    printf("\n=== %s TEST RESULTS ===\n", device_name);
    printf("Total readings: %d\n", results->total_reads);
    printf("Successful readings: %d\n", results->successful_reads);
    printf("Success rate: %.1f%%\n",
           (results->total_reads > 0) ? (100.0 * results->successful_reads / results->total_reads) : 0);

    if (results->successful_reads > 0) {
        printf("Average time per reading: %.2f ms\n", results->avg_time_ms);
        printf("Minimum time: %.2f ms\n", results->min_time_us / 1000.0);
        printf("Maximum time: %.2f ms\n", results->max_time_us / 1000.0);
        printf("Total time for all readings: %.2f ms\n", results->total_time_us / 1000.0);
    } else {
        printf("No successful readings!\n");
    }
    printf("========================\n\n");
}

/* Compare devices and recommend faster one */
void compare_and_recommend(test_results_t *orno_results, test_results_t *sun2k_results) {
    printf("=== COMPARISON AND RECOMMENDATION ===\n");

    if (orno_results->successful_reads == 0 && sun2k_results->successful_reads == 0) {
        printf("ERROR: Neither device could be read successfully!\n");
        return;
    }

    if (orno_results->successful_reads > 0 && sun2k_results->successful_reads > 0) {
        printf("Both devices are working. Comparison:\n");
        printf("- ORNO average time: %.2f ms\n", orno_results->avg_time_ms);
        printf("- SUN2K average time: %.2f ms\n", sun2k_results->avg_time_ms);

        if (orno_results->avg_time_ms < sun2k_results->avg_time_ms) {
            printf("RECOMMENDATION: ORNO is faster by %.2f ms per reading\n",
                   sun2k_results->avg_time_ms - orno_results->avg_time_ms);
        } else if (sun2k_results->avg_time_ms < orno_results->avg_time_ms) {
            printf("RECOMMENDATION: SUN2K is faster by %.2f ms per reading\n",
                   orno_results->avg_time_ms - sun2k_results->avg_time_ms);
        } else {
            printf("RECOMMENDATION: Both devices have similar performance\n");
        }
    } else if (orno_results->successful_reads > 0) {
        printf("RECOMMENDATION: Only ORNO is working properly. Use ORNO for fast voltage readings.\n");
    } else if (sun2k_results->successful_reads > 0) {
        printf("RECOMMENDATION: Only SUN2K is working properly. Use SUN2K for fast voltage readings.\n");
    }

    printf("=====================================\n");
}

int main(int argc, char *argv[]) {
    const char *USB_DEV = USB_DEV_DEFAULT;
    int ORNO_SLAVE = ORNO_SLAVE_ADR;
    int SUN2000_SLAVE = SUN2000_SLAVE_ADR;

    test_results_t orno_results = {0};
    test_results_t sun2k_results = {0};

    printf("==============================================\n");
    printf("Voltage Reading Speed Test\n");
    printf("Testing ORNO vs SUN2K voltage reading speed\n");
    printf("==============================================\n");
    printf("Configuration:\n");
    printf("  Device: %s\n", USB_DEV);
    printf("  ORNO slave address: %d\n", ORNO_SLAVE);
    printf("  SUN2K slave address: %d\n", SUN2000_SLAVE);
    printf("  Number of readings per device: %d\n", NUM_READINGS);
    printf("==============================================\n\n");

    // Test ORNO
    modbus_t *orno_ctx = init_orno_context(USB_DEV, ORNO_SLAVE);
    if (orno_ctx) {
        test_orno_voltage(orno_ctx, &orno_results);
        modbus_close(orno_ctx);
        modbus_free(orno_ctx);
    } else {
        printf("ORNO: Could not initialize context. Skipping ORNO tests.\n");
        orno_results.successful_reads = 0;
    }

    usleep(100000); // 100ms delay between devices

    // Test SUN2K
    modbus_t *sun2k_ctx = init_sun2k_context(USB_DEV, SUN2000_SLAVE);
    if (sun2k_ctx) {
        test_sun2k_voltage(sun2k_ctx, &sun2k_results);
        modbus_close(sun2k_ctx);
        modbus_free(sun2k_ctx);
    } else {
        printf("SUN2K: Could not initialize context. Skipping SUN2K tests.\n");
        sun2k_results.successful_reads = 0;
    }

    // Print results
    if (orno_results.successful_reads > 0) {
        print_results("ORNO", &orno_results);
    }

    if (sun2k_results.successful_reads > 0) {
        print_results("SUN2K", &sun2k_results);
    }

    // Compare and recommend
    compare_and_recommend(&orno_results, &sun2k_results);

    printf("\nTest completed.\n");
    return 0;
}