from __future__ import annotations

import numpy as np
import pytest

from k1link.perception.geometry_math import Kb4ProjectionProfile
from k1link.perception.spatial_evidence import project_metric_obstacles_to_camera


def _identity_profile() -> Kb4ProjectionProfile:
    transform = np.eye(4, dtype=np.float64)
    transform.setflags(write=False)
    return Kb4ProjectionProfile(
        width=800,
        height=600,
        intrinsic_fx_fy_cx_cy=(100.0, 100.0, 400.0, 300.0),
        distortion_kb4=(0.0, 0.0, 0.0, 0.0),
        t_camera_from_lidar=transform,
    )


def test_metric_obstacle_projection_uses_native_kb4_voxel_bounds() -> None:
    projections = project_metric_obstacles_to_camera(
        [
            {
                "component_id": "static-a",
                "cells": [{"x": -1, "y": -1, "z": 2}],
            },
            {
                "component_id": "baseline-b",
                "cells": [{"x": 1, "y": 1, "z": 2}],
            },
        ],
        position_map_xyz=(0.0, 0.0, 0.0),
        orientation_map_from_lidar_xyzw=(0.0, 0.0, 0.0, 1.0),
        profile=_identity_profile(),
        occupied_voxel_size_m=1.0,
        component_ids={"static-a"},
    )

    assert set(projections) == {"static-a"}
    projection = projections["static-a"]
    assert projection["projection"] == "factory-kb4-occupied-voxel-bounds"
    assert projection["authority"] == "visual-derived"
    assert projection["projected_cell_count"] == 1
    assert projection["nearest_depth_m"] == pytest.approx(2.0)
    left, top, right, bottom = projection["bbox_xyxy"]
    assert left < right == pytest.approx(400.0)
    assert top < bottom == pytest.approx(300.0)