"""The arm's motion with the boom joint (the user, 2026-10-16: "there need be another joint ... to be able to put down
subtub tenders into moon pool ... after user gets in arms bent from the new joint ... and puts tender inside moon pool").
exec'd inside build_dockarm.py and rig_dockarm.py (shares their namespace: R_TIP, ZS, LINK, BOARD_Z, STOWED_Z ...).

Joint J on each boom at XJ from the hub axis, ZJ high: the boom outboard of it pitches down about the tangential axis.
One handle value y (metres let down) runs the whole way, with r = reach_in (how far the head has moved in toward the hub
when it reaches BOARD; the vessel interior uses 2.34 on the launch arm):

    y in 0 .. BOARD (5.3)       the hoist of before: the boom stays level, the two links let the head down, moving it in by
                                x = r (y / 5.3)^2. The knee's bend D = |wrist - shoulder| = sqrt(x^2 + y^2).
    y in BOARD .. BOARD + POOL  the head keeps going straight down (x stays r), the knee keeps the bend it had at BOARD,
                                and the boom pitches down just as far as it takes for the links to reach: the shoulder is
                                the intersection of the circle of radius LJ about J with the circle of radius D about the
                                wrist (the upper one). Only when the circles stop meeting (r near 0, deep) does the knee
                                open, D = |wrist - J| - LJ + 0.05, and the boom points at the wrist.
    The head stays level throughout: wrist = elbow - shoulder + boom.

In the ship (hub 7.5 m over the deck, the deck well where the launch Tender hangs at BOARD): POOL = 4.0 puts the Tender's
top 1.3 m below the deck, its keel 3.2 m above the hull's aperture, on the launch arm with r 2.34 the boom pitches to 52
degrees and the shoulder opens to 118. With r 0 the same depth needs 65 and 165 degrees (the arm nearly straight); the
links cannot go deeper than about 4.15 m below BOARD at r 0, so POOL is 4.0 for every r.

Everything is written as driver expressions in stages (a Blender driver expression holds 255 characters and runs without
Python when it is a "simple expression"): six helper values M1..M6, each a triple stored in the location channels of a
helper: the location channels of an empty in the unrigged model (read as TRANSFORMS variables), custom properties of a
bone in the rig (read as SINGLE_PROP variables: drivers of one armature reading each other's bone transforms evaluated in no
reliable order, found on 2026-10-16).
"""
XJ, ZJ = 1.50, 0.85                                   # the boom joint: 1.5 m out from the hub axis, mid-height of the boom root
POOL_M = 4.0                                          # metres below BOARD that the handle goes on down (POOL stop)
BOARD_M = STOWED_Z - BOARD_Z                          # 5.3
Y_MAX = BOARD_M + POOL_M
LJ = math.hypot(R_TIP - XJ, ZS - ZJ)                  # joint to shoulder axis
A0 = math.atan2(ZS - ZJ, R_TIP - XJ)                  # the boom's rest slope (1.15 degrees up)
BOOM_PITCH_LIMIT_DEG = 75.0                           # joint range on the rig (65.1 is the most the formulas ask for, at r 0 and POOL)
SHOULDER_LIMIT_DEG = 172.0                            # 165 at r 0 and POOL; 118 on the ship's launch arm
WRIST_LIMIT_DEG = 105.0

def arm_pose(y, r):
    """Reference solution in Python (what the drivers compute). Boom frame: x radial out, z up, the hub axis at x 0.
    Returns the four joint angles (radians, positive = lower) and the shoulder, elbow and wrist points."""
    B = BOARD_M
    yb = min(y, B)
    wx = R_TIP - r * yb * yb / (B * B)
    wz = ZS - y
    dk = math.sqrt((r * yb * yb / (B * B)) ** 2 + yb * yb)
    L = math.hypot(wx - XJ, wz - ZJ)
    cx, cz = (wx - XJ) / L, (wz - ZJ) / L
    D = dk if y <= B else min(max(dk, L - LJ + 0.05), 5.94)
    a = (LJ * LJ - D * D + L * L) / (2 * L)
    h = math.sqrt(max(LJ * LJ - a * a, 0.0))
    sx, sz = XJ + a * cx - h * cz, ZJ + a * cz + h * cx
    th = 0.0 if y <= B else A0 - math.atan2(a * cz + h * cx, a * cx - h * cz)
    if y <= B:                                        # the hoist stage straight from y and r (exact; no cancellation noise at y 0)
        xin, ydn, q = r * yb * yb / (B * B), yb, min(dk / (2 * LINK), 0.99)
    else:
        xin = -((wx - sx) * math.cos(th) - (wz - sz) * math.sin(th))
        ydn = -((wx - sx) * math.sin(th) + (wz - sz) * math.cos(th))
        q = min(max(math.hypot(wx - sx, wz - sz) / (2 * LINK), 0.0), 0.99)
    phs = math.atan2(ydn + 1e-9, xin) - math.acos(q)
    phe = math.pi - 2.0 * math.acos(q)
    phw = phe - phs + th
    E = (sx - LINK * math.cos(phs - th), sz - LINK * math.sin(phs - th))
    return dict(boom=th, shoulder=phs, elbow=phe, wrist=phw, S=(sx, sz), E=E, W=(wx, wz), D=D)

# ------------------------------------------------------------------ the driver chain
def _c(v, nd=6):
    return ("%%.%df" % nd) % v
_K = dict(RT=_c(R_TIP), ZS=_c(ZS), B=_c(BOARD_M, 2), BB=_c(BOARD_M * BOARD_M, 4), XJ=_c(XJ, 3), ZJ=_c(ZJ, 3), LJ=_c(LJ), LJ2=_c(LJ * LJ), A0=_c(A0, 7), L2=_c(2 * LINK, 2))
def _e(s):
    for k, v in _K.items():
        s = s.replace("{%s}" % k, v)
    return s
# stage -> three (variables, expression) for its location x, y, z. A variable names an earlier stage's channel as "M2.x",
# or the inputs "y" (handle, metres down) and "r" (reach_in).
CHAIN = [
    ("M1", [(["y", "r"], "{RT} - r*min(y,{B})*min(y,{B})/{BB}"),                                                       # wx
            (["y"], "{ZS} - y"),                                                                                        # wz
            (["y", "r"], "sqrt(pow(r*min(y,{B})*min(y,{B})/{BB}, 2) + min(y,{B})*min(y,{B}))")]),                      # dk: the knee's bend at min(y, BOARD)
    ("M2", [(["M1.x", "M1.y"], "sqrt(pow(wx - {XJ}, 2) + pow(wz - {ZJ}, 2))"),                                        # L: joint to wrist
            (["M1.x", "M1.y"], "(wx - {XJ})/sqrt(pow(wx - {XJ}, 2) + pow(wz - {ZJ}, 2))"),                            # cx
            (["M1.x", "M1.y"], "(wz - {ZJ})/sqrt(pow(wx - {XJ}, 2) + pow(wz - {ZJ}, 2))")]),                          # cz
    ("M3", [(["y", "M1.z", "M2.x"], "dk if y <= {B} else min(max(dk, L - {LJ} + 0.05), 5.94)"),                      # D: the knee's bend used
            ([], "0.0"), ([], "0.0")]),
    ("M4", [(["M3.x", "M2.x"], "({LJ2} - D*D + L*L)/(2.0*L)"),                                                        # a: along J->wrist to the shoulder
            (["M3.x", "M2.x"], "sqrt(max({LJ2} - pow(({LJ2} - D*D + L*L)/(2.0*L), 2), 0.0))"),                        # h: across, up and out
            ([], "0.0")]),
    ("M5", [(["M4.x", "M4.y", "M2.y", "M2.z"], "{XJ} + a*cx - h*cz"),                                                 # sx
            (["M4.x", "M4.y", "M2.y", "M2.z"], "{ZJ} + a*cz + h*cx"),                                                 # sz
            (["y", "M4.x", "M4.y", "M2.y", "M2.z"], "0.0 if y <= {B} else {A0} - atan2(a*cz + h*cx, a*cx - h*cz)")]),   # th: boom pitch, down positive
    ("M6", [(["y", "r", "M1.x", "M1.y", "M5.x", "M5.y", "M5.z"], "r*min(y,{B})*min(y,{B})/{BB} if y <= {B} else -((wx - sx)*cos(th) - (wz - sz)*sin(th))"),   # xin: wrist in from the shoulder, boom frame
            (["y", "M1.x", "M1.y", "M5.x", "M5.y", "M5.z"], "min(y,{B}) if y <= {B} else -((wx - sx)*sin(th) + (wz - sz)*cos(th))"),                     # ydn: wrist down from the shoulder
            (["y", "M1.z", "M1.x", "M1.y", "M5.x", "M5.y"], "clamp(dk/{L2}, 0.0, 0.99) if y <= {B} else clamp(sqrt(pow(wx - sx, 2) + pow(wz - sz, 2))/{L2}, 0.0, 0.99)")]),   # q = D / 6
]
VAR_NAME = {"M1.x": "wx", "M1.y": "wz", "M1.z": "dk", "M2.x": "L", "M2.y": "cx", "M2.z": "cz", "M3.x": "D", "M4.x": "a", "M4.y": "h",
            "M5.x": "sx", "M5.y": "sz", "M5.z": "th", "M6.x": "xin", "M6.y": "ydn", "M6.z": "q", "y": "y", "r": "r"}
OUT_EXPR = {"boom": (["M5.z"], "th"),
            "shoulder": (["M6.x", "M6.y", "M6.z"], "atan2(ydn + 0.000000001, xin) - acos(q)"),
            "elbow": (["M6.z"], "pi - 2.0*acos(q)"),
            "wrist": (["M6.x", "M6.y", "M6.z", "M5.z"], "(pi - 2.0*acos(q)) - (atan2(ydn + 0.000000001, xin) - acos(q)) + th")}

def _fill(driver, varnames, expr, inputs, channel_of):
    driver.type = 'SCRIPTED'
    for vn in varnames:
        v = driver.variables.new()
        v.name = VAR_NAME[vn]
        if vn in inputs:
            v.type = 'SINGLE_PROP'
            v.targets[0].id, v.targets[0].data_path = inputs[vn]
        else:
            stage, ch = vn.split(".")
            spec = channel_of(stage)
            t = v.targets[0]
            if spec.get("prop_path"):                  # the helper's values as custom properties (the rig: drivers of one armature
                v.type = 'SINGLE_PROP'                 # must not read each other's bone transforms, that order is not guaranteed)
                t.id = spec["id"]
                t.data_path = spec["prop_path"] + '["%s"]' % ch
            else:                                      # the helper's location channels (the unrigged model: one empty per stage)
                v.type = 'TRANSFORMS'
                t.id = spec["id"]
                if spec.get("bone_target"):
                    t.bone_target = spec["bone_target"]
                t.transform_type = {"x": 'LOC_X', "y": 'LOC_Y', "z": 'LOC_Z'}[ch]
                t.transform_space = 'LOCAL_SPACE'
    driver.expression = _e(expr)
    assert len(driver.expression) < 250, (len(driver.expression), driver.expression)

def build_chain(driver_add_of, inputs, channel_of):
    """Writes the chain's drivers. driver_add_of(stage, index) -> the fcurve of that helper's location channel;
    inputs: {"y": (id, data_path), "r": (id, data_path)}, read as SINGLE_PROP; channel_of(stage) -> {"id": ..., "bone_target": ...}
    for the TRANSFORMS variables that read a helper's location."""
    for stage, chans in CHAIN:
        for i, (vn, expr) in enumerate(chans):
            _fill(driver_add_of(stage, i).driver, vn, expr, inputs, channel_of)

def drive_output(fcurve, key, sign, inputs, channel_of):
    """One joint's driver: key in boom / shoulder / elbow / wrist; sign -1 negates (object rotations about Y)."""
    vn, expr = OUT_EXPR[key]
    _fill(fcurve.driver, vn, ("-(%s)" % expr) if sign < 0 else expr, inputs, channel_of)