"""Arducam B0332 (OV9281 1MP global-shutter USB, M12 lens) camera + printable cradle mount + URDF.
Datasheet: board 38x38x1.5mm, M12xP0.5 lens EFL2.8, holes 34x34 & 28x28, JST B4B-ZR connector.
Camera frame: board in XY plane (back at Z=0), optical axis = +Z. Mount = L-bracket, camera bolts to
the 28x28 pattern; a perpendicular foot (2x M3) is the placeholder interface to the gripper/wrist."""
import numpy as np, trimesh
def box(x0,x1,y0,y1,z0,z1):
    b=trimesh.creation.box(extents=[x1-x0,y1-y0,z1-z0]); b.apply_translation([(x0+x1)/2,(y0+y1)/2,(z0+z1)/2]); return b
def cylZ(x,y,r,z0,z1):
    c=trimesh.creation.cylinder(radius=r,height=z1-z0,sections=48); c.apply_translation([x,y,(z0+z1)/2]); return c
def cylY(x,z,r,y0,y1):
    c=trimesh.creation.cylinder(radius=r,height=y1-y0,sections=40); c.apply_transform(trimesh.transformations.rotation_matrix(np.pi/2,[1,0,0])); c.apply_translation([x,(y0+y1)/2,z]); return c
def U(p): return trimesh.boolean.union(p,engine="manifold")
def D(a,b): return trimesh.boolean.difference([a,b],engine="manifold")

# ---- dims (mm) ----
BW=38.0; BT=1.5                # board 38x38x1.5
HOLE=28.0                      # use inner 28x28 mount pattern
STANDOFF=4.0; SR=2.5           # standoff height / radius
HOLDER_OD=16.0; HOLDER_H=6.0   # M12 lens holder
LENS_OD=12.0; LENS_H=15.0      # M12 lens barrel
JST=(9,5,6)                    # B4B-ZR connector block approx
PLATE=44.0; PT=3.0
# board sits on standoffs: back at Z=STANDOFF
zb=STANDOFF
# ---- camera body (visual only) ----
board=box(-BW/2,BW/2,-BW/2,BW/2,zb,zb+BT)
holder=cylZ(0,0,HOLDER_OD/2,zb+BT,zb+BT+HOLDER_H)
lens=cylZ(0,0,LENS_OD/2,zb+BT+HOLDER_H,zb+BT+HOLDER_H+LENS_H)
jst=box(BW/2-JST[0],BW/2,-JST[1]/2,JST[1]/2,zb-JST[2]+BT,zb+BT)  # on +X edge, under board
cam=trimesh.util.concatenate([board,holder,lens,jst]); cam.export("camera_body.stl")
opt_z=zb+BT+HOLDER_H+LENS_H    # optical frame at lens front
# ---- mount: backing plate + 4 standoffs + perpendicular foot ----
plate=box(-PLATE/2,PLATE/2,-PLATE/2,PLATE/2,-PT,0)
so=[cylZ(sx,sy,SR,0,zb) for sx in (-HOLE/2,HOLE/2) for sy in (-HOLE/2,HOLE/2)]
foot=box(-PLATE/2,PLATE/2,-PLATE/2-3,-PLATE/2,-24,0)          # perpendicular foot at -Y edge (X-Z plane)
mount=U([plate]+so+[foot])
# camera bolt holes (M2 clearance) through standoffs+plate
for sx in (-HOLE/2,HOLE/2):
    for sy in (-HOLE/2,HOLE/2):
        mount=D(mount,cylZ(sx,sy,1.1,-PT-1,zb+1))
# foot mount holes (M3) through the foot (along Y)
for hx in (-13,13):
    mount=D(mount,cylY(hx,-14,1.7,-PLATE/2-4,-PLATE/2+1))
mount.export("camera_mount.stl")
print(f"camera_body: bbox={np.round(cam.extents,1).tolist()}mm  optical_z={opt_z:.1f}mm")
print(f"camera_mount: wt={mount.is_watertight} bodies={len(mount.split(only_watertight=False))} vol={mount.volume/1000:.1f}cm3")

# ---- URDF ----
import xml.etree.ElementTree as ET
def vis(link,fn,rgba):
    v=ET.SubElement(link,"visual"); ET.SubElement(v,"origin",{"xyz":"0 0 0","rpy":"0 0 0"})
    ET.SubElement(ET.SubElement(v,"geometry"),"mesh",{"filename":fn,"scale":"0.001 0.001 0.001"})
    ET.SubElement(ET.SubElement(v,"material",{"name":fn}),"color",{"rgba":rgba})
r=ET.Element("robot",{"name":"arducam_b0332_mount"})
lm=ET.SubElement(r,"link",{"name":"camera_mount"}); vis(lm,"camera_mount.stl","0.30 0.66 0.45 1")
lb=ET.SubElement(r,"link",{"name":"camera_body"}); vis(lb,"camera_body.stl","0.12 0.12 0.14 1")
j=ET.SubElement(r,"joint",{"name":"mount_to_cam","type":"fixed"}); ET.SubElement(j,"parent",{"link":"camera_mount"}); ET.SubElement(j,"child",{"link":"camera_body"})
lo=ET.SubElement(r,"link",{"name":"camera_optical_frame"})
jo=ET.SubElement(r,"joint",{"name":"cam_to_optical","type":"fixed"}); ET.SubElement(jo,"parent",{"link":"camera_body"}); ET.SubElement(jo,"child",{"link":"camera_optical_frame"})
ET.SubElement(jo,"origin",{"xyz":f"0 0 {opt_z/1000:.4f}","rpy":"0 0 0"})
ET.indent(ET.ElementTree(r),"  "); ET.ElementTree(r).write("camera_mount.urdf",encoding="unicode",xml_declaration=True)
print("wrote camera_mount.urdf")