from __future__ import annotations

import numpy as np
import pytest

from k1link.compute.semantic_geometry_fusion import (
    CameraGeometryFusionProfile,
    SemanticGeometryFusionError,
    _geometry_clusters,
    _semantic_support,
)
from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import (
    ProjectedPointCloud,
)


def _object(*, observed_geometry: bool = False) -> dict[str, object]:
    value: dict[str, object] = {
        "source_track_id": 42,
        "track_id": 4200,
        "label": "car",
        "association_group": "vehicle",
        "score": 0.91,
        "bbox_xyxy": [10.0, 10.0, 90.0, 90.0],
        "cuboid_status": "rejected-fewer-than-8-clustered-points",
        "camera_motion_state": "static",
        "camera_motion_confidence": 0.8,
        "motion_state": "unknown",
        "motion_status": "e26-insufficient-independent-evidence",
    }
    if observed_geometry:
        value.update(
            {
                "observed_cuboid_center_map": [2.0, 0.0, 0.5],
                "observed_cuboid_half_size": [1.0, 0.5, 0.5],
                "observed_cuboid_quaternion_xyzw": [0.0, 0.0, 0.0, 1.0],
            }
        )
    return value


def _projection(point_count: int) -> ProjectedPointCloud:
    return ProjectedPointCloud(
        pixels_xy=np.column_stack(
            (
                np.linspace(20.0, 80.0, point_count),
                np.linspace(20.0, 80.0, point_count),
            )
        ).astype(np.float64),
        depths_m=np.linspace(2.0, 2.4, point_count).astype(np.float64),
        source_indices=np.arange(point_count, dtype=np.int64),
        source_point_count=point_count,
        camera_front_point_count=point_count,
    )


def test_camera_semantic_and_connected_occupied_support_agree() -> None:
    points = np.asarray(
        [
            [2.0, 0.0, 0.5],
            [2.2, 0.0, 0.6],
            [2.1, 0.2, 0.0],
            [2.3, 0.2, -0.1],
        ],
        dtype=np.float64,
    )
    support = _semantic_support(
        _object(),
        projected=_projection(4),
        frame_points_map=points,
        point_class=np.asarray([2, 2, 1, 3], dtype=np.uint8),
        point_height_m=np.asarray([0.5, 0.6, 0.0, -0.1], dtype=np.float32),
        source_available=True,
        surface_valid=True,
        profile=CameraGeometryFusionProfile(),
    )
    assert support.document["geometry_status"] == "agree"
    assert support.document["range_m"] == pytest.approx(2.0666666667)
    assert support.document["unknown_is_occupied"] is True
    assert support.document["navigation_or_safety_accepted"] is False
    assert support.occupied_source_indices.tolist() == [0, 1]


def test_camera_only_and_surface_conflict_remain_explicit() -> None:
    points = np.column_stack(
        (
            np.linspace(2.0, 3.0, 6),
            np.zeros(6),
            np.zeros(6),
        )
    ).astype(np.float64)
    camera_only = _semantic_support(
        _object(),
        projected=_projection(6),
        frame_points_map=points,
        point_class=np.asarray([2, 1, 1, 1, 1, 1], dtype=np.uint8),
        point_height_m=np.asarray([0.5, 0, 0, 0, 0, 0], dtype=np.float32),
        source_available=True,
        surface_valid=True,
        profile=CameraGeometryFusionProfile(),
    )
    conflict = _semantic_support(
        _object(observed_geometry=True),
        projected=_projection(6),
        frame_points_map=points,
        point_class=np.ones(6, dtype=np.uint8),
        point_height_m=np.zeros(6, dtype=np.float32),
        source_available=True,
        surface_valid=True,
        profile=CameraGeometryFusionProfile(),
    )
    unknown = _semantic_support(
        _object(),
        projected=None,
        frame_points_map=np.empty((0, 3), dtype=np.float64),
        point_class=np.empty(0, dtype=np.uint8),
        point_height_m=np.empty(0, dtype=np.float32),
        source_available=False,
        surface_valid=False,
        profile=CameraGeometryFusionProfile(),
    )
    assert camera_only.document["geometry_status"] == "single-source-camera"
    assert conflict.document["geometry_status"] == "conflict"
    assert unknown.document["geometry_status"] == "unknown"


def test_unclaimed_occupied_component_is_a_separate_geometry_layer() -> None:
    points = np.asarray(
        [
            [1.00, 0.00, 0.30],
            [1.20, 0.00, 0.35],
            [1.45, 0.00, 0.40],
            [1.65, 0.00, 0.45],
            [4.00, 0.00, 0.30],
            [4.20, 0.00, 0.35],
            [4.45, 0.00, 0.40],
            [4.65, 0.00, 0.45],
        ],
        dtype=np.float64,
    )
    clusters = _geometry_clusters(
        points_map=points,
        point_class=np.full(8, 2, dtype=np.uint8),
        point_height_m=np.linspace(0.3, 0.45, 8).astype(np.float32),
        sensor_position_map=np.zeros(3, dtype=np.float64),
        claimed_source_indices={0},
        profile=CameraGeometryFusionProfile(),
    )
    assert len(clusters) == 1
    assert clusters[0]["geometry_status"] == "single-source-geometry"
    assert clusters[0]["semantic_class"] is None
    assert clusters[0]["point_count"] == 4
    assert clusters[0]["unknown_is_occupied"] is True


def test_profile_rejects_free_space_shortcuts() -> None:
    with pytest.raises(SemanticGeometryFusionError):
        CameraGeometryFusionProfile(
            geometry_minimum_cluster_points=0,
        )