ESP32 code brrrrrrrrr

/*
 * ESP32 MAVLink TCP Bridge for SIMCom A7670C
 * Version 9.5.3 - Patched RX Priority Edition
 *
 * Based on v9.5 with critical RX priority fixes:
 * - quickRxCheck() called frequently during TX
 * - Smaller TX chunks (32 bytes) for more RX opportunities
 * - RX checked before/after every TX chunk
 * - Critical command detection and logging
 * - Max 100ms blocking per write call
 */

#define TINY_GSM_MODEM_SIM7600
#define TINY_GSM_RX_BUFFER 2048

#include <TinyGsmClient.h>
#include <esp_task_wdt.h>

// ============== WDT VERSION COMPATIBILITY ==============
#if defined(ESP_IDF_VERSION) && ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 0, 0)
    #define WDT_API_V5
#endif

#define WDT_TIMEOUT_SECONDS 15

// ============== CONFIGURATION ==============
const char* targetHost = "45.77.132.119";
const uint16_t targetPort = 5760;
const char manualAPN[] = "";

// ============== PIN DEFINITIONS ==============
#define LTE_TX_PIN      17
#define LTE_RX_PIN      18
#define LTE_PWRKEY_PIN  4
#define FC_TX_PIN       11
#define FC_RX_PIN       12
#define FC_BAUD_RATE    57600

// ============== TIMING CONSTANTS (ms) ==============
#define MAVLINK_BYTE_TIMEOUT      100
#define STATUS_PRINT_INTERVAL     3000
#define MODEM_BOOT_WAIT_MS        8000
#define NETWORK_WAIT_TIMEOUT      90000
#define MODEM_INIT_TIMEOUT        30000
#define CFUN_TRANSITION_WAIT      5000

// === RX PRIORITY TUNING ===
#define WRITE_CHUNK_SIZE          32      // Smaller = more RX checks
#define WRITE_MAX_TIME_MS         100     // Max blocking per write call
#define MAX_TX_BYTES_PER_LOOP     256
#define MAX_RX_BYTES_PER_LOOP     512
#define MAX_FC_BYTES_PER_LOOP     256
#define RX_CHECK_INTERVAL_BYTES   100     // Check RX every N bytes from FC

// === HEALTH THRESHOLDS ===
#define WRITE_DURATION_FAST_MS    30
#define WRITE_DURATION_NORMAL_MS  100
#define WRITE_DURATION_SLOW_MS    300
#define WRITE_DURATION_STALL_MS   500

#define BUFFER_WARNING_THRESHOLD  2000
#define BUFFER_CRITICAL_THRESHOLD 4000

#define RX_SILENCE_WARNING_MS     5000
#define RX_SILENCE_CRITICAL_MS    15000
#define ACTIVITY_TIMEOUT_MS       60000

#define WRITE_HISTORY_SIZE        10
#define BUFFER_TREND_SAMPLES      10
#define MAX_CONSECUTIVE_TX_FAILS  15

// === OTHER ===
#define CONNECTION_TIMEOUT_MS     10000
#define HEALTH_CHECK_INTERVAL     1000
#define KEEPALIVE_INTERVAL_MS     1000

#define TX_BUFFER_HIGH_WATER      4000
#define TX_BUFFER_LOW_WATER       1000
#define MIN_FREE_HEAP_KB          20
#define LOOP_TIME_LIMIT_MS        100

#define INITIAL_HEARTBEAT_COUNT   5
#define INITIAL_HEARTBEAT_DELAY   200

// ============== RETRY LIMITS ==============
#define MAX_TCP_RETRIES           5
#define MAX_NETWORK_RETRIES       3
#define MAX_HARD_RESET_RETRIES    2
#define MAX_WAKEUP_RETRIES        3

// ============== BUFFER SIZES ==============
#define SERIAL_RX_BUF_SIZE   4096
#define RING_TX_BUF_SIZE     8192
#define MAVLINK_MAX_FRAME    280

// ============== MAVLINK MESSAGE IDS ==============
#define MAVLINK_MSG_ID_HEARTBEAT          0
#define MAVLINK_MSG_ID_SET_MODE           11
#define MAVLINK_MSG_ID_COMMAND_LONG       76
#define MAVLINK_MSG_ID_COMMAND_INT        75
#define MAVLINK_MSG_ID_MANUAL_CONTROL     69
#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE 70

#define MAV_CMD_COMPONENT_ARM_DISARM      400
#define MAV_CMD_NAV_RETURN_TO_LAUNCH      20
#define MAV_CMD_DO_SET_MODE               176

// ============== HARDWARE SERIAL ==============
HardwareSerial SerialLTE(1);
HardwareSerial SerialFC(2);

// ============== TINYGSM ==============
TinyGsm lteModem(SerialLTE);
TinyGsmClient tcpClient(lteModem);

// ============== ENUMS ==============
enum ModemStatus {
    MODEM_OFF, MODEM_STANDBY, MODEM_BOOTING, MODEM_READY,
    MODEM_SEARCHING, MODEM_REGISTERED, MODEM_CONNECTED, MODEM_DEGRADED
};

const char* modemStatusNames[] = {
    "OFF", "STANDBY", "BOOTING", "READY",
    "SEARCHING", "REGISTERED", "CONNECTED", "DEGRADED"
};

enum BridgeState {
    ST_POWER_ON, ST_WAIT_MODEM, ST_CHECK_STATUS, ST_WAKEUP_MODEM,
    ST_CLEAR_CONFIG, ST_CHECK_SIM, ST_GET_IMSI, ST_DETECT_APN,
    ST_WAIT_NETWORK, ST_GPRS_CONNECT, ST_TCP_CONNECT, ST_SEND_HEARTBEATS,
    ST_RUNNING, ST_TCP_RECONNECT, ST_NETWORK_RECOVER, ST_HARD_RESET,
    ST_SYSTEM_REBOOT
};

const char* stateNames[] = {
    "POWER_ON", "WAIT_MODEM", "CHECK_STATUS", "WAKEUP_MODEM",
    "CLEAR_CONFIG", "CHECK_SIM", "GET_IMSI", "DETECT_APN",
    "WAIT_NETWORK", "GPRS_CONNECT", "TCP_CONNECT", "SEND_HEARTBEATS",
    "RUNNING", "TCP_RECONNECT", "NET_RECOVER", "HARD_RESET", "SYS_REBOOT"
};

enum ConnectionHealth {
    CONN_EXCELLENT, CONN_GOOD, CONN_SLOW, CONN_DEGRADED, CONN_STALLED, CONN_DEAD
};

const char* healthNames[] = {
    "EXCELLENT", "GOOD", "SLOW", "DEGRADED", "STALLED", "DEAD"
};

// ============== APN DATABASE ==============
struct APNConfig {
    const char* mccMnc;
    const char* carrier;
    const char* apnName;
};

const APNConfig apnList[] = {
    {"45412", "CMHK", "cmhk"},
    {"45413", "CMHK", "cmhk"},
    {"45400", "CSL", "hkcsl"},
    {"45403", "3HK", "mobile.three.com.hk"},
    {"46000", "CMCC", "cmnet"},
    {"46001", "CU", "3gnet"},
    {"", "Generic", "internet"},
};
const int apnListCount = sizeof(apnList) / sizeof(apnList[0]);

// ============== WATCHDOG ==============
void initWatchdog() {
#ifdef WDT_API_V5
    esp_task_wdt_config_t wdt_config = {
        .timeout_ms = WDT_TIMEOUT_SECONDS * 1000,
        .idle_core_mask = 0,
        .trigger_panic = true
    };
    esp_err_t err = esp_task_wdt_reconfigure(&wdt_config);
    if (err != ESP_OK) {
        esp_task_wdt_deinit();
        esp_task_wdt_init(&wdt_config);
    }
    esp_task_wdt_add(NULL);
#else
    esp_task_wdt_init(WDT_TIMEOUT_SECONDS, true);
    esp_task_wdt_add(NULL);
#endif
    Serial.printf("[WDT] Initialized (%d sec)\n", WDT_TIMEOUT_SECONDS);
}

void feedWatchdog() {
    esp_task_wdt_reset();
}

// ============== RING BUFFER ==============
template<size_t CAPACITY>
class CircularBuffer {
private:
    uint8_t data[CAPACITY];
    volatile size_t writeIdx = 0;
    volatile size_t readIdx = 0;

public:
    void reset() { writeIdx = readIdx = 0; }

    size_t count() const {
        size_t w = writeIdx, r = readIdx;
        return (w >= r) ? (w - r) : (CAPACITY - r + w);
    }

    size_t space() const { return CAPACITY - count() - 1; }
    bool isEmpty() const { return writeIdx == readIdx; }

    bool write(uint8_t byte) {
        size_t next = (writeIdx + 1) % CAPACITY;
        if (next == readIdx) return false;
        data[writeIdx] = byte;
        writeIdx = next;
        return true;
    }

    bool writeFrame(const uint8_t* buf, size_t len) {
        if (space() < len) return false;
        for (size_t i = 0; i < len; i++) write(buf[i]);
        return true;
    }

    int read() {
        if (writeIdx == readIdx) return -1;
        uint8_t byte = data[readIdx];
        readIdx = (readIdx + 1) % CAPACITY;
        return byte;
    }

    size_t readBytes(uint8_t* buf, size_t maxLen) {
        size_t n = 0;
        while (n < maxLen && !isEmpty()) {
            int b = read();
            if (b >= 0) buf[n++] = (uint8_t)b;
            else break;
        }
        return n;
    }

    void discardAll() { readIdx = writeIdx; }

    size_t discardOldest(size_t targetRemain) {
        size_t current = count();
        if (current <= targetRemain) return 0;
        size_t toDiscard = current - targetRemain;
        readIdx = (readIdx + toDiscard) % CAPACITY;
        return toDiscard;
    }
};

// ============== MAVLINK PARSER ==============
class MavlinkFrameParser {
private:
    enum State { IDLE, GOT_STX, GOT_LEN, PAYLOAD } state = IDLE;
    uint8_t frameBuf[MAVLINK_MAX_FRAME];
    uint16_t frameIdx = 0;
    uint16_t totalLen = 0;
    uint8_t stxByte = 0;
    unsigned long lastByteMs = 0;

public:
    void reset() { state = IDLE; frameIdx = 0; totalLen = 0; }

    uint16_t feed(uint8_t byte) {
        unsigned long now = millis();
        if (state != IDLE && (now - lastByteMs) > MAVLINK_BYTE_TIMEOUT) reset();
        lastByteMs = now;

        switch (state) {
            case IDLE:
                if (byte == 0xFE || byte == 0xFD) {
                    frameBuf[0] = byte;
                    stxByte = byte;
                    frameIdx = 1;
                    state = GOT_STX;
                }
                break;
            case GOT_STX:
                frameBuf[frameIdx++] = byte;
                if (stxByte == 0xFE) {
                    totalLen = 8 + byte;
                    state = PAYLOAD;
                } else if (frameIdx >= 2) {
                    state = GOT_LEN;
                }
                break;
            case GOT_LEN:
                frameBuf[frameIdx++] = byte;
                if (frameIdx >= 3) {
                    uint8_t payloadLen = frameBuf[1];
                    uint8_t incompat = frameBuf[2];
                    totalLen = 12 + payloadLen + ((incompat & 0x01) ? 13 : 0);
                    state = PAYLOAD;
                }
                break;
            case PAYLOAD:
                if (frameIdx < MAVLINK_MAX_FRAME) frameBuf[frameIdx++] = byte;
                if (frameIdx >= totalLen) {
                    uint16_t len = totalLen;
                    reset();
                    return len;
                }
                break;
        }
        if (frameIdx >= MAVLINK_MAX_FRAME) reset();
        return 0;
    }

    const uint8_t* getFrame() const { return frameBuf; }

    uint8_t getMessageId() const {
        if (frameBuf[0] == 0xFE) return frameBuf[5];
        if (frameBuf[0] == 0xFD) return frameBuf[7];
        return 0xFF;
    }

    bool isCriticalCommand() const {
        uint8_t msgId = getMessageId();
        return (msgId == MAVLINK_MSG_ID_SET_MODE ||
                msgId == MAVLINK_MSG_ID_COMMAND_LONG ||
                msgId == MAVLINK_MSG_ID_COMMAND_INT ||
                msgId == MAVLINK_MSG_ID_MANUAL_CONTROL ||
                msgId == MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE);
    }

    uint16_t getCommandId() const {
        if (getMessageId() == MAVLINK_MSG_ID_COMMAND_LONG && frameBuf[0] == 0xFE) {
            return frameBuf[6 + 28] | (frameBuf[6 + 29] << 8);
        }
        return 0;
    }
};

// ============== CONNECTION MONITOR ==============
class ConnectionMonitor {
private:
    unsigned long lastRxMs = 0;
    unsigned long lastTxSuccessMs = 0;
    unsigned long lastActivityMs = 0;

    unsigned long avgWriteDurationMs = 0;
    unsigned long lastWriteDurationMs = 0;
    unsigned long maxWriteDurationMs = 0;
    unsigned long currentWriteStartMs = 0;

    size_t lastBufferSize = 0;
    size_t peakBufferSize = 0;
    int consecutiveGrows = 0;

    uint32_t txBytesThisPeriod = 0;
    uint32_t rxBytesThisPeriod = 0;
    uint32_t fcBytesThisPeriod = 0;
    unsigned long periodStartMs = 0;
    float txBytesPerSec = 0;
    float rxBytesPerSec = 0;
    float fcBytesPerSec = 0;

    uint32_t consecutiveTxFails = 0;
    uint32_t totalTxAttempts = 0;
    uint32_t totalTxFails = 0;

    ConnectionHealth currentHealth = CONN_GOOD;

public:
    void reset() {
        unsigned long now = millis();
        lastRxMs = lastTxSuccessMs = lastActivityMs = periodStartMs = now;
        avgWriteDurationMs = lastWriteDurationMs = maxWriteDurationMs = currentWriteStartMs = 0;
        lastBufferSize = peakBufferSize = 0;
        consecutiveGrows = 0;
        txBytesThisPeriod = rxBytesThisPeriod = fcBytesThisPeriod = 0;
        txBytesPerSec = rxBytesPerSec = fcBytesPerSec = 0;
        consecutiveTxFails = totalTxAttempts = totalTxFails = 0;
        currentHealth = CONN_GOOD;
    }

    void onWriteStart() { currentWriteStartMs = millis(); }

    void onWriteComplete(size_t attempted, size_t sent) {
        unsigned long now = millis();
        unsigned long duration = currentWriteStartMs ? (now - currentWriteStartMs) : 0;
        currentWriteStartMs = 0;
        totalTxAttempts++;
        lastWriteDurationMs = duration;
        if (duration > maxWriteDurationMs) maxWriteDurationMs = duration;
        avgWriteDurationMs = (avgWriteDurationMs * 3 + duration) / 4;

        if (sent > 0) {
            lastTxSuccessMs = lastActivityMs = now;
            consecutiveTxFails = 0;
            txBytesThisPeriod += sent;
        } else {
            consecutiveTxFails++;
            totalTxFails++;
        }
    }

    void onWriteSuccess(size_t bytes) {
        lastTxSuccessMs = lastActivityMs = millis();
        consecutiveTxFails = 0;
        txBytesThisPeriod += bytes;
    }

    void onWriteFail() {
        consecutiveTxFails++;
        totalTxFails++;
    }

    void onRxData(size_t bytes) {
        lastRxMs = lastActivityMs = millis();
        rxBytesThisPeriod += bytes;
    }

    void onFcData(size_t bytes) { fcBytesThisPeriod += bytes; }

    void updateStats(size_t currentBufferSize) {
        if (currentBufferSize > peakBufferSize) peakBufferSize = currentBufferSize;
        if (currentBufferSize > lastBufferSize + 100) consecutiveGrows++;
        else if (currentBufferSize + 100 < lastBufferSize) consecutiveGrows = 0;
        lastBufferSize = currentBufferSize;

        unsigned long now = millis();
        unsigned long elapsed = now - periodStartMs;
        if (elapsed >= 1000) {
            txBytesPerSec = (float)txBytesThisPeriod * 1000.0f / elapsed;
            rxBytesPerSec = (float)rxBytesThisPeriod * 1000.0f / elapsed;
            fcBytesPerSec = (float)fcBytesThisPeriod * 1000.0f / elapsed;
            txBytesThisPeriod = rxBytesThisPeriod = fcBytesThisPeriod = 0;
            periodStartMs = now;
        }
    }

    ConnectionHealth evaluate(size_t currentBufferSize) {
        updateStats(currentBufferSize);

        int score = 100;
        if (avgWriteDurationMs > WRITE_DURATION_STALL_MS) score -= 50;
        else if (avgWriteDurationMs > WRITE_DURATION_SLOW_MS) score -= 30;
        else if (avgWriteDurationMs > WRITE_DURATION_NORMAL_MS) score -= 15;

        if (currentBufferSize > BUFFER_CRITICAL_THRESHOLD) score -= 40;
        else if (currentBufferSize > BUFFER_WARNING_THRESHOLD) score -= 20;

        if (consecutiveGrows >= 5) score -= 25;
        if (consecutiveTxFails >= 10) score -= 50;
        else if (consecutiveTxFails >= 5) score -= 30;

        unsigned long rxAge = millis() - lastRxMs;
        if (rxAge > RX_SILENCE_CRITICAL_MS) score -= 20;
        else if (rxAge > RX_SILENCE_WARNING_MS) score -= 10;

        if (fcBytesPerSec > 100 && txBytesPerSec > 0) {
            float ratio = txBytesPerSec / fcBytesPerSec;
            if (ratio < 0.3f) score -= 30;
            else if (ratio < 0.6f) score -= 15;
        }

        if ((millis() - lastActivityMs) > ACTIVITY_TIMEOUT_MS) score = 0;

        if (score <= 0) currentHealth = CONN_DEAD;
        else if (score <= 20) currentHealth = CONN_STALLED;
        else if (score <= 40) currentHealth = CONN_DEGRADED;
        else if (score <= 60) currentHealth = CONN_SLOW;
        else if (score <= 85) currentHealth = CONN_GOOD;
        else currentHealth = CONN_EXCELLENT;

        return currentHealth;
    }

    void printDebugInfo() {
        Serial.printf("    Write: avg=%lums last=%lums max=%lums\n",
                     avgWriteDurationMs, lastWriteDurationMs, maxWriteDurationMs);
        Serial.printf("    Buffer: %d peak=%d grows=%d\n",
                     (int)lastBufferSize, (int)peakBufferSize, consecutiveGrows);
        Serial.printf("    TxFails: %d/%d | RxAge: %lums\n",
                     (int)consecutiveTxFails, (int)totalTxAttempts, millis() - lastRxMs);
        Serial.printf("    Rate: FC=%.1f TX=%.1f RX=%.1f KB/s\n",
                     fcBytesPerSec/1024, txBytesPerSec/1024, rxBytesPerSec/1024);
    }

    unsigned long getTimeSinceRx() const { return millis() - lastRxMs; }
    unsigned long getTimeSinceTx() const { return millis() - lastTxSuccessMs; }
    float getTxRate() const { return txBytesPerSec; }
    float getRxRate() const { return rxBytesPerSec; }
    float getFcRate() const { return fcBytesPerSec; }
    size_t getPeakBuffer() const { return peakBufferSize; }
    ConnectionHealth getHealth() const { return currentHealth; }
    const char* getHealthString() const { return healthNames[currentHealth]; }
};

// ============== GLOBAL VARIABLES ==============
BridgeState bridgeState = ST_POWER_ON;
ModemStatus currentModemStatus = MODEM_OFF;

unsigned long stateEntryMs = 0;
unsigned long lastStatusPrintMs = 0;
unsigned long lastHealthCheckMs = 0;
unsigned long lastKeepaliveMs = 0;
unsigned long lastModemCheckMs = 0;
unsigned long loopStartMs = 0;

int tcpRetryCount = 0;
int networkRetryCount = 0;
int hardResetCount = 0;
int wakeupRetryCount = 0;
int initialHeartbeatCount = 0;

char currentAPN[64] = "";
char currentCarrier[32] = "";
char simIMSI[20] = "";

MavlinkFrameParser fcParser;
MavlinkFrameParser tcpParser;
CircularBuffer<RING_TX_BUF_SIZE> txBuffer;
ConnectionMonitor connMonitor;

bool networkReady = false;
bool buffersInitialized = false;
volatile bool forceReconnect = false;
bool verboseDebug = true;

struct BridgeStats {
    uint32_t mavFramesTx = 0;
    uint32_t mavFramesRx = 0;
    uint32_t criticalCmdsRx = 0;
    uint32_t bytesSentTcp = 0;
    uint32_t bytesRecvTcp = 0;
    uint32_t bytesSentFc = 0;
    uint32_t bytesRecvFc = 0;
    uint32_t bufferOverflows = 0;
    uint32_t discardedBytes = 0;
    uint32_t writeStalls = 0;
    uint32_t connectionDeaths = 0;
    uint32_t tcpReconnects = 0;
    uint32_t networkReconnects = 0;
    uint32_t hardResets = 0;
    uint32_t heartbeatsSent = 0;
    uint32_t heapWarnings = 0;
    uint32_t rxChecks = 0;
    uint32_t rxInterrupts = 0;
    uint32_t loopOverruns = 0;
} stats;

unsigned long sessionStartMs = 0;
uint32_t minFreeHeap = UINT32_MAX;
uint32_t maxRxGapMs = 0;
unsigned long lastRxCheckMs = 0;

// ============== HEARTBEAT ==============
uint8_t heartbeatSeq = 0;
static const uint8_t MAVLINK_CRC_EXTRA_HEARTBEAT = 50;

void crc_accumulate(uint8_t data, uint16_t* crc) {
    uint8_t tmp = data ^ (uint8_t)(*crc & 0xFF);
    tmp ^= (tmp << 4);
    *crc = (*crc >> 8) ^ ((uint16_t)tmp << 8) ^ ((uint16_t)tmp << 3) ^ ((uint16_t)tmp >> 4);
}

size_t buildHeartbeat(uint8_t* buf) {
    buf[0] = 0xFE; buf[1] = 9; buf[2] = heartbeatSeq++;
    buf[3] = 254; buf[4] = 190; buf[5] = 0;
    buf[6] = 0; buf[7] = 0; buf[8] = 0; buf[9] = 0;
    buf[10] = 6; buf[11] = 8; buf[12] = 0; buf[13] = 0; buf[14] = 3;
    uint16_t crc = 0xFFFF;
    for (int i = 1; i <= 14; i++) crc_accumulate(buf[i], &crc);
    crc_accumulate(MAVLINK_CRC_EXTRA_HEARTBEAT, &crc);
    buf[15] = crc & 0xFF; buf[16] = (crc >> 8) & 0xFF;
    return 17;
}

// ============== HELPERS ==============
bool checkHeapHealth() {
    uint32_t freeHeap = ESP.getFreeHeap();
    if (freeHeap < minFreeHeap) minFreeHeap = freeHeap;
    if (freeHeap < (MIN_FREE_HEAP_KB * 1024)) {
        stats.heapWarnings++;
        txBuffer.discardAll();
        return false;
    }
    return true;
}

void checkLoopTime(const char* phase) {
    if (millis() - loopStartMs > LOOP_TIME_LIMIT_MS) {
        stats.loopOverruns++;
        feedWatchdog();
    }
}

String sendATCommand(const char* cmd, unsigned long timeout = 2000) {
    while (SerialLTE.available()) SerialLTE.read();
    SerialLTE.println(cmd);
    String response = "";
    unsigned long start = millis();
    while (millis() - start < timeout) {
        while (SerialLTE.available()) response += (char)SerialLTE.read();
        if (response.indexOf("OK") >= 0 || response.indexOf("ERROR") >= 0) break;
        delay(10);
        feedWatchdog();
    }
    return response;
}

bool sendATExpect(const char* cmd, const char* expect, unsigned long timeout = 2000) {
    return sendATCommand(cmd, timeout).indexOf(expect) >= 0;
}

// ============== QUICK RX CHECK (CRITICAL!) ==============
// This function is called frequently during TX operations
// It reads from TCP and forwards to FC immediately
bool quickRxCheck() {
    if (bridgeState != ST_RUNNING || !tcpClient.connected()) return false;

    // Track RX gap for diagnostics
    unsigned long now = millis();
    unsigned long gap = now - lastRxCheckMs;
    if (gap > maxRxGapMs && lastRxCheckMs > 0) maxRxGapMs = gap;
    lastRxCheckMs = now;

    stats.rxChecks++;

    int available = tcpClient.available();
    if (available <= 0) return false;

    // Read small chunk immediately
    uint8_t buf[64];
    size_t toRead = min(available, 64);
    size_t bytesRead = 0;

    for (size_t i = 0; i < toRead; i++) {
        int b = tcpClient.read();
        if (b >= 0) buf[bytesRead++] = (uint8_t)b;
        else break;
    }

    if (bytesRead > 0) {
        // Forward to FC IMMEDIATELY - this is the critical path!
        size_t sent = SerialFC.write(buf, bytesRead);

        connMonitor.onRxData(bytesRead);
        stats.bytesRecvTcp += bytesRead;
        stats.bytesSentFc += sent;

        // Parse for critical commands (logging)
        for (size_t i = 0; i < bytesRead; i++) {
            if (tcpParser.feed(buf[i]) > 0) {
                stats.mavFramesRx++;

                if (tcpParser.isCriticalCommand()) {
                    stats.criticalCmdsRx++;
                    uint8_t msgId = tcpParser.getMessageId();

                    Serial.printf("[RX] *** CRITICAL CMD @%lu: ", millis());

                    if (msgId == MAVLINK_MSG_ID_SET_MODE) {
                        Serial.println("MODE CHANGE ***");
                    } else if (msgId == MAVLINK_MSG_ID_COMMAND_LONG) {
                        uint16_t cmdId = tcpParser.getCommandId();
                        if (cmdId == MAV_CMD_COMPONENT_ARM_DISARM) Serial.println("ARM/DISARM ***");
                        else if (cmdId == MAV_CMD_NAV_RETURN_TO_LAUNCH) Serial.println("RTL ***");
                        else if (cmdId == MAV_CMD_DO_SET_MODE) Serial.println("DO_SET_MODE ***");
                        else Serial.printf("CMD_%d ***\n", cmdId);
                    } else if (msgId == MAVLINK_MSG_ID_MANUAL_CONTROL) {
                        Serial.println("MANUAL_CONTROL ***");
                    } else if (msgId == MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE) {
                        Serial.println("RC_OVERRIDE ***");
                    } else {
                        Serial.printf("MsgID=%d ***\n", msgId);
                    }
                }
            }
        }
        return true;
    }
    return false;
}

// ============== SAFE TCP WRITE WITH RX CHECKS ==============
size_t safeTcpWrite(const uint8_t* data, size_t len) {
    if (!tcpClient.connected() || len == 0) return 0;

    unsigned long startMs = millis();
    size_t totalSent = 0;
    size_t offset = 0;

    while (offset < len) {
        // *** CHECK RX BEFORE EACH CHUNK ***
        if (quickRxCheck()) {
            stats.rxInterrupts++;
        }

        // Time limit check
        if (millis() - startMs > WRITE_MAX_TIME_MS) {
            break;
        }

        // Send small chunk
        size_t toSend = min(len - offset, (size_t)WRITE_CHUNK_SIZE);

        connMonitor.onWriteStart();
        size_t sent = tcpClient.write(data + offset, toSend);
        connMonitor.onWriteComplete(toSend, sent);

        if (sent > 0) {
            totalSent += sent;
            offset += sent;
            stats.bytesSentTcp += sent;
        } else {
            // Write failed
            break;
        }

        // *** CHECK RX AFTER EACH CHUNK ***
        if (quickRxCheck()) {
            stats.rxInterrupts++;
        }

        feedWatchdog();
    }

    return totalSent;
}

// ============== MODEM FUNCTIONS ==============
ModemStatus getModemStatus() {
    feedWatchdog();
    bool responding = false;
    for (int i = 0; i < 3; i++) {
        if (sendATExpect("AT", "OK", 500)) { responding = true; break; }
        delay(100);
    }
    if (!responding) return MODEM_OFF;

    String resp = sendATCommand("AT+CFUN?", 1000);
    int cfun = 1;
    int idx = resp.indexOf("+CFUN:");
    if (idx >= 0) cfun = resp.substring(idx + 6).toInt();

    if (cfun == 0 || cfun == 4) return MODEM_STANDBY;

    if (cfun == 1) {
        resp = sendATCommand("AT+CPIN?", 3000);
        if (resp.indexOf("ERROR") >= 0) return MODEM_DEGRADED;

        resp = sendATCommand("AT+CREG?", 2000);
        if (resp.indexOf(",1") >= 0 || resp.indexOf(",5") >= 0) {
            resp = sendATCommand("AT+CGPADDR=1", 2000);
            if (resp.indexOf(".") >= 0 && resp.indexOf("0.0.0.0") < 0) return MODEM_CONNECTED;
            return MODEM_REGISTERED;
        }
        if (resp.indexOf(",2") >= 0) return MODEM_SEARCHING;
        return MODEM_READY;
    }
    return MODEM_BOOTING;
}

ModemStatus quickModemCheck() {
    if (!sendATExpect("AT", "OK", 500)) return MODEM_OFF;
    return MODEM_REGISTERED;
}

bool wakeUpModem() {
    Serial.println("[WAKEUP] Waking up modem...");
    for (int i = 0; i < 5; i++) { sendATCommand("AT", 300); delay(100); feedWatchdog(); }
    if (!sendATExpect("AT", "OK", 1000)) return false;

    String resp = sendATCommand("AT+CFUN?", 1000);
    int cfun = 1;
    int idx = resp.indexOf("+CFUN:");
    if (idx >= 0) cfun = resp.substring(idx + 6).toInt();
    if (cfun != 1) {
        if (!sendATExpect("AT+CFUN=1", "OK", 10000)) return false;
        delay(CFUN_TRANSITION_WAIT);
        feedWatchdog();
    }

    resp = sendATCommand("AT+CPIN?", 5000);
    if (resp.indexOf("ERROR") >= 0) {
        sendATCommand("AT+CFUN=0", 3000);
        delay(2000);
        feedWatchdog();
        if (!sendATExpect("AT+CFUN=1", "OK", 10000)) return false;
        delay(CFUN_TRANSITION_WAIT);
    }
    return (getModemStatus() >= MODEM_READY);
}

bool isModemResponding() {
    for (int i = 0; i < 3; i++) {
        if (sendATExpect("AT", "OK", 500)) return true;
        delay(100);
    }
    return false;
}

void pulsePowerKey(unsigned long durationMs = 1500) {
    digitalWrite(LTE_PWRKEY_PIN, HIGH);
    delay(durationMs);
    digitalWrite(LTE_PWRKEY_PIN, LOW);
    feedWatchdog();
}

bool powerOnModule() {
    if (isModemResponding()) return true;
    pulsePowerKey(1500);
    unsigned long start = millis();
    while (millis() - start < MODEM_BOOT_WAIT_MS) { delay(500); feedWatchdog(); }
    return isModemResponding();
}

bool powerOffModule() {
    if (!isModemResponding()) return true;
    sendATCommand("AT+CPOF", 3000);
    delay(2000);
    feedWatchdog();
    if (!isModemResponding()) return true;
    pulsePowerKey(1500);
    delay(3000);
    return !isModemResponding();
}

void hardResetModule() {
    stats.hardResets++;
    tcpClient.stop();
    int attempts = 0;
    while (isModemResponding() && attempts < 3) { powerOffModule(); attempts++; feedWatchdog(); }
    delay(2000);
    pulsePowerKey(1500);
    unsigned long start = millis();
    while (millis() - start < MODEM_BOOT_WAIT_MS) { delay(500); feedWatchdog(); }
}

bool waitModemReady(unsigned long timeout = MODEM_INIT_TIMEOUT) {
    unsigned long start = millis();
    while (millis() - start < timeout) {
        feedWatchdog();
        if (sendATExpect("AT", "OK", 1000)) { delay(500); return true; }
        delay(500);
    }
    return false;
}

void clearModemConfig() {
    feedWatchdog();
    sendATExpect("ATE0", "OK", 500);
    sendATExpect("AT+CMEE=2", "OK", 500);
    sendATCommand("AT+CIPCLOSE=0", 2000);
    sendATCommand("AT+CIPSHUT", 5000);
    feedWatchdog();
    sendATCommand("AT+CGACT=0,1", 3000);
    sendATCommand("AT+NETCLOSE", 3000);
    delay(500);
}

// ============== SIM & APN ==============
bool checkSimPresent() {
    currentModemStatus = getModemStatus();
    if (currentModemStatus == MODEM_OFF || currentModemStatus == MODEM_STANDBY ||
        currentModemStatus == MODEM_DEGRADED) return false;
    String resp = sendATCommand("AT+CPIN?", 5000);
    if (resp.indexOf("READY") >= 0) return true;
    resp = sendATCommand("AT+CICCID", 3000);
    return (resp.indexOf("89") >= 0);
}

bool waitForSimReady(unsigned long timeout = 20000) {
    unsigned long start = millis();
    while (millis() - start < timeout) { feedWatchdog(); if (checkSimPresent()) return true; delay(2000); }
    return false;
}

bool readIMSI() {
    String resp = sendATCommand("AT+CIMI", 2000);
    for (unsigned int i = 0; i < resp.length(); i++) {
        if (isdigit(resp[i])) {
            int len = 0;
            while (i + len < resp.length() && isdigit(resp[i + len]) && len < 15) {
                simIMSI[len] = resp[i + len]; len++;
            }
            simIMSI[len] = '\0';
            return len >= 5;
        }
    }
    return false;
}

bool detectAPN() {
    if (strlen(manualAPN) > 0) {
        strncpy(currentAPN, manualAPN, sizeof(currentAPN) - 1);
        strcpy(currentCarrier, "Manual");
        return true;
    }
    String resp = sendATCommand("AT+CGNAPN", 3000);
    int start = resp.indexOf('"');
    int end = resp.indexOf('"', start + 1);
    if (start >= 0 && end > start + 1) {
        resp.substring(start + 1, end).toCharArray(currentAPN, sizeof(currentAPN));
        strcpy(currentCarrier, "Auto");
        return true;
    }
    if (strlen(simIMSI) >= 5) {
        for (int i = 0; i < apnListCount; i++) {
            if (strlen(apnList[i].mccMnc) >= 5 &&
                strncmp(simIMSI, apnList[i].mccMnc, strlen(apnList[i].mccMnc)) == 0) {
                strncpy(currentAPN, apnList[i].apnName, sizeof(currentAPN) - 1);
                strncpy(currentCarrier, apnList[i].carrier, sizeof(currentCarrier) - 1);
                return true;
            }
        }
    }
    strcpy(currentAPN, "internet");
    strcpy(currentCarrier, "Generic");
    return true;
}

// ============== NETWORK ==============
bool isNetworkRegistered() {
    const char* regCmds[] = {"AT+CREG?", "AT+CGREG?", "AT+CEREG?"};
    for (auto cmd : regCmds) {
        String resp = sendATCommand(cmd, 2000);
        if (resp.indexOf(",1") >= 0 || resp.indexOf(",5") >= 0) return true;
    }
    return false;
}

bool activateDataConnection() {
    feedWatchdog();
    char cmd[80];
    snprintf(cmd, sizeof(cmd), "AT+CGDCONT=1,\"IP\",\"%s\"", currentAPN);
    sendATExpect(cmd, "OK", 2000);
    if (lteModem.gprsConnect(currentAPN, "", "")) { delay(1000); feedWatchdog(); }
    else {
        sendATExpect("AT+CGACT=1,1", "OK", 10000);
        feedWatchdog();
        sendATCommand("AT+NETOPEN", 10000);
        delay(2000);
    }
    String resp = sendATCommand("AT+CGPADDR=1", 3000);
    if (resp.indexOf(".") >= 0 && resp.indexOf("0.0.0.0") < 0) return true;
    resp = sendATCommand("AT+IPADDR", 3000);
    return (resp.indexOf(".") >= 0 && resp.indexOf("0.0.0.0") < 0);
}

// ============== HEARTBEAT ==============
bool sendHeartbeat() {
    if (!tcpClient.connected()) return false;

    // Check RX before sending heartbeat
    quickRxCheck();

    uint8_t hb[17];
    size_t len = buildHeartbeat(hb);

    connMonitor.onWriteStart();
    size_t sent = tcpClient.write(hb, len);
    connMonitor.onWriteComplete(len, sent);

    if (sent == len) {
        stats.heartbeatsSent++;
        stats.bytesSentTcp += sent;
        lastKeepaliveMs = millis();

        // Check RX after sending heartbeat
        quickRxCheck();
        return true;
    }
    return false;
}

// ============== DATA FORWARDING (RX PRIORITY!) ==============
void processForwarding() {
    loopStartMs = millis();

    if (!buffersInitialized) {
        while (SerialFC.available()) SerialFC.read();
        return;
    }

    if (!checkHeapHealth()) return;

    // === PRIORITY 1: Check RX multiple times ===
    if (bridgeState == ST_RUNNING && tcpClient.connected()) {
        // Check RX up to 5 times to drain any pending data
        for (int i = 0; i < 5; i++) {
            if (!quickRxCheck()) break;  // No more data
        }
    }

    // === PRIORITY 2: FC -> TX Buffer ===
    int fcBytesProcessed = 0;
    while (SerialFC.available() && fcBytesProcessed < MAX_FC_BYTES_PER_LOOP) {
        uint8_t byte = SerialFC.read();
        fcBytesProcessed++;
        stats.bytesRecvFc++;

        uint16_t frameLen = fcParser.feed(byte);

        if (frameLen > 0) {
            stats.mavFramesTx++;

            if (bridgeState == ST_RUNNING && tcpClient.connected()) {
                if (txBuffer.space() < frameLen) {
                    size_t discarded = txBuffer.discardOldest(txBuffer.count() / 2);
                    stats.discardedBytes += discarded;
                    stats.bufferOverflows++;
                }

                if (!txBuffer.writeFrame(fcParser.getFrame(), frameLen)) {
                    stats.bufferOverflows++;
                }
            }
        }

        // Check RX frequently while reading FC
        if (fcBytesProcessed % RX_CHECK_INTERVAL_BYTES == 0) {
            quickRxCheck();
        }
    }

    connMonitor.onFcData(fcBytesProcessed);
    checkLoopTime("FC");

    // === Check RX again before TX ===
    quickRxCheck();

    // === PRIORITY 3: TX Buffer -> TCP ===
    if (bridgeState == ST_RUNNING && tcpClient.connected()) {
        size_t available = txBuffer.count();

        if (available > 0) {
            static uint8_t txChunk[MAX_TX_BYTES_PER_LOOP];

            size_t toSend = min(available, (size_t)MAX_TX_BYTES_PER_LOOP);
            toSend = min(toSend, sizeof(txChunk));
            toSend = txBuffer.readBytes(txChunk, toSend);

            if (toSend > 0) {
                size_t sent = safeTcpWrite(txChunk, toSend);

                if (sent < toSend) {
                    stats.discardedBytes += (toSend - sent);
                }
            }
        }
    }

    // === Final RX check ===
    quickRxCheck();

    checkLoopTime("TX");
    feedWatchdog();
}

// ============== STATE MACHINE ==============
void changeState(BridgeState newState) {
    Serial.printf("[STATE] %s -> %s\n", stateNames[bridgeState], stateNames[newState]);
    bridgeState = newState;
    stateEntryMs = millis();
    forceReconnect = false;
}

void handleStateMachine() {
    unsigned long elapsed = millis() - stateEntryMs;

    // Always check RX in RUNNING state
    if (bridgeState == ST_RUNNING) {
        quickRxCheck();
    }

    switch (bridgeState) {
        case ST_POWER_ON:
            Serial.println("\n=== ESP32 MAVLink Bridge v9.5.3 ===");
            Serial.println("=== Patched RX Priority Edition ===\n");
            Serial.printf("[HEAP] Free: %dKB\n", ESP.getFreeHeap() / 1024);
            if (powerOnModule()) changeState(ST_WAIT_MODEM);
            else changeState(ST_HARD_RESET);
            break;

        case ST_WAIT_MODEM:
            if (waitModemReady()) changeState(ST_CHECK_STATUS);
            else changeState(ST_HARD_RESET);
            break;

        case ST_CHECK_STATUS:
            currentModemStatus = getModemStatus();
            Serial.printf("[STATUS] Modem: %s\n", modemStatusNames[currentModemStatus]);
            if (currentModemStatus == MODEM_OFF) changeState(ST_POWER_ON);
            else if (currentModemStatus == MODEM_STANDBY || currentModemStatus == MODEM_DEGRADED) {
                wakeupRetryCount = 0; changeState(ST_WAKEUP_MODEM);
            }
            else if (currentModemStatus == MODEM_BOOTING) {
                if (elapsed > 10000) changeState(ST_WAKEUP_MODEM); else delay(1000);
            }
            else changeState(ST_CLEAR_CONFIG);
            break;

        case ST_WAKEUP_MODEM:
            if (wakeUpModem()) changeState(ST_CLEAR_CONFIG);
            else {
                wakeupRetryCount++;
                if (wakeupRetryCount >= MAX_WAKEUP_RETRIES) changeState(ST_HARD_RESET);
                else { delay(2000); changeState(ST_CHECK_STATUS); }
            }
            break;

        case ST_CLEAR_CONFIG:
            clearModemConfig();
            changeState(ST_CHECK_SIM);
            break;

        case ST_CHECK_SIM:
            currentModemStatus = getModemStatus();
            if (currentModemStatus == MODEM_STANDBY || currentModemStatus == MODEM_DEGRADED) {
                changeState(ST_WAKEUP_MODEM);
            } else if (waitForSimReady()) changeState(ST_GET_IMSI);
            else { if (elapsed > 30000) changeState(ST_HARD_RESET); else delay(3000); }
            break;

        case ST_GET_IMSI:
            readIMSI();
            changeState(ST_DETECT_APN);
            break;

        case ST_DETECT_APN:
            detectAPN();
            Serial.printf("[APN] %s (%s)\n", currentAPN, currentCarrier);
            changeState(ST_WAIT_NETWORK);
            break;

        case ST_WAIT_NETWORK:
            feedWatchdog();
            if (isNetworkRegistered()) changeState(ST_GPRS_CONNECT);
            else if (elapsed > NETWORK_WAIT_TIMEOUT) {
                networkRetryCount++;
                if (networkRetryCount >= MAX_NETWORK_RETRIES) changeState(ST_HARD_RESET);
                else changeState(ST_CHECK_STATUS);
            } else delay(2000);
            break;

        case ST_GPRS_CONNECT:
            if (activateDataConnection()) {
                networkReady = true;
                if (!buffersInitialized) {
                    txBuffer.reset();
                    fcParser.reset();
                    tcpParser.reset();
                    buffersInitialized = true;
                }
                changeState(ST_TCP_CONNECT);
            } else {
                networkRetryCount++;
                if (networkRetryCount >= MAX_NETWORK_RETRIES) changeState(ST_HARD_RESET);
                else { delay(2000); changeState(ST_WAIT_NETWORK); }
            }
            break;

        case ST_TCP_CONNECT:
            Serial.printf("[TCP] Connecting to %s:%d...\n", targetHost, targetPort);
            while (SerialLTE.available()) SerialLTE.read();
            tcpClient.setTimeout(CONNECTION_TIMEOUT_MS);
            if (tcpClient.connect(targetHost, targetPort)) {
                Serial.println("[TCP] Connected!");
                connMonitor.reset();
                initialHeartbeatCount = 0;
                forceReconnect = false;
                lastRxCheckMs = millis();
                maxRxGapMs = 0;
                changeState(ST_SEND_HEARTBEATS);
            } else {
                Serial.println("[TCP] Failed");
                stats.tcpReconnects++;
                tcpRetryCount++;
                if (tcpRetryCount >= MAX_TCP_RETRIES) changeState(ST_NETWORK_RECOVER);
                else delay(2000);
            }
            break;

        case ST_SEND_HEARTBEATS:
            feedWatchdog();
            if (initialHeartbeatCount < INITIAL_HEARTBEAT_COUNT) {
                if (sendHeartbeat()) {
                    initialHeartbeatCount++;
                    Serial.printf("[HB] Initial %d/%d\n", initialHeartbeatCount, INITIAL_HEARTBEAT_COUNT);
                }
                delay(INITIAL_HEARTBEAT_DELAY);
            } else {
                Serial.println("\n*** BRIDGE READY ***");
                Serial.println("*** RX Priority Active - Frequent Checks ***\n");
                tcpRetryCount = 0;
                networkRetryCount = 0;
                if (sessionStartMs == 0) sessionStartMs = millis();
                changeState(ST_RUNNING);
            }
            if (!tcpClient.connected()) {
                Serial.println("[HB] Lost connection");
                changeState(ST_TCP_RECONNECT);
            }
            break;

        case ST_RUNNING:
            // RX check already done at start of function

            if (forceReconnect) {
                Serial.println("[TCP] Forced reconnect");
                changeState(ST_TCP_RECONNECT);
                return;
            }

            if (millis() - lastHealthCheckMs > HEALTH_CHECK_INTERVAL) {
                lastHealthCheckMs = millis();

                if (!tcpClient.connected()) {
                    Serial.println("[TCP] Disconnected");
                    changeState(ST_TCP_RECONNECT);
                    return;
                }

                ConnectionHealth health = connMonitor.evaluate(txBuffer.count());
                if (health == CONN_DEAD) {
                    Serial.println("[TCP] Connection DEAD!");
                    stats.connectionDeaths++;
                    tcpClient.stop();
                    changeState(ST_TCP_RECONNECT);
                    return;
                }
                if (health == CONN_STALLED) stats.writeStalls++;
            }

            if (millis() - lastKeepaliveMs > KEEPALIVE_INTERVAL_MS) {
                sendHeartbeat();
            }

            if (millis() - lastModemCheckMs > 60000) {
                lastModemCheckMs = millis();
                if (quickModemCheck() == MODEM_OFF) {
                    Serial.println("[RUN] Modem died!");
                    tcpClient.stop();
                    buffersInitialized = false;
                    changeState(ST_POWER_ON);
                }
            }
            break;

        case ST_TCP_RECONNECT:
            feedWatchdog();
            tcpClient.stop();
            delay(500);
            txBuffer.discardAll();
            fcParser.reset();
            stats.tcpReconnects++;
            tcpRetryCount++;
            Serial.printf("[TCP] Reconnect %d/%d\n", tcpRetryCount, MAX_TCP_RETRIES);

            if (!isModemResponding()) changeState(ST_POWER_ON);
            else if (!isNetworkRegistered()) changeState(ST_NETWORK_RECOVER);
            else if (tcpRetryCount >= MAX_TCP_RETRIES) changeState(ST_NETWORK_RECOVER);
            else { delay(1000 * tcpRetryCount); changeState(ST_TCP_CONNECT); }
            break;

        case ST_NETWORK_RECOVER:
            Serial.println("[NET] Recovery...");
            feedWatchdog();
            stats.networkReconnects++;
            tcpClient.stop();
            txBuffer.discardAll();
            fcParser.reset();
            clearModemConfig();
            networkRetryCount++;
            tcpRetryCount = 0;
            if (networkRetryCount >= MAX_NETWORK_RETRIES) changeState(ST_HARD_RESET);
            else changeState(ST_CHECK_STATUS);
            break;

        case ST_HARD_RESET:
            Serial.printf("[RESET] Hard reset %d/%d\n", hardResetCount + 1, MAX_HARD_RESET_RETRIES);
            feedWatchdog();
            buffersInitialized = false;
            networkReady = false;
            txBuffer.reset();
            fcParser.reset();
            hardResetModule();
            hardResetCount++;
            tcpRetryCount = 0;
            networkRetryCount = 0;
            wakeupRetryCount = 0;
            if (hardResetCount >= MAX_HARD_RESET_RETRIES) changeState(ST_SYSTEM_REBOOT);
            else changeState(ST_WAIT_MODEM);
            break;

        case ST_SYSTEM_REBOOT:
            Serial.println("\n!!! REBOOTING !!!\n");
            delay(1000);
            ESP.restart();
            break;
    }
}

// ============== STATUS ==============
void printStatus() {
    if (millis() - lastStatusPrintMs < STATUS_PRINT_INTERVAL) return;
    lastStatusPrintMs = millis();

    uint32_t currentHeap = ESP.getFreeHeap();

    Serial.println("═══════════════════════════════════════");
    Serial.printf("  State: %s | Health: %s\n",
                 stateNames[bridgeState], connMonitor.getHealthString());

    if (bridgeState == ST_RUNNING) {
        Serial.printf("  TX buf: %d/%d (peak: %d)\n",
                     txBuffer.count(), RING_TX_BUF_SIZE, (int)connMonitor.getPeakBuffer());
        Serial.printf("  FC: %.1f KB/s -> TX: %.1f KB/s | RX: %.1f KB/s\n",
                     connMonitor.getFcRate() / 1024.0f,
                     connMonitor.getTxRate() / 1024.0f,
                     connMonitor.getRxRate() / 1024.0f);
        Serial.printf("  MAV: TX=%lu RX=%lu | HB=%lu\n",
                     stats.mavFramesTx, stats.mavFramesRx, stats.heartbeatsSent);
        Serial.printf("  RX: checks=%lu interrupts=%lu critical=%lu\n",
                     stats.rxChecks, stats.rxInterrupts, stats.criticalCmdsRx);
        Serial.printf("  Max RX gap: %lu ms\n", maxRxGapMs);

        if (verboseDebug) {
            connMonitor.printDebugInfo();
        }
    }

    if (stats.discardedBytes || stats.bufferOverflows) {
        Serial.printf("  Discarded: %luB | Overflows: %lu\n",
                     stats.discardedBytes, stats.bufferOverflows);
    }

    Serial.printf("  Heap: %dKB (min: %dKB) | Reconnects: %lu\n",
                 currentHeap / 1024, minFreeHeap / 1024, stats.tcpReconnects);
    Serial.println("═══════════════════════════════════════\n");

    // Reset max gap for next period
    maxRxGapMs = 0;
}

// ============== SETUP ==============
void setup() {
    Serial.begin(115200);
    delay(500);

    Serial.println("\n========================================");
    Serial.println("ESP32 MAVLink Bridge v9.5.3");
    Serial.println("Patched RX Priority Edition");
    Serial.println("----------------------------------------");
    Serial.println("Changes from v9.5:");
    Serial.println("- quickRxCheck() every 32 bytes TX");
    Serial.println("- Max 100ms per write call");
    Serial.println("- RX checked before/after each chunk");
    Serial.println("- Critical command logging");
    Serial.println("========================================\n");

    initWatchdog();

    SerialFC.setRxBufferSize(SERIAL_RX_BUF_SIZE);
    SerialFC.begin(FC_BAUD_RATE, SERIAL_8N1, FC_RX_PIN, FC_TX_PIN);

    SerialLTE.setRxBufferSize(SERIAL_RX_BUF_SIZE);
    SerialLTE.begin(115200, SERIAL_8N1, LTE_RX_PIN, LTE_TX_PIN);

    pinMode(LTE_PWRKEY_PIN, OUTPUT);
    digitalWrite(LTE_PWRKEY_PIN, LOW);

    Serial.printf("[INIT] Heap: %dKB\n", ESP.getFreeHeap() / 1024);
    minFreeHeap = ESP.getFreeHeap();

    delay(3000);
    feedWatchdog();

    stateEntryMs = millis();
    lastStatusPrintMs = millis();
    lastHealthCheckMs = millis();
    lastKeepaliveMs = millis();
    lastModemCheckMs = millis();
    lastRxCheckMs = millis();
    loopStartMs = millis();

    feedWatchdog();
}

// ============== LOOP ==============
void loop() {
    feedWatchdog();
    processForwarding();
    handleStateMachine();
    printStatus();
    yield();
}