"""Почему пробы не поехали: есть ли коллизия на лентах и куда падают тела.

Ни одна проба не сдвинулась, две улетели. Это картина не буксования, а отсутствия опоры:
если у према Belt нет коллайдера, тело проваливается сквозь ленту и дальше поведение
случайно. Поэтому проверяется опора, а не сила трения.
"""
import sys
REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
    sys.path.insert(0, REPO)

import omni.usd, omni.timeline
import isaacsim.core.experimental.utils.app as app_utils
from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema
from isaacsim.core.experimental.prims import RigidPrim

stage = omni.usd.get_context().get_stage()
bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])

BELTS = ["/World/ConveyorTrack_05/Belt", "/World/ConveyorTrack/Belt",
         "/World/ConveyorTrack_03/Belt", "/World/ConveyorTrack_04/Belt",
         "/World/ConveyorTrack_01/Belt", "/World/ConveyorTrack_06/Belt",
         "/World/ConveyorTrack_03/Belt_01"]

print("ОПОРА ПОД ТОВАРОМ:\n")
for path in BELTS:
    pr = stage.GetPrimAtPath(path)
    if not pr.IsValid():
        print(f"  {path}: НЕТ ПРЕМА"); continue
    r = bb.ComputeWorldBound(pr).ComputeAlignedRange()
    mn, mx = r.GetMin(), r.GetMax()
    own = pr.HasAPI(UsdPhysics.CollisionAPI)
    rb = pr.HasAPI(UsdPhysics.RigidBodyAPI)
    kin = UsdPhysics.RigidBodyAPI(pr).GetKinematicEnabledAttr().Get() if rb else None
    # коллайдеры среди потомков
    kids, meshes = [], []
    for d in Usd.PrimRange(pr):
        if d == pr:
            continue
        if d.HasAPI(UsdPhysics.CollisionAPI):
            kids.append(d.GetName())
        if d.IsA(UsdGeom.Mesh):
            meshes.append(d.GetName())
    print(f"  {path}")
    print(f"     тип={pr.GetTypeName()}  коллизия_на_себе={own}  rigid={rb} кинематик={kin}")
    print(f"     габарит z {mn[2]:.3f}..{mx[2]:.3f}   xy {mn[0]:.2f}..{mx[0]:.2f} / {mn[1]:.2f}..{mx[1]:.2f}")
    print(f"     потомков с коллизией: {len(kids)} {kids[:4]}   мешей: {len(meshes)} {meshes[:4]}")

# что вообще есть под точкой (-1.0, 0.0): все коллайдеры, чей габарит её накрывает
print("\nЧТО НАКРЫВАЕТ ТОЧКУ (-1.0, 0.0) сверху вниз:")
hits = []
for p in stage.Traverse():
    if not p.HasAPI(UsdPhysics.CollisionAPI):
        continue
    r = bb.ComputeWorldBound(p).ComputeAlignedRange()
    mn, mx = r.GetMin(), r.GetMax()
    if mn[0] <= -1.0 <= mx[0] and mn[1] <= 0.0 <= mx[1]:
        hits.append((mx[2], str(p.GetPath()), mn[2]))
for top, path, bot in sorted(hits, reverse=True)[:10]:
    print(f"   z {bot:6.3f}..{top:6.3f}  {path}")
if not hits:
    print("   НИЧЕГО - под товаром нет ни одного коллайдера")

# где сейчас лежат пробы
probe = stage.GetPrimAtPath("/World/_BeltProbe")
if probe.IsValid():
    print("\nГДЕ ОКАЗАЛИСЬ ПРОБЫ:")
    for c in probe.GetChildren():
        r = bb.ComputeWorldBound(c).ComputeAlignedRange()
        m = r.GetMidpoint()
        print(f"   {c.GetName():10s} ({m[0]:+7.2f}, {m[1]:+7.2f}, {m[2]:+7.2f})")