"""SHORTER arm variant (Cameron 2026-07-14): links 1->2 and 2->3 halved (246->123mm, 206->103mm),
downstream modules follow rigidly, connectors REGENERATED for the new spans. -> two_servo_shorter.urdf"""
import os, 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"
L = g.read_layout(GLB)
L = [np.array(T, float) for T in L]
p = [T[:3, 3].copy() for T in L]
v12, v23 = p[2] - p[1], p[3] - p[2]
new_p = list(p)
new_p[2] = p[1] + 0.5 * v12
new_p[3] = new_p[2] + 0.5 * v23
d3 = new_p[3] - p[3]
for i in (4, 5):
    new_p[i] = p[i] + d3
for i, T in enumerate(L):
    T[:3, 3] = new_p[i]
g.LAYOUT = L
print("halved links: 1-2", round(np.linalg.norm(v12)*500, 1), "mm, 2-3", round(np.linalg.norm(v23)*500, 1), "mm")

g.CONNECTOR_FMT = "connector_shorter_{}.stl"
for i in range(1, len(g.LAYOUT)):
    c = curvy_connector(i)
    c.export(f"connector_shorter_{i}.stl")
    print(f"connector_shorter_{i}: vol={c.volume/1000:.1f}cm3 wt={c.is_watertight}")
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_shorter.urdf")
finally:
    shutil.copy("_bh_bak.stl", "base_holder.stl")
    os.remove("_bh_bak.stl")
print("wrote two_servo_shorter.urdf")