"""Compute our modular-arm joint BENDS that reproduce smith300's joint-axis RELATIONSHIPS,
using OUR servo's own output (+Y) as every joint axis (physically correct, not a hack).

Principle: smith300 joints rotate about local +Z; ours about local +Y (the real output shaft).
Let C be the fixed change-of-basis mapping our +Y -> smith300's +Z (C = Rx(+90deg), since
Rx(90)@[0,1,0]=[0,0,1]). If our link frame is L_i_our = L_i_smith @ C, then our joint axis +Y
physically coincides with smith300's joint axis +Z at every joint, AND the relative rotation
between consecutive joint frames becomes the conjugate of smith300's:
    R_i_our = C^-1 @ R_i_smith @ C
So we feed R_i_our (as rpy) into the modular generator's per-joint BEND, axis stays +Y.
This keeps each servo mounted with output = its joint (plausible) while matching smith300's DOF.
"""
import numpy as np
from scipy.spatial.transform import Rotation as Rot

# smith300 joint origin rpy (URDF rpy = extrinsic xyz = Rz(yaw)Ry(pitch)Rx(roll)), pan1..pan7
SMITH_RPY = [
    (3.14159, 0, 3.14159),
    (0, -1.5708, -1.5708),
    (0, 0, 1.5708),
    (0, 0, 0),
    (1.5708, 0, 0),
    (0, 1.5708, -3.14159),
    (0, 0, 0),
]
C = Rot.from_euler("x", 90, degrees=True)          # our +Y -> smith300 +Z
Cinv = C.inv()

print("# conjugated per-joint BENDS (our +Y joint axis reproduces smith300 DOF):")
print("BENDS = [")
for i, rpy in enumerate(SMITH_RPY, 1):
    R = Rot.from_euler("xyz", rpy)                  # smith300 joint frame rotation
    R_our = Cinv * R * C                            # conjugate into our +Y convention
    r, p, y = R_our.as_euler("xyz")
    # sanity: our +Y mapped through R_our, vs smith300 +Z through R -> should coincide via C
    print(f"    ({r:+.5f}, {p:+.5f}, {y:+.5f}),   # j{i}: smith300 rpy {rpy}")
print("]")

# verification: the physical joint-axis of consecutive joints should match smith300's relative angle
print("\n# verify: angle between consecutive joint axes (deg) — ours vs smith300")
A_s = np.eye(3); A_o = np.eye(3)
axes_s = []; axes_o = []
for i, rpy in enumerate(SMITH_RPY):
    R = Rot.from_euler("xyz", rpy).as_matrix()
    A_s = A_s @ R
    axes_s.append(A_s @ np.array([0, 0, 1.0]))      # smith joint axis (world), local +Z
    R_our = (Cinv * Rot.from_euler("xyz", rpy) * C).as_matrix()
    A_o = A_o @ R_our
    axes_o.append(A_o @ np.array([0, 1.0, 0]))       # our joint axis (world), local +Y
for i in range(1, len(SMITH_RPY)):
    da_s = np.degrees(np.arccos(np.clip(np.dot(axes_s[i-1], axes_s[i]), -1, 1)))
    da_o = np.degrees(np.arccos(np.clip(np.dot(axes_o[i-1], axes_o[i]), -1, 1)))
    print(f"  j{i}->j{i+1}: smith={da_s:6.1f}   ours={da_o:6.1f}   {'OK' if abs(da_s-da_o)<0.5 else 'MISMATCH'}")