"""
Parametric YOKE connector (build123d) that wraps the servo output on BOTH sides:
a C-bracket gripping the output horn (+Z top) AND the idler bearing (-Z bottom), with a
spine on the +X side and a structural link extending to the next module.

Servo frame (servo mesh at identity): output axis +Z at (AX, AY)=(12,0); horn at z=HORN_Z,
idler at z=IDLER_Z. Built in that frame so it can be rendered directly on the servo.
"""
from build123d import Box, Cylinder, Pos, export_stl

# --- servo output geometry (measured) ---
AX, AY = 12.0, 0.0
HORN_Z, IDLER_Z = 20.2, -19.4
BOLT_R, BOLT_D, BORE_D = 7.0, 2.9, 9.0

# --- yoke parameters (mm) ---
ARM_T = 4.0          # grip-plate thickness
ARM_W = 30.0         # grip-plate width (Y)
ARM_REACH = 14.0     # how far the plate extends past the axis toward -X (covers the disc)
SPINE_OFF = 19.0     # spine X distance from axis (past the body edge ~10.7 from axis)
SPINE_T = 6.0        # spine thickness (X)
SPINE_W = 26.0       # spine width (Y)
LINK_SEC = (22.0, 14.0)  # link cross-section (height Z, width Y)


def make_yoke(link_len=60.0):
    top_zc = HORN_Z + ARM_T / 2
    bot_zc = IDLER_Z - ARM_T / 2
    spine_h = (HORN_Z + ARM_T) - (IDLER_Z - ARM_T)
    spine_zc = (HORN_Z + ARM_T + IDLER_Z - ARM_T) / 2
    arm_x0, arm_x1 = -ARM_REACH, SPINE_OFF + SPINE_T / 2
    arm_len = arm_x1 - arm_x0
    arm_cx = (arm_x0 + arm_x1) / 2
    link_cx = SPINE_OFF + SPINE_T / 2 + link_len / 2

    y = (Pos(SPINE_OFF, 0, spine_zc) * Box(SPINE_T, SPINE_W, spine_h)
         + Pos(arm_cx, 0, top_zc) * Box(arm_len, ARM_W, ARM_T)          # top arm (horn grip)
         + Pos(arm_cx, 0, bot_zc) * Box(arm_len, ARM_W, ARM_T)          # bottom arm (idler grip)
         + Pos(link_cx, 0, spine_zc) * Box(link_len, LINK_SEC[1], LINK_SEC[0]))  # link to next module
    # bolt holes (R7 circle) + center bore through both grip plates, along Z
    for hx, hy in [(BOLT_R, 0), (-BOLT_R, 0), (0, BOLT_R), (0, -BOLT_R)]:
        y -= Pos(hx, hy, top_zc) * Cylinder(BOLT_D / 2, ARM_T * 3)
        y -= Pos(hx, hy, bot_zc) * Cylinder(BOLT_D / 2, ARM_T * 3)
    y -= Pos(0, 0, top_zc) * Cylinder(BORE_D / 2, ARM_T * 3)
    y -= Pos(0, 0, bot_zc) * Cylinder(BORE_D / 2, ARM_T * 3)
    return Pos(AX, AY, 0) * y          # into servo frame


if __name__ == "__main__":
    import sys
    L = float(sys.argv[1]) if len(sys.argv) > 1 else 60.0
    export_stl(make_yoke(L), "yoke.stl", tolerance=0.1, ascii_format=False)
    print("yoke ok")