from guardrail_space.collision_oracle import check_pair_conflict, aggregate_check


def _make_line_plan(pid: str, start: float, x0: float, y0: float, x1: float, y1: float) -> dict:
    # simple two-point trajectory from start time to start+1.0
    return {"id": pid, "trajectory": [(start, x0, y0), (start + 1.0, x1, y1)]}


def test_direct_collision_detected():
    a = _make_line_plan("A", 0.0, 0.0, 0.0, 1.0, 0.0)
    b = _make_line_plan("B", 0.0, 0.0, 0.0, -1.0, 0.0)
    res = check_pair_conflict(a, b, distance_threshold=0.1)
    assert res["verdict"] == "conflict"
    assert len(res["conflicts"]) >= 1


def test_spatially_separated_are_compatible():
    a = _make_line_plan("A", 0.0, 0.0, 0.0, 1.0, 0.0)
    b = _make_line_plan("B", 0.0, 10.0, 0.0, 11.0, 0.0)
    res = check_pair_conflict(a, b, distance_threshold=0.5)
    assert res["verdict"] == "compatible"


def test_aggregate_pairwise():
    p1 = _make_line_plan("p1", 0.0, 0.0, 0.0, 1.0, 0.0)
    p2 = _make_line_plan("p2", 0.0, 0.0, 0.0, -1.0, 0.0)
    p3 = _make_line_plan("p3", 0.0, 10.0, 0.0, 11.0, 0.0)
    out = aggregate_check([p1, p2, p3], distance_threshold=0.2)
    # p1 vs p2 conflict, others compatible
    assert out[("p1", "p2")]["verdict"] == "conflict"
    assert out[("p1", "p3")]["verdict"] == "compatible"
    assert out[("p2", "p3")]["verdict"] == "compatible"