#!/usr/bin/env python3
"""Turn the plow round: pivot upstream, free end downstream at the discharge edge.

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

Measured on the built scene, with goods travelling in **-X**:

    pivot      x = -7.050            <- DOWNSTREAM end
    free tip   x = -6.658 (at 42 deg) <- UPSTREAM end, y +0.353

That is a plough mounted backwards. A plough is an inclined plane: the belt drives the item
along the blade toward the blade's **downstream** end, and the item leaves there. With the
downstream end sitting at the pivot on the belt centreline (y = 0), goods are funnelled
*inward*, slip past the pivot and carry on down the line. They can never discharge.

It explains every symptom: the 0.39 m ceiling is the brief shove from the sweep, after
which the item slides back toward the centre; moving the lanes inboard changed nothing
because the lane edge was never what goods were failing to reach; and goods pile in the
wedge between the blade and the belt centre, which is what the viewport shows.

The fix is the mounting, not the length:

    pivot  x -7.050 -> -6.522   (upstream end of the same physical span)
    arm    extends -X instead of +X (rotateZ 180 -> 0)

so at 42 deg the free end lands near x -6.92, y +-0.353 - downstream of the pivot and out
at the discharge side. Goods now slide *outward and forward* along the blade.

**The swing sign flips with the mount.** `plow_sort.calibrate_mapping()` says to measure it
rather than reason about it; re-measure after running this.
"""
from __future__ import annotations

import math
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"


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

    xc = UsdGeom.XformCache()
    piv = xc.GetLocalToWorldTransform(stage.GetPrimAtPath(ARM)).ExtractTranslation()
    r = UsdGeom.BBoxCache(0, ["default"]).ComputeWorldBound(
        stage.GetPrimAtPath(ARM)).ComputeAlignedRange()
    reach = r.GetMax()[0] - piv[0]              # +0.528: arm points upstream today
    print(f"before: pivot x={piv[0]:.3f}, arm reaches {reach:+.3f} m in X "
          f"({'UPSTREAM - wrong way' if reach > 0 else 'downstream'})")

    xf = UsdGeom.Xformable(prim)
    moved = flipped = False
    for op in xf.GetOrderedXformOps():
        if op.GetOpType() == UsdGeom.XformOp.TypeTranslate and not moved:
            t = op.Get()
            op.Set(Gf.Vec3d(t[0] + reach, t[1], t[2]))   # pivot to the upstream end
            moved = True
        elif op.GetOpType() == UsdGeom.XformOp.TypeRotateZ and not flipped:
            op.Set(float((op.Get() or 0.0) + 180.0) % 360.0)   # arm now points downstream
            flipped = True
    if not (moved and flipped):
        sys.exit(f"{DIVERTER} needs both a translate and a rotateZ op "
                 f"(moved={moved}, flipped={flipped})")

    stage.GetRootLayer().Save()

    xc2 = UsdGeom.XformCache()
    piv2 = xc2.GetLocalToWorldTransform(stage.GetPrimAtPath(ARM)).ExtractTranslation()
    r2 = UsdGeom.BBoxCache(0, ["default"]).ComputeWorldBound(
        stage.GetPrimAtPath(ARM)).ComputeAlignedRange()
    L = abs(reach)
    print(f"after : pivot x={piv2[0]:.3f}, arm spans x[{r2.GetMin()[0]:.3f}, "
          f"{r2.GetMax()[0]:.3f}]")
    for a in (0, 20, 42):
        print(f"   {a:2d} deg: free end x={piv2[0] - L * math.cos(math.radians(a)):+.3f} "
              f"y={L * math.sin(math.radians(a)):+.3f}  (downstream of pivot = correct)")
    print(f"saved {SCENE}")
    print("NOTE: the swing sign flips with the mount - re-measure calibrate_mapping()")


if __name__ == "__main__":
    main()