#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""PDO 周期极限测速探针（真机复用工具）。

对**正在运行**的 ecat_server 逐级下发热调周期 /api/motor/period，量每一档的
实测周期率、分段耗时(cyc_us)、wkc、自旋状态和 HTTP 读快照延迟。热调不停循环、
喂狗不断流，所以跑这个脚本不会把从站弄掉 OP（区别于反复打 motor/start）。

前提：ecat_server 已启动（端口 8090 在听）。**循环没跑时脚本会自动 open+start**，
      所以断电重启板子后，直接敲本脚本一条命令即可，不必先开电机页或手敲 start。
      若自动 start 失败(典型=SSC 邮箱楔死)，脚本会把原因如实打出来并退出。

用法(在本目录的 venv 下跑，系统 python 也行——只用标准库)：
    venv\\Scripts\\python.exe rate_probe.py
    venv\\Scripts\\python.exe rate_probe.py --periods 0.5,0.25,0.125 --sample 2
    venv\\Scripts\\python.exe rate_probe.py --restore 0.25     # 测完留在 4kHz 档
    venv\\Scripts\\python.exe rate_probe.py --no-autostart     # 循环没跑时不自动拉起
默认从快照读当前周期，测完恢复到那个值。--substr 指定自动 open 的网卡子串(默认 I226)。

读数怎么解读：
  * 真机一送一回(send+receive)实测 ≈165us，这就是物理墙：比 165us 更快的
    档位"实测率"必然低于目标，不是脚本或设置的错，cyc_us.total.p50 会贴住
    周期值甚至跑超。保底 4kHz(250us) 在物理预算内。
  * measured 明显低于 target 且 cyc_us 远小于周期 → 是节拍问题（自旋没生效？
    看 spin 列），不是总线问题。
  * 自旋占满一个逻辑核是预期行为（脚本进程不占，占的是 ecat_server 的
    pdo-cycle 线程，正常应钉在 P 核）。
"""
import argparse
import json
import statistics
import sys
import time
import urllib.request

DEFAULT_URL = "http://127.0.0.1:8090"


def post(url, path, body, timeout=5.0):
    req = urllib.request.Request(
        url + path, data=json.dumps(body).encode("utf-8"),
        headers={"Content-Type": "application/json"}, method="POST")
    with urllib.request.urlopen(req, timeout=timeout) as r:
        return json.loads(r.read().decode("utf-8"))


def sample(url, seconds):
    """在 seconds 窗口里轮询快照：返回(实测Hz, HTTP延迟p50/p90(ms), 最后快照)。

    实测时刻必须和 cycles 读数**同取同舍**：旧版分子取最后一次轮询的 cycles、
    分母却在循环退出后才算 —— 退出前还有一次 sleep(0.1)，dt 比 cycles 窗口多出
    0.1s，Hz 被系统性低估 3~5% 且与周期无关(2026-09-04 实踩：把 99.2% 报成 96%，
    一度被误读成"HTTP 负载吃掉 3 个点")。
    """
    lat = []
    first = last = None          # (读数时刻, cycles) —— 同一时刻成对取
    t_end = time.perf_counter() + seconds
    last_snap = None
    while time.perf_counter() < t_end:
        t0 = time.perf_counter()
        snap = post(url, "/api/motor/state", {})
        t1 = time.perf_counter()
        lat.append((t1 - t0) * 1000.0)
        if not snap.get("ok"):
            raise RuntimeError("motor/state 报错(可能掉出 OP): %s" % snap.get("log"))
        c = snap.get("cycles", 0)
        if first is None:
            first = (t1, c)
        last = (t1, c)
        last_snap = snap
        time.sleep(0.1)
    dt = last[0] - first[0]
    hz = (last[1] - first[1]) / dt if dt > 0 else 0.0
    lat.sort()
    return hz, (lat[len(lat) // 2], lat[int(len(lat) * 0.9)]), last_snap


def main():
    ap = argparse.ArgumentParser(description="EtherCAT PDO 周期极限测速")
    ap.add_argument("--url", default=DEFAULT_URL)
    ap.add_argument("--periods", default="1,0.5,0.25,0.125,0.0625",
                    help="逗号分隔的周期(ms)档位，允许亚毫秒")
    ap.add_argument("--sample", type=float, default=2.0, help="每档采样秒数")
    ap.add_argument("--restore", type=float, default=None,
                    help="测完恢复到的周期(ms)；缺省=脚本启动前的原值")
    ap.add_argument("--substr", default="I226",
                    help="网卡匹配子串(自动 open 时用)，默认 I226")
    ap.add_argument("--no-autostart", action="store_true",
                    help="循环没跑时不要自动 open+start，直接报错退出")
    a = ap.parse_args()

    st = post(a.url, "/api/motor/state", {})
    if not st.get("ok"):
        # 循环没跑：断电重启后这是常态(服务活着但没人 open/start)。自动补上，
        # 让"断电后只敲 rate_probe 一条命令"成立 —— 不必先开电机页或手敲 start。
        if a.no_autostart:
            print("后台 PDO 循环没在跑(--no-autostart)：", st.get("log"))
            return 1
        print("循环没跑，尝试自动 open + motor/start …")
        try:
            op = post(a.url, "/api/open", {"substr": a.substr}, timeout=25.0)
            if not op.get("ok"):
                print("  open 失败:", op.get("log"))
                return 1
            ms0 = post(a.url, "/api/motor/start", {"period_ms": 4}, timeout=40.0)
            if not ms0.get("ok"):
                # 这里最典型的失败就是 SSC 邮箱楔死(config_map 报 MailboxError)，
                # 如实把原因打出来，别让人以为脚本坏了。
                print("  motor/start 失败(多半是邮箱楔死，需断电重启板子):")
                for line in (ms0.get("log") or []):
                    print("   ", line[:120])
                return 1
        except OSError as e:
            print("  连不上服务(没起? 端口对吗?):", e)
            return 1
        st = post(a.url, "/api/motor/state", {})
        if not st.get("ok"):
            print("  start 后仍读不到快照:", st.get("log"))
            return 1
        print("  已自动进 OP 并起循环。")
    orig = st.get("period_ms", 4)

    print(f"服务 {a.url} · 当前周期 {orig}ms · 从站 wkc={st.get('wkc')}/{st.get('expected_wkc')}")
    print(f"{'周期':>9} {'目标Hz':>7} {'实测Hz':>8} {'达成率':>6} "
          f"{'cyc_us p50/p90':>15} {'HTTP p50':>8} spin 电机")
    for ms in [float(x) for x in a.periods.split(",")]:
        r = post(a.url, "/api/motor/period", {"period_ms": ms})
        if not r.get("ok"):
            print("  档位 %s 被拒绝: %s" % (ms, r.get("log")))
            continue
        time.sleep(0.3)                       # 换周期后先让统计攒几个样本
        try:
            hz, (p50, p90), snap = sample(a.url, a.sample)
        except (RuntimeError, OSError) as e:
            print(f"  {ms}ms 档采样失败: {e}")
            break
        cyc = (snap.get("cyc_us") or {}).get("total") or {}
        rate = hz / (1000.0 / ms) * 100.0
        label = f"{ms*1000:g}us" if ms < 1 else f"{ms:g}ms"
        print(f"{label:>9} {1000.0/ms:>7.0f} {hz:>8.0f} {rate:>5.0f}% "
              f"{str(cyc.get('p50'))+' / '+str(cyc.get('p90')):>15} "
              f"{p50:>6.2f}ms  {str(snap.get('spin'))[0]}  {len(snap.get('motors') or [])}")
        if snap.get("wkc") != snap.get("expected_wkc"):
            print("  !! wkc 不达预期，从站可能在丢帧，停止继续加压")
            break

    restore = a.restore if a.restore is not None else orig
    r = post(a.url, "/api/motor/period", {"period_ms": restore})
    print("恢复:", r.get("log"))
    print("提示: 真机总线一送一回 ≈165us —— 比它快的档位达成率上不去是物理墙，"
          "不是配置错；保底 4kHz(250us) 在预算内。")
    return 0


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