"""Prevent a reconstructed trunk/face from being admitted between ground rays."""

import importlib.util
from pathlib import Path

import numpy as np

SPEC = importlib.util.spec_from_file_location(
    "spawn_clearance", Path(__file__).parents[1] / "simulation/ai-polygon/spawn_clearance.py"
)
MODULE = importlib.util.module_from_spec(SPEC)
SPEC.loader.exec_module(MODULE)


def count(points, xy=(0, 0), heading=0, plane=(0, 0, 0)):
    return MODULE.obstructing_triangles(np.array(points), np.array([[0, 1, 2]]), xy, heading, plane)


def test_narrow_trunk_between_support_rays_and_crossing_triangle():
    assert count([[0.21, 0.21, 0], [0.23, 0.21, 0], [0.21, 0.21, 2]]) == 1
    # All vertices outside the prism, but the face crosses through its centre.
    assert count([[-2, 0, 0.3], [2, 0, 0.3], [0, 2, 0.3]]) == 1


def test_ground_step_and_overhead_geometry_do_not_block_start():
    for z in (0, 0.05, 0.10, 1.5):
        assert count([[-2, -2, z], [2, -2, z], [0, 2, z]]) == 0
    assert count([[-2, -2, 0.12], [2, -2, 0.12], [0, 2, 0.12]]) == 1


def test_triangle_bounding_box_alone_does_not_reject_empty_corner():
    assert count([[0.4, 2, 0.5], [2, 0.4, 0.5], [2, 2, 0.5]]) == 0


def test_clearance_tracks_translation_heading_and_support_slope():
    points = np.array([[0.6, 0, 0.3], [0.7, 0, 0.3], [0.6, 0.03, 0.8]])
    assert count(points) == 0
    assert count(points, heading=45) == 1
    points[:, 2] += points[:, 0] * 0.2 + 4
    points[:, :2] += [10, -3]
    assert count(points, (10, -3), 45, (0.2, 0, 4)) == 1
    assert count([[8, -5, 3.6], [12, -5, 4.4], [10, -1, 4]], (10, -3), 45, (0.2, 0, 4)) == 0