"""RM75 机械臂适配层。

对上提供统一的当前位姿读取、笛卡尔位姿目标发送和停止接口；对下根据配置
选择 mock 积分模拟器或睿尔曼 Python API2 真机通信。
"""

from __future__ import annotations

import math
from dataclasses import dataclass
from numbers import Number
from typing import Any


def _angle_delta(target: float, current: float) -> float:
    return math.atan2(math.sin(target - current), math.cos(target - current))


@dataclass
class ArmPose:
    x: float
    y: float
    z: float
    rx: float = 0.0
    ry: float = 0.0
    rz: float = 0.0

    def xyz(self) -> list[float]:
        return [self.x, self.y, self.z]

    def rpy(self) -> list[float]:
        return [self.rx, self.ry, self.rz]


class MockRealManAdapter:
    """无机械臂时使用的运动学模拟器，用于验证 ROS2 遥操链路。"""

    def __init__(self, initial_pose: list[float], dt: float) -> None:
        self._pose = ArmPose(*initial_pose[:6])
        self._dt = dt
        self.last_velocity = [0.0] * 6
        self.last_tool_open: bool | None = None

    def connect(self) -> None:
        return

    def get_current_pose(self) -> ArmPose:
        return self._pose

    def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None:
        del follow
        self.last_velocity = [
            (pose.x - self._pose.x) / self._dt,
            (pose.y - self._pose.y) / self._dt,
            (pose.z - self._pose.z) / self._dt,
            _angle_delta(pose.rx, self._pose.rx) / self._dt,
            _angle_delta(pose.ry, self._pose.ry) / self._dt,
            _angle_delta(pose.rz, self._pose.rz) / self._dt,
        ]
        self._pose = pose

    def stop(self) -> None:
        self.last_velocity = [0.0] * 6

    def close(self) -> None:
        self.stop()

    def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None:
        del config_file, peripheral_arm

    def set_tool_enabled(self, open_tool: bool) -> None:
        self.last_tool_open = open_tool


class RealManAdapter:
    """睿尔曼 Python API2 的笛卡尔位姿透传适配层。"""

    def __init__(
        self,
        robot_ip: str,
        robot_port: int,
        avoid_singularity: int,
        frame_type: int,
        logger: Any | None = None,
        configure_safety_limits: bool = True,
        max_line_speed: float = 1.0,
        max_angular_speed: float = 1.5,
        max_line_acc: float = 1.0,
        max_angular_acc: float = 2.0,
        joint_max_speed: float = 180.0,
        joint_max_acc: float = 180.0,
        move_to_initial_pose_on_connect: bool = False,
        initial_joint_pose: list[float] | None = None,
        initial_tcp_pose: list[float] | None = None,
        init_move_speed: int = 20,
        canfd_trajectory_mode: int = 2,
        canfd_radio: int = 0,
    ) -> None:
        self._robot_ip = robot_ip
        self._robot_port = robot_port
        self._avoid_singularity = avoid_singularity
        self._frame_type = frame_type
        self._logger = logger
        self._configure_safety_limits = configure_safety_limits
        self._max_line_speed = max_line_speed
        self._max_angular_speed = max_angular_speed
        self._max_line_acc = max_line_acc
        self._max_angular_acc = max_angular_acc
        self._joint_max_speed = joint_max_speed
        self._joint_max_acc = joint_max_acc
        self._move_to_initial_pose_on_connect = move_to_initial_pose_on_connect
        self._initial_joint_pose = initial_joint_pose
        self._initial_tcp_pose = initial_tcp_pose
        self._init_move_speed = init_move_speed
        self._canfd_trajectory_mode = canfd_trajectory_mode
        self._canfd_radio = canfd_radio
        self._scissorgripper: int | None = None
        self._arm: Any | None = None

    def connect(self) -> None:
        try:
            from Robotic_Arm.rm_robot_interface import RoboticArm, rm_thread_mode_e
        except ImportError as exc:
            raise RuntimeError(
                "未安装睿尔曼 Python API2。请安装厂商 SDK，或用 use_mock:=true 先跑模拟模式。"
            ) from exc

        self._arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E)
        handle = self._arm.rm_create_robot_arm(self._robot_ip, self._robot_port)
        self._check_robot_handle(handle)
        self._log_info(
            "RealMan connected: "
            f"ip={self._robot_ip}, port={self._robot_port}, "
            f"avoid_singularity={self._avoid_singularity}, "
            f"frame_type={self._frame_type}, command=rm_movep_canfd"
        )
        if self._configure_safety_limits:
            self._apply_safety_limits()
        if self._move_to_initial_pose_on_connect:
            self._move_to_initial_pose()

    def get_current_pose(self) -> ArmPose:
        self._require_arm()
        state = self._arm.rm_get_current_arm_state()
        pose = self._find_pose(state)
        if pose is None:
            raise RuntimeError(f"无法从睿尔曼状态中解析当前 TCP 位姿：{state!r}")
        return ArmPose(*pose[:6])

    def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None:
        self._require_arm()
        ret = self._arm.rm_movep_canfd(
            [pose.x, pose.y, pose.z, pose.rx, pose.ry, pose.rz],
            follow,
            self._canfd_trajectory_mode,
            self._canfd_radio,
        )
        self._check_return(ret, "rm_movep_canfd")

    def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None:
        self._require_arm()
        from .fun_peripheral import load_peripheral_config, peripheral_cfg

        config = load_peripheral_config(config_file, peripheral_arm)
        self._scissorgripper = config.scissorgripper
        tool_name = list(config.tools_in_ee.keys())[config.scissorgripper]
        self._log_info(
            "开始配置 RealMan 末端外设："
            f"arm={peripheral_arm}, scissorgripper={config.scissorgripper}, "
            f"tool={tool_name}, set_initial_tool_state={config.set_initial_tool_state}"
        )
        peripheral_cfg(
            self._arm,
            config.scissorgripper,
            config.tools_in_ee,
            set_initial_tool_state=config.set_initial_tool_state,
        )
        self._log_info(f"RealMan 末端外设配置完成：arm={peripheral_arm}, tool={tool_name}")

    def set_tool_enabled(self, open_tool: bool) -> None:
        self._require_arm()
        if self._scissorgripper is None:
            raise RuntimeError("末端外设尚未初始化，无法执行开合命令")

        from .fun_peripheral import tool_exe

        tool_exe(self._arm, self._scissorgripper, open_tool)

    def stop(self) -> None:
        if self._arm is None:
            return
        try:
            self._arm.rm_set_arm_slow_stop()
        except Exception:
            pass

    def close(self) -> None:
        if self._arm is None:
            return
        self.stop()
        try:
            self._arm.rm_delete_robot_arm()
        finally:
            self._arm = None

    def _require_arm(self) -> None:
        if self._arm is None:
            raise RuntimeError("睿尔曼机械臂尚未连接")

    def _apply_safety_limits(self) -> None:
        # 真机安全限幅尽量下发到控制器；不支持的 SDK 接口会在 _try_call 中降级为警告。
        self._try_call("rm_set_avoid_singularity_mode", int(self._avoid_singularity))
        self._try_call("rm_set_arm_max_line_speed", self._max_line_speed)
        self._try_call("rm_set_arm_max_angular_speed", self._max_angular_speed)
        self._try_call("rm_set_arm_max_line_acc", self._max_line_acc)
        self._try_call("rm_set_arm_max_angular_acc", self._max_angular_acc)
        for joint_index in range(1, 8):
            self._try_call("rm_set_joint_max_speed", joint_index, self._joint_max_speed)
            self._try_call("rm_set_joint_max_acc", joint_index, self._joint_max_acc)

    def _move_to_initial_pose(self) -> None:
        if self._initial_joint_pose is None or self._initial_tcp_pose is None:
            raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose 和 initial_tcp_pose")

        ret = self._arm.rm_movej(self._initial_joint_pose, self._init_move_speed, 0, 0, 1)
        self._check_return(ret, "rm_movej(initial_joint_pose)")
        ret = self._arm.rm_movel(self._initial_tcp_pose, self._init_move_speed, 0, 0, 1)
        self._check_return(ret, "rm_movel(initial_tcp_pose)")

    def _try_call(self, name: str, *args: Any) -> None:
        func = getattr(self._arm, name, None)
        if func is None:
            self._log_warn(f"当前睿尔曼 SDK 不支持 {name}，跳过该安全配置。")
            return

        try:
            ret = func(*args)
            self._check_return(ret, name)
        except Exception as exc:
            self._log_warn(f"{name} 安全配置失败，继续使用软件侧限幅：{exc}")

    def _log_warn(self, message: str) -> None:
        if self._logger is not None:
            self._logger.warn(message)

    def _log_info(self, message: str) -> None:
        if self._logger is not None:
            self._logger.info(message)

    def _check_robot_handle(self, handle: Any) -> None:
        handle_id = getattr(handle, "id", None)
        if handle_id == -1:
            raise RuntimeError(
                "rm_create_robot_arm failed: TCP may be reachable, but the RealMan "
                f"controller did not return robot info from {self._robot_ip}:{self._robot_port}"
            )

    @staticmethod
    def _check_return(ret: Any, name: str) -> None:
        code = RealManAdapter._return_code(ret)
        if isinstance(code, int) and code != 0:
            raise RuntimeError(f"{name} failed with code {code}: {ret!r}")

    @staticmethod
    def _return_code(ret: Any) -> Any:
        return ret[0] if isinstance(ret, tuple) and ret else ret

    @classmethod
    def _find_pose(cls, obj: Any) -> list[float] | None:
        # 不同 SDK 版本返回字段可能略有差异，因此递归查找常见 TCP 位姿字段。
        if isinstance(obj, dict):
            for key in ("pose", "tool_pose", "tcp_pose", "current_pose"):
                pose = cls._as_pose(obj.get(key))
                if pose is not None:
                    return pose
            for value in obj.values():
                pose = cls._find_pose(value)
                if pose is not None:
                    return pose
        elif isinstance(obj, (list, tuple)):
            pose = cls._as_pose(obj)
            if pose is not None:
                return pose
            for value in obj:
                pose = cls._find_pose(value)
                if pose is not None:
                    return pose
        elif hasattr(obj, "to_dictionary"):
            try:
                return cls._find_pose(obj.to_dictionary(7))
            except TypeError:
                return cls._find_pose(obj.to_dictionary())
        elif hasattr(obj, "to_dict"):
            return cls._find_pose(obj.to_dict())
        else:
            for key in ("pose", "tool_pose", "tcp_pose", "current_pose"):
                if hasattr(obj, key):
                    pose = cls._as_pose(getattr(obj, key))
                    if pose is not None:
                        return pose
        return None

    @staticmethod
    def _as_pose(value: Any) -> list[float] | None:
        if isinstance(value, (list, tuple)) and len(value) >= 6:
            if all(isinstance(item, Number) for item in value[:6]):
                return [float(item) for item in value[:6]]
        if isinstance(value, dict):
            position = value.get("position")
            euler = value.get("euler")
            if isinstance(position, dict) and isinstance(euler, dict):
                keys = ("x", "y", "z")
                rpy_keys = ("rx", "ry", "rz")
                if all(key in position for key in keys) and all(key in euler for key in rpy_keys):
                    return [
                        float(position["x"]),
                        float(position["y"]),
                        float(position["z"]),
                        float(euler["rx"]),
                        float(euler["ry"]),
                        float(euler["rz"]),
                    ]
        if all(hasattr(value, attr) for attr in ("position", "euler")):
            position = getattr(value, "position")
            euler = getattr(value, "euler")
            if all(hasattr(position, key) for key in ("x", "y", "z")) and all(
                hasattr(euler, key) for key in ("rx", "ry", "rz")
            ):
                return [
                    float(position.x),
                    float(position.y),
                    float(position.z),
                    float(euler.rx),
                    float(euler.ry),
                    float(euler.rz),
                ]
        return None
