"""verify_px4.py —— 仅飞控验证（不接吊舱）：连飞控 + 订阅遥测 + offboard 启停。

增量流程第④级「仅飞控」：确认飞控侧遥测与 offboard 正常，再进第⑤级地面联调。

用法：
  python utils/pod_bridge/verify_px4.py serial:///dev/serial0:57600
  python utils/pod_bridge/verify_px4.py serial:///dev/serial0:57600 --offboard --seconds 15

判读标准（每行遥测）：
  * 连接成功 → 打印「飞控已连接」
  * 位置流：has_position=True、position_valid=True（fix_type>=3）、lat/lon 非 0
  * 姿态流：has_attitude=True（yaw 有值）
  * GPS：fix_type>=3、num_satellites>0
  * 遥测持续刷新（time_ms / position_time_ms 每拍递增）→ 链路稳定
  * --offboard：未上锁(armed)时 offboard.start 可能返回 False 属正常，仅验证启停不报错
"""

import argparse
import asyncio
import sys
import time
from pathlib import Path

sys.path.insert(0, str(Path(__file__).resolve().parents[1]))  # utils/ 入 sys.path

from pod_bridge.px4_gateway import Px4Gateway  # noqa: E402


def _monotonic_ms() -> int:
    return int(time.monotonic() * 1000)


async def run(conn: str, seconds: int, do_offboard: bool) -> int:
    px4 = await Px4Gateway.connect(conn, now_ms_fn=_monotonic_ms)
    await px4.start_telemetry()
    print(f"-- 遥测订阅已启动，连续打印 {seconds}s …")
    print("   (has_pos pos_valid fix sat | lat lon rel_alt | yaw | time_ms/pos_ms)")

    for i in range(seconds):
        await asyncio.sleep(1.0)
        s = px4.own_state()
        print(f"[{i:2d}s] pos={int(s.has_position)} valid={int(s.position_valid)} "
              f"fix={s.fix_type} sat={s.num_satellites:2d} | "
              f"{s.latitude_deg:.6f} {s.longitude_deg:.6f} {s.relative_altitude_m:5.1f}m | "
              f"yaw={s.yaw_rad:+.2f} att={int(s.has_attitude)} | "
              f"t={s.time_ms} pos_t={s.position_time_ms}")

    s = px4.own_state()
    print("\n-- 判读 --")
    ok = True
    if not s.has_position:
        print("  ✗ 位置流无数据（position 流未到达）"); ok = False
    if not s.position_valid:
        print(f"  ✗ 位置无效（fix_type={s.fix_type} < 3，GPS 未 3D 定位）"); ok = False
    if not s.has_attitude:
        print("  ✗ 姿态流无数据（attitude_euler 未到达）"); ok = False
    if ok:
        print("  ✓ 遥测正常：位置/姿态/GPS 均有效，链路稳定")

    if do_offboard:
        print("\n-- offboard 启停测试 --")
        started = await px4.start_offboard()
        print(f"  start_offboard -> {started}")
        if started:
            await asyncio.sleep(2.0)
            await px4.send_brake_hover(_monotonic_ms())
            await px4.stop_offboard()
            print("  ✓ offboard 启停完成（已零速刹车）")
        else:
            print("  ⚠ offboard 启动失败（未 armed 属正常，armed 后重试）")

    await px4.stop_telemetry()
    return 0 if ok else 1


def main() -> int:
    ap = argparse.ArgumentParser(description="仅飞控验证（不接吊舱）")
    ap.add_argument("conn", help="飞控连接串，如 serial:///dev/serial0:57600 或 udp://:14540")
    ap.add_argument("--seconds", type=int, default=10, help="遥测打印时长（秒）")
    ap.add_argument("--offboard", action="store_true", help="额外测试 offboard 启停")
    args = ap.parse_args()
    return asyncio.run(run(args.conn, args.seconds, args.offboard))


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