#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""测量「点击发送」到「电机 STATUS 显示开始运动」的端到端延迟。

口径说明（务必读）：
  t_click  = 脚本在发起 motor/send 之前的时间戳
             ≈ 网页点击处理完、即将 fetch 的时刻（未含浏览器主线程/桥调用开销）
  t_http   = motor/send HTTP 返回（控制面入队完成）
  t_can    = GD32 bridge.tx_count/ack 变化（EtherCAT 出口，命令已进 CAN）
  t_motion = 电机 STATUS 中 rtAngle/rtVelocity 相对基线发生可测变化
             （软件可见的「开始动」代理，不是编码器轴端绝对时刻）

用法:
  python scripts/measure_click_to_motion.py
  python scripts/measure_click_to_motion.py --node 3 --ch 2 --vel 40 --timeout 3
"""
from __future__ import annotations

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

PAGE = os.environ.get("DCU_PAGE_BASE", "http://127.0.0.1:2625")
ECAT = PAGE + "/ecat/api"


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


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  # NaN → 0

    return {
        "rtAngle": f32(13),
        "rtVelocity": f32(17),
        "rtCurrent": f32(21),
    }


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


def build_cmd(mode, voltage, current, velocity, angle, 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_cmd(ch, node, data_hex):
    return post(ECAT + "/motor/send", {
        "frames": [{"ch": ch, "id": (node << 16) | 0x0200, "dlc": 32, "data": data_hex}]
    }, 6)


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--node", type=int, default=3)
    ap.add_argument("--ch", type=int, default=2)
    ap.add_argument("--vel", type=float, default=40.0)
    ap.add_argument("--timeout", type=float, default=3.0)
    args = ap.parse_args()
    node, ch = args.node, args.ch

    post(ECAT + "/motor/start",
         {"period_ms": 0.25, "dc_cycle_us": 250, "dc_shift_us": 0, "force_reopen": True}, 30)
    time.sleep(0.3)

    base, outlet0, _ = motor_sample(node, ch)
    print(f"基线 STATUS node={node} ch={ch}: {base}")
    print(f"基线 GD32 outlet tx/ack: {outlet0}")
    if not base:
        print("FAIL: 该节点无 STATUS（请确认 EtherCAT 在线且节点有回报）")
        return 1

    ang0, vel0 = base["rtAngle"], base["rtVelocity"]
    data = build_cmd(mode=2, voltage=24.0, current=0.0, velocity=args.vel, angle=0.0)
    print(f"将发送 CMD mode=Velocity v={args.vel} → 稍后自动 Disable")

    t_click = time.perf_counter()
    http_ms, resp = send_cmd(ch, node, data)
    t_http = time.perf_counter()
    print(f"motor/send HTTP {http_ms:.1f}ms ok={resp.get('ok')} queued={resp.get('queued')}")

    t_can = None
    t_motion = None
    motion_detail = None
    can_detail = None
    t0 = t_click
    while time.perf_counter() - t0 < args.timeout:
        st, outlet, _ = motor_sample(node, ch)
        now = time.perf_counter()
        if t_can is None and outlet0 and outlet != outlet0 and outlet[0] is not None:
            t_can = now
            can_detail = f"outlet {outlet0} → {outlet}"
        if st:
            d_ang = abs(st["rtAngle"] - ang0)
            d_vel = abs(st["rtVelocity"] - vel0)
            # 开始运动：角度变化 >0.3° 或速度偏离基线 >5
            if t_motion is None and (d_ang > 0.3 or d_vel > 5.0):
                t_motion = now
                motion_detail = (
                    f"Δangle={d_ang:.3f}° Δvel={d_vel:.2f} "
                    f"angle {ang0:.2f}→{st['rtAngle']:.2f} vel {vel0:.2f}→{st['rtVelocity']:.2f}"
                )
        if t_motion is not None and t_can is not None:
            break
        time.sleep(0.002)

    stop = build_cmd(mode=0, voltage=24.0, current=0, velocity=0, angle=0)
    send_cmd(ch, node, stop)

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

    print("\n=== 端到端（脚本 t_click ≈ 点击后即将发请求）===")
    print(f"  t_click → HTTP 返回     : {ms(t_http - t_click):.1f} ms")
    print(f"  t_click → GD32 CAN 出口 : {ms(t_can - t_click) if t_can else '未观测到'} ms  {can_detail or ''}")
    print(f"  t_click → STATUS 开始动 : {ms(t_motion - t_click) if t_motion else '超时未观测到'} ms  {motion_detail or ''}")
    if t_can:
        print(f"  HTTP 返回 → CAN 出口    : {ms(t_can - t_http):.1f} ms")
    if t_motion and t_can:
        print(f"  CAN 出口 → STATUS 动    : {ms(t_motion - t_can):.1f} ms  ← 固件/回报段")
    if t_motion:
        print(f"\n软件口径「点击→STATUS 显示开始动」≈ {ms(t_motion - t_click):.1f} ms")
        print("（不含真实鼠标事件前的浏览器调度；机械轴端时刻会更晚）")
        return 0
    print("\nFAIL: 超时未在 STATUS 中看到运动（电机未响应或回报未更新）")
    return 1


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