#!/usr/bin/env python3
"""Inspect or explicitly release Unitree high-level motion control for H2.

The default operation is read-only. Releasing a motion mode is intentionally
double-gated because it changes robot state and must remain an operator action.
"""

from __future__ import annotations

import argparse
import json
from pathlib import Path
import sys
import time

ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path:
    sys.path.insert(0, str(ROOT))

from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
from unitree_sdk2py.core.channel import ChannelFactoryInitialize
from runtime.motion_attestation import (
    ATTESTATION_PATH,
    motion_switcher_released,
    write_attestation,
)


RELEASE_ACK = "RELEASE-H2-HIGH-LEVEL-CONTROL"


def _mode(client: MotionSwitcherClient) -> tuple[int, dict]:
    status, result = client.CheckMode()
    motion_switcher_released(status, result)
    return int(status), result


def _released(result: dict) -> bool:
    return motion_switcher_released(0, result)


def main() -> None:
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument("--network-interface", required=True)
    parser.add_argument("--release", action="store_true", help="Release the active high-level mode.")
    parser.add_argument(
        "--write-attestation",
        action="store_true",
        help="After a successful CheckMode, write the short-lived token required by the runner.",
    )
    parser.add_argument(
        "--ack-release",
        default="",
        help=f"Required with --release; exact value: {RELEASE_ACK}",
    )
    args = parser.parse_args()

    # A new check always invalidates any earlier token before contacting the
    # robot, so a failed/active result cannot leave an old pass consumable.
    try:
        ATTESTATION_PATH.unlink()
    except FileNotFoundError:
        pass

    interface = str(args.network_interface)
    state_path = Path("/sys/class/net") / interface / "operstate"
    if not state_path.is_file():
        raise RuntimeError(f"network interface does not exist: {interface}")
    state = state_path.read_text(encoding="utf-8").strip()
    if state != "up":
        raise RuntimeError(f"network interface {interface} is not UP (operstate={state!r})")
    if args.release and args.ack_release != RELEASE_ACK:
        raise RuntimeError(f"--release requires --ack-release {RELEASE_ACK}")

    ChannelFactoryInitialize(0, interface)
    client = MotionSwitcherClient()
    client.SetTimeout(5.0)
    client.Init()
    _, result = _mode(client)
    print("[MOTION MODE] " + json.dumps(result, sort_keys=True, ensure_ascii=False))

    if not args.release:
        if not _released(result):
            raise SystemExit(
                "active high-level motion mode detected; inspect it, then rerun with the explicit release gate"
            )
        print("[MOTION MODE PASS] no active high-level motion mode")
        if args.write_attestation:
            write_attestation(interface)
            print(f"[MOTION ATTESTATION] wrote owner-only token {ATTESTATION_PATH}")
        return

    for attempt in range(1, 4):
        if _released(result):
            break
        status, _ = client.ReleaseMode()
        if status != 0:
            raise RuntimeError(f"MotionSwitcher ReleaseMode failed on attempt {attempt}: status={status}")
        time.sleep(1.0)
        _, result = _mode(client)
        print(
            f"[MOTION MODE] post-release attempt={attempt} "
            + json.dumps(result, sort_keys=True, ensure_ascii=False)
        )
    if not _released(result):
        raise RuntimeError("high-level motion mode remains active after three release attempts")
    print("[MOTION MODE PASS] high-level motion control is released")
    if args.write_attestation:
        write_attestation(interface)
        print(f"[MOTION ATTESTATION] wrote owner-only token {ATTESTATION_PATH}")


if __name__ == "__main__":
    main()