#!/usr/bin/env python3
"""通过 Flexiv RDK 控制 Studio 设备 GripperDahuan / GripperDahuanModbus。

必须在 RDK 的 Python（rdk_py310）下运行。使用 *lite*（轻量）RDK 实例，
以便和 run_real.sh 共用机器人（常规 RDK 被 ros2_control 占用）。

控制器自己走 RS485（用自带的 /dev/ttyUSB0），不走工控机的串口。

  /home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py --list
  /home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py status
  /home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py init
  /home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py open
  /home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py close
"""

from __future__ import annotations

import argparse
import sys
import time
from pathlib import Path

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

from rdk_switch_tool import _iface_ipv4s  # noqa: E402


def connect_lite(sn: str, iface: str = ""):
    import flexivrdk

    ips = _iface_ipv4s(iface)
    if ips:
        try:
            return flexivrdk.Robot(sn, ips, True, True)
        except TypeError:
            pass
        except RuntimeError as exc:
            msg = str(exc).lower()
            if "whitelist" not in msg and "filtered out" not in msg:
                raise
    return flexivrdk.Robot(sn, [], True, True)


def _fmt_params(p) -> str:
    return (
        f"width=[{p.min_width:.4f},{p.max_width:.4f}] m  "
        f"vel=[{p.min_vel:.4f},{p.max_vel:.4f}] m/s  "
        f"force=[{p.min_force:.1f},{p.max_force:.1f}] N"
    )


def _fmt_states(s) -> str:
    return f"width={s.width:.4f} m  force={s.force:.2f} N  moving={s.is_moving}"


def _clamp(val: float, lo: float, hi: float) -> float:
    if lo > hi:
        lo, hi = hi, lo
    return max(lo, min(hi, val))


def _move(grip, params, width: float, vel: float, force: float) -> None:
    vmin = float(params.min_vel)
    vmax = float(params.max_vel)
    if vmax <= 0:
        vmax = 0.05
    # 移动速度不能为 0（RDK 限制）。
    vel = _clamp(vel, max(vmin, 1e-4), vmax)
    force = _clamp(force, float(params.min_force), float(params.max_force))
    width = _clamp(width, float(params.min_width), float(params.max_width))
    print(f"移动 width={width:.4f} m  vel={vel:.4f} m/s  force={force:.1f} N")
    grip.Move(width, vel, force)


def main() -> int:
    ap = argparse.ArgumentParser(description="Flexiv RDK 夹爪（GripperDahuanModbus）")
    ap.add_argument("--sn", default="")
    ap.add_argument("--iface", default="")
    ap.add_argument("--device", default="", help="Studio 设备名")
    ap.add_argument("--force-n", type=float, default=0.0, help="力上限 [N]；0=取中值")
    ap.add_argument("--vel", type=float, default=0.0, help="速度 [m/s]；0=最大速度的一半")
    ap.add_argument(
        "cmd",
        nargs="?",
        default="status",
        choices=["list", "status", "init", "open", "close", "pos", "enable"],
    )
    ap.add_argument("value", nargs="?", default="", help="pos: 米，或 0–1000")
    args = ap.parse_args()

    sys.path.insert(0, str(ROOT / "scripts" / "lib"))
    from export_site_env import load_site, resolve_robot  # type: ignore

    data = load_site()
    robot_cfg = resolve_robot(data)
    g = data.get("gripper") or {}
    sn = (args.sn or robot_cfg.get("sn") or "").strip()
    iface = (args.iface or robot_cfg.get("iface") or "").strip()
    device = (args.device or g.get("device_name") or "GripperDahuanModbus").strip()
    if not sn:
        print("错误：没拿到机械臂序列号（SN）", file=sys.stderr)
        return 2

    import flexivrdk

    print(f"=== RDK lite 夹爪  SN={sn}  device={device}  iface={iface or 'auto'} ===")
    robot = connect_lite(sn, iface)
    dev = flexivrdk.Device(robot)
    names = list(dev.list() or [])

    if args.cmd == "list":
        print("Studio 设备列表:")
        for n in names:
            try:
                en = dev.enabled(n)
                cn = dev.connected(n)
            except Exception:
                en, cn = "?", "?"
            mark = "  <--" if n == device else ""
            print(f"  {n}: enabled={en} connected={cn}{mark}")
        return 0

    if device not in names:
        print(
            f"错误：设备 {device!r} 不在 Studio 设备池里。\n  可用: {', '.join(names)}",
            file=sys.stderr,
        )
        return 1

    print(
        f"  Studio: enabled={dev.enabled(device)}  connected={dev.connected(device)}"
    )
    if not dev.connected(device):
        print(
            "警告：connected=False —— 控制器自己的 /dev/ttyUSB0 没接到夹爪。\n"
            "  USB–RS485 要插 Flexiv 机箱 USB（不是工控机），A/B + 24V 到 PGEA。\n"
            "  或用法兰工具口 485（腕部白绿+/绿-）。"
        )

    grip = flexivrdk.Gripper(robot)
    if args.cmd in ("enable", "status", "init", "open", "close", "pos"):
        try:
            grip.Enable(device)
            print(f"使能 [{device}] 成功")
        except Exception as exc:
            print(f"错误：使能失败: {exc}", file=sys.stderr)
            return 1

    params = grip.params()
    print("  参数:", _fmt_params(params))
    print("  状态:", _fmt_states(grip.states()))

    force = float(args.force_n or g.get("force_n") or 0.0)
    if force <= 0:
        force = 0.5 * (float(params.min_force) + float(params.max_force))
    vel = float(args.vel or g.get("vel_mps") or 0.0)
    if vel <= 0:
        vel = 0.5 * float(params.max_vel)

    if args.cmd in ("enable", "status"):
        if not dev.connected(device):
            return 2
        return 0

    if args.cmd == "init":
        print("初始化（手指回零，约 10 秒）…")
        grip.Init()
        time.sleep(2.0)
        print("  状态:", _fmt_states(grip.states()))
        return 0

    if args.cmd == "open":
        _move(grip, params, float(params.max_width), vel, force)
        time.sleep(0.4)
        print("  状态:", _fmt_states(grip.states()))
        return 0

    if args.cmd == "close":
        _move(grip, params, float(params.min_width), vel, force)
        time.sleep(0.4)
        print("  状态:", _fmt_states(grip.states()))
        return 0

    if args.cmd == "pos":
        if args.value == "":
            print("用法: pos <米|0-1000>", file=sys.stderr)
            return 2
        raw = float(args.value)
        if raw > 1.5:
            span = float(params.max_width) - float(params.min_width)
            width = float(params.min_width) + (raw / 1000.0) * span
        else:
            width = raw
        _move(grip, params, width, vel, force)
        time.sleep(0.4)
        print("  状态:", _fmt_states(grip.states()))
        return 0
    return 0


if __name__ == "__main__":
    try:
        raise SystemExit(main())
    except Exception as exc:
        print(f"错误：{exc}", file=sys.stderr)
        raise SystemExit(1)