#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""pdo-fast-send 端到端分段延迟（模式切换代理）。

电机启动/转动有惯性，rtAngle/rtVelocity 阈值难踩准。改用 **CONFIG 回报的
Target_Control_Mode（0x000N0101 B0）** 作为「命令已被电机接收并应用」的可见
代理：CMD 的 mode 字节一旦被接受，下一拍 CONFIG 就会以新 mode 回报。

路径:
  json  — HTTP POST /ecat/api/motor/send（浏览器同款页面代理）
  fast  — ecat_send_raw_frames（桌面桥同构 ESF1 TCP，不含 pywebview hop）

分段:
  t_click  = 发请求前
  t_sent   = 控制面返回（入队完成）
  t_can    = bridge.tx_count/ack 变化（EtherCAT→GD32 CAN 出口）
  t_mode   = 观测到目标节点 CONFIG B0 == 期望 mode（xrx 或 motor/rx）

用法:
  python scripts/check_e2e_click_motion.py
  python scripts/check_e2e_click_motion.py --node 3 --ch 2 --trials 5
  python scripts/check_e2e_click_motion.py --proxy motion   # 旧：角度/速度阈值
"""
from __future__ import annotations

import argparse
import json
import os
import statistics
import struct
import sys
import time
import urllib.request

sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), "..")))
from esp32_server import ecat_send_raw_frames  # noqa: E402

PAGE = os.environ.get("DCU_PAGE_BASE", "http://127.0.0.1:2625")
ECAT = PAGE + "/ecat/api"
# 观测默认打 native 直连：页面代理 p50~2ms×多次 + xrx 单帧槽会把 CONFIG 观测抬到 ~12ms
NATIVE = os.environ.get("DCU_NATIVE_API", "http://127.0.0.1:8090/api")
OBS = NATIVE  # 观测环（motor/state、motor/rx）

# 安全模式对：0=Disable（输出关闭），2=Velocity 但目标速度 0（不驱动转动）
SAFE_TOGGLE = (0, 2)


def post(url, obj=None, raw=None, ctype="application/json", timeout=8.0):
    if raw is not None:
        data = raw
        headers = {"Content-Type": ctype}
    else:
        data = json.dumps(obj or {}, ensure_ascii=False, separators=(",", ":")).encode("utf-8")
        headers = {"Content-Type": ctype}
    req = urllib.request.Request(url, data=data, headers=headers, method="POST")
    t0 = time.perf_counter()
    with urllib.request.urlopen(req, timeout=timeout) as r:
        body = r.read()
    ms = (time.perf_counter() - t0) * 1000.0
    try:
        return ms, json.loads(body.decode("utf-8", "replace") or "{}")
    except Exception:
        return ms, {"_raw_len": len(body)}


def decode_status(data_hex: str):
    if not data_hex or len(data_hex) < 36:
        return None
    raw = bytes.fromhex(data_hex[:72] if len(data_hex) >= 72 else data_hex)
    if len(raw) < 21:
        return None

    def f32(off):
        if off + 4 > len(raw):
            return 0.0
        v = struct.unpack_from("<f", raw, off)[0]
        return v if v == v else 0.0

    return {"rtAngle": f32(13), "rtVelocity": f32(17), "rtCurrent": f32(21), "b10": raw[10]}


def motor_sample(node: int, ch: int):
    _, snap = post(OBS + "/motor/state", {}, timeout=5)
    if not snap.get("ok"):
        return None, None, snap
    br = snap.get("bridge") or {}
    outlet = (br.get("tx_count"), br.get("ack"), br.get("last_tx_id_hex"))
    st = None
    for m in snap.get("motors") or []:
        if m.get("node") == node and m.get("ch") == ch:
            st = decode_status(m.get("data") or "")
            break
    return st, outlet, snap


def build_cmd(mode, voltage=24.0, current=0.0, velocity=0.0, angle=0.0, pid=100):
    buf = bytearray(32)
    buf[0] = mode & 0xFF
    struct.pack_into("<H", buf, 1, int(round(voltage * 1000)) & 0xFFFF)
    struct.pack_into("<f", buf, 3, float(current))
    struct.pack_into("<f", buf, 7, float(velocity))
    struct.pack_into("<f", buf, 11, float(angle))
    struct.pack_into("<h", buf, 15, pid)
    struct.pack_into("<h", buf, 17, pid)
    struct.pack_into("<h", buf, 19, pid)
    return buf.hex()


def send_path(path: str, ch: int, node: int, data_hex: str):
    frames = [{"ch": ch, "id": (node << 16) | 0x0200, "dlc": 32, "data": data_hex}]
    if path == "fast":
        t0 = time.perf_counter()
        resp = ecat_send_raw_frames(frames, 2000)
        return (time.perf_counter() - t0) * 1000.0, resp
    ms, resp = post(ECAT + "/motor/send", {"frames": frames}, timeout=6)
    return ms, resp


def outlet_changed(a, b):
    if not a or not b:
        return False
    return (a[0], a[1]) != (b[0], b[1]) and a[0] is not None


def cfg_mode_from_hex(data_hex: str):
    if not data_hex or len(data_hex) < 2:
        return None
    try:
        return int(data_hex[:2], 16)
    except ValueError:
        return None


def scan_config_mode(node: int, snap: dict):
    """从 xrx + motor/rx 找目标节点 CONFIG(0x000N0101) 的 B0 mode。"""
    want_id = (node << 16) | 0x0101
    hits = []
    x = snap.get("xrx") or {}
    if x.get("id") == want_id:
        m = cfg_mode_from_hex(x.get("data") or "")
        if m is not None:
            hits.append(("xrx", m, x.get("data")))
    rxb = snap.get("rx_batch") or {}
    # frames 可能在快照里；若空则单独 drain
    for f in rxb.get("frames") or []:
        if f.get("id") == want_id:
            m = cfg_mode_from_hex(f.get("data") or "")
            if m is not None:
                hits.append(("rxb", m, f.get("data")))
    return hits


def drain_rx_modes(node: int):
    want_id = (node << 16) | 0x0101
    _, rx = post(OBS + "/motor/rx", {}, timeout=4)
    out = []
    for f in rx.get("frames") or []:
        if f.get("id") == want_id:
            m = cfg_mode_from_hex(f.get("data") or "")
            if m is not None:
                out.append(m)
    return out


def observe_config_mode(node: int):
    """返回 (mode_or_None, source)。优先 drain rx（CONFIG 主通道），再 xrx。"""
    modes = drain_rx_modes(node)
    if modes:
        return modes[-1], "rx"
    _, snap = post(OBS + "/motor/state", {}, timeout=4)
    hits = scan_config_mode(node, snap)
    if hits:
        return hits[-1][1], hits[-1][0]
    return None, None


def run_once(path: str, node: int, ch: int, proxy: str, timeout: float, next_mode: int) -> dict:
    st0, outlet0, _ = motor_sample(node, ch)
    # 基线 mode：SIG 必须相对它变化，避免 CONFIG 持续回报同一 mode 造成假阳性
    base_mode, base_src = observe_config_mode(node)
    if proxy == "mode":
        # 强制与基线不同
        if base_mode == next_mode:
            next_mode = SAFE_TOGGLE[1] if base_mode == SAFE_TOGGLE[0] else SAFE_TOGGLE[0]
        data = build_cmd(mode=next_mode, voltage=24.0, current=0.0, velocity=0.0, angle=0.0)
        expect = next_mode
    else:
        data = build_cmd(mode=2, voltage=24.0, current=0.0, velocity=40.0, angle=0.0)
        expect = 2
        base_mode = None

    t_click = time.perf_counter()
    sent_ms, resp = send_path(path, ch, node, data)
    t_sent = time.perf_counter()

    t_can = None
    t_mode = None
    t_motion = None
    can_detail = mode_detail = motion_detail = ""
    ang0 = st0["rtAngle"] if st0 else 0.0
    vel0 = st0["rtVelocity"] if st0 else 0.0

    while time.perf_counter() - t_click < timeout:
        st, outlet, snap = motor_sample(node, ch)
        now = time.perf_counter()
        if t_can is None and outlet_changed(outlet0, outlet):
            t_can = now
            can_detail = f"{outlet0} → {outlet}"
        if t_mode is None and proxy == "mode":
            # 观测优先 drain rx：CONFIG 约 ~200Hz/节点，xrx 只有单槽会被其它节点挤掉
            drained = drain_rx_modes(node)
            if expect in drained and expect != base_mode:
                t_mode = now
                mode_detail = f"mode {base_mode}→{expect} via rx"
            else:
                hits = scan_config_mode(node, snap)
                for src, m, raw in reversed(hits):
                    if m == expect and m != base_mode:
                        t_mode = now
                        mode_detail = f"mode {base_mode}→{m} via {src} data={(raw or '')[:16]}"
                        break
        if st and t_motion is None and proxy == "motion":
            d_ang = abs(st["rtAngle"] - ang0)
            d_vel = abs(st["rtVelocity"] - vel0)
            if d_ang > 0.3 or abs(d_vel) > 5.0:
                t_motion = now
                motion_detail = f"Δang={d_ang:.3f} Δvel={d_vel:.2f}"
        done_mode = (proxy == "mode" and t_mode is not None) or (proxy == "motion" and t_motion is not None)
        if t_can is not None and done_mode:
            break
        # 直连 native 时观测环约 0.6–1ms；页面代理则 ≥2ms×N，会主导 p50
        time.sleep(0)

    send_path(path, ch, node, build_cmd(mode=0, voltage=24.0, current=0.0, velocity=0.0, angle=0.0))
    time.sleep(0.08)

    def ms(dt):
        return None if dt is None else dt * 1000.0

    t_sig = t_mode if proxy == "mode" else t_motion
    return {
        "ok": True,
        "path": path,
        "proxy": proxy,
        "base_mode": base_mode,
        "base_src": base_src,
        "expect_mode": expect if proxy == "mode" else None,
        "resp_ok": bool(resp.get("ok")),
        "resp_path": resp.get("path"),
        "sent_ms": sent_ms,
        "click_to_sent": ms(t_sent - t_click),
        "click_to_can": ms(None if t_can is None else t_can - t_click),
        "click_to_sig": ms(None if t_sig is None else t_sig - t_click),
        "sent_to_can": ms(None if t_can is None else t_can - t_sent),
        "sent_to_sig": ms(None if t_sig is None else t_sig - t_sent),
        "can_to_sig": ms(None if (t_can is None or t_sig is None) else t_sig - t_can),
        "can_detail": can_detail,
        "sig_detail": mode_detail if proxy == "mode" else motion_detail,
        "base_vel": vel0,
        "sig_ok": t_sig is not None,
    }


def summarize(rows, key):
    xs = [r[key] for r in rows if r.get(key) is not None]
    if not xs:
        return "n/a"
    return (
        f"n={len(xs)} mean={statistics.mean(xs):.2f}ms "
        f"p50={statistics.median(xs):.2f} min={min(xs):.2f} max={max(xs):.2f}"
    )


def main() -> int:
    ap = argparse.ArgumentParser()
    ap.add_argument("--node", type=int, default=3)
    ap.add_argument("--ch", type=int, default=2)
    ap.add_argument("--timeout", type=float, default=2.0)
    ap.add_argument("--trials", type=int, default=4)
    ap.add_argument("--paths", type=str, default="json,fast")
    ap.add_argument("--proxy", type=str, default="mode", choices=["mode", "motion"])
    args = ap.parse_args()
    paths = [p.strip() for p in args.paths.split(",") if p.strip()]

    _, health = post(OBS + "/native/health", {})
    print("health:", json.dumps(health, ensure_ascii=False)[:220])
    print(f"观测环直连: {OBS}（发送仍按 path 走 page/fast）")
    post(ECAT + "/motor/start",
         {"period_ms": 0.25, "dc_cycle_us": 250, "dc_shift_us": 0}, timeout=30)
    time.sleep(0.25)

    cur_mode, src = observe_config_mode(args.node)
    print(f"基线 CONFIG mode node={args.node}: {cur_mode} (via {src})")
    print(f"代理={args.proxy}  模式对={SAFE_TOGGLE}  每次在两者间切换，目标速度恒 0")

    all_rows: dict[str, list] = {}
    for path in paths:
        print(f"\n======== path={path} proxy={args.proxy} node={args.node} ch={args.ch} ========")
        rows = []
        for i in range(args.trials):
            # 每轮现读基线再选「与基线不同」的 mode
            cur_mode, _src = observe_config_mode(args.node)
            if args.proxy == "mode":
                if cur_mode == SAFE_TOGGLE[0]:
                    next_mode = SAFE_TOGGLE[1]
                elif cur_mode == SAFE_TOGGLE[1]:
                    next_mode = SAFE_TOGGLE[0]
                else:
                    next_mode = SAFE_TOGGLE[0]
            else:
                next_mode = 2
            print(f"--- trial {i+1}/{args.trials} base_mode={cur_mode} → send mode={next_mode} ---")
            r = run_once(path, args.node, args.ch, args.proxy, args.timeout, next_mode)
            rows.append(r)
            if not r.get("resp_ok") or not r.get("sig_ok", True):
                print("  FAIL/NOSIG", {k: r.get(k) for k in
                      ("resp_ok", "sig_ok", "base_mode", "expect_mode", "resp_path", "sig_detail")})
            else:
                print(
                    f"  via={r['resp_path']} mode {r.get('base_mode')}→{r.get('expect_mode')} "
                    f"sent={r['sent_ms']:.2f}ms "
                    f"click→sent={r['click_to_sent']:.2f} "
                    f"click→can={r['click_to_can'] if r['click_to_can'] is not None else 'n/a'} "
                    f"click→sig={r['click_to_sig'] if r['click_to_sig'] is not None else 'n/a'} "
                    f"sent→can={r['sent_to_can'] if r['sent_to_can'] is not None else 'n/a'} "
                    f"sent→sig={r['sent_to_sig'] if r['sent_to_sig'] is not None else 'n/a'} "
                    f"can→sig={r['can_to_sig'] if r['can_to_sig'] is not None else 'n/a'}"
                )
                if r.get("can_detail"):
                    print("   CAN", r["can_detail"])
                if r.get("sig_detail"):
                    print("   SIG", r["sig_detail"])
            time.sleep(0.35)
        all_rows[path] = rows
        good = [r for r in rows if r.get("ok") and r.get("resp_ok") and r.get("sig_ok", True)]
        print(f"\n=== {path} 汇总 ({len(good)}/{len(rows)}) ===")
        for key in ("sent_ms", "click_to_sent", "click_to_can", "click_to_sig",
                    "sent_to_can", "sent_to_sig", "can_to_sig"):
            print(f"  {key:14s} {summarize(good, key)}")

    print("\n======== 口径 ========")
    if args.proxy == "mode":
        print("主信号: CONFIG Target_Control_Mode (0x000N0101 B0) 变为期望值")
        print("        = 电机已接收并回报新控制模式（不依赖转动/角度阈值）")
    else:
        print("主信号: STATUS rtAngle/rtVelocity 阈值（旧口径，启动惯性会漏测）")
    print("t_click 不含真实鼠标与 pywebview hop；观测分辨率受 motor/state/rx 轮询限制。")
    print("fast 路径为 GUI 桥同构 Python 调用。")
    return 0


if __name__ == "__main__":
    sys.exit(main())