"""Real printable structural BRIDGE per child link: yoke far-end -> this link's holder.

Replaces the placeholder box bar. Built in the child link frame (mm): a load-graded tapered beam from
the yoke far-end to the holder, a boss cupping the yoke tip, and a boss mating the holder. One watertight
solid per link -> STL, referenced at identity by so100_para.py. Endpoints come from the SAME servo
placement + optimized rolls as the URDF (single source of truth), so they line up exactly.
"""
import math
import numpy as np
from build123d import Vector, Cylinder, Cone, Axis, export_stl
from so100_para import JOINTS, ROLLS, servo_pose, _T, _mat, LINKS

YOKE_TIP = np.array([-0.118, 0.0, 0.0])
HOLDER_C = np.array([0.0028, -0.0096, 0.0])
HOLDER_HALF = np.array([0.0102, 0.019, 0.0158])           # wrap_holder half-extents (servo frame, m)
TIP_R, HOLD_R, BEAM_R_TIP, BEAM_R_HOLD = 12.0, 14.0, 9.0, 12.0
CHILD = {1: "shoulder", 2: "upper_arm", 3: "lower_arm", 4: "wrist", 5: "wrist2", 6: "gripper", 7: "jaw"}


def _orient(solid, axis_vec):
    z = Vector(0, 0, 1)
    a = Vector(*axis_vec).normalized()
    ang = math.degrees(math.acos(max(-1.0, min(1.0, z.dot(a)))))
    ax = z.cross(a)
    if ax.length < 1e-9:
        ax = Vector(1, 0, 0)
    return solid.rotate(Axis((0, 0, 0), tuple(ax)), ang)


def _cone(p1, p2, r1, r2):
    p1, p2 = Vector(*p1), Vector(*p2)
    v = p2 - p1
    c = _orient(Cone(r1, r2, v.length), tuple(v))
    return c.translate(((p1.X + p2.X) / 2, (p1.Y + p2.Y) / 2, (p1.Z + p2.Z) / 2))


def _cyl_at(center, axis, r, h):
    return _orient(Cylinder(r, h), axis).translate(tuple(center))


def endpoints(k):
    """tip (yoke far-end) and target (this link's holder, or jaw finger) in child frame, mm."""
    pj = JOINTS[k - 1]
    Rp, tp = servo_pose(pj[3], pj[4], pj[5], ROLLS[k - 1])
    P = np.linalg.inv(_T(pj[3], pj[4])) @ _mat(Rp, tp)
    tip = (P @ np.append(YOKE_TIP, 1))[:3]
    if k < len(JOINTS):
        cj = JOINTS[k]
        Rc, tc = servo_pose(cj[3], cj[4], cj[5], ROLLS[k])
        center = Rc.apply(HOLDER_C) + tc
        # mate the boss to the holder's OUTER SURFACE facing the bridge (not buried at the centre):
        # walk back from the centre toward the yoke tip by the holder half-extent along the approach.
        u = (center - tip) / np.linalg.norm(center - tip)
        half_ext = float(np.dot(np.abs(Rc.inv().apply(u)), HOLDER_HALF))
        tgt = center - u * (half_ext - 0.003)              # leave 3mm overlap for a solid bonded joint
    else:
        tgt = np.array([0.0, -0.03, 0.0])
    return tip * 1000.0, tgt * 1000.0


def build(k):
    tip, tgt = endpoints(k)
    L = float(np.linalg.norm(np.array(tgt) - np.array(tip)))   # bridge length (mm)
    # LOAD-GRADED thickness: longer bridges sit closer to the base (more bending moment) -> beefier.
    r_hold = min(20.0, max(11.0, 10.0 + 0.062 * L))
    r_tip = 0.72 * r_hold
    u = (Vector(*tgt) - Vector(*tip)).normalized()
    beam = _cone(tuple(tip), tuple(tgt), r_tip, r_hold)
    cup = _cyl_at(tuple(tip), tuple(u), max(TIP_R, r_tip + 3), 14)   # cups the yoke far-end
    boss = _cyl_at(tuple(tgt), tuple(u), max(HOLD_R, r_hold + 2), 16)  # mates the holder
    return beam + cup + boss


if __name__ == "__main__":
    for k, child in CHILD.items():
        solid = build(k)
        f = f"so100_bridge_{child}.stl"
        export_stl(solid, f)
        print(f"wrote {f}  ({'valid' if solid.is_valid else 'INVALID'})")