"""Пушер после снятия коллизии с луча лазера.

Товар вставал на x = -3.044 и не доходил до точки срабатывания PUSH_X = -3.9. Причина
найдена в списке коллайдеров: /World/SortingRig/LaserGate/Beam - визуализация луча
датчика - имеет ВКЛЮЧЁННУЮ коллизию и перекрывает всю ширину полотна (y -0.42..+0.42
при ленте -0.45..+0.45) на высоте 19 мм над лентой. Датчик должен смотреть, а не
преграждать; коллизия с него снимается, после чего проверяется сам пушер.
"""
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
from robozon_sorter.sim.mechanics import Cell
from robozon_sorter.sim.plow import Plow

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)

# снять коллизию со всей визуализации датчиков
off = []
for p in stage.Traverse():
    path = str(p.GetPath())
    if "LaserGate" in path or "AimRay" in path or "TriggerRay" in path:
        a = p.GetAttribute("physics:collisionEnabled")
        if a:
            a.Set(False); off.append(path)
        elif p.HasAPI(UsdPhysics.CollisionAPI):
            UsdPhysics.CollisionAPI(p).CreateCollisionEnabledAttr().Set(False); off.append(path)
print(f"коллизия снята с {len(off)} премов визуализации датчиков:")
for o in off:
    print(f"   {o}")

bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
TOP = bb.ComputeWorldBound(stage.GetPrimAtPath("/World/ConveyorTrack_03/Belt")
                           ).ComputeAlignedRange().GetMax()[2]
GRIP = stage.GetPrimAtPath(plow_cell.GRIP_MATERIAL)

if stage.GetPrimAtPath("/World/_Goods").IsValid():
    stage.RemovePrim("/World/_Goods")
stage.DefinePrim("/World/_Goods", "Xform")
c = UsdGeom.Cube.Define(stage, "/World/_Goods/push_D"); 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)
rb = PhysxSchema.PhysxRigidBodyAPI.Apply(p)
rb.CreateEnableCCDAttr().Set(True); rb.CreateSolverPositionIterationCountAttr().Set(32)
if GRIP.IsValid():
    UsdShade.MaterialBindingAPI.Apply(p).Bind(
        UsdShade.Material(GRIP), bindingStrength=UsdShade.Tokens.strongerThanDescendants,
        materialPurpose="physics")

cell = Cell(stage, items={})
plow = Plow(stage); plow.target(C.PLOW_PRESET["D"])
print(f"\nход ножа {C.BLADE_HOME_Y} -> {C.BLADE_OUT_Y} ({C.BLADE_STROKE*1000:.0f} мм), "
      f"срабатывание у PUSH_X={C.PUSH_X}, скорость ножа {C.PUSHER_SPEED} м/с")

tl.play(); await app_utils.update_app_async(steps=25)
rp = RigidPrim(paths=["/World/_Goods/push_D"])
P, fired, t_str = [], False, None
t0 = float(tl.get_current_time()); t_end = t0 + 7.0
print("\n   t,с      x        y      событие")
while float(tl.get_current_time()) < t_end:
    q = rp.get_world_poses()[0].numpy()[0]
    P.append(q.copy())
    t = float(tl.get_current_time())
    ev = ""
    if not fired and float(q[0]) <= C.PUSH_X + 0.08:
        ev = "КОМАНДА ножу"
        print(f"   {t-t0:5.2f}  {float(q[0]):+7.3f}  {float(q[1]):+6.3f}   {ev}")
        t_str = await cell.stroke(app_utils, out=True)
        fired = True
        ev = ""
    if len(P) % 8 == 1:
        print(f"   {t-t0:5.2f}  {float(q[0]):+7.3f}  {float(q[1]):+6.3f}   {ev}")
    await app_utils.update_app_async(steps=4)
tl.stop(); await app_utils.update_app_async(steps=5)

dy = float(P[-1][1]) - float(P[0][1])
dx = float(P[-1][0]) - float(P[0][0])
print(f"\n   нож сработал: {fired}, ход занял {t_str if t_str else 0:.2f} с")
print(f"   товар: ({float(P[0][0]):+.2f},{float(P[0][1]):+.2f}) -> "
      f"({float(P[-1][0]):+.2f},{float(P[-1][1]):+.2f})")
print(f"   смещение: по X {dx*1000:+.0f} мм, по Y {dy*1000:+.0f} мм")
print(f"   ВЫВОД: {'ТОВАР УВЕДЁН НА ВЕТКУ' if dy > 0.20 else 'товар НЕ уведён на ветку'}")