"""Why goods stop at x=-6.0: place one item either side of the belt junction and watch.

Run A parks an item at x=-5.5, upstream of the transfer, and lets it drive at it.
Run B starts one already at x=-6.3, past the transfer, on the wide belt through the plow.

If A stalls and B runs, the transfer is blocked by structure, not by a dead belt. The
suspect is the downstream end frame of ConveyorTrack_03: `open_junction()` clears the shell
collider on the plow track and both lanes, but not on the track goods arrive *on*, so its
end plate stands across the path at exactly x=-6.0.
"""
import sys, time
REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
    sys.path.insert(0, REPO)

# The live Isaac process keeps every module it has ever imported, so an edited
# robozon_sorter/ on disk is invisible to a second run in the same session. Drop the
# package from sys.modules first or you spend the evening re-testing the old code.
for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]:
    del sys.modules[_m]
import importlib
importlib.invalidate_caches()   # a *new* module file is invisible until the finder is reset

import omni.timeline, isaacsim.core.experimental.utils.app as app_utils
from pxr import UsdPhysics
from robozon_sorter import config as C
from robozon_sorter.sim import plow_sort, plow_vision
from robozon_sorter.sim.mechanics import Cell

OPEN_03 = bool(globals().get("open_03", False))   # also clear ConveyorTrack_03's shell
START_X = float(globals().get("start_x", -5.5))
ITEM = globals().get("item", "barrel")
SECONDS = float(globals().get("seconds", 6.0))

stage, info = plow_vision.load(belt_speed=1.0, script_control=True)
plow_sort.keep_lanes_active(stage)
plow_sort.configure_lanes(stage, 1.0)
opened = plow_sort.open_junction(stage)
extra = None
if OPEN_03:
    p = stage.GetPrimAtPath("/World/ConveyorTrack_03/SM_ConveyorBelt_A24_02")
    a = p.GetAttribute("physics:collisionEnabled") or \
        UsdPhysics.CollisionAPI.Apply(p).CreateCollisionEnabledAttr()
    a.Set(False)
    extra = str(p.GetPath())
print(f"opened {len(opened)} shells, extra={extra}")

items = {k: v["zone"] for k, v in info["items"].items()}
await app_utils.update_app_async(steps=30)
cell = Cell(stage, items.keys())
cell.park_all()
await app_utils.update_app_async(steps=10)

cell.place(ITEM, (START_X, 0.0, C.BELT_Z + 0.10))
app_utils.play(commit=True)
await app_utils.update_app_async(steps=20)

print(f"\n{ITEM} from x={START_X}  (plow at x={C.PLOW_POS[0]})")
t0, last = time.time(), None
while time.time() - t0 < SECONDS:
    await app_utils.update_app_async(steps=12)
    p = cell.pose(ITEM)
    x, y, z = (float(v) for v in p[:3])
    moved = "" if last is None else f" dx={x - last:+.3f}"
    print(f"  t={time.time() - t0:4.1f} x={x:+.3f} y={y:+.3f} z={z:+.3f}{moved}")
    last = x
    if z < C.BELT_Z - 0.5:
        print("  -> fell off"); break

app_utils.stop()
await app_utils.update_app_async(steps=10)
print(f"verdict: {'REACHED PLOW' if last is not None and last < -6.6 else 'STALLED at x=%.2f' % (last or 0)}")