#!/usr/bin/env python3
"""确定性复现「工件被推走 ⇒ 规没进孔」：在 k1 之后把工件挪 25mm。

为什么需要它：这个故障在自然运行里是**随机**的（实测三跑坏一跑），
靠碰运气验证判据不可靠。这里改成主动制造：等流程走到 S10（离开 k1、去 k2），
就把工件沿 +Y 推 25mm —— 而 k2 的孔位是 S05 一次性定下的，于是必然失配。

判据（要看的就是这个）：S07 必须报「规没有进孔」（HOLE_LOST），
**不是** 之前那句把人引去查工具系的「转轴偏差」。
"""
from __future__ import annotations

import pathlib
import subprocess
import sys
import time

ROOT = pathlib.Path(__file__).resolve().parents[1]
sys.path.insert(0, str(ROOT / "src" / "arm_sim_gazebo"))

LOG = pathlib.Path("/tmp/sim_guard.log")
WORLD = "assembly"
SHIFT_M = -0.020  # 与 2026-09-28 自然失效同向同量级（工件 y: 0.000 → -0.0206）



def _shift() -> None:
    from arm_sim_gazebo.sim_pose_service import set_model_pose

    # 工件当前在 (0.520, 0.000, 0.400)，沿 +Y 挪 25mm（远超孔半径，必然错过）
    ok, out = set_model_pose(WORLD, "workpiece", (0.520, SHIFT_M, 0.400), (0.0, 0.0, 0.0, 1.0))
    print(f"[driver] 推工件 {SHIFT_M * 1000:.0f}mm -> {'成功' if ok else '失败'}: {out[:160]}")


def main() -> int:
    LOG.write_text("")
    proc = subprocess.Popen(
        ["bash", "scripts/run_gauge_assembly.sh", "--arm", "sim", "--sim"],
        cwd=str(ROOT), stdout=LOG.open("w"), stderr=subprocess.STDOUT,
    )
    print(f"[driver] 流程已起 pid={proc.pid}，等它走到 S10…")

    shifted = False
    t0 = time.monotonic()
    while proc.poll() is None and time.monotonic() - t0 < 1500:
        txt = LOG.read_text(errors="replace")
        if not shifted and "S10_to_k2" in txt:
            print("[driver] 检测到 S10_to_k2 —— 现在推工件")
            _shift()
            shifted = True
        time.sleep(1.0)

    proc.wait(timeout=30)
    txt = LOG.read_text(errors="replace")
    print(f"[driver] 流程退出码 = {proc.returncode}, 是否推过工件 = {shifted}")
    for line in txt.splitlines():
        if "规没有进孔" in line or "孔位失效" in line or "转轴偏差" in line:
            print("  |", line.strip()[:150])
    return 0


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