#!/usr/bin/env python3
"""演示：起立 → 原地旋转一段时间 → 停止 → 趴下恢复。

用法::

    uv run python demo_stand_rotate_lie.py --seconds 5 --yaw 0.3

    # 指定证书目录和机器人地址
    uv run python demo_stand_rotate_lie.py --certs /path/to/certs --host 10.21.33.103

流程与《山猫S10 软件开发指南》的遥控时序一致：

1. mTLS 连接 30003/TCP，开启 2Hz 心跳（机器人随后开始主动上报状态）
2. 切到常规模式
3. 下发站立，等待进入 RL 控制状态（ASDU 站立完成后会自动进入）
4. 切到基础步态（必须在完全停止时切换）
5. 以 10Hz 持续下发 Yaw 轴指令，原地旋转指定秒数
6. 下发全 0 停止，再下发趴下，等待回到趴卧/待机状态

任何异常、Ctrl-C 或超时都会走安全收尾：先发全 0 停止，若仍处于站立/RL
状态则下发趴下，最后关闭连接。
"""

from __future__ import annotations

import argparse
import logging
import os
import signal
import sys
import threading
import time
from pathlib import Path

from s10_robot import (
    Gait,
    Mode,
    MotionState,
    S10Client,
    S10CommandError,
    S10Error,
    S10Timeout,
)

log = logging.getLogger("demo")

ABORT = threading.Event()


def _handle_signal(signum, _frame):
    log.warning("收到信号 %s，准备安全收尾", signal.Signals(signum).name)
    ABORT.set()


def resolve_certs(explicit: str | None) -> Path:
    candidates = []
    if explicit:
        candidates.append(Path(explicit))
    if os.environ.get("S10_CERTS"):
        candidates.append(Path(os.environ["S10_CERTS"]))
    candidates.append(Path("certs"))
    candidates.append(
        Path.home()
        / ".zcode/workspace/draft-183385f3-4f14-43ad-ab22-e5deace0e05c/backup/certs"
    )
    for path in candidates:
        if (path / "client_chain.crt").is_file() and (path / "client.key").is_file():
            return path
    raise SystemExit(
        "找不到证书目录，请用 --certs 指定包含 client_chain.crt / client.key / root_ca.crt 的目录"
    )


def describe(robot: S10Client) -> str:
    basic = robot.basic_status
    return (
        f"MotionState={basic.get('MotionState')} Gait={basic.get('Gait')} "
        f"Mode={basic.get('ControlUsageMode')} HES={basic.get('HES')}"
    )


def check_alive(robot: S10Client) -> None:
    basic = robot.basic_status
    if basic.get("HES") == 1:
        raise S10Error("硬急停已触发，终止运动")
    errors = robot.active_errors
    for err in errors:
        if max(err.get("Severities") or [0]) >= 5:
            raise S10Error(f"出现 FATAL 故障: {err.get('Name')} ({err.get('Code')})")


def rotate_in_place(robot: S10Client, yaw: float, seconds: float, rate_hz: float) -> None:
    """以固定频率持续下发 Yaw 轴指令，旋转指定时间。"""
    period = 1.0 / rate_hz
    deadline = time.monotonic() + seconds
    next_tick = time.monotonic()
    sent = 0
    log.info("开始原地旋转: yaw=%+.2f, %g 秒, %g Hz", yaw, seconds, rate_hz)
    while time.monotonic() < deadline:
        if ABORT.is_set():
            log.warning("旋转被中断")
            break
        check_alive(robot)
        state = robot.basic_status.get("MotionState")
        if state != MotionState.RL:
            raise S10Error(f"旋转过程中运动状态异常: MotionState={state}（预期 {MotionState.RL}）")
        robot.move(yaw=yaw)
        sent += 1
        next_tick += period
        time.sleep(max(0.0, next_tick - time.monotonic()))
    log.info("旋转结束，共下发 %d 帧轴指令", sent)


def safe_recover(robot: S10Client) -> None:
    """异常/中断后的安全收尾：停止移动，必要时趴下。"""
    if not robot.connected:
        return
    try:
        robot.stop()
    except S10Error as exc:
        log.warning("发送停止指令失败: %s", exc)
    state = robot.basic_status.get("MotionState")
    if state in (MotionState.STANDING, MotionState.RL, MotionState.JOINT_DAMP):
        try:
            log.info("安全收尾：下发趴下")
            robot.lie_down()
            robot.wait_motion_state({MotionState.LYING, MotionState.DEFAULT}, timeout=15)
            log.info("已趴下: %s", describe(robot))
        except (S10Error, S10Timeout) as exc:
            log.error("趴下失败，请手动接管: %s", exc)


def run(args: argparse.Namespace) -> int:
    certs = resolve_certs(args.certs)
    log.info("证书目录: %s", certs)

    robot = S10Client(
        host=args.host,
        port=args.port,
        cert=str(certs / "client_chain.crt"),
        key=str(certs / "client.key"),
        ca=str(certs / "root_ca.crt"),
        verify=args.verify,
        heartbeat_hz=args.heartbeat_hz,
        request_timeout=args.timeout,
    )

    try:
        robot.connect()
        log.info("等待首次状态上报…")
        try:
            robot.wait_basic("MotionState", range(-2, 0x1002), timeout=5.0)
        except S10Timeout:
            log.warning("5 秒内未收到状态上报，继续尝试下发指令")
        log.info("初始状态: %s", describe(robot))

        if ABORT.is_set():
            return 130

        log.info("① 切换到常规模式")
        robot.set_mode(Mode.NORMAL)
        try:
            robot.wait_mode({Mode.NORMAL}, timeout=3)
        except S10Timeout:
            log.warning("模式回读超时，继续")

        log.info("② 下发站立")
        robot.stand()
        log.info("   等待进入 RL 控制状态（最多 25 秒）…")
        robot.wait_motion_state({MotionState.RL}, timeout=25)
        log.info("   已站立并进入 RL 控制: %s", describe(robot))

        log.info("③ 切换到基础步态 0x%04X", Gait.BASIC)
        robot.set_gait(Gait.BASIC)
        time.sleep(0.3)

        log.info("④ 原地旋转")
        rotate_in_place(robot, args.yaw, args.seconds, args.rate)

        log.info("⑤ 停止移动，等待完全静止")
        robot.stop()
        robot.stop()
        time.sleep(0.5)

        log.info("⑥ 下发趴下")
        robot.lie_down()
        state = robot.wait_motion_state({MotionState.LYING, MotionState.DEFAULT}, timeout=20)
        log.info("   已恢复: MotionState=%s（%s）", state, describe(robot))
        return 0

    except S10CommandError as exc:
        log.error("机器人拒绝指令: %s", exc)
        return 2
    except (S10Error, S10Timeout) as exc:
        log.error("执行失败: %s", exc)
        return 1
    finally:
        safe_recover(robot)
        robot.close()
        log.info("连接已关闭")


def main(argv: list[str] | None = None) -> int:
    parser = argparse.ArgumentParser(
        description="山猫 S10：起立 → 原地旋转 → 趴下 恢复演示",
        formatter_class=argparse.ArgumentDefaultsHelpFormatter,
    )
    parser.add_argument("--host", default="10.21.33.103", help="机器人地址")
    parser.add_argument("--port", type=int, default=30003, help="TLS 端口")
    parser.add_argument("--certs", default=None, help="证书目录（含 client_chain.crt/client.key/root_ca.crt）")
    parser.add_argument("--yaw", type=float, default=0.3,
                        help="偏航比例 [-1,1]，正值为逆时针")
    parser.add_argument("--seconds", type=float, default=5.0, help="旋转时长（秒）")
    parser.add_argument("--rate", type=float, default=10.0, help="轴指令下发频率（Hz）")
    parser.add_argument("--heartbeat-hz", type=float, default=2.0, help="心跳频率（Hz）")
    parser.add_argument("--timeout", type=float, default=5.0, help="单条指令响应超时（秒）")
    parser.add_argument("--verify", action="store_true", help="严格校验服务端证书链")
    parser.add_argument("-v", "--verbose", action="store_true", help="输出调试日志")
    args = parser.parse_args(argv)

    if not -1.0 <= args.yaw <= 1.0:
        parser.error("--yaw 必须在 [-1, 1] 之间")
    if args.seconds <= 0:
        parser.error("--seconds 必须大于 0")
    if args.rate <= 0:
        parser.error("--rate 必须大于 0")

    logging.basicConfig(
        level=logging.DEBUG if args.verbose else logging.INFO,
        format="%(asctime)s.%(msecs)03d %(levelname)-7s %(name)s: %(message)s",
        datefmt="%H:%M:%S",
    )

    signal.signal(signal.SIGINT, _handle_signal)
    signal.signal(signal.SIGTERM, _handle_signal)

    return run(args)


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