"""Per child link: yoke far-end vs this link's holder centre, and the gap a bridge must span."""
import numpy as np
from so100_para import JOINTS, ROLLS, servo_pose, _T, _mat, LINKS

YOKE_TIP = np.array([-0.118, 0.0, 0.0])     # yoke_exact far end in servo frame (m)
HOLDER_C = np.array([0.0028, -0.0096, 0.0]) # wrap_holder centre in servo frame (m)

for k in range(1, 6):
    _, _, _, xyz, rpy, axis, lo, hi = JOINTS[k]
    R_s, t_s = servo_pose(xyz, rpy, axis, ROLLS[k])
    holder = R_s.apply(HOLDER_C) + t_s                       # this link's holder centre (child frame)
    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)       # parent yoke pose in child frame
    tip = (P @ np.append(YOKE_TIP, 1))[:3]                   # yoke far end in child frame
    gap = np.linalg.norm(holder - tip)
    print(f"{LINKS[k]:10s} yoke_tip(mm)={np.round(tip*1000,1).tolist()}  "
          f"holder(mm)={np.round(holder*1000,1).tolist()}  GAP={gap*1000:5.1f}mm")