from __future__ import annotations

import importlib.util
import json
import sys
from collections import defaultdict, deque
from pathlib import Path
from types import ModuleType

import numpy as np


def _module() -> ModuleType:
    path = (
        Path(__file__).parents[1]
        / "experiments"
        / "perception"
        / "fuse_e6_tracking_lidar.py"
    )
    spec = importlib.util.spec_from_file_location("e6_tracking_lidar", path)
    assert spec is not None and spec.loader is not None
    module = importlib.util.module_from_spec(spec)
    sys.modules[spec.name] = module
    spec.loader.exec_module(module)
    return module


def _profile_path() -> Path:
    return (
        Path(__file__).parents[1]
        / "experiments"
        / "perception"
        / "e6_tracking_lidar_profile.json"
    )


def test_profile_pins_e4_e5_and_tight_sync_gate() -> None:
    module = _module()

    profile, digest = module._read_profile(_profile_path())

    assert len(digest) == 64
    assert profile["inputs"]["tracking_result_id"].startswith("e5-tracking-")
    assert profile["inputs"]["semantic_result_id"].startswith("result-")
    assert profile["temporal"]["maximum_lidar_camera_delta_ms"] == 100.0
    assert profile["projection"]["occlusion_policy"] == (
        "nearest-depth-per-rounded-pixel"
    )


def test_profile_rejects_unsafe_sync_window(tmp_path: Path) -> None:
    module = _module()
    profile = json.loads(_profile_path().read_text(encoding="utf-8"))
    profile["temporal"]["maximum_lidar_camera_delta_ms"] = 500.0
    path = tmp_path / "profile.json"
    path.write_text(json.dumps(profile), encoding="utf-8")

    try:
        module._read_profile(path)
    except RuntimeError as exc:
        assert "contract" in str(exc)
    else:
        raise AssertionError("unsafe synchronization window was accepted")


def test_depth_buffer_keeps_nearest_point_per_rounded_pixel() -> None:
    module = _module()
    pixels = np.asarray([[10.2, 20.2], [10.4, 20.4], [12.0, 20.0]])
    depths = np.asarray([7.0, 3.0, 5.0])

    selected = module._visible_projection_indices(pixels, depths)

    assert selected.tolist() == [1, 2]


def test_depth_cluster_prefers_largest_contiguous_group() -> None:
    module = _module()
    depths = np.asarray([2.0, 2.1, 8.0, 8.1, 8.2])

    selected = module._depth_cluster(
        np.arange(5, dtype=np.int64),
        depths,
        minimum_gap_m=0.5,
        gap_fraction=0.06,
    )

    assert selected.tolist() == [2, 3, 4]


def test_box_candidates_apply_inner_roi() -> None:
    module = _module()
    pixels = np.asarray([[0.0, 0.0], [5.0, 5.0], [50.0, 50.0], [99.0, 99.0]])
    inset = {
        "horizontal_fraction": 0.05,
        "top_fraction": 0.04,
        "bottom_fraction": 0.02,
    }

    selected = module._box_candidates((0.0, 0.0, 100.0, 100.0), pixels, inset)

    assert selected.tolist() == [1, 2]


def test_vehicle_group_nms_suppresses_overlapping_car_truck_tracks() -> None:
    module = _module()
    tracks = [
        {
            "track_id": 1,
            "label": "car",
            "score": 0.9,
            "bbox_xyxy": [0.0, 0.0, 100.0, 100.0],
        },
        {
            "track_id": 2,
            "label": "truck",
            "score": 0.4,
            "bbox_xyxy": [2.0, 2.0, 102.0, 102.0],
        },
        {
            "track_id": 3,
            "label": "person",
            "score": 0.8,
            "bbox_xyxy": [2.0, 2.0, 102.0, 102.0],
        },
    ]

    selected = module._suppress_duplicate_vehicle_tracks(
        tracks,
        vehicle_labels={"car", "truck", "bus"},
        iou_threshold=0.55,
    )

    assert [item["track_id"] for item in selected] == [1, 3]


def test_spatial_cluster_separates_distant_same_depth_surfaces() -> None:
    module = _module()
    points = np.asarray(
        [[0.0, 0.0, 0.0], [0.2, 0.1, 0.1], [5.0, 0.0, 0.0], [5.2, 0.1, 0.0]]
    )

    selected = module._spatial_cluster(
        np.arange(4, dtype=np.int64),
        points,
        radius_m=0.5,
    )

    assert selected.size == 2
    assert set(selected.tolist()) in ({0, 1}, {2, 3})


def test_semantic_supported_track_produces_point_cuboid_and_distance() -> None:
    module = _module()
    profile = json.loads(_profile_path().read_text(encoding="utf-8"))
    semantic = np.zeros((600, 800), dtype=np.uint8)
    semantic[100:200, 100:200] = 1
    pixels = np.asarray([[120.0, 120.0], [130.0, 130.0], [140.0, 140.0]])
    depths = np.asarray([5.0, 5.1, 5.2])
    points = np.asarray(
        [[1.0, 2.0, 0.2], [1.2, 2.1, 0.8], [1.4, 2.2, 1.6]],
        dtype=np.float64,
    )
    tracks = [
        {
            "track_id": 7,
            "label": "person",
            "score": 0.9,
            "bbox_xyxy": [100.0, 100.0, 200.0, 200.0],
        }
    ]

    result = module._fuse_tracks(
        tracks=tracks,
        semantic_map=semantic,
        pixels_xy=pixels,
        depths_m=depths,
        projected_source_indices=np.arange(3, dtype=np.int64),
        points_map=points,
        points_lidar=points,
        profile=profile,
        distance_history=defaultdict(lambda: deque(maxlen=5)),
    )

    assert len(result) == 1
    assert result[0].cuboid is not None
    assert result[0].clustered_points == 3
    assert result[0].distance_median_m is not None
    assert result[0].distance_smoothed_m == result[0].distance_median_m


def test_semantic_mismatch_never_extrudes_rectangle_into_cuboid() -> None:
    module = _module()
    profile = json.loads(_profile_path().read_text(encoding="utf-8"))
    pixels = np.asarray([[120.0, 120.0], [130.0, 130.0], [140.0, 140.0]])
    points = np.asarray(
        [[1.0, 2.0, 0.2], [1.2, 2.1, 0.8], [1.4, 2.2, 1.6]],
        dtype=np.float64,
    )

    result = module._fuse_tracks(
        tracks=[
            {
                "track_id": 7,
                "label": "person",
                "score": 0.9,
                "bbox_xyxy": [100.0, 100.0, 200.0, 200.0],
            }
        ],
        semantic_map=np.full((600, 800), 4, dtype=np.uint8),
        pixels_xy=pixels,
        depths_m=np.asarray([5.0, 5.1, 5.2]),
        projected_source_indices=np.arange(3, dtype=np.int64),
        points_map=points,
        points_lidar=points,
        profile=profile,
        distance_history=defaultdict(lambda: deque(maxlen=5)),
    )

    assert result[0].cuboid is None
    assert result[0].status == "rejected-no-semantic-lidar-support"


def test_distance_innovation_rejects_background_jump_for_same_track() -> None:
    module = _module()
    profile = json.loads(_profile_path().read_text(encoding="utf-8"))
    semantic = np.zeros((600, 800), dtype=np.uint8)
    semantic[100:200, 100:200] = 1
    pixels = np.asarray([[120.0, 120.0], [130.0, 130.0], [140.0, 140.0]])
    tracks = [
        {
            "track_id": 7,
            "label": "person",
            "score": 0.9,
            "bbox_xyxy": [100.0, 100.0, 200.0, 200.0],
        }
    ]
    history = defaultdict(lambda: deque(maxlen=5))
    near = np.asarray(
        [[2.0, 0.0, 0.2], [2.1, 0.1, 0.8], [2.2, 0.2, 1.5]],
        dtype=np.float64,
    )
    far = near + np.asarray([6.0, 0.0, 0.0])

    first = module._fuse_tracks(
        tracks=tracks,
        semantic_map=semantic,
        pixels_xy=pixels,
        depths_m=np.asarray([2.0, 2.1, 2.2]),
        projected_source_indices=np.arange(3, dtype=np.int64),
        points_map=near,
        points_lidar=near,
        profile=profile,
        distance_history=history,
    )
    second = module._fuse_tracks(
        tracks=tracks,
        semantic_map=semantic,
        pixels_xy=pixels,
        depths_m=np.asarray([8.0, 8.1, 8.2]),
        projected_source_indices=np.arange(3, dtype=np.int64),
        points_map=far,
        points_lidar=far,
        profile=profile,
        distance_history=history,
    )

    assert first[0].cuboid is not None
    assert second[0].cuboid is None
    assert second[0].status == "rejected-distance-innovation"
