#include <stdlib.h>
#include <string.h>
#ifdef _WIN32
#include <winsock2.h>
#include <ws2tcpip.h>
#else
#include <arpa/inet.h>
#endif
#include "../lib/platform_compat.h"
#include "../lib/debug_config.h"
#include "../lib/mem.h"
#include "../lib/u_async.h"
#include "../lib/ll_queue.h"
#include "utun_instance.h"
#include "etcp.h"
#include "etcp_connections.h"
#include "etcp_router.h"
#include "config_parser.h"
#include "secure_channel.h"
#include "route_node.h"
#include "route_bgp.h"
#include "route_ping.h"
#include "route_connectivity.h"
#include "conn_mgr.h"

static struct CONN_MGR_ENTRY* cm_find_entry(struct CONN_MGR* mgr, uint64_t node_id);
static void cm_start_phase_direct(struct CONN_MGR_ENTRY* entry);
static void cm_start_phase_reverse(struct CONN_MGR_ENTRY* entry);
static int  cm_start_phase_indirect(struct CONN_MGR_ENTRY* entry);
static void cm_start_local_scan(struct CONN_MGR_ENTRY* entry);
static void cm_idle_timer_cb(void* arg);
static void cm_deliver_result(struct CONN_MGR_ENTRY* entry, int result);
static void cm_bg_ping_timer_cb(void* arg);
static int  cm_nat_compatible(struct ETCP_SOCKET* our, uint8_t target_config_type, uint8_t target_nat_type);
static uint16_t cm_get_node_min_rtt(struct NODEINFO_Q* nq);
static int  cm_has_direct_ip(struct NODEINFO_Q* nq);
static int  cm_has_local_addr(struct NODEINFO_Q* nq);
static void cm_entry_destroy(struct CONN_MGR_ENTRY* entry);
static void cm_exchange_probe_retry_cb(void* arg);
static uint64_t cm_get_node_max_probe_time(struct NODEINFO_Q* nq);
static int  cm_is_rtt_fresh(struct NODEINFO_Q* nq, uint64_t now_tb);

static void conn_mgr_router_recv_handler(struct ETCP_CONN* conn, struct ll_entry* entry);
static void cm_handle_direct_req(struct ETCP_CONN* conn, const uint8_t* data, size_t len);
static void cm_handle_interm_exchange_req(struct ETCP_CONN* conn, struct CONN_MGR_INTERM_EXCHANGE_REQ* req);
static void cm_handle_interm_exchange_resp(struct CONN_MGR* mgr, const uint8_t* data, size_t len);
static void cm_handle_interm_selected(struct CONN_MGR* mgr, const uint8_t* data, size_t len);
static void cm_handle_disconnect(struct CONN_MGR* mgr, uint64_t node_id);
static void cm_direct_ready_cb(struct ETCP_CONN* conn, void* arg);
static void cm_reverse_ready_cb(struct ETCP_CONN* conn, void* arg);
static void cm_reverse_timeout_cb(void* arg);
static void cm_exchange_timeout_cb(void* arg);
static void cm_candidate_ping_timer_cb(void* arg);

struct cm_reverse_pending {
    struct cm_reverse_pending* next;
    uint32_t request_id;
    struct CONN_MGR_ENTRY* entry;
    struct ETCP_CONN** conns;
    uint8_t conn_count;
};

struct cm_exchange_pending {
    struct cm_exchange_pending* next;
    uint32_t request_id;
    struct CONN_MGR_ENTRY* entry;
    void*    timeout_timer;
    struct CONN_MGR_INTERM_EXCHANGE_RESP cached_resp;
    uint8_t  resp_received;
    uint8_t  probes_done;
};

struct CONN_MGR* conn_mgr_init(struct UTUN_INSTANCE* instance) {
    if (!instance) { DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "conn_mgr_init: NULL instance"); return NULL; }
    struct CONN_MGR* mgr = u_calloc(1, sizeof(struct CONN_MGR));
    if (!mgr) { DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "conn_mgr_init: alloc failed"); return NULL; }
    mgr->instance = instance;
    mgr->entry_capacity = 8;
    mgr->entries = u_calloc(mgr->entry_capacity, sizeof(struct CONN_MGR_ENTRY));
    if (!mgr->entries) { DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "conn_mgr_init: entries alloc failed"); u_free(mgr); return NULL; }
    etcp_router_bind(instance, ETCP_RT_ID_CONN_MGR, conn_mgr_router_recv_handler);
    mgr->bg_ping_timer = uasync_set_timeout(instance->ua, CONN_MGR_BG_PING_INTERVAL_TB, mgr, cm_bg_ping_timer_cb, "conn_mgr_bg_ping");
    mgr->candidate_ping_timer = uasync_set_timeout(instance->ua, CONN_MGR_CANDIDATE_PING_TB, mgr, cm_candidate_ping_timer_cb, "conn_mgr_cand_ping");
    mgr->bg_ping_cycle_start_tb = get_time_tb();
    mgr->initialized = 1;
    DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "conn_mgr: initialized, bg_ping started");
    return mgr;
}

void conn_mgr_destroy(struct CONN_MGR* mgr) {
    if (!mgr) return;
    if (mgr->bg_ping_timer) { uasync_cancel_timeout(mgr->instance->ua, mgr->bg_ping_timer); mgr->bg_ping_timer = NULL; }
    if (mgr->candidate_ping_timer) { uasync_cancel_timeout(mgr->instance->ua, mgr->candidate_ping_timer); mgr->candidate_ping_timer = NULL; }
    etcp_router_unbind(mgr->instance, ETCP_RT_ID_CONN_MGR);
    for (size_t i = 0; i < mgr->entry_count; i++) cm_entry_destroy(&mgr->entries[i]);
    struct cm_exchange_pending* ep = mgr->exchange_pending;
    while (ep) {
        struct cm_exchange_pending* next = ep->next;
        if (ep->timeout_timer) uasync_cancel_timeout(mgr->instance->ua, ep->timeout_timer);
        u_free(ep);
        ep = next;
    }
    mgr->exchange_pending = NULL;
    u_free(mgr->entries);
    mgr->initialized = 0;
    DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "conn_mgr: destroyed, entries=%zu", (size_t)mgr->entry_count);
    u_free(mgr);
}

static void cm_entry_destroy(struct CONN_MGR_ENTRY* entry) {
    if (!entry || entry->state == CONN_MGR_STATE_DISCONNECTED) return;
    if (entry->idle_timer) { uasync_cancel_timeout(entry->mgr->instance->ua, entry->idle_timer); entry->idle_timer = NULL; }
    if (entry->main.timer) { uasync_cancel_timeout(entry->mgr->instance->ua, entry->main.timer); entry->main.timer = NULL; }
    struct cm_exchange_pending** pp = &entry->mgr->exchange_pending;
    while (*pp) {
        if ((*pp)->entry == entry) {
            if ((*pp)->timeout_timer && (*pp)->timeout_timer != entry->main.timer) {
                uasync_cancel_timeout(entry->mgr->instance->ua, (*pp)->timeout_timer);
                (*pp)->timeout_timer = NULL;
            }
            struct cm_exchange_pending* ep = *pp; *pp = ep->next; u_free(ep);
        } else pp = &(*pp)->next;
    }
    entry->state = CONN_MGR_STATE_DISCONNECTED;
    entry->conn_type = CONN_TYPE_NONE;
}

static struct CONN_MGR_ENTRY* cm_ensure_entry(struct CONN_MGR* mgr, uint64_t node_id) {
    struct CONN_MGR_ENTRY* e = cm_find_entry(mgr, node_id);
    if (e) return e;
    if (mgr->entry_count >= mgr->entry_capacity) {
        size_t new_cap = mgr->entry_capacity * 2;
        struct CONN_MGR_ENTRY* new_e = u_realloc(mgr->entries, new_cap * sizeof(struct CONN_MGR_ENTRY));
        if (!new_e) { DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "conn_mgr: realloc entries failed"); return NULL; }
        memset(new_e + mgr->entry_capacity, 0, (new_cap - mgr->entry_capacity) * sizeof(struct CONN_MGR_ENTRY));
        mgr->entries = new_e;
        mgr->entry_capacity = new_cap;
    }
    struct CONN_MGR_ENTRY* new_entry = &mgr->entries[mgr->entry_count++];
    memset(new_entry, 0, sizeof(*new_entry));
    new_entry->node_id = node_id;
    new_entry->mgr = mgr;
    return new_entry;
}

static struct CONN_MGR_ENTRY* cm_find_entry(struct CONN_MGR* mgr, uint64_t node_id) {
    for (size_t i = 0; i < mgr->entry_count; i++)
        if (mgr->entries[i].node_id == node_id && mgr->entries[i].state != CONN_MGR_STATE_DISCONNECTED)
            return &mgr->entries[i];
    return NULL;
}

static void cm_deliver_result(struct CONN_MGR_ENTRY* entry, int result) {
    struct cm_cb_node* node = entry->cb_list;
    while (node) { struct cm_cb_node* next = node->next; node->cb(result, entry->node_id, node->arg); u_free(node); node = next; }
    entry->cb_list = NULL;
}

static void cm_update_nodeinfo(struct CONN_MGR_ENTRY* entry) {
    struct ROUTE_BGP* bgp = entry->mgr->instance->bgp;
    if (!bgp) return;
    struct NODEINFO_Q* nq = nodeinfo_find_by_id(bgp, entry->node_id);
    if (!nq) return;
    nq->conn_mgr_type = entry->conn_type;
    memcpy(nq->conn_mgr_intermediaries, entry->intermediaries, sizeof(entry->intermediaries));
    nq->conn_mgr_intermediariy_count = entry->intermediariy_count;
}

static void cm_clear_nodeinfo(struct CONN_MGR* mgr, uint64_t node_id) {
    struct NODEINFO_Q* nq = nodeinfo_find_by_id(mgr->instance->bgp, node_id);
    if (!nq) return;
    nq->conn_mgr_type = CONN_TYPE_NONE;
    nq->conn_mgr_intermediariy_count = 0;
}

int conn_mgr_connect_node(struct CONN_MGR* mgr, uint64_t node_id, uint32_t idle_timeout_ms,
                          conn_mgr_connect_callback_t cb, void* cb_arg) {
    if (!mgr) return CONN_MGR_ERR_INTERNAL;
    struct ROUTE_BGP* bgp = mgr->instance->bgp;
    if (!bgp) { DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "conn_mgr_connect: BGP not initialized"); return CONN_MGR_ERR_INTERNAL; }
    struct NODEINFO_Q* target = nodeinfo_find_by_id(bgp, node_id);
    if (!target) { DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "conn_mgr_connect: node 0x%016llx not found", (unsigned long long)node_id); return CONN_MGR_ERR_NOT_FOUND; }
    struct CONN_MGR_ENTRY* entry = cm_find_entry(mgr, node_id);
    if (entry && entry->state == CONN_MGR_STATE_CONNECTED) {
        DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "conn_mgr_connect: node 0x%016llx already connected", (unsigned long long)node_id);
        return CONN_MGR_ERR_ALREADY_CONNECTED;
    }
    if (entry && entry->state == CONN_MGR_STATE_CONNECTING) {
        DEBUG_WARN(DEBUG_CATEGORY_GENERAL, "conn_mgr_connect: node 0x%016llx already connecting, adding cb", (unsigned long long)node_id);
        struct cm_cb_node* cn = u_calloc(1, sizeof(struct cm_cb_node));
        if (cn) { cn->cb = cb; cn->arg = cb_arg; cn->next = entry->cb_list; entry->cb_list = cn; }
        return CONN_MGR_OK;
    }
    struct ETCP_CONN* existing = route_bgp_find_conn_for_node(bgp, node_id);
    if (existing && existing->peer_node_id == node_id && existing->links) {
        struct ETCP_LINK* l = existing->links;
        while (l) { if (l->link_status && l->initialized) break; l = l->next; }
        if (l) {
            entry = cm_ensure_entry(mgr, node_id);
            if (!entry) return CONN_MGR_ERR_INTERNAL;
            entry->state = CONN_MGR_STATE_CONNECTED;
            entry->conn_type = CONN_TYPE_DIRECT;
            entry->idle_timeout_ms = idle_timeout_ms;
            entry->alien = target->alien;
            entry->last_traffic_tb = get_time_tb();
            { struct cm_cb_node* cn = u_calloc(1, sizeof(struct cm_cb_node));
              if (cn) { cn->cb = cb; cn->arg = cb_arg; entry->cb_list = cn; } }
    cm_update_nodeinfo(entry);
            cm_deliver_result(entry, CONN_MGR_OK);
            return CONN_MGR_OK;
        }
    }
    entry = cm_ensure_entry(mgr, node_id);
    if (!entry) return CONN_MGR_ERR_INTERNAL;
    entry->state = CONN_MGR_STATE_CONNECTING;
    entry->alien = target->alien;
    { struct cm_cb_node* cn = u_calloc(1, sizeof(struct cm_cb_node));
      if (cn) { cn->cb = cb; cn->arg = cb_arg; cn->next = entry->cb_list; entry->cb_list = cn; } }
    entry->idle_timeout_ms = idle_timeout_ms;
    entry->last_traffic_tb = get_time_tb();
    entry->local_scan_state = CM_TRY_NONE;
    entry->main_connect_state = CM_TRY_NONE;
    entry->main.phase = 0;
    entry->main.timer = NULL;
    entry->main.request_id = 0;
    int has_our_local = cm_has_local_addr(bgp->local_node);
    int has_target_local = cm_has_local_addr(target);
    if (has_our_local && has_target_local) { entry->local_scan_state = CM_TRY_PENDING; cm_start_local_scan(entry); }
    entry->main_connect_state = CM_TRY_PENDING;
    cm_start_phase_direct(entry);
    return CONN_MGR_OK;
}

int conn_mgr_disconnect_node(struct CONN_MGR* mgr, uint64_t node_id) {
    if (!mgr) return CONN_MGR_ERR_INTERNAL;
    struct CONN_MGR_ENTRY* entry = cm_find_entry(mgr, node_id);
    if (!entry) return CONN_MGR_ERR_NOT_FOUND;
    struct CONN_MGR_DISCONNECT pkt;
    pkt.cmd = ETCP_RT_ID_CONN_MGR;
    pkt.subcmd = CONN_MGR_SUBCMD_DISCONNECT;
    pkt.node_id = node_id;
    struct ll_entry* qe = queue_entry_new(sizeof(pkt));
    if (qe) { memcpy(qe->data, &pkt, sizeof(pkt)); etcp_route_send(mgr->instance, node_id, qe, 1); }
    cm_clear_nodeinfo(mgr, node_id);
    cm_entry_destroy(entry);
    entry->state = CONN_MGR_STATE_DISCONNECTED;
    entry->conn_type = CONN_TYPE_NONE;
    DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "conn_mgr: disconnected node 0x%016llx", (unsigned long long)node_id);
    return CONN_MGR_OK;
}

int conn_mgr_set_idle_timeout(struct CONN_MGR* mgr, uint64_t node_id, uint32_t timeout_ms) {
    if (!mgr) return CONN_MGR_ERR_INTERNAL;
    struct CONN_MGR_ENTRY* entry = cm_find_entry(mgr, node_id);
    if (!entry) return CONN_MGR_ERR_NOT_FOUND;
    entry->idle_timeout_ms = timeout_ms;
    if (entry->state == CONN_MGR_STATE_CONNECTED && timeout_ms > 0 && !entry->idle_timer)
        entry->idle_timer = uasync_set_timeout(mgr->instance->ua, CONN_MGR_IDLE_CHECK_INTERVAL_TB, entry, cm_idle_timer_cb, "conn_mgr_idle");
    if (timeout_ms == 0 && entry->idle_timer) { uasync_cancel_timeout(mgr->instance->ua, entry->idle_timer); entry->idle_timer = NULL; }
    return CONN_MGR_OK;
}

int conn_mgr_get_status(struct CONN_MGR* mgr, uint64_t node_id, uint8_t* out_state, uint8_t* out_conn_type) {
    if (!mgr || !out_state || !out_conn_type) return CONN_MGR_ERR_INTERNAL;
    struct CONN_MGR_ENTRY* entry = cm_find_entry(mgr, node_id);
    if (!entry) { *out_state = CONN_MGR_STATE_DISCONNECTED; *out_conn_type = CONN_TYPE_NONE; return CONN_MGR_OK; }
    *out_state = entry->state;
    *out_conn_type = entry->conn_type;
    return CONN_MGR_OK;
}

int conn_mgr_add_alien_node(struct CONN_MGR* mgr, const uint8_t* nodeinfo_data, size_t len) {
    if (!mgr || !nodeinfo_data || len < sizeof(struct NODEINFO)) return CONN_MGR_ERR_INTERNAL;
    struct ROUTE_BGP* bgp = mgr->instance->bgp;
    if (!bgp) return CONN_MGR_ERR_INTERNAL;
    const struct NODEINFO* ni = (const struct NODEINFO*)nodeinfo_data;
    uint64_t node_id = ni->node_id;
    struct NODEINFO_Q* existing = nodeinfo_find_by_id(bgp, node_id);
    if (existing) { DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "conn_mgr: alien node 0x%016llx already exists, ignoring", (unsigned long long)node_id); return CONN_MGR_OK; }
    size_t dyn_sz = len - sizeof(struct NODEINFO);
    size_t data_sz = sizeof(struct NODEINFO_Q) - sizeof(struct ll_entry) + dyn_sz + 8;
    struct NODEINFO_Q* nq = (struct NODEINFO_Q*)queue_entry_new(data_sz);
    if (!nq) { DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "conn_mgr: alien node alloc failed"); return CONN_MGR_ERR_INTERNAL; }
    memcpy(&nq->node, nodeinfo_data, len);
    nq->alien = 1;
    nq->dirty = 0;
    nq->last_ver = ni->ver;
    nq->connectivity.ping_req_time = 0;
    if (bgp->nodes) queue_data_put_with_index(bgp->nodes, &nq->ll);
    DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "conn_mgr: added alien node 0x%016llx", (unsigned long long)node_id);
    return CONN_MGR_OK;
}

int conn_mgr_send(struct CONN_MGR* mgr, uint64_t node_id, struct ll_entry* entry) {
    if (!mgr || !entry) return -1;
    struct CONN_MGR_ENTRY* e = cm_find_entry(mgr, node_id);
    if (!e || e->state != CONN_MGR_STATE_CONNECTED) { queue_entry_free(entry); return -1; }
    e->last_traffic_tb = get_time_tb();
    struct ETCP_ROUTER_CONN* rconn = etcp_router_conn_get(mgr->instance, node_id, ETCP_RT_ID_CONN_MGR);
    if (!rconn) { queue_entry_free(entry); return -1; }
    int ret = etcp_router_conn_send(rconn, entry->dgram, entry->len);
    queue_entry_free(entry);
    return ret;
}

void conn_mgr_update_best_candidates(struct CONN_MGR* mgr, uint64_t node_id, uint16_t rtt) {
    if (!mgr || node_id == mgr->instance->node_id || rtt == 0) return;
    for (uint8_t i = 0; i < mgr->best_candidate_count; i++) {
        if (mgr->best_candidates[i].node_id == node_id) {
            mgr->best_candidates[i].rtt = rtt;
            for (uint8_t j = i; j > 0 && mgr->best_candidates[j].rtt < mgr->best_candidates[j-1].rtt; j--) {
                struct CONN_MGR_CANDIDATE tmp = mgr->best_candidates[j];
                mgr->best_candidates[j] = mgr->best_candidates[j-1];
                mgr->best_candidates[j-1] = tmp;
            }
            for (uint8_t j = i; j + 1 < mgr->best_candidate_count && mgr->best_candidates[j].rtt > mgr->best_candidates[j+1].rtt; j++) {
                struct CONN_MGR_CANDIDATE tmp = mgr->best_candidates[j];
                mgr->best_candidates[j] = mgr->best_candidates[j+1];
                mgr->best_candidates[j+1] = tmp;
            }
            return;
        }
    }
    if (mgr->best_candidate_count < CONN_MGR_MAX_CANDIDATES) {
        mgr->best_candidates[mgr->best_candidate_count].node_id = node_id;
        mgr->best_candidates[mgr->best_candidate_count].rtt = rtt;
        mgr->best_candidate_count++;
    } else if (rtt < mgr->best_candidates[CONN_MGR_MAX_CANDIDATES - 1].rtt) {
        mgr->best_candidates[CONN_MGR_MAX_CANDIDATES - 1].node_id = node_id;
        mgr->best_candidates[CONN_MGR_MAX_CANDIDATES - 1].rtt = rtt;
    } else return;
    for (uint8_t i = mgr->best_candidate_count - 1; i > 0; i--) {
        if (mgr->best_candidates[i].rtt >= mgr->best_candidates[i-1].rtt) break;
        struct CONN_MGR_CANDIDATE tmp = mgr->best_candidates[i];
        mgr->best_candidates[i] = mgr->best_candidates[i-1];
        mgr->best_candidates[i-1] = tmp;
    }
}

static void cm_idle_timer_cb(void* arg) {
    struct CONN_MGR_ENTRY* entry = (struct CONN_MGR_ENTRY*)arg;
    struct CONN_MGR* mgr = entry->mgr;
    uint64_t now_tb = get_time_tb();
    if (entry->state != CONN_MGR_STATE_CONNECTED || entry->idle_timeout_ms == 0) return;
    uint64_t idle = now_tb - entry->last_traffic_tb;
    if (idle > (uint64_t)entry->idle_timeout_ms * 10) {
        DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "conn_mgr: node 0x%016llx idle %llums, disconnecting",
                   (unsigned long long)entry->node_id, (unsigned long long)(idle / 10));
        conn_mgr_disconnect_node(mgr, entry->node_id);
        return;
    }
    entry->idle_timer = uasync_set_timeout(mgr->instance->ua, CONN_MGR_IDLE_CHECK_INTERVAL_TB, entry, cm_idle_timer_cb, "conn_mgr_idle");
}

static int cm_nat_compatible(struct ETCP_SOCKET* our, uint8_t target_config_type, uint8_t target_nat_type) {
    uint8_t tt = target_nat_type >= NAT_VERIFIED_UNKNOWN ? CFG_SERVER_TYPE_PUBLIC : target_config_type;
    uint8_t ot = our->type;
    if (ot == CFG_SERVER_TYPE_PUBLIC || ot == CFG_SERVER_TYPE_UNKNOWN) return 1;
    if (ot == CFG_SERVER_TYPE_NAT && our->nat_type == NAT_VERIFIED_EIM && tt == CFG_SERVER_TYPE_PUBLIC) return 1;
    if (ot == CFG_SERVER_TYPE_NAT && our->nat_type == NAT_VERIFIED_EIM && tt == CFG_SERVER_TYPE_NAT) return 1;
    if ((ot == CFG_SERVER_TYPE_PRIVATE || ot == CFG_SERVER_TYPE_LOCAL) && tt == CFG_SERVER_TYPE_PRIVATE) return 2;
    return 0;
}

static uint16_t cm_get_node_min_rtt(struct NODEINFO_Q* nq) {
    uint16_t rtt = 0xFFFF;
    if (nq->connectivity.interface_status == PROBE_RESULT_REACHABLE && nq->connectivity.interface_min_rtt < rtt)
        rtt = nq->connectivity.interface_min_rtt;
    if (nq->connectivity.nat_status == PROBE_RESULT_REACHABLE && nq->connectivity.nat_min_rtt < rtt)
        rtt = nq->connectivity.nat_min_rtt;
    if (nq->connectivity.real_status == PROBE_RESULT_REACHABLE && nq->connectivity.real_min_rtt < rtt)
        rtt = nq->connectivity.real_min_rtt;
    return rtt;
}

static uint64_t cm_get_node_max_probe_time(struct NODEINFO_Q* nq) {
    uint64_t t = nq->connectivity.interface_probe_time;
    if (nq->connectivity.nat_probe_time > t) t = nq->connectivity.nat_probe_time;
    if (nq->connectivity.real_probe_time > t) t = nq->connectivity.real_probe_time;
    return t;
}

static int cm_is_rtt_fresh(struct NODEINFO_Q* nq, uint64_t now_tb) {
    uint64_t last = cm_get_node_max_probe_time(nq);
    return (last > 0 && (now_tb - last) < (uint64_t)(CONN_MGR_CANDIDATE_CACHE_MS * 10));
}

static int cm_has_direct_ip(struct NODEINFO_Q* nq) {
    if (!nq) return 0;
    const struct NODEINFO_IPV4_ADDR* addrs = NULL;
    const struct NODEINFO_IPV4_SOCKET_META* metas = NULL;
    int addr_count = get_node_v4_addrs(nq, &addrs);
    int meta_count = get_node_v4_sockets_meta(nq, &metas);
    for (int i = 0; i < addr_count; i++) {
        if (addrs[i].type == ADDR_TYPE_REAL || addrs[i].type == ADDR_TYPE_NAT) {
            for (int j = 0; j < meta_count; j++) {
                if (metas[j].id == addrs[i].socket_id &&
                    (metas[j].config_type == CFG_SERVER_TYPE_PUBLIC || metas[j].config_type == CFG_SERVER_TYPE_UNKNOWN
                     || metas[j].nat_type == NAT_VERIFIED_EIM || metas[j].nat_type == NAT_VERIFIED_DIRECT))
                    return 1;
            }
        }
    }
    return 0;
}

static int cm_has_local_addr(struct NODEINFO_Q* nq) {
    if (!nq) return 0;
    const struct NODEINFO_IPV4_ADDR* addrs = NULL;
    const struct NODEINFO_IPV4_SOCKET_META* metas = NULL;
    int addr_count = get_node_v4_addrs(nq, &addrs);
    int meta_count = get_node_v4_sockets_meta(nq, &metas);
    for (int i = 0; i < addr_count; i++) {
        if (addrs[i].type == ADDR_TYPE_INTERFACE) {
            for (int j = 0; j < meta_count; j++) {
                if (metas[j].id == addrs[i].socket_id &&
                    (metas[j].config_type == CFG_SERVER_TYPE_PRIVATE || metas[j].config_type == CFG_SERVER_TYPE_LOCAL))
                    return 1;
            }
        }
    }
    return 0;
}

static void cm_ping_cb(int success, uint16_t rtt, void* arg, uint64_t nonce,
                        const uint8_t* resp_data, size_t resp_data_len);
static void cm_direct_timeout_cb(void* arg);

struct cm_ping_ctx {
    struct CONN_MGR_ENTRY* entry;
    struct sockaddr_storage addr;
    struct ETCP_SOCKET* sock;
    uint8_t phase;            /* 0=local_scan, 1=direct */
    uint8_t attempt;
};

static void cm_start_local_scan(struct CONN_MGR_ENTRY* entry) {
    struct ROUTE_BGP* bgp = entry->mgr->instance->bgp;
    struct NODEINFO_Q* target = nodeinfo_find_by_id(bgp, entry->node_id);
    if (!target) { entry->local_scan_state = CM_TRY_FAILED; return; }
    const struct NODEINFO_IPV4_ADDR* addrs = NULL;
    const struct NODEINFO_IPV4_SOCKET_META* metas = NULL;
    int addr_count = get_node_v4_addrs(target, &addrs);
    int meta_count = get_node_v4_sockets_meta(target, &metas);
    for (int i = 0; i < addr_count; i++) {
        if (addrs[i].type != ADDR_TYPE_INTERFACE) continue;
        for (int j = 0; j < meta_count; j++) {
            if (metas[j].id != addrs[i].socket_id ||
                (metas[j].config_type != CFG_SERVER_TYPE_PRIVATE && metas[j].config_type != CFG_SERVER_TYPE_LOCAL)) continue;
            struct ETCP_SOCKET* s = entry->mgr->instance->etcp_sockets;
            while (s) {
                if ((s->type == CFG_SERVER_TYPE_PRIVATE || s->type == CFG_SERVER_TYPE_LOCAL) && s->local_addr.ss_family == AF_INET) {
                    struct sockaddr_in sin;
                    memset(&sin, 0, sizeof(sin));
                    sin.sin_family = AF_INET;
                    memcpy(&sin.sin_addr.s_addr, addrs[i].addr, 4);
                    sin.sin_port = htons(addrs[i].port);
                    struct sockaddr_storage sa;
                    memset(&sa, 0, sizeof(sa));
                    memcpy(&sa, &sin, sizeof(sin));
                    struct cm_ping_ctx* ctx = u_calloc(1, sizeof(struct cm_ping_ctx));
                    if (!ctx) continue;
                    ctx->entry = entry;
                    ctx->addr = sa;
                    ctx->sock = s;
                    ctx->phase = 0;
                    ctx->attempt = 0;
                    etcp_send_ping_to_socket(entry->mgr->instance, s, target->node.public_key, &sa,
                                             CONN_MGR_LOCAL_SCAN_TIMEOUT_MS, cm_ping_cb, ctx, NULL, 0);
                    return;
                }
                s = s->next;
            }
        }
    }
    entry->local_scan_state = CM_TRY_FAILED;
    DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "conn_mgr: local scan no candidates for 0x%016llx", (unsigned long long)entry->node_id);
}

static void cm_start_phase_direct(struct CONN_MGR_ENTRY* entry) {
    if (entry->main_connect_state == CM_TRY_OK) return;
    if (entry->local_scan_state == CM_TRY_OK) return;
    struct ROUTE_BGP* bgp = entry->mgr->instance->bgp;
    struct NODEINFO_Q* target = nodeinfo_find_by_id(bgp, entry->node_id);
    if (!target) { cm_deliver_result(entry, CONN_MGR_ERR_NOT_FOUND); return; }
    const struct NODEINFO_IPV4_ADDR* addrs = NULL;
    const struct NODEINFO_IPV4_SOCKET_META* metas = NULL;
    int addr_count = get_node_v4_addrs(target, &addrs);
    int meta_count = get_node_v4_sockets_meta(target, &metas);
    uint8_t priority_order[] = { ADDR_TYPE_REAL, ADDR_TYPE_NAT, ADDR_TYPE_INTERFACE };
    for (int pri = 0; pri < 3; pri++) {
        for (int i = 0; i < addr_count; i++) {
            if (addrs[i].type != priority_order[pri]) continue;
            for (int j = 0; j < meta_count; j++) {
                if (metas[j].id != addrs[i].socket_id) continue;
                struct ETCP_SOCKET* s = entry->mgr->instance->etcp_sockets;
                while (s) {
                    int compat = cm_nat_compatible(s, metas[j].config_type, metas[j].nat_type);
                    if (compat && s->local_addr.ss_family == AF_INET) {
                        struct sockaddr_in sin;
                        memset(&sin, 0, sizeof(sin));
                        sin.sin_family = AF_INET;
                        memcpy(&sin.sin_addr.s_addr, addrs[i].addr, 4);
                        sin.sin_port = htons(addrs[i].port);
                        struct sockaddr_storage sa;
                        memset(&sa, 0, sizeof(sa));
                        memcpy(&sa, &sin, sizeof(sin));
                        struct ETCP_CONN* conn = etcp_connection_create(entry->mgr->instance, NULL);
                        if (conn) {
                            etcp_conn_set_ready_cbk(conn, cm_direct_ready_cb, entry);
                            sc_set_peer_public_key(&conn->crypto_ctx, target->node.public_key, 0);
                            struct ETCP_LINK* link = etcp_link_new(conn, s, &sa, 0);
                            if (link) {
                                entry->main.timer = uasync_set_timeout(entry->mgr->instance->ua,
                                    CONN_MGR_CONNECT_DIRECT_TIMEOUT_MS * 10, entry, cm_direct_timeout_cb, "conn_mgr_direct");
                                DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "conn_mgr: phase 1 (direct) started INIT to 0x%016llx",
                                           (unsigned long long)entry->node_id);
                                return;
                            }
                        }
                    }
                    s = s->next;
                }
            }
        }
    }
    DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "conn_mgr: phase 1 (direct) no compatible pairs for 0x%016llx",
               (unsigned long long)entry->node_id);
    int has_our = cm_has_direct_ip(bgp->local_node);
    int has_target = cm_has_direct_ip(target);
    if (has_our && !has_target) { cm_start_phase_reverse(entry); return; }
    cm_start_phase_indirect(entry);
}

static void cm_direct_timeout_cb(void* arg) {
    struct CONN_MGR_ENTRY* entry = (struct CONN_MGR_ENTRY*)arg;
    entry->main.timer = NULL;
    struct ROUTE_BGP* bgp = entry->mgr->instance->bgp;
    struct NODEINFO_Q* target = nodeinfo_find_by_id(bgp, entry->node_id);
    int has_our = cm_has_direct_ip(bgp->local_node);
    int has_target = target ? cm_has_direct_ip(target) : 0;
    if (entry->local_scan_state != CM_TRY_OK) {
        if (has_our && !has_target) { cm_start_phase_reverse(entry); return; }
        cm_start_phase_indirect(entry);
        return;
    }
    if (entry->local_scan_state == CM_TRY_FAILED && entry->main_connect_state == CM_TRY_FAILED)
        cm_deliver_result(entry, CONN_MGR_ERR_UNREACHABLE);
}

static void cm_direct_ready_cb(struct ETCP_CONN* conn, void* arg) {
    struct CONN_MGR_ENTRY* entry = (struct CONN_MGR_ENTRY*)arg;
    if (!conn || conn->peer_node_id != entry->node_id) {
        DEBUG_WARN(DEBUG_CATEGORY_GENERAL, "conn_mgr: direct ready cb peer mismatch conn=%p peer=0x%llx entry=0x%llx",
                   (void*)conn, (unsigned long long)conn->peer_node_id, (unsigned long long)entry->node_id);
        return;
    }
    DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "conn_mgr: direct ready for 0x%016llx", (unsigned long long)entry->node_id);
    if (entry->main.timer) { uasync_cancel_timeout(entry->mgr->instance->ua, entry->main.timer); entry->main.timer = NULL; }
    entry->main_connect_state = CM_TRY_OK;
    entry->conn_type = CONN_TYPE_DIRECT;
    entry->state = CONN_MGR_STATE_CONNECTED;
    cm_update_nodeinfo(entry);
    cm_deliver_result(entry, CONN_MGR_OK);
}

static void cm_ping_cb(int success, uint16_t rtt, void* arg, uint64_t nonce,
                        const uint8_t* resp_data, size_t resp_data_len) {
    struct cm_ping_ctx* ctx = (struct cm_ping_ctx*)arg;
    struct CONN_MGR_ENTRY* entry = ctx->entry;
    (void)nonce; (void)resp_data; (void)resp_data_len;
    if (entry->main_connect_state == CM_TRY_OK) { u_free(ctx); return; }
    if (success && ctx->phase == 0) {
        entry->local_scan_state = CM_TRY_OK;
        entry->conn_type = CONN_TYPE_DIRECT;
        entry->state = CONN_MGR_STATE_CONNECTED;
        if (entry->main.timer) { uasync_cancel_timeout(entry->mgr->instance->ua, entry->main.timer); entry->main.timer = NULL; }
        DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "conn_mgr: local scan OK for 0x%016llx rtt=%u", (unsigned long long)entry->node_id, rtt);
    cm_update_nodeinfo(entry);
        cm_deliver_result(entry, CONN_MGR_OK);
        u_free(ctx);
        return;
    }
    ctx->attempt++;
    if (ctx->attempt < CONN_MGR_LOCAL_SCAN_ATTEMPTS && ctx->phase == 0) {
        etcp_send_ping_to_socket(entry->mgr->instance, ctx->sock,
                                  nodeinfo_find_by_id(entry->mgr->instance->bgp, entry->node_id)->node.public_key,
                                  &ctx->addr, CONN_MGR_LOCAL_SCAN_TIMEOUT_MS, cm_ping_cb, ctx, NULL, 0);
        return;
    }
    DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "conn_mgr: %s failed for 0x%016llx after %d attempts",
               ctx->phase == 0 ? "local scan" : "direct", (unsigned long long)entry->node_id, ctx->attempt);
    if (ctx->phase == 0) entry->local_scan_state = CM_TRY_FAILED;
    u_free(ctx);
}

static void cm_start_phase_reverse(struct CONN_MGR_ENTRY* entry) {
    struct ROUTE_BGP* bgp = entry->mgr->instance->bgp;
    struct NODEINFO_Q* local = bgp->local_node;
    if (!local) { cm_deliver_result(entry, CONN_MGR_ERR_INTERNAL); return; }
    uint32_t req_id = ++entry->mgr->next_request_id;
    entry->main.request_id = req_id;
    entry->main.phase = 2;
    const struct NODEINFO_IPV4_ADDR* addrs = NULL;
    const struct NODEINFO_IPV4_SOCKET_META* metas = NULL;
    int addr_count = get_node_v4_addrs(local, &addrs);
    int meta_count = get_node_v4_sockets_meta(local, &metas);
    uint8_t direct_count = 0;
    struct { uint8_t type; uint8_t ip[4]; uint16_t port; uint8_t socket_id; } out_addrs[8];
    for (int i = 0; i < addr_count && direct_count < 8; i++) {
        if (addrs[i].type != ADDR_TYPE_REAL && addrs[i].type != ADDR_TYPE_NAT) continue;
        for (int j = 0; j < meta_count; j++) {
            if (metas[j].id == addrs[i].socket_id &&
                (metas[j].config_type == CFG_SERVER_TYPE_PUBLIC || metas[j].config_type == CFG_SERVER_TYPE_UNKNOWN
                 || metas[j].nat_type == NAT_VERIFIED_EIM || metas[j].nat_type == NAT_VERIFIED_DIRECT)) {
                out_addrs[direct_count].type = addrs[i].type;
                memcpy(out_addrs[direct_count].ip, addrs[i].addr, 4);
                out_addrs[direct_count].port = htons(addrs[i].port);
                out_addrs[direct_count].socket_id = addrs[i].socket_id;
                direct_count++;
                break;
            }
        }
    }
    if (direct_count == 0) { cm_start_phase_indirect(entry); return; }
    size_t pkt_size = CONN_MGR_DIRECT_REQ_HDR_SIZE + (size_t)direct_count * 8;
    uint8_t* pkt = u_malloc(pkt_size);
    if (!pkt) { cm_deliver_result(entry, CONN_MGR_ERR_INTERNAL); return; }
    struct CONN_MGR_DIRECT_REQ* req = (struct CONN_MGR_DIRECT_REQ*)pkt;
    req->cmd = ETCP_RT_ID_CONN_MGR;
    req->subcmd = CONN_MGR_SUBCMD_DIRECT_REQ;
    req->request_id = req_id;
    req->addr_count = direct_count;
    uint8_t* p = pkt + CONN_MGR_DIRECT_REQ_HDR_SIZE;
    for (uint8_t i = 0; i < direct_count; i++) {
        *p++ = out_addrs[i].type;
        memcpy(p, out_addrs[i].ip, 4); p += 4;
        memcpy(p, &out_addrs[i].port, 2); p += 2;
        *p++ = out_addrs[i].socket_id;
    }
    struct ll_entry* qe = queue_entry_new(pkt_size);
    if (!qe) { u_free(pkt); cm_deliver_result(entry, CONN_MGR_ERR_INTERNAL); return; }
    memcpy(qe->data, pkt, pkt_size);
    qe->len = (uint16_t)pkt_size;
    u_free(pkt);
    etcp_route_send(entry->mgr->instance, entry->node_id, qe, 1);

    struct cm_reverse_pending* rp = u_calloc(1, sizeof(struct cm_reverse_pending));
    if (rp) { rp->request_id = req_id; rp->entry = entry; rp->next = entry->mgr->reverse_pending;
        entry->mgr->reverse_pending = rp; }
    entry->main.timer = uasync_set_timeout(entry->mgr->instance->ua, CONN_MGR_CONNECT_REVERSE_TIMEOUT_MS * 10,
                                            entry, cm_reverse_timeout_cb, "conn_mgr_reverse");
    DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "conn_mgr: phase 2 (reverse) sent DIRECT_REQ to 0x%016llx with %u addrs",
               (unsigned long long)entry->node_id, direct_count);
}

static void cm_reverse_timeout_cb(void* arg) {
    struct CONN_MGR_ENTRY* entry = (struct CONN_MGR_ENTRY*)arg;
    entry->main.timer = NULL;
    struct cm_reverse_pending** pp = &entry->mgr->reverse_pending;
    while (*pp) { if ((*pp)->entry == entry) { struct cm_reverse_pending* rp = *pp; *pp = rp->next; u_free(rp); break; } pp = &(*pp)->next; }
    cm_start_phase_indirect(entry);
}

static void cm_reverse_ready_cb(struct ETCP_CONN* conn, void* arg) {
    struct cm_reverse_pending* rp = (struct cm_reverse_pending*)arg;
    struct CONN_MGR_ENTRY* entry = rp->entry;
    if (!conn || conn->peer_node_id != entry->node_id) { u_free(rp); return; }
    DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "conn_mgr: reverse ready for 0x%016llx", (unsigned long long)entry->node_id);
    if (entry->main.timer) { uasync_cancel_timeout(entry->mgr->instance->ua, entry->main.timer); entry->main.timer = NULL; }
    struct cm_reverse_pending** pp = &entry->mgr->reverse_pending;
    while (*pp) { if (*pp == rp) { *pp = rp->next; break; } pp = &(*pp)->next; }
    entry->main_connect_state = CM_TRY_OK;
    entry->conn_type = CONN_TYPE_REVERSE;
    entry->state = CONN_MGR_STATE_CONNECTED;
    cm_update_nodeinfo(entry);
    cm_deliver_result(entry, CONN_MGR_OK);
    u_free(rp);
}

static int cm_start_phase_indirect(struct CONN_MGR_ENTRY* entry) {
    struct CONN_MGR* mgr = entry->mgr;
    if (mgr->best_candidate_count == 0) {
        DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "conn_mgr: phase 3 (indirect) no candidates for 0x%016llx",
                    (unsigned long long)entry->node_id);
        return CONN_MGR_ERR_UNREACHABLE;
    }
    uint32_t req_id = ++mgr->next_request_id;
    entry->main.request_id = req_id;
    entry->main.phase = 3;
    struct CONN_MGR_INTERM_EXCHANGE_REQ req;
    memset(&req, 0, sizeof(req));
    req.cmd = ETCP_RT_ID_CONN_MGR;
    req.subcmd = CONN_MGR_SUBCMD_INTERM_EXCHANGE_REQ;
    req.request_id = req_id;
    req.candidate_count = mgr->best_candidate_count > 4 ? 4 : mgr->best_candidate_count;
    for (uint8_t i = 0; i < req.candidate_count; i++)
        req.candidates[i] = mgr->best_candidates[i];
    struct ll_entry* qe = queue_entry_new(sizeof(req));
    if (!qe) return CONN_MGR_ERR_INTERNAL;
    memcpy(qe->data, &req, sizeof(req));
    etcp_route_send(entry->mgr->instance, entry->node_id, qe, 1);
    entry->main.timer = uasync_set_timeout(mgr->instance->ua, CONN_MGR_INTERM_EXCHANGE_TIMEOUT_MS * 10,
                                            entry, cm_exchange_timeout_cb, "conn_mgr_interm_exch");
    {
        struct cm_exchange_pending* ep = u_calloc(1, sizeof(struct cm_exchange_pending));
        if (ep) { ep->request_id = req_id; ep->entry = entry; ep->timeout_timer = entry->main.timer;
            ep->next = mgr->exchange_pending; mgr->exchange_pending = ep; }
    }
    DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "conn_mgr: phase 3 (indirect) sent EXCHANGE_REQ to 0x%016llx with %u candidates",
               (unsigned long long)entry->node_id, req.candidate_count);
    return CONN_MGR_OK;
}

static void cm_exchange_timeout_cb(void* arg) {
    struct CONN_MGR_ENTRY* entry = (struct CONN_MGR_ENTRY*)arg;
    entry->main.timer = NULL;
    struct cm_exchange_pending** pp = &entry->mgr->exchange_pending;
    while (*pp) { if ((*pp)->entry == entry) { struct cm_exchange_pending* ep = *pp; *pp = ep->next; u_free(ep); break; } pp = &(*pp)->next; }
    DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "conn_mgr: exchange timeout for 0x%016llx", (unsigned long long)entry->node_id);
    cm_deliver_result(entry, CONN_MGR_ERR_TIMEOUT);
}

static void cm_compute_intermediaries(struct CONN_MGR_ENTRY* entry, struct CONN_MGR_INTERM_EXCHANGE_RESP* resp) {
    struct { uint64_t node_id; uint64_t total_rtt; } all[8];
    uint8_t count = 0;

    for (uint8_t i = 0; i < resp->your_count && count < 8; i++) {
        uint64_t id = resp->your_candidates[i].node_id;
        uint16_t our_rtt = 0;
        for (uint8_t j = 0; j < CONN_MGR_MAX_CANDIDATES; j++) {
            if (entry->mgr->best_candidates[j].node_id == id) { our_rtt = entry->mgr->best_candidates[j].rtt; break; }
        }
        all[count].node_id = id;
        all[count].total_rtt = (uint64_t)our_rtt + resp->your_candidates[i].rtt;
        count++;
    }
    for (uint8_t i = 0; i < resp->my_count && count < 8; i++) {
        uint64_t id = resp->my_candidates[i].node_id;
        struct NODEINFO_Q* nq = nodeinfo_find_by_id(entry->mgr->instance->bgp, id);
        uint16_t our_rtt = nq ? cm_get_node_min_rtt(nq) : 0;
        if (our_rtt == 0xFFFF) our_rtt = 0;
        all[count].node_id = id;
        all[count].total_rtt = (uint64_t)our_rtt + resp->my_candidates[i].rtt;
        count++;
    }

    for (uint8_t i = 0; i < count; i++)
        for (uint8_t j = i + 1; j < count; j++)
            if (all[j].total_rtt < all[i].total_rtt) { typeof(all[0]) t = all[i]; all[i] = all[j]; all[j] = t; }

    uint8_t sel = count > CONN_MGR_MAX_INTERMEDIARIES ? CONN_MGR_MAX_INTERMEDIARIES : count;
    for (uint8_t i = 0; i < sel; i++) entry->intermediaries[i] = all[i].node_id;
    entry->intermediariy_count = sel;
    entry->rr_idx = 0;

    struct CONN_MGR_INTERM_SELECTED pkt;
    memset(&pkt, 0, sizeof(pkt));
    pkt.cmd = ETCP_RT_ID_CONN_MGR;
    pkt.subcmd = CONN_MGR_SUBCMD_INTERM_SELECTED;
    pkt.request_id = 0;
    pkt.count = sel;
    for (uint8_t i = 0; i < sel; i++) {
        pkt.selected[i].node_id = all[i].node_id;
        pkt.selected[i].rtt = 0;
        struct NODEINFO_Q* nq = nodeinfo_find_by_id(entry->mgr->instance->bgp, all[i].node_id);
        if (nq) pkt.selected[i].rtt = cm_get_node_min_rtt(nq);
    }
    struct ll_entry* qe = queue_entry_new(sizeof(pkt));
    if (qe) { memcpy(qe->data, &pkt, sizeof(pkt)); etcp_route_send(entry->mgr->instance, entry->node_id, qe, 1); }

    entry->conn_type = CONN_TYPE_INDIRECT;
    entry->state = CONN_MGR_STATE_CONNECTED;
    DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "conn_mgr: indirect OK for 0x%016llx via %u intermediaries",
               (unsigned long long)entry->node_id, sel);
    cm_update_nodeinfo(entry);
    cm_deliver_result(entry, CONN_MGR_OK);
}

static void cm_exchange_probe_retry_cb(void* arg) {
    struct cm_exchange_pending* ep = (struct cm_exchange_pending*)arg;
    struct CONN_MGR* mgr = ep->entry->mgr;
    ep->timeout_timer = NULL;
    uint8_t need_probe = 0;
    uint64_t now_tb = get_time_tb();
    for (uint8_t i = 0; i < ep->cached_resp.my_count && i < 4; i++) {
        struct NODEINFO_Q* nq = nodeinfo_find_by_id(mgr->instance->bgp, ep->cached_resp.my_candidates[i].node_id);
        if (!nq || !cm_is_rtt_fresh(nq, now_tb)) { need_probe++; break; }
    }
    if (need_probe == 0) {
        cm_compute_intermediaries(ep->entry, &ep->cached_resp);
        struct cm_exchange_pending** pp = &mgr->exchange_pending;
        while (*pp) { if (*pp == ep) { *pp = ep->next; break; } pp = &(*pp)->next; }
        u_free(ep);
    } else {
        ep->timeout_timer = uasync_set_timeout(mgr->instance->ua, 2000, ep, cm_exchange_probe_retry_cb, "cm_exch_probe");
    }
}

static void cm_handle_interm_exchange_resp(struct CONN_MGR* mgr, const uint8_t* data, size_t len) {
    const struct CONN_MGR_INTERM_EXCHANGE_RESP* resp = (const struct CONN_MGR_INTERM_EXCHANGE_RESP*)data;
    size_t min_size = offsetof(struct CONN_MGR_INTERM_EXCHANGE_RESP, your_candidates);
    if (len < min_size) return;
    struct cm_exchange_pending* ep = mgr->exchange_pending;
    while (ep) { if (ep->request_id == resp->request_id) break; ep = ep->next; }
    if (!ep) return;

    if (ep->entry->main.timer) { uasync_cancel_timeout(mgr->instance->ua, ep->entry->main.timer); ep->entry->main.timer = NULL; }

    memcpy(&ep->cached_resp, resp, len < sizeof(ep->cached_resp) ? len : sizeof(ep->cached_resp));
    ep->resp_received = 1;
    ep->probes_done = 0;

    uint8_t need_probe = 0;
    uint64_t now_tb = get_time_tb();
    for (uint8_t i = 0; i < ep->cached_resp.my_count && i < 4; i++) {
        struct NODEINFO_Q* nq = nodeinfo_find_by_id(mgr->instance->bgp, ep->cached_resp.my_candidates[i].node_id);
        if (!nq || !cm_is_rtt_fresh(nq, now_tb)) { need_probe++; route_connectivity_probe_node(mgr->instance, nq); }
    }
    if (need_probe == 0) { ep->probes_done = 1; cm_compute_intermediaries(ep->entry, &ep->cached_resp);
        struct cm_exchange_pending** pp = &mgr->exchange_pending;
        while (*pp) { if (*pp == ep) { *pp = ep->next; break; } pp = &(*pp)->next; }
        u_free(ep); return; }
    ep->timeout_timer = uasync_set_timeout(mgr->instance->ua, 2000, ep, cm_exchange_probe_retry_cb, "cm_exch_probe");
}

static void cm_handle_interm_selected(struct CONN_MGR* mgr, const uint8_t* data, size_t len) {
    const struct CONN_MGR_INTERM_SELECTED* sel = (const struct CONN_MGR_INTERM_SELECTED*)data;
    if (len < offsetof(struct CONN_MGR_INTERM_SELECTED, selected)) return;
    struct CONN_MGR_ENTRY* entry = NULL;
    for (size_t i = 0; i < mgr->entry_count; i++)
        if (mgr->entries[i].state == CONN_MGR_STATE_CONNECTING) { entry = &mgr->entries[i]; break; }
    if (!entry) return;
    uint8_t cnt = sel->count > CONN_MGR_MAX_INTERMEDIARIES ? CONN_MGR_MAX_INTERMEDIARIES : sel->count;
    for (uint8_t i = 0; i < cnt; i++) entry->intermediaries[i] = sel->selected[i].node_id;
    entry->intermediariy_count = cnt;
    entry->rr_idx = 0;
    entry->conn_type = CONN_TYPE_INDIRECT;
    entry->state = CONN_MGR_STATE_CONNECTED;
    DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "conn_mgr: got INTERM_SELECTED count=%u", cnt);
}

static void conn_mgr_router_recv_handler(struct ETCP_CONN* conn, struct ll_entry* entry) {
    if (!entry || !entry->dgram || entry->len < 3) return;
    uint8_t* dgram = entry->dgram + 1;       // skip svc_id byte from router
    size_t len = entry->len - 1;
    uint8_t subcmd = dgram[1];
    struct CONN_MGR* mgr = conn->instance->conn_mgr;

    if (mgr) {
        struct CONN_MGR_ENTRY* e = cm_find_entry(mgr, conn->peer_node_id);
        if (e && e->state == CONN_MGR_STATE_CONNECTED) e->last_traffic_tb = get_time_tb();
    }

    switch (subcmd) {
    case CONN_MGR_SUBCMD_DIRECT_REQ:
        cm_handle_direct_req(conn, dgram, len);
        break;
    case CONN_MGR_SUBCMD_DIRECT_RESP:
        break;
    case CONN_MGR_SUBCMD_INTERM_EXCHANGE_REQ:
        if (len >= CONN_MGR_INTERM_EXCHANGE_REQ_SIZE)
            cm_handle_interm_exchange_req(conn, (struct CONN_MGR_INTERM_EXCHANGE_REQ*)dgram);
        break;
    case CONN_MGR_SUBCMD_INTERM_EXCHANGE_RESP:
        if (mgr) cm_handle_interm_exchange_resp(mgr, dgram, len);
        break;
    case CONN_MGR_SUBCMD_INTERM_SELECTED:
        if (mgr) cm_handle_interm_selected(mgr, dgram, len);
        break;
    case CONN_MGR_SUBCMD_DISCONNECT:
        if (len >= CONN_MGR_DISCONNECT_SIZE && mgr) {
            struct CONN_MGR_DISCONNECT* disc = (struct CONN_MGR_DISCONNECT*)dgram;
            cm_handle_disconnect(mgr, disc->node_id);
        }
        break;
    }
}

static void cm_handle_direct_req(struct ETCP_CONN* conn, const uint8_t* data, size_t len) {
    struct CONN_MGR* mgr = conn->instance->conn_mgr;
    if (!mgr) return;
    struct CONN_MGR_DIRECT_REQ* req = (struct CONN_MGR_DIRECT_REQ*)data;
    size_t expected = CONN_MGR_DIRECT_REQ_HDR_SIZE + (size_t)req->addr_count * 8;
    if (len < expected) { DEBUG_WARN(DEBUG_CATEGORY_GENERAL, "conn_mgr: DIRECT_REQ too short"); return; }
    DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "conn_mgr: got DIRECT_REQ from 0x%016llx with %u addrs",
               (unsigned long long)conn->peer_node_id, req->addr_count);
    uint8_t* p = (uint8_t*)data + CONN_MGR_DIRECT_REQ_HDR_SIZE;
    uint64_t src_node_id = conn->peer_node_id;
    for (uint8_t i = 0; i < req->addr_count; i++) {
        uint8_t type = *p++;
        uint8_t ip[4]; memcpy(ip, p, 4); p += 4;
        uint16_t port; memcpy(&port, p, 2); p += 2;
        uint8_t sock_id = *p++;
        (void)sock_id; (void)type;
        struct ETCP_SOCKET* s = conn->instance->etcp_sockets;
        while (s) {
            if ((s->type == CFG_SERVER_TYPE_PUBLIC || s->type == CFG_SERVER_TYPE_UNKNOWN) && s->local_addr.ss_family == AF_INET) {
                struct sockaddr_in sin;
                memset(&sin, 0, sizeof(sin)); sin.sin_family = AF_INET;
                memcpy(&sin.sin_addr.s_addr, ip, 4); sin.sin_port = port;
                struct sockaddr_storage sa;
                memset(&sa, 0, sizeof(sa)); memcpy(&sa, &sin, sizeof(sin));
                struct ETCP_CONN* new_conn = etcp_connection_create(conn->instance, NULL);
                if (new_conn) {
                    struct NODEINFO_Q* nq = nodeinfo_find_by_id(conn->instance->bgp, src_node_id);
                    if (nq) sc_set_peer_public_key(&new_conn->crypto_ctx, nq->node.public_key, 0);
                    struct cm_reverse_pending* rp = u_calloc(1, sizeof(struct cm_reverse_pending));
                    if (rp) { rp->request_id = req->request_id; rp->entry = NULL; rp->next = mgr->reverse_pending;
                        etcp_conn_set_ready_cbk(new_conn, cm_reverse_ready_cb, rp); mgr->reverse_pending = rp; }
                    etcp_link_new(new_conn, s, &sa, 0);
                }
                break;
            }
            s = s->next;
        }
    }
}

static void cm_handle_interm_exchange_req(struct ETCP_CONN* conn,
                                          struct CONN_MGR_INTERM_EXCHANGE_REQ* req) {
    struct CONN_MGR* mgr = conn->instance->conn_mgr;
    if (!mgr) return;
    uint64_t now_tb = get_time_tb();
    for (uint8_t i = 0; i < req->candidate_count && i < 4; i++) {
        uint64_t cand_id = req->candidates[i].node_id;
        struct NODEINFO_Q* nq = nodeinfo_find_by_id(mgr->instance->bgp, cand_id);
        if (nq && cm_is_rtt_fresh(nq, now_tb)) continue;
        route_connectivity_probe_node(mgr->instance, nq);
    }
    struct CONN_MGR_INTERM_EXCHANGE_RESP resp;
    memset(&resp, 0, sizeof(resp));
    resp.cmd = ETCP_RT_ID_CONN_MGR;
    resp.subcmd = CONN_MGR_SUBCMD_INTERM_EXCHANGE_RESP;
    resp.request_id = req->request_id;
    resp.my_count = mgr->best_candidate_count > 4 ? 4 : mgr->best_candidate_count;
    for (uint8_t i = 0; i < resp.my_count; i++)
        resp.my_candidates[i] = mgr->best_candidates[i];
    resp.your_count = req->candidate_count > 4 ? 4 : req->candidate_count;
    for (uint8_t i = 0; i < resp.your_count; i++) {
        resp.your_candidates[i].node_id = req->candidates[i].node_id;
        struct NODEINFO_Q* nq2 = nodeinfo_find_by_id(mgr->instance->bgp, req->candidates[i].node_id);
        uint16_t rtt = nq2 ? cm_get_node_min_rtt(nq2) : 0;
        resp.your_candidates[i].rtt = rtt;
    }
    size_t resp_size = offsetof(struct CONN_MGR_INTERM_EXCHANGE_RESP, your_candidates) +
                       (size_t)resp.your_count * sizeof(struct CONN_MGR_CANDIDATE);
    struct ll_entry* qe = queue_entry_new(resp_size);
    if (qe) {
        memcpy(qe->data, &resp, resp_size);
        etcp_route_send(mgr->instance, conn->peer_node_id, qe, 1);
    }
    DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "conn_mgr: sent EXCHANGE_RESP to 0x%016llx my=%u your=%u",
               (unsigned long long)conn->peer_node_id, resp.my_count, resp.your_count);
}

static void cm_handle_disconnect(struct CONN_MGR* mgr, uint64_t node_id) {
    struct CONN_MGR_ENTRY* entry = cm_find_entry(mgr, node_id);
    if (!entry) return;
    cm_clear_nodeinfo(mgr, node_id);
    cm_entry_destroy(entry);
    DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "conn_mgr: remote disconnect from 0x%016llx", (unsigned long long)node_id);
}

static void cm_bg_ping_timer_cb(void* arg) {
    struct CONN_MGR* mgr = (struct CONN_MGR*)arg;
    struct ROUTE_BGP* bgp = mgr->instance->bgp;
    if (!bgp || !bgp->nodes) {
        mgr->bg_ping_timer = uasync_set_timeout(mgr->instance->ua, CONN_MGR_BG_PING_INTERVAL_TB, mgr, cm_bg_ping_timer_cb, "conn_mgr_bg_ping");
        return;
    }
    uint64_t now_tb = get_time_tb();
    struct ll_entry* e = bgp->nodes->head;
    size_t total = 0;
    while (e) { total++; e = e->next; }
    if (total == 0) {
        mgr->bg_ping_timer = uasync_set_timeout(mgr->instance->ua, CONN_MGR_BG_PING_INTERVAL_TB, mgr, cm_bg_ping_timer_cb, "conn_mgr_bg_ping");
        return;
    }
    if (mgr->bg_ping_cursor >= total) {
        mgr->bg_ping_cursor = 0;
        if ((now_tb - mgr->bg_ping_cycle_start_tb) < CONN_MGR_BG_PING_CYCLE_MIN_TB) {
            uint64_t delay = CONN_MGR_BG_PING_CYCLE_MIN_TB - (now_tb - mgr->bg_ping_cycle_start_tb);
            mgr->bg_ping_timer = uasync_set_timeout(mgr->instance->ua, (int)delay, mgr, cm_bg_ping_timer_cb, "conn_mgr_bg_ping");
            return;
        }
        mgr->bg_ping_cycle_start_tb = now_tb;
    }
    size_t cursor = 0;
    e = bgp->nodes->head;
    while (e && cursor < mgr->bg_ping_cursor) { cursor++; e = e->next; }
    if (e) {
        struct NODEINFO_Q* nq = (struct NODEINFO_Q*)e;
        uint64_t node_id = nq->node.node_id;
        if (node_id != mgr->instance->node_id) {
            struct ETCP_CONN* dconn = route_bgp_find_conn_for_node(bgp, node_id);
            int has_active = 0;
            if (dconn && dconn->links) {
                struct ETCP_LINK* l = dconn->links;
                while (l) { if (l->initialized && l->link_status) { has_active = 1; break; } l = l->next; }
            }
            if (!has_active) {
                etcp_send_ping(mgr->instance, nq->node.public_key, NULL, CONN_PROBE_TIMEOUT_MS,
                               NULL, NULL, NULL, 0);
            }
        }
        mgr->bg_ping_cursor++;
    }
    mgr->bg_ping_timer = uasync_set_timeout(mgr->instance->ua, CONN_MGR_BG_PING_INTERVAL_TB, mgr, cm_bg_ping_timer_cb, "conn_mgr_bg_ping");
}

static void cm_candidate_ping_timer_cb(void* arg) {
    struct CONN_MGR* mgr = (struct CONN_MGR*)arg;
    uint64_t now_tb = get_time_tb();
    for (uint8_t i = 0; i < mgr->best_candidate_count; i++) {
        uint64_t cand_id = mgr->best_candidates[i].node_id;
        struct NODEINFO_Q* nq = nodeinfo_find_by_id(mgr->instance->bgp, cand_id);
        if (!nq) {
            memmove(&mgr->best_candidates[i], &mgr->best_candidates[i+1], (mgr->best_candidate_count - i - 1) * sizeof(mgr->best_candidates[0]));
            mgr->best_candidate_count--; i--; continue;
        }
        uint64_t last = cm_get_node_max_probe_time(nq);
        if (last > 0 && (now_tb - last) >= CONN_MGR_CANDIDATE_STALE_TB) {
            DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "conn_mgr: candidate 0x%016llx stale, removing", (unsigned long long)cand_id);
            memmove(&mgr->best_candidates[i], &mgr->best_candidates[i+1], (mgr->best_candidate_count - i - 1) * sizeof(mgr->best_candidates[0]));
            mgr->best_candidate_count--; i--; continue;
        }
        struct ETCP_CONN* dconn = route_bgp_find_conn_for_node(mgr->instance->bgp, cand_id);
        int has_active = 0;
        if (dconn && dconn->links) {
            struct ETCP_LINK* l = dconn->links;
            while (l) { if (l->initialized && l->link_status) { has_active = 1; break; } l = l->next; }
        }
        if (has_active)
            etcp_send_ping(mgr->instance, nq->node.public_key, &dconn->links->remote_addr, CONN_PROBE_TIMEOUT_MS, NULL, NULL, NULL, 0);
        else
            route_connectivity_probe_node(mgr->instance, nq);
    }
    mgr->candidate_ping_timer = uasync_set_timeout(mgr->instance->ua, CONN_MGR_CANDIDATE_PING_TB,
                                                     mgr, cm_candidate_ping_timer_cb, "conn_mgr_cand_ping");
}