"""Full bottom-hugging servo holder (Cameron 2026-06-25). The sleeve+flange wrap_holder reads sparse;
this CRADLES the whole bottom of the servo: a solid block under+around the body, then the servo body is
boolean-SUBTRACTED so the cavity conforms to it (a hug), then the two mounting holes are cut.

Implementation: the raw st3215 mesh isn't watertight and the geared STEP crashes OCP, so we subtract the
servo's CONVEX HULL (a clean watertight solid hugging the housing) via the manifold3d mesh-boolean engine
— stable, and the right shape (we want to hug the smooth body, not the internal gears). Output axis is
kept clear so the perfect yoke still rotates.  Frame (mm): output axis +Y through (-25.5,0).
"""
import numpy as np
import trimesh

SERVO = "/data/cameron/repos/smith300_para_stuff/st3215.stl"
WALL = 3.0
CLEAR = 0.3              # print gap: dilate the hull before subtracting
WRAP_FRAC = 0.80
YOKE_CLEAR = 1.0        # mm clearance carved around the yoke
BOLT_R = 1.3            # SMALL screw (~M2.5): just a clearance hole around each real screw hole
# the servo's real -Y (bottom) mount-hole positions (x,z), drilled along +Y. Screws pass through the
# holder bottom into the servo's threaded holes to mount the holder ON the servo.
HOLES = [(-17.2, 10.2), (-17.2, -10.2), (7.2, 10.2), (7.2, -10.2)]


def _cyl_Y(x, z, r, h, y0):
    """A cylinder of radius r, length h, axis +Y, starting at y0, centred at (x,*,z)."""
    c = trimesh.creation.cylinder(radius=r, height=h)            # axis Z by default
    c.apply_transform(trimesh.transformations.rotation_matrix(np.pi / 2, [1, 0, 0]))  # Z->Y
    c.apply_translation([x, y0 + h / 2, z])
    return c


def make():
    servo = trimesh.load(SERVO, force="mesh")
    lo, hi = servo.bounds
    hull = servo.convex_hull
    hull.vertices += hull.vertex_normals * CLEAR                 # dilate outward for print clearance

    ytop = lo[1] + WRAP_FRAC * (hi[1] - lo[1])
    block = trimesh.creation.box(
        extents=[(hi[0] - lo[0]) + 2 * WALL, (ytop - (lo[1] - WALL)), (hi[2] - lo[2]) + 2 * WALL])
    block.apply_translation([(lo[0] + hi[0]) / 2, (lo[1] - WALL + ytop) / 2, (lo[2] + hi[2]) / 2])

    part = trimesh.boolean.difference([block, hull], engine="manifold")            # the conformal hug
    # clear EXACTLY where the (perfect) yoke sits: subtract the yoke itself (dilated for clearance).
    # yoke_exact.stl is authored in this same servo frame at the rest pose, so subtract it directly.
    yoke = trimesh.load("/data/cameron/repos/smith300_para_stuff/yoke_exact.stl", force="mesh")
    yoke.vertices += yoke.vertex_normals * YOKE_CLEAR
    part = trimesh.boolean.difference([part, yoke], engine="manifold")
    for (x, z) in HOLES:                                          # small screw holes at the EXACT positions
        part = trimesh.boolean.difference([part, _cyl_Y(x, z, BOLT_R, 40.0, -20.0)], engine="manifold")
    return part


if __name__ == "__main__":
    part = make()
    part.export("wrap_holder_full.stl")
    print(f"wrote wrap_holder_full.stl  vol={part.volume/1000:.1f}cm3  watertight={part.is_watertight}  "
          f"bbox={np.round(part.extents,1).tolist()}")