from __future__ import annotations

import numpy as np
import pytest

from k1link.compute.l31_pointpillars_ravnoves import (
    L31PointPillarsRavnovesError,
    nearest_pose_indices,
    select_visual_frame_indices,
    sensor_frame_xyzi,
)


def test_nearest_pose_binding_prefers_earlier_pose_on_tie() -> None:
    indices, age_ms = nearest_pose_indices(
        np.asarray([10, 20, 31], dtype=np.int64) * 1_000_000,
        np.asarray([5, 15, 30], dtype=np.int64) * 1_000_000,
    )
    assert indices.tolist() == [0, 1, 2]
    assert age_ms.tolist() == [5.0, 5.0, 1.0]


def test_sensor_frame_xyzi_inverts_map_pose() -> None:
    points = np.asarray([[11.0, 20.0, 30.0], [10.0, 22.0, 30.0]])
    intensity = np.asarray([255, 0], dtype=np.uint8)
    result = sensor_frame_xyzi(
        points,
        intensity,
        position_map_xyz=np.asarray([10.0, 20.0, 30.0]),
        orientation_map_from_lidar_xyzw=np.asarray([0.0, 0.0, 0.0, 1.0]),
    )
    np.testing.assert_allclose(
        result,
        np.asarray([[1.0, 0.0, 0.0, 1.0], [0.0, 2.0, 0.0, 0.0]]),
    )


def test_visual_selection_prefers_vehicles_with_route_coverage() -> None:
    selected = select_visual_frame_indices(
        [0, 3, 1, 0, 0, 2],
        [5, 3, 9, 0, 4, 2],
        maximum_frames=3,
    )
    assert selected == (1, 2, 5)


def test_visual_selection_rejects_impossible_class_count() -> None:
    with pytest.raises(L31PointPillarsRavnovesError):
        select_visual_frame_indices([2], [1])