"""Through-beams across each lane entry: did the item actually get onto its lane, and at
what blade angle and sweep rate.

The delivery number alone cannot tune the plow. An item that ends on the floor and one that
never left the belt both score zero, but they need opposite corrections - the first was
pushed too hard, the second not hard enough. A beam at the lane entry separates them: it
fires the moment the item crosses onto the lane, so a run yields, per item,

    crossed  yes/no          - did the push reach the lane at all
    angle    deg             - where the blade was at the crossing
    rate     deg/s           - how fast it was sweeping at that instant
    speed    m/s             - how fast the item was going as it crossed

which is what the sweep rate is tuned against. A rate that crosses every item but at high
speed is throwing them; one that crosses none is too slow.

The beams are real `raycast_closest` queries, like the gate before the pusher, placed
**along the lane entry line** rather than across the belt - the item is travelling sideways
here, so the beam has to lie along the direction it is leaving.
"""
from __future__ import annotations

from .. import config as C

# Beams sit just inside each lane entry, spanning the lane's width in X, so anything pushed
# across breaks one. Y is the entry edge after scripts/move_lanes_inboard.py.
# Beams sit ON each lane, not at its entry line, so a break means "this item is riding the
# lane" rather than "this item touched the boundary". Each is an origin + direction + length,
# because lane C is laid at 45 deg and cannot be described by a y value the way B can.
#
#   lane B  perpendicular, x -7.03..-6.57, y -2.38..-0.38  -> beam across it at y = -0.80
#   lane C  45 deg, near edge y = x + 7.637                -> beam across it at y = +0.90,
#           where the lane occupies roughly x -7.6..-6.7
BEAMS = {
    # lane C: straight run, belt x[-10.00,-8.00] y[-0.45,0.00]; beam across it at x = -8.60
    "lane_C": dict(o=(-8.60, -0.50, C.BELT_Z + 0.03), d=(0.0, 1.0, 0.0), L=0.55),
    # lane B: 45 deg band from (-7.84,0.16) to (-9.25,1.57); beam across it 0.7 m in,
    # so its direction is the lane's perpendicular (0.707, 0.707), not a world axis.
    "lane_B": dict(o=(-8.53, 0.46, C.BELT_Z + 0.03), d=(0.7071, 0.7071, 0.0), L=0.55),
}
ITEMS_PREFIX = "/World/Items/"


class LaneBeams:
    """crossing detector at each lane entry"""

    def __init__(self, stage, cell, plow=None):
        self.stage = stage
        self.cell = cell
        self.plow = plow
        self.crossings: dict[str, dict] = {}     # item -> first crossing record
        self._t = 0.0
        from omni.physx import get_physx_scene_query_interface
        self._q = get_physx_scene_query_interface()

    def tick(self, dt):
        self._t += dt

    def _hit(self, b):
        """name of whatever breaks this beam, else None"""
        h = self._q.raycast_closest(list(b["o"]), list(b["d"]), b["L"])
        if not h or not h.get("hit"):
            return None
        path = str(h.get("rigidBody") or h.get("collision") or "")
        if not path.startswith(ITEMS_PREFIX):
            return None
        return path[len(ITEMS_PREFIX):].split("/")[0] or None

    def poll(self, rate=None):
        """call each physics step; records the first crossing of each item"""
        for lane, b in BEAMS.items():
            name = self._hit(b)
            if name is None or name in self.crossings:
                continue
            try:
                v = self.cell._rp[name].get_velocities()[0].numpy()[0]
                speed = float((v[0] ** 2 + v[1] ** 2 + v[2] ** 2) ** 0.5)
            except Exception:
                speed = 0.0
            self.crossings[name] = dict(
                item=name, lane=lane, t=round(self._t, 3),
                angle=round(self.plow.angle, 1) if self.plow else None,
                commanded=round(self.plow.commanded, 1) if self.plow else None,
                rate=None if rate is None else round(rate, 1),
                speed=round(speed, 2))

    def crossed(self, name):
        return name in self.crossings

    def report(self):
        return list(self.crossings.values())