"""Does the kinematic blade actually PUSH, or does it pass through and let PhysX untangle?

The question the whole plow rests on and which nothing so far has answered. A kinematic body
moved with `setKinematicTarget` sweeps: PhysX derives a velocity from the pose delta and
transfers momentum to whatever it meets. A body moved by writing its pose is a **teleport**:
it reappears somewhere else, and anything it now overlaps is pushed apart by depenetration
only - a shove with no momentum behind it, roughly proportional to how deep the overlap is
rather than to how fast the blade was going.

The two look identical in a viewport and identical in a contact sensor. They differ in one
measurable: the item's velocity while the blade is on it.

    push     item picks up lateral speed close to the blade's tangential speed
    teleport item barely moves, gets a small separation nudge, and stops

This puts one item against a stationary blade, sweeps the blade through it, and records the
item's velocity every step. It also confirms the arm's simulated pose actually changes - if
PhysX never sees the rotation, the blade is a ghost and no amount of rate tuning matters.
"""
import sys
import time

REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
    sys.path.insert(0, REPO)
import importlib
for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]:
    del sys.modules[_m]
importlib.invalidate_caches()

import omni.timeline
import 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_cell, plow_vision, staging
from robozon_sorter.sim.mechanics import Cell
from robozon_sorter.sim.plow import Plow

ITEM = globals().get("item", "box_300x200x200")
RATE = float(globals().get("rate", 300.0))
BELT = bool(globals().get("belt", True))     # is the belt driving the item at the time

stage, info = plow_vision.load(belt_speed=0.8 if BELT else 0.0, script_control=True,
                               meshes_dir=f"{REPO}/assets/items")
staging.stage_cell(stage, preset="bright", floor=True)
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)

arm = stage.GetPrimAtPath(C.PLOW_ARM)
print("--- what the arm IS ---")
print("  kinematic :", UsdPhysics.RigidBodyAPI(arm).GetKinematicEnabledAttr().Get())
print("  hinge on  :", stage.GetPrimAtPath(C.PLOW_HINGE).GetAttribute(
    "physics:jointEnabled").Get())

plow = Plow(stage, kinematic=True)
plow.target(0.0)
# put the item just in front of the blade, offset to the side the blade sweeps toward
cell.place(ITEM, (-6.75, 0.10, C.BELT_Z + 0.06))
tl = omni.timeline.get_timeline_interface()
tl.play()
await app_utils.update_app_async(steps=40)


def vel():
    v = cell._rp[ITEM].get_velocities()[0].numpy()[0]
    return float(v[0]), float(v[1]), float((v[0] ** 2 + v[1] ** 2 + v[2] ** 2) ** 0.5)


p0 = cell.pose(ITEM)
print(f"\n--- before: item at x={float(p0[0]):+.3f} y={float(p0[1]):+.3f}, "
      f"arm measured {plow.angle:+.1f} deg ---")

print(f"\n--- sweeping to +42 deg at {RATE} deg/s ---")
print(f"{'step':>4} {'cmd':>7} {'arm':>7} {'item y':>8} {'vy':>7} {'|v|':>6}")
dt = 1.0 / 120.0
rows = []
for i in range(220):
    done = plow.step_toward(42.0, dt, rate=RATE)
    await app_utils.update_app_async(steps=1)
    p = cell.pose(ITEM)
    vx, vy, sp = vel()
    rows.append((plow.commanded, plow.angle, float(p[1]), vy, sp))
    if i % 12 == 0 or (done and i % 4 == 0):
        print(f"{i:4d} {plow.commanded:7.1f} {plow.angle:7.1f} {float(p[1]):8.3f} "
              f"{vy:7.2f} {sp:6.2f}")
    if done and i > 60:
        break

arm_moved = max(abs(r[1]) for r in rows)
peak_vy = max(abs(r[3]) for r in rows)
y_gain = max(r[2] for r in rows) - float(p0[1])
tip_speed = C.PLOW_ARM_LEN * RATE * 3.14159 / 180.0

print(f"\n--- verdict ---")
print(f"  arm reached           {arm_moved:.1f} deg      (commanded 42)")
print(f"  blade tip speed       {tip_speed:.2f} m/s")
print(f"  item peak |vy|        {peak_vy:.2f} m/s")
print(f"  item lateral travel   {y_gain:+.3f} m")
if arm_moved < 5:
    print("  => PhysX never saw the rotation: the blade is a ghost")
elif peak_vy < 0.15 * tip_speed:
    print("  => TELEPORT, not a sweep: the arm arrives without momentum and the item only")
    print("     gets a depenetration nudge. Rate tuning cannot fix this.")
else:
    print("  => real push: the item takes up a fair share of the blade's tip speed")
tl.stop()