"""Scene-authoring admission for a full rover footprint, never an AI map.

Ground-height samples can miss a narrow trunk between sample rays. Test mesh
triangles against the occupied prism using the separating-axis theorem, which
also catches a face crossing the body when all of its vertices lie outside.
"""

import math

import numpy as np


def obstructing_triangles(vertices, faces, xy, heading, plane, *, step=0.1, height=1.1):
    """Count triangles above qualified step height inside the 1 x 1 m start.

    ``plane`` is z = a*(x-xy[0]) + b*(y-xy[1]) + c, fitted to the start's support.
    This is a conservative preparation gate, not a claim of route traversability.
    It neither changes the collider nor supplies privileged geometry to inference.
    """
    triangles = np.asarray(vertices)[faces]
    center_xy = np.asarray(xy)
    selected = (triangles[:, :, :2].min(axis=1) <= center_xy + 0.71).all(axis=1) & (
        triangles[:, :, :2].max(axis=1) >= center_xy - 0.71
    ).all(axis=1)
    triangles = triangles[selected].astype(np.float64)
    if not len(triangles):
        return 0
    delta = triangles[:, :, :2] - center_xy
    angle = math.radians(heading)
    rotation = np.array([[math.cos(angle), -math.sin(angle)], [math.sin(angle), math.cos(angle)]])
    triangles[:, :, 2] -= delta @ np.asarray(plane[:2]) + plane[2]
    triangles[:, :, :2] = delta @ rotation
    low = step + 1e-4  # Same centimetre-scale capability boundary as navigation.
    half = np.array([0.5, 0.5, (height - low) / 2])
    triangles[:, :, 2] -= (height + low) / 2
    selected = (triangles.min(axis=1) <= half).all(axis=1) & (triangles.max(axis=1) >= -half).all(
        axis=1
    )
    triangles = triangles[selected]
    if not len(triangles):
        return 0
    edges = np.roll(triangles, -1, axis=1) - triangles
    axes = [np.cross(edges[:, 0], edges[:, 1])]
    for edge in range(3):
        for box_axis in np.eye(3):
            axes.append(np.cross(edges[:, edge], box_axis))
    overlaps = np.ones(len(triangles), dtype=bool)
    for axis in axes:
        projections = np.einsum("nvi,ni->nv", triangles, axis)
        radius = np.abs(axis) @ half
        overlaps &= (projections.min(axis=1) <= radius + 1e-10) & (
            projections.max(axis=1) >= -radius - 1e-10
        )
    return int(overlaps.sum())