"""Realistic self-collision sweep: step EACH joint independently across its URDF limits (others at 0),
report any servo-servo or bridge-through-other-servo collision with the offending joint+angle.
(Co-rotating all joints by one q, as before, is not a real configuration.)
"""
import sys
import numpy as np
import xml.etree.ElementTree as ET
from so100_check import check, D

urdf = sys.argv[1] if len(sys.argv) > 1 else "so100_para_connected.urdf"
root = ET.parse(D + urdf).getroot()
jlim = []
for j in root.findall("joint"):
    if j.get("type") == "revolute":
        lim = j.find("limit")
        jlim.append((j.get("name"), float(lim.get("lower")), float(lim.get("upper"))))

N = 13
bad = 0
for name, lo, hi in jlim:
    hits = []
    for a in np.linspace(lo, hi, N):
        sp, bs = check(urdf, {name: float(a)})
        if sp or bs:
            hits.append((round(float(a), 2), len(sp), len(bs)))
    if hits:
        bad += 1
        print(f"  {name:14s} [{lo:+.2f},{hi:+.2f}] COLLIDES at: {hits}")
    else:
        print(f"  {name:14s} [{lo:+.2f},{hi:+.2f}] clean")
print("CLEAN" if bad == 0 else f"{bad} joint(s) self-collide within limits")