"""'Beauty' variant: filleted holders (holder_fillet.stl) + curvy/waisted connectors (with the connector_3
override) + filleted base. Separate files (two_servo_beauty.urdf). Servo cavity/holes unchanged."""
import os
import shutil
import numpy as np
import trimesh
import gen_two_servo as g
from gen_curvy import curvy_connector

GLB = "two_servo_from_blender.glb"

g.LAYOUT = g.read_layout(GLB)

# filleted base_holder = union(holder_fillet, base pieces) — same base pieces, filleted holder
scene = trimesh.load(GLB)
base_inv = np.linalg.inv(np.array(scene.graph["servo_0"][0], float))
pieces = []
for node in sorted(scene.graph.nodes_geometry):
    if node.startswith("base_cube"):
        T, gname = scene.graph[node]
        m = scene.geometry[gname].copy()
        m.apply_transform(base_inv @ np.array(T, float))
        m.apply_scale(1000.0 / g.GLB_SCALE)
        pieces.append(m.bounding_box_oriented)
base_h = trimesh.boolean.union([trimesh.load("holder_fillet.stl", force="mesh")] + pieces, engine="manifold")
base_h.export("base_holder_fillet.stl")
print(f"base_holder_fillet: wt={base_h.is_watertight} bodies={len(base_h.split(only_watertight=False))}")

# curvy connectors (connector_3 pinched via WAIST_BY_CONN)
g.CONNECTOR_FMT = "connector_beauty_{}.stl"
for i in range(1, len(g.LAYOUT)):
    c = curvy_connector(i)
    c.export(f"connector_beauty_{i}.stl")
    print(f"connector_beauty_{i}: vol={c.volume/1000:.1f}cm3 wt={c.is_watertight}")

# URDF: holders -> holder_fillet, base -> base_holder_fillet (temp-swap the hardcoded base_holder.stl)
g.PARTS[1] = ("holder_fillet.stl",) + tuple(g.PARTS[1][1:])
g.PARTS[2] = ("yoke_fillet.stl",) + tuple(g.PARTS[2][1:])
shutil.copy("base_holder.stl", "_bh_bak.stl")
shutil.copy("base_holder_fillet.stl", "base_holder.stl")
try:
    g.gen_urdf("two_servo_beauty.urdf")
finally:
    shutil.copy("_bh_bak.stl", "base_holder.stl")
    os.remove("_bh_bak.stl")