"""The cell's six-axis robot: dimensions, forward kinematics, the mesh (rigid-skinned to its bones) and the armature.
Exec'd by rc_build.py, rc_fitpose.py and the checks after rc_lib.py.

Robot frame (the armature object's space): origin on the A1 axis at the base's underside (on the pedestal's top),
z up, the arm along +x in the bind pose, +y to the robot's left. The picture shows the robot's right side (-y): the
upper arm, the big black A2 gear cover and the A3 cover are on that side, as on the picture.

Bind pose (all joint angles 0): upper arm vertical, forearm horizontal, wrist straight, tool along +x.
Joints (Blender bones; bend joints turn about the bone's local X = robot +y, roll joints about the bone's Y):
    A1 base turn (about z)          A2 shoulder (about y, + leans the upper arm forward)
    A3 elbow (about y, + tips the forearm down)    A4 forearm roll (about its own axis)
    A5 wrist bend (about y, + down)                A6 flange roll          Tool_Spin the tool's nozzle (spins)
Sizes from the picture through its camera (rc_lib.py): a mid-size robot, upper arm 0.82 m, forearm 0.70 m.
"""
import math
from mathutils import Vector, Matrix

# ------------------------------------------------------------------ dimensions (m)
R_BASE = 0.30          # base flange radius (the picture's black base ring is 0.66 m across at its depth)
H_BASE = 0.12          # fixed base height; the turret turns above it
P_A2 = Vector((0.04, 0.0, 0.40))     # shoulder axis
L2 = 0.82              # A2 -> A3 (upper arm)
X4, A3OFF = 0.24, 0.05  # A4 joint along the forearm from A3, and the forearm axis above A3
L3 = 0.70              # A3 -> A5 along the forearm
X6 = 0.09              # A5 -> A6 joint
L5 = 0.14              # A5 -> flange face
LT = 0.40              # flange -> tool tip
Y_ARM0, Y_ARM1 = -0.29, -0.155       # the upper arm's slab (its outer face toward -y)
Y_TUR0, Y_TUR1 = -0.14, 0.22         # the turret's shoulder block

P_A3 = P_A2 + Vector((0, 0, L2))
P_A4 = P_A3 + Vector((X4, 0, A3OFF))
P_A5 = P_A3 + Vector((L3, 0, A3OFF))
P_A6 = P_A5 + Vector((X6, 0, 0))
P_FL = P_A5 + Vector((L5, 0, 0))
P_TIP = P_FL + Vector((LT, 0, 0))

LIMITS = {   # degrees (my convention, see above). A1, A4, A6 as a mid-size robot's; A2, A3 and A5 narrowed so that no
    # joint alone can fold the arm into itself from the picture's pose (rc_check.py sweeps each one): A2 past 90 puts
    # the upper arm on the turret's skirt, A3 past 48 the tool into the base, A5 past 95 the flange into the fork
    "A1": (-185, 185), "A2": (-60, 90), "A3": (-120, 48), "A4": (-350, 350), "A5": (-95, 95), "A6": (-350, 350)}
JOINTS = ["A1", "A2", "A3", "A4", "A5", "A6"]
ROB = Vector((-1.06, -0.05, 0.62))   # where the robot stands in the cell: on its pedestal, left of the middle


def fk(th):
    """joint frames in the robot frame for angles th (radians, dict); returns dict of 4x4 matrices"""
    g = lambda k: th.get(k, 0.0)
    T = {}
    T["A1"] = Matrix.Rotation(g("A1"), 4, 'Z')
    T["A2"] = T["A1"] @ Matrix.Translation(P_A2) @ Matrix.Rotation(g("A2"), 4, 'Y')
    T["A3"] = T["A2"] @ Matrix.Translation((0, 0, L2)) @ Matrix.Rotation(g("A3"), 4, 'Y')
    T["A4"] = T["A3"] @ Matrix.Translation((X4, 0, A3OFF)) @ Matrix.Rotation(g("A4"), 4, 'X')
    T["A5"] = T["A4"] @ Matrix.Translation((L3 - X4, 0, 0)) @ Matrix.Rotation(g("A5"), 4, 'Y')
    T["A6"] = T["A5"] @ Matrix.Translation((X6, 0, 0)) @ Matrix.Rotation(g("A6"), 4, 'X')
    T["FL"] = T["A6"] @ Matrix.Translation((L5 - X6, 0, 0))
    T["TIP"] = T["FL"] @ Matrix.Translation((LT, 0, 0))
    return T


# landmarks on the picture (pixels) and where they sit on the robot (joint frame, local point)
MARKS = {"A2_cover": ("A2", (0, -0.31, 0), (340.0, 545.0)),
         "A3_cover": ("A3", (0, -0.30, 0), (410.0, 328.0)),
         "A5_hub":   ("A5", (0, -0.10, 0), (562.0, 410.0)),
         "flange":   ("FL", (0, 0, 0), (590.0, 432.0)),
         "tip":      ("TIP", (0, 0, 0), (630.0, 552.0))}


# ------------------------------------------------------------------ a mesh builder that remembers each vertex's bone
class MBG(MB):
    def __init__(s):
        super().__init__()
        s.grp = []
        s.cur = {"Base": 1.0}

    def v(s, co):
        s.co.append(Vector(co))
        s.grp.append(s.cur)
        return len(s.co) - 1

    def bone(s, name):
        s.cur = {name: 1.0}


def hull2(pts):
    """convex hull (CCW) of 2D points"""
    P = sorted(set((round(a, 6), round(b, 6)) for a, b in pts))
    if len(P) < 3:
        return P
    cr = lambda o, a, b: (a[0] - o[0]) * (b[1] - o[1]) - (a[1] - o[1]) * (b[0] - o[0])
    lo, up = [], []
    for p in P:
        while len(lo) >= 2 and cr(lo[-2], lo[-1], p) <= 0:
            lo.pop()
        lo.append(p)
    for p in reversed(P):
        while len(up) >= 2 and cr(up[-2], up[-1], p) <= 0:
            up.pop()
        up.append(p)
    return lo[:-1] + up[:-1]


def circ(c, r, n=24, a0=0.0):
    return [(c[0] + r * math.cos(a0 + 2 * math.pi * k / n), c[1] + r * math.sin(a0 + 2 * math.pi * k / n)) for k in range(n)]


def yprism(mb, P_xz, y0, y1, mat, ch=0.008):
    """a chamfered prism of an (x, z) outline from y0 (front, -y side) to y1"""
    return prism(mb, P_xz, y0, y1, mat, ch=ch)


def axbox(mb, x0, x1, y0, y1, z0, z1, mat, ch=0.01):
    c = max(0.0, min(ch, (x1 - x0) * 0.3, (z1 - z0) * 0.3, (y1 - y0) * 0.3))
    P = ([(x0 + c, z0), (x1 - c, z0), (x1, z0 + c), (x1, z1 - c), (x1 - c, z1), (x0 + c, z1), (x0, z1 - c), (x0, z0 + c)]
         if c > 1e-5 else [(x0, z0), (x1, z0), (x1, z1), (x0, z1)])
    prism(mb, P, y0, y1, mat, ch=c)


def cyl(mb, prof, O, A, nseg, mats, open0=False, open1=False, **kw):
    """lathe() with quad (Coons) caps instead of fans: a profile starting or ending at r 0 is closed by a cap"""
    prof = list(prof)
    mats = [mats] * (len(prof) - 1) if isinstance(mats, str) else list(mats)
    c0 = c1 = False
    if prof[0][0] <= 1e-9:
        prof, mats, c0 = prof[1:], mats[1:], not open0
    if prof[-1][0] <= 1e-9:
        prof, mats, c1 = prof[:-1], mats[:-1], not open1
    if len(prof) < 2:
        return None
    return lathe(mb, prof, O, A, nseg, mats, cap0=c0, cap1=c1, **kw)


def annulus(mb, O, A, r0, r1, t0, t1, nseg, mat):
    """a closed ring (washer) round axis A: inner r0, outer r1, from t0 to t1"""
    A, d1, d2 = frame_axes(A)
    O = Vector(O)
    ring = lambda r, t: [mb.v(O + A * t + r * (math.cos(2 * math.pi * k / nseg) * d1 + math.sin(2 * math.pi * k / nseg) * d2))
                         for k in range(nseg)]
    R = [ring(r0, t0), ring(r1, t0), ring(r1, t1), ring(r0, t1)]
    for i in range(4):
        mb.bridge(R[i], R[(i + 1) % 4], mat)


def bolt_ring(mb, centre, axis, rad, n, r, h, mat, phase=0.0, e1=None):
    A, d1, d2 = frame_axes(axis)
    if e1 is not None:
        d1 = (Vector(e1) - A * Vector(e1).dot(A)).normalized()
        d2 = A.cross(d1).normalized()
    for k in range(n):
        a = phase + 2 * math.pi * k / n
        bolt(mb, Vector(centre) + rad * (math.cos(a) * d1 + math.sin(a) * d2), A, r, h, mat, nseg=8)


def cable(mb, pts, radius, weights, mat, nseg=8, n_per=6, ribs=True):
    """a corrugated hose through 3D points; weights(t) -> {bone: w} along its length (t 0..1)"""
    C = catmull(pts, n_per)
    T = [(C[min(i + 1, len(C) - 1)] - C[max(i - 1, 0)]).normalized() for i in range(len(C))]
    ref = Vector((0, 0, 1)) if abs(T[0].z) < 0.9 else Vector((1, 0, 0))
    n1 = (ref - T[0] * ref.dot(T[0])).normalized()
    L = [0.0]
    for a, b in zip(C, C[1:]):
        L.append(L[-1] + (b - a).length)
    rings = []
    for i, c in enumerate(C):
        if i:
            n1 = (n1 - T[i] * n1.dot(T[i])).normalized()
        n2 = T[i].cross(n1)
        mb.cur = weights(L[i] / L[-1])
        rr = radius * (1.0 + (0.10 if (ribs and i % 2) else 0.0))
        rings.append([mb.v(c + rr * (math.cos(2 * math.pi * k / nseg) * n1 + math.sin(2 * math.pi * k / nseg) * n2))
                      for k in range(nseg)])
    for a, b in zip(rings, rings[1:]):
        mb.bridge(a, b, mat)
    q = nseg // 4
    mb.cur = weights(0.0)
    mb.coons(rings[0], [0, q, 2 * q, 3 * q], mat)
    mb.cur = weights(1.0)
    mb.coons(rings[-1], [0, q, 2 * q, 3 * q], mat)


def blend(*stops):
    """weights along a cable: stops [(t, bone)], linear between neighbours"""
    def w(t):
        for (t0, b0), (t1, b1) in zip(stops, stops[1:]):
            if t <= t1:
                f = 0.0 if t1 <= t0 else min(max((t - t0) / (t1 - t0), 0.0), 1.0)
                if b0 == b1 or f <= 0.0:
                    return {b0: 1.0}
                if f >= 1.0:
                    return {b1: 1.0}
                return {b0: 1.0 - f, b1: f}
        return {stops[-1][1]: 1.0}
    return w


# ------------------------------------------------------------------ the mesh (bind pose, robot frame)
ROBOT_MATS = ["M_RC_Orange", "M_RC_Black", "M_RC_Rubber", "M_RC_Copper", "M_RC_Chrome", "M_RC_Gap", "M_RC_Decal"]


def build_robot_mesh():
    mb = MBG()
    O, B, R, CU, CH, G = "M_RC_Orange", "M_RC_Black", "M_RC_Rubber", "M_RC_Copper", "M_RC_Chrome", "M_RC_Gap"
    Z = Vector((0, 0, 1)); Yv = Vector((0, 1, 0)); Xv = Vector((1, 0, 0))
    # --- fixed base (black): a bolted flange, a drum, a ring
    mb.bone("Base")
    cyl(mb, [(0.0, 0.001), (R_BASE, 0.001), (R_BASE, 0.030), (R_BASE - 0.006, 0.036), (0.270, 0.036), (0.270, 0.098),
               (0.290, 0.100), (0.290, 0.116), (0.282, 0.120), (0.0, 0.120)], (0, 0, 0), Z, 40, B, open0=True)
    bolt_ring(mb, (0, 0, 0.036), Z, 0.286, 8, 0.011, 0.010, CH, phase=math.pi / 8)
    cyl(mb, [(0.0, 0.0), (0.030, 0.0), (0.030, 0.034), (0.0, 0.034)], (-0.262, 0.0, 0.070), -Xv, 12, B)  # cable socket
    # --- turret (A1, orange)
    mb.bone("A1")
    cyl(mb, [(0.0, 0.121), (0.300, 0.121), (0.300, 0.146), (0.292, 0.154), (0.268, 0.156), (0.262, 0.205),
               (0.0, 0.205)], (0, 0, 0), Z, 40, O)
    bolt_ring(mb, (0, 0, 0.154), Z, 0.282, 12, 0.008, 0.007, B)
    axbox(mb, -0.21, 0.20, Y_TUR0, Y_TUR1, 0.200, 0.43, O, ch=0.03)
    cyl(mb, [(0.0, Y_TUR0), (0.185, Y_TUR0), (0.195, Y_TUR0 + 0.012), (0.195, Y_TUR1 - 0.012), (0.185, Y_TUR1),
               (0.0, Y_TUR1)], (P_A2.x, 0, P_A2.z), Yv, 32, O)                                        # round shoulder
    cyl(mb, [(0.0, Y_TUR1 - 0.005), (0.105, Y_TUR1 - 0.005), (0.105, 0.33), (0.098, 0.34), (0.088, 0.34),
               (0.088, 0.365), (0.0, 0.365)], (P_A2.x, 0, P_A2.z), Yv, 28, [O, O, O, O, B, B])        # A2 motor, cap
    cyl(mb, [(0.0, 0.200), (0.070, 0.200), (0.070, 0.44), (0.062, 0.45), (0.058, 0.45), (0.058, 0.475),
               (0.0, 0.475)], (-0.25, 0.08, 0), Z, 24, [O, O, O, O, B, B])                             # A1 motor, cap
    axbox(mb, -0.33, -0.21, -0.07, 0.07, 0.22, 0.33, B, ch=0.01)                                      # cable inlet
    # --- upper arm (A2): a tapered link round the two bosses, the black gear covers on its outer face
    mb.bone("A2")
    up = hull2(circ((P_A2.x, P_A2.z), 0.19, 28) + circ((P_A3.x, P_A3.z), 0.13, 28))
    yprism(mb, up, Y_ARM0, Y_ARM1, O, ch=0.012)
    rib = hull2(circ((P_A2.x, P_A2.z + 0.24), 0.07, 16) + circ((P_A3.x, P_A3.z - 0.20), 0.05, 16))
    yprism(mb, rib, Y_ARM0 - 0.012, Y_ARM0 + 0.01, O, ch=0.006)                                       # a raised web
    cyl(mb, [(0.0, -Y_ARM0 - 0.004), (0.160, -Y_ARM0 - 0.004), (0.160, -Y_ARM0 + 0.012), (0.150, -Y_ARM0 + 0.018),
               (0.060, -Y_ARM0 + 0.018), (0.055, -Y_ARM0 + 0.030), (0.0, -Y_ARM0 + 0.030)],
          (P_A2.x, 0, P_A2.z), -Yv, 36, B)
    bolt_ring(mb, (P_A2.x, Y_ARM0 - 0.018, P_A2.z), -Yv, 0.118, 8, 0.009, 0.007, CH)
    # --- elbow (A3): the upper arm's top cover belongs to A2; the forearm housing turns with A3
    cyl(mb, [(0.0, -Y_ARM0 - 0.004), (0.100, -Y_ARM0 - 0.004), (0.100, -Y_ARM0 + 0.010), (0.092, -Y_ARM0 + 0.016),
               (0.036, -Y_ARM0 + 0.016), (0.034, -Y_ARM0 + 0.026), (0.0, -Y_ARM0 + 0.026)],
          (P_A3.x, 0, P_A3.z), -Yv, 28, B)
    bolt_ring(mb, (P_A3.x, Y_ARM0 - 0.016, P_A3.z), -Yv, 0.070, 6, 0.007, 0.006, CH)
    mb.bone("A3")
    hous = hull2(circ((P_A3.x, P_A3.z), 0.150, 28) +
                 [(P_A3.x - 0.33, P_A3.z - 0.10), (P_A3.x - 0.33, P_A3.z + 0.16), (P_A3.x + X4, P_A3.z - 0.10 + A3OFF),
                  (P_A3.x + X4, P_A3.z + 0.13 + A3OFF)])
    yprism(mb, hous, -0.150, 0.150, O, ch=0.02)
    for (yy, zz, r) in ((0.0, 0.07, 0.075), (-0.07, -0.05, 0.052), (0.07, -0.05, 0.052)):            # A4-A6 motors
        cyl(mb, [(0.0, 0.0), (r, 0.0), (r, 0.11), (r - 0.008, 0.12), (r - 0.012, 0.12), (r - 0.012, 0.15),
                   (0.0, 0.15)], (P_A3.x - 0.33, yy, P_A3.z + zz), -Xv, 24, [O, O, O, O, B, B])
    axbox(mb, P_A3.x - 0.10, P_A3.x + 0.10, -0.06, 0.06, P_A3.z + 0.18, P_A3.z + 0.22, B, ch=0.008)  # hose clamp base
    # --- forearm (A4): a collar, the tube, the wrist fork with its black hub covers
    mb.bone("A4")
    ax4 = P_A4
    cyl(mb, [(0.0, 0.002), (0.108, 0.002), (0.108, 0.025), (0.100, 0.030), (0.095, 0.060), (0.082, 0.36),
             (0.082, 0.38), (0.0, 0.38)], ax4, Xv, 32, [B, B, O, O, O, O, O])
    fork = hull2(circ((P_A5.x, P_A5.z), 0.088, 24) + [(P_A5.x - 0.135, P_A5.z - 0.062), (P_A5.x - 0.135, P_A5.z + 0.062)])
    for y0, y1 in ((-0.097, -0.062), (0.062, 0.097)):
        yprism(mb, fork, y0, y1, O, ch=0.008)
    yprism(mb, [(P_A5.x - 0.135, P_A5.z - 0.062), (P_A5.x - 0.102, P_A5.z - 0.062), (P_A5.x - 0.102, P_A5.z + 0.062),
                (P_A5.x - 0.135, P_A5.z + 0.062)], -0.064, 0.064, O, ch=0.006)                         # fork bridge
    for s in (-1, 1):
        cyl(mb, [(0.0, 0.0), (0.052, 0.0), (0.052, 0.010), (0.024, 0.012), (0.020, 0.020), (0.0, 0.020)],
              (P_A5.x, s * 0.096, P_A5.z), s * Yv, 24, B)
    # --- wrist (A5): the inner body and the hand toward A6
    mb.bone("A5")
    cyl(mb, [(0.0, -0.059), (0.072, -0.059), (0.072, 0.059), (0.0, 0.059)], (P_A5.x, 0, P_A5.z), Yv, 24, O)
    cyl(mb, [(0.0, 0.0), (0.056, 0.0), (0.056, X6 - 0.004), (0.0, X6 - 0.004)], P_A5, Xv, 24, O)
    # --- flange (A6)
    mb.bone("A6")
    cyl(mb, [(0.0, 0.0), (0.064, 0.0), (0.064, 0.030), (0.058, 0.036), (0.056, 0.036), (0.056, L5 - X6),
               (0.0, L5 - X6)], P_A6, Xv, 24, [O, O, O, B, B, B])
    # --- the tool: a copper cage spindle on the flange (Tool), its nozzle spins (Tool_Spin)
    mb.bone("Tool")
    FL = P_FL
    cyl(mb, [(0.0, 0.0), (0.075, 0.0), (0.075, 0.016), (0.0, 0.016)], FL, Xv, 28, CU)                 # adapter
    cyl(mb, [(0.0, 0.016), (0.038, 0.016), (0.038, 0.300), (0.0, 0.300)], FL, Xv, 20, B)              # core
    for t in (0.030, 0.100, 0.170, 0.240, 0.282):
        annulus(mb, FL, Xv, 0.062, 0.086, t, t + 0.016, 28, CU)
    for k in range(8):
        a = 2 * math.pi * k / 8 + math.pi / 8
        c = FL + Vector((0.0, 0.074 * math.cos(a), 0.074 * math.sin(a)))
        cyl(mb, [(0.0, 0.020), (0.0065, 0.020), (0.0065, 0.296), (0.0, 0.296)], c, Xv, 8, CU)
    for s in (-1, 1):                                                                                  # side actuators
        axbox(mb, FL.x + 0.105, FL.x + 0.195, s * 0.086 - 0.016, s * 0.086 + 0.016, FL.z - 0.02, FL.z + 0.02, CU,
              ch=0.004)
        bolt(mb, FL + Vector((0.15, s * 0.102, 0)), s * Yv, 0.008, 0.006, CH)
    mb.bone("Tool_Spin")
    cyl(mb, [(0.0, 0.300), (0.034, 0.300), (0.034, 0.318), (0.030, 0.322), (0.030, 0.360), (0.022, 0.372),
               (0.016, 0.395), (0.010, LT), (0.0, LT)], FL, Xv, 20, [CU, CU, CU, CH, CH, CH, CH, CH])
    # --- hoses: the dress pack behind the upper arm (turret -> elbow), the forearm hose (elbow -> tool)
    cable(mb, [(-0.30, 0.0, 0.30), (-0.30, -0.02, 0.48), (-0.24, -0.21, 0.70), (-0.20, -0.23, 0.98),
               (-0.17, -0.21, 1.18), (-0.25, -0.13, 1.42), (-0.20, -0.03, 1.39)], 0.026,
          blend((0.0, "A1"), (0.18, "A1"), (0.38, "A2"), (0.70, "A2"), (0.90, "A3"), (1.0, "A3")), R, nseg=12)
    cable(mb, [(P_A3.x + 0.02, 0.0, P_A3.z + 0.215), (P_A3.x + 0.20, 0.0, P_A3.z + 0.30), (P_A3.x + 0.45, 0.0, P_A3.z + 0.27),
               (P_A5.x - 0.06, 0.0, P_A5.z + 0.15), (P_FL.x + 0.02, 0.0, P_FL.z + 0.13), (P_FL.x + 0.07, 0.0, P_FL.z + 0.11)],
          0.022, blend((0.0, "A3"), (0.22, "A3"), (0.42, "A4"), (0.66, "A4"), (0.84, "A5"), (1.0, "Tool")), R, nseg=12)
    # --- the lettering on the forearm's outer side (a decal patch, generic marking)
    mb.bone("A4")
    tube_r = lambda t: 0.095 + (0.082 - 0.095) * (t - 0.06) / 0.30 + 0.0007
    info = decal_band(mb, ax4, Xv, tube_r, 0.10, 0.31, math.pi, math.radians(34), "M_RC_Decal", DECALS["robot"])
    return mb, info


def decal_band(mb, O, A, r, t0, t1, ang, half, mat, uvrect, nu=8, nv=4):
    """a curved decal patch on a cylinder (axis A through O, radius r), around angle ang from frame_axes' d1"""
    A, d1, d2 = frame_axes(A)
    u0, v0, u1, v1 = uvrect
    grid = []
    for j in range(nv + 1):
        a = ang - half + 2 * half * j / nv          # row 0 is the texture's top (toward +z on the -y side)
        row = []
        for i in range(nu + 1):
            t = t0 + (t1 - t0) * i / nu
            rr = r(t) if callable(r) else r
            row.append(mb.v(Vector(O) + A * t + rr * (math.cos(a) * d1 + math.sin(a) * d2)))
        grid.append(row)
    for j in range(nv):
        for i in range(nu):
            q = [grid[j + 1][i], grid[j + 1][i + 1], grid[j][i + 1], grid[j][i]]
            uv = [(u0 + (u1 - u0) * i / nu, v0 + (v1 - v0) * (nv - j - 1) / nv),
                  (u0 + (u1 - u0) * (i + 1) / nu, v0 + (v1 - v0) * (nv - j - 1) / nv),
                  (u0 + (u1 - u0) * (i + 1) / nu, v0 + (v1 - v0) * (nv - j) / nv),
                  (u0 + (u1 - u0) * i / nu, v0 + (v1 - v0) * (nv - j) / nv)]
            mb.f(q, mat, uv)
    return dict(decal_axis=list(A), d1=list(d1))


# ------------------------------------------------------------------ the armature
BONES = [  # name, parent, head, tail, roll toward (bone Z), deform
    ("Base", None, (0, 0, 0), (0, 0, H_BASE), (1, 0, 0), True),
    ("A1", "Base", (0, 0, H_BASE), (0, 0, P_A2.z), (1, 0, 0), True),
    ("A2", "A1", tuple(P_A2), tuple(P_A3), (1, 0, 0), True),
    ("A3", "A2", tuple(P_A3), tuple(P_A5), (0, 0, -1), True),
    ("A4", "A3", tuple(P_A4), tuple(P_A5), (0, 0, -1), True),
    ("A5", "A4", tuple(P_A5), tuple(P_A6), (0, 0, -1), True),
    ("A6", "A5", tuple(P_A6), tuple(P_FL), (0, 0, -1), True),
    ("Tool", "A6", tuple(P_FL), tuple(P_FL + Vector((0.30, 0, 0))), (0, 0, -1), True),
    ("Tool_Spin", "Tool", tuple(P_FL + Vector((0.30, 0, 0))), tuple(P_TIP), (0, 0, -1), True),
]
AXIS = {"A1": "Y", "A2": "X", "A3": "X", "A4": "Y", "A5": "X", "A6": "Y", "Tool_Spin": "Y"}


def build_armature(name, coll):
    arm_data = bpy.data.armatures.new(name)
    arm = bpy.data.objects.new(name, arm_data)
    coll.objects.link(arm)
    arm_data.display_type = 'OCTAHEDRAL'
    arm.show_in_front = True
    vl = bpy.context.view_layer
    vl.update()
    for o in list(vl.objects):
        if o is not None:
            o.select_set(False)
    arm.select_set(True)
    vl.objects.active = arm
    bpy.ops.object.mode_set(mode='EDIT')
    eb = arm_data.edit_bones
    for nm, par, h, t, roll_to, deform in BONES:
        b = eb.new(nm)
        b.head, b.tail = Vector(h), Vector(t)
        b.align_roll(Vector(roll_to))
        b.use_deform = deform
        if par:
            b.parent = eb[par]
            b.use_connect = False
    bpy.ops.object.mode_set(mode='OBJECT')
    for pb in arm.pose.bones:
        pb.rotation_mode = 'XYZ'
        ax = AXIS.get(pb.name)
        pb.lock_location = (True, True, True)
        pb.lock_scale = (True, True, True)
        pb.lock_rotation = tuple(c != ax for c in "XYZ") if ax else (True, True, True)
        pb.lock_rotation_w = True
        for c in "xyz":
            setattr(pb, "lock_ik_" + c, c.upper() != ax if ax else True)
        lim = LIMITS.get(pb.name)
        if lim and ax:
            con = pb.constraints.new('LIMIT_ROTATION')
            con.name = "Joint limit"
            con.owner_space = 'LOCAL'
            setattr(con, "use_limit_" + ax.lower(), True)
            setattr(con, "min_" + ax.lower(), math.radians(lim[0]))
            setattr(con, "max_" + ax.lower(), math.radians(lim[1]))
            setattr(pb, "use_ik_limit_" + ax.lower(), True)
            setattr(pb, "ik_min_" + ax.lower(), math.radians(lim[0]))
            setattr(pb, "ik_max_" + ax.lower(), math.radians(lim[1]))
    return arm


def set_pose(arm, th):
    """th: dict of radians per joint (A1..A6, Tool_Spin)"""
    for nm, ax in AXIS.items():
        pb = arm.pose.bones[nm]
        e = [0.0, 0.0, 0.0]
        e["XYZ".index(ax)] = th.get(nm, 0.0)
        pb.rotation_euler = e