# SPDX-License-Identifier: Apache-2.0
"""LinkerHand SDK (0~255) ↔ URDF 关节弧度映射。逻辑与 ROS2 SDK mapping.py 一致。"""

from __future__ import annotations

# O6
O6_L_MIN = [0, 0, 0, 0, 0, 0]
O6_L_MAX = [0.58, 1.36, 1.6, 1.6, 1.6, 1.6]
O6_L_DIR = [-1, -1, -1, -1, -1, -1]
O6_R_MIN = O6_L_MIN
O6_R_MAX = O6_L_MAX
O6_R_DIR = O6_L_DIR

# G20（新 linkerhand_g20_left.urdf）限位；SDK 索引语义与 L20 相同
L20_L_MIN = [0, 0, 0, 0, 0, 0.0, -0.23, -0.23, -0.23, -0.23, 0.0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
# 通道5 thumb_cmc_roll max：试调 1.47（原 1.4）
L20_L_MAX = [0.84, 1.22, 1.22, 1.22, 1.22, 1.47, 0.23, 0.23, 0.23, 0.23, 1.57, 0, 0, 0, 0, 1.29, 1.55, 1.55, 1.55, 1.55]
L20_L_DIR = [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
L20_R_MIN = [0, 0, 0, 0, 0, 0.0, -0.23, -0.23, -0.23, -0.23, 0.0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
L20_R_MAX = [0.84, 1.22, 1.22, 1.22, 1.22, 1.47, 0.23, 0.23, 0.23, 0.23, 1.57, 0, 0, 0, 0, 1.29, 1.55, 1.55, 1.55, 1.55]
L20_R_DIR = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]

# 左手 G20：四指侧摆 (SDK 6..9) 与弯曲方向相反 —— SDK0→下限、255→上限（见 g20_mapping）
G20_L_MIN = L20_L_MIN
G20_L_MAX = L20_L_MAX
G20_L_DIR = [-1, -1, -1, -1, -1, -1, 1, 1, 1, 1, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
G20_R_MIN = L20_R_MIN
G20_R_MAX = L20_R_MAX
G20_R_DIR = L20_R_DIR

O6_SDK_JOINTS = [
    "thumb_cmc_pitch",
    "thumb_cmc_yaw",
    "index_mcp_pitch",
    "middle_mcp_pitch",
    "ring_mcp_pitch",
    "pinky_mcp_pitch",
]

# SDK 索引 → URDF 关节名（不含 lh_/rh_ 前缀）
L20_SDK_TO_URDF = {
    0: "thumb_cmc_pitch",
    1: "index_mcp_pitch",
    2: "middle_mcp_pitch",
    3: "ring_mcp_pitch",
    4: "pinky_mcp_pitch",
    5: "thumb_cmc_roll",
    6: "index_mcp_roll",
    7: "middle_mcp_roll",
    8: "ring_mcp_roll",
    9: "pinky_mcp_roll",
    10: "thumb_cmc_yaw",
    15: "thumb_mcp",
    16: "index_pip",
    17: "middle_pip",
    18: "ring_pip",
    19: "pinky_pip",
}

# URDF dip 关节 mimic pip：dip = pip * multiplier（G20 URDF）
DIP_MIMIC_MULTIPLIER = 0.89
THUMB_IP_MIMIC_MULTIPLIER = 1.02


def scale_value(original_value: float, a_min: float, a_max: float, b_min: float, b_max: float) -> float:
    if a_max == a_min:
        return b_min
    return (original_value - a_min) * (b_max - b_min) / (a_max - a_min) + b_min


def clamp(value: float, low: float, high: float) -> float:
    return max(low, min(high, value))


def _tables(hand: str, side: str) -> tuple[list[float], list[float], list[int]]:
    if hand == "O6":
        if side == "left":
            return O6_L_MIN, O6_L_MAX, O6_L_DIR
        return O6_R_MIN, O6_R_MAX, O6_R_DIR
    if hand == "G20":
        if side == "left":
            return G20_L_MIN, G20_L_MAX, G20_L_DIR
        return G20_R_MIN, G20_R_MAX, G20_R_DIR
    if side == "left":
        return L20_L_MIN, L20_L_MAX, L20_L_DIR
    return L20_R_MIN, L20_R_MAX, L20_R_DIR


def range_to_arc(values: list[float], hand: str, side: str) -> list[float]:
    """SDK 0~255 → 弧度（每 DOF 一条）。"""
    j_min, j_max, j_dir = _tables(hand, side)
    num = 6 if hand == "O6" else 20
    arcs = [0.0] * num
    for i in range(num):
        if hand in ("L20", "G20") and 11 <= i <= 14:
            continue
        val = clamp(values[i] if i < len(values) else 0.0, 0.0, 255.0)
        if j_dir[i] == -1:
            arcs[i] = scale_value(val, 0.0, 255.0, j_max[i], j_min[i])
        else:
            arcs[i] = scale_value(val, 0.0, 255.0, j_min[i], j_max[i])
    return arcs


def arc_to_range(arcs: list[float], hand: str, side: str) -> list[int]:
    """弧度 → SDK 0~255。"""
    j_min, j_max, j_dir = _tables(hand, side)
    num = 6 if hand == "O6" else 20
    ranges = [0] * num
    for i in range(num):
        if hand in ("L20", "G20") and 11 <= i <= 14:
            ranges[i] = 255
            continue
        val = clamp(arcs[i] if i < len(arcs) else 0.0, j_min[i], j_max[i])
        if j_dir[i] == -1:
            ranges[i] = int(round(scale_value(val, j_min[i], j_max[i], 255.0, 0.0)))
        else:
            ranges[i] = int(round(scale_value(val, j_min[i], j_max[i], 0.0, 255.0)))
    return ranges


def mimic_joint_names(hand: str, side: str) -> set[str]:
    """PhysX/Newton mimic 从动关节，不可施加 position drive。"""
    if hand == "O6":
        prefix = "lh_" if side == "left" else "rh_"
        return {
            f"{prefix}thumb_ip",
            f"{prefix}index_dip",
            f"{prefix}middle_dip",
            f"{prefix}ring_dip",
            f"{prefix}pinky_dip",
        }
    return {"pinky_dip", "ring_dip", "middle_dip", "index_dip", "thumb_ip"}


# G20 URDF / Isaac USD 规范关节名（PLAN 名义弧度桥按名匹配）
G20_ACTUATED_JOINT_NAMES: tuple[str, ...] = tuple(
    sorted({name for name in L20_SDK_TO_URDF.values()})
)
G20_MIMIC_JOINT_NAMES: tuple[str, ...] = (
    "thumb_ip",
    "index_dip",
    "middle_dip",
    "ring_dip",
    "pinky_dip",
)
G20_ALL_JOINT_NAMES: tuple[str, ...] = tuple(
    sorted(set(G20_ACTUATED_JOINT_NAMES) | set(G20_MIMIC_JOINT_NAMES))
)

# mimic 从动 ← (主动, 倍率)；与 G20 URDF / Physics mimic 一致
G20_MIMIC_OF: dict[str, tuple[str, float]] = {
    "thumb_ip": ("thumb_mcp", THUMB_IP_MIMIC_MULTIPLIER),
    "index_dip": ("index_pip", DIP_MIMIC_MULTIPLIER),
    "middle_dip": ("middle_pip", DIP_MIMIC_MULTIPLIER),
    "ring_dip": ("ring_pip", DIP_MIMIC_MULTIPLIER),
    "pinky_dip": ("pinky_pip", DIP_MIMIC_MULTIPLIER),
}


def g20_required_joint_names() -> frozenset[str]:
    """启动校验：Articulation 必须包含的全部 G20 关节名。"""
    return frozenset(G20_ALL_JOINT_NAMES)


def g20_actuated_joint_names() -> frozenset[str]:
    return frozenset(G20_ACTUATED_JOINT_NAMES)


def complete_g20_mimic_positions(positions: dict[str, float]) -> dict[str, float]:
    """补全缺失的 mimic 关节；已给出的 mimic 值优先保留。"""
    out = dict(positions)
    for mimic, (master, ratio) in G20_MIMIC_OF.items():
        if mimic in out:
            continue
        if master in out:
            out[mimic] = float(out[master]) * float(ratio)
    return out


# SDK 张开预设（与 gui_control/config/constants.py 一致）
L20_OPEN_CMD = [
    255, 255, 255, 255, 255, 255, 10, 100, 180, 240, 245,
    255, 255, 255, 255, 255, 255, 255, 255, 255,
]
G20_OPEN_CMD = [
    255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245,
    255, 255, 255, 255, 255, 255, 255, 255, 255,
]


def open_hand_command(hand: str) -> list[float]:
    if hand == "O6":
        return [255.0] * 6
    if hand == "G20":
        return [float(v) for v in G20_OPEN_CMD]
    return [float(v) for v in L20_OPEN_CMD]


def _joint_prefix(side: str, hand: str) -> str:
    if hand == "O6":
        return "lh_" if side == "left" else "rh_"
    return ""


O6_MIMIC_OF = {
    "thumb_ip": ("thumb_cmc_pitch", 2.29),
    "index_dip": ("index_mcp_pitch", 0.89),
    "middle_dip": ("middle_mcp_pitch", 0.89),
    "ring_dip": ("ring_mcp_pitch", 0.89),
    "pinky_dip": ("pinky_mcp_pitch", 0.89),
}


def sdk_range_to_urdf_positions(values: list[float], hand: str, side: str) -> dict[str, float]:
    """将 SDK 指令转为 URDF 关节名 → 弧度。USD 中 mimic 关节由物理引擎跟随。"""
    arcs = range_to_arc(values, hand, side)
    prefix = _joint_prefix(side, hand)
    out: dict[str, float] = {}

    if hand == "O6":
        for sdk_idx, short_name in enumerate(O6_SDK_JOINTS):
            out[f"{prefix}{short_name}"] = arcs[sdk_idx]
        return out

    for sdk_idx, short_name in L20_SDK_TO_URDF.items():
        arc = arcs[sdk_idx]
        if sdk_idx == 15:
            # SDK 控制指尖弧度；thumb_ip mimic thumb_mcp
            out["thumb_mcp"] = arc / THUMB_IP_MIMIC_MULTIPLIER
        elif sdk_idx in (16, 17, 18, 19):
            # SDK 控制 DIP 电机弧度；URDF 中 dip mimic pip
            out[short_name] = arc / DIP_MIMIC_MULTIPLIER
        else:
            out[short_name] = arc
    return out


def sdk_range_to_full_urdf_positions(values: list[float], hand: str, side: str) -> dict[str, float]:
    """主动关节 + 解析 mimic 从动关节，供运动学逐帧写入。"""
    out = sdk_range_to_urdf_positions(values, hand, side)
    prefix = _joint_prefix(side, hand)

    if hand == "O6":
        for mimic_suffix, (master_suffix, ratio) in O6_MIMIC_OF.items():
            master = out.get(f"{prefix}{master_suffix}")
            if master is not None:
                out[f"{prefix}{mimic_suffix}"] = master * ratio
        return out

    for finger in ("index", "middle", "ring", "pinky"):
        pip = out.get(f"{finger}_pip")
        if pip is not None:
            out[f"{finger}_dip"] = pip * DIP_MIMIC_MULTIPLIER
    thumb_mcp = out.get("thumb_mcp")
    if thumb_mcp is not None:
        out["thumb_ip"] = thumb_mcp * THUMB_IP_MIMIC_MULTIPLIER
    return out


def urdf_positions_to_sdk_range(joint_positions: dict[str, float], hand: str, side: str) -> list[int]:
    """从 URDF 主动关节读数反算 SDK 0~255 反馈（不读 mimic 从动关节）。"""
    mapping_hand = hand if hand in ("O6", "G20", "L20") else "L20"
    prefix = _joint_prefix(side, mapping_hand)
    arcs = [0.0] * (6 if mapping_hand == "O6" else 20)

    if mapping_hand == "O6":
        for sdk_idx, short_name in enumerate(O6_SDK_JOINTS):
            key = f"{prefix}{short_name}"
            arcs[sdk_idx] = joint_positions.get(key, 0.0)
        return arc_to_range(arcs, mapping_hand, side)

    for sdk_idx, short_name in L20_SDK_TO_URDF.items():
        if sdk_idx == 15:
            thumb_mcp = joint_positions.get("thumb_mcp", 0.0)
            arcs[sdk_idx] = thumb_mcp * THUMB_IP_MIMIC_MULTIPLIER
        elif sdk_idx in (16, 17, 18, 19):
            pip = joint_positions.get(short_name, 0.0)
            arcs[sdk_idx] = pip * DIP_MIMIC_MULTIPLIER
        else:
            arcs[sdk_idx] = joint_positions.get(short_name, 0.0)
    return arc_to_range(arcs, mapping_hand, side)