// etcp.c - ETCP Protocol Implementation (refactored and expanded based on etcp_protocol.txt)
#include "etcp.h"
#include "etcp_loadbalancer.h"
#include "../lib/u_async.h"
#include "../lib/debug_config.h"
#include "crc32.h"
#include <stdlib.h>
#include <string.h>
#include <sys/time.h>
#include <math.h>  // For bandwidth calcs

// Enable comprehensive debug output for ETCP module
#define DEBUG_CATEGORY_ETCP_DETAILED 1

// Constants from spec (adjusted for completeness)
#define MAX_INFLIGHT_BYTES 65536  // Initial window
#define RETRANS_K1 2.0f           // RTT multiplier for retrans timeout
#define RETRANS_K2 1.5f           // Jitter multiplier
#define ACK_DELAY_TB 20           // ACK timer delay (2ms in 0.1ms units)
#define BURST_DELAY_FACTOR 4      // Delay before burst
#define BURST_SIZE 5              // Packets in burst (1 delayed + 4 burst)
#define RTT_HISTORY_SIZE 10       // For jitter calc
#define MAX_PENDING 32            // For ACKs/retrans
#define SECTION_HEADER_SIZE 3     // type(1) + len(2)

// Forward declaration for callback function
static void input_queue_cb(struct ll_queue* q, struct ll_entry* entry, void* arg);

// Timestamp diff (with wrap-around)
static uint16_t timestamp_diff(uint16_t t1, uint16_t t2) {
    DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "timestamp_diff: t1=%u, t2=%u", t1, t2);
    if (t1 >= t2) {
        uint16_t diff = t1 - t2;
        DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "timestamp_diff: normal case, diff=%u", diff);
        return diff;
    }
    uint16_t diff = (0xFFFF - t2) + t1 + 1;
    DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "timestamp_diff: wraparound case, diff=%u", diff);
    return diff;
}

// Get current time in 0.1ms units
uint64_t get_current_time_units() {
    struct timeval tv;
    gettimeofday(&tv, NULL);
    uint64_t time_units = ((uint64_t)tv.tv_sec * 10000ULL) + (tv.tv_usec / 100);
    DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "get_current_time_units: tv_sec=%ld, tv_usec=%ld, result=%llu", 
                tv.tv_sec, tv.tv_usec, (unsigned long long)time_units);
    return time_units;
}

uint16_t get_current_timestamp() {
    uint16_t timestamp = (uint16_t)(get_current_time_units() & 0xFFFF);
    DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "get_current_timestamp: result=%u", timestamp);
    return timestamp;
}

// Internal functions (expanded)
static void retrans_timer_cb(void* arg);
static void ack_timer_cb(void* arg);
static void update_rtt(struct ETCP_CONN* etcp, uint16_t new_rtt);
static void calculate_jitter(struct ETCP_CONN* etcp);
static void detect_gaps_and_request_retrans(struct ETCP_CONN* etcp);
static void process_section_ack(struct ETCP_CONN* etcp, uint8_t* data, uint16_t len);
static void process_section_retrans(struct ETCP_CONN* etcp, uint8_t* data, uint16_t len);
static void process_section_timestamp(struct ETCP_CONN* etcp, uint8_t* data, uint16_t len);
static void process_section_payload(struct ETCP_CONN* etcp, uint8_t* data, uint16_t len);
static void process_section_meas_ts(struct ETCP_CONN* etcp, uint8_t* data, uint16_t len);
static void process_section_meas_resp(struct ETCP_CONN* etcp, uint8_t* data, uint16_t len);
static uint16_t append_optional_sections(struct ETCP_CONN* etcp, uint8_t* buf, struct ETCP_LINK* link);
static void deliver_contiguous_packets(struct ETCP_CONN* etcp);
static void retrans_check(struct ETCP_CONN* etcp);
static inflight_packet_t* find_inflight(struct ETCP_CONN* etcp, uint16_t id, uint8_t state);
static void move_to_wait_send(struct ETCP_CONN* etcp, inflight_packet_t* pkt);
static void remove_inflight(struct ETCP_CONN* etcp, inflight_packet_t* pkt);
static void free_inflight_packet(inflight_packet_t* pkt);
static void start_burst_measurement(struct ETCP_CONN* etcp, struct ETCP_LINK* link);
static void update_link_bandwidth(struct ETCP_LINK* link, uint32_t new_bw);

// Add RTT history to struct (for jitter)
/// In etcp.h, add to ETCP_CONN:
/// uint16_t rtt_history[RTT_HISTORY_SIZE];
/// uint8_t rtt_history_idx;

// Create ETCP connection (unchanged)
struct ETCP_CONN* etcp_connection_create(struct UTUN_INSTANCE* instance) {
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_connection_create: instance=%p", instance);
    
    struct ETCP_CONN* etcp = calloc(1, sizeof(struct ETCP_CONN));
    if (!etcp) {
        DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "etcp_connection_create: failed to allocate ETCP_CONN");
        return NULL;
    }
    
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_connection_create: allocated etcp=%p", etcp);
    
    etcp->instance = instance;
    etcp->input_queue = queue_new(instance->ua, instance->pkt_pool);
    etcp->output_queue = queue_new(instance->ua, instance->pkt_pool);
    etcp->mtu = 1500;  // Default
    etcp->window_size = MAX_INFLIGHT_BYTES;
    etcp->next_tx_id = 1;
    
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_connection_create: queues created, mtu=%d, window_size=%u, next_tx_id=%u", 
                etcp->mtu, etcp->window_size, etcp->next_tx_id);
    
    // Init crypto (as per original)
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_connection_create: initializing crypto context");
    if (sc_init_ctx(&etcp->crypto_ctx, &instance->my_keys) != SC_OK) {
        DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "etcp_connection_create: crypto initialization failed");
        etcp_connection_close(etcp);
        return NULL;
    }
    
    DEBUG_INFO(DEBUG_CATEGORY_ETCP, "etcp_connection_create: successfully created ETCP connection %p", etcp);
    return etcp;
}

// Close connection (expanded free)
void etcp_connection_close(struct ETCP_CONN* etcp) {
    if (!etcp) {
        DEBUG_WARN(DEBUG_CATEGORY_ETCP, "etcp_connection_close: called with NULL etcp");
        return;
    }
    
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_connection_close: closing connection %p", etcp);
    
    // Cancel timers
    if (etcp->retrans_timer) {
        DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_connection_close: canceling retrans_timer");
        uasync_cancel_timeout(etcp->instance->ua, etcp->retrans_timer);
    }
    if (etcp->ack_timer) {
        DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_connection_close: canceling ack_timer");
        uasync_cancel_timeout(etcp->instance->ua, etcp->ack_timer);
    }
    
    // Free inflight
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_connection_close: freeing wait_ack_list");
    inflight_packet_t* pkt = etcp->wait_ack_list;
    int wait_ack_count = 0;
    while (pkt) {
        inflight_packet_t* next = pkt->next;
        free_inflight_packet(pkt);
        pkt = next;
        wait_ack_count++;
    }
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_connection_close: freed %d packets from wait_ack_list", wait_ack_count);
    
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_connection_close: freeing wait_send_list");
    pkt = etcp->wait_send_list;
    int wait_send_count = 0;
    while (pkt) {
        inflight_packet_t* next = pkt->next;
        free_inflight_packet(pkt);
        pkt = next;
        wait_send_count++;
    }
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_connection_close: freed %d packets from wait_send_list", wait_send_count);
    
    // Free rx_list
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_connection_close: freeing rx_list");
    rx_packet_t* rx = etcp->rx_list;
    int rx_count = 0;
    while (rx) {
        rx_packet_t* next = rx->next;
        free(rx->data);
        free(rx);
        rx = next;
        rx_count++;
    }
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_connection_close: freed %d packets from rx_list", rx_count);
    
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_connection_close: freeing queues");
    queue_free(etcp->input_queue);
    queue_free(etcp->output_queue);
    
    DEBUG_INFO(DEBUG_CATEGORY_ETCP, "etcp_connection_close: connection %p closed successfully", etcp);
    free(etcp);
}

static void free_inflight_packet(inflight_packet_t* pkt) {
    if (pkt) {
        DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "free_inflight_packet: freeing packet %p (id=%u, data_len=%u)", 
                    pkt, pkt->id, pkt->data_len);
        free(pkt->data);
        free(pkt);
    }
}

// Reset connection (unchanged)
void etcp_conn_reset(struct ETCP_CONN* etcp) {
    if (!etcp) {
        DEBUG_WARN(DEBUG_CATEGORY_ETCP, "etcp_conn_reset: called with NULL etcp");
        return;
    }
    
    DEBUG_INFO(DEBUG_CATEGORY_ETCP, "etcp_conn_reset: resetting connection %p", etcp);
    
    // Cancel timers
    if (etcp->retrans_timer) uasync_cancel_timeout(etcp->instance->ua, etcp->retrans_timer);
    if (etcp->ack_timer) uasync_cancel_timeout(etcp->instance->ua, etcp->ack_timer);
    
    // Free lists
    inflight_packet_t* pkt = etcp->wait_ack_list;
    while (pkt) {
        inflight_packet_t* next = pkt->next;
        free_inflight_packet(pkt);
        pkt = next;
    }
    etcp->wait_ack_list = NULL;
    pkt = etcp->wait_send_list;
    while (pkt) {
        inflight_packet_t* next = pkt->next;
        free_inflight_packet(pkt);
        pkt = next;
    }
    etcp->wait_send_list = NULL;
    
    rx_packet_t* rx = etcp->rx_list;
    while (rx) {
        rx_packet_t* next = rx->next;
        free(rx->data);
        free(rx);
        rx = next;
    }
    etcp->rx_list = NULL;
    
    // Reset state
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_conn_reset: clearing packet lists and resetting state");
    etcp->next_tx_id = 1;
    etcp->last_rx_id = 0;
    etcp->last_delivered_id = 0;
    etcp->unacked_bytes = 0;
    etcp->pending_ack_count = 0;
    etcp->pending_retrans_count = 0;
    etcp->rtt_last = 0;
    etcp->rtt_avg_10 = 0;
    etcp->rtt_avg_100 = 0;
    etcp->jitter = 0;
    etcp->burst_in_progress = 0;
    etcp->rtt_history_idx = 0;
    memset(etcp->rtt_history, 0, sizeof(etcp->rtt_history));
    // Queues cleared externally if needed
    
    DEBUG_INFO(DEBUG_CATEGORY_ETCP, "etcp_conn_reset: connection %p reset completed", etcp);
}

// Input decrypted packet (unchanged structure, but calls expanded processes)
void etcp_conn_input(struct ETCP_DGRAM* pkt) {
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_conn_input: pkt=%p, link=%p, etcp=%p", 
                pkt, pkt ? pkt->link : NULL, pkt && pkt->link ? pkt->link->etcp : NULL);
    
    if (!pkt || !pkt->link || !pkt->link->etcp) {
        DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "etcp_conn_input: invalid parameters (pkt=%p, link=%p, etcp=%p)", 
                    pkt, pkt ? pkt->link : NULL, pkt && pkt->link ? pkt->link->etcp : NULL);
        return;
    }
    
    struct ETCP_CONN* etcp = pkt->link->etcp;
    uint8_t* data = pkt->data;
    uint16_t len = pkt->data_len;
    uint16_t ts = pkt->timestamp;
    
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_conn_input: processing packet - etcp=%p, data_len=%u, timestamp=%u, link=%p", 
                etcp, len, ts, pkt->link);
    
    // Update link activity
    pkt->link->last_activity = get_current_time_units();
    pkt->link->last_recv_local_time = pkt->link->last_activity;
    pkt->link->last_recv_timestamp = ts;
    
    DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "etcp_conn_input: updated link activity - last_activity=%llu, last_recv_timestamp=%u", 
                (unsigned long long)pkt->link->last_activity, pkt->link->last_recv_timestamp);
    
    // Parse sections
    uint16_t pos = 0;
    while (pos < len) {
        if (pos + SECTION_HEADER_SIZE > len) break;
        
        uint8_t type = data[pos];
        uint16_t sec_len = (data[pos+1] << 8) | data[pos+2];
        pos += SECTION_HEADER_SIZE;
        
        if (pos + sec_len > len) break;
        
        uint8_t* sec_data = data + pos;
        switch (type) {
            case ETCP_SECTION_ACK:
                process_section_ack(etcp, sec_data, sec_len);
                break;
            case ETCP_SECTION_RETRANS:
                process_section_retrans(etcp, sec_data, sec_len);
                break;
            case ETCP_SECTION_TIMESTAMP:
                process_section_timestamp(etcp, sec_data, sec_len);
                break;
            case ETCP_SECTION_PAYLOAD:
                process_section_payload(etcp, sec_data, sec_len);
                break;
            case ETCP_SECTION_MEAS_TS:
                process_section_meas_ts(etcp, sec_data, sec_len);
                break;
            case ETCP_SECTION_MEAS_RESP:
                process_section_meas_resp(etcp, sec_data, sec_len);
                break;
            default:
                DEBUG_WARN(DEBUG_CATEGORY_ETCP, "Unknown section type: 0x%02x", type);
                break;
        }
        pos += sec_len;
    }
    
    // Post-processing
    detect_gaps_and_request_retrans(etcp);
    deliver_contiguous_packets(etcp);
    
    // Trigger if waiting
    if (etcp->wait_timeout_active) {
        etcp->wait_timeout_active = 0;
        // Assume load balancer polls again
    }
}

// Request next packet (expanded)
struct ETCP_DGRAM* etcp_request_pkt(struct ETCP_CONN* etcp) {
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: etcp=%p", etcp);
    
    if (!etcp) {
        DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: called with NULL etcp");
        return NULL;
    }
    
    DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: input_queue_count=%d, wait_send_list=%p, unacked_bytes=%u, window_size=%u",
                queue_entry_count(etcp->input_queue), etcp->wait_send_list, 
                etcp->unacked_bytes, etcp->window_size);
    
    // Check waiting conditions
    if (queue_entry_count(etcp->input_queue) == 0 && etcp->wait_send_list == NULL) {
        DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: no packets available, setting callback and returning NULL");
        // Wait for input or retrans
        // Set callback for resume
        queue_set_callback(etcp->input_queue, input_queue_cb, etcp);  // Define cb to resume
        return NULL;
    }
    
    if (etcp->unacked_bytes >= etcp->window_size) {
        DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: window full (unacked_bytes=%u >= window_size=%u), waiting",
                    etcp->unacked_bytes, etcp->window_size);
        // Wait ACK or timeout
        etcp->wait_timeout_active = 1;
        return NULL;
    }
    
    if (etcp->unacked_bytes >= etcp->window_size) {
        // Wait ACK or timeout
        etcp->wait_timeout_active = 1;
        return NULL;
    }
    
    // Get packet (prefer retrans)
    inflight_packet_t* ipkt = etcp->wait_send_list;
    if (ipkt) {
        DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: using retransmission packet id=%u", ipkt->id);
        remove_inflight(etcp, ipkt);
    } else {
        DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: getting new packet from input queue");
        struct ll_entry* entry = queue_entry_get(etcp->input_queue);
        if (!entry) {
            DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: no entry available from input queue");
            return NULL;
        }
        
        ipkt = calloc(1, sizeof(inflight_packet_t));
        if (!ipkt) {
            DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: failed to allocate inflight_packet_t");
            queue_entry_put_first(etcp->input_queue, entry);
            return NULL;
        }
        
        ipkt->id = etcp->next_tx_id++;
        ipkt->data_len = ll_entry_size(entry);
        ipkt->data = malloc(ipkt->data_len);
        
        DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: created new packet id=%u, data_len=%u", 
                    ipkt->id, ipkt->data_len);
        
        if (!ipkt->data) {
            DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: failed to allocate packet data buffer");
            free(ipkt);
            queue_entry_put_first(etcp->input_queue, entry);
            return NULL;
        }
        memcpy(ipkt->data, ll_entry_data(entry), ipkt->data_len);
        ipkt->payload_len = ipkt->data_len;  // Assume full is payload for window
        queue_entry_free(entry);
    }
    
    // Select link
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: selecting link via loadbalancer");
    struct ETCP_LINK* link = etcp_loadbalancer_select_link(etcp);
    if (!link) {
        DEBUG_WARN(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: no suitable link available");
        // Put back if new
        if (ipkt->send_count == 0) {
            DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: putting new packet back to input queue");
            struct ll_entry* entry = queue_entry_new(ipkt->data_len);
            memcpy(ll_entry_data(entry), ipkt->data, ipkt->data_len);
            queue_entry_put_first(etcp->input_queue, entry);
        }
        free_inflight_packet(ipkt);
        return NULL;
    }
    
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: selected link %p", link);
    
    // Build packet buf
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: building packet with optional sections");
    uint8_t buf[1600];
    uint16_t len = append_optional_sections(etcp, buf, link);
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: optional sections added, len=%u", len);
    
    // Add payload (assume payload includes ID as first 2 bytes)
    uint16_t pay_len = ipkt->payload_len + 2;  // ID
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: payload section - pay_len=%u (payload_len=%u + 2 for ID)", 
                pay_len, ipkt->payload_len);
    
    if (len + SECTION_HEADER_SIZE + pay_len > sizeof(buf)) {
        DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: packet too large (%u + %u + %u > %u), dropping", 
                    len, SECTION_HEADER_SIZE, pay_len, (unsigned)sizeof(buf));
        free_inflight_packet(ipkt);
        return NULL;
    }
    buf[len++] = ETCP_SECTION_PAYLOAD;
    buf[len++] = (pay_len >> 8) & 0xFF;
    buf[len++] = pay_len & 0xFF;
    buf[len++] = (ipkt->id >> 8) & 0xFF;
    buf[len++] = ipkt->id & 0xFF;
    memcpy(buf + len, ipkt->data, ipkt->payload_len);
    len += ipkt->payload_len;
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: payload added, total len=%u", len);
    
    // Alloc dgram
    struct ETCP_DGRAM* dgram = memory_pool_alloc(etcp->instance->pkt_pool);
    if (!dgram) {
        DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: failed to allocate dgram from memory pool");
        free_inflight_packet(ipkt);
        return NULL;
    }
    dgram->link = link;
    dgram->data_len = len;
    dgram->noencrypt_len = 0;
    dgram->timestamp = get_current_timestamp();
    memcpy(dgram->data, buf, len);
    
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: allocated dgram %p (link=%p, data_len=%u, timestamp=%u)", 
                dgram, link, len, dgram->timestamp);
    
    // Update inflight
    ipkt->last_link = link;
    ipkt->last_timestamp = dgram->timestamp;
    ipkt->send_count++;
    etcp->unacked_bytes += ipkt->payload_len;
    etcp->bytes_sent_total += len;  // Approx
    
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: updated inflight - send_count=%u, unacked_bytes=%u, bytes_sent_total=%u", 
                ipkt->send_count, etcp->unacked_bytes, etcp->bytes_sent_total);
    
    // Move to wait_ack
    ipkt->state = INFLIGHT_STATE_WAIT_ACK;
    ipkt->next = etcp->wait_ack_list;
    etcp->wait_ack_list = ipkt;
    
    // Add to wait_ack list
    ipkt->next = etcp->wait_ack_list;
    etcp->wait_ack_list = ipkt;
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: added packet %u to wait_ack_list", ipkt->id);
    
    // Start retrans timer if empty before
    if (etcp->wait_ack_list->next == NULL && !etcp->retrans_timer) {
        DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: starting retrans_timer (wait_ack_list was empty)");
        etcp->retrans_timer = uasync_set_timeout(etcp->instance->ua, 20, etcp, retrans_timer_cb);
    }
    
    // Update load balancer after send (call from connections after actual send)
    // etcp_loadbalancer_update_after_send(link, len + headers);  // Assume called externally
    
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_request_pkt: successfully created dgram %p for packet id=%u", 
                dgram, ipkt->id);
    return dgram;
}

// Input queue callback (placeholder to resume load balancer)
static void input_queue_cb(struct ll_queue* q, struct ll_entry* entry, void* arg) {
    (void)q;
    (void)entry;
    struct ETCP_CONN* etcp = arg;
    // Resume polling etcp_request_pkt
    queue_resume_callback(q);
}

// Append optional (expanded with debug)
static uint16_t append_optional_sections(struct ETCP_CONN* etcp, uint8_t* buf, struct ETCP_LINK* link) {
    DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "append_optional_sections: etcp=%p, link=%p", etcp, link);
    
    uint16_t len = 0;
    int sections_added = 0;
    
    // Timestamp (assume 2 bytes TS)
    uint16_t ts = get_current_timestamp();
    buf[len++] = ETCP_SECTION_TIMESTAMP;
    buf[len++] = 0;  // len high
    buf[len++] = 2;  // len low
    buf[len++] = (ts >> 8) & 0xFF;
    buf[len++] = ts & 0xFF;
    sections_added++;
    DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "append_optional_sections: added TIMESTAMP section (ts=%u, len=%u)", ts, len);
    
    // ACK (assume count(1) + [id(2)+ts(2)]*count)
    if (etcp->pending_ack_count > 0) {
        uint16_t ack_len = 1 + etcp->pending_ack_count * 4;
        buf[len++] = ETCP_SECTION_ACK;
        buf[len++] = (ack_len >> 8) & 0xFF;
        buf[len++] = ack_len & 0xFF;
        buf[len++] = etcp->pending_ack_count;
        
        DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "append_optional_sections: adding ACK section with %u ACKs", etcp->pending_ack_count);
        
        for (uint8_t i = 0; i < etcp->pending_ack_count; i++) {
            uint16_t id = etcp->pending_ack_ids[i];
            uint16_t ats = etcp->pending_ack_ts[i];
            buf[len++] = (id >> 8) & 0xFF;
            buf[len++] = id & 0xFF;
            buf[len++] = (ats >> 8) & 0xFF;
            buf[len++] = ats & 0xFF;
            DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "append_optional_sections: ACK[%u] - id=%u, ts=%u", i, id, ats);
        }
        
        etcp->pending_ack_count = 0;
        etcp->ack_packets_count++;
        sections_added++;
        DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "append_optional_sections: added ACK section (total_len=%u)", len);
    }
    
    // RETRANS (similar: count(1) + id(2)*count)
    if (etcp->pending_retrans_count > 0) {
        uint16_t ret_len = 1 + etcp->pending_retrans_count * 2;
        buf[len++] = ETCP_SECTION_RETRANS;
        buf[len++] = (ret_len >> 8) & 0xFF;
        buf[len++] = ret_len & 0xFF;
        buf[len++] = etcp->pending_retrans_count;
        
        DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "append_optional_sections: adding RETRANS section with %u requests", etcp->pending_retrans_count);
        
        for (uint8_t i = 0; i < etcp->pending_retrans_count; i++) {
            uint16_t id = etcp->pending_retrans_ids[i];
            buf[len++] = (id >> 8) & 0xFF;
            buf[len++] = id & 0xFF;
            DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "append_optional_sections: RETRANS[%u] - id=%u", i, id);
        }
        
        etcp->pending_retrans_count = 0;
        sections_added++;
        DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "append_optional_sections: added RETRANS section (total_len=%u)", len);
    }
    
    // MEAS_TS if burst (assume [start_id(2)][n(1)])
    if (etcp->burst_in_progress) {
        DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "append_optional_sections: burst measurement in progress, would add MEAS_TS");
        // Logic for adding to burst packets
        // Placeholder
    }
    
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "append_optional_sections: completed - added %u sections, total_len=%u", 
                sections_added, len);
    
    return len;
}

// Process ACK (expanded)
static void process_section_ack(struct ETCP_CONN* etcp, uint8_t* data, uint16_t len) {
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_ack: etcp=%p, data=%p, len=%u", etcp, data, len);
    
    if (len < 1) {
        DEBUG_WARN(DEBUG_CATEGORY_ETCP, "process_section_ack: invalid length %u", len);
        return;
    }
    uint8_t count = data[0];
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_ack: count=%u", count);
    
    if (1 + count * 4 > len) {
        DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "process_section_ack: invalid count %u for len %u", count, len);
        return;
    }
    
    uint16_t lp_ts = 0;
    int acked_count = 0;
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_ack: processing %u ACK entries", count);
    
    for (uint8_t i = 0; i < count; i++) {
        uint16_t id = (data[1 + i*4] << 8) | data[2 + i*4];
        uint16_t ts = (data[3 + i*4] << 8) | data[4 + i*4];
        DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "process_section_ack: entry %u - id=%u, ts=%u", i, id, ts);
        
        inflight_packet_t* pkt = find_inflight(etcp, id, INFLIGHT_STATE_WAIT_ACK);
        if (pkt) {
            DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_ack: found inflight packet %u, removing (payload_len=%u)", 
                        id, pkt->payload_len);
            etcp->unacked_bytes -= pkt->payload_len;
            remove_inflight(etcp, pkt);
            free_inflight_packet(pkt);
            lp_ts = ts;  // Update last positive ts
            acked_count++;
        } else {
            DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "process_section_ack: no inflight packet found for id=%u", id);
        }
    }
    
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_ack: successfully ACKed %u/%u packets", acked_count, count);
    // Use lp_ts in retrans_check if needed
    etcp->last_rx_ack_id = (data[1 + (count-1)*4] << 8) | data[2 + (count-1)*4];  // Last ACK ID
}

// Process RETRANS (expanded)
static void process_section_retrans(struct ETCP_CONN* etcp, uint8_t* data, uint16_t len) {
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_retrans: etcp=%p, data=%p, len=%u", etcp, data, len);
    
    if (len < 1) {
        DEBUG_WARN(DEBUG_CATEGORY_ETCP, "process_section_retrans: invalid length %u", len);
        return;
    }
    uint8_t count = data[0];
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_retrans: count=%u", count);
    
    if (1 + count * 2 > len) {
        DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "process_section_retrans: invalid count %u for len %u", count, len);
        return;
    }
    
    int retrans_count = 0;
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_retrans: processing %u retrans requests", count);
    
    for (uint8_t i = 0; i < count; i++) {
        uint16_t id = (data[1 + i*2] << 8) | data[2 + i*2];
        DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "process_section_retrans: requesting retrans for id=%u", id);
        
        inflight_packet_t* pkt = find_inflight(etcp, id, INFLIGHT_STATE_WAIT_ACK);
        if (pkt) {
            DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_retrans: found packet %u, marking for retransmission", id);
            pkt->retrans_req_count++;
            pkt->need_retrans = 1;
            move_to_wait_send(etcp, pkt);
            retrans_count++;
        } else {
            DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "process_section_retrans: no inflight packet found for id=%u", id);
        }
    }
    
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_retrans: marked %u/%u packets for retransmission", retrans_count, count);
}

// Process TIMESTAMP (expanded)
static void process_section_timestamp(struct ETCP_CONN* etcp, uint8_t* data, uint16_t len) {
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_timestamp: etcp=%p, data=%p, len=%u", etcp, data, len);
    
    if (len != 2) {
        DEBUG_WARN(DEBUG_CATEGORY_ETCP, "process_section_timestamp: invalid length %u (expected 2)", len);
        return;
    }
    uint16_t recv_ts = (data[0] << 8) | data[1];
    uint16_t current_ts = get_current_timestamp();
    uint16_t rtt = timestamp_diff(current_ts, recv_ts);
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_timestamp: recv_ts=%u, current_ts=%u, calculated_rtt=%u", 
                recv_ts, current_ts, rtt);
    update_rtt(etcp, rtt);
}

// Process PAYLOAD (expanded)
static void process_section_payload(struct ETCP_CONN* etcp, uint8_t* data, uint16_t len) {
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_payload: etcp=%p, data=%p, len=%u", etcp, data, len);
    
    if (len < 2) {
        DEBUG_WARN(DEBUG_CATEGORY_ETCP, "process_section_payload: invalid length %u (minimum 2)", len);
        return;
    }
    uint16_t id = (data[0] << 8) | data[1];
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_payload: packet id=%u, last_rx_id=%u", id, etcp->last_rx_id);
    
    if (id <= etcp->last_rx_id) {
        DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "process_section_payload: duplicate packet id=%u (last_rx_id=%u), ignoring", 
                    id, etcp->last_rx_id);
        return;  // Dup
    }
    
    rx_packet_t* new_rx = calloc(1, sizeof(rx_packet_t));
    if (!new_rx) {
        DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "process_section_payload: failed to allocate rx_packet_t");
        return;
    }
    
    new_rx->id = id;
    new_rx->timestamp = get_current_timestamp();
    new_rx->data_len = len - 2;
    
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_payload: allocated rx_packet %p (id=%u, data_len=%u, timestamp=%u)", 
                new_rx, new_rx->id, new_rx->data_len, new_rx->timestamp);
    
    new_rx->data = malloc(new_rx->data_len);
    if (!new_rx->data) {
        DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "process_section_payload: failed to allocate rx data buffer (size=%u)", new_rx->data_len);
        free(new_rx);
        return;
    }
    memcpy(new_rx->data, data + 2, new_rx->data_len);
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_payload: copied %u bytes of payload data", new_rx->data_len);
    
    // Insert sorted (assume IDs sequential, no wrap for simplicity)
    DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "process_section_payload: inserting packet into rx_list (id=%u)", id);
    rx_packet_t** ptr = &etcp->rx_list;
    while (*ptr && (*ptr)->id < id) ptr = &(*ptr)->next;
    if (*ptr && (*ptr)->id == id) {
        DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "process_section_payload: duplicate packet id=%u found in rx_list, discarding", id);
        free(new_rx->data);
        free(new_rx);
        return;
    }
    new_rx->next = *ptr;
    *ptr = new_rx;
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_payload: successfully inserted packet id=%u into rx_list", id);
    
    if (id > etcp->last_rx_id) {
        DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "process_section_payload: updating last_rx_id from %u to %u", 
                    etcp->last_rx_id, id);
        etcp->last_rx_id = id;
    }
    
    // Pending ACK
    if (etcp->pending_ack_count < MAX_PENDING) {
        DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "process_section_payload: adding packet id=%u to pending ACKs (count=%u)", 
                    id, etcp->pending_ack_count + 1);
        etcp->pending_ack_ids[etcp->pending_ack_count] = id;
        etcp->pending_ack_ts[etcp->pending_ack_count++] = new_rx->timestamp;
    } else {
        DEBUG_WARN(DEBUG_CATEGORY_ETCP, "process_section_payload: pending ACK buffer full (%u), cannot add id=%u", 
                  MAX_PENDING, id);
    }
    
    // Schedule ACK
    if (!etcp->ack_timer) {
        DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_payload: scheduling ACK timer (delay=%u tb)", ACK_DELAY_TB);
        etcp->ack_timer = uasync_set_timeout(etcp->instance->ua, ACK_DELAY_TB, etcp, ack_timer_cb);
    }
    
    etcp->bytes_received_total += len;
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_payload: packet id=%u processed successfully, bytes_received_total=%u", 
                id, etcp->bytes_received_total);
}

// Process MEAS_TS (expanded: send resp)
static void process_section_meas_ts(struct ETCP_CONN* etcp, uint8_t* data, uint16_t len) {
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_meas_ts: etcp=%p, data=%p, len=%u", etcp, data, len);
    
    if (len < 3) {
        DEBUG_WARN(DEBUG_CATEGORY_ETCP, "process_section_meas_ts: invalid length %u (minimum 3)", len);
        return;
    }
    uint16_t start_id = (data[0] << 8) | data[1];
    uint8_t n = data[2];
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_meas_ts: start_id=%u, n=%u", start_id, n);
    
    // Send MEAS_RESP (add to pending or direct)
    // Placeholder: assume added to next packet
    DEBUG_INFO(DEBUG_CATEGORY_ETCP, "MEAS_TS: start_id=%u n=%u", start_id, n);
    // Build resp [start_id(2)][n(1)][recv_ts(2)]
    // But since appended in optional, set state
}

// Process MEAS_RESP (expanded: calc bw)
static void process_section_meas_resp(struct ETCP_CONN* etcp, uint8_t* data, uint16_t len) {
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_meas_resp: etcp=%p, data=%p, len=%u", etcp, data, len);
    
    // Assume format [start_id(2)][n(1)][recv_ts(2)]
    if (len < 5) {
        DEBUG_WARN(DEBUG_CATEGORY_ETCP, "process_section_meas_resp: invalid length %u (minimum 5)", len);
        return;
    }
    uint16_t start_id = (data[0] << 8) | data[1];
    uint8_t n = data[2];
    uint16_t recv_ts = (data[3] << 8) | data[4];
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "process_section_meas_resp: start_id=%u, n=%u, recv_ts=%u", start_id, n, recv_ts);
    
    if (etcp->burst_start_id != start_id || !etcp->burst_in_progress) return;
    
    // Calc bw based on burst (placeholder logic)
    uint16_t delta = timestamp_diff(get_current_timestamp(), recv_ts);
    uint32_t bw = (BURST_SIZE * etcp->mtu * 8) / (delta / 1000.0);  // bits/sec approx
    update_link_bandwidth(etcp->links, bw);  // Assume primary link
    etcp->burst_in_progress = 0;
}

// Detect gaps (expanded)
static void detect_gaps_and_request_retrans(struct ETCP_CONN* etcp) {
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "detect_gaps_and_request_retrans: etcp=%p, last_delivered_id=%u", 
                etcp, etcp->last_delivered_id);
    
    rx_packet_t* rx = etcp->rx_list;
    uint16_t expected = etcp->last_delivered_id + 1;
    int gaps_found = 0;
    int retrans_requested = 0;
    
    DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "detect_gaps_and_request_retrans: starting gap detection from expected_id=%u", expected);
    
    while (rx) {
        DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "detect_gaps_and_request_retrans: checking rx packet id=%u (expected=%u)", 
                    rx->id, expected);
        
        if (rx->id > expected) {
            // Gap: add retrans for expected to rx->id -1
            DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "detect_gaps_and_request_retrans: gap detected - expected=%u, found=%u", 
                        expected, rx->id);
            gaps_found++;
            
            for (uint16_t gid = expected; gid < rx->id && etcp->pending_retrans_count < MAX_PENDING; gid++) {
                etcp->pending_retrans_ids[etcp->pending_retrans_count++] = gid;
                retrans_requested++;
                DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "detect_gaps_and_request_retrans: queued retrans request for id=%u", gid);
            }
            expected = rx->id + 1;
        } else if (rx->id == expected) {
            DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "detect_gaps_and_request_retrans: found expected packet id=%u", expected);
            expected++;
        } else {
            DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "detect_gaps_and_request_retrans: packet id=%u < expected=%u, already processed", 
                        rx->id, expected);
        }
        rx = rx->next;
    }
    
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "detect_gaps_and_request_retrans: completed - gaps_found=%d, retrans_requested=%u", 
                gaps_found, retrans_requested);
}

// Deliver contiguous (expanded)
static void deliver_contiguous_packets(struct ETCP_CONN* etcp) {
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "deliver_contiguous_packets: etcp=%p, last_delivered_id=%u", 
                etcp, etcp->last_delivered_id);
    
    rx_packet_t** ptr = &etcp->rx_list;
    int delivered_count = 0;
    uint32_t delivered_bytes = 0;
    
    while (*ptr && (*ptr)->id == etcp->last_delivered_id + 1) {
        rx_packet_t* rx = *ptr;
        DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "deliver_contiguous_packets: delivering packet id=%u (data_len=%u)", 
                    rx->id, rx->data_len);
        
        struct ll_entry* entry = queue_entry_new(rx->data_len);
        if (entry) {
            memcpy(ll_entry_data(entry), rx->data, rx->data_len);
            queue_entry_put(etcp->output_queue, entry);
            delivered_bytes += rx->data_len;
            DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "deliver_contiguous_packets: queued packet id=%u to output_queue", rx->id);
        } else {
            DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "deliver_contiguous_packets: failed to allocate queue entry for packet id=%u", rx->id);
        }
        
        etcp->last_delivered_id = rx->id;
        *ptr = rx->next;
        free(rx->data);
        free(rx);
        delivered_count++;
    }
    
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "deliver_contiguous_packets: delivered %u contiguous packets (%u bytes), last_delivered_id=%u", 
                delivered_count, delivered_bytes, etcp->last_delivered_id);
}

// Retrans check (expanded with lp_ts)
static void retrans_check(struct ETCP_CONN* etcp) {
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "retrans_check: etcp=%p, wait_ack_list=%p, rtt_avg_10=%u, jitter=%u", 
                etcp, etcp->wait_ack_list, etcp->rtt_avg_10, etcp->jitter);
    
    uint64_t now = get_current_time_units();
    inflight_packet_t** ptr = &etcp->wait_ack_list;
    uint16_t lp_ts = 0;
    int checked_packets = 0;
    int retrans_packets = 0;
    uint16_t timeout_calc = (uint16_t)(etcp->rtt_avg_10 * RETRANS_K1 + etcp->jitter * RETRANS_K2);
    
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "retrans_check: timeout calculation: rtt_avg_10=%u * %.1f + jitter=%u * %.1f = %u", 
                etcp->rtt_avg_10, RETRANS_K1, etcp->jitter, RETRANS_K2, timeout_calc);
    
    while (*ptr) {
        inflight_packet_t* pkt = *ptr;
        checked_packets++;
        uint16_t timeout = (uint16_t)(etcp->rtt_avg_10 * RETRANS_K1 + etcp->jitter * RETRANS_K2);
        uint16_t time_since_send = timestamp_diff(now, pkt->last_timestamp);
        
        DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "retrans_check: checking packet id=%u (time_since_send=%u, timeout=%u, need_retrans=%u)", 
                    pkt->id, time_since_send, timeout, pkt->need_retrans);
        
        if (time_since_send > timeout) {
            DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "retrans_check: packet id=%u timed out, marking for retransmission", pkt->id);
            pkt->need_retrans = 1;
            move_to_wait_send(etcp, pkt);
            etcp->retransmissions_count++;
            retrans_packets++;
            ptr = &etcp->wait_ack_list;  // Restart scan after move
            continue;
        }
        // lp_ts logic (per spec: if acked, remember ts; if later unacked with ts < lp_ts, ?)
        // Assume if pkt acked (but shouldn't be in list), or for forward progress
        ptr = &pkt->next;
    }
    
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "retrans_check: completed - checked %d packets, retransmitted %d packets, total_retransmissions=%u", 
                checked_packets, retrans_packets, etcp->retransmissions_count);
    
    // Reschedule if list not empty
    if (etcp->wait_ack_list) {
        DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "retrans_check: rescheduling retrans_timer (wait_ack_list not empty)");
        etcp->retrans_timer = uasync_set_timeout(etcp->instance->ua, 20, etcp, retrans_timer_cb);
    } else {
        DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "retrans_check: clearing retrans_timer (wait_ack_list empty)");
        etcp->retrans_timer = NULL;
    }
}

// Find inflight (expanded)
static inflight_packet_t* find_inflight(struct ETCP_CONN* etcp, uint16_t id, uint8_t state) {
    DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "find_inflight: etcp=%p, id=%u, state=%u", etcp, id, state);
    
    inflight_packet_t* list = (state == INFLIGHT_STATE_WAIT_ACK) ? etcp->wait_ack_list : etcp->wait_send_list;
    inflight_packet_t* pkt = list;
    int position = 0;
    
    while (pkt && pkt->id != id) {
        pkt = pkt->next;
        position++;
    }
    
    if (pkt) {
        DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "find_inflight: found packet %u at position %d in %s list", 
                    id, position, (state == INFLIGHT_STATE_WAIT_ACK) ? "wait_ack" : "wait_send");
    } else {
        DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "find_inflight: packet %u not found in %s list", 
                    id, (state == INFLIGHT_STATE_WAIT_ACK) ? "wait_ack" : "wait_send");
    }
    
    return pkt;
    return pkt;
}

// Move to wait_send (expanded)
static void move_to_wait_send(struct ETCP_CONN* etcp, inflight_packet_t* pkt) {
    DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "move_to_wait_send: etcp=%p, pkt=%p (id=%u, state=%u)", 
                etcp, pkt, pkt->id, pkt->state);
    
    remove_inflight(etcp, pkt);
    pkt->state = INFLIGHT_STATE_WAIT_SEND;
    pkt->next = etcp->wait_send_list;
    etcp->wait_send_list = pkt;
    
    DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "move_to_wait_send: moved packet %u to wait_send_list", pkt->id);
}

// Remove inflight (expanded)
static void remove_inflight(struct ETCP_CONN* etcp, inflight_packet_t* pkt) {
    DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "remove_inflight: etcp=%p, pkt=%p (id=%u, state=%u)", 
                etcp, pkt, pkt->id, pkt->state);
    
    inflight_packet_t** list = (pkt->state == INFLIGHT_STATE_WAIT_ACK) ? &etcp->wait_ack_list : &etcp->wait_send_list;
    inflight_packet_t** ptr = list;
    int position = 0;
    
    while (*ptr && *ptr != pkt) {
        ptr = &(*ptr)->next;
        position++;
    }
    
    if (*ptr) {
        DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "remove_inflight: found packet %u at position %d in %s list", 
                    pkt->id, position, (pkt->state == INFLIGHT_STATE_WAIT_ACK) ? "wait_ack" : "wait_send");
        *ptr = (*ptr)->next;
    } else {
        DEBUG_WARN(DEBUG_CATEGORY_ETCP, "remove_inflight: packet %u not found in %s list", 
                  pkt->id, (pkt->state == INFLIGHT_STATE_WAIT_ACK) ? "wait_ack" : "wait_send");
    }
}

// Start burst (placeholder expanded)
static void start_burst_measurement(struct ETCP_CONN* etcp, struct ETCP_LINK* link) {
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "start_burst_measurement: etcp=%p, link=%p", etcp, link);
    
    // Periodically call this
    etcp->burst_in_progress = 1;
    uint16_t old_start_id = etcp->burst_start_id;
    etcp->burst_start_id = etcp->next_tx_id;
    
    DEBUG_INFO(DEBUG_CATEGORY_ETCP, "start_burst_measurement: started burst measurement - burst_start_id=%u (was %u)", 
                etcp->burst_start_id, old_start_id);
    
    // Delay first, then burst 4
    // Assume handled in request_pkt by adding meas_ts to next BURST_SIZE pkts
}

// Update link bw (expanded)
static void update_link_bandwidth(struct ETCP_LINK* link, uint32_t new_bw) {
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "update_link_bandwidth: link=%p, new_bw=%u bits/sec (current=%u)", 
                link, new_bw, link->bandwidth);
    
    uint32_t old_bw = link->bandwidth;
    link->bandwidth = (uint32_t)((link->bandwidth * 0.9) + (new_bw * 0.1));  // Smooth
    
    DEBUG_INFO(DEBUG_CATEGORY_ETCP, "update_link_bandwidth: updated link bandwidth from %u to %u bits/sec (new_bw=%u)", 
                old_bw, link->bandwidth, new_bw);
}

// Update RTT (expanded with history)
static void update_rtt(struct ETCP_CONN* etcp, uint16_t new_rtt) {
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "update_rtt: etcp=%p, new_rtt=%u (last_rtt=%u, avg_10=%u, avg_100=%u)", 
                etcp, new_rtt, etcp->rtt_last, etcp->rtt_avg_10, etcp->rtt_avg_100);
    
    etcp->rtt_last = new_rtt;
    etcp->rtt_avg_10 = (etcp->rtt_avg_10 * 9 + new_rtt) / 10;
    etcp->rtt_avg_100 = (etcp->rtt_avg_100 * 99 + new_rtt) / 100;
    
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "update_rtt: updated RTT metrics - last=%u, avg_10=%u, avg_100=%u", 
                etcp->rtt_last, etcp->rtt_avg_10, etcp->rtt_avg_100);
    
    // History for jitter
    etcp->rtt_history[etcp->rtt_history_idx++] = new_rtt;
    if (etcp->rtt_history_idx >= RTT_HISTORY_SIZE) etcp->rtt_history_idx = 0;
    DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "update_rtt: stored RTT in history at index %u", 
                (etcp->rtt_history_idx + RTT_HISTORY_SIZE - 1) % RTT_HISTORY_SIZE);
    
    calculate_jitter(etcp);
}

static void calculate_jitter(struct ETCP_CONN* etcp) {
    uint16_t min_rtt = UINT16_MAX;
    uint16_t max_rtt = 0;
    int valid_samples = 0;
    
    DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "calculate_jitter: analyzing RTT history (history_idx=%u)", etcp->rtt_history_idx);
    
    for (int i = 0; i < RTT_HISTORY_SIZE; i++) {
        if (etcp->rtt_history[i] == 0) {
            DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "calculate_jitter: history[%d] = 0 (invalid), skipping", i);
            continue;
        }
        valid_samples++;
        if (etcp->rtt_history[i] < min_rtt) {
            DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "calculate_jitter: new min_rtt=%u (was %u) at history[%d]", 
                        etcp->rtt_history[i], min_rtt, i);
            min_rtt = etcp->rtt_history[i];
        }
        if (etcp->rtt_history[i] > max_rtt) {
            DEBUG_TRACE(DEBUG_CATEGORY_ETCP, "calculate_jitter: new max_rtt=%u (was %u) at history[%d]", 
                        etcp->rtt_history[i], max_rtt, i);
            max_rtt = etcp->rtt_history[i];
        }
    }
    
    uint16_t old_jitter = etcp->jitter;
    etcp->jitter = max_rtt - min_rtt;
    
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "calculate_jitter: completed - valid_samples=%d, min_rtt=%u, max_rtt=%u, jitter=%u (was %u)", 
                valid_samples, min_rtt, max_rtt, etcp->jitter, old_jitter);
}

// Timers (expanded with debug)
static void retrans_timer_cb(void* arg) {
    struct ETCP_CONN* etcp = (struct ETCP_CONN*)arg;
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "retrans_timer_cb: etcp=%p, triggered retransmission check", etcp);
    retrans_check(etcp);
}

static void ack_timer_cb(void* arg) {
    struct ETCP_CONN* etcp = (struct ETCP_CONN*)arg;
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "ack_timer_cb: etcp=%p, processing pending ACKs", etcp);
    etcp->ack_timer = NULL;
    
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "ack_timer_cb: processing %u pending ACKs", etcp->pending_ack_count);
    
    // Force send ACK if pending (e.g., empty payload pkt)
    // For now, assume appended next
    
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "ack_timer_cb: ACK processing completed");
}

// Get stats (expanded)
void etcp_get_stats(struct ETCP_CONN* etcp, size_t* packets_sent, size_t* packets_recv,
                    size_t* pool_allocs, size_t* pool_reuse) {
    DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_get_stats: etcp=%p", etcp);
    
    if (packets_sent) {
        *packets_sent = etcp->total_packets_sent;
        DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_get_stats: total_packets_sent=%zu", *packets_sent);
    }
    if (packets_recv) {
        *packets_recv = etcp->bytes_received_total / 1400;  // Approx MTU
        DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_get_stats: bytes_received_total=%u, estimated_packets=%zu", 
                    etcp->bytes_received_total, *packets_recv);
    }
    if (pool_allocs || pool_reuse) {
        DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_get_stats: getting memory pool stats");
        memory_pool_get_stats(etcp->instance->pkt_pool, pool_allocs, pool_reuse);
        if (pool_allocs) DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_get_stats: pool_allocs=%zu", *pool_allocs);
        if (pool_reuse) DEBUG_DEBUG(DEBUG_CATEGORY_ETCP, "etcp_get_stats: pool_reuse=%zu", *pool_reuse);
    }
}