"""Что держит товар на x = -3.15: список коллайдеров в этой полосе + трасса остановки.

Пушер проверить не вышло - товар не доехал до точки срабатывания (PUSH_X = -3.9) и встал
раньше. Прежде чем править пушер, надо понять, упирается товар в препятствие или теряет
привод: это разные неисправности, и внешне они неотличимы.
"""
import sys, math
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, UsdShade
from isaacsim.core.experimental.prims import RigidPrim
from robozon_sorter import config as C
from robozon_sorter.sim import plow_cell

stage = omni.usd.get_context().get_stage()
tl = omni.timeline.get_timeline_interface()
if tl.is_playing():
    tl.stop(); await app_utils.update_app_async(steps=10)

bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
print("КОЛЛАЙДЕРЫ, накрывающие полосу x -3.4..-3.0 на высоте ленты (z 1.75..2.10):")
for p in stage.Traverse():
    if not p.HasAPI(UsdPhysics.CollisionAPI):
        continue
    en = p.GetAttribute("physics:collisionEnabled")
    if en and en.Get() is False:
        continue
    r = bb.ComputeWorldBound(p).ComputeAlignedRange()
    mn, mx = r.GetMin(), r.GetMax()
    if mx[0] < -3.4 or mn[0] > -3.0:
        continue
    if mx[2] < 1.75 or mn[2] > 2.10:
        continue
    if abs(mn[1]) > 0.6 and abs(mx[1]) > 0.6 and mn[1] * mx[1] > 0:
        continue
    print(f"   x {mn[0]:+7.3f}..{mx[0]:+7.3f}  y {mn[1]:+6.2f}..{mx[1]:+6.2f}  "
          f"z {mn[2]:+6.3f}..{mx[2]:+6.3f}  {p.GetPath()}")

# какая лента под этой точкой и что у неё со скоростью
print("\nЛЕНТЫ ПОД x=-3.15:")
for path in list(plow_cell.BELTS) + [plow_cell.BRANCH]:
    pr = stage.GetPrimAtPath(path)
    if not pr.IsValid():
        continue
    r = bb.ComputeWorldBound(pr).ComputeAlignedRange()
    mn, mx = r.GetMin(), r.GetMax()
    if mn[0] <= -3.15 <= mx[0]:
        v = pr.GetAttribute("physxSurfaceVelocity:surfaceVelocity").Get()
        e = pr.GetAttribute("physxSurfaceVelocity:surfaceVelocityEnabled")
        print(f"   {path}: v={v} включено={e.Get() if e else None} "
              f"y {mn[1]:+.2f}..{mx[1]:+.2f}")

# трасса: пустить товар и печатать x, пока не встанет
TOP = bb.ComputeWorldBound(stage.GetPrimAtPath("/World/ConveyorTrack_03/Belt")
                           ).ComputeAlignedRange().GetMax()[2]
if stage.GetPrimAtPath("/World/_Stall").IsValid():
    stage.RemovePrim("/World/_Stall")
stage.DefinePrim("/World/_Stall", "Xform")
c = UsdGeom.Cube.Define(stage, "/World/_Stall/item"); c.CreateSizeAttr().Set(2.0)
xf = UsdGeom.Xformable(c.GetPrim())
xf.AddTranslateOp().Set(Gf.Vec3d(-2.40, 0.0, TOP + 0.055))
xf.AddScaleOp().Set(Gf.Vec3f(0.05, 0.05, 0.05))
p = c.GetPrim()
UsdPhysics.RigidBodyAPI.Apply(p); UsdPhysics.CollisionAPI.Apply(p)
UsdPhysics.MassAPI.Apply(p).CreateMassAttr().Set(0.5)
PhysxSchema.PhysxRigidBodyAPI.Apply(p).CreateSolverPositionIterationCountAttr().Set(32)
g = stage.GetPrimAtPath(plow_cell.GRIP_MATERIAL)
if g.IsValid():
    UsdShade.MaterialBindingAPI.Apply(p).Bind(
        UsdShade.Material(g), bindingStrength=UsdShade.Tokens.strongerThanDescendants,
        materialPurpose="physics")

tl.play(); await app_utils.update_app_async(steps=25)
rp = RigidPrim(paths=["/World/_Stall/item"])
print("\nТРАССА (старт x=-2.40):")
prev, t0 = None, float(tl.get_current_time())
for i in range(28):
    q = rp.get_world_poses()[0].numpy()[0]
    t = float(tl.get_current_time())
    d = "-" if prev is None else f"{(float(q[0])-prev)*1000:+7.1f} мм"
    print(f"   t={t-t0:5.2f}s  x={float(q[0]):+7.3f}  y={float(q[1]):+6.3f}  "
          f"z={float(q[2]):+6.3f}   {d}")
    prev = float(q[0])
    await app_utils.update_app_async(steps=6)
tl.stop(); await app_utils.update_app_async(steps=5)