"""Door robot arm: forearm (J4..J5), wrist head (J5), flange (J6), tool A (gripper hub + four fingers), tool B
(the red sketch of ref6: flared neck + square plate).

Frames: forearm: origin J4 on the roll axis (A axes); wrist head, flange and tools: origin WC (the wrist centre), x'
along the tool axis (= A x at rest).

Changes to the design, made here (reasons in STATUS.md):
- wrist head drum radius 0.090 (design 0.1045): a 0.1045 drum reaches 0.1045 ahead of WC and the flange's knurled
  ring (from 0.0925) would cut into it; knurl 0.0925..0.121, flange face 0.121..0.132 (the hub starts at 0.132);
- the wrist block (fix round, CRITIQUE_2 must 1): two cheek slabs (|y| 0.121..0.165) rising 22 deg from the J5 lobe
  (R 0.110) back to the motor cylinder, a lower core between them and a web under the head; see the notes above
  _wrist_block for the numbers. The design's wrist top cylinder (F15) is left out: standing on the slope it broke
  ref1's side line, and ref1 shows no cylinder on the slab;
- the knob stem of the door fixture is 0.06 thick (the finger closing proof used 0.06; the parts list said 0.08).

Coder stage 2 (CRITIQUE_1 item 4, 1):
- forearm: a rounded housing (ref1 x_img 760-1030, y_img 520-705): the dark side plate ends at the front in a half
  circle round the dial (dial dia 0.27), the white housing wraps the dial's front with a large radius, the '7' groove
  and the white-ringed bolt at the plate's rear top, the belly a stepped part with a long flat bar;
- wrist block (replaced in the fix round, see above); J5 lobes R 0.110, J5 caps dia 0.19 (ref1);
- tool B (item 1): the neck is a plain cone 0.13 m long (dia 0.154 -> 0.26), the plate 0.55 x 0.55 x 0.044; B's
  wrist centre is 0.326 m off the door face (A: 0.63), so B has its own pose (the rig solves it separately).
"""
import math
import bmesh
from mathutils import Vector, Matrix
from dra_lib import (Link, lathe, slab, rbox, tube, knurl, star_slab, merge, xform, frame, basis, mirror_y,
                     W, G, S, R, O, GL, LA, TAU, circle2, decal_strip, decal_poly, decal_dot, stack, ccw, face,
                     sd_circle, sd_hull, sd_union, sd_inter, sd_half, contour, hull2)
from dra_shapes import (rrect, stadium, xz_slab, yz_slab, xy_slab, screw, dome, hexbolt, ring, cyl, grooved_cyl,
                        holed_plate, notched_disc, half_discs, quarter_discs, u_handle, ribbed_boot, thread_rod, M_XZ)
import dra_rigdata as rd

V2 = lambda x, y: Vector((x, y))
WCF = V2(1.034, 0.0)                    # wrist centre in the forearm frame (x, z)


DIAL = V2(0.275, 0.040)                  # dial centre (forearm frame x, z)


def build_forearm():
    L = Link('forearm')
    L.base = rd.J4.copy()
    # F01 roll bearing ring (thin dark ring behind the dial housing)
    L.put(lathe([(0.165, -0.0002, 0.001, G), (0.209, -0.0002, 0.003, G), (0.209, 0.026, 0.003, G), (0.165, 0.026, 0.001, G)], 48,
                closed=True), frame((0, 0, 0), 'X'), G, 'F01', seats=('F02',))
    # F02 dial housing (fix round, CRITIQUE_2 should 6; ref1 x 850-970 y 555-665): flat top, a large round (R 0.11)
    # over the dial's front into a front face at x 0.52 that the motor cylinder comes out of, then a step forward to
    # x 0.60 over the belly
    ho = [(0.022, -0.150, 0.025), (0.022, 0.180, 0.015), (0.065, 0.205, 0.015), (0.520, 0.205, 0.110),
          (0.520, -0.045, 0.012), (0.600, -0.060, 0.015), (0.600, -0.150, 0.015)]
    L.put(xz_slab(ho, -0.196, 0.196, 0.012, 0.012), mat=W, pid='F02', seats=('F01', 'F09'))
    # F03 dark side plates: a band from the roll ring forward that ends in a half circle round the dial
    arc = circle2(DIAL, 0.142, 18, math.radians(72), math.radians(-102), endpoint=True)
    plate = [V2(0.040, 0.176)] + arc + [V2(0.160, -0.070), V2(0.040, -0.030)]
    po = [(p.x, p.y, 0.004) for p in ccw(plate)]
    for s in (1, -1):
        sd = '.L' if s > 0 else '.R'
        bm = xz_slab(po, 0.192, 0.208, 0.0, 0.0025, n=1)
        if s < 0:
            mirror_y(bm)
        L.put(bm, mat=G, pid='F03' + sd, seats=('F02',))
        # the '7' groove at the plate's rear top (thin bright strips)
        for (x0, z0, x1, z1) in ((0.066, 0.150, 0.112, 0.150), (0.112, 0.150, 0.096, 0.100)):
            M = Matrix.Translation(((x0 + x1) / 2, s * 0.2085, (z0 + z1) / 2)) @ Matrix.Rotation(
                -math.atan2(z1 - z0, x1 - x0), 4, 'Y')
            L.put(rbox((math.hypot(x1 - x0, z1 - z0) + 0.006, 0.002, 0.006), 0.0008, segs=1), M, S, 'F03g' + sd,
                  seats=('F03' + sd,))
        Md = frame((DIAL.x, s * 0.2075, DIAL.y), (0, s, 0))
        L.put(stack([(0.270, 0.006, 'gunmetal'), (0.248, 0.004, 'gunmetal'), (0.224, 0.003, 'glass_dark')], 48, base=0.001), Md,
              None, 'F04' + sd, seats=('F03' + sd,))
        L.put(half_discs(0.104, 0.016, 0.010, G, segs=16, bev=0.001, z0=0.012), Md @ Matrix.Rotation(math.pi / 2, 4, 'Z'), G,
              'F04' + sd, seats=('F04' + sd,))           # the groove runs up and down (fix round, CRITIQUE_2 should 6)
        L.put(stack([(0.132, 0.004, 'gunmetal'), (0.050, 0.010, 'glass_dark')], 32, base=0.0), Md @ Matrix.Translation((0, 0, 0.0215)),
              None, 'F04c' + sd, seats=('F04' + sd,))
        for k in range(2):                              # the notches left and right of the groove
            a_ = math.pi * k
            L.put(rbox((0.012, 0.016, 0.006), 0.001, segs=1), Md @ Matrix.Translation((0.124 * math.cos(a_), 0.124 * math.sin(a_), 0.012))
                  @ Matrix.Rotation(a_, 4, 'Z'), G, 'F04n' + sd, seats=('F04' + sd,))
        # small bolt with a white ring on the plate's rear (ref1 x_img 1010)
        L.put(stack([(0.030, 0.004, 'paint_white'), (0.016, 0.006, 'steel')], 12, base=0.001), frame((0.062, s * 0.2078, 0.050), (0, s, 0)),
              None, 'F03b' + sd, seats=('F03' + sd,))
    # F05 belly: a stepped part under the housing with a seam, and the long flat bar under it (ref1)
    L.put(rbox((0.470, 0.330, 0.060), 0.008, rv=0.025, vax=0), Matrix.Translation((0.320, 0, -0.168)), W, 'F05', seats=('F02',))
    L.put(rbox((0.470, 0.332, 0.004), 0.001), Matrix.Translation((0.320, 0, -0.146)), G, 'F05g', seats=('F05', 'F02'))
    L.put(rbox((0.420, 0.300, 0.020), 0.005, rv=0.010, vax=0), Matrix.Translation((0.330, 0, -0.205)), W, 'F05s', seats=('F05',))
    L.put(rbox((0.380, 0.055, 0.012), 0.003), Matrix.Translation((0.330, 0, -0.219)), G, 'F05b', seats=('F05s',))
    # F07 J5/J6 motor cylinder, ringed, coming out of the housing's rounded front into the wrist block (ref1 x_img 760-860)
    L.put(grooved_cyl(0.066, 0.26, [0.140, 0.165, 0.190, 0.215], depth=0.004, width=0.007, segs=40, f=0.006, mat=W),
          frame((0.400, 0, 0.095), 'X'), None, 'F07', seats=('F02', 'F09'))
    _wrist_block(L)
    # F08 hose under the wrist block with metal ferrules and an orange band near its front end
    path = [(0.53, -0.022, -0.150), (0.62, -0.022, -0.168), (0.74, -0.022, -0.160), (0.83, -0.022, -0.151),
            (0.880, -0.022, -0.148)]
    L.put(tube(path, 0.0242, 14, 0.06, caps=True), None, R, 'F08', seats=('F05', 'F05g', 'F09w', 'F08f', 'F02', 'F09'))
    for x in (0.548, 0.842):
        L.put(lathe([(0.0, -0.0165, 0, S), (0.0308, -0.0165, 0.002, S), (0.0308, 0.0165, 0.002, S), (0.0, 0.0165, 0, S)], 16),
              frame((x, -0.022, -0.152), (1, 0, -0.06 if x < 0.6 else 0.12)), S, 'F08f',
              seats=('F08', 'F05', 'F05g', 'F09w', 'F02', 'F09'))
    L.put(lathe([(0.0247, -0.0066, 0, O), (0.0262, -0.0066, 0.0008, O), (0.0262, 0.0066, 0.0008, O), (0.0247, 0.0066, 0, O)], 16,
                closed=True), frame((0.752, -0.022, -0.1593), (1, 0, 0.08)), O, 'F08o', seats=('F08',))
    _forearm_markings(L)
    return L


# The wrist block (CRITIQUE_2 must 1): two sloped cheek slabs, as ref1, ref2 and ref6 show and the user's red sketch of
# version B draws. Each cheek is one long slab rising 22 deg from the J5 lobe back to the motor cylinder, with a raised
# panel on its face (the groove line under its top edge), the fin bar on it (left; an oval slot plate on the right)
# and the orange triangles. The top line is as high as the gripper allows: at the door's deepest wrist bends
# (frames 9-15 and 100-112: J5 -68..-74 deg with J6 45..52 deg) the finger motors pass over the cheeks at x 0.88-0.96
# down to 0.129 above the forearm axis (build/wrist_envelope_grid.json, every quarter frame), and down to 0.124 in the
# geo_sweep's own poses (J5 3 deg past the path, J6 10 deg off it: build/wrist_envelope_sweep.json). The line below
# keeps at least 10 mm from the first and 4 mm from the second, so no pocket is needed; it runs 0.0725 over the wrist
# centre (ref1: about 0.09) and meets the J5 lobe in a concave round. Between the cheeks a lower core (its top 35 mm
# under theirs) joins them, the motor cylinder and the housing; under the head the web with the handle.
TOP_A = math.radians(22.0)
TOP_T = math.tan(TOP_A)
TOP_Z0 = 0.0725                          # the top line over the wrist centre
LOBE_R = 0.110                           # the J5 lobe round the wrist centre (the J5 cap is dia 0.19)
CHEEK_Y = (0.121, 0.165)                 # the cheeks' inner and outer faces (|y|)
PANEL_IN, PANEL_T = 0.018, 0.006         # the raised panel: inset from the cheek's edges, height
CORE_DROP = 0.035                        # the core's top under the cheeks' top line
T1 = V2(0.612, 0.0)                      # the cheeks' top-back corner (x; z from the line)
B1 = V2(0.632, 0.050)                    # the cheeks' bottom-back corner, on the motor cylinder's level
B2 = V2(0.868, -0.110)                   # where the bottom edge (35 deg) meets the flat bottom over the web


def _slope_z(x):
    """the cheeks' top line (forearm x, z): 22 degrees, 0.0725 over the wrist centre, 0.243 at the block's back"""
    return TOP_Z0 + TOP_T * (WCF.x - x)


def _isect(p, d, q, e):
    """the point where the lines p + t d and q + s e meet (2D)"""
    den = d.x * e.y - d.y * e.x
    t = ((q.x - p.x) * e.y - (q.y - p.y) * e.x) / den
    return p + d * t


def _line_circle_back(p, d, c, r):
    """of the two points where the line p + t d meets the circle (c, r), the one with the smaller x"""
    d = d.normalized()
    b = d.dot(p - c)
    cc = (p - c).length_squared - r * r
    q = math.sqrt(max(b * b - cc, 0.0))
    P = [p + d * (-b - q), p + d * (-b + q)]
    return min(P, key=lambda v: v.x)


def _top_line():
    """(a point, the direction going back and up, the upward normal) of the cheeks' top line"""
    dl = V2(-math.cos(TOP_A), math.sin(TOP_A))
    return V2(WCF.x, TOP_Z0), dl, V2(math.sin(TOP_A), math.cos(TOP_A))


def _cheek_outline():
    """the cheek's outline (x, z, round), counter-clockwise: the bottom edge, the flat bottom over the web, the J5 lobe,
    a concave round (R 0.030) into the top line, the top line, the back edge"""
    A0, dl, n = _top_line()
    rf = 0.030
    h = n.dot(A0) + rf - n.dot(WCF)
    F = WCF + n * h
    C = F + dl * math.sqrt((LOBE_R + rf) ** 2 - h * h)            # the round's centre, over the valley
    Tl = C - n * rf                                                 # where it leaves the top line
    Tc = WCF + (C - WCF) * (LOBE_R / (LOBE_R + rf))                 # where it meets the lobe
    a_tc = math.atan2(Tc.y - WCF.y, Tc.x - WCF.x)
    lobe = circle2(WCF, LOBE_R, 26, -math.pi / 2, a_tc, endpoint=True)
    a0 = math.atan2(Tc.y - C.y, Tc.x - C.x)
    a1 = a0 + (math.atan2(Tl.y - C.y, Tl.x - C.x) - a0 + math.pi) % TAU - math.pi     # the short way, under C
    rnd = [C + V2(math.cos(a0 + (a1 - a0) * k / 5), math.sin(a0 + (a1 - a0) * k / 5)) * rf for k in range(1, 6)]
    t1 = V2(T1.x, _slope_z(T1.x))
    pts = [(B1.x, B1.y, 0.030), (B2.x, B2.y, 0.020)] + [(p.x, p.y, 0.0) for p in lobe] + \
        [(p.x, p.y, 0.0) for p in rnd] + [(t1.x, t1.y, 0.042)]
    return pts


def _panel_outline():
    """the raised panel on the cheek's face: the cheek's edges moved PANEL_IN inward, and in front an arc (R 0.108)
    round the J5 cap (dia 0.19), so the cap sits on the cheek itself"""
    A0, dl, n = _top_line()
    d = PANEL_IN
    t1 = V2(T1.x, _slope_z(T1.x))
    db = (B1 - t1).normalized()
    dbb = (B2 - B1).normalized()
    left = lambda v: V2(-v.y, v.x)
    top_p, top_d = A0 - n * d, dl
    back_p, back_d = t1 + left(db) * d, db
    bot_p, bot_d = B1 + left(dbb) * d, dbb
    flat_p, flat_d = V2(B2.x, B2.y + d), V2(1.0, 0.0)
    t1i = _isect(top_p, top_d, back_p, back_d)
    b1i = _isect(back_p, back_d, bot_p, bot_d)
    b2i = _isect(bot_p, bot_d, flat_p, flat_d)
    rp = 0.108
    pt = _line_circle_back(top_p, top_d, WCF, rp)
    pb = _line_circle_back(flat_p, flat_d, WCF, rp)
    a_b = math.atan2(pb.y - WCF.y, pb.x - WCF.x) % TAU                 # about 238 deg
    a_t = math.atan2(pt.y - WCF.y, pt.x - WCF.x) % TAU                 # about 140 deg
    arc = [WCF + V2(math.cos(a_b + (a_t - a_b) * k / 10), math.sin(a_b + (a_t - a_b) * k / 10)) * rp for k in range(1, 10)]
    pts = [(b1i.x, b1i.y, 0.014), (b2i.x, b2i.y, 0.010), (pb.x, pb.y, 0.006)] + [(p.x, p.y, 0.0) for p in arc] + \
        [(pt.x, pt.y, 0.006), (t1i.x, t1i.y, 0.024)]
    return pts


def _core_outline():
    """the core between the cheeks (its top CORE_DROP under theirs): from the housing and the motor cylinder at the
    back to an arc (R 0.112) behind the head drum, its bottom along the cheeks' and on the web"""
    A0, dl, n = _top_line()
    cp = V2(A0.x, A0.y - CORE_DROP)
    pt = _line_circle_back(cp, dl, WCF, 0.112)
    a_t = math.atan2(pt.y - WCF.y, pt.x - WCF.x) % TAU                 # about 140 deg
    a_b = math.radians(200)
    arc = [WCF + V2(math.cos(a_b + (a_t - a_b) * k / 8), math.sin(a_b + (a_t - a_b) * k / 8)) * 0.112 for k in range(0, 9)]
    xb = 0.600
    pts = [(xb, 0.020, 0.008), (0.665, 0.020, 0.016), (B2.x, -0.108, 0.012), (arc[0].x, -0.108, 0.0)] + \
        [(p.x, p.y, 0.0) for p in arc] + [(xb, _slope_z(xb) - CORE_DROP, 0.020)]
    return pts


def _face_frame(x, z, s):
    """a frame on the left (s = 1) or right (s = -1) panel face at forearm (x, z): local X along the top line (going
    forward and down), local Z out of the face"""
    y = s * (CHEEK_Y[1] + PANEL_T)
    return basis((x, y, z), (0, s, 0), (math.cos(TOP_A), 0, -math.sin(TOP_A)))


def _wrist_block(L):
    """F09 core, F09c cheeks (+ F09p panels), F09w web; F10 fin bar / F10r slot plate on the panels, F12 screw domes,
    F13 bottom handle, F14 J5 caps (see the notes above)"""
    # F09 core between the cheeks
    L.put(xz_slab(ccw_pts(_core_outline()), -0.123, 0.123, 0.004, 0.004, steps=2), mat=W, pid='F09',
          seats=('F07', 'F02', 'F09w', 'F09c.L', 'F09c.R'))
    # F09c cheeks and their raised panels F09p
    cho = ccw_pts(_cheek_outline())
    pan = ccw_pts(_panel_outline())
    for s in (1, -1):
        sd = '.L' if s > 0 else '.R'
        bm = xz_slab(cho, CHEEK_Y[0], CHEEK_Y[1], 0.006, 0.012, steps=3)
        if s < 0:
            mirror_y(bm)
        L.put(bm, mat=W, pid='F09c' + sd, seats=('F09', 'F09w'))
        bm = xz_slab(pan, CHEEK_Y[1] - 0.001, CHEEK_Y[1] + PANEL_T, 0.0, 0.0025, n=2, steps=2)
        if s < 0:
            mirror_y(bm)
        L.put(bm, mat=W, pid='F09p' + sd, seats=('F09c' + sd,))
    # F09w web under the head (its top 0.105 below the wrist centre); the hose runs into its back face
    web = [(0.855, -0.191, 0.012), (1.060, -0.191, 0.03), (1.094, -0.150, 0.02), (1.094, -0.105, 0.0), (0.855, -0.105, 0.0)]
    L.put(xz_slab(web, -0.165, 0.165, 0.014, 0.014, steps=3), mat=W, pid='F09w',
          seats=('F09', 'F09c.L', 'F09c.R'))
    # F14 J5 caps on both cheeks (stepped concentric rings, dia 0.19 as in ref1)
    for s in (1, -1):
        sd = '.L' if s > 0 else '.R'
        Mc = frame((WCF.x, s * CHEEK_Y[1], WCF.y), (0, s, 0))
        L.put(stack([(0.190, 0.008, 'gunmetal'), (0.156, 0.009, 'gunmetal'), (0.118, 0.009, 'gunmetal'), (0.080, 0.008, 'steel'),
                     (0.040, 0.008, 'gunmetal')], 40, base=0.002), Mc, None, 'F14' + sd, seats=('F09c' + sd, 'F09w'))
        # F12 two white screw domes per side on the panel, behind the cap
        for (x, z) in ((0.905, -0.035), (0.905, -0.074)):
            hit, nn = L.snap((x, s * 0.5, z), (0, -s, 0))
            if hit is not None:
                L.put(dome(0.0130, 0.008, W), frame(hit, nn), None, 'F12' + sd,
                      seats=('F09p' + sd, 'F09c' + sd, 'F09w', 'F09'))
    # F10 fin bar on the left panel along the top line (ref1, ref6: a raised, stepped bar), F10r an oval slot plate
    # on the right panel in the same place
    xm = 0.735
    zm = _slope_z(xm) - 0.060
    for s, pid in ((1, 'F10'), (-1, 'F10r')):
        Mf = _face_frame(xm, zm, s)
        if s > 0:
            L.put(rbox((0.170, 0.040, 0.008), 0.0025, rv=0.012, vax=2), Mf @ Matrix.Translation((0, 0, 0.003)), W, pid,
                  seats=('F09p.L',))
            L.put(rbox((0.150, 0.024, 0.009), 0.003, rv=0.009, vax=2), Mf @ Matrix.Translation((0, 0, 0.0105)), W, pid,
                  seats=('F09p.L', 'F10'))
        else:
            sl = rbox((0.120, 0.040, 0.008), 0.0015, ops=[('+z', -0.045, 0.045, -0.010, 0.010, -0.005)], rbev=0.004, rim_segs=2)
            L.put(sl, Mf @ Matrix.Translation((0, 0, 0.003)), W, pid, seats=('F09p.R',))
    # F13 flat-bar loop under the web
    loop = [(0.875, 0.0, -0.191), (0.875, 0.0, -0.222), (0.981, 0.0, -0.222), (0.981, 0.0, -0.191)]
    for dy in (-0.013, 0.013):
        L.put(tube([(x, dy, z) for x, _, z in loop], 0.0055, 8, 0.012), None, G, 'F13', seats=('F09w',))


def ccw_pts(pts):
    """(x, z, round) points made counter-clockwise (keeps the rounds)"""
    a = sum(pts[i - 1][0] * pts[i][1] - pts[i][0] * pts[i - 1][1] for i in range(len(pts))) / 2
    return pts if a > 0 else list(reversed(pts))


def _forearm_markings(L):
    for s in (1, -1):
        # F06 2 x 3 dot grid below the dials
        M = L.on_surface((0.470, s * 0.6, -0.085), (0, -s, 0), up=(0, 0, 1))
        if M is not None:
            bm = bmesh.new()
            for i in range(3):
                for j in range(2):
                    d = decal_dot((-0.007 + 0.014 * j, -0.014 + 0.014 * i), 0.0055, 10)
                    merge(bm, d)
                    d.free()
            L.decal(bm, M, 'F06' + ('.L' if s > 0 else '.R'))
    # F11 three triangles and two dots (left), 2 x 2 dots (right) on the wrist block cheeks
    M = L.on_surface((0.845, 0.6, 0.062), (0, -1, 0), up=(0, 0, 1))
    if M is not None:
        bm = bmesh.new()
        for k in range(3):
            t = decal_poly([(-0.031 + k * 0.021, 0.008), (-0.020 + k * 0.021, 0.008), (-0.0255 + k * 0.021, 0.020)])
            merge(bm, t)
            t.free()
        for k in range(2):
            d = decal_dot((-0.025 + k * 0.017, -0.014), 0.0042, 8)
            merge(bm, d)
            d.free()
        L.decal(bm, M, 'F11.L')
    M = L.on_surface((0.845, -0.6, 0.062), (0, 1, 0), up=(0, 0, 1))
    if M is not None:
        bm = bmesh.new()
        for i in range(2):
            for j in range(2):
                d = decal_dot((-0.006 + 0.012 * i, -0.006 + 0.012 * j), 0.0032, 8)
                merge(bm, d)
                d.free()
        L.decal(bm, M, 'F11.R')


def build_wrist_head():
    """W01 head drum turning on J5 between the cheeks, W02 neck collar ahead of it"""
    L = Link('wrist head')
    L.base = rd.WC.copy()
    L.put(lathe([(0.0, -0.1166, 0, G), (0.084, -0.1166, 0.004, G), (0.090, -0.110, 0.004, G), (0.090, 0.110, 0.004, G),
                 (0.084, 0.1166, 0.004, G), (0.0, 0.1166, 0, G)], 40), frame((0, 0, 0), 'Y'), G, 'W01')
    L.put(lathe([(0.0, 0.040, 0, G), (0.0825, 0.064, 0.002, G), (0.0825, 0.090, 0.002, G), (0.0, 0.090, 0, G)], 40),
          frame((0, 0, 0), 'X'), G, 'W02', seats=('W01',))
    return L


def build_flange():
    """FL1 knurled ring (J6) and FL2 flange face with six screws on its back"""
    L = Link('flange')
    L.base = rd.WC.copy()
    k = knurl(0.0735, 0.0792, 0.0285, 36, 0.003, S)
    L.put(k, frame((0.0925, 0, 0), 'X'), S, 'FL1')
    L.put(lathe([(0.0, 0.121, 0, G), (0.088, 0.121, 0.0015, G), (0.088, 0.132, 0.0015, G), (0.0, 0.132, 0, G)], 40),
          frame((0, 0, 0), 'X'), G, 'FL2', seats=('FL1',))
    for i in range(6):
        a = TAU * (i + 0.5) / 6
        L.put(screw(0.005, 0.004), frame((0.121, 0.0835 * math.cos(a), 0.0835 * math.sin(a)), '-X'), None, 'FL2s', seats=('FL2',))
    return L


# ---------------------------------------------------------------- tool A: hub, brackets, motors; fingers
KN = rd.KNUCKLE                 # (x', r)


def build_tool_a():
    L = Link('tool A hub')
    L.base = rd.WC.copy()
    prof = [(0.0, 0.132, 0, G), (0.1355, 0.132, 0.002, G), (0.1355, 0.152, 0.0, G), (0.1375, 0.152, 0.0, W)]
    for g in (0.180, 0.210, 0.240):
        prof += [(0.1375, g - 0.003, 0, W), (0.1335, g - 0.002, 0, W), (0.1335, g + 0.002, 0, W), (0.1375, g + 0.003, 0, W)]
    prof += [(0.1375, 0.277, 0.0, W), (0.1355, 0.277, 0, G), (0.1355, 0.297, 0.002, G), (0.110, 0.297, 0, G), (0.110, 0.303, 0.001, G),
             (0.0825, 0.303, 0, G), (0.0825, 0.309, 0.001, G), (0.0495, 0.309, 0, G), (0.0495, 0.315, 0.001, S), (0.0, 0.315, 0, S)]
    L.put(lathe(prof, 40), frame((0, 0, 0), 'X'), None, 'A01')
    for which, a in rd.FINGERS.items():
        Rr = rd.roll(a)
        sd = '.' + which
        # A02 bracket: a clevis on the hub's front rim holding the knuckle pin
        for sy in (1, -1):
            cheek = rbox((0.0515, 0.012, 0.062), 0.0025)
            L.put(cheek, Rr @ Matrix.Translation((0.2443, sy * 0.027, 0.150)), G, 'A02' + sd, seats=('A01',))
        L.put(rbox((0.040, 0.066, 0.020), 0.003), Rr @ Matrix.Translation((0.240, 0, 0.128)), G, 'A02' + sd, seats=('A01', 'A02' + sd))
        # knuckle pin with caps on both cheeks
        L.put(cyl(0.0075, 0.074, 8, 0.0, S, z0=-0.037), Rr @ Matrix.Translation((KN[0], 0, KN[1])) @ frame((0, 0, 0), 'Y'), S,
              'A04' + sd, seats=('A02' + sd,))
        for sy in (1, -1):
            L.put(stack([(0.0528, 0.0070, 'gunmetal'), (0.030, 0.004, 'gunmetal')], 12, base=0.001),     # dark caps (CRITIQUE_2 4)
                  Rr @ frame((KN[0], sy * 0.033, KN[1]), (0, sy, 0)), None, 'A04' + sd, seats=('A02' + sd,))
        # A03 knurled drive motor fixed to the bracket, pointing back and outward (18.4 deg)
        # on the design's line (knuckle -> WC + (47, 99) px), starting 0.051 from the knuckle to clear the finger's eye
        d = Vector((-0.949, 0, 0.316)).normalized()
        start = Vector((0.187, 0, 0.190))
        L.put(knurl(0.022, 0.026, 0.080, 16, 0.003, G), Rr @ frame(start, d), G, 'A03' + sd, seats=('A02' + sd, 'A03m' + sd))
        L.put(stack([(0.040, 0.005, 'gunmetal'), (0.018, 0.004, 'gunmetal')], 16, base=0.001), Rr @ frame(start + d * 0.080, d), None,
              'A03' + sd, seats=('A03' + sd,))
        for sy in (1, -1):
            lug = rbox((0.038, 0.010, 0.022), 0.002)
            L.put(lug, Rr @ Matrix.Translation((0.204, sy * 0.026, 0.187)) @ Matrix.Rotation(math.radians(15.1), 4, 'Y'), G,
                  'A03m' + sd, seats=('A02' + sd, 'A03' + sd))
    return L


def _finger_points():
    kx, kr = KN
    pin = V2(kx + rd.PROX_LEN * math.cos(rd.PROX_OPEN), kr + rd.PROX_LEN * math.sin(rd.PROX_OPEN))
    hb = pin + V2(math.cos(rd.DIST_OPEN_ABS), math.sin(rd.DIST_OPEN_ABS)) * rd.DIST_LEN
    return V2(kx, kr), pin, hb


def build_finger(which, part):
    """one finger link of the top finger turned to its place round the tool axis (rest = open, ref1)"""
    L = Link('finger %s %s' % (which, part))
    L.base = rd.WC.copy()
    Rr = rd.roll(rd.FINGERS[which])
    k, pin, hb = _finger_points()
    if part == "proximal":
        d = (pin - k).normalized()
        ang = math.atan2(d.y, d.x)
        Mx = Rr @ Matrix.Translation((k.x, 0, k.y)) @ Matrix.Rotation(-ang, 4, 'Y')      # local x along the link
        Lp = (pin - k).length
        # eye round the knuckle pin (bore 0.0085), the bar, the clevis at the distal pin, boot, tendon rod
        L.put(lathe([(0.0085, -0.0195, 0.0, G), (0.022, -0.0195, 0.003, G), (0.022, 0.0195, 0.003, G), (0.0085, 0.0195, 0.0, G)], 16,
                    closed=True), Mx @ frame((0, 0, 0), 'Y'), G, 'A05e')
        L.put(rbox((Lp - 0.037, 0.0396, 0.0440), 0.004, rv=0.011, vax=1), Mx @ Matrix.Translation(((Lp - 0.037) / 2 + 0.013, 0, 0)), G,
              'A05', seats=('A05e',))
        for sy in (1, -1):
            L.put(rbox((0.053, 0.0075, 0.040), 0.002, rv=0.012, vax=1), Mx @ Matrix.Translation((Lp - 0.0085, sy * 0.02225, 0)), G,
                  'A05c', seats=('A05',))
            L.put(stack([(0.044, 0.0060, 'gunmetal'), (0.024, 0.004, 'gunmetal')], 12, base=0.001),      # dark caps (CRITIQUE_2 4)
                  Mx @ frame((Lp, sy * 0.0260, 0), (0, sy, 0)), None, 'A06', seats=('A05c',))
        L.put(cyl(0.006, 0.052, 10, 0.0005, S, z0=-0.026), Mx @ Matrix.Translation((Lp, 0, 0)) @ frame((0, 0, 0), 'Y'), S, 'A06',
              seats=('A05c',))
        # the ribbed section (ref6, ref1): nine flat rubber rib plates across the link, under the tendon rod
        for k in range(9):
            L.put(rbox((0.0055, 0.054, 0.052), 0.0015, rv=0.010, vax=0, segs=1),
                  Mx @ Matrix.Translation((0.058 + k * 0.0095, 0, -0.004)), R, 'A05b', seats=('A05', 'A05b'))
        L.put(thread_rod(0.0055, Lp - 0.07, 0.014, S, 6), Mx @ Matrix.Translation((0.022, 0, 0.0285)) @ frame((0, 0, 0), 'X'), S,
              'A05t', seats=('A05', 'A05b', 'A05c'))
        for x in (0.022, Lp - 0.050):
            L.put(rbox((0.008, 0.016, 0.010), 0.0015), Mx @ Matrix.Translation((x, 0, 0.026)), G, 'A05t', seats=('A05', 'A05t'))
    else:
        d = (hb - pin).normalized()
        ang = math.atan2(d.y, d.x)
        Mx = Rr @ Matrix.Translation((pin.x, 0, pin.y)) @ Matrix.Rotation(-ang, 4, 'Y')
        Ld = (hb - pin).length
        L.put(lathe([(0.007, -0.0172, 0.001, G), (0.017, -0.0172, 0.002, G), (0.017, 0.0172, 0.002, G), (0.007, 0.0172, 0.001, G)], 20,
                    closed=True), Mx @ frame((0, 0, 0), 'Y'), G, 'A07e')
        # a curved beak (ref6, CRITIQUE_1 item 12): the centre line bows outward (local +z) and comes back to the hook,
        # the beak tapers from 0.040 at the eye to 0.014 at the tip, its rear end round the pin; an oval slot through
        # its two outer layers; the hook turned inward (toward the tool axis = local -z) as before
        beak = _beak_outline(Ld)
        slot = _beak_slot(Ld)
        bore = [(0.0072 * math.cos(math.radians(a)), 0.0072 * math.sin(math.radians(a)), 0.0) for a in range(0, 360, 30)]
        L.put(xz_slab(beak, -0.0100, 0.0100, 0.0, 0.0, n=1, holes=[bore]), Mx, G, 'A07', seats=('A07e',))
        for s_ in (1, -1):
            bm = xz_slab(beak, 0.0098, 0.0172, 0.0, 0.0025, n=1, holes=[slot, bore])
            if s_ < 0:
                mirror_y(bm)
            L.put(bm, Mx, G, 'A07', seats=('A07e', 'A07'))
        hook = [(Ld - 0.012, -0.004, 0.0), (Ld + 0.006, -0.002, 0.003), (Ld + 0.002, -0.0396, 0.004), (Ld - 0.012, -0.037, 0.003)]
        L.put(xz_slab(hook, -0.0150, 0.0150, 0.002, 0.002), Mx, G, 'A07h', seats=('A07',))
    return L


def _beak_centre(x, Ld):
    return 0.008 * math.sin(math.pi * min(1.0, max(0.0, x / Ld)))


def _beak_half(x, Ld):
    return 0.020 - 0.013 * (min(1.0, max(0.0, x / Ld)) ** 1.2)


def _beak_outline(Ld):
    """the beak's side outline (x along the link from the distal pin, z outward), counter-clockwise"""
    xs = [0.004 + (Ld - 0.004) * k / 10 for k in range(11)]
    outer = [(x, _beak_centre(x, Ld) + _beak_half(x, Ld), 0.0) for x in xs]
    inner = [(x, _beak_centre(x, Ld) - _beak_half(x, Ld), 0.0) for x in reversed(xs)]
    rear = [(0.0 + 0.017 * math.cos(math.radians(a)), 0.017 * math.sin(math.radians(a)), 0.0) for a in (240, 205, 180, 155, 120)]
    return outer + [(Ld + 0.006, _beak_centre(Ld, Ld), 0.003)] + inner + rear


def _beak_slot(Ld):
    """the oval slot along the beak's middle (a stadium following the centre line)"""
    pts = []
    x0, x1, r = 0.038, 0.104, 0.0055
    n = 6
    for k in range(n + 1):                    # upper edge, front to back is built as: round end at x1, at x0
        a = -math.pi / 2 + math.pi * k / n
        pts.append((x1 + r * math.cos(a), _beak_centre(x1, Ld) + r * math.sin(a), 0.0))
    for k in range(n + 1):
        a = math.pi / 2 + math.pi * k / n
        pts.append((x0 + r * math.cos(a), _beak_centre(x0, Ld) + r * math.sin(a), 0.0))
    return pts


# ---------------------------------------------------------------- tool B: the red sketch
def build_tool_b():
    """the red sketch of ref6 (CRITIQUE_1 item 1): a plain cone neck 0.13 m long from the flange (dia 0.154) to the
    plate (dia 0.26), two ring grooves; a flat square plate 0.55 x 0.55 x 0.044 bolted to the door's pad"""
    L = Link('tool B')
    L.base = rd.WC.copy()
    x0, x1 = 0.132, 0.262
    rr = lambda x: 0.077 + (0.130 - 0.077) * (x - x0) / (x1 - x0)
    prof = [(0.0, x0, 0, W), (0.077, x0, 0.002, W)]
    for xg in (0.172, 0.212):                               # two ring grooves
        prof += [(rr(xg - 0.004), xg - 0.004, 0, W), (rr(xg) - 0.004, xg - 0.002, 0, W), (rr(xg) - 0.004, xg + 0.002, 0, W),
                 (rr(xg + 0.004), xg + 0.004, 0, W)]
    prof += [(0.130, x1, 0.006, W), (0.0, x1, 0, W)]
    L.put(lathe(prof, 48), frame((0, 0, 0), 'X'), None, 'B01')
    L.put(rbox((0.044, 0.55, 0.55), 0.002, rv=0.033, vax=0, segs=2, segs_v=4), Matrix.Translation((x1 + 0.022, 0, 0)), G, 'B02',
          seats=('B01',))
    pos = []
    for a_ in (-0.115, 0.115):
        pos += [(0.23, a_), (-0.23, a_), (a_, 0.23), (a_, -0.23)]
    for (y, z) in pos:
        L.put(hexbolt(0.0396, 0.020), frame((x1, y, z), '-X'), None, 'B03', seats=('B02',))
    # B04 orange L ticks at the corners of the plate's back face (legs pointing inward along the two edges)
    for (sy, sz) in ((1, 1), (1, -1), (-1, 1), (-1, -1)):
        M = basis((x1, sy * 0.238, sz * 0.238), (-1, 0, 0), (0, -sy, 0))
        f = float(-sz * sy)
        tk = decal_poly([(0.0, 0.0), (0.040, 0.0), (0.040, 0.007 * f), (0.0, 0.007 * f)])
        t2 = decal_poly([(0.0, 0.0), (0.007, 0.0), (0.007, 0.040 * f), (0.0, 0.040 * f)])
        merge(tk, t2)
        t2.free()
        L.decal(tk, M, 'B04')
    return L