#include <fcntl.h>
#include <stdio.h>
#include <string.h>
#include <string>
#include <unistd.h>
#include "inverter.h"
#include "tools.h"
#include "main.h"

#include <vector>

#include <fcntl.h>
#include <termios.h>

cInverter::cInverter(std::string devicename, int qpiri, int qpiws, int qmod, int qpigs) {
    device = devicename;
    status1[0] = 0;
    status2[0] = 0;
    warnings[0] = 0;
    mode = 0;
    buf_qpiri = qpiri;
    buf_qpiws = qpiws;
    buf_qmod = qmod;
    buf_qpigs = qpigs;
    last_reply_size = 0;
    lprintf("INVERTER: Initialized with buffer sizes - QPIRI:%d QPIWS:%d QMOD:%d QPIGS:%d", 
            buf_qpiri, buf_qpiws, buf_qmod, buf_qpigs);
}

string *cInverter::GetQpigsStatus() {
    m.lock();
    string *result = new string(status1);
    m.unlock();
    return result;
}

string *cInverter::GetQpiriStatus() {
    m.lock();
    string *result = new string(status2);
    m.unlock();
    return result;
}

string *cInverter::GetWarnings() {
    m.lock();
    string *result = new string(warnings);
    m.unlock();
    return result;
}

void cInverter::SetMode(char newmode) {
    m.lock();
    if (mode && newmode != mode)
        ups_status_changed = true;
    mode = newmode;
    m.unlock();
}

int cInverter::GetMode() {
    int result;
    m.lock();

    switch (mode) {
        case 'P': result = 1;   break;  // Power_On
        case 'S': result = 2;   break;  // Standby
        case 'L': result = 3;   break;  // Line
        case 'B': result = 4;   break;  // Battery
        case 'F': result = 5;   break;  // Fault
        case 'H': result = 6;   break;  // Power_Saving
        default:  result = 0;   break;  // Unknown
    }

    m.unlock();
    return result;
}

bool cInverter::query(const char *cmd, int replysize) {
    time_t started;
    int fd;
    int i=0, n;

    fd = open(this->device.data(), O_RDWR | O_NOCTTY);
    if (fd == -1) {
        lprintf("INVERTER: Unable to open device file (errno=%d %s)", errno, strerror(errno));
        sleep(5);
        return false;
    }


    // Once connected, set the baud rate and other serial config (Don't rely on this being correct on the system by default...)
    speed_t baud = B2400;

    // Speed settings (in this case, 2400 8N1)
    struct termios settings;
    tcgetattr(fd, &settings);

    cfsetospeed(&settings, baud);      // baud rate
    cfsetispeed(&settings, baud);      // input baud rate
    settings.c_cflag &= ~PARENB;       // no parity
    settings.c_cflag &= ~CSTOPB;       // 1 stop bit
    settings.c_cflag &= ~CSIZE;
    settings.c_cflag |= CS8 | CLOCAL | CREAD;  // 8 bits, local, enable receiver
    settings.c_iflag = 0;              // raw input
    settings.c_oflag = 0;              // raw output
    settings.c_lflag = 0;              // no canonical/echo
    settings.c_cc[VMIN] = 0;
    settings.c_cc[VTIME] = 1;          // 0.1s read timeout

    tcsetattr(fd, TCSANOW, &settings); // apply the settings
    
    // CRITICAL: Flush both input and output buffers to clear any residual data
    tcflush(fd, TCIOFLUSH);
    
    // Wait a bit for the device to settle after flush
    usleep(100000); // 100ms delay

    // Clear internal buffer
    memset(buf, 0, sizeof(buf));

    // ---------------------------------------------------------------

    // Generating CRC for a command
    uint16_t crc = cal_crc_half((uint8_t*)cmd, strlen(cmd));
    n = strlen(cmd);
    memcpy(&buf, cmd, n);
    lprintf("INVERTER: Current CRC: %X %X", crc >> 8, crc & 0xff);

    buf[n++] = crc >> 8;
    buf[n++] = crc & 0xff;
    buf[n++] = 0x0d;

    //send a command
    int written = write(fd, &buf, n);
    if (written != n) {
        lprintf("INVERTER: %s write failed (wrote %d of %d bytes)", cmd, written, n);
        close(fd);
        return false;
    }
    
    // Flush output to ensure command is sent
    tcdrain(fd);
    
    // Critical delay after write (like Python implementation)
    usleep(500000); // 500ms delay
    
    // Clear buffer again before reading
    memset(buf, 0, sizeof(buf));
    time(&started);

    bool started_frame = false;
    unsigned char ch = 0;
    int max_read = (int)sizeof(buf) - 1;

    do {
        n = read(fd, (void*)&ch, 1); // Read one byte at a time for reliable terminator detection
        if (n < 0) {
            if (time(NULL) - started > 2) {
                lprintf("INVERTER: %s read timeout", cmd);
                break;
            } else {
                usleep(10000); // 10ms
                continue;
            }
        }
        
        if (n > 0) {
            if (!started_frame) {
                if (ch != '(') {
                    continue;
                }
                started_frame = true;
                buf[0] = ch;
                i = 1;
                continue;
            }

            buf[i++] = ch;

            // Check if we've received the terminator (CR or LF)
            if (ch == 0x0d || ch == 0x0a) {
                lprintf("INVERTER: %s received terminator (0x%02X) at byte %d", cmd, ch, i);
                break;
            }

            if (i >= (int)sizeof(buf) - 1) {
                lprintf("INVERTER: %s buffer full before terminator", cmd);
                break;
            }
        }

    } while ((i < max_read || !started_frame) && (time(NULL) - started < 5));
    close(fd);

    if (i > 0 && started_frame) {
        last_reply_size = i;

        lprintf("INVERTER: %s reply size (%d bytes, expected %d)", cmd, i, replysize);

        // Dump raw buffer for debugging
        if (debugFlag) {
            lprintf("INVERTER: Raw buffer hex dump (first 50 bytes):");
            for(int x = 0; x < (i < 50 ? i : 50); x++) {
                fprintf(stderr, "%02X ", buf[x]);
                if ((x+1) % 16 == 0) fprintf(stderr, "\n");
            }
            fprintf(stderr, "\n");
        }

        if (buf[0]!='(' ) {
            lprintf("INVERTER: %s: incorrect start byte (got 0x%02X, expected '(').  Buffer: %s", cmd, buf[0], buf);
            return false;
        }
        
        if (buf[i-1]!=0x0d && buf[i-1]!=0x0a) {
            lprintf("INVERTER: %s: incorrect stop byte (got 0x%02X at pos %d, expected CR or LF).  Buffer: %s", cmd, buf[i-1], i-1, buf);
            return false;
        }
        
        if (!(CheckCRC(buf, i))) {
            lprintf("INVERTER: %s: CRC Failed!  Reply size: %d  Buffer: %s", cmd, i, buf);
            return false;
        }

        buf[i-3] = '\0'; //nullterminating on first CRC byte
        lprintf("INVERTER: %s: %d bytes read: %s", cmd, i, buf);

        // Treat NAK as a failed query
        if (strncmp((const char*)buf + 1, "NAK", 3) == 0) {
            lprintf("INVERTER: %s: NAK reply", cmd);
            return false;
        }

        lprintf("INVERTER: %s query finished", cmd);
        
        // If expected size doesn't match actual size, log it
        if (i != replysize) {
            lprintf("INVERTER: WARNING - %s actual size (%d) differs from configured size (%d)", cmd, i, replysize);
            lprintf("INVERTER: SUGGESTION - Update inverter.conf to set the buffer size to %d", i);
        }
        
        return true;
    } else {
        last_reply_size = 0;
        lprintf("INVERTER: %s reply too short (%d bytes)", cmd, i);
        return false;
    }
}

void cInverter::poll() {
    int n,j;
    
    fprintf(stderr, "[POLL] Thread started, runOnce=%s\n", runOnce ? "true" : "false");

    while (true) {

        // Reading mode
        if (!ups_qmod_changed) {
            fprintf(stderr, "[POLL] Reading QMOD...\n");
            if (query("QMOD", buf_qmod)) {
                SetMode(buf[1]);
                ups_qmod_changed = true;
                fprintf(stderr, "[POLL] QMOD completed\n");
            }
            usleep(200000); // allow inverter to settle between commands
        }

        // reading status (QPIGS)
        if (!ups_qpigs_changed) {
            fprintf(stderr, "[POLL] Reading QPIGS...\n");
            if (query("QPIGS", buf_qpigs)) {
                m.lock();
                strcpy(status1, (const char*)buf+1);
                m.unlock();
                ups_qpigs_changed = true;
                fprintf(stderr, "[POLL] QPIGS completed\n");
            }
            usleep(200000); // allow inverter to settle between commands
        }

        // Reading QPIRI status
        if (!ups_qpiri_changed) {
            fprintf(stderr, "[POLL] Reading QPIRI...\n");
            if (query("QPIRI", buf_qpiri)) {
                m.lock();
                strcpy(status2, (const char*)buf+1);
                m.unlock();
                ups_qpiri_changed = true;
                fprintf(stderr, "[POLL] QPIRI completed\n");
            }
            usleep(200000); // allow inverter to settle between commands
        }

        // Get any device warnings...
        if (!ups_qpiws_changed) {
            fprintf(stderr, "[POLL] Reading QPIWS...\n");
            if (query("QPIWS", buf_qpiws)) {
                m.lock();
                strcpy(warnings, (const char*)buf+1);
                m.unlock();
                ups_qpiws_changed = true;
                fprintf(stderr, "[POLL] QPIWS completed\n");
            }
            usleep(200000); // allow inverter to settle between commands
        }

        // If runOnce mode and all data collected, exit the thread
        if (runOnce && ups_qmod_changed && ups_qpigs_changed && ups_qpiri_changed && ups_qpiws_changed) {
            fprintf(stderr, "[POLL] All data collected, exiting (run-once mode)\n");
            return;
        }

        fprintf(stderr, "[POLL] Flags: QMOD=%d QPIGS=%d QPIRI=%d QPIWS=%d, sleeping...\n", 
                ups_qmod_changed.load(), ups_qpigs_changed.load(), 
                ups_qpiri_changed.load(), ups_qpiws_changed.load());

        sleep(2);
    }
}

void cInverter::ExecuteCmd(const string cmd) {
    // Sending any command raw - use larger buffer to accept full responses
    if (query(cmd.data(), 200)) {
        m.lock();
        strcpy(status2, (const char*)buf+1);
        m.unlock();
    }
}

uint16_t cInverter::cal_crc_half(uint8_t *pin, uint8_t len) {
    uint16_t crc;

    uint8_t da;
    uint8_t *ptr;
    uint8_t bCRCHign;
    uint8_t bCRCLow;

    uint16_t crc_ta[16]= {
        0x0000,0x1021,0x2042,0x3063,0x4084,0x50a5,0x60c6,0x70e7,
        0x8108,0x9129,0xa14a,0xb16b,0xc18c,0xd1ad,0xe1ce,0xf1ef
    };

    ptr=pin;
    crc=0;

    while(len--!=0) {
        da=((uint8_t)(crc>>8))>>4;
        crc<<=4;
        crc^=crc_ta[da^(*ptr>>4)];
        da=((uint8_t)(crc>>8))>>4;
        crc<<=4;
        crc^=crc_ta[da^(*ptr&0x0f)];
        ptr++;
    }

    bCRCLow = crc;
    bCRCHign= (uint8_t)(crc>>8);

    if(bCRCLow==0x28||bCRCLow==0x0d||bCRCLow==0x0a)
        bCRCLow++;
    if(bCRCHign==0x28||bCRCHign==0x0d||bCRCHign==0x0a)
        bCRCHign++;

    crc = ((uint16_t)bCRCHign)<<8;
    crc += bCRCLow;
    return(crc);
}

bool cInverter::CheckCRC(unsigned char *data, int len) {
    uint16_t crc = cal_crc_half(data, len-3);
    return data[len-3]==(crc>>8) && data[len-2]==(crc&0xff);
}

int cInverter::openSerial() {
    int fd = open(this->device.data(), O_RDWR | O_NOCTTY);
    if (fd == -1) {
        lprintf("INVERTER: Unable to open device file (errno=%d %s)", errno, strerror(errno));
        return -1;
    }

    speed_t baud = B2400;
    struct termios settings;
    tcgetattr(fd, &settings);

    cfsetospeed(&settings, baud);
    cfsetispeed(&settings, baud);
    settings.c_cflag &= ~PARENB;
    settings.c_cflag &= ~CSTOPB;
    settings.c_cflag &= ~CSIZE;
    settings.c_cflag |= CS8 | CLOCAL | CREAD;
    settings.c_iflag = 0;
    settings.c_oflag = 0;
    settings.c_lflag = 0;
    settings.c_cc[VMIN] = 0;
    settings.c_cc[VTIME] = 1;

    tcsetattr(fd, TCSANOW, &settings);
    tcflush(fd, TCIOFLUSH);
    usleep(300000);

    return fd;
}

void cInverter::closeSerial(int fd) {
    if (fd >= 0) {
        close(fd);
    }
}

bool cInverter::sendCommand(int fd, const std::string &cmd) {
    unsigned char outbuf[128];
    size_t cmd_len = cmd.size();
    if (cmd_len + 3 > sizeof(outbuf)) {
        return false;
    }

    tcflush(fd, TCIFLUSH);

    uint16_t crc = cal_crc_half((uint8_t*)cmd.data(), cmd_len);
    memcpy(outbuf, cmd.data(), cmd_len);
    outbuf[cmd_len++] = crc >> 8;
    outbuf[cmd_len++] = crc & 0xff;
    outbuf[cmd_len++] = 0x0d;

    int written = write(fd, outbuf, cmd_len);
    if (written != (int)cmd_len) {
        lprintf("INVERTER: %s write failed (wrote %d of %d bytes)", cmd.c_str(), written, (int)cmd_len);
        return false;
    }

    tcdrain(fd);
    usleep(500000);
    return true;
}

bool cInverter::readReply(int fd, std::string &payload) {
    unsigned char localbuf[1024];
    memset(localbuf, 0, sizeof(localbuf));

    time_t started;
    time(&started);

    bool started_frame = false;
    unsigned char ch = 0;
    int i = 0;

    while (i < (int)sizeof(localbuf) - 1 && (time(NULL) - started < 7)) {
        int n = read(fd, &ch, 1);
        if (n > 0) {
            if (!started_frame) {
                if (ch != '(') {
                    continue;
                }
                started_frame = true;
                localbuf[0] = ch;
                i = 1;
                continue;
            }

            if (ch == '(') {
                localbuf[0] = ch;
                i = 1;
                continue;
            }

            localbuf[i++] = ch;
            if (ch == 0x0d || ch == 0x0a) {
                break;
            }
        } else {
            usleep(10000);
        }
    }

    if (!started_frame || i <= 0) {
        return false;
    }

    if (localbuf[i-1] != 0x0d && localbuf[i-1] != 0x0a) {
        if (debugFlag) {
            lprintf("INVERTER: readReply missing terminator (size=%d)", i);
        }
        return false;
    }

    if (!CheckCRC(localbuf, i)) {
        if (debugFlag) {
            lprintf("INVERTER: readReply CRC failed (size=%d)", i);
        }
        return false;
    }

    localbuf[i-3] = '\0';
    if (strncmp((const char*)localbuf + 1, "NAK", 3) == 0) {
        return false;
    }

    payload = std::string((char*)localbuf + 1);
    last_reply_size = i;
    return true;
}

// Auto-discover the correct buffer size for a command by reading until CR
int cInverter::query_auto(const char *cmd, int max_size) {
    time_t started;
    int fd;
    int i=0, n;
    unsigned char temp_buf[1024];
    
    memset(temp_buf, 0, sizeof(temp_buf));

    fd = open(this->device.data(), O_RDWR | O_NOCTTY);
    if (fd == -1) {
        lprintf("INVERTER: Unable to open device file for auto-discovery");
        return -1;
    }

    // Configure serial port
    speed_t baud = B2400;
    struct termios settings;
    tcgetattr(fd, &settings);
    cfsetospeed(&settings, baud);
    cfsetispeed(&settings, baud);
    settings.c_cflag &= ~PARENB;
    settings.c_cflag &= ~CSTOPB;
    settings.c_cflag &= ~CSIZE;
    settings.c_cflag |= CS8 | CLOCAL | CREAD;
    settings.c_iflag = 0;
    settings.c_oflag = 0;
    settings.c_lflag = 0;
    settings.c_cc[VMIN] = 0;
    settings.c_cc[VTIME] = 5; // 0.5s read timeout
    tcsetattr(fd, TCSANOW, &settings);
    
    // Flush all buffers
    tcflush(fd, TCIOFLUSH);
    usleep(100000); // 100ms delay to ensure clean state

    // Generate and send command with CRC
    uint16_t crc = cal_crc_half((uint8_t*)cmd, strlen(cmd));
    n = strlen(cmd);
    memcpy(&temp_buf, cmd, n);
    temp_buf[n++] = crc >> 8;
    temp_buf[n++] = crc & 0xff;
    temp_buf[n++] = 0x0d;

    write(fd, &temp_buf, n);
    tcdrain(fd);
    usleep(500000); // allow inverter to respond
    
    // Clear buffer for reading
    memset(temp_buf, 0, sizeof(temp_buf));
    time(&started);

    // Read until we find CR/LF or timeout
    bool started_frame = false;
    unsigned char ch = 0;
    while (i < max_size && (time(NULL) - started < 5)) {
        n = read(fd, &ch, 1); // Read one byte at a time
        if (n > 0) {
            if (!started_frame) {
                if (ch != '(') {
                    continue;
                }
                started_frame = true;
                temp_buf[0] = ch;
                i = 1;
                continue;
            }

            temp_buf[i++] = ch;

            // Found the terminator
            if (ch == 0x0d || ch == 0x0a) {
                lprintf("INVERTER: Auto-discovery for %s: found CR at byte %d", cmd, i);
                break;
            }
        } else {
            usleep(10000); // 10ms between reads
        }
    }
    
    close(fd);
    
    // Validate the response
    if (i > 0 && temp_buf[0] == '(' && (temp_buf[i-1] == 0x0d || temp_buf[i-1] == 0x0a)) {
        if (i >= 5 && temp_buf[1] == 'N' && temp_buf[2] == 'A' && temp_buf[3] == 'K') {
            lprintf("INVERTER: Auto-discovery for %s got NAK", cmd);
            return -1;
        }
        lprintf("INVERTER: Auto-discovery for %s successful: %d bytes", cmd, i);
        return i;
    } else {
        lprintf("INVERTER: Auto-discovery for %s failed (read %d bytes)", cmd, i);
        
        // Dump what we received for debugging
        if (i > 0) {
            lprintf("INVERTER: Received data (hex):");
            for(int x = 0; x < i; x++) {
                fprintf(stderr, "%02X ", temp_buf[x]);
            }
            fprintf(stderr, "\n");
            lprintf("INVERTER: Received data (ascii):");
            for(int x = 0; x < i; x++) {
                fprintf(stderr, "%c", (temp_buf[x] >= 32 && temp_buf[x] < 127) ? temp_buf[x] : '.');
            }
            fprintf(stderr, "\n");
        }
        
        return -1;
    }
}

// Auto-discover buffer sizes for all commands
void cInverter::AutoDiscoverBufferSizes() {
    printf("\n=== AUTO-DISCOVERY MODE ===\n");
    printf("Testing inverter to find correct buffer sizes...\n\n");

    int qmod_size = query("QMOD", buf_qmod) ? last_reply_size : query_auto("QMOD", 20);
    if (qmod_size > 0) {
        printf("✓ QMOD buffer size: %d\n", qmod_size);
    } else {
        printf("✗ QMOD auto-discovery failed\n");
    }
    sleep(1);

    int qpigs_size = query("QPIGS", buf_qpigs) ? last_reply_size : query_auto("QPIGS", 150);
    if (qpigs_size > 0) {
        printf("✓ QPIGS buffer size: %d\n", qpigs_size);
    } else {
        printf("✗ QPIGS auto-discovery failed\n");
    }
    sleep(1);

    int qpiri_size = query("QPIRI", buf_qpiri) ? last_reply_size : query_auto("QPIRI", 150);
    if (qpiri_size > 0) {
        printf("✓ QPIRI buffer size: %d\n", qpiri_size);
    } else {
        printf("✗ QPIRI auto-discovery failed\n");
    }
    sleep(1);

    int qpiws_size = query("QPIWS", buf_qpiws) ? last_reply_size : query_auto("QPIWS", 100);
    if (qpiws_size > 0) {
        printf("✓ QPIWS buffer size: %d\n", qpiws_size);
    } else {
        printf("✗ QPIWS auto-discovery failed\n");
    }
    
    printf("\n=== SUGGESTED CONFIGURATION ===\n");
    printf("Update your /etc/inverter/inverter.conf with these values:\n\n");
    if (qmod_size > 0) printf("qmod=%d\n", qmod_size);
    if (qpigs_size > 0) printf("qpigs=%d\n", qpigs_size);
    if (qpiri_size > 0) printf("qpiri=%d\n", qpiri_size);
    if (qpiws_size > 0) printf("qpiws=%d\n", qpiws_size);
    printf("\n");
    
    // Output in parsable format for scripts
    printf("DISCOVERY_QMOD=%d\n", qmod_size > 0 ? qmod_size : 5);
    printf("DISCOVERY_QPIGS=%d\n", qpigs_size > 0 ? qpigs_size : 110);
    printf("DISCOVERY_QPIRI=%d\n", qpiri_size > 0 ? qpiri_size : 103);
    printf("DISCOVERY_QPIWS=%d\n", qpiws_size > 0 ? qpiws_size : 40);
    printf("DISCOVERY_SUCCESS=%s\n", (qmod_size > 0 && qpigs_size > 0 && qpiri_size > 0 && qpiws_size > 0) ? "true" : "false");
}

// Discover number of parallel inverters
int cInverter::DiscoverParallelInverters() {
    fprintf(stderr, "\n=== PARALLEL INVERTER DISCOVERY ===\n");
    fprintf(stderr, "Checking for parallel inverter configuration...\n\n");
    
    int count = 0;
    char cmd[16];
    std::string found_serials[10];  // Track unique serials
    
    // Test QPGS0 through QPGS9
    for (int i = 0; i < 10; i++) {
        snprintf(cmd, sizeof(cmd), "QPGS%d", i);
        
        if (query(cmd, 200)) {
            // Check if response is valid (not NAK)
            if (buf[0] == '(' && buf[1] != 'N') {
                // Extract serial number (starts at position 3)
                char serial[20] = {0};
                int j = 0;
                for (int k = 3; k < 17 && buf[k] != ' '; k++) {
                    serial[j++] = buf[k];
                }
                
                // Check if serial is valid (not empty, not all zeros, not "0.0")
                bool valid_serial = false;
                if (j > 0) {
                    bool all_zero = true;
                    for (int k = 0; k < j; k++) {
                        if (serial[k] != '0') {
                            all_zero = false;
                            break;
                        }
                    }

                    if (!all_zero && strcmp(serial, "0.0") != 0) {
                        valid_serial = true;
                    }
                }
                
                // Check if serial is duplicate
                bool duplicate = false;
                std::string serial_str(serial);
                for (int k = 0; k < count; k++) {
                    if (found_serials[k] == serial_str) {
                        duplicate = true;
                        break;
                    }
                }
                
                if (valid_serial && j > 0 && !duplicate) {
                    found_serials[count] = serial_str;
                    count++;
                    fprintf(stderr, "✓ Inverter #%d via %s (Serial: %s)\n", count, cmd, serial);
                    printf("INVERTER_%d_SERIAL=%s\n", count, serial);
                    printf("INVERTER_%d_QPGS=%d\n", count, i);
                } else if (duplicate) {
                    fprintf(stderr, "⊗ Skipping %s (Duplicate serial: %s)\n", cmd, serial);
                } else {
                    fprintf(stderr, "⊗ Skipping %s (Invalid serial: %s)\n", cmd, serial);
                }
            }
        }
        usleep(100000); // 100ms between queries
    }
    
    fprintf(stderr, "\n=== DISCOVERY RESULT ===\n");
    fprintf(stderr, "Total unique parallel inverters: %d\n", count);
    printf("PARALLEL_COUNT=%d\n", count);
    
    return count;
}

// Get parallel status for specific inverter
string cInverter::GetParallelStatus(int inverter_num) {
    char cmd[16];
    snprintf(cmd, sizeof(cmd), "QPGS%d", inverter_num);
    
    if (query(cmd, 200)) {
        if (buf[0] == '(' && buf[1] != 'N') {
            // Return data without leading '('
            return string((char*)buf + 1);
        }
    }
    
    return "";
}

int cInverter::QueryParallelQpgs(int count, std::vector<std::string> &replies) {
    replies.clear();
    if (count <= 0) {
        return 0;
    }

    replies.resize(count);
    int fd = openSerial();
    if (fd < 0) {
        return 0;
    }

    int ok = 0;
    for (int i = 0; i < count; i++) {
        std::string cmd = "QPGS" + std::to_string(i);
        for (int attempt = 0; attempt < 3; attempt++) {
            if (sendCommand(fd, cmd)) {
                std::string payload;
                if (readReply(fd, payload)) {
                    replies[i] = payload;
                    ok++;
                    break;
                }
            }
            usleep(600000);
        }
        usleep(500000);
    }

    closeSerial(fd);
    return ok;
}