"""Same as pusher_diag.py but WITHOUT the pre-sweep settle wait that let the item slide
clean past the blade's x-window before the sweep even started - fire the sweep the
instant the item is placed, matching the real pipeline's timing exactly."""
import sys
REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
    sys.path.insert(0, REPO)
for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]:
    del sys.modules[_m]
import importlib
importlib.invalidate_caches()

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

from robozon_sorter import config as C
from robozon_sorter.sim import scene as _scene

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)

blade_prim = stage.GetPrimAtPath(_scene.BLADE)
def blade_op():
    for op in UsdGeom.Xformable(blade_prim).GetOrderedXformOps():
        if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
            return op
blade_base = blade_op().Get()
def blade_to(y):
    b = blade_base
    blade_op().Set(Gf.Vec3d(b[0], y - _scene.BLADE_PARENT_Y, b[2]))
blade_to(C.BLADE_HOME_Y)

name = "probe_item2"
items_dir = C.ROOT / "assets" / "items"
prim = UsdGeom.Xform.Define(stage, f"/World/Items/{name}").GetPrim()
prim.GetReferences().ClearReferences()
prim.GetReferences().AddReference(str(items_dir / "box_300x200x200.usd"))
xf = UsdGeom.Xformable(prim)
xf.ClearXformOpOrder()
xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(C.PUSH_X + 0.08, 0.0, C.BELT_Z + 0.10))
UsdPhysics.RigidBodyAPI.Apply(prim)
UsdPhysics.RigidBodyAPI(prim).CreateKinematicEnabledAttr().Set(False)
UsdPhysics.MassAPI.Apply(prim).CreateMassAttr().Set(0.6)
px = PhysxSchema.PhysxRigidBodyAPI.Apply(prim)
px.CreateEnableCCDAttr().Set(True)
px.CreateSolverPositionIterationCountAttr().Set(24)
px.CreateSleepThresholdAttr().Set(0.0)
UsdGeom.Imageable(prim).MakeVisible()
rp = RigidPrim(paths=[f"/World/Items/{name}"])

tl.play()
await app_utils.update_app_async(steps=3)   # bare minimum so PhysX registers the body, no drift budget
p0 = rp.get_world_poses()[0].numpy()[0]
print(f"start of sweep: x={p0[0]:+.3f} y={p0[1]:+.3f} z={p0[2]:+.3f}")
print(f"blade x-window at this Y: computed from Geom scale, PUSH_X={C.PUSH_X}")

speed = C.PUSHER_MAX_SAFE
a, b = C.BLADE_HOME_Y, C.BLADE_OUT_Y
duration = abs(b - a) / speed
t0 = float(tl.get_current_time())
i = 0
print(f"{'i':>3} {'t':>6} {'blade_y':>8} {'item_x':>8} {'item_y':>8} {'vx':>7} {'vy':>7}")
while True:
    u = min(1.0, (float(tl.get_current_time()) - t0) / duration)
    blade_to(a + (b - a) * u)
    await app_utils.update_app_async(steps=1)
    p = rp.get_world_poses()[0].numpy()[0]
    v = rp.get_velocities()[0].numpy()[0]
    print(f"{i:3d} {float(tl.get_current_time())-t0:6.3f} {a+(b-a)*u:8.3f} "
          f"{p[0]:8.3f} {p[1]:8.3f} {v[0]:7.3f} {v[1]:7.3f}")
    i += 1
    if u >= 1.0:
        break

p_final = rp.get_world_poses()[0].numpy()[0]
print(f"\nafter stroke: x={p_final[0]:+.3f} y={p_final[1]:+.3f} (moved y by {p_final[1]-p0[1]:+.3f})")
tl.stop()