"""Smoke test for scene/plow_cell.usd: load the cell, run the belts, swing the plow under
script control and confirm the arm physically follows.

Run it inside a live Isaac Sim through the code editor's python server, e.g.

    python isaacsim_send.py --context plow --file scripts/smoke_plow_cell.py

It asserts the things that were silent failures during the transfer: that the referenced
plow geometry actually composed (a stub resolves to 8 points, the real base to 72k), that
the belts got a surface velocity, and that commanding the drive moves the arm rather than
just setting an attribute nothing reads.
"""
import sys

REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
    sys.path.insert(0, REPO)

import omni.timeline
import isaacsim.core.experimental.utils.app as app_utils
from pxr import PhysxSchema, UsdGeom

from robozon_sorter import config as C
from robozon_sorter.sim import plow_cell
from robozon_sorter.sim.plow import Plow

stage, info = plow_cell.load(script_control=True)
print("loaded:", info)

# --- geometry actually composed? --------------------------------------------------------
for path, label, floor in ((C.PLOW_BASE + "/Geom/Mesh", "plow base", 1000),
                           (C.PLOW_ARM + "/Geom/Mesh", "plow arm", 1000)):
    pts = UsdGeom.Mesh(stage.GetPrimAtPath(path)).GetPointsAttr().Get()
    n = len(pts) if pts else 0
    print(f"  {label:10s} {n:>7} points {'OK' if n > floor else 'FAIL (stub or missing)'}")

# --- belts driven? ----------------------------------------------------------------------
driven = 0
for b in plow_cell.BELTS + [plow_cell.BRANCH]:
    p = stage.GetPrimAtPath(b)
    if p.IsValid() and p.HasAPI(PhysxSchema.PhysxSurfaceVelocityAPI):
        v = PhysxSchema.PhysxSurfaceVelocityAPI(p).GetSurfaceVelocityAttr().Get()
        if v and any(abs(c) > 1e-6 for c in v):
            driven += 1
print(f"  belts driven: {driven}/{len(plow_cell.BELTS) + 1}")

# --- plow moves? ------------------------------------------------------------------------
tl = omni.timeline.get_timeline_interface()
tl.play()
await app_utils.update_app_async(steps=30)

plow = Plow(stage)
rest = plow.angle
print(f"  rest angle {rest:+.2f} deg")

await plow.swing(app_utils, C.PLOW_SWING)
out = plow.angle
print(f"  swung to   {out:+.2f} deg (commanded {C.PLOW_SWING:+.1f})")

await plow.swing(app_utils, 0.0)
back = plow.angle
print(f"  returned   {back:+.2f} deg")

tl.stop()
moved = abs(out - rest) > 0.5 * C.PLOW_SWING
homed = abs(back) < 5.0
print(f"RESULT: arm moved={moved} returned_home={homed} "
      f"{'PASS' if moved and homed else 'FAIL'}")