"""A7 / ARM7 gold kinematics from lkls73_i1_arm_description/urdf/ARM7_urdf.urdf.

STEP Viewer / step2urdf UI stores linear joint origins in **millimetres**;
export applies ×0.001 → metres. Values below are therefore in mm so they match
the interactive UI and still export to the gold URDF metre numbers.
"""

from __future__ import annotations

from app.models import JointDef, LinkDef, RobotDraft

# UI / CAD units (mm). Gold URDF metres × 1000.
A7_LINK_ORDER = [
    "base_link",
    "A1_Link",
    "A2_Link",
    "A3_Link",
    "A4_Link",
    "A5_Link",
    "A6_Link",
    "A7_Link",
    "A8_Link",
]

# (name, parent, child, type, xyz_mm, axis, lower, upper, effort, velocity)
A7_GOLD_JOINTS_MM: list[tuple] = [
    ("A1_joint", "base_link", "A1_Link", "revolute", [0.0, 0.0, 9.0], [0.0, 0.0, -1.0], -2.9, 1.0, 80.0, 10.47),
    ("A2_joint", "A1_Link", "A2_Link", "revolute", [0.0, 0.0, 59.5], [1.0, 0.0, 0.0], -0.15, 3.14, 80.0, 10.47),
    ("A3_joint", "A2_Link", "A3_Link", "revolute", [0.0, 0.0, 123.0], [0.0, 0.0, -1.0], -2.35, 2.35, 36.0, 9.1),
    ("A4_joint", "A3_Link", "A4_Link", "revolute", [0.0, -0.10871, 187.1], [1.0, 0.0, 0.0], 0.0, 2.2, 36.0, 9.1),
    ("A5_joint", "A4_Link", "A5_Link", "revolute", [0.0, -0.13906, 124.0], [0.0, 0.0, -1.0], -2.35, 2.35, 36.0, 9.1),
    ("A6_joint", "A5_Link", "A6_Link", "revolute", [0.0, -0.20755, 88.099], [1.0, 0.0, 0.0], -1.57, 1.57, 36.0, 9.1),
    ("A7_joint", "A6_Link", "A7_Link", "revolute", [0.60033, 0.45533, 65.501], [0.0, 1.0, 0.0], -1.57, 1.57, 36.0, 9.1),
    ("A8_joint", "A7_Link", "A8_Link", "fixed", [-0.041388, -1.1787, 44.999], [0.0, 0.0, 0.0], 0.0, 0.0, 0.0, 0.0),
]


def gold_joint_defs() -> list[JointDef]:
    out: list[JointDef] = []
    for name, parent, child, jtype, xyz, axis, lo, hi, eff, vel in A7_GOLD_JOINTS_MM:
        out.append(
            JointDef(
                name=name,
                joint_type=jtype,  # type: ignore[arg-type]
                parent=parent,
                child=child,
                origin_xyz=list(xyz),
                origin_rpy=[0.0, 0.0, 0.0],
                axis=list(axis),
                lower=lo,
                upper=hi,
                effort=eff,
                velocity=vel,
                rationale="ARM7_urdf.urdf gold kinematics (origins in mm for step2urdf UI).",
            )
        )
    return out


def empty_gold_links() -> list[LinkDef]:
    return [LinkDef(name=n, part_names=[]) for n in A7_LINK_ORDER]


def enforce_a7_gold_kinematics(draft: RobotDraft) -> RobotDraft:
    """Keep part_names from draft when link names match; always replace joints with gold.

    Also remaps common aliases (link1 → A1_Link, etc.) so LLM/profile seeds still work.
    """
    alias = {
        "link1": "A1_Link",
        "link2": "A2_Link",
        "link3": "A3_Link",
        "link4": "A4_Link",
        "link5": "A5_Link",
        "link6": "A6_Link",
        "link7": "A7_Link",
        "link_1": "A1_Link",
        "link_2": "A2_Link",
        "link_3": "A3_Link",
        "link_4": "A4_Link",
        "link_5": "A5_Link",
        "link_6": "A6_Link",
        "link_7": "A7_Link",
        "a1_link": "A1_Link",
        "a2_link": "A2_Link",
        "a3_link": "A3_Link",
        "a4_link": "A4_Link",
        "a5_link": "A5_Link",
        "a6_link": "A6_Link",
        "a7_link": "A7_Link",
        "a8_link": "A8_Link",
    }

    buckets: dict[str, list[str]] = {n: [] for n in A7_LINK_ORDER}
    for link in draft.links:
        key = alias.get(link.name, alias.get(link.name.lower(), link.name))
        if key not in buckets:
            # Unknown link → fold into base
            key = "base_link"
        for pn in link.part_names:
            if pn not in buckets[key]:
                buckets[key].append(pn)

    links = [LinkDef(name=n, part_names=buckets[n]) for n in A7_LINK_ORDER]
    joints = gold_joint_defs()
    notes = list(draft.notes) + [
        "A7 关节轴/原点已强制替换为 ARM7 金标（原点单位 mm，导出时 ×0.001 → m）。",
        "智谱仅用于零件归组；请勿依赖模型猜测的 axis/rpy。",
        "轴规律: A1/A3/A5 → [0,0,-1]; A2/A4/A6 → [1,0,0]; A7 → [0,1,0]; A8 fixed。",
    ]
    return RobotDraft(
        name=draft.name or "ARM7_urdf",
        profile="a7",
        links=links,
        joints=joints,
        notes=notes,
        raw_parts=draft.raw_parts,
        axis_candidates=draft.axis_candidates,
        llm_raw=draft.llm_raw,
    )