#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""N2/N3 对接电机诊断发令（ECAT）。

前提：用户保证机构处于安全状态（无障碍、可运动）。
默认参数偏保守：span=200°，vel=80°/s，行程 hold 短，全程可 Ctrl+C 急停。

子命令:
  status      只读：链路 / 状态 / CONFIG
  align       下发统一 CONFIG（mode/limitV/PID）+ 双机 CMD
  openloop    开环速度对比：双机同 v，采 速度/电流/积分
  stop        巡航后同时刹停，测停车滞后与 ∫(v2+v3)
  trip        位置行程（mode3）：同向或镜像，采全程积分
  full        依次跑 align → openloop → stop → trip，并汇总 JSON
  stopnow     立即发速度=0（急停）

示例:
  python scripts/test_motor_pair_n2_n3.py status
  python scripts/test_motor_pair_n2_n3.py align --limit-v 22.0 --pid 100
  python scripts/test_motor_pair_n2_n3.py stop --vel 80 --cruise 1.2
  python scripts/test_motor_pair_n2_n3.py full --mode-mirror --span 200
  python scripts/test_motor_pair_n2_n3.py stopnow
"""
from __future__ import annotations

import argparse
import csv
import http.client
import json
import os
import struct
import sys
import threading
import time
from collections import defaultdict

HOST, PORT = "127.0.0.1", 8090
NODES = (2, 3)
LAB = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), "lab")
PERIOD = 0.25  # ms，以 motor/state 为准

_tls = threading.local()
_lock = threading.Lock()
_stop_flag = threading.Event()
_rows: list = []
_seen: dict = {}


# ───────────────────────── HTTP / ECAT ─────────────────────────

def post(path: str, body: bytes | None = None, timeout: float = 8.0) -> dict:
    if body is None:
        body = b"{}"
    c = getattr(_tls, "c", None)
    if c is None:
        c = _tls.c = http.client.HTTPConnection(HOST, PORT, timeout=timeout)
    for _ in range(2):
        try:
            c.request("POST", path, body=body,
                      headers={"Content-Type": "application/json"})
            return json.loads(c.getresponse().read().decode("utf-8", "replace"))
        except Exception:
            try:
                c.close()
            except Exception:
                pass
            _tls.c = c = http.client.HTTPConnection(HOST, PORT, timeout=timeout)
    raise RuntimeError(f"POST {path} failed")


def ensure_ecat() -> tuple[float, dict]:
    st = post("/api/motor/state")
    period = st.get("period_ms") or PERIOD
    wkc = str(st.get("wkc") or "")
    motors = st.get("motors") or []
    if wkc not in ("3", "3/3") or len(motors) < 2:
        print("ECAT 未就绪 wkc=%s motors=%d → motor/start" % (wkc, len(motors)))
        r = post("/api/motor/start",
                 json.dumps({"period_ms": 0.25, "dc_cycle_us": 250}).encode(),
                 timeout=35.0)
        print(" start:", json.dumps(r, ensure_ascii=False)[:240])
        time.sleep(0.4)
        st = post("/api/motor/state")
        period = st.get("period_ms") or PERIOD
    return period, st


# ───────────────────────── CMD 编码 ─────────────────────────

# ───────────────── 安全：已机械对接，硬联锁 ─────────────────
# 规则（不可绕过，除非命令行 --i-understand-coupled-risk 且仍受硬限位约束）：
# 1) 只允许 mode3 + 有限角度目标，两轴目标必须互为相反数（镜像）。
# 2) 禁止 mode2 开环持续转（对接后会一直拧）。
# 3) 速度只表示「走向目标的速率」，符号必须 = sign(target - current)。
# 4) 发令前若 |a2+a3| 过大（错位）→ 拒绝运动，要求人工回零。
# 5) 硬限位：|ang|、目标、单次行程超限 → 拒绝。
# 6) 运行中看门狗：一台已停（|v|<8）另一台仍 |v|>40 超过 300ms → 双机急停。
FORCE_MIRROR = True
MAX_ABS_ANGLE = 2500.0       # 编码器绝对角限（多圈保护）
MAX_STROKE = 1200.0          # 单次目标相对当前位置最大行程
MAX_ASUM_ERR = 40.0          # |a2+a3| 超过则判定错位，禁止运动
STOP_VEL_EPS = 8.0
SPIN_VEL_WARN = 40.0
ASYMMETRY_MS = 300.0


def _read_pair():
    snap = snap_motors()
    a2 = (snap.get(2) or {}).get("ang")
    v2 = (snap.get(2) or {}).get("vel")
    a3 = (snap.get(3) or {}).get("ang")
    v3 = (snap.get(3) or {}).get("vel")
    return a2, v2, a3, v3, snap


def safety_precheck_motion(target2, target3, speed, force=False):
    """运动前硬检查。返回 (ok, msg, items)。items 为带符号速度的 CMD。"""
    a2, v2, a3, v3, snap = _read_pair()
    if a2 is None or a3 is None:
        return False, "N2/N3 状态缺失，拒绝运动", None
    if abs(a2) > MAX_ABS_ANGLE or abs(a3) > MAX_ABS_ANGLE:
        return False, ("角度超硬限位 a2=%.1f a3=%.1f（限±%.0f）——请人工回位"
                       % (a2, a3, MAX_ABS_ANGLE)), None
    if abs(target2) > MAX_ABS_ANGLE or abs(target3) > MAX_ABS_ANGLE:
        return False, "目标超硬限位", None
    if abs(target2 + target3) > 1.0:
        return False, "目标必须镜像：target2+target3=0（得到 %s/%s）" % (target2, target3), None
    if abs(target2 - a2) > MAX_STROKE or abs(target3 - a3) > MAX_STROKE:
        return False, ("单次行程过大 Δ2=%.1f Δ3=%.1f（限±%.0f）"
                       % (target2 - a2, target3 - a3, MAX_STROKE)), None
    asum = a2 + a3
    if abs(asum) > MAX_ASUM_ERR and not force:
        return False, ("错位过大 a2+a3=%.1f°（a2=%.1f a3=%.1f）。"
                       "禁止电控继续跑，请先人工把机构回到对称位（建议 a2≈-a3，和≈0）再测。"
                       % (asum, a2, a3)), None
    # 方向：必须朝向各自目标
    v2s = 0.0 if abs(target2 - a2) < 0.5 else (speed if target2 > a2 else -speed)
    v3s = 0.0 if abs(target3 - a3) < 0.5 else (speed if target3 > a3 else -speed)
    # 对接刚性镜像：位移方向应相反。若当前接近对称却要求同向位移 → 拒绝
    if abs(asum) <= MAX_ASUM_ERR:
        d2, d3 = target2 - a2, target3 - a3
        if abs(d2) > 1 and abs(d3) > 1 and d2 * d3 > 0:
            return False, ("拒绝：镜像机构不允许同向位移 Δ2=%.1f Δ3=%.1f"
                           % (d2, d3)), None
        # 速度也应反号
        if abs(v2s) > 0.5 and abs(v3s) > 0.5 and v2s * v3s > 0:
            return False, "拒绝：速度同号 v2=%s v3=%s" % (v2s, v3s), None
    items = [(2, float(v2s), float(target2)), (3, float(v3s), float(target3))]
    return True, ("precheck ok a2=%.2f a3=%.2f asum=%.2f cmd=%s"
                  % (a2, a3, asum, items)), items


def _check_pair(items, allow_same=False):
    """双机指令必须：角度目标镜像；速度与目标方向一致且反号。"""
    if not FORCE_MIRROR:
        return
    d = {int(n): (float(v), float(a)) for n, v, a in items}
    if 2 not in d or 3 not in d:
        return
    v2, t2 = d[2]
    v3, t3 = d[3]
    if abs(t2 + t3) > 1.0 and (abs(t2) > 1 or abs(t3) > 1):
        raise RuntimeError("SAFETY: 角度目标必须镜像 t2=%s t3=%s" % (t2, t3))
    if abs(v2) > 0.5 and abs(v3) > 0.5 and v2 * v3 > 0:
        raise RuntimeError("SAFETY: 速度禁止同号 v2=%s v3=%s（会拧线）" % (v2, v3))
    a2, cur_v2, a3, cur_v3, _ = _read_pair()
    if a2 is not None and a3 is not None:
        if abs(a2) > MAX_ABS_ANGLE or abs(a3) > MAX_ABS_ANGLE:
            raise RuntimeError("SAFETY: 编码器超限 a2=%.1f a3=%.1f" % (a2, a3))
        if abs(v2) > 0.5 and abs(t2 - a2) > 0.5:
            if v2 * (t2 - a2) < 0:
                raise RuntimeError("SAFETY: N2 速度方向与目标相反 v=%s a=%.1f t=%.1f"
                                   % (v2, a2, t2))
        if abs(v3) > 0.5 and abs(t3 - a3) > 0.5:
            if v3 * (t3 - a3) < 0:
                raise RuntimeError("SAFETY: N3 速度方向与目标相反 v=%s a=%.1f t=%.1f"
                                   % (v3, a3, t3))


def _watchdog_stop(reason):
    print("!! WATCHDOG STOP:", reason)
    for _ in range(6):
        try:
            post("/api/motor/send", json.dumps({"frames": [
                {"ch": 2, "id": hex((2 << 16) | 0x0200), "dlc": 32,
                 "data": pack_cmd(mode=3, voltage=24, velocity=0,
                                  angle=(_read_pair()[0] or 0), pid=(100, 100, 100))},
                {"ch": 2, "id": hex((3 << 16) | 0x0200), "dlc": 32,
                 "data": pack_cmd(mode=3, voltage=24, velocity=0,
                                  angle=(_read_pair()[2] or 0), pid=(100, 100, 100))},
            ]}).encode())
        except Exception:
            pass
        time.sleep(0.03)
    a2, v2, a3, v3, _ = _read_pair()
    print("   after stop a2=%s v2=%s a3=%s v3=%s" % (a2, v2, a3, v3))


def pack_cmd(mode=3, voltage=24.0, current=0.0, velocity=100.0,
             angle=0.0, pid=(100, 100, 100)) -> str:
    """与 pages/motor.html buildCmdBytes 一致（DLC32）。"""
    b = bytearray(32)
    b[0] = int(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, int(pid[0]))
    struct.pack_into("<h", b, 17, int(pid[1]))
    struct.pack_into("<h", b, 19, int(pid[2]))
    return bytes(b).hex()


def pack_config(mode=3, limit_v=22.0, current=0.0, velocity=100.0,
                angle=0.0) -> str:
    """CONFIG 帧与 CMD 同布局（回声/写参数用）。"""
    return pack_cmd(mode=mode, voltage=limit_v, current=current,
                    velocity=velocity, angle=angle, pid=(0, 0, 0))


def burst_cmds(items, mode=3, voltage=24.0, pid=(100, 100, 100),
               allow_same_dir=False):
    """items: [(node, velocity, angle)] 一次 POST 同一 seq 沿。"""
    _check_pair(items, allow_same=allow_same_dir)
    frames = []
    for n, vel, ang in items:
        frames.append({
            "ch": 2,
            "id": hex((int(n) << 16) | 0x0200),
            "dlc": 32,
            "data": pack_cmd(mode=mode, voltage=voltage, velocity=vel,
                             angle=ang, pid=pid),
        })
    return post("/api/motor/send", json.dumps({"frames": frames}).encode())


def send_config(cfg: dict, nodes=NODES):
    """下发统一 CONFIG。cfg: {mode, limit_v, velocity, angle}"""
    frames = []
    for n in nodes:
        frames.append({
            "ch": 2,
            "id": hex((int(n) << 16) | 0x0200),
            "dlc": 32,
            "data": pack_config(mode=cfg.get("mode", 3),
                                limit_v=cfg.get("limit_v", 22.0),
                                velocity=cfg.get("velocity", 100.0),
                                angle=cfg.get("angle", 0.0)),
        })
    return post("/api/motor/send", json.dumps({"frames": frames}).encode())


# ───────────────────────── 状态 ─────────────────────────

def decode_status(raw: bytes):
    if len(raw) < 21:
        return None
    ang = struct.unpack_from("<f", raw, 13)[0]
    vel = struct.unpack_from("<f", raw, 17)[0]
    if ang != ang or vel != vel:
        return None
    d = {
        "ang": ang,
        "vel": vel,
        "pwr": struct.unpack_from("<H", raw, 11)[0] * 0.001 if len(raw) >= 13 else None,
        "ucur": struct.unpack_from("<h", raw, 4)[0] if len(raw) >= 6 else None,
        "rt_cur": struct.unpack_from("<f", raw, 21)[0] if len(raw) >= 25 else 0.0,
        "motT": struct.unpack_from("<h", raw, 2)[0] * 0.01 if len(raw) >= 4 else None,
        "b10": raw[10] if len(raw) > 10 else 0,
    }
    return d


def snap_motors() -> dict:
    out = {}
    st = post("/api/motor/state")
    for m in st.get("motors") or []:
        n = m.get("node")
        if n not in NODES:
            continue
        dec = decode_status(bytes.fromhex(m.get("data") or ""))
        if dec:
            dec["online"] = bool(m.get("online"))
            dec["seq"] = m.get("seq")
            out[n] = dec
    out["_cycles"] = st.get("cycles")
    out["_period_ms"] = st.get("period_ms")
    return out


def read_configs_from_rx() -> dict:
    cfgs = {}
    try:
        rx = post("/api/motor/rx")
    except Exception:
        return cfgs
    for f in (rx.get("frames") or rx.get("parsed") or []):
        if not isinstance(f, dict):
            continue
        fid = f.get("id", 0)
        try:
            if isinstance(fid, str):
                fid = int(fid, 0)
        except Exception:
            continue
        if (fid & 0xFFFF) != 0x0101:
            continue
        n = (fid >> 16) & 0xFF
        if n not in NODES:
            continue
        try:
            raw = bytes.fromhex(f.get("data") or "")
        except Exception:
            continue
        if len(raw) >= 21:
            cfgs[n] = {
                "mode": raw[0],
                "limit_V": struct.unpack_from("<H", raw, 1)[0] * 0.001,
                "cur": struct.unpack_from("<f", raw, 3)[0],
                "vel": struct.unpack_from("<f", raw, 7)[0],
                "ang": struct.unpack_from("<f", raw, 11)[0],
                "pid": [struct.unpack_from("<h", raw, o)[0] for o in (15, 17, 19)],
                "hex": raw[:24].hex(),
            }
    return cfgs


# ───────────────────────── 采集 ─────────────────────────

def poller():
    while not _stop_flag.is_set():
        try:
            d = post("/api/motor/state")
        except Exception:
            time.sleep(0.008)
            continue
        cyc = d.get("cycles", 0) or 0
        wall = time.time()
        for m in d.get("motors") or []:
            n = m.get("node")
            if n not in NODES:
                continue
            sq = m.get("seq", 0) or 0
            with _lock:
                if _seen.get(n) == sq:
                    continue
                _seen[n] = sq
            dec = decode_status(bytes.fromhex(m.get("data") or ""))
            if not dec:
                continue
            with _lock:
                _rows.append({
                    "wall": wall, "cyc": cyc, "node": n, "seq": sq,
                    "ang": dec["ang"], "vel": dec["vel"],
                    "rt_cur": dec["rt_cur"], "motT": dec["motT"],
                })


def start_poll(n=6):
    del _rows[:]
    _seen.clear()
    _stop_flag.clear()
    ths = [threading.Thread(target=poller, daemon=True) for _ in range(n)]
    for t in ths:
        t.start()
    return ths


def stop_poll(ths):
    _stop_flag.set()
    for t in ths:
        t.join(timeout=2)


def aligned_series():
    """返回 [(cyc,t,a2,v2,a3,v3,vsum,asum)] 以 N2 cycles 对齐。"""
    by = defaultdict(list)
    for r in _rows:
        by[r["node"]].append(r)
    for n in by:
        by[n].sort(key=lambda x: x["cyc"])
    s2, s3 = by.get(2, []), by.get(3, [])
    if not s2 or not s3:
        return []

    def interp(n, cyc):
        s = by[n]
        if cyc <= s[0]["cyc"]:
            return s[0]["ang"], s[0]["vel"]
        if cyc >= s[-1]["cyc"]:
            return s[-1]["ang"], s[-1]["vel"]
        lo, hi = 0, len(s) - 1
        while lo + 1 < hi:
            mid = (lo + hi) // 2
            if s[mid]["cyc"] <= cyc:
                lo = mid
            else:
                hi = mid
        c0, a0, v0 = s[lo]["cyc"], s[lo]["ang"], s[lo]["vel"]
        c1, a1, v1 = s[hi]["cyc"], s[hi]["ang"], s[hi]["vel"]
        if c1 == c0:
            return a1, v1
        f = (cyc - c0) / (c1 - c0)
        return a0 + (a1 - a0) * f, v0 + (v1 - v0) * f

    out = []
    for r in s2:
        a3, v3 = interp(3, r["cyc"])
        out.append((r["cyc"], r["wall"], r["ang"], r["vel"], a3, v3,
                    r["vel"] + v3, r["ang"] + a3, r["rt_cur"]))
    return out


def trapz_vel(series, period_ms, key_v2=3, key_v3=5):
    """series 对齐列表 → (I2, I3, Isum) 全程梯形积分。"""
    I2 = I3 = 0.0
    for i in range(1, len(series)):
        dt = (series[i][0] - series[i - 1][0]) * period_ms / 1000.0
        if dt < 0:
            dt = 0
        I2 += 0.5 * (series[i][key_v2] + series[i - 1][key_v2]) * dt
        I3 += 0.5 * (series[i][key_v3] + series[i - 1][key_v3]) * dt
    return I2, I3, I2 + I3


def write_series_csv(path, series, meta=None):
    with open(path, "w", newline="", encoding="utf-8") as fp:
        w = csv.writer(fp)
        w.writerow(["cycles", "wall", "a2", "v2", "a3", "v3", "vsum", "asum", "rt_cur2"])
        for row in series:
            w.writerow(row)
        if meta:
            fp.write("# meta " + json.dumps(meta, ensure_ascii=False) + "\n")


def analyze_pair(series, period_ms, label=""):
    if len(series) < 40:
        return {"label": label, "error": "too_few_samples", "n": len(series)}
    mov = [r for r in series if abs(r[3]) > 15 or abs(r[5]) > 15]
    cr = [r for r in series if abs(r[3]) > 80 and abs(r[5]) > 80]
    I2, I3, Isum = trapz_vel(series, period_ms)
    out = {
        "label": label,
        "n": len(series),
        "I2": I2, "I3": I3, "I_sum_v2_plus_v3": Isum,
        "asum_start_end": [series[0][7], series[-1][7]],
        "asum_delta": series[-1][7] - series[0][7],
        "a2_start_end": [series[0][2], series[-1][2]],
        "a3_start_end": [series[0][4], series[-1][4]],
    }
    if cr:
        out["cruise_mean_abs_v2"] = sum(abs(r[3]) for r in cr) / len(cr)
        out["cruise_mean_abs_v3"] = sum(abs(r[5]) for r in cr) / len(cr)
        out["cruise_mean_vsum"] = sum(r[6] for r in cr) / len(cr)
        out["cruise_rms_vsum"] = (sum(r[6] ** 2 for r in cr) / len(cr)) ** 0.5
    if mov:
        out["mov_mean_abs_v2"] = sum(abs(r[3]) for r in mov) / len(mov)
        out["mov_mean_abs_v3"] = sum(abs(r[5]) for r in mov) / len(mov)
        out["mov_rms_vsum"] = (sum(r[6] ** 2 for r in mov) / len(mov)) ** 0.5
        out["mov_max_abs_vsum"] = max(abs(r[6]) for r in mov)
    return out


# ───────────────────────── 连续发令 ─────────────────────────

def drive(items, duration, rate=25.0, mode=3, voltage=24.0, pid=(100, 100, 100),
          allow_same_dir=False):
    """连续发令 + 看门狗。已对接：仅 mode3 + 镜像角度目标 + 低速优先。"""
    if mode != 3:
        raise RuntimeError("SAFETY: 已对接仅允许 mode3，禁止 mode=%s" % mode)
    _check_pair(items, allow_same=allow_same_dir)
    t_end = time.perf_counter() + duration
    dt = 1.0 / max(rate, 1.0)
    n_ok = 0
    last_err = None
    spin_t0 = None
    while time.perf_counter() < t_end:
        t0 = time.perf_counter()
        try:
            burst_cmds(items, mode=mode, voltage=voltage, pid=pid,
                       allow_same_dir=allow_same_dir)
            n_ok += 1
        except Exception as e:
            last_err = str(e)
            print("  send err:", e)
            if "SAFETY" in str(e):
                _watchdog_stop(str(e))
                break
        try:
            a2, v2, a3, v3, _ = _read_pair()
            if v2 is not None and v3 is not None:
                s2, s3 = abs(v2) < STOP_VEL_EPS, abs(v3) < STOP_VEL_EPS
                spin = abs(v2) > SPIN_VEL_WARN or abs(v3) > SPIN_VEL_WARN
                if (s2 != s3) and spin:
                    if spin_t0 is None:
                        spin_t0 = time.perf_counter()
                    elif (time.perf_counter() - spin_t0) * 1000.0 >= ASYMMETRY_MS:
                        _watchdog_stop("一台已停另一台仍在转 v2=%.1f v3=%.1f" % (v2, v3))
                        break
                else:
                    spin_t0 = None
            if a2 is not None and a3 is not None and (
                    abs(a2) > MAX_ABS_ANGLE or abs(a3) > MAX_ABS_ANGLE):
                _watchdog_stop("角度超限 a2=%.1f a3=%.1f" % (a2, a3))
                break
        except Exception:
            pass
        pad = dt - (time.perf_counter() - t0)
        if pad > 0:
            time.sleep(pad)
    return n_ok, last_err


def hold_position(pid=(100, 100, 100), voltage=24.0, rate=20.0, seconds=0.4,
                  targets=None):
    snap = snap_motors()
    if targets is None:
        targets = []
        for n in NODES:
            if n in snap:
                targets.append((n, 0.0, snap[n]["ang"]))
            else:
                targets.append((n, 0.0, 0.0))
    return drive(targets, seconds, rate=rate, mode=3, voltage=voltage, pid=pid)


# ───────────────────────── 子命令 ─────────────────────────

def cmd_status(_args):
    period, st = ensure_ecat()
    print("链路 wkc=%s period=%sms cycles=%s motors=%s"
          % (st.get("wkc"), period, st.get("cycles"),
             [(m.get("node"), m.get("online"), m.get("seq"))
              for m in st.get("motors") or []]))
    print("\n--- live STATUS ---")
    snap = snap_motors()
    for n in NODES:
        d = snap.get(n)
        if not d:
            continue
        print(" N%s ang=%.3f vel=%.2f pwr=%s motT=%s b10=%s online=%s"
              % (n, d["ang"], d["vel"], d["pwr"], d["motT"], d["b10"], d["online"]))
    print("\n--- CONFIG from /api/motor/rx ---")
    cfgs = read_configs_from_rx()
    if not cfgs:
        print(" (rx 暂无 0x0101 CONFIG，可先发一帧或等电机上报)")
    for n, c in sorted(cfgs.items()):
        print(" N%s %s" % (n, c))
    return 0


def cmd_align(args):
    period, st = ensure_ecat()
    print("align: limitV=%.2f PID=%s mode=%s" % (args.limit_v, args.pid, args.mode))
    print("before CONFIG:", json.dumps(read_configs_from_rx(), ensure_ascii=False))
    r = send_config({"mode": args.mode, "limit_v": args.limit_v,
                     "velocity": args.vel, "angle": 0.0})
    print("config send:", r)
    time.sleep(0.15)
    # 用统一参数发 CMD（速度 0，保持）
    snap = snap_motors()
    items = []
    for n in NODES:
        ang = snap.get(n, {}).get("ang", 0.0)
        items.append((n, 0.0, ang))
    for _ in range(int(args.hold * 25)):
        burst_cmds(items, mode=args.mode, voltage=args.limit_v + 2,
                   pid=tuple(args.pid))
        time.sleep(0.04)
    print("after CONFIG:", json.dumps(read_configs_from_rx(), ensure_ascii=False))
    print("live:", json.dumps(
        {str(n): snap_motors().get(n) for n in NODES if snap_motors().get(n)},
        ensure_ascii=False)[:400])
    return 0


def cmd_openloop(args):
    """已对接：禁止 mode2 开环。只做短时 mode3 镜像微动（±30°）测响应。"""
    period, _ = ensure_ecat()
    print("SAFETY: 对接后禁止 open-loop mode2。改为 mode3 镜像小行程 ±20° @ vel≤50")
    speed = min(abs(args.vel), 50.0)
    span = 20.0
    a2, _, a3, _, _ = _read_pair()
    if a2 is None or a3 is None:
        print("无状态")
        return 2
    # 在当前位置附近做镜像微动：目标 = 当前 ± 小量，并强制 target2=-target3
    # 仅当当前已接近对称时允许
    if abs(a2 + a3) > MAX_ASUM_ERR:
        print("拒绝：a2+a3=%.1f 错位过大，先人工回位" % (a2 + a3))
        return 2
    t2 = span if a2 < 0 else -span
    t3 = -t2
    # 若当前位置离目标太近/太远则改用以 0 为中心
    t2, t3 = span, -span
    ok, msg, items = safety_precheck_motion(t2, t3, speed)
    print(msg)
    if not ok:
        return 2
    ths = start_poll(args.threads)
    t0 = time.time()
    drive(items, args.cruise, args.rate, mode=3, voltage=args.voltage,
          pid=tuple(args.pid))
    # 回到对称位附近 0/0 或停在当前位置
    ok2, msg2, items2 = safety_precheck_motion(0.0, 0.0, speed)
    print(msg2)
    if ok2:
        drive(items2, args.settle, args.rate, mode=3, voltage=args.voltage,
              pid=tuple(args.pid))
    t1 = time.time()
    stop_poll(ths)
    series = aligned_series()
    period = post("/api/motor/state").get("period_ms") or period
    tag = "micro_mirror"
    csv_path = os.path.join(LAB, "pair_test_%s.csv" % tag)
    meta = {"test": "micro_mirror_mode3", "span": span, "vel": speed,
            "t0": t0, "t1": t1}
    write_series_csv(csv_path, series, meta)
    an = analyze_pair(series, period, label=tag)
    an["csv"] = csv_path
    an["command"] = meta
    jpath = os.path.join(LAB, "pair_test_%s.json" % tag)
    with open(jpath, "w", encoding="utf-8") as fp:
        json.dump(an, fp, ensure_ascii=False, indent=2)
    print(json.dumps(an, ensure_ascii=False, indent=2))
    print("CSV", csv_path, "JSON", jpath)
    return 0


def cmd_stop(args):
    """镜像 mode3：短行程后停在目标，禁止开环刹停测法。"""
    period, _ = ensure_ecat()
    speed = min(abs(args.vel), 80.0)
    print("stop-test：对接机构仅允许 mode3 镜像行程后自然停靠（禁止 mode2）")
    a2, _, a3, _, _ = _read_pair()
    if a2 is None:
        return 2
    # 从对称位附近：先到 ±span，再回 0（行程用 stop.hold）
    span = min(args.span if hasattr(args, "span") else 150.0, 200.0)
    ok, msg, items = safety_precheck_motion(span, -span, speed)
    print("leg1", msg)
    if not ok:
        return 2
    ths = start_poll(args.threads)
    t0 = time.time()
    drive(items, args.cruise + 1.5, args.rate, mode=3,
          voltage=args.voltage, pid=tuple(args.pid))
    t_cmd_stop = time.time()
    ok2, msg2, items2 = safety_precheck_motion(0.0, 0.0, speed)
    print("leg2→0", msg2)
    if ok2:
        drive(items2, args.hold + 1.5, args.rate, mode=3,
              voltage=args.voltage, pid=tuple(args.pid))
    t1 = time.time()
    stop_poll(ths)
    series = aligned_series()
    period = post("/api/motor/state").get("period_ms") or period
    tag = "stop_mirror_mode3"
    csv_path = os.path.join(LAB, "pair_test_%s.csv" % tag)
    meta = {"test": "stop_mirror_mode3", "span": span, "vel": speed,
            "t0": t0, "t_mid": t_cmd_stop, "t1": t1}
    write_series_csv(csv_path, series, meta)
    an = analyze_pair(series, period, label=tag)
    an["csv"] = csv_path
    an["command"] = meta
    an["live_end"] = {str(n): snap_motors().get(n) for n in NODES if snap_motors().get(n)}
    jpath = os.path.join(LAB, "pair_test_%s.json" % tag)
    with open(jpath, "w", encoding="utf-8") as fp:
        json.dump(an, fp, ensure_ascii=False, indent=2)
    print(json.dumps(an, ensure_ascii=False, indent=2))
    print("CSV", csv_path, "JSON", jpath)
    return 0
    hold_position(seconds=0.35, pid=tuple(args.pid), voltage=args.voltage)
    ths = start_poll(args.threads)
    t0 = time.time()
    print("cruise...")
    n1, e1 = drive(cruise_items, args.cruise, args.rate, mode=2,
                   voltage=args.voltage, pid=tuple(args.pid))
    t_cmd_stop = time.time()
    print("STOP → 0")
    n2, e2 = drive(stop_items, args.hold, args.rate, mode=2,
                   voltage=args.voltage, pid=tuple(args.pid))
    t1 = time.time()
    stop_poll(ths)
    series = aligned_series()
    period = post("/api/motor/state").get("period_ms") or period
    csv_path = os.path.join(LAB, "pair_test_%s.csv" % tag)
    meta = {"test": "stop", "vel": vel, "mirror": args.mirror,
            "t0": t0, "t_stop_cmd": t_cmd_stop, "t1": t1,
            "sends_cruise": n1, "sends_stop": n2, "err": [e1, e2]}
    write_series_csv(csv_path, series, meta)

    # 停止沿分析：以发 STOP 时刻为界
    if len(series) < 20:
        print("samples too few")
        return 2
    # 找 wall 接近 t_cmd_stop 的索引
    idx_stop = 0
    for i, row in enumerate(series):
        if row[1] >= t_cmd_stop:
            idx_stop = max(0, i - 1)
            break
    pre = series[:idx_stop + 1]
    post_s = series[idx_stop:]

    def win_stats(seg):
        if len(seg) < 5:
            return None
        v2 = [r[3] for r in seg]
        v3 = [r[5] for r in seg]
        vs = [r[6] for r in seg]
        I2, I3, Isum = trapz_vel(seg, period)
        return {
            "n": len(seg),
            "mean_v2": sum(v2) / len(v2),
            "mean_v3": sum(v3) / len(v3),
            "mean_abs_v2": sum(abs(x) for x in v2) / len(v2),
            "mean_abs_v3": sum(abs(x) for x in v3) / len(v3),
            "rms_vsum": (sum(x * x for x in vs) / len(vs)) ** 0.5,
            "max_abs_vsum": max(abs(x) for x in vs),
            "I2": I2, "I3": I3, "Isum": Isum,
        }

    # 各轴 |v| 穿越 20 的时刻
    def cross(seg, node_i, thr=20.0, falling=True):
        vi = 3 if node_i == 2 else 5
        for i in range(1, len(seg)):
            v0, v1 = abs(seg[i - 1][vi]), abs(seg[i][vi])
            if falling and v0 >= thr > v1:
                return seg[i - 1][0] + (v0 - thr) / (v0 - v1 + 1e-12) * (seg[i][0] - seg[i - 1][0])
            if (not falling) and v0 < thr <= v1:
                return seg[i - 1][0] + (thr - v0) / (v1 - v0 + 1e-12) * (seg[i][0] - seg[i - 1][0])
        return None

    c2 = cross(post_s, 2)
    c3 = cross(post_s, 3)
    off_delta_ms = None
    if c2 is not None and c3 is not None:
        off_delta_ms = (c3 - c2) * period

    an = analyze_pair(series, period, label=tag)
    an.update({
        "csv": csv_path,
        "command": meta,
        "pre_stop": win_stats(pre),
        "post_stop": win_stats(post_s),
        "off_cross20_ms_n3_minus_n2": off_delta_ms,
        "verdict_hint": {
            "cruise": "比 pre_stop 的 mean_abs_v2/v3 谁大",
            "stop": "比 post_stop 的 max_abs_vsum 与 off_delta_ms；Δ>0 表示 N3 晚停",
            "integral": "post_stop.Isum 绝对值 = 停车段被顶开的积分",
        },
    })
    jpath = os.path.join(LAB, "pair_test_%s.json" % tag)
    with open(jpath, "w", encoding="utf-8") as fp:
        json.dump(an, fp, ensure_ascii=False, indent=2)
    print(json.dumps(an, ensure_ascii=False, indent=2))
    print("CSV", csv_path, "JSON", jpath)
    return 0


def cmd_trip(args):
    """mode3 镜像行程：n2=+span, n3=-span（已对接强制镜像）。"""
    period, _ = ensure_ecat()
    span = abs(args.span)
    pos = [(2, span), (3, -span)]
    zero = [(2, 0.0), (3, 0.0)]
    tag = "trip_mirror"
    print("trip %s span=±%.1f vel=%.1f hold=%.2f（强制镜像）"
          % (tag, span, args.vel, args.hold))
    snap = snap_motors()
    a2 = snap.get(2, {}).get("ang", 0.0)
    # 选择更远的端点作为第一目标
    if abs(a2 - span) < abs(a2 - 0) and abs(a2) > span * 0.5:
        first, second = zero, pos
    elif abs(a2) < span * 0.5:
        first, second = pos, zero
    else:
        first, second = zero, pos

    ths = start_poll(args.threads)
    t0 = time.time()
    print("leg1 →", first)
    snap = snap_motors()
    t2t, t3t = first[0][1], first[1][1]
    drive(_mirror_cmd_items(t2t, t3t, abs(args.vel), snap), args.hold,
          args.rate, mode=3, voltage=args.voltage, pid=tuple(args.pid))
    t_mid = time.time()
    print("leg2 →", second)
    snap = snap_motors()
    t2t, t3t = second[0][1], second[1][1]
    drive(_mirror_cmd_items(t2t, t3t, abs(args.vel), snap), args.hold,
          args.rate, mode=3, voltage=args.voltage, pid=tuple(args.pid))
    t1 = time.time()
    stop_poll(ths)
    series = aligned_series()
    period = post("/api/motor/state").get("period_ms") or period
    csv_path = os.path.join(LAB, "pair_test_%s.csv" % tag)
    meta = {"test": "trip_mirror", "span": span, "vel": args.vel,
            "t0": t0, "t_mid": t_mid, "t1": t1, "coupled": True}
    write_series_csv(csv_path, series, meta)
    an = analyze_pair(series, period, label=tag)
    an["csv"] = csv_path
    an["command"] = meta
    an["live_end"] = {str(n): snap_motors().get(n) for n in NODES if snap_motors().get(n)}
    jpath = os.path.join(LAB, "pair_test_%s.json" % tag)
    with open(jpath, "w", encoding="utf-8") as fp:
        json.dump(an, fp, ensure_ascii=False, indent=2)
    print(json.dumps(an, ensure_ascii=False, indent=2))
    print("CSV", csv_path, "JSON", jpath)
    return 0


def _mirror_cmd_items(target2, target3, speed, snap=None):
    """仅通过 safety_precheck 生成 CMD；禁止把速度强行改成相反符号。"""
    ok, msg, items = safety_precheck_motion(target2, target3, abs(speed))
    print("  precheck:", msg)
    if not ok:
        raise RuntimeError("SAFETY: " + msg)
    return items


def cmd_lowspeed(args):
    """低速镜像试控：先只读检查，再极小行程。错位大则拒绝一切运动。"""
    period, st = ensure_ecat()
    a2, v2, a3, v3, snap = _read_pair()
    asum = None
    if a2 is not None and a3 is not None:
        asum = a2 + a3
    print("\n======== 低速试控前置检查 ========")
    print("a2=%s a3=%s a2+a3=%s  v2=%s v3=%s" % (a2, a3, asum, v2, v3))
    print("硬限位: |ang|≤%.0f  |a2+a3|≤%.0f  禁止 mode2  禁止同向" %
          (MAX_ABS_ANGLE, MAX_ASUM_ERR))
    if a2 is None or a3 is None:
        print("结论: 状态缺失 → 不发运动指令")
        return 2
    if abs(a2) > MAX_ABS_ANGLE or abs(a3) > MAX_ABS_ANGLE:
        print("结论: 编码器超限 → 不发运动指令。请人工把机构回到对称位。")
        return 2
    if asum is not None and abs(asum) > MAX_ASUM_ERR:
        print("结论: 错位 a2+a3=%.1f° 过大 → 拒绝电控运动。" % asum)
        print("请先人工盘车/松耦合，使 a2≈-a3（和接近 0），再跑 lows peed。")
        return 2
    speed = min(abs(args.vel), 20.0)   # 低速硬顶 20°/s
    span = min(abs(args.span), 15.0)   # 小行程硬顶 15°
    t2, t3 = span, -span
    if abs(t2 - a2) > 3 * span:
        # 当前离目标太远则以当前对称附近为中心：目标 = round 到对称
        t2 = span if a2 < 0 else -span
        t3 = -t2
    ok, msg, items = safety_precheck_motion(t2, t3, speed)
    print("试控目标 ±%.1f° @ %.1f°/s" % (span, speed))
    print(msg)
    if not ok:
        print("结论: precheck 失败，不发令")
        return 2
    print("发令 items:", items)
    ths = start_poll(args.threads)
    t0 = time.time()
    n_ok, err = drive(items, max(args.hold, 2.0), rate=10.0, mode=3,
                      voltage=args.voltage, pid=tuple(args.pid))
    print("行程发送完成 n_ok=%s err=%s" % (n_ok, err))
    # 回 0（若仍安全）
    ok2, msg2, items2 = safety_precheck_motion(0.0, 0.0, speed)
    print("回零:", msg2)
    if ok2:
        drive(items2, max(args.hold, 2.0), rate=10.0, mode=3,
              voltage=args.voltage, pid=tuple(args.pid))
    else:
        # 保持当前位置
        a2, _, a3, _, _ = _read_pair()
        if a2 is not None and a3 is not None:
            try:
                drive([(2, 0.0, a2), (3, 0.0, a3)], 0.6, rate=10.0, mode=3,
                      voltage=args.voltage, pid=tuple(args.pid))
            except Exception as e:
                print("hold err", e)
    t1 = time.time()
    stop_poll(ths)
    a2e, v2e, a3e, v3e, _ = _read_pair()
    print("结束: a2=%s v2=%s a3=%s v3=%s asum=%s" %
          (a2e, v2e, a3e, v3e, (None if a2e is None or a3e is None else a2e + a3e)))
    series = aligned_series()
    period = post("/api/motor/state").get("period_ms") or period
    csv_path = os.path.join(LAB, "pair_test_lowspeed.csv")
    meta = {"test": "lowspeed", "vel": speed, "span": span,
            "t0": t0, "t1": t1, "items": items}
    write_series_csv(csv_path, series, meta)
    an = analyze_pair(series, period, label="lowspeed")
    an["csv"] = csv_path
    an["command"] = meta
    an["end"] = {"a2": a2e, "v2": v2e, "a3": a3e, "v3": v3e,
                 "asum": None if a2e is None or a3e is None else a2e + a3e}
    jpath = os.path.join(LAB, "pair_test_lowspeed.json")
    with open(jpath, "w", encoding="utf-8") as fp:
        json.dump(an, fp, ensure_ascii=False, indent=2)
    print(json.dumps(an, ensure_ascii=False, indent=2)[:800])
    print("CSV", csv_path, "JSON", jpath)
    return 0


def cmd_fairtrip(args):
    """公平行程：先双机 mode3 停到 0（镜像速度），再同时去 +span/-span。"""
    period, _ = ensure_ecat()
    span = abs(args.span)
    print("fairtrip: settle@0 → mirror ±%.0f @ vel=%.1f hold=%.2f（强制镜像）"
          % (span, args.vel, args.hold))

    ths = start_poll(args.threads)
    t0 = time.time()
    print("1) 同步到 0（速度按镜像指向 0）")
    items0 = _mirror_cmd_items(0.0, 0.0, abs(args.vel))
    print("   cmd", items0)
    drive(items0, args.settle,
          args.rate, mode=3, voltage=args.voltage, pid=tuple(args.pid))
    snap = snap_motors()
    print("   at0:", {n: (snap.get(n) or {}).get("ang") for n in NODES})
    t_go = time.time()
    print("2) 同时镜像 ±span")
    items1 = _mirror_cmd_items(span, -span, abs(args.vel), snap)
    print("   cmd", items1)
    drive(items1, args.hold,
          args.rate, mode=3, voltage=args.voltage, pid=tuple(args.pid))
    t1 = time.time()
    stop_poll(ths)
    snap = snap_motors()
    print("   end:", {n: (snap.get(n) or {}).get("ang") for n in NODES},
          {n: (snap.get(n) or {}).get("vel") for n in NODES})

    series = aligned_series()
    period = post("/api/motor/state").get("period_ms") or period
    csv_path = os.path.join(LAB, "pair_test_fairtrip.csv")
    meta = {"test": "fairtrip", "span": span, "vel": args.vel,
            "t0": t0, "t_go": t_go, "t1": t1}
    write_series_csv(csv_path, series, meta)

    # 只分析 t_go 之后的行程
    seg = [r for r in series if r[1] >= t_go]
    an = analyze_pair(seg, period, label="fairtrip")
    an["csv"] = csv_path
    an["command"] = meta
    an["live_end"] = {str(n): snap.get(n) for n in NODES if snap.get(n)}

    # 到达时刻：|ang - target| < 3 且 |v| < 10
    def arrive(node, target):
        vi_ang = 2 if node == 2 else 4
        vi_vel = 3 if node == 5 else 5
        vi_vel = 3 if node == 2 else 5
        for r in seg:
            if abs(r[vi_ang] - target) < 3.0 and abs(r[vi_vel]) < 10.0:
                return r[1]
        return None
    t2 = arrive(2, span)
    t3 = arrive(3, -span)
    an["arrive_wall"] = {"n2": t2, "n3": t3}
    if t2 and t3:
        an["arrive_delta_ms_n3_minus_n2"] = (t3 - t2) * 1000.0

    # 行程中段 20%-80% 的 vsum / 巡航
    if len(seg) > 50:
        w = seg[len(seg) // 5: max(len(seg) - len(seg) // 5, len(seg) // 5 + 2)]
        I2, I3, Isum = trapz_vel(w, period)
        an["mid_stroke"] = {
            "n": len(w),
            "mean_abs_v2": sum(abs(r[3]) for r in w) / len(w),
            "mean_abs_v3": sum(abs(r[5]) for r in w) / len(w),
            "mean_vsum": sum(r[6] for r in w) / len(w),
            "rms_vsum": (sum(r[6] ** 2 for r in w) / len(w)) ** 0.5,
            "max_abs_vsum": max(abs(r[6]) for r in w),
            "I2": I2, "I3": I3, "Isum": Isum,
        }
    jpath = os.path.join(LAB, "pair_test_fairtrip.json")
    with open(jpath, "w", encoding="utf-8") as fp:
        json.dump(an, fp, ensure_ascii=False, indent=2)
    print(json.dumps(an, ensure_ascii=False, indent=2))
    print("CSV", csv_path, "JSON", jpath)
    return 0


def cmd_stopnow(_args):
    print("STOP NOW")
    for _ in range(8):
        try:
            burst_cmds([(2, 0.0, 0.0), (3, 0.0, 0.0)], mode=2)
        except Exception as e:
            print(" ", e)
        time.sleep(0.03)
    print("live", json.dumps(
        {str(n): snap_motors().get(n) for n in NODES if snap_motors().get(n)},
        ensure_ascii=False)[:300])
    return 0


def cmd_full(args):
    """对接安全套件：仅镜像 openloop / stop / fairtrip（禁止同向）。"""
    results = {}
    print("\n======== FULL-MIRROR: 1/4 align ========")
    cmd_align(args)

    print("\n======== FULL-MIRROR: 2/4 openloop mirror ========")
    cmd_openloop(args)
    p_ol = os.path.join(LAB, "pair_test_openloop_mirror_vel%s.json" % abs(args.vel))
    if os.path.exists(p_ol):
        results["openloop_mirror"] = json.loads(open(p_ol, encoding="utf-8").read())

    print("\n======== FULL-MIRROR: 3/4 stop mirror ========")
    cmd_stop(args)
    p_st = os.path.join(LAB, "pair_test_stop_mirror.json")
    if os.path.exists(p_st):
        results["stop_mirror"] = json.loads(open(p_st, encoding="utf-8").read())

    print("\n======== FULL-MIRROR: 4/4 fairtrip mirror ========")
    cmd_fairtrip(args)
    p_ft = os.path.join(LAB, "pair_test_fairtrip.json")
    if os.path.exists(p_ft):
        results["fairtrip"] = json.loads(open(p_ft, encoding="utf-8").read())

    out = os.path.join(LAB, "pair_test_FULL_mirror_summary.json")
    slim = {"coupled": True, "rule": "mirror_only_v2=-v3", "results": {}}
    for k, v in results.items():
        slim["results"][k] = {kk: v.get(kk) for kk in (
            "label", "cruise_mean_abs_v2", "cruise_mean_abs_v3",
            "cruise_mean_vsum", "mov_rms_vsum", "mov_max_abs_vsum",
            "I_sum_v2_plus_v3", "asum_delta", "arrive_delta_ms_n3_minus_n2",
            "off_cross20_ms_n3_minus_n2", "mid_stroke", "pre_stop", "post_stop",
        ) if kk in v}
    with open(out, "w", encoding="utf-8") as fp:
        json.dump(slim, fp, ensure_ascii=False, indent=2)
    print(json.dumps(slim, ensure_ascii=False, indent=2))
    print("summary ->", out)
    return 0


def _who_slower(an):
    v2 = an.get("cruise_mean_abs_v2")
    v3 = an.get("cruise_mean_abs_v3")
    if not v2 or not v3:
        return None
    if abs(v2 - v3) < 1.0:
        return "almost_equal"
    return "N2_slower" if v2 < v3 else "N3_slower"


# ───────────────────────── CLI ─────────────────────────

def build_parser():
    p = argparse.ArgumentParser(
        description="N2/N3 对接电机诊断发令（ECAT）— 请确认机构安全")
    p.add_argument("--pid", type=int, nargs=3, default=[100, 100, 100],
                   help="PID current/vel/angle P，默认 100 100 100")
    p.add_argument("--voltage", type=float, default=24.0)
    p.add_argument("--rate", type=float, default=25.0, help="发令频率 Hz")
    p.add_argument("--threads", type=int, default=6)
    sub = p.add_subparsers(dest="cmd", required=True)

    sub.add_parser("status", help="只读状态与 CONFIG")
    slowp = sub.add_parser("lowspeed", help="低速镜像试控（错位大则拒绝）")
    slowp.add_argument("--vel", type=float, default=15.0)
    slowp.add_argument("--span", type=float, default=10.0)
    slowp.add_argument("--hold", type=float, default=2.5)

    sp = sub.add_parser("align", help="统一 CONFIG 并保持")
    sp.add_argument("--limit-v", type=float, default=22.0)
    sp.add_argument("--vel", type=float, default=100.0)
    sp.add_argument("--mode", type=int, default=3)
    sp.add_argument("--hold", type=float, default=0.8)

    so = sub.add_parser("openloop", help="开环速度对比 mode2")
    so.add_argument("--vel", type=float, default=80.0)
    so.add_argument("--cruise", type=float, default=1.5)
    so.add_argument("--settle", type=float, default=0.6)

    ss = sub.add_parser("stop", help="巡航→同时刹停")
    ss.add_argument("--vel", type=float, default=80.0)
    ss.add_argument("--cruise", type=float, default=1.2)
    ss.add_argument("--hold", type=float, default=0.8)
    ss.add_argument("--mirror", action="store_true", help="v2=-v3 镜像巡航")

    st = sub.add_parser("trip", help="mode3 位置行程")
    st.add_argument("--span", type=float, default=200.0)
    st.add_argument("--vel", type=float, default=80.0)
    st.add_argument("--hold", type=float, default=2.5)
    st.add_argument("--mirror", action="store_true")

    sfair = sub.add_parser("fairtrip", help="先到0再同时±span，公平对比")
    sfair.add_argument("--span", type=float, default=400.0)
    sfair.add_argument("--vel", type=float, default=200.0)
    sfair.add_argument("--hold", type=float, default=2.5)
    sfair.add_argument("--settle", type=float, default=2.0)

    sub.add_parser("stopnow", help="急停速度=0")

    sf = sub.add_parser("full", help="align+openloop+stop+trip 全套")
    sf.add_argument("--vel", type=float, default=80.0)
    sf.add_argument("--cruise", type=float, default=1.2)
    sf.add_argument("--hold", type=float, default=2.2)
    sf.add_argument("--settle", type=float, default=0.6)
    sf.add_argument("--span", type=float, default=200.0)
    sf.add_argument("--limit-v", type=float, default=22.0)
    sf.add_argument("--mode", type=int, default=3)
    sf.add_argument("--mirror", action="store_true")
    return p


def main(argv=None):
    args = build_parser().parse_args(argv)
    print("⚠  请确认电机2/3机构处于安全状态；Ctrl+C 后请手动 stopnow")
    print("target:", args.cmd, "pid", args.pid)
    handlers = {
        "status": cmd_status,
        "lowspeed": cmd_lowspeed,
        "align": cmd_align,
        "openloop": cmd_openloop,
        "stop": cmd_stop,
        "trip": cmd_trip,
        "fairtrip": cmd_fairtrip,
        "stopnow": cmd_stopnow,
        "full": cmd_full,
    }
    try:
        return handlers[args.cmd](args)
    except KeyboardInterrupt:
        print("\n!! 中断 → 急停")
        try:
            cmd_stopnow(args)
        except Exception:
            pass
        return 130


if __name__ == "__main__":
    raise SystemExit(main())