#!/usr/bin/env python3
"""USB 链路体检：RealSense 的**协商速率**与**掉线/掉速**事件。

为什么单独做这个检查
--------------------
2026-09-28 现场实测（D435 顶置相机）：相机原本协商到 USB 3.2、1280x720@30
跑得好好的，中途日志出现

    xioctl(UVCIOC_CTRL_QUERY) failed ... No such file or directory

也就是设备**从总线上消失**，几秒后重新枚举 —— 但这次是 USB **2.1**。
USB3 接口里 SuperSpeed 是一对独立的差分针脚，它们一旦接触不良（线松、座子磨损、
线被机械臂拽过），USB2 的针脚往往还能通：于是设备"还在"，只是**悄悄掉速**。

这种故障软件侧查不出来：
  · ``lsusb`` 照样列出这台相机（它不显示速率）；
  · 相机节点照样启动，只是分辨率/帧率被降级（日志里一句 WARN 很容易被刷掉）；
  · 于是现象变成"流程里相机帧过期 / 粗定位找不到工件"，看起来像视觉算法的问题。

一开始就报"协商速率不对"，能把这一类问题从"查算法"挪回"插紧线"。

还有一种更彻底的坏法：**Hub 在内核里卡死**
--------------------------------------------
2026-09-28 又实测到一次：不只是掉速，而是挂在同一个 Hub 下的**四台设备一起消失**
（腕部 D405 + 顶置 D435 + 左右两个视触觉）。此时 ``/sys/bus/usb/devices`` 里那个
Hub 节点还在，但读它的属性会**永久阻塞**：

    /sys/bus/usb/devices/1-3.4/product   ← 读 2 秒、20 秒、永不返回

原因是内核正在跟这个卡死的设备对话，拿不到应答就不返回。于是本脚本自己也挂住 ——
用户看到的是"体检卡死"，比报一行错更难判断。所以这里的 sysfs 读取**一律带超时**，
超时就当作一条 error 结论报出来（见 ``SysfsReader``）：宁可少读到一个字段，
也不能让体检本身失去意义。

判定口径（这里就是**唯一一处**）
--------------------------------
  · 5000 Mbps  → USB3，OK（D435 可用 1280x720@30）
  · 480 Mbps   → USB2，RealSense 会掉到 640x480@15 —— WARN 并给出拔插建议
  · 12 / 1.5   → USB 全速/低速，串口转换器正常，不告警
  · 读属性超时 → Hub/设备内核级卡死，ERROR（软件复位要 root，现场断电重启该 Hub）
  · 一台都没有 → ERROR

用法::

    python3 scripts/lib/usb_health.py                  # 单次体检
    python3 scripts/lib/usb_health.py --watch 10       # 盯 10 秒，报掉线/掉速次数
    python3 scripts/lib/usb_health.py --json           # 给脚本消费
"""

from __future__ import annotations

import argparse
import json
import threading
import time
from dataclasses import dataclass, asdict
from pathlib import Path
from typing import Dict, Iterable, List, Optional, Sequence, Tuple

SYS_USB = Path("/sys/bus/usb/devices")

# RealSense 的 product 字符串形如 "Intel(R) RealSense(TM) Depth Camera 435"
REALSENSE_TOKEN = "RealSense"
# 型号 → 期望的最小协商速率 [Mbps]
REALSENSE_MODELS: Tuple[Tuple[str, int], ...] = (
    ("435", 5000),
    ("455", 5000),
    ("415", 5000),
    ("405", 5000),
)

USB3_MBPS = 5000
USB2_MBPS = 480

# 见到 480Mbps 时的排查口径。相机节点 realsense_color_pub 里也有一句等价的
# 运行时提示（USB_RESEAT_HINT）—— test_usb_health.py 会钉住两处都提到
# "lsusb -t"，防止两份说法各自演化成互相矛盾的建议。
USB_RESEAT_HINT = (
    "只协商到 480Mbps 通常是 SuperSpeed 那一对针脚没接上，不是相机坏了："
    "拔插一次、换一根 USB3 线，或换到机箱后面板直连的蓝色口。"
    "核对速率：lsusb -t（设备后面写 480M 还是 5000M）。"
)

#: 读 sysfs 属性的超时 [s]。正常读取是微秒级，1 秒足够宽松。
DEFAULT_READ_TIMEOUT_S = 1.0


class SysfsReader:
    """带超时的 sysfs 读取 —— 卡死的 USB 设备会让读取**永久阻塞**。

    为什么不能直接用 ``Path.read_text``：内核在跟卡死的设备对话时，
    ``/sys/bus/usb/devices/1-3.4/product`` 这类属性读下去不返回，Python 就
    卡在系统调用里。对体检工具来说这是最坏的结果 —— 本该报出故障的那一次
    恰恰什么都不输出。

    超时后读属性用的线程会一直留在后台（Python 没法强杀阻塞在系统调用里的
    线程），所以同一个路径**只尝试一次**并记入 ``blocked``，后续直接跳过。
    这样 ``--watch`` 反复采样也不会攒出几十个卡住的线程。
    """

    def __init__(self, timeout_s: float = DEFAULT_READ_TIMEOUT_S) -> None:
        self.timeout_s = timeout_s
        #: 读取超时的 sysfs 路径（按出现顺序，去重）
        self.blocked: List[str] = []
        self._tried_blocked: set[str] = set()

    def read(self, path: Path) -> str:
        key = str(path)
        if key in self._tried_blocked:
            return ""
        done = threading.Event()
        box: Dict[str, str] = {}

        def _worker() -> None:
            try:
                box["v"] = path.read_text(encoding="utf-8", errors="replace").strip()
            except OSError:
                box["v"] = ""
            finally:
                done.set()

        threading.Thread(target=_worker, daemon=True).start()
        if not done.wait(self.timeout_s):
            self._tried_blocked.add(key)
            if key not in self.blocked:
                self.blocked.append(key)
            return ""
        return box.get("v", "")


def blocked_devices(blocked: Sequence[str]) -> List[str]:
    """把阻塞的 sysfs 路径归到 USB 设备路径上（``/sys/.../1-3.4/product`` → ``1-3.4``）。

    归类是为了给出**该动哪一个**：报"1-3.4/product 读不了"没用，
    要能说出"1-3.4 这个 Hub 卡死了，挂在它下面的设备一起没了"。
    """
    devs: List[str] = []
    for raw in blocked:
        parts = Path(raw).parts
        if "devices" in parts:
            name = parts[parts.index("devices") + 1]
        else:
            name = Path(raw).parent.name
        if name and name not in devs:
            devs.append(name)
    return devs


@dataclass(frozen=True)
class UsbDevice:
    """一个 USB 设备（按 sysfs 的拓扑路径标识，如 ``1-3.2``）。"""

    path: str
    vid: str
    pid: str
    product: str
    speed_mbps: Optional[int]
    serial: str = ""

    @property
    def is_realsense(self) -> bool:
        return REALSENSE_TOKEN.lower() in self.product.lower()

    @property
    def model(self) -> str:
        """RealSense 型号（``d435`` / ``d405``）；不是 RealSense 就返回空串。"""
        if not self.is_realsense:
            return ""
        for token, _ in REALSENSE_MODELS:
            if token in self.product:
                return f"d{token}"
        return "realsense"


@dataclass(frozen=True)
class Finding:
    """一条体检结论。``level`` 只允许 ok/warn/error，便于脚本判定成败。"""

    level: str
    text: str


def classify_speed(speed_mbps: Optional[int]) -> str:
    """把协商速率归类成 ``usb3`` / ``usb2`` / ``low`` / ``unknown``。

    12 和 1.5 Mbps 是 USB 全速/低速（串口转换器、HID），属于正常，不是"掉速"。
    """
    if speed_mbps is None:
        return "unknown"
    if speed_mbps >= USB3_MBPS:
        return "usb3"
    if speed_mbps == USB2_MBPS:
        return "usb2"
    if speed_mbps <= 12:
        return "low"
    return "other"


def _read(path: Path) -> str:
    try:
        return path.read_text(encoding="utf-8", errors="replace").strip()
    except OSError:
        return ""


def _parse_speed(raw: str) -> Optional[int]:
    """sysfs 的 speed 单位是 Mbps，可能带小数（``1.5``）。"""
    try:
        return int(round(float(raw)))
    except (TypeError, ValueError):
        return None


def read_devices(
    root: Path = SYS_USB, reader: Optional[SysfsReader] = None
) -> List[UsbDevice]:
    """列出当前 USB 设备（不需要 root）。

    ``reader`` 传入时，读取超时的路径会记在它上面（见 :class:`SysfsReader`）——
    调用方据此把"Hub 卡死"报出来，而不是默默少列几台设备。
    """
    rd = reader or SysfsReader()
    out: List[UsbDevice] = []
    if not root.is_dir():
        return out
    for entry in sorted(root.iterdir()):
        vid = rd.read(entry / "idVendor")
        if not vid:
            continue  # 不是设备节点（接口/端口目录没有 idVendor）
        out.append(
            UsbDevice(
                path=entry.name,
                vid=vid,
                pid=rd.read(entry / "idProduct"),
                product=rd.read(entry / "product"),
                speed_mbps=_parse_speed(rd.read(entry / "speed")),
                serial=rd.read(entry / "serial"),
            )
        )
    return out


def findings_for(
    devices: Sequence[UsbDevice], blocked: Sequence[str] = ()
) -> List[Finding]:
    """对一批设备给出体检结论（纯函数，便于单测）。

    ``blocked`` 是读取超时的 sysfs 路径（``SysfsReader.blocked``）。
    """
    out: List[Finding] = []
    # 卡死的 Hub 要**排在最前面**：它是根因，后面"一台相机都没枚举到"只是症状。
    for dev in blocked_devices(blocked):
        out.append(
            Finding(
                "error",
                f"USB 设备 {dev} 的 sysfs 属性读取超时 —— 内核正卡在跟它对话，"
                f"这一台**及其下游设备**会一起从系统里消失"
                f"（相机/触觉都挂它下面时，现象是「相机和触觉同时没了」）。"
                f"软件复位需要 root；现场处置是给这个 Hub 断电重启："
                f"拔插它的上游线，或把设备改插到主板后面板直连的 USB 口"
                f"（两个 RealSense 都走同一个总线供电的小 Hub 很容易把它拖垮）。",
            )
        )
    cams = [d for d in devices if d.is_realsense]
    if not cams:
        out.append(
            Finding(
                "error",
                "一台 RealSense 都没枚举到。先看上面有没有「sysfs 读取超时」那条"
                "（有就是 Hub 卡死，不是相机的问题）；否则拔插一次，并确认没有 "
                "realsense-viewer / 其它进程占着相机。",
            )
        )
        return out

    for cam in cams:
        kind = classify_speed(cam.speed_mbps)
        label = f"{cam.model or 'RealSense'}（{cam.path}）"
        if kind == "usb3":
            out.append(Finding("ok", f"{label} 协商 {cam.speed_mbps} Mbps（USB3）"))
        elif kind == "usb2":
            out.append(
                Finding(
                    "warn",
                    f"{label} 只协商到 {cam.speed_mbps} Mbps（USB2）—— "
                    f"图会掉到 640x480@15。{USB_RESEAT_HINT}",
                )
            )
        else:
            out.append(
                Finding(
                    "error",
                    f"{label} 协商速率异常：{cam.speed_mbps} Mbps。"
                    "换线/换口后重试。",
                )
            )
    return out


def has_errors(findings: Iterable[Finding]) -> bool:
    return any(f.level == "error" for f in findings)


def flap_events(snapshots: Sequence[Sequence[str]]) -> List[str]:
    """从连续几次"设备路径快照"里找出掉线/重连事件。

    ``snapshots`` 是每轮采样到的 RealSense 设备路径列表。返回人话描述，
    空列表 = 没抖。
    """
    events: List[str] = []
    for i in range(1, len(snapshots)):
        before, after = set(snapshots[i - 1]), set(snapshots[i])
        gone = sorted(before - after)
        back = sorted(after - before)
        if gone:
            events.append(f"第 {i} 次采样：消失 {', '.join(gone)}")
        if back:
            events.append(f"第 {i} 次采样：出现 {', '.join(back)}")
    return events


def watch_realsense(
    seconds: float,
    interval_s: float = 1.0,
    root: Path = SYS_USB,
    reader: Optional[SysfsReader] = None,
) -> List[List[str]]:
    """在 ``seconds`` 秒内反复采样 RealSense 的设备路径。"""
    rd = reader or SysfsReader()
    snaps: List[List[str]] = []
    deadline = time.monotonic() + max(0.0, seconds)
    while True:
        snaps.append(sorted(d.path for d in read_devices(root, rd) if d.is_realsense))
        if time.monotonic() >= deadline:
            break
        time.sleep(interval_s)
    return snaps


def main(argv: Optional[Sequence[str]] = None) -> int:
    ap = argparse.ArgumentParser(description="USB 链路体检（RealSense 速率/掉线）")
    ap.add_argument("--watch", type=float, default=0.0, help="盯多少秒（默认单次）")
    ap.add_argument("--json", action="store_true", help="输出 JSON")
    args = ap.parse_args(argv)

    # 整轮共用一个 reader：某条路径一旦确认卡死就不再重读，
    # 否则 --watch 每次采样都会再攒一个卡在内核里的线程。
    reader = SysfsReader()
    devices = read_devices(reader=reader)
    findings = findings_for(devices, reader.blocked)
    snaps = watch_realsense(args.watch, reader=reader) if args.watch > 0 else []
    flaps = flap_events(snaps) if snaps else []
    if flaps:
        findings.append(
            Finding(
                "error",
                f"{len(snaps)} 次采样里 RealSense 掉线/重连 {len(flaps)} 次："
                + "；".join(flaps)
                + "。这属于**物理链路**问题（线/座子/供电 Hub），"
                "不是软件 —— 先插紧/换线再跑流程。",
            )
        )

    payload: Dict[str, object] = {
        "devices": [asdict(d) for d in devices if d.is_realsense],
        "findings": [asdict(f) for f in findings],
        "flaps": flaps,
        "blocked_sysfs": list(reader.blocked),
    }
    if args.json:
        print(json.dumps(payload, ensure_ascii=False, indent=2))
    else:
        for d in devices:
            if d.is_realsense:
                print(
                    f"  {d.model or d.product:8} {d.path:8} "
                    f"{str(d.speed_mbps) + ' Mbps':>10}  vid:pid={d.vid}:{d.pid}"
                )
        if reader.blocked:
            # 单独打一行：它意味着"下面那些设备根本没被列出来"，
            # 不写清楚的话，上面的列表看起来就像"设备没了"而没有原因。
            print(f"  （{len(reader.blocked)} 条 sysfs 属性读取超时，详见下面的 FAIL）")
        for f in findings:
            mark = {"ok": "OK  ", "warn": "WARN", "error": "FAIL"}[f.level]
            print(f"  [{mark}] {f.text}")
    return 1 if has_errors(findings) else 0


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