"""RealMan 机械臂末端外设配置工具。

scissorgripper 参数约定：
0：工具板 IO 控制的剪刀夹爪
1：Modbus 控制的电动夹爪
2：控制器 DO3/DO4 控制的小型剪刀夹爪

当前 XR 工程在真机连接阶段初始化外设，遥操作节点随后复用同一个
RealMan 连接执行手动 topic 或 trigger toggle 夹爪开合命令。
"""

from __future__ import annotations

import time
from dataclasses import dataclass
from pathlib import Path
from typing import Any

import yaml


@dataclass(frozen=True)
class PeripheralConfig:
    scissorgripper: int
    tools_in_ee: dict[str, list[list[float]]]
    set_initial_tool_state: bool = False

    @property
    def tool_name(self) -> str:
        return list(self.tools_in_ee)[self.scissorgripper]

    @property
    def tool_pose(self) -> list[float]:
        return list(self.tools_in_ee[self.tool_name][0])


def load_peripheral_config(config_file: str, arm: str) -> PeripheralConfig:
    """从 bringup YAML 读取指定左右臂的外设配置。"""
    path = Path(config_file).expanduser()
    if not path.is_file():
        raise FileNotFoundError(f"外设配置文件不存在：{path}")

    with path.open("r", encoding="utf-8") as stream:
        data = yaml.safe_load(stream) or {}
    if not isinstance(data, dict):
        raise ValueError(f"外设配置文件格式错误：{path}")

    tools_in_ee = _load_tools_in_ee(data.get("tools_in_ee"), path)
    arm_configs = data.get("arms", {})
    if not isinstance(arm_configs, dict) or arm not in arm_configs:
        raise ValueError(f"{path} 中缺少 arms.{arm} 外设配置")

    arm_config = arm_configs[arm]
    if not isinstance(arm_config, dict):
        raise ValueError(f"{path} 中 arms.{arm} 必须是映射")

    scissorgripper = int(arm_config["scissorgripper"])
    tool_count = len(tools_in_ee)
    if not -tool_count <= scissorgripper < tool_count:
        raise ValueError(
            f"arms.{arm}.scissorgripper={scissorgripper} 超出工具列表范围"
        )

    set_initial_tool_state = _as_bool(
        arm_config.get(
            "set_initial_tool_state",
            data.get("set_initial_tool_state", False),
        )
    )
    return PeripheralConfig(scissorgripper, tools_in_ee, set_initial_tool_state)


def _load_tools_in_ee(raw_tools: Any, path: Path) -> dict[str, list[list[float]]]:
    if not isinstance(raw_tools, dict) or not raw_tools:
        raise ValueError(f"{path} 中缺少 tools_in_ee 配置")

    tools_in_ee: dict[str, list[list[float]]] = {}
    for tool_name, tool_config in raw_tools.items():
        if not isinstance(tool_config, dict):
            raise ValueError(f"tools_in_ee.{tool_name} 必须是映射")
        pose = _float_list(tool_config.get("pose"), 7, f"tools_in_ee.{tool_name}.pose")
        load = _float_list(tool_config.get("load"), 7, f"tools_in_ee.{tool_name}.load")
        tools_in_ee[str(tool_name)] = [pose, load]
    return tools_in_ee


def _float_list(value: Any, expected_length: int, name: str) -> list[float]:
    if not isinstance(value, (list, tuple)) or len(value) != expected_length:
        raise ValueError(f"{name} 必须包含 {expected_length} 个数值")
    return [float(item) for item in value]


def _as_bool(value: Any) -> bool:
    if isinstance(value, bool):
        return value
    if isinstance(value, str):
        return value.strip().lower() in ("1", "true", "yes", "on")
    return bool(value)


def _tool_name_for_index(tools_in_ee: dict[str, list[list[float]]], scissorgripper: int) -> str:
    return list(tools_in_ee.keys())[scissorgripper]


def _check_sdk_return(result: Any, operation: str) -> None:
    if result != 0:
        raise RuntimeError(f"{operation} failed with code {result}: {result!r}")


def _configure_tool_frame(robot, tool_frame, tool_name: str) -> None:
    frames = robot.rm_get_total_tool_frame()
    if not isinstance(frames, dict):
        raise RuntimeError(
            f"rm_get_total_tool_frame returned invalid data: {frames!r}"
        )
    _check_sdk_return(
        frames.get("return_code"),
        "rm_get_total_tool_frame",
    )
    tool_names = frames.get("tool_names")
    if not isinstance(tool_names, (list, tuple)):
        raise RuntimeError(
            f"rm_get_total_tool_frame returned invalid tool_names: {tool_names!r}"
        )

    if tool_name in tool_names:
        operation = "rm_update_tool_frame"
        result = robot.rm_update_tool_frame(frame=tool_frame)
    else:
        operation = "rm_set_manual_tool_frame"
        result = robot.rm_set_manual_tool_frame(frame=tool_frame)
    _check_sdk_return(result, operation)
    _check_sdk_return(
        robot.rm_change_tool_frame(tool_name),
        "rm_change_tool_frame",
    )


def cal_tool_frame(handle, scissorgripper, tools_in_ee):
    """根据末端工具配置生成 RealMan 工具坐标系。"""
    from Robotic_Arm.rm_robot_interface import rm_frame_t

    tool_name = _tool_name_for_index(tools_in_ee, scissorgripper)
    tool_pose = tools_in_ee[tool_name][0]
    tool_load = tools_in_ee[tool_name][1]
    eu = handle.rm_algo_quaternion2euler([tool_pose[6], tool_pose[3], tool_pose[4], tool_pose[5]])
    tool_frame = rm_frame_t(tool_name, [tool_pose[0], tool_pose[1], tool_pose[2], eu[0], eu[1], eu[2]], tool_load[0],
                            tool_load[1], tool_load[2], tool_load[3])
    return tool_frame, tool_name


def alg_init(scissorgripper, tools_in_ee):
    """初始化算法模型、工具坐标系和关节运动限制。"""
    from Robotic_Arm.rm_robot_interface import Algo, rm_force_type_e, rm_robot_arm_model_e

    arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E
    force_type = rm_force_type_e.RM_MODEL_RM_B_E
    algo_handle = Algo(arm_model, force_type)
    tool_frame, tool_name = cal_tool_frame(algo_handle, scissorgripper, tools_in_ee)
    algo_handle.rm_algo_set_toolframe(tool_frame)

    algo_handle.rm_algo_set_joint_max_limit([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0])
    algo_handle.rm_algo_set_joint_min_limit([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0])
    algo_handle.rm_algo_set_joint_max_speed([60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0])
    algo_handle.rm_algo_set_joint_max_acc([
        166.6666717529297,
        166.6666717529297,
        166.6666717529297,
        166.6666717529297,
        166.6666717529297,
        166.6666717529297,
        166.6666717529297,
    ])
    algo_handle.rm_algo_set_redundant_parameter_traversal_mode(False)

    return algo_handle


def peripheral_cfg(
    robot,
    scissorgripper,
    tools_in_ee,
    *,
    set_initial_tool_state: bool = False,
):
    """配置机械臂控制器、当前工具坐标系和末端工具通信方式。"""
    # 控制器输出 12V；IO1/IO2 配置为输入，IO3/IO4 配置为输出。
    robot.rm_set_voltage(2, True)
    time.sleep(0.2)
    robot.rm_set_io_mode(1, 0, 50, 2)
    robot.rm_set_io_mode(2, 0, 50, 2)
    robot.rm_set_io_mode(3, 1, 50, 2)
    robot.rm_set_io_mode(4, 1, 50, 2)

    time.sleep(0.2)
    tool_frame, tool_name = cal_tool_frame(robot, scissorgripper, tools_in_ee)
    _configure_tool_frame(robot, tool_frame, tool_name)

    if scissorgripper == 0:
        # 剪刀夹爪通过工具板数字输出控制。
        robot.rm_set_tool_voltage(voltage_type=2)
        time.sleep(0.2)
        robot.rm_set_tool_IO_mode(1, 1)
        robot.rm_set_tool_IO_mode(2, 1)

        if set_initial_tool_state:
            robot.rm_set_tool_do_state(1, 0)
            robot.rm_set_tool_do_state(2, 1)
    elif scissorgripper == 1:
        # 电动夹爪通过工具板 Modbus 寄存器控制。
        from Robotic_Arm.rm_robot_interface import rm_peripheral_read_write_params_t

        modbus_sts = robot.rm_set_modbus_mode(port=1, baudrate=115200, timeout=2)
        if modbus_sts != 0:
            print(f"警告：Modbus 模式设置失败：{modbus_sts}")
        else:
            print("Modbus 模式配置成功")

        addr = 1
        # 依次设置目标速度、目标力矩、目标加速度和目标减速度。
        reg_value = [255, 60, 255, 255]
        for i, reg_addr in enumerate([11, 12, 13, 14]):
            write_params = rm_peripheral_read_write_params_t(1, reg_addr, addr, 1)
            robot.rm_write_single_register(write_params, reg_value[i])
            time.sleep(0.5)

        if set_initial_tool_state:
            set_tool_position(robot, percent=0.75, device=1, scissorgripper=scissorgripper)
            time.sleep(1.5)
            set_tool_position(robot, percent=0.15, device=1, scissorgripper=scissorgripper)

    elif scissorgripper == 2:
        # 小型剪刀夹爪通过控制器 DO3/DO4 控制。
        if set_initial_tool_state:
            robot.rm_set_do_state(io_num=3, state=1)
            robot.rm_set_do_state(io_num=4, state=0)
            time.sleep(2)
            robot.rm_set_do_state(io_num=3, state=0)
            robot.rm_set_do_state(io_num=4, state=1)

    return robot


def set_tool_position(robot, percent, device=1, scissorgripper=0):
    """设置末端工具开合位置：0.0 表示闭合，1.0 表示完全张开。"""
    if scissorgripper == 1:
        from Robotic_Arm.rm_robot_interface import rm_peripheral_read_write_params_t

        pos_value = int(percent * 255)
        write_params = rm_peripheral_read_write_params_t(1, 10, device, 1)
        robot.rm_write_single_register(write_params, pos_value)
        # 写触发寄存器后夹爪才会执行目标位置。
        write_params = rm_peripheral_read_write_params_t(1, 15, device, 1)
        time.sleep(0.5)
        robot.rm_write_single_register(write_params, 0x01)
        time.sleep(0.5)

    elif scissorgripper == 0:
        # 剪刀夹爪只有全闭合和全张开两个状态。
        if percent == 0:
            robot.rm_set_tool_do_state(1, 1)
            robot.rm_set_tool_do_state(2, 0)
        elif percent == 1:
            robot.rm_set_tool_do_state(1, 0)
            robot.rm_set_tool_do_state(2, 1)
    elif scissorgripper == 2:
        # 小型剪刀夹爪同样只有全闭合和全张开两个状态。
        if percent == 0:
            robot.rm_set_do_state(3, 1)
            robot.rm_set_do_state(4, 0)
        elif percent == 1:
            robot.rm_set_do_state(3, 0)
            robot.rm_set_do_state(4, 1)


def tool_exe(robot, scissorgripper, cmd):
    """根据布尔命令执行末端工具开合。"""
    switch_state = cmd
    if switch_state:
        set_tool_position(robot, 1, device=1, scissorgripper=scissorgripper)
    else:
        set_tool_position(robot, 0, device=1, scissorgripper=scissorgripper)