#!/usr/bin/env python3
"""Drop the plow so its blade sweeps at belt level instead of over the goods.

    /home/whatevenif/isaacsim/python.sh scripts/lower_plow_to_belt.py

Measured on the built scene:

    blade   z 1.810 .. 1.890
    belt    top z 1.781
    gap under the blade = 29 mm

29 mm is enough for flat goods to pass straight under the blade, and tall ones get caught
near their top edge and tipped rather than led across. It is the reason items reached only
y ~ 0.24 while the arm was correctly holding 42 deg with 402 mm of reach: the blade was not
touching them at all, so no amount of angle or lane geometry could have helped.

The whole ``DiverterEnd`` is moved, not just the arm: base and arm keep their relative
placement, so the hinge stays consistent whether the arm is driven kinematically or by its
joint. The pedestal sinks the same 27 mm, which is invisible - it stands on the floor.

Target clearance is 2 mm: enough that the blade is not grinding on the belt collider,
little enough that nothing rides under it.
"""
from __future__ import annotations

import sys
from pathlib import Path

from pxr import Gf, Usd, UsdGeom

SCENE = Path(__file__).resolve().parent.parent / "scene" / "plow_cell.usd"
DIVERTER = "/World/Diverters/DiverterEnd"
ARM = DIVERTER + "/Arm"
BELT = "/World/ConveyorTrack_04/Belt"
CLEARANCE = 0.002


def _range(stage, path):
    return UsdGeom.BBoxCache(0, ["default"]).ComputeWorldBound(
        stage.GetPrimAtPath(path)).ComputeAlignedRange()


def main():
    if not SCENE.exists():
        sys.exit(f"{SCENE} not found")
    stage = Usd.Stage.Open(str(SCENE))

    arm = _range(stage, ARM)
    belt = _range(stage, BELT)
    if arm.IsEmpty() or belt.IsEmpty():
        sys.exit("arm or belt has no bounds - wrong scene?")

    blade_bottom, belt_top = arm.GetMin()[2], belt.GetMax()[2]
    gap = blade_bottom - belt_top
    drop = gap - CLEARANCE
    print(f"blade bottom z {blade_bottom:.4f} | belt top z {belt_top:.4f}")
    print(f"gap {gap * 1000:.0f} mm -> target {CLEARANCE * 1000:.0f} mm, dropping {drop * 1000:.0f} mm")

    if abs(drop) < 1e-4:
        print("already at height, nothing to do")
        return

    prim = stage.GetPrimAtPath(DIVERTER)
    xf = UsdGeom.Xformable(prim)
    for op in xf.GetOrderedXformOps():
        if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
            t = op.Get()
            op.Set(Gf.Vec3d(t[0], t[1], t[2] - drop))
            break
    else:
        sys.exit(f"{DIVERTER} has no translate op to move")

    stage.GetRootLayer().Save()
    after = _range(stage, ARM)
    print(f"blade now z[{after.GetMin()[2]:.4f}, {after.GetMax()[2]:.4f}] "
          f"-> clearance {(after.GetMin()[2] - belt_top) * 1000:.0f} mm")
    print(f"saved {SCENE}")


if __name__ == "__main__":
    main()