/* ecat_native.c — 原型 A：原生 PDO 守护进程（SOEM 直用，无 Python/GIL）
 *
 * 目的：验证"换语言"能把 PDO 循环的天花板从 Python 的 ~5kHz 抬到多高、
 *       尾抖动能多干净。总线核心收发本来就是 C（pysoem 只是 SOEM 的包装），
 *       这里把热循环从解释器里拿出来，其余一切不动。
 *
 * 契约：复刻 ecat_server.py 的 3 条热路由（POST /api/motor/state|period|dbg），
 *       只绑 127.0.0.1、无 CORS —— 与 Python 服务同口径，cadence_diag/rate_probe
 *       直接 --base http://127.0.0.1:8091 就能打过来。其余路由仍留在 Python。
 *
 * 时间线元组与 Python 完全同构：(t0, ts, tm, tr, te, k)
 *   t0=循环顶  ts=send 前(原生无锁等待，≈t0)  tm=send 后  tr=receive 后
 *   te=循环底(跳拍判定时刻)  k=本拍网格跳过数
 * 时刻用 QueryPerformanceCounter 秒 —— 与 Python perf_counter 同一时间基
 * （实测差 ≤6µs），cadence_diag 的差分口径不变。
 */
#include <winsock2.h>
#include <ws2tcpip.h>
#include <windows.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "ethercat.h"

#define RING 2048                 /* 与 Python _cyc_t maxlen 同 */
#define IO_MAP_SIZE 4096
#define STATS_N 400               /* state 里 cyc_us 分位数的样本窗口，同 _cyc_stats(n=400) */

/* ── 计时原语 ── */
static double g_qpc2s;
static inline double now_s(void)
{
    LARGE_INTEGER c;
    QueryPerformanceCounter(&c);
    return (double)c.QuadPart * g_qpc2s;
}
static inline void spin_until(double deadline)
{
    while (now_s() < deadline)
        YieldProcessor();         /* PAUSE：SMT 兄弟友好，不烧功耗 */
}

/* ── 共享状态 ── */
typedef struct { double t0, ts, tm, tr, te; int k; } beat_t;

static beat_t g_ring[RING];
static int g_ring_i;                          /* 写指针：仅 PDO 线程动 */
static long long g_total;                     /* 累计拍数 */
static long long g_skip_total;                /* 累计网格跳过拍 */
static CRITICAL_SECTION g_ring_lock;          /* 保护 ring/total/skip 的读侧 */
static CRITICAL_SECTION g_cfg_lock;           /* 保护 g_period_ms */
static double g_period_ms = 0.25;
static volatile LONG g_stop;
static volatile LONG g_wkc;
static long long g_expected_wkc;
static int g_slavecount;
static char g_slave_names[16][64];
static uint8 g_iomap[IO_MAP_SIZE];
static ULONGLONG g_pcore_mask;                /* --mask，0=不钉 */

/* ── PDO 线程：网格 + 自旋 + 分段计时，逻辑与 _pdo_worker 一一对应 ── */
static DWORD WINAPI pdo_worker(LPVOID arg)
{
    double per, t0, ts, tm, tr, now, te, next_t = 0;
    int have_next = 0, k, i, wkc;

    SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_TIME_CRITICAL);
    if (g_pcore_mask)
        SetThreadAffinityMask(GetCurrentThread(), (DWORD_PTR)g_pcore_mask);

    while (!InterlockedOr(&g_stop, 0)) {
        t0 = now_s();
        EnterCriticalSection(&g_cfg_lock);
        per = g_period_ms / 1000.0;
        LeaveCriticalSection(&g_cfg_lock);
        if (!have_next) { next_t = t0 + per; have_next = 1; }

        ts = now_s();                       /* 原生无锁无填充 → pre≈0 */
        ec_send_processdata();
        tm = now_s();
        wkc = ec_receive_processdata(EC_TIMEOUTRET);
        tr = now_s();
        InterlockedExchange(&g_wkc, wkc);

        /* 绝对网格截止点 + 超期跳过（不挪锚点），与 Python 同 */
        now = now_s();
        k = 0;
        if (next_t <= now) {
            while (next_t <= now) { next_t += per; k++; }
        }
        te = now;

        EnterCriticalSection(&g_ring_lock);
        i = g_ring_i;
        g_ring_i = (i + 1) % RING;
        g_ring[i].t0 = t0; g_ring[i].ts = ts; g_ring[i].tm = tm;
        g_ring[i].tr = tr; g_ring[i].te = te; g_ring[i].k = k;
        g_total++;
        g_skip_total += k;
        LeaveCriticalSection(&g_ring_lock);

        if (k == 0) {                       /* 没超期才等下一拍；超期直接落下去补 */
            spin_until(next_t);
            next_t += per;
        }
    }
    return 0;
}

/* ── 分位数（读侧现算，同 _cyc_fresh 的口径）── */
static int cmp_d(const void *a, const void *b)
{
    double x = *(const double *)a, y = *(const double *)b;
    return (x > y) - (x < y);
}
static void pct3(const double *v, int n, char *out, size_t osz)
{
    double *s = (double *)malloc(n * sizeof(double));
    int i;
    if (!s) { snprintf(out, osz, "{}"); return; }
    memcpy(s, v, n * sizeof(double));
    qsort(s, n, sizeof(double), cmp_d);
    snprintf(out, osz, "{\"p50\":%.1f,\"p90\":%.1f,\"max\":%.1f}",
             s[(int)(0.5 * (n - 1))], s[(int)(0.9 * (n - 1))], s[n - 1]);
    free(s);
}

/* 取最近 n 拍（按时间序）。ring 单写者，读侧加锁拷贝。 */
static int recent_beats(beat_t *dst, int n)
{
    int have, start, i;
    EnterCriticalSection(&g_ring_lock);
    have = (g_total < RING) ? (int)g_total : RING;
    if (n > have) n = have;
    start = (g_ring_i - n + RING * 2) % RING;
    for (i = 0; i < n; i++) dst[i] = g_ring[(start + i) % RING];
    LeaveCriticalSection(&g_ring_lock);
    return n;
}

static void build_cyc_us(char *out, size_t osz)
{
    static beat_t b[RING];
    static double work[STATS_N], send[STATS_N], rx[STATS_N], tot[STATS_N];
    char sw[96], sr[96], st[96], swork[96];
    int n = recent_beats(b, RING), i, m = n < STATS_N ? n : STATS_N;
    if (m < 2) { snprintf(out, osz, "{}"); return; }
    for (i = 0; i < m; i++) {
        beat_t *p = &b[n - m + i];
        work[i] = (p->te - p->t0) * 1e6;
        send[i] = (p->tm - p->ts) * 1e6;
        rx[i]   = (p->tr - p->tm) * 1e6;
        tot[i]  = (p->tr - p->t0) * 1e6;
    }
    pct3(work, m, swork, sizeof swork);
    pct3(send, m, sw, sizeof sw);
    pct3(rx, m, sr, sizeof sr);
    pct3(tot, m, st, sizeof st);
    snprintf(out, osz,
             "\"cyc_us\":{\"n\":%d,\"work\":%s,\"send\":%s,\"rx\":%s,\"total\":%s,"
             "\"late\":0,\"lost\":0,\"gc\":{\"n\":[0,0,0],\"max_us\":[0,0,0],\"pdo\":0}}",
             m, swork, sw, sr, st);
}

/* ── 极简 HTTP/1.1（POST-only JSON，thread-per-connection）── */
static int json_num(const char *body, const char *key, double *val)
{
    const char *p = strstr(body, key);
    if (!p) return 0;
    p += strlen(key);
    while (*p && *p != ':') p++;
    if (*p != ':') return 0;
    p++;
    while (*p == ' ' || *p == '\t') p++;
    char *end;
    double v = strtod(p, &end);
    if (end == p) return 0;
    *val = v;
    return 1;
}

static void handle_conn(SOCKET cs)
{
    char *buf = (char *)malloc(65536);
    int len = 0, i, cl = 0;
    char *hdr_end, *body, path[128] = "";
    char *resp = (char *)malloc(256 * 1024);
    int rl = 0;
    char extra[4096];

    if (!buf || !resp) { closesocket(cs); free(buf); free(resp); return; }
    /* 读到头 */
    for (;;) {
        int n = recv(cs, buf + len, 65535 - len, 0);
        if (n <= 0) goto done;
        len += n; buf[len] = 0;
        hdr_end = strstr(buf, "\r\n\r\n");
        if (hdr_end) break;
    }
    body = hdr_end + 4;
    {   /* Content-Length + 收满 body */
        char *p = buf;
        for (p = strstr(p, "\n"); p && p < hdr_end; p = strstr(p + 1, "\n")) {
            /* p 停在上一行的 \n 上，下一行从 p+1 开始 */
            if (!_strnicmp(p + 1, "content-length:", 15)) { cl = atoi(p + 16); break; }
        }
        while ((body + cl) > (buf + len)) {
            int need = (int)((body + cl) - (buf + len));
            int r = recv(cs, buf + len, need > 65535 - len ? 65535 - len : need, 0);
            if (r <= 0) goto done;
            len += r; buf[len] = 0;
        }
        if (cl > 65535 - (int)(body - buf)) cl = (int)(buf + len - body);
        body[cl] = 0;
    }
    if (sscanf(buf, "%*s %127s", path) != 1) goto done;

    if (!strcmp(path, "/api/motor/state")) {
        build_cyc_us(extra, sizeof extra);
        rl = snprintf(resp, 256 * 1024,
            "{\"ok\":true,\"log\":[],\"cycles\":%lld,\"period_ms\":%.4f,\"spin\":true,"
            "\"wkc\":%d,\"expected_wkc\":%d,\"slot_period_ms\":%.4f,\"win_bases\":0,"
            "\"queue_depth\":0,\"sent_seq\":0,\"sent_frames\":0,\"ts\":0,"
            "\"head\":{},\"motors\":[",
            g_total, g_period_ms, (int)g_wkc, (int)g_expected_wkc, g_period_ms);
        for (i = 0; i < g_slavecount && i < 16; i++)
            rl += snprintf(resp + rl, 256 * 1024 - rl, "%s{\"name\":\"%s\",\"valid\":true}",
                           i ? "," : "", g_slave_names[i]);
        rl += snprintf(resp + rl, 256 * 1024 - rl, "],\"bridge\":{},\"xrx\":{},\"rx_batch\":{},%s}", extra);
    } else if (!strcmp(path, "/api/motor/period")) {
        double v = -1, old = g_period_ms;
        if (json_num(body, "period_us", &v) && v > 0) v /= 1000.0;
        else if (!json_num(body, "period_ms", &v)) v = -1;
        if (v < 0.0625 || v > 1000.0) {
            rl = snprintf(resp, 256 * 1024,
                "{\"ok\":false,\"log\":[\"周期 %s 超出允许范围 [62.5us, 1000ms]\"]}", body);
        } else {
            EnterCriticalSection(&g_cfg_lock);
            g_period_ms = v;
            LeaveCriticalSection(&g_cfg_lock);
            rl = snprintf(resp, 256 * 1024,
                "{\"ok\":true,\"log\":[\"PDO 周期(native) %gms → %gms\"],\"period_ms\":%.4f}",
                old, v, v);
        }
    } else if (!strcmp(path, "/api/motor/dbg")) {
        static beat_t b[RING];
        int n = recent_beats(b, RING);
        rl = snprintf(resp, 256 * 1024,
            "{\"ok\":true,\"period_ms\":%.4f,\"spin\":true,\"gc_enabled\":false,"
            "\"cycles\":%lld,\"skip_total\":%lld,\"cyc\":[",
            g_period_ms, g_total, g_skip_total);
        for (i = 0; i < n; i++)
            rl += snprintf(resp + rl, 256 * 1024 - rl,
                "%s[%.7f,%.7f,%.7f,%.7f,%.7f,%d]",
                i ? "," : "", b[i].t0, b[i].ts, b[i].tm, b[i].tr, b[i].te, b[i].k);
        rl += snprintf(resp + rl, 256 * 1024 - rl, "],\"gc_events\":[],\"http\":[]}");
    } else if (!strcmp(path, "/") || !strcmp(path, "/api/native/health")) {
        rl = snprintf(resp, 256 * 1024,
            "{\"ok\":true,\"name\":\"ecat_native\",\"slaves\":%d,\"wkc\":%d,\"cycles\":%lld}",
            g_slavecount, (int)g_wkc, g_total);
    } else {
        rl = snprintf(resp, 256 * 1024,
            "{\"ok\":false,\"log\":[\"接口不存在(native 原型只有 state/period/dbg): %s\"]}", path);
    }
    {   /* 发送 */
        char hdr[256];
        int hl = snprintf(hdr, sizeof hdr,
            "HTTP/1.1 200 OK\r\nContent-Type: application/json; charset=utf-8\r\n"
            "Cache-Control: no-store\r\nContent-Length: %d\r\nConnection: close\r\n\r\n", rl);
        send(cs, hdr, hl, 0);
        send(cs, resp, rl, 0);
    }
done:
    closesocket(cs);
    free(buf);
    free(resp);
}

static DWORD WINAPI conn_thread(LPVOID p)
{
    handle_conn((SOCKET)(intptr_t)p);
    return 0;
}

int main(int argc, char **argv)
{
    const char *substr = "I226";
    int port = 8091, i;
    LARGE_INTEGER f;
    ec_adaptert *ad, *pick = NULL;
    WSADATA wsa;
    SOCKET ls;
    struct sockaddr_in sa;
    HANDLE th;

    for (i = 1; i < argc - 0; i++) {
        if (!strcmp(argv[i], "--substr") && i + 1 < argc) substr = argv[++i];
        else if (!strcmp(argv[i], "--port") && i + 1 < argc) port = atoi(argv[++i]);
        else if (!strcmp(argv[i], "--mask") && i + 1 < argc)
            g_pcore_mask = strtoull(argv[++i], NULL, 0);
    }

    QueryPerformanceFrequency(&f);
    g_qpc2s = 1.0 / (double)f.QuadPart;
    InitializeCriticalSection(&g_ring_lock);
    InitializeCriticalSection(&g_cfg_lock);

    /* 找网卡（desc/name 含 substr） */
    for (ad = ec_find_adapters(); ad; ad = ad->next) {
        if (strstr(ad->desc, substr) || strstr(ad->name, substr)) { pick = ad; break; }
    }
    if (!pick) {
        fprintf(stderr, "[native] 找不到含 \"%s\" 的网卡\n", substr);
        return 2;
    }
    if (!ec_init(pick->name)) {
        fprintf(stderr, "[native] ec_init 失败: %s\n", pick->name);
        return 3;
    }
    printf("[native] 打开网卡: %s\n", pick->desc);

    /* 预清理：广播 INIT|ACK 清掉上一主站(可能被强杀)残留的 AL 错误位，再重枚举。
       SSC 的 AL_ControlInd 对"错误位已置 + 请求不含 ACK(bit4)"的迁移直接忽略
       (ecatslv.c)，这是 native 之前卡 PREOP 的根因 —— 与 Python motor/start 的
       预清理同理。config_init 会重写 SM0/SM1，等效"断开重连"。 */
    ec_slave[0].state = EC_STATE_INIT | EC_STATE_ACK;
    ec_writestate(0);
    ec_statecheck(0, EC_STATE_INIT, 1000000);
    if (ec_config_init(FALSE) <= 0) {
        fprintf(stderr, "[native] 枚举从站失败（邮箱楔死需断电重启板子）\n");
        ec_close();
        return 4;
    }
    ec_config_map(&g_iomap);
    ec_readstate();
    /* LRW 的期望 WKC：每从站读输出+1、写输入+1、回读+1 → outputsWKC*2 + inputsWKC
       （与 pysoem 同式；本机 1 从站 = 1*2+1 = 3，与实测 wkc 一致） */
    g_expected_wkc = ec_group[0].outputsWKC * 2 + ec_group[0].inputsWKC;
    g_slavecount = ec_slavecount;
    for (i = 1; i <= ec_slavecount && i <= 16; i++)
        snprintf(g_slave_names[i - 1], 64, "%s", ec_slave[i].name);
    printf("[native] 枚举 %d 从站, config_map 后 AL=0x%02x, expected_wkc=%lld\n",
           ec_slavecount, ec_slave[1].state & 0x0F, g_expected_wkc);

    /* config_map 内部请求 SAFEOP 不带 ACK，残留错误位下会被忽略 → 显式补一次
       SAFEOP|ACK 写，确保到 SAFEOP。 */
    ec_slave[0].state = EC_STATE_SAFE_OP | EC_STATE_ACK;
    ec_writestate(0);
    ec_statecheck(0, EC_STATE_SAFE_OP, 2000000);

    /* SAFEOP→OP：从站要先收到≥1帧过程数据才 ack OP，且带错误位时只有 OP|ACK
       才清错并迁移 → 边发 PD 边带 ACK 重写 OP，每 200ms 一轮，readstate 拿真实态。
       进 OP 后再起稳态线程（避免主线程与线程并发操作同一 ecx_port 打架）。 */
    for (i = 0; i < 60; i++) {
        ec_send_processdata();
        ec_receive_processdata(EC_TIMEOUTRET);
        ec_slave[0].state = EC_STATE_OPERATIONAL | EC_STATE_ACK;
        ec_writestate(0);
        ec_readstate();
        if ((ec_slave[1].state & 0x0F) == EC_STATE_OPERATIONAL) break;
        Sleep(200);
    }
    if ((ec_slave[1].state & 0x0F) != EC_STATE_OPERATIONAL) {
        fprintf(stderr, "[native] 进 OP 失败（AL=0x%02x code=0x%04x）—— 若反复如此，"
                        "可能是上一主站退出后 SSC 邮箱楔死，需断电重启板子\n",
                ec_slave[1].state & 0x0F, ec_slave[1].ALstatuscode);
        ec_close();
        return 5;
    }
    printf("[native] OP 达成: %d 从站\n", ec_slavecount);

    th = CreateThread(NULL, 0, pdo_worker, NULL, 0, NULL);
    SetThreadPriority(th, THREAD_PRIORITY_TIME_CRITICAL);

    WSAStartup(MAKEWORD(2, 2), &wsa);
    ls = socket(AF_INET, SOCK_STREAM, 0);
    { int one = 1; setsockopt(ls, SOL_SOCKET, SO_REUSEADDR, (char *)&one, sizeof one); }
    sa.sin_family = AF_INET;
    sa.sin_port = htons((u_short)port);
    sa.sin_addr.s_addr = htonl(INADDR_LOOPBACK);   /* 只绑 127.0.0.1，同 Python 口径 */
    if (bind(ls, (struct sockaddr *)&sa, sizeof sa) == SOCKET_ERROR) {
        fprintf(stderr, "[native] bind %d 失败: %d\n", port, WSAGetLastError());
        return 6;
    }
    listen(ls, 32);
    printf("[native] 监听 http://127.0.0.1:%d (state/period/dbg)\n", port);

    for (;;) {
        SOCKET cs = accept(ls, NULL, NULL);
        HANDLE t;
        if (cs == INVALID_SOCKET) continue;
        t = CreateThread(NULL, 0, conn_thread, (LPVOID)(intptr_t)cs, 0, NULL);
        if (t) CloseHandle(t); else closesocket(cs);
    }
    return 0;
}