"""Пушер и плуг новой сцены ШТАТНЫМ механизмом проекта.

Прошлый прогон был поставлен неверно: я командовал силовым приводом шарнира, а проект от
него отказался. configure_plow(kinematic_arm=True) делает лезвие КИНЕМАТИЧЕСКИМ, выключает
сам шарнир (physics:jointEnabled=False) и пишет угол напрямую - в комментарии сказано, что
привод перенастраивали трижды и он не держал, звеня на +-21.4 градуса быстрее, чем его
успевала вести команда. Нож пушера так же не ездит по своему призматическому суставу:
сустав выключен, а нож переносится записью трансформа (mechanics.Cell.blade_to).
Поэтому здесь всё идёт через plow_cell.prepare() + Plow + Cell.

Сверх штатного добавлено то, чего код проекта про эту сборку не знает:
* узлы ConveyorBeltGraph УДАЛЯЮТСЯ - deactivate недостаточно, уже собранный граф
  продолжает обнулять surfaceVelocity на каждом тике;
* приводятся ConveyorTrack_05 и новая угловая ConveyorTrack_06: список лент в scene.py
  заканчивается на _05 и седьмой дорожки не содержит.

Время берётся из таймлайна: заданная частота физики не применяется, фактический шаг
83.33 мс, и на предположении о 120 Гц скорости выходили ровно вдвое завышенными.
"""
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.plow import Plow

SPEED = 1.0
SCENE = f"{REPO}/scene/plow_cell_90_45_test.usd"

tl = omni.timeline.get_timeline_interface()
if tl.is_playing():
    tl.stop(); await app_utils.update_app_async(steps=10)
omni.usd.get_context().open_stage(SCENE)
await app_utils.update_app_async(steps=60)
stage = omni.usd.get_context().get_stage()

killed = [p.GetPath() for p in stage.Traverse() if "ConveyorBeltGraph" in p.GetName()]
for path in killed:
    stage.RemovePrim(path)
await app_utils.update_app_async(steps=10)

info = plow_cell.prepare(stage, belt_speed=SPEED, script_control=True, kinematic_arm=True)
print(f"prepare: плуг готов={info['plow_ready']}, лент приведено={len(info['belts'])}, "
      f"скорость={info['belt_speed']}  (узлов графа удалено {len(killed)})")

# дорожки, которых нет в списке проекта
for path, intent in (("/World/ConveyorTrack_05/Belt", (-1, 0, 0)),
                     ("/World/ConveyorTrack_06/Belt", (0, 1, 0))):
    pr = stage.GetPrimAtPath(path)
    if pr.IsValid():
        v = plow_cell.drive_belt(stage, path, intent, SPEED)
        PhysxSchema.PhysxSurfaceVelocityAPI(pr).CreateSurfaceVelocityEnabledAttr().Set(True)
        print(f"  дополнительно приведена {path.split('/World/')[-1]}: v={v}")
for path in plow_cell.BELTS + [plow_cell.BRANCH]:
    pr = stage.GetPrimAtPath(path)
    if pr.IsValid():
        PhysxSchema.PhysxSurfaceVelocityAPI(pr).CreateSurfaceVelocityEnabledAttr().Set(True)

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

def spawn(name, x, y, size=0.05, mass=0.5):
    path = f"/World/_Goods/{name}"
    c = UsdGeom.Cube.Define(stage, path); c.CreateSizeAttr().Set(2.0)
    xf = UsdGeom.Xformable(c.GetPrim())
    xf.AddTranslateOp().Set(Gf.Vec3d(x, y, TOP + size + 0.005))
    xf.AddScaleOp().Set(Gf.Vec3f(size, size, size))
    p = c.GetPrim()
    UsdPhysics.RigidBodyAPI.Apply(p); UsdPhysics.CollisionAPI.Apply(p)
    UsdPhysics.MassAPI.Apply(p).CreateMassAttr().Set(mass)
    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")
    return path

# ---------- ПЛУГ: угол по классу, заранее ----------------------------------------------
plow = Plow(stage)
print(f"\nПЛУГ. углы по классам {C.PLOW_PRESET}, широкий B={C.PLOW_B_ANGLE}, "
      f"ось плуга X={C.PLOW_X}, кинематическое лезвие")
print(f"  {'класс':10s} {'цель°':>6s} {'угол лезвия°':>13s} {'смещ.Y,мм':>10s} "
      f"{'скольж.,мм':>11s} {'конец X,Y':>16s}  вывод")
print("  " + "-" * 92)
out = {}
for label, deg in (("D", C.PLOW_PRESET["D"]), ("B", C.PLOW_PRESET["B"]),
                   ("C", C.PLOW_PRESET["C"]), ("B широкий", C.PLOW_B_ANGLE)):
    if stage.GetPrimAtPath("/World/_Goods").IsValid():
        stage.RemovePrim("/World/_Goods")
    stage.DefinePrim("/World/_Goods", "Xform")
    gp = spawn("item", -6.30, 0.0)
    plow.target(deg)                                   # ЗАРАНЕЕ, до подхода товара
    tl.play()
    await app_utils.update_app_async(steps=30)
    reached = plow.angle
    rp = RigidPrim(paths=[gp])
    P, T = [], []
    t_end = float(tl.get_current_time()) + 4.0
    while float(tl.get_current_time()) < t_end:
        P.append(rp.get_world_poses()[0].numpy()[0].copy())
        T.append(float(tl.get_current_time()))
        await app_utils.update_app_async(steps=3)
    tl.stop(); await app_utils.update_app_async(steps=5)
    y0, dy = float(P[0][1]), float(P[-1][1]) - float(P[0][1])
    a = math.radians(reached)
    ex, ey = math.cos(a), math.sin(a)
    slide, prev = 0.0, None
    for p in P:
        if C.PLOW_SWEEP_X1 >= float(p[0]) >= C.PLOW_SWEEP_X0:
            if prev is not None:
                slide += abs((float(p[0])-prev[0])*ex + (float(p[1])-prev[1])*ey) * 1000
            prev = (float(p[0]), float(p[1]))
    side = "ушёл в +Y" if dy > 0.05 else ("ушёл в -Y" if dy < -0.05 else "прошёл прямо")
    print(f"  {label:10s} {deg:6.1f} {reached:13.1f} {dy*1000:10.0f} {slide:11.0f} "
          f"({float(P[-1][0]):+6.2f},{float(P[-1][1]):+6.2f})  {side}")
    out[label] = dict(target=deg, reached=round(reached, 1), dy_mm=round(dy*1000),
                      slide_mm=round(slide), end=[round(float(P[-1][0]), 2),
                                                  round(float(P[-1][1]), 2)])

# ---------- ПУШЕР ------------------------------------------------------------------------
print(f"\nПУШЕР. ход {C.BLADE_HOME_Y} -> {C.BLADE_OUT_Y} ({C.BLADE_STROKE*1000:.0f} мм), "
      f"срабатывание у PUSH_X={C.PUSH_X}")
from robozon_sorter.sim.mechanics import Cell
if stage.GetPrimAtPath("/World/_Goods").IsValid():
    stage.RemovePrim("/World/_Goods")
stage.DefinePrim("/World/_Goods", "Xform")
gp = spawn("push_D", -2.80, 0.0)
cell = Cell(stage, items={})
plow.target(C.PLOW_PRESET["D"])
tl.play(); await app_utils.update_app_async(steps=25)
rp = RigidPrim(paths=[gp])
P, fired, stroke_s = [], False, None
t_end = float(tl.get_current_time()) + 6.0
while float(tl.get_current_time()) < t_end:
    p = rp.get_world_poses()[0].numpy()[0]
    P.append(p.copy())
    if not fired and float(p[0]) <= C.PUSH_X + 0.08:
        print(f"   товар дошёл до x={float(p[0]):+.2f} - ход ножа")
        stroke_s = await cell.stroke(app_utils, out=True)
        fired = True
    await app_utils.update_app_async(steps=3)
tl.stop(); await app_utils.update_app_async(steps=5)
dy = float(P[-1][1]) - float(P[0][1])
print(f"   ход ножа занял {stroke_s if stroke_s 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}), по Y {dy*1000:+.0f} мм")
print(f"   {'ТОВАР УВЕДЁН НА ВЕТКУ' if dy > 0.15 else 'товар НЕ уведён на ветку'}")
out["pusher"] = dict(dy_mm=round(dy*1000), fired=fired)
globals()["PLOW2"] = out