#include "etcp_connections.h"
#include <arpa/inet.h>
#include <net/if.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <string.h>
#include "routing.h"
#include "utun_instance.h"
#include "config_parser.h"
#include "crc32.h"
#include "etcp.h"
#include "../lib/memory_pool.h"
#include "../lib/u_async.h"
#include <stdlib.h>
#include <time.h>

// Simple debug macros to replace missing debug_config.h
#define DEBUG_CATEGORY_CONNECTION 1
#define DEBUG_CATEGORY_ETCP 2
#define DEBUG_CATEGORY_CRYPTO 3
#define DEBUG_CATEGORY_CONFIG 4
#define DEBUG_CATEGORY_MEMORY 5

// Forward declaration
static void etcp_connections_read_callback(int fd, void* arg);

#define DEBUG_ERROR(category, fmt, ...) fprintf(stderr, "ERROR: " fmt "\n", ##__VA_ARGS__)
#define DEBUG_WARN(category, fmt, ...) fprintf(stderr, "WARN: " fmt "\n", ##__VA_ARGS__)
#define DEBUG_INFO(category, fmt, ...) fprintf(stdout, "INFO: " fmt "\n", ##__VA_ARGS__)

// Minimal packet dump - only shows first 16 bytes to avoid slowing down
static void dump_packet_bytes(const char* prefix, const uint8_t* data, size_t len) {
    printf("[ETCP DUMP] %s: len=%zu; dump: ", prefix, len);
    size_t show = len < 160 ? len : 160;
    for (size_t i = 0; i < show; i++) {
        printf("%02x ", data[i]);
    }
    if (len > 160) printf("...");
    printf("\n");
}


// Forward declarations for missing functions
struct ETCP_CONN* etcp_connection_create(struct UTUN_INSTANCE* instance);

// CONNECTION MANAGEMENT (!!!это всё должно быть static!!!)
static void etcp_link_remove_from_connections(struct ETCP_SOCKET* conn, struct ETCP_LINK* link);


// Отправка кодограмм протокола (!!!это всё должно быть static!!!)
static void etcp_link_send_init(struct ETCP_LINK* link);
static int etcp_link_send_reset(struct ETCP_LINK* link);
static void etcp_link_init_timer_cbk(void* arg);

#define INIT_TIMEOUT_INITIAL 500
#define INIT_TIMEOUT_MAX 50000

static void etcp_link_send_init(struct ETCP_LINK* link) {
    if (!link || !link->etcp || !link->etcp->instance) return;
    
    struct ETCP_DGRAM* dgram = malloc(sizeof(struct ETCP_DGRAM) + 100);
    if (!dgram) return;
    
    dgram->link = link;
    dgram->noencrypt_len = SC_PUBKEY_SIZE;
    size_t offset = 0;
    
    dgram->data[offset++] = ETCP_INIT_REQUEST;
    
    uint64_t node_id = link->etcp->instance->node_id;
    dgram->data[offset++] = (node_id >> 56) & 0xFF;
    dgram->data[offset++] = (node_id >> 48) & 0xFF;
    dgram->data[offset++] = (node_id >> 40) & 0xFF;
    dgram->data[offset++] = (node_id >> 32) & 0xFF;
    dgram->data[offset++] = (node_id >> 24) & 0xFF;
    dgram->data[offset++] = (node_id >> 16) & 0xFF;
    dgram->data[offset++] = (node_id >> 8) & 0xFF;
    dgram->data[offset++] = node_id & 0xFF;
    
    dgram->data[offset++] = (link->mtu >> 8) & 0xFF;
    dgram->data[offset++] = link->mtu & 0xFF;
    
    dgram->data[offset++] = (link->keepalive_interval >> 8) & 0xFF;
    dgram->data[offset++] = link->keepalive_interval & 0xFF;
    
    memcpy(dgram->data + offset, link->etcp->instance->my_keys.public_key, SC_PUBKEY_SIZE);
    dgram->data_len = offset + SC_PUBKEY_SIZE;
    
    DEBUG_INFO(DEBUG_CATEGORY_CONNECTION, "Sending INIT request to link, node_id=%llu, retry=%d", (unsigned long long)node_id, link->init_retry_count);
    
    // Debug: print remote address before sending
    if (link->remote_addr.ss_family == AF_INET) {
        struct sockaddr_in* sin = (struct sockaddr_in*)&link->remote_addr;
        char addr_str[INET_ADDRSTRLEN];
        inet_ntop(AF_INET, &sin->sin_addr, addr_str, INET_ADDRSTRLEN);
        printf("[ETCP] INIT sending to %s:%d, link=%p, conn_fd=%d\n", addr_str, ntohs(sin->sin_port), link, link->conn->fd);
    }
    
    etcp_encrypt_send(dgram);
    free(dgram);
    
    link->init_retry_count++;
    
    if (!link->init_timer && link->is_server == 0) {
        link->init_timeout = INIT_TIMEOUT_INITIAL;
        link->init_timer = uasync_set_timeout(link->etcp->instance->ua, link->init_timeout, link, etcp_link_init_timer_cbk);
    } else if (link->init_timer) {
        if ((link->init_retry_count % 10) == 0 && link->init_timeout < INIT_TIMEOUT_MAX) {
            link->init_timeout *= 2;
            if (link->init_timeout > INIT_TIMEOUT_MAX) link->init_timeout = INIT_TIMEOUT_MAX;
        }
        uasync_cancel_timeout(link->etcp->instance->ua, link->init_timer);
        link->init_timer = uasync_set_timeout(link->etcp->instance->ua, link->init_timeout, link, etcp_link_init_timer_cbk);
    }
}

static void etcp_link_init_timer_cbk(void* arg) {
    struct ETCP_LINK* link = (struct ETCP_LINK*)arg;
    if (!link || link->initialized || link->is_server != 0) return;
    
    link->init_timer = NULL;
    etcp_link_send_init(link);
}

static int etcp_link_send_reset(struct ETCP_LINK* link) {
    if (!link) return -1;
    
    struct ETCP_DGRAM* dgram = malloc(sizeof(struct ETCP_DGRAM) + 1);
    if (!dgram) return -1;
    
    dgram->link = link;
    dgram->data_len = 1;
    dgram->noencrypt_len = 0;
    dgram->data[0] = 0x06;
    
    DEBUG_INFO(DEBUG_CATEGORY_CONNECTION, "Sending RESET to link");
    int ret = etcp_encrypt_send(dgram);
    free(dgram);
    return ret;
}

static uint32_t sockaddr_hash(struct sockaddr_storage* addr) {
    socklen_t addr_len = (addr->ss_family == AF_INET) ? sizeof(struct sockaddr_in) : sizeof(struct sockaddr_in6);
    return crc32_calc((void*)addr, addr_len);
    }

// Бинарный поиск линка по ip_port_hash
static int find_link_index(struct ETCP_SOCKET* e_sock, uint32_t hash) {
    if (!e_sock || e_sock->num_channels == 0) return -1;
    
    int left = 0;
    int right = e_sock->num_channels - 1;
    
    while (left <= right) {
        int mid = left + (right - left) / 2;
        if (e_sock->links[mid]->ip_port_hash == hash) {
            return mid;
        } else if (e_sock->links[mid]->ip_port_hash < hash) {
            left = mid + 1;
        } else {
            right = mid - 1;
        }
    }
    
    return -(left + 1);
}

// Реалокация массива линков с увеличением в 2 раза
static int realloc_links(struct ETCP_SOCKET* e_sock) {
    size_t new_max = e_sock->max_channels == 0 ? 8 : e_sock->max_channels * 2;
    struct ETCP_LINK** new_links = realloc(e_sock->links, new_max * sizeof(struct ETCP_LINK*));
    if (!new_links) return -1;
    
    e_sock->links = new_links;
    e_sock->max_channels = new_max;
    return 0;
}

// Вставка линка в отсортированный массив
static int insert_link(struct ETCP_SOCKET* e_sock, struct ETCP_LINK* link) {
    if (!e_sock || !link) return -1;
    
    if (e_sock->num_channels >= e_sock->max_channels) {
        if (realloc_links(e_sock) < 0) return -1;
    }
    
    int idx = find_link_index(e_sock, link->ip_port_hash);
    if (idx >= 0) return -1;
    
    idx = -(idx + 1);
    
    if (idx < (int)e_sock->num_channels) {
        memmove(&e_sock->links[idx + 1], &e_sock->links[idx], 
                (e_sock->num_channels - idx) * sizeof(struct ETCP_LINK*));
    }
    
    e_sock->links[idx] = link;
    e_sock->num_channels++;
    return 0;
}

// Удаление линка из массива
static void remove_link(struct ETCP_SOCKET* e_sock, uint32_t hash) {
    if (!e_sock || e_sock->num_channels == 0) return;
    
    int idx = find_link_index(e_sock, hash);
    if (idx < 0) return;
    
    if (idx < (int)e_sock->num_channels - 1) {
        memmove(&e_sock->links[idx], &e_sock->links[idx + 1], 
                (e_sock->num_channels - idx - 1) * sizeof(struct ETCP_LINK*));
    }
    
    e_sock->num_channels--;
}

// надо править, используй sockaddr_hash
struct ETCP_LINK* etcp_link_find_by_addr(struct ETCP_SOCKET* e_sock, struct sockaddr_storage* addr) {
    if (!e_sock || !addr) return NULL;
    
    int idx = find_link_index(e_sock, sockaddr_hash(addr));
    if (idx < 0) return NULL;
    
    return e_sock->links[idx];
}


// ===============================

struct ETCP_SOCKET* etcp_socket_add(struct UTUN_INSTANCE* instance, struct sockaddr_storage* ip, uint32_t netif_index, int so_mark, uint8_t type) {
    if (!instance) return NULL;
    
    struct ETCP_SOCKET* e_sock = calloc(1, sizeof(struct ETCP_SOCKET));
    if (!e_sock) {
        DEBUG_ERROR(DEBUG_CATEGORY_MEMORY, "Failed to allocate connection");
        return NULL;
    }

    int family = AF_INET;
    if (ip) {
        family = ip->ss_family;
        if (family != AF_INET && family != AF_INET6) {
            DEBUG_ERROR(DEBUG_CATEGORY_CONNECTION, "Unsupported address family: %d", family);
            free(e_sock);
            return NULL;
        }
    }
    
    e_sock->fd = socket(family, SOCK_DGRAM, 0);
    if (e_sock->fd < 0) {
        DEBUG_ERROR(DEBUG_CATEGORY_CONNECTION, "Failed to create socket: %s", strerror(errno));
        free(e_sock);
        return NULL;
    }
    
    int flags = fcntl(e_sock->fd, F_GETFL, 0);
    fcntl(e_sock->fd, F_SETFL, flags | O_NONBLOCK);
    
    // Set socket mark if specified
    if (so_mark > 0) {
#ifdef SO_MARK
        if (setsockopt(e_sock->fd, SOL_SOCKET, SO_MARK, &so_mark, sizeof(so_mark)) < 0) {
            DEBUG_ERROR(DEBUG_CATEGORY_CONNECTION, "Failed to set SO_MARK: %s", strerror(errno));
        }
#endif
    }
    
    // Bind to interface if specified
    if (netif_index > 0) {
#ifdef SO_BINDTODEVICE
        char ifname[IF_NAMESIZE];
        if (if_indextoname(netif_index, ifname)) {
            if (setsockopt(e_sock->fd, SOL_SOCKET, SO_BINDTODEVICE, ifname, strlen(ifname)) < 0) {
                DEBUG_ERROR(DEBUG_CATEGORY_CONNECTION, "Failed to bind to interface %s: %s", ifname, strerror(errno));
            }
        }
#endif
    }
    
    // Store the local address and bind socket if provided
    if (ip) {
        memcpy(&e_sock->local_addr, ip, sizeof(struct sockaddr_storage));
        
        // CRITICAL: Actually bind the socket to the address - this was missing!
        socklen_t addr_len = (ip->ss_family == AF_INET) ? sizeof(struct sockaddr_in) : sizeof(struct sockaddr_in6);
        if (bind(e_sock->fd, (struct sockaddr*)ip, addr_len) < 0) {
            perror("bind");
            printf("[ETCP] Failed to bind socket to address family %d\n", ip->ss_family);
            if (ip->ss_family == AF_INET) {
                struct sockaddr_in* sin = (struct sockaddr_in*)ip;
                char addr_str[INET_ADDRSTRLEN];
                inet_ntop(AF_INET, &sin->sin_addr, addr_str, INET_ADDRSTRLEN);
                printf("[ETCP] Failed to bind to %s:%d\n", addr_str, ntohs(sin->sin_port));
            }
            close(e_sock->fd);
            free(e_sock);
            return NULL;
        }
        
        printf("[ETCP] Successfully bound socket to local address, family=%d\n", ip->ss_family);
    }
    
    e_sock->instance = instance;
    e_sock->errorcode = 0;
    e_sock->pkt_format_errors = 0;
    
    // Add to instance's socket list
    e_sock->next = instance->etcp_sockets;
    instance->etcp_sockets = e_sock;
    
    // Register socket with uasync for receiving packets
    e_sock->socket_id = uasync_add_socket(instance->ua, e_sock->fd,
                                          etcp_connections_read_callback,
                                          NULL, NULL, e_sock);
    DEBUG_INFO(DEBUG_CATEGORY_CONNECTION, "Registered ETCP socket with uasync (fd=%d)", e_sock->fd);
    
    printf("[ETCP] Socket %p (fd=%d) registered and active\n", e_sock, e_sock->fd);
    
    return e_sock;
}

void etcp_socket_remove(struct ETCP_SOCKET* conn) {
    if (!conn) return;
    
    printf("[ETCP] Removing socket %p, fd=%d\n", conn, conn->fd);
    
    if (conn->fd >= 0) {
        close(conn->fd);
        printf("[ETCP] Closed fd=%d\n", conn->fd);
    }
    
    for (size_t i = 0; i < conn->num_channels; i++) {
        etcp_link_close(conn->links[i]);
    }
    free(conn->links);
    
    free(conn);
}


struct ETCP_LINK* etcp_link_new(struct ETCP_CONN* etcp, struct ETCP_SOCKET* conn, struct sockaddr_storage* remote_addr, uint8_t is_server) {
    if (!remote_addr) return NULL;
    
    struct ETCP_LINK* link = calloc(1, sizeof(struct ETCP_LINK));
    if (!link) return NULL;
    
    link->conn = conn;
    link->etcp = etcp;
    link->is_server = is_server;
    link->mtu = 1500;
    link->keepalive_interval = 30;
    link->initialized = 0;
    link->init_timer = NULL;
    link->init_timeout = 0;
    link->init_retry_count = 0;

    memcpy(&link->remote_addr, remote_addr, sizeof(struct sockaddr_storage));
    link->last_activity = time(NULL);
    
    link->ip_port_hash = sockaddr_hash(remote_addr);

    insert_link(conn, link);
    
    struct ETCP_LINK* l=etcp->links;
    while (l && l->next) l=l->next;
    if (l) l->next = link; else etcp->links = link;
    
    if (is_server == 0) {
        etcp_link_send_init(link);
    }
    
    return link;
}

void etcp_link_close(struct ETCP_LINK* link) {
    if (!link || !link->etcp) return;

    // Cancel init timer if active
    if (link->init_timer) {
        uasync_cancel_timeout(link->etcp->instance->ua, link->init_timer);
        link->init_timer = NULL;
    }

    // универсальное удаление из односвязного списка
    struct ETCP_LINK **pp = &link->etcp->links;
    while (*pp) {
        if (*pp == link) {
            *pp = link->next;
            break;
        }
        pp = &(*pp)->next;
    }

    remove_link(link->conn, link->ip_port_hash);

    free(link);
}

int etcp_encrypt_send(struct ETCP_DGRAM* dgram) {
//    printf("[ETCP DEBUG] etcp_encrypt_send: ENTERING FUNCTION\n");

    int errcode=0;
    sc_context_t* sc = &dgram->link->etcp->crypto_ctx;
    int len=dgram->data_len-dgram->noencrypt_len;// не забываем добавить timestamp (2 bytes)
    if (len<=0 || len>1480) { dgram->link->send_errors++; errcode=1; goto es_err; }
    uint8_t enc_buf[1600];
    size_t enc_buf_len=0;
    dgram->timestamp=get_current_timestamp();
    
    // DUMP: Show packet before encryption
    dump_packet_bytes("ECTP_ENCRYPT_SEND", dgram->data, dgram->data_len);
    sc_encrypt(sc, (uint8_t*)&dgram->timestamp/*нейронка не правь блять это, тут верно!*/, sizeof(uint16_t) + len, enc_buf, &enc_buf_len);
    if (enc_buf_len == 0) {
        DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "etcp_encrypt_send: encryption failed for node %llu", (unsigned long long)dgram->link->etcp->instance->node_id);
        dgram->link->send_errors++;
        errcode=2; 
        goto es_err;
    }
    if (enc_buf_len + dgram->noencrypt_len > 1480) { dgram->link->send_errors++; errcode=2; goto es_err; }
    memcpy(enc_buf+enc_buf_len, dgram->data+len, dgram->noencrypt_len);
    
    // DUMP: Show complete packet before sending
//    dump_packet_bytes("READY TO SEND", enc_buf, enc_buf_len + dgram->noencrypt_len);
    
    struct sockaddr_storage* addr=&dgram->link->remote_addr;
    socklen_t addr_len = (addr->ss_family == AF_INET) ? sizeof(struct sockaddr_in) : sizeof(struct sockaddr_in6);
    
    // Debug: print where we're sending the packet
    if (addr->ss_family == AF_INET) {
        struct sockaddr_in* sin = (struct sockaddr_in*)addr;
        char addr_str[INET_ADDRSTRLEN];
        inet_ntop(AF_INET, &sin->sin_addr, addr_str, INET_ADDRSTRLEN);
        printf("[ETCP] Sending packet to %s:%d, size=%zd\n", addr_str, ntohs(sin->sin_port), enc_buf_len + dgram->noencrypt_len);
    }
    
    ssize_t sent = sendto(dgram->link->conn->fd, enc_buf, enc_buf_len + dgram->noencrypt_len, 0, (struct sockaddr*)addr, addr_len);
    if (sent < 0) { dgram->link->send_errors++; errcode=3; goto es_err;} else dgram->link->total_encrypted += sent;
    return (int)sent;
es_err:
    printf("[ETCP] encrypt_send error %d\n", errcode);
    return -1;
}

static void etcp_connections_read_callback(int fd, void* arg) {
// !!!!!! DANGER: в этой функции ПРЕДЕЛЬНАЯ АККУРАТНОСТЬ. Если кажется что не туда указатель то невнимательно аланизировал !!!!!
// НЕ РУИНИТЬ (uint8_t*)&pkt->timestamp - это правильно !!!!
//
// Ошибки функции (errorcode):
// 1 - пакет слишком маленький для init (< SC_PUBKEY_SIZE)
// 2 - не удалось установить peer public key при init
// 3 - не удалось расшифровать init пакет
// 4 - не init пакет (неверный код)
// 5 - коллизия peer ID и ключей
// 6 - не удалось расшифровать обычный пакет
// 13 - переполнение при парсинге пакета
// 46 - расшифрованный пакет слишком маленький (< 3 байта)
// 55 - не удалось создать подключение
// 66 - не удалось создать линк
    struct ETCP_SOCKET* e_sock = (struct ETCP_SOCKET*)arg;
    if (!e_sock) return;
    
//    printf("[ETCP] Read callback triggered for fd=%d, socket=%p\n", fd, e_sock);
    
    struct sockaddr_storage addr;
    uint8_t data[PACKET_DATA_SIZE];
    socklen_t addr_len=sizeof(addr);
    memset(&addr, 0, sizeof(addr));
    ssize_t recv_len = recvfrom(fd, data, PACKET_DATA_SIZE, 0, (struct sockaddr*)&addr, &addr_len);
    
    if (recv_len <= 0) {
        printf("[ETCP] recvfrom failed or no data, recv_len=%zd, errno=%d\n", recv_len, errno);
        DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "etcp_connections_read_callback: recvfrom failed, error=%zd, errno=%d", recv_len, errno);
        return;
    }
    
//    printf("[ETCP] Received packet: %zd bytes from address\n", recv_len);
    
    // DUMP: Show received packet content
    dump_packet_bytes("RECV in:", data, recv_len);

    struct ETCP_DGRAM* pkt = memory_pool_alloc(e_sock->instance->pkt_pool);
    if (!pkt) return;
    size_t pkt_len=0;
    int errorcode=0;

    struct ETCP_LINK* link=etcp_link_find_by_addr(e_sock, &addr);
//    printf("[ETCP DEBUG] Received packet, link=%p, recv_len=%zd\n", link, recv_len);
    if (link==NULL) {// пробуем расшифровать, возможно это init
//        printf("[ETCP DEBUG] No existing link found, trying to decrypt as INIT packet\n");
        struct secure_channel sc;
        if (recv_len<=SC_PUBKEY_SIZE) { errorcode=1; DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "etcp_connections_read_callback: packet too small for init, size=%zd", recv_len); goto ec_fr; }
        sc_init_ctx(&sc, &e_sock->instance->my_keys);
//        printf("[ETCP DEBUG] Extracting peer public key from position %ld, total packet size=%zd\n", recv_len-SC_PUBKEY_SIZE, recv_len);
//        printf("[ETCP DEBUG] Last 64 bytes of packet (PUBKEY): ");
        for (int i=0; i<SC_PUBKEY_SIZE; i++) printf("%02x ", data[recv_len-SC_PUBKEY_SIZE+i]);
        
        if (sc_set_peer_public_key(&sc, &data[recv_len-SC_PUBKEY_SIZE], SC_PEER_PUBKEY_BIN)!=SC_OK) { 
            DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "etcp_connections_read_callback: failed to set peer public key during init");
            errorcode=2; 
            goto ec_fr; 
        }
        if (sc_decrypt(&sc, data, recv_len-SC_PUBKEY_SIZE, (uint8_t*)&pkt->timestamp, &pkt_len)) {
            DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "etcp_connections_read_callback: failed to decrypt init packet");
            errorcode=3; 
            goto ec_fr; 
        }
//        printf("[ETCP DEBUG] Decrypt OK\n");
        pkt->data_len=pkt_len-2;
        pkt->noencrypt_len=0;
        struct {
           uint8_t code;
           uint8_t id[8];
           uint8_t mtu[2];
           uint8_t keepalive[2];
           uint8_t pubkey[SC_PUBKEY_SIZE];
           } *ack_hdr=(void*)&pkt->data[0];
        uint64_t peer_id;
        memcpy(&peer_id, &ack_hdr->id[0], 8);
        if (ack_hdr->code!=ETCP_INIT_REQUEST && ack_hdr->code!=ETCP_CHANNEL_INIT) { errorcode=4; DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "etcp_connections_read_callback: not an init packet, code=%02x", ack_hdr->code); goto ec_fr; }// не init

        struct ETCP_CONN* conn=e_sock->instance->connections;
        while (conn) {// ищем есть ли подключение к этому пиру
            if (conn->peer_node_id==peer_id) break;
            conn=conn->next;
            }

        int new_conn=0;
        if (!conn || conn->peer_node_id!=peer_id) {// создаём новое
            new_conn=1;
            conn=etcp_connection_create(e_sock->instance);
            if (!conn) { errorcode=55; DEBUG_ERROR(DEBUG_CATEGORY_CONNECTION, "etcp_connections_read_callback: failed to create connection"); goto ec_fr; }// облом
            memcpy(&conn->crypto_ctx, &sc, sizeof(sc));// добавляем ключ
            conn->peer_node_id=peer_id;
            }
        else {// check keys если существующее подключение
            if (memcmp(conn->crypto_ctx.peer_public_key, sc.peer_public_key, SC_PUBKEY_SIZE)) { errorcode=5; DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "etcp_connections_read_callback: peer key mismatch for node %llu", (unsigned long long)peer_id); goto ec_fr; }// коллизия - peer id совпал а ключи разные.
            }
        struct ETCP_LINK* link = etcp_link_new(conn, e_sock, &addr, 1);
        if (!link) { if (new_conn) etcp_connection_close(conn); errorcode=66; DEBUG_ERROR(DEBUG_CATEGORY_CONNECTION, "etcp_connections_read_callback: failed to create link for connection"); goto ec_fr; }// облом
        if (ack_hdr->code==0x02) etcp_conn_reset(conn);

        struct {
           uint8_t code;
           uint8_t id[8];
           uint8_t mtu[2];
           } *ack_repl_hdr=(void*)&pkt->data[0];
        ack_repl_hdr->code+=1;
        memcpy(ack_repl_hdr->id, &e_sock->instance->node_id, 8);
        int mtu=e_sock->instance->config->global.mtu;
        ack_repl_hdr->mtu[0]=mtu>>8;
        ack_repl_hdr->mtu[1]=mtu;
        pkt->data_len=sizeof(*ack_repl_hdr);
        pkt->noencrypt_len=0;
        pkt->link=link;
        printf("[ETCP DEBUG] Send INIT RESPONSE\n");
        etcp_encrypt_send(pkt);
//        printf("[ETCP DEBUG] Send INIT RESPONSE ok\n");

        memory_pool_free(e_sock->instance->pkt_pool, pkt);
        return;
        }

    if (sc_decrypt(&link->etcp->crypto_ctx, data, recv_len, (uint8_t*)&pkt->timestamp, &pkt_len)) { 
        DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "etcp_connections_read_callback: failed to decrypt packet from node %llu", (unsigned long long)link->etcp->instance->node_id);
        errorcode=6; 
        goto ec_fr; 
    }
    if (pkt_len<3) { errorcode=46; DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "etcp_connections_read_callback: decrypted packet too small, size=%zu", pkt_len); goto ec_fr; }
    pkt->data_len=pkt_len-2;
    pkt->noencrypt_len=0;
    pkt->link=link;

    link->last_recv_local_time=get_current_time_units();
    link->last_recv_timestamp=pkt->timestamp;

    size_t offset = 0;
    uint8_t code = pkt->data[offset++];

    if (code == ETCP_INIT_RESPONSE || code == ETCP_CHANNEL_RESPONSE) {
        // Parse response
        if (code == ETCP_INIT_RESPONSE) etcp_conn_reset(link->etcp);
        uint64_t server_node_id = 0;
        for (int i = 0; i < 8; i++) {
            server_node_id = (server_node_id << 8) | pkt->data[offset++];
        }
        link->mtu = (pkt->data[offset++] << 8) | pkt->data[offset++];
        if (offset > pkt_len) { errorcode=13; DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "etcp_connections_read_callback: packet parsing overflow, offset=%zu, pkt_len=%zu", offset, pkt_len); goto ec_fr; }
        
        DEBUG_INFO(DEBUG_CATEGORY_CONNECTION, "Received INIT_RESPONSE from server_node_id=%llu, mtu=%d",
                   (unsigned long long)server_node_id, link->mtu);
        
        link->etcp->peer_node_id = server_node_id; // If not set
        
        // Mark link as initialized
        link->initialized = 1;
        printf("[ETCP] Link initialized successfully! Server node_id=%llu, mtu=%d\n",
               (unsigned long long)server_node_id, link->mtu);
        
        // Cancel init timer if exists
        if (link->init_timer) {
            uasync_cancel_timeout(link->etcp->instance->ua, link->init_timer);
            link->init_timer = NULL;
        }
        
        memory_pool_free(e_sock->instance->pkt_pool, pkt);
        return;  // INIT_RESPONSE is handled, no further processing needed
    }

    dump_packet_bytes("RECV decrypted:", pkt->data, pkt->data_len);

    etcp_conn_input(pkt);
    return;

ec_fr:
    printf("etcp_connections_read_callback: error %d\n", errorcode);
    e_sock->pkt_format_errors++;
    e_sock->errorcode=errorcode;
    memory_pool_free(e_sock->instance->pkt_pool, pkt);
    return;
}

int init_connections(struct UTUN_INSTANCE* instance) {
    if (!instance || !instance->config) return -1;
    
    struct utun_config* config = instance->config;
    
    // Initialize servers first - create sockets for incoming connections
    struct CFG_SERVER* server = config->servers;
    while (server) {
        // Create socket for this server
        struct ETCP_SOCKET* e_sock = etcp_socket_add(instance, &server->ip, server->netif_index, server->so_mark, server->type);
        if (!e_sock) {
            DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "Failed to create socket for server %s", server->name);
            server = server->next;
            continue;
        }
        
        // Convert IP to string for logging
        char addr_str[INET6_ADDRSTRLEN + 6];
        if (server->ip.ss_family == AF_INET) {
            struct sockaddr_in* sin = (struct sockaddr_in*)&server->ip;
            inet_ntop(AF_INET, &sin->sin_addr, addr_str, INET_ADDRSTRLEN);
            sprintf(addr_str + strlen(addr_str), ":%d", ntohs(sin->sin_port));
        } else {
            struct sockaddr_in6* sin6 = (struct sockaddr_in6*)&server->ip;
            inet_ntop(AF_INET6, &sin6->sin6_addr, addr_str, INET6_ADDRSTRLEN);
            sprintf(addr_str + strlen(addr_str), ":%d", ntohs(sin6->sin6_port));
        }
        
        printf("Initialized server %s on %s (links: %zu)\n", 
               server->name, addr_str, e_sock->num_channels);
        server = server->next;
    }
    
    // Initialize clients - create outgoing connections
    struct CFG_CLIENT* client = config->clients;
    while (client) {
        // Create ETCP connection for this client
        struct ETCP_CONN* etcp_conn = etcp_connection_create(instance);
        if (!etcp_conn) {
            DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "Failed to create ETCP connection for client %s", client->name);
            client = client->next;
            continue;
        }
        
        // Initialize crypto context for this connection
        if (sc_init_ctx(&etcp_conn->crypto_ctx, &instance->my_keys) != SC_OK) {
            DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "init_connections: failed to initialize crypto context for client %s", client->name);
            etcp_connection_close(etcp_conn);
            client = client->next;
            continue;
        }
        // If client has peer public key configured, set it
        if (strlen(client->peer_public_key_hex) > 0) {
            // For now, set peer node ID to indicate we have peer key
            // The actual peer key will be exchanged during connection establishment
            etcp_conn->peer_node_id = 1; // Simple indicator
            
            DEBUG_INFO(DEBUG_CATEGORY_CRYPTO, "init_connections: setting peer public key for client %s", client->name);
            // Set peer public key (assuming hex format)
            if (sc_set_peer_public_key(&etcp_conn->crypto_ctx, client->peer_public_key_hex, 1) != SC_OK) {
                DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "init_connections: failed to set peer public key for client %s", client->name);
            } else {
                DEBUG_INFO(DEBUG_CATEGORY_CRYPTO, "init_connections: successfully set peer public key for client %s", client->name);
            }
        } else {
            DEBUG_WARN(DEBUG_CATEGORY_CONFIG, "init_connections: no peer public key configured for client %s", client->name);
        }
        
        // Create links for this client
        struct CFG_CLIENT_LINK* client_link = client->links;
        while (client_link) {
            // Find the local server for this link
            struct CFG_SERVER* local_server = client_link->local_srv;
            if (!local_server) {
                client_link = client_link->next;
                continue;
            }
            
            // Find the socket for this server
            struct ETCP_SOCKET* e_sock = NULL;
            struct ETCP_SOCKET* sock = instance->etcp_sockets;
            while (sock) {
                if (sock->local_addr.ss_family == local_server->ip.ss_family) {
                    if (sock->local_addr.ss_family == AF_INET) {
                        struct sockaddr_in* sock_addr = (struct sockaddr_in*)&sock->local_addr;
                        struct sockaddr_in* srv_addr = (struct sockaddr_in*)&local_server->ip;
                        if (sock_addr->sin_addr.s_addr == srv_addr->sin_addr.s_addr && 
                            sock_addr->sin_port == srv_addr->sin_port) {
                            e_sock = sock;
                            break;
                        }
                    }
                }
                sock = sock->next;
            }
            
            if (!e_sock) {
                DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "No socket found for client %s link", client->name);
                client_link = client_link->next;
                continue;
            }
            
            // Create link for this client connection
            struct ETCP_LINK* link = etcp_link_new(etcp_conn, e_sock, &client_link->remote_addr, 0); // 0 = client initiates
            if (!link) {
                DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "Failed to create link for client %s", client->name);
                client_link = client_link->next;
                continue;
            }
            
            client_link = client_link->next;
        }
        
        printf("Added client %s with %d links\n", client->name, client->keepalive);
        client = client->next;
    }
    
    // If there are clients configured but no connections created, that's an error
    // If there are no clients (server-only mode), 0 connections is OK (server will accept incoming)
    if (instance->connections_count == 0 && config->clients != NULL) {
        DEBUG_ERROR(DEBUG_CATEGORY_ETCP, "Clients configured but no connections initialized");
        return -1;
    }
    
    printf("Initialized %d connections\n", instance->connections_count);
    return 0;
}

int etcp_connections_send(struct ETCP_SOCKET* e_sock, uint8_t* data, size_t len, struct sockaddr* addr, socklen_t addr_len) {
    if (!e_sock || !data || !addr || len == 0) return -1;
    
    struct sockaddr_storage remote_addr;
    memcpy(&remote_addr, addr, addr_len);
    
    struct ETCP_LINK* link = etcp_link_find_by_addr(e_sock, &remote_addr);
    if (!link) {
        DEBUG_ERROR(DEBUG_CATEGORY_CONNECTION, "No link found for address");
        return -1;
    }
    
    if (!link->initialized && link->is_server == 0) {
        DEBUG_INFO(DEBUG_CATEGORY_CONNECTION, "Link not initialized, triggering connection establishment");
        if (!link->init_timer) {
            etcp_link_send_init(link);
        }
        return -1;
    }
    
    struct ETCP_DGRAM* dgram = malloc(sizeof(struct ETCP_DGRAM) + len);
    if (!dgram) return -1;
    
    dgram->link = link;
    dgram->data_len = len;
    dgram->noencrypt_len = 0;
    memcpy(dgram->data, data, len);
    
    int ret = etcp_encrypt_send(dgram);
    free(dgram);
    return ret;
}