"""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().
"""
import math
import bpy
from mathutils import Vector, Matrix

J2 = Vector((-0.341, 0.0, 0.517))
J3 = Vector((-1.21, 0.0, 1.826))
J4 = Vector((-0.077, 0.0, 2.0614))
WC = Vector((0.957, 0.0, 2.0614))
TRUNNION = Vector((-0.8646, 0.0, 0.847))
KNOB = Vector((-1.6764, 0.0, 1.4564))
PINION = Vector((0.0, 0.0, -0.100))

# 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 _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"))

BY_NAME = {l["name"]: l for l in LINKS}


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


def finish_link(L, spec):
    """the Link's coordinates are arm-frame A (rest); the mesh origin goes on the 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)
    for k in ("gland_U08", "gland_L17"):
        if hasattr(L, k):
            me[k] = list(Vector(getattr(L, k)) - shift)
    me["joint_A"] = list(spec["joint"])
    me["parent_link"] = spec["parent"] or ""
    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
    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"] if spec["parent"] else Vector((0, 0, 0))
        ob.location = Vector(spec["joint"]) - Vector(pj)
        ob.rotation_euler = (0, 0, 0)
        ob["link"] = spec["name"]
        objs[spec["name"]] = ob
    return root, objs