from __future__ import annotations

import json
from pathlib import Path
from types import SimpleNamespace

import numpy as np
import pytest
from fastapi import APIRouter
from fastapi.routing import APIRoute

from k1link.compute import (
    DEFAULT_GROUND_BENCHMARK_PROFILE,
    K1_LIDAR_PACK_V2_PROFILE,
    GroundBenchmarkProfile,
    GroundSegmentation,
    LidarGroundBenchmarkV1,
    LidarGroundError,
    LidarReplayPointFrame,
    LidarReplayPoseFrame,
    LocalPercentileGroundSegmenter,
    PatchworkPPGroundSegmenter,
    build_lidar_ground_annotation_template,
    build_lidar_ground_benchmark,
    lidar_ground_frame_detail,
    score_ground_labels,
)
from k1link.web.lidar_api import build_lidar_router


class _Replay:
    def __init__(self) -> None:
        self.pack_id = f"lidar-replay-pack-{'a' * 64}"
        self.identity = {
            "logical_content_sha256": "b" * 64,
            "session_id": "synthetic-ground-session",
        }
        self.profile = K1_LIDAR_PACK_V2_PROFILE
        self._points = (
            np.asarray(
                [
                    [-1.0, -1.0, 0.00],
                    [-0.5, -1.0, 0.02],
                    [0.0, -1.0, 0.01],
                    [0.5, -1.0, 0.03],
                    [1.0, -1.0, 0.00],
                    [-1.0, 0.0, 0.01],
                    [-0.5, 0.0, 0.02],
                    [0.0, 0.0, 0.04],
                    [0.5, 0.0, 0.35],
                    [1.0, 0.0, 0.70],
                ],
                dtype=np.float64,
            ),
            np.asarray(
                [
                    [-1.0, -1.0, 0.01],
                    [-0.5, -1.0, 0.01],
                    [0.0, -1.0, 0.02],
                    [0.5, -1.0, 0.02],
                    [1.0, -1.0, 0.01],
                    [-1.0, 0.0, 0.02],
                    [-0.5, 0.0, 0.01],
                    [0.0, 0.0, 0.03],
                    [0.5, 0.0, 0.40],
                    [1.0, 0.0, 0.80],
                ],
                dtype=np.float64,
            ),
        )
        self.arrays = {
            "point_offsets": np.asarray([0, 10, 20], dtype="<i8"),
            "point_capture_sequence": np.asarray([1, 3], dtype="<i8"),
            "pose_received_monotonic_ns": np.asarray(
                [1_001_000_000, 1_101_000_000],
                dtype="<i8",
            ),
        }

    @property
    def point_frame_count(self) -> int:
        return 2

    @property
    def pose_frame_count(self) -> int:
        return 2

    @property
    def point_count(self) -> int:
        return 20

    def point_frame(self, index: int) -> LidarReplayPointFrame:
        xyz = self._points[index]
        return LidarReplayPointFrame(
            capture_sequence=1 + index * 2,
            payload_bytes=100,
            received_at_epoch_ns=2_000_000_000 + index * 100_000_000,
            received_monotonic_ns=1_000_000_000 + index * 100_000_000,
            header_seq=10 + index,
            header_stamp=100 + index,
            scaler=1000,
            raw_xyz=(xyz * 1000).astype(np.int64),
            xyz_map=xyz,
            rgbi=np.full(10, 0xFFFFFF80, dtype=np.uint32),
            intensity=np.full(10, 128, dtype=np.uint8),
        )

    def pose_frame(self, index: int) -> LidarReplayPoseFrame:
        return LidarReplayPoseFrame(
            capture_sequence=2 + index * 2,
            payload_bytes=80,
            received_at_epoch_ns=2_001_000_000 + index * 100_000_000,
            received_monotonic_ns=1_001_000_000 + index * 100_000_000,
            header_seq=20 + index,
            header_stamp=200 + index,
            header_scaler=1000,
            pose_stamp=201 + index,
            position_map=(0.0, 0.0, 0.0),
            orientation_map_from_lidar=(0.0, 0.0, 0.0, 1.0),
            distance=0.0,
            pose_accuracy=0.001,
        )


class _Candidate:
    @property
    def identity(self) -> dict[str, object]:
        return {
            "provider_id": "test-patchwork/v1",
            "binary_sha256": "c" * 64,
        }

    def segment(self, xyzi: np.ndarray) -> GroundSegmentation:
        ground = xyzi[:, 2] <= 0.025
        assigned = np.ones(xyzi.shape[0], dtype=np.bool_)
        return GroundSegmentation(ground, assigned, 0.5)


def _endpoint(router: APIRouter, path: str) -> object:
    for route in router.routes:
        if isinstance(route, APIRoute) and route.path == path and "GET" in route.methods:
            return route.endpoint
    raise AssertionError(f"GET {path} route is missing")


def test_local_percentile_ground_is_point_aligned_and_non_mutating() -> None:
    replay = _Replay()
    xyz = replay._points[0]
    xyzi = np.column_stack((xyz, np.ones(xyz.shape[0]))).astype(np.float32)
    unchanged = xyzi.copy()

    result = LocalPercentileGroundSegmenter(profile=DEFAULT_GROUND_BENCHMARK_PROFILE).segment(xyzi)

    assert result.ground_mask.shape == (10,)
    assert result.assigned_mask.all()
    assert 0 < np.count_nonzero(result.ground_mask) < 10
    np.testing.assert_array_equal(xyzi, unchanged)


def test_local_percentile_grid_index_preserves_exact_radius_result() -> None:
    random = np.random.default_rng(42)
    xyzi = random.normal(size=(240, 4)).astype(np.float32)
    xyzi[:, :2] *= 4
    profile = GroundBenchmarkProfile(
        current_cell_size_m=0.6,
        current_local_radius_m=1.7,
        current_lower_percentile=11,
        current_maximum_below_ground_m=0.2,
        current_maximum_above_ground_m=0.18,
        current_minimum_local_points=5,
    )

    actual = LocalPercentileGroundSegmenter(profile=profile).segment(xyzi)

    xyz = xyzi[:, :3].astype(np.float64)
    cell_keys = np.floor(xyz[:, :2] / profile.current_cell_size_m).astype(np.int64)
    unique_cells, inverse = np.unique(cell_keys, axis=0, return_inverse=True)
    expected = np.zeros(xyzi.shape[0], dtype=np.bool_)
    global_ground_z = float(np.percentile(xyz[:, 2], profile.current_lower_percentile))
    for cell_index in range(unique_cells.shape[0]):
        point_indices = np.flatnonzero(inverse == cell_index)
        center_xy = np.median(xyz[point_indices, :2], axis=0)
        delta_xy = xyz[:, :2] - center_xy
        local = xyz[
            np.einsum("ij,ij->i", delta_xy, delta_xy)
            <= profile.current_local_radius_m**2,
            2,
        ]
        ground_z = (
            float(np.percentile(local, profile.current_lower_percentile))
            if local.size >= profile.current_minimum_local_points
            else global_ground_z
        )
        z = xyz[point_indices, 2]
        expected[point_indices] = (
            z >= ground_z - profile.current_maximum_below_ground_m
        ) & (z <= ground_z + profile.current_maximum_above_ground_m)

    np.testing.assert_array_equal(actual.ground_mask, expected)


def test_ground_benchmark_is_immutable_diagnostic_evidence(tmp_path: Path) -> None:
    replay = _Replay()
    output = build_lidar_ground_benchmark(
        replay,  # type: ignore[arg-type]
        tmp_path / "benchmarks",
        patchwork=_Candidate(),
    )
    result = LidarGroundBenchmarkV1(output)
    try:
        assert result.report["status"] == "diagnostic-only"
        assert result.report["input_domain"]["accepted"] is False
        assert result.report["labels"]["metrics_available"] is False
        assert result.report["decision"]["status"] == "do-not-promote-on-current-vendor-map"
        assert result.arrays["current_ground"].shape == (20,)
        assert result.arrays["candidate_ground"].shape == (20,)
    finally:
        result.close()

    assert (
        build_lidar_ground_benchmark(
            replay,  # type: ignore[arg-type]
            tmp_path / "benchmarks",
            patchwork=_Candidate(),
        )
        == output
    )

    manifest_path = output / "manifest.json"
    manifest = json.loads(manifest_path.read_text(encoding="utf-8"))
    manifest["identity"]["points"] = 21
    manifest_path.write_text(json.dumps(manifest), encoding="utf-8")
    with pytest.raises(LidarGroundError, match="identity"):
        LidarGroundBenchmarkV1(output)


def test_operator_height_correction_is_explicit_and_stays_diagnostic(
    tmp_path: Path,
) -> None:
    replay = _Replay()
    observed_z: list[np.ndarray] = []

    class RecordingCandidate(_Candidate):
        def segment(self, xyzi: np.ndarray) -> GroundSegmentation:
            observed_z.append(xyzi[:, 2].copy())
            return super().segment(xyzi)

    profile = GroundBenchmarkProfile(
        profile_id="k1-handheld-operator-height-ground-ab/v1",
        patchwork_sensor_height_proxy_m=1.27,
        patchwork_map_vertical_origin_offset_m=1.27,
        patchwork_height_evidence="operator-estimated",
    )
    output = build_lidar_ground_benchmark(
        replay,  # type: ignore[arg-type]
        tmp_path / "benchmarks",
        patchwork=RecordingCandidate(),
        profile=profile,
    )
    result = LidarGroundBenchmarkV1(output)
    try:
        normalization = result.report["input_domain"]["normalization"]
        assert normalization == {
            "height_evidence": "operator-estimated",
            "map_vertical_origin_offset_m": 1.27,
            "sensor_height_m": 1.27,
        }
        assert result.report["input_domain"]["physical_sensor_height_known"] is False
        frame = lidar_ground_frame_detail(
            result,
            replay,  # type: ignore[arg-type]
            0,
        )
    finally:
        result.close()

    np.testing.assert_allclose(
        observed_z[0],
        replay._points[0][:, 2] - 1.27,
        atol=1e-6,
    )
    assert frame["schema_version"] == "missioncore.lidar-ground-frame/v1"
    assert frame["point_count"] == 10
    assert frame["coordinate_frame"] == "map"
    assert frame["ground_truth"] is False
    assert frame["masks"]["disagreement"]
    assert str(tmp_path) not in repr(frame)


def test_ground_profile_rejects_unattested_height_configuration() -> None:
    with pytest.raises(LidarGroundError, match="without height evidence"):
        GroundBenchmarkProfile(patchwork_sensor_height_proxy_m=1.27)
    with pytest.raises(LidarGroundError, match="positive sensor height"):
        GroundBenchmarkProfile(patchwork_height_evidence="operator-estimated")


def test_annotation_template_starts_all_ignore_and_never_ground_truth(
    tmp_path: Path,
) -> None:
    replay = _Replay()
    output = build_lidar_ground_annotation_template(
        replay,  # type: ignore[arg-type]
        tmp_path / "annotations",
        requested_frames=2,
    )
    manifest = json.loads((output / "manifest.json").read_text(encoding="utf-8"))
    arrays = np.load(output / "labels-template.npz", allow_pickle=False)
    try:
        assert manifest["ground_truth"] is False
        assert manifest["identity"]["review_status"] == "unreviewed"
        assert arrays["labels"].shape == (20,)
        assert not np.any(arrays["labels"])
    finally:
        arrays.close()


def test_ground_api_is_read_only_path_free_and_pack_filtered(
    tmp_path: Path,
) -> None:
    replay = _Replay()
    root = tmp_path / "benchmarks"
    output = build_lidar_ground_benchmark(
        replay,  # type: ignore[arg-type]
        root,
        patchwork=_Candidate(),
    )
    router = build_lidar_router(
        root_provider=lambda: None,
        ground_root_provider=lambda: root,
    )
    catalog_route = _endpoint(router, "/api/v1/lidar/ground-benchmarks")
    detail_route = _endpoint(
        router,
        "/api/v1/lidar/ground-benchmarks/{benchmark_id}",
    )
    _endpoint(
        router,
        "/api/v1/lidar/ground-benchmarks/{benchmark_id}/frames/{frame_index}",
    )

    catalog = catalog_route(  # type: ignore[operator]
        pack_id=replay.pack_id,
        limit=20,
    )
    detail = detail_route(benchmark_id=output.name)  # type: ignore[operator]

    assert catalog["schema_version"] == "missioncore.lidar-ground-benchmark-catalog/v1"
    assert catalog["valid_total"] == 1
    assert catalog["items"][0]["decision"]["production_promotion"] is False
    assert detail["benchmark"]["benchmark_id"] == output.name
    assert detail["access"] == "read-only"
    assert str(tmp_path) not in repr({"catalog": catalog, "detail": detail})


def test_reviewed_ground_metrics_keep_ignore_out_of_denominators() -> None:
    prediction = GroundSegmentation(
        ground_mask=np.asarray([True, True, False, False, False, False]),
        assigned_mask=np.asarray([True, True, True, True, False, True]),
        latency_ms=1.0,
    )
    labels = np.asarray([1, 2, 2, 3, 4, 0], dtype=np.uint8)

    metrics = score_ground_labels(prediction, labels)

    assert metrics["reviewed_points"] == 5
    assert metrics["ground_iou"] == pytest.approx(0.5)
    assert metrics["curb_recall"] == pytest.approx(0.5)
    assert metrics["low_obstacle_recall"] == pytest.approx(1.0)
    assert metrics["reflection_noise_rejection"] == pytest.approx(1.0)


def test_patchwork_adapter_records_binary_and_rejects_overlapping_indices(
    tmp_path: Path,
) -> None:
    binary = tmp_path / "pypatchworkpp.so"
    binary.write_bytes(b"synthetic-binding")

    class Parameters:
        pass

    class Estimator:
        def __init__(self, _params: object) -> None:
            pass

        def estimateGround(self, _points: np.ndarray) -> None:
            pass

        def getGroundIndices(self) -> list[int]:
            return [0, 1]

        def getNongroundIndices(self) -> list[int]:
            return [1, 2]

    module = SimpleNamespace(
        __file__=str(binary),
        __version__="test",
        Parameters=Parameters,
        patchworkpp=Estimator,
    )
    adapter = PatchworkPPGroundSegmenter(  # type: ignore[arg-type]
        module,
        DEFAULT_GROUND_BENCHMARK_PROFILE,
    )
    assert adapter.identity["binary_sha256"]
    with pytest.raises(LidarGroundError, match="assigned one point twice"):
        adapter.segment(np.zeros((3, 4), dtype=np.float32))
