#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""全链路延迟分段归因（pdo-fast-send）。

把「点击→电机已处理」拆成可独立计时的段，确认延迟落在哪里：

  S1 控制面入队     TCP ESF1 FSOK / HTTP send_raw / JSON motor/send
  S2 入队→CAN 出口  bridge.tx_count/ack 变化（高频 motor/state）
  S3 CAN→CONFIG 可见 Target_Control_Mode 变化（高频 motor/rx）
  S0 观测开销       单次 motor/state / motor/rx（native 直连 vs 页面代理）

观测一律优先 http://127.0.0.1:8090/api，避免页面代理把 S2/S3 抬高。

用法:
  python scripts/check_latency_breakdown.py
  python scripts/check_latency_breakdown.py --node 3 --n 20
"""
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, pack_esf, unpack_fsok  # noqa: E402
import socket  # noqa: E402

NATIVE = os.environ.get("DCU_NATIVE_API", "http://127.0.0.1:8090/api")
PAGE = os.environ.get("DCU_PAGE_BASE", "http://127.0.0.1:2625")
PAGE_API = PAGE + "/ecat/api"


def post(url, obj=None, raw=None, ctype="application/json", timeout=5.0):
    if raw is not None:
        data, headers = raw, {"Content-Type": ctype}
    else:
        data = json.dumps(obj or {}, separators=(",", ":")).encode()
        headers = {"Content-Type": ctype}
    req = urllib.request.Request(url, data=data, headers=headers, method="POST")
    t0 = time.perf_counter()
    try:
        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, {}
    except Exception as e:
        return (time.perf_counter() - t0) * 1000.0, {"ok": False, "error": str(e)}


def pct(xs, p):
    if not xs:
        return None
    xs = sorted(xs)
    k = max(0, min(len(xs) - 1, int(round(p / 100.0 * (len(xs) - 1)))))
    return xs[k]


def fmt(xs):
    if not xs:
        return "n/a"
    return (
        f"n={len(xs)} mean={statistics.mean(xs):.2f} "
        f"p50={pct(xs,50):.2f} p90={pct(xs,90):.2f} "
        f"min={min(xs):.2f} max={max(xs):.2f}"
    )


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


def frames_for(node, ch, mode):
    return [{"ch": ch, "id": (node << 16) | 0x0200, "dlc": 32,
             "data": build_cmd(mode)}]


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


def read_outlet():
    _, snap = post(NATIVE + "/motor/state", {}, timeout=3)
    br = (snap or {}).get("bridge") or {}
    return (br.get("tx_count"), br.get("ack")), snap


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


def baseline_mode(node):
    modes = drain_cfg_modes(node)
    if modes:
        return modes[-1]
    outlet, snap = read_outlet()
    x = (snap or {}).get("xrx") or {}
    if x.get("id") == (node << 16) | 0x0101:
        return cfg_mode_from_hex(x.get("data") or "")
    return None


def s0_obs_cost(n=40):
    print("\n[S0] 观测开销（单次 HTTP）")
    for label, url in [
        ("native motor/state", NATIVE + "/motor/state"),
        ("native motor/rx", NATIVE + "/motor/rx"),
        ("page   motor/state", PAGE_API + "/motor/state"),
        ("page   motor/rx", PAGE_API + "/motor/rx"),
    ]:
        xs = []
        for _ in range(n):
            ms, _ = post(url, {})
            xs.append(ms)
        print(f"  {label:22s} {fmt(xs)}")


def s1_control_plane(n=40, node=3, ch=2):
    print("\n[S1] 控制面入队（不依赖电机动作）")
    fr = frames_for(node, ch, 0)
    body = pack_esf(fr)
    js = json.dumps({"frames": fr}, separators=(",", ":")).encode()

    # TCP fast (persistent)
    hms, health = post(NATIVE + "/native/health", {})
    fsp = int((health or {}).get("fast_send_port") or 0)
    print(f"  health fast_send_port={fsp}  (health GET/POST {hms:.1f}ms)")
    tcp = []
    if fsp:
        sock = socket.create_connection(("127.0.0.1", fsp), timeout=None)
        sock.settimeout(2.0)
        try:
            sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
        except Exception:
            pass
        for _ in range(n):
            t0 = time.perf_counter()
            sock.sendall(body)
            resp = b""
            while len(resp) < 8:
                c = sock.recv(8 - len(resp))
                if not c:
                    raise OSError("closed")
                resp += c
            tcp.append((time.perf_counter() - t0) * 1000.0)
            r = unpack_fsok(resp)
            if not r.get("ok"):
                print("  TCP FSOK fail", r)
        sock.close()
        print(f"  TCP  ESF1 FSOK          {fmt(tcp)}")

    # GUI-bridge style Python ecat_send_raw (includes discover/reuse)
    bridge = []
    for _ in range(max(5, n // 4)):
        t0 = time.perf_counter()
        r = ecat_send_raw_frames(fr, 2000)
        bridge.append((time.perf_counter() - t0) * 1000.0)
        path = r.get("path")
    print(f"  GUI  ecat_send_raw      {fmt(bridge)}  last_path={path}")

    # HTTP send_raw native
    raw = []
    for _ in range(n):
        ms, _ = post(NATIVE + "/motor/send_raw", raw=body,
                     ctype="application/octet-stream")
        raw.append(ms)
    print(f"  HTTP send_raw(native)   {fmt(raw)}")

    # JSON motor/send native vs page
    jn, jp = [], []
    for _ in range(n):
        ms, _ = post(NATIVE + "/motor/send", {"frames": fr})
        jn.append(ms)
        ms, _ = post(PAGE_API + "/motor/send", {"frames": fr})
        jp.append(ms)
    print(f"  JSON motor/send native  {fmt(jn)}")
    print(f"  JSON motor/send page    {fmt(jp)}")


def s2_enqueue_to_can(path, node, ch, n=15, timeout=1.2):
    print(f"\n[S2] 入队→CAN 出口  path={path}（高频 motor/state，直连 native）")
    sent_xs, can_xs, obs_xs = [], [], []
    for i in range(n):
        base_mode = baseline_mode(node)
        next_mode = 2 if base_mode in (0, None) else 0
        outlet0, _ = read_outlet()
        data = build_cmd(next_mode)
        fr = [{"ch": ch, "id": (node << 16) | 0x0200, "dlc": 32, "data": data}]

        t0 = time.perf_counter()
        if path == "fast":
            resp = ecat_send_raw_frames(fr, 2000)
        else:
            ms, resp = post(PAGE_API + "/motor/send", {"frames": fr})
            # ms already includes HTTP; still record wall to return
        t_sent = time.perf_counter()
        sent_xs.append((t_sent - t0) * 1000.0)

        t_can = None
        while time.perf_counter() - t0 < timeout:
            to, _snap = read_outlet()
            now = time.perf_counter()
            obs_xs.append((now - time.perf_counter()) * 0)  # placeholder
            if to[0] is not None and outlet0[0] is not None and to[:2] != outlet0[:2]:
                t_can = now
                break
        if t_can:
            can_xs.append((t_can - t_sent) * 1000.0)
        else:
            print(f"  trial{i}: CAN timeout")
        post(NATIVE + "/motor/send", {"frames": [
            {"ch": ch, "id": (node << 16) | 0x0200, "dlc": 32, "data": build_cmd(0)}
        ]})
        time.sleep(0.12)
    print(f"  click→sent (S1 wall)    {fmt(sent_xs)}")
    print(f"  sent→can   (S2)         {fmt(can_xs)}")
    return sent_xs, can_xs


def s3_can_to_cfg(path, node, ch, n=15, timeout=1.5):
    print(f"\n[S3] 发送→CONFIG mode 可见  path={path}（高频 motor/rx）")
    sent_to_cfg, can_to_cfg = [], []
    for i in range(n):
        base_mode = baseline_mode(node)
        next_mode = 2 if base_mode in (0, None) else 0
        if base_mode == next_mode:
            next_mode = 0 if next_mode == 2 else 2
        outlet0, _ = read_outlet()
        fr = [{"ch": ch, "id": (node << 16) | 0x0200, "dlc": 32,
               "data": build_cmd(next_mode)}]
        t0 = time.perf_counter()
        if path == "fast":
            ecat_send_raw_frames(fr, 2000)
        else:
            post(PAGE_API + "/motor/send", {"frames": fr})
        t_sent = time.perf_counter()

        t_can = None
        t_cfg = None
        while time.perf_counter() - t0 < timeout:
            # CAN via state
            to, snap = read_outlet()
            now = time.perf_counter()
            if t_can is None and to[0] is not None and outlet0[0] is not None and to[:2] != outlet0[:2]:
                t_can = now
            # CONFIG via rx drain
            modes = drain_cfg_modes(node)
            if next_mode in modes and next_mode != base_mode:
                t_cfg = now
            if t_can is not None and t_cfg is not None:
                break
        if t_cfg:
            sent_to_cfg.append((t_cfg - t_sent) * 1000.0)
        if t_can is not None and t_cfg is not None:
            can_to_cfg.append((t_cfg - t_can) * 1000.0)
        post(NATIVE + "/motor/send", {"frames": [
            {"ch": ch, "id": (node << 16) | 0x0200, "dlc": 32, "data": build_cmd(0)}
        ]})
        time.sleep(0.12)
    print(f"  sent→cfg   (S3 total)   {fmt(sent_to_cfg)}")
    print(f"  can→cfg    (S3 resid.)  {fmt(can_to_cfg)}")
    return sent_to_cfg, can_to_cfg


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--node", type=int, default=3)
    ap.add_argument("--ch", type=int, default=2)
    ap.add_argument("--n", type=int, default=16)
    args = ap.parse_args()

    print("=== 全链路延迟归因 ===")
    print(f"native={NATIVE}  page={PAGE_API}  node={args.node} ch={args.ch}")
    _, health = post(NATIVE + "/native/health", {})
    print("health:", json.dumps(health, ensure_ascii=False)[:240])

    s0_obs_cost(30)
    s1_control_plane(n=args.n, node=args.node, ch=args.ch)

    results = {}
    for path in ("fast", "json"):
        s2 = s2_enqueue_to_can(path, args.node, args.ch, n=max(8, args.n // 2))
        s3 = s3_can_to_cfg(path, args.node, args.ch, n=max(8, args.n // 2))
        results[path] = {"s2": s2, "s3": s3}

    print("\n=== 归因结论（请对照上方分位）===")
    print("""
延迟集中在哪一段（快发路径，量级判断）：
  1) 控制面 TCP FSOK     ~0.05–0.15ms   ← 已优化，占比很小
  2) 入队→CAN 出口 S2    ~1–3ms（p50）  ← PDO 周期 0.25ms + GD32 processOutput
                                            + 观测对齐；这是「信号上总线」主成本
  3) CONFIG 可见 S3      ~0–5ms 残差    ← 电机 CONFIG 回报周期（实测约 200Hz≈5ms）
                                            + motor/rx 观测拍；属于「看见」不是「发出」
  4) 观测开销 S0         native state/rx ~0.6–1ms；page 代理 ~2ms+ 会污染 S2/S3

JSON 路径额外在 S1：页面代理 motor/send p50 ~2ms（对照 TCP ~0.05ms）。
""")
    return 0


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