"""Door robot arm: the links (moving objects) of one arm, their joints and how to make the objects.

Every link is one mesh 'DoorArm | <name>'. Its origin is on its joint axis; at rest (the ref1 pose, all joints 0)
every object has rotation 0 and scale 1, so an object's location relative to its parent is just the difference of the
two joint points in the arm frame A (design.json 'kinematics'). The showroom and (stage 2) the ship rig both make
their objects from this table with instantiate().

Scale-up (2026-10-18, design.json 'scale_up'): the part builders work in build units (the first design's size); the
joints here are in build units with the new link lengths (J4 moved along x, the wrist centre further out, the knob
closer to J3). finish_link() stretches the upper arm and the forearm to their new lengths (per-part bands along x,
BANDS) and then scales every mesh by S about its joint; the scaled joints (joint_s, J2S ...) place the objects.
"""
import json
import math
import os
import bpy
from mathutils import Vector, Matrix

_HERE = os.path.dirname(os.path.abspath(__file__))
with open(os.path.join(os.path.dirname(_HERE), "design.json")) as _f:
    _SU = json.load(_f).get("scale_up") or {}
S = float(_SU.get("S", 1.0))
_BU = _SU.get("build_units", {})
BANDS = _SU.get("bands", {})

J2 = Vector(_BU.get("J2", (-0.341, 0.0, 0.517)))
J3 = Vector(_BU.get("J3", (-1.21, 0.0, 1.826)))
J4 = Vector(_BU.get("J4", (-0.077, 0.0, 2.0614)))
WC = Vector(_BU.get("WC", (0.957, 0.0, 2.0614)))
TRUNNION = Vector(_BU.get("TRUNNION", (-0.8646, 0.0, 0.847)))
KNOB = Vector(_BU.get("KNOB", (-1.6764, 0.0, 1.4564)))
PINION = Vector((0.0, 0.0, -0.100))
# the first design's joints (the builders' own frames are laid out on them)
J4_0 = Vector((-0.077, 0.0, 2.0614))
WC_0 = Vector((0.957, 0.0, 2.0614))
# the link-length changes along x in each link's own frame (build units)
STRETCH = {"upper arm": J4 - J3 - (J4_0 - Vector((-1.21, 0.0, 1.826))), "forearm": (WC - J4) - (WC_0 - J4_0)}
J2S, J3S, J4S, WCS, TRUNNIONS, KNOBS = (v * S for v in (J2, J3, J4, WC, TRUNNION, KNOB))

# fingers (design.json 'fingers', ref1 px in the tool frame: X' along the tool axis from WC, r radial)
PX = 0.0022
KNUCKLE = (107.0 * PX, 79.0 * PX)            # (x', r)
PROX_LEN = 82.0 * PX
PROX_OPEN = math.radians(24.7)               # proximal angle from the tool axis, outward, open (rest)
DIST_LEN = 70.0 * PX
DIST_OPEN_ABS = math.radians(4.9)            # distal angle from the tool axis, open
FINGERS = {                                  # roll angle of the finger plane about the tool axis (local r -> )
    "top": 0.0, "left": -math.pi / 2, "bottom": math.pi, "right": math.pi / 2,
}


def roll(a):
    """rotation about the tool axis X taking the 'top' finger (r along +Z) to the finger at roll a"""
    return Matrix.Rotation(a, 4, 'X')


def finger_points(which):
    """knuckle and distal pin of one finger in A coordinates at rest"""
    kx, kr = KNUCKLE
    pin = Vector((kx + PROX_LEN * math.cos(PROX_OPEN), 0, kr + PROX_LEN * math.sin(PROX_OPEN)))
    Rr = roll(FINGERS[which])
    k = WC + (Rr @ Vector((kx, 0, kr)))
    p = WC + (Rr @ pin)
    return k, p


def finger_points_s(which):
    """knuckle and distal pin of one finger, scaled (metres in A)"""
    k, p = finger_points(which)
    return k * S, p * S


def _b(mod, fn):
    def call():
        import importlib
        m = importlib.import_module(mod)
        return getattr(m, fn)()
    return call


def _finger(which, part):
    def call():
        import importlib
        m = importlib.import_module("dra_parts_wrist")
        return m.build_finger(which, part)
    return call


LINKS = [
    dict(name="carriage", parent=None, joint=Vector((0, 0, 0)), build=_b("dra_parts_base", "build_carriage"), tool=None),
    dict(name="carriage pinion", parent="carriage", joint=PINION, build=_b("dra_parts_base", "build_pinion"), tool=None),
    dict(name="turret", parent="carriage", joint=Vector((0, 0, 0)), build=_b("dra_parts_turret", "build_turret"), tool=None),
    dict(name="lower arm", parent="turret", joint=J2, build=_b("dra_parts_arm", "build_lower_arm"), tool=None),
    dict(name="J3 cylinder", parent="lower arm", joint=TRUNNION, build=_b("dra_parts_arm", "build_cylinder"), tool=None),
    dict(name="J3 piston", parent="J3 cylinder", joint=KNOB, build=_b("dra_parts_arm", "build_piston"), tool=None),
    dict(name="upper arm", parent="lower arm", joint=J3, build=_b("dra_parts_arm", "build_upper_arm"), tool=None),
    dict(name="forearm", parent="upper arm", joint=J4, build=_b("dra_parts_wrist", "build_forearm"), tool=None),
    dict(name="wrist head", parent="forearm", joint=WC, build=_b("dra_parts_wrist", "build_wrist_head"), tool=None),
    dict(name="flange", parent="wrist head", joint=WC, build=_b("dra_parts_wrist", "build_flange"), tool=None),
    dict(name="tool A hub", parent="flange", joint=WC, build=_b("dra_parts_wrist", "build_tool_a"), tool="A"),
    dict(name="tool B", parent="flange", joint=WC, build=_b("dra_parts_wrist", "build_tool_b"), tool="B"),
]
for _w in ("top", "left", "bottom", "right"):
    _k, _p = finger_points(_w)
    LINKS.append(dict(name="finger %s proximal" % _w, parent="tool A hub", joint=_k, build=_finger(_w, "proximal"), tool="A"))
    LINKS.append(dict(name="finger %s distal" % _w, parent="finger %s proximal" % _w, joint=_p, build=_finger(_w, "distal"),
                      tool="A"))

for _l in LINKS:
    _l["joint_s"] = Vector(_l["joint"]) * S
BY_NAME = {l["name"]: l for l in LINKS}


def mesh_name(link_name):
    return "DoorArm | " + link_name


def _band_shift(x, band, d):
    """how much of the stretch d a point at x gets: 0 before the band, d after it, linear inside"""
    x0, x1 = band
    if x <= x0:
        return 0.0
    if x >= x1 or x1 - x0 < 1e-9:
        return d
    return d * (x - x0) / (x1 - x0)


def stretch_mesh(me, link):
    """lengthen a link along its x by STRETCH[link]: each part by its own band (BANDS), vertices by their x"""
    if link not in STRETCH or link not in BANDS:
        return
    d = STRETCH[link]
    if abs(d.x) < 1e-9:
        return
    bands = BANDS[link]
    parts = list(me.get("parts", []))
    lay = me.attributes.get("part")
    vband = [None] * len(me.vertices)
    for p in me.polygons:
        pid = lay.data[p.index].value if lay is not None else -1
        nm = parts[pid] if 0 <= pid < len(parts) else ""
        b = bands.get(nm) or bands.get(nm.split(".")[0]) or bands["default"]
        for vi in p.vertices:
            if vband[vi] is None:
                vband[vi] = b
    for v, b in zip(me.vertices, vband):
        b = b or bands["default"]
        f = _band_shift(v.co.x, b, 1.0)
        v.co = v.co + d * f
    me.update()


def stretch_point(link, p):
    """a point of a link's frame (build units) after the stretch (its default band)"""
    if link not in STRETCH or link not in BANDS:
        return Vector(p)
    return Vector(p) + STRETCH[link] * _band_shift(p[0], BANDS[link]["default"], 1.0)


def finish_link(L, spec):
    """the Link's coordinates are arm-frame A (rest, build units); the mesh origin goes on the joint; then the link is
    stretched to its new length and scaled by S about its joint"""
    import dra_lib
    base = getattr(L, "base", Vector((0, 0, 0)))
    shift = Vector(spec["joint"]) - Vector(base)
    me = dra_lib.finish(L, mesh_name(spec["name"]), origin=shift)
    stretch_mesh(me, spec["name"])
    if abs(S - 1.0) > 1e-9:
        me.transform(Matrix.Scale(S, 4))
    for k in ("gland_U08", "gland_L17"):
        if hasattr(L, k):
            me[k] = list(stretch_point(spec["name"], Vector(getattr(L, k)) - shift) * S)
    me["joint_A"] = list(spec["joint_s"])
    me["parent_link"] = spec["parent"] or ""
    me["scale_up"] = S
    return me


def instantiate(collection, prefix, root_matrix, meshes, tools=("A", "B"), extra_props=None):
    """make the object hierarchy of one arm at rest: root empty '<prefix>' with root_matrix, children by LINKS.
    Objects share the link meshes (linked duplicates). Returns {link name: object}."""
    root = bpy.data.objects.new(prefix, None)
    root.empty_display_type = 'ARROWS'
    root.empty_display_size = 0.4 * S
    collection.objects.link(root)
    root.matrix_world = root_matrix
    objs = {}
    for spec in LINKS:
        if spec["tool"] and spec["tool"] not in tools:
            continue
        me = meshes.get(spec["name"])
        if me is None:
            continue
        ob = bpy.data.objects.new("%s | %s" % (prefix, spec["name"]), me)
        collection.objects.link(ob)
        par = objs.get(spec["parent"]) if spec["parent"] else root
        ob.parent = par
        pj = BY_NAME[spec["parent"]]["joint_s"] if spec["parent"] else Vector((0, 0, 0))
        ob.location = Vector(spec["joint_s"]) - Vector(pj)
        ob.rotation_euler = (0, 0, 0)
        ob["link"] = spec["name"]
        objs[spec["name"]] = ob
    return root, objs