from __future__ import annotations

import hashlib
import json
import time
from pathlib import Path

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

from k1link.compute import (
    E10_LIDAR_PACK_SCHEMA,
    E10LidarFieldSource,
    K1LocalSurfaceProfile,
    K1LocalSurfaceShadowInput,
    K1LocalSurfaceShadowRuntime,
    K1LocalSurfaceV1,
    build_k1_local_surface,
)
from k1link.compute.lidar_local_surface import (
    DEFAULT_K1_LOCAL_SURFACE_PROFILE as REPLAY_LOCAL_SURFACE_PROFILE,
)
from k1link.compute.lidar_local_surface_geometry import (
    DEFAULT_K1_LOCAL_SURFACE_PROFILE as WORKER_LOCAL_SURFACE_PROFILE,
)
from k1link.web.lidar_api import build_lidar_router


def _canonical_json(value: object) -> bytes:
    return json.dumps(
        value,
        ensure_ascii=False,
        sort_keys=True,
        separators=(",", ":"),
        allow_nan=False,
    ).encode()


def _sha256(path: Path) -> str:
    return hashlib.sha256(path.read_bytes()).hexdigest()


def test_replay_and_minimal_worker_pin_the_same_local_surface_profile() -> None:
    assert WORKER_LOCAL_SURFACE_PROFILE.to_dict() == REPLAY_LOCAL_SURFACE_PROFILE.to_dict()


def _source_pack(root: Path) -> Path:
    frame_count = 8
    available = np.asarray([True, True, True, True, True, True, True, False])
    poses: list[list[float]] = []
    clouds: list[np.ndarray] = []
    offsets = [0]
    for frame_index in range(frame_count):
        pose_x = frame_index * 0.35
        pose_y = 0.1 * np.sin(frame_index)
        ground_z = 0.04 * pose_x - 0.015 * pose_y
        poses.append([pose_x, pose_y, ground_z + 1.42])
        if not available[frame_index]:
            offsets.append(offsets[-1])
            continue
        axis = np.linspace(-3.5, 3.5, 12)
        xx, yy = np.meshgrid(axis + pose_x, axis + pose_y)
        zz = 0.04 * xx - 0.015 * yy + 0.008 * np.sin(xx * 2 + frame_index)
        ground = np.column_stack((xx.ravel(), yy.ravel(), zz.ravel()))
        obstacle_xy = ground[::13, :2]
        obstacle_z = 0.04 * obstacle_xy[:, 0] - 0.015 * obstacle_xy[:, 1] + 0.75
        obstacle = np.column_stack((obstacle_xy, obstacle_z))
        cloud = np.concatenate((ground, obstacle)).astype("<f4")
        clouds.append(cloud)
        offsets.append(offsets[-1] + cloud.shape[0])
    points = np.concatenate(clouds).astype("<f4")
    arrays = {
        "frame_indices": np.arange(frame_count, dtype="<i8"),
        "source_frame_indices": np.arange(100, 100 + frame_count * 10, 10, dtype="<i8"),
        "session_seconds": np.arange(frame_count, dtype="<f8") * 0.1 + 10.0,
        "sample_available": available.astype("?"),
        "cloud_offsets": np.asarray(offsets, dtype="<i8"),
        "cloud_points_map": points,
        "pose_positions_map": np.asarray(poses, dtype="<f8"),
        "pose_quaternions_map_from_lidar": np.tile(
            np.asarray([0.0, 0.0, 0.0, 1.0], dtype="<f8"),
            (frame_count, 1),
        ),
        "lidar_camera_delta_ms": np.zeros(frame_count, dtype="<f8"),
        "pose_point_delta_ms": np.asarray(
            [4.0, 5.0, 6.0, 7.0, 130.0, 5.0, 6.0, 0.0],
            dtype="<f8",
        ),
        "intrinsic_fx_fy_cx_cy": np.asarray(
            [100.0, 100.0, 50.0, 50.0],
            dtype="<f8",
        ),
        "distortion_kb4": np.zeros(4, dtype="<f8"),
        "t_camera_from_lidar": np.eye(4, dtype="<f8"),
    }
    identity = {
        "schema_version": E10_LIDAR_PACK_SCHEMA,
        "session_id": "synthetic-ravnoves00-local-surface",
        "frame_count": frame_count,
        "available_lidar_frames": int(np.count_nonzero(available)),
        "point_count": int(points.shape[0]),
        "timeline_start_seconds": 10.0,
        "timeline_end_seconds": 10.7,
    }
    identity_sha256 = hashlib.sha256(_canonical_json(identity)).hexdigest()
    pack = root / f"e10-lidar-pack-{identity_sha256}"
    pack.mkdir(parents=True)
    arrays_path = pack / "lidar-pack.npz"
    np.savez_compressed(arrays_path, **arrays)  # type: ignore[arg-type]
    manifest = {
        "schema_version": E10_LIDAR_PACK_SCHEMA,
        "pack_id": pack.name,
        "identity_sha256": identity_sha256,
        "identity": identity,
        "artifact": {
            "path": arrays_path.name,
            "media_type": "application/x-npz",
            "byte_length": arrays_path.stat().st_size,
            "sha256": _sha256(arrays_path),
        },
    }
    (pack / "manifest.json").write_bytes(_canonical_json(manifest))
    return pack


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


def test_k1_local_surface_is_dynamic_source_bound_and_read_only(
    tmp_path: Path,
) -> None:
    source_path = _source_pack(tmp_path / "source")
    source_artifact = source_path / "lidar-pack.npz"
    source_sha256 = _sha256(source_artifact)
    profile = K1LocalSurfaceProfile(
        profile_id="synthetic-dynamic-local-surface/v1",
        local_radius_m=5.0,
        cell_size_m=0.5,
        surface_ttl_s=0.5,
        minimum_surface_cells=12,
    )
    source = E10LidarFieldSource(source_path)
    try:
        output = build_k1_local_surface(
            source,
            tmp_path / "models",
            profile=profile,
        )
        duplicate = build_k1_local_surface(
            source,
            tmp_path / "models",
            profile=profile,
        )
    finally:
        source.close()
    assert duplicate == output
    assert _sha256(source_artifact) == source_sha256

    model = K1LocalSurfaceV1(output)
    source = E10LidarFieldSource(source_path)
    try:
        detail = model.frame_detail(source, 2)
        review = model.review_detail(source)
        assert model.report["source"]["passive_processing_only"] is True
        assert model.report["source"]["firmware_or_device_commands_used"] is False
        assert model.report["surface_model"]["hardcoded_height_m"] is None
        assert model.report["occupancy_policy"]["absence_of_points_means_free"] is False
        assert model.report["metrics"]["frames"]["valid"] >= 5
        temporal = model.report["metrics"]["temporal_qualification"]
        assert temporal["prediction"]["current_frame_excluded"] is True
        assert temporal["prediction"]["sample_count"] >= 4
        assert temporal["prediction"]["residual_p95_m"]["p95"] < 0.05
        assert temporal["stability"]["sample_count"] >= 4
        assert temporal["step_candidates"]["is_ground_truth"] is False
        assert temporal["step_candidates"]["frames_with_candidates"] > 0
        assert detail["valid"] is True
        assert detail["surface"]["local_radius_m"] == 5.0
        assert 1.2 < detail["surface"]["sensor_height_m"] < 1.6
        assert detail["counts"]["surface"] > 50
        assert detail["counts"]["occupied"] > 0
        assert detail["counts"]["step_candidate"] > 0
        assert detail["prediction"]["current_frame_excluded"] is True
        assert detail["prediction"]["available"] is True
        evidence = detail["prediction"]["evidence"]
        assert evidence["available"] is True
        assert evidence["current_frame_excluded_from_plane"] is True
        assert evidence["basis"] == "current-lower-cell-observations"
        assert evidence["ground_truth"] is False
        assert len(evidence["cell_points_xyz_m"]) == detail["prediction"]["cell_count"]
        assert len(evidence["cell_signed_residual_m"]) == detail["prediction"]["cell_count"]
        assert len(evidence["cell_inlier"]) == detail["prediction"]["cell_count"]
        assert (
            sum(evidence["cell_inlier"]) / detail["prediction"]["cell_count"]
            == (detail["prediction"]["inlier_fraction"])
        )
        assert detail["temporal"]["compared"] is True
        assert detail["authority"]["commands_enabled"] is False
        assert review["available"] is True
        assert review["ground_truth"] is False
        assert review["criteria"]["prediction_inlier_fraction_floor"] == 0.85
        assert review["summary"]["item_count"] == len(review["items"])
        assert "1.27" not in repr({"identity": model.identity, "report": model.report})
        assert str(tmp_path) not in repr(detail)
        assert str(tmp_path) not in repr(review)
    finally:
        source.close()
        model.close()

    router = build_lidar_router(
        root_provider=lambda: None,
        ground_root_provider=lambda: None,
        field_review_root_provider=lambda: None,
        local_surface_root_provider=lambda: output.parent,
        e10_source_root_provider=lambda: source_path.parent,
    )
    catalog_route = _endpoint(router, "/api/v1/lidar/local-surfaces")
    frame_route = _endpoint(
        router,
        "/api/v1/lidar/local-surfaces/{model_id}/frames/{frame_index}",
    )
    timeline_route = _endpoint(
        router,
        "/api/v1/lidar/local-surfaces/{model_id}/timeline",
    )
    review_route = _endpoint(
        router,
        "/api/v1/lidar/local-surfaces/{model_id}/review",
    )
    catalog = catalog_route(limit=10)  # type: ignore[operator]
    frame = frame_route(model_id=output.name, frame_index=2)  # type: ignore[operator]
    timeline = timeline_route(model_id=output.name)  # type: ignore[operator]
    review = review_route(model_id=output.name)  # type: ignore[operator]
    assert catalog["valid_total"] == 1
    assert catalog["items"][0]["status"] == "diagnostic-only"
    assert frame["model_id"] == output.name
    assert frame["access"] == "read-only"
    assert timeline["frame_count"] == 8
    assert sum(timeline["prediction_available"]) >= 4
    assert len(timeline["temporal_jump"]) == 8
    assert str(tmp_path) not in repr(timeline)
    assert review["review_profile_id"] == "missioncore-local-surface-attention/v1"
    assert review["access"] == "read-only"
    assert str(tmp_path) not in repr(review)


def _shadow_input(
    source: E10LidarFieldSource,
    frame_index: int,
) -> K1LocalSurfaceShadowInput:
    offsets = source.arrays["cloud_offsets"]
    start = int(offsets[frame_index])
    end = int(offsets[frame_index + 1])
    points = np.asarray(
        source.arrays["cloud_points_map"][start:end],
        dtype=np.float64,
    ).copy()
    position = np.asarray(
        source.arrays["pose_positions_map"][frame_index],
        dtype=np.float64,
    ).copy()
    points.flags.writeable = False
    position.flags.writeable = False
    return K1LocalSurfaceShadowInput(
        frame_index=frame_index,
        source_frame_index=int(source.arrays["source_frame_indices"][frame_index]),
        session_seconds=float(source.arrays["session_seconds"][frame_index]),
        pose_binding_age_ms=abs(float(source.arrays["pose_point_delta_ms"][frame_index])),
        points_map=points,
        position_map=position,
        published_monotonic_ns=time.monotonic_ns(),
    )


def test_k1_local_surface_shadow_matches_replay_and_stays_non_authoritative(
    tmp_path: Path,
) -> None:
    source_path = _source_pack(tmp_path / "source")
    profile = K1LocalSurfaceProfile(
        profile_id="synthetic-shadow-local-surface/v1",
        local_radius_m=5.0,
        cell_size_m=0.5,
        surface_ttl_s=0.5,
        minimum_surface_cells=12,
    )
    source = E10LidarFieldSource(source_path)
    try:
        output = build_k1_local_surface(
            source,
            tmp_path / "models",
            profile=profile,
        )
    finally:
        source.close()

    source = E10LidarFieldSource(source_path)
    model = K1LocalSurfaceV1(output)
    runtime = K1LocalSurfaceShadowRuntime(
        "synthetic-shadow",
        profile=profile,
        queue_capacity=2,
        result_capacity=16,
    )
    try:
        published = []
        for frame_index in range(source.frame_count):
            if not bool(source.arrays["sample_available"][frame_index]):
                continue
            runtime.publish(_shadow_input(source, frame_index))
            assert runtime.wait_until_idle(2.0)
            published.append(frame_index)
        runtime.close()
        results = runtime.results()
        assert [item.frame_index for item in results] == published
        offsets = source.arrays["cloud_offsets"]
        for result in results:
            frame_index = result.frame_index
            start = int(offsets[frame_index])
            end = int(offsets[frame_index + 1])
            if bool(model.arrays["frame_valid"][frame_index]):
                assert result.state == "valid"
                assert result.sensor_height_m == pytest.approx(
                    float(model.arrays["sensor_height_m"][frame_index]),
                    abs=1e-12,
                )
                assert result.slope_deg == pytest.approx(
                    float(model.arrays["slope_deg"][frame_index]),
                    abs=1e-12,
                )
                assert result.roughness_m == pytest.approx(
                    float(model.arrays["roughness_m"][frame_index]),
                    abs=1e-12,
                )
                assert result.confidence == pytest.approx(
                    float(model.arrays["confidence"][frame_index]),
                    abs=1e-12,
                )
                assert np.array_equal(
                    result.point_class,
                    model.arrays["point_class"][start:end],
                )
                assert np.array_equal(
                    result.point_step_candidate,
                    model.arrays["point_step_candidate"][start:end],
                )
            else:
                assert result.state == "pose-stale"
        snapshot = runtime.snapshot()
        assert snapshot["queue"]["capacity"] == 2
        assert snapshot["queue"]["published"] == len(published)
        assert snapshot["queue"]["consumed"] == len(published)
        assert snapshot["queue"]["dropped_overflow"] == 0
        assert snapshot["results"]["failed"] == 0
        assert snapshot["occupancy_policy"]["absence_of_points_means_free"] is False
        assert snapshot["authority"]["commands_enabled"] is False
        assert snapshot["authority"]["navigation_or_safety_accepted"] is False
        assert snapshot["closed"] is True
    finally:
        runtime.close()
        source.close()
        model.close()


def test_k1_local_surface_shadow_overload_is_bounded_and_latest_wins(
    tmp_path: Path,
) -> None:
    source_path = _source_pack(tmp_path / "source")
    source = E10LidarFieldSource(source_path)
    runtime = K1LocalSurfaceShadowRuntime(
        "synthetic-overload",
        profile=K1LocalSurfaceProfile(
            profile_id="synthetic-shadow-overload/v1",
            local_radius_m=5.0,
            cell_size_m=0.5,
            minimum_surface_cells=12,
        ),
        queue_capacity=1,
        result_capacity=2,
    )
    try:
        template = _shadow_input(source, 0)
        for frame_index in range(200):
            runtime.publish(
                K1LocalSurfaceShadowInput(
                    frame_index=frame_index,
                    source_frame_index=10_000 + frame_index,
                    session_seconds=10.0 + frame_index * 0.1,
                    pose_binding_age_ms=4.0,
                    points_map=template.points_map,
                    position_map=template.position_map,
                    published_monotonic_ns=time.monotonic_ns(),
                )
            )
        runtime.close()
        snapshot = runtime.snapshot()
        queue = snapshot["queue"]
        assert queue["maximum_depth"] <= queue["capacity"] == 1
        assert queue["dropped_overflow"] > 0
        assert queue["consumed"] + queue["dropped_overflow"] == queue["published"]
        assert snapshot["results"]["depth"] <= 2
        assert runtime.results()[-1].frame_index == 199
        assert snapshot["results"]["latest"]["frame_index"] == 199
        assert snapshot["authority"]["commands_enabled"] is False
    finally:
        runtime.close()
        source.close()