from __future__ import annotations

from pathlib import Path

import numpy as np

from k1link.compute.camera_ego_motion import (
    CameraEgoMotionTracker,
    _camera_evidence,
    _fuse_camera_lidar_object,
    evaluate_camera_ego_motion_benchmark,
    read_camera_ego_motion_benchmark,
    read_camera_ego_motion_profile,
)
from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import (
    Kb4ProjectionProfile,
    project_map_points_kb4,
)

ROOT = Path(__file__).parents[1]


def _profile() -> dict[str, object]:
    profile, digest = read_camera_ego_motion_profile(
        ROOT
        / "experiments"
        / "perception"
        / "e26_camera_ego_motion_profile.json"
    )
    assert len(digest) == 64
    return profile


def _projection() -> Kb4ProjectionProfile:
    transform = np.eye(4, dtype=np.float64)
    transform.setflags(write=False)
    return Kb4ProjectionProfile(
        source_id="sensor.camera.right",
        calibration_slot="camera_1",
        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 _bbox_for_world_point(
    point_map: np.ndarray,
    position_map: np.ndarray,
    *,
    width: float = 50.0,
    height: float = 60.0,
) -> list[float]:
    projected = project_map_points_kb4(
        [point_map],
        position_map_xyz=(
            float(position_map[0]),
            float(position_map[1]),
            float(position_map[2]),
        ),
        orientation_map_from_lidar_xyzw=(0.0, 0.0, 0.0, 1.0),
        profile=_projection(),
    )
    u, v = projected.pixels_xy[0]
    inset = float(
        _profile()["camera_evidence"]["footpoint_inset_fraction"]  # type: ignore[index]
    )
    bottom = float(v) + inset * height
    return [
        float(u) - width * 0.5,
        bottom - height,
        float(u) + width * 0.5,
        bottom,
    ]


def _observe_track(
    tracker: CameraEgoMotionTracker,
    *,
    moving: bool,
) -> dict[str, object]:
    evidence: dict[str, object] = {}
    for index in range(18):
        session_seconds = index * 0.1
        position = np.asarray([index * 0.1, 0.0, 0.0], dtype=np.float64)
        point = np.asarray(
            [index * 0.35 if moving else 0.0, 0.0, 6.0],
            dtype=np.float64,
        )
        evidence = tracker.observe(
            source_track_id=7,
            group="person",
            session_seconds=session_seconds,
            bbox_xyxy=_bbox_for_world_point(point, position),
            detector_score=0.9,
            position_map_xyz=position,
            orientation_map_from_lidar_xyzw=np.asarray(
                [0.0, 0.0, 0.0, 1.0],
                dtype=np.float64,
            ),
        )
    return evidence


def test_e26_profile_and_benchmark_are_pinned_and_diagnostic_only() -> None:
    profile = _profile()
    benchmark, digest = read_camera_ego_motion_benchmark(
        ROOT / "experiments" / "perception" / "e26_motion_benchmark.json"
    )
    assert profile["authority"] == {
        "commands_enabled": False,
        "navigation_or_safety_accepted": False,
    }
    assert profile["fusion"]["unknown_is_occupied"] is True
    assert profile["fusion"]["camera_only_metric_velocity_valid"] is False
    assert benchmark["benchmark_id"] == (
        "ravnoves00-camera-ego-reviewed-anchors-v1"
    )
    assert len(benchmark["events"]) == 11
    assert len(digest) == 64


def test_e26_multiview_rays_accept_one_static_world_hypothesis() -> None:
    tracker = CameraEgoMotionTracker(_profile(), _projection())
    evidence = _observe_track(tracker, moving=False)
    assert evidence["camera_evidence_current"] is True
    assert evidence["camera_motion_state"] == "static"
    assert float(evidence["camera_static_residual_p80_degrees"]) < 1e-6
    assert float(evidence["camera_static_positive_depth_fraction"]) == 1.0
    assert tracker.snapshot()["peak_tracks"] == 1


def test_e26_multiview_rays_reject_a_moving_world_hypothesis() -> None:
    tracker = CameraEgoMotionTracker(_profile(), _projection())
    evidence = _observe_track(tracker, moving=True)
    assert evidence["camera_evidence_current"] is True
    assert evidence["camera_motion_state"] == "dynamic"
    assert float(evidence["camera_motion_confidence"]) > 0.0


def test_e26_invalid_fisheye_ray_is_unavailable_not_a_run_failure() -> None:
    tracker = CameraEgoMotionTracker(_profile(), _projection())
    evidence = tracker.observe(
        source_track_id=99,
        group="vehicle",
        session_seconds=1.0,
        bbox_xyxy=[100000.0, 100000.0, 100100.0, 100100.0],
        detector_score=0.9,
        position_map_xyz=np.zeros(3, dtype=np.float64),
        orientation_map_from_lidar_xyzw=np.asarray(
            [0.0, 0.0, 0.0, 1.0],
            dtype=np.float64,
        ),
    )
    assert evidence["camera_evidence_current"] is False
    assert tracker.snapshot()["invalid_camera_rays"] == 1
    assert tracker.snapshot()["active_tracks"] == 0


def test_e26_camera_lidar_conflict_is_unknown_not_averaged() -> None:
    profile = _profile()
    tracker = CameraEgoMotionTracker(profile, _projection())
    camera_object = {
        "track_id": 9,
        "association_group": "vehicle",
        "bbox_xyxy": [100.0, 100.0, 160.0, 160.0],
        "label": "car",
        "score": 0.9,
    }
    lidar_object = {
        **camera_object,
        "track_id": 240009,
        "source_track_id": 9,
        "motion_state": "static",
        "motion_confidence": 0.8,
        "speed_mps": 0.0,
        "velocity_map_mps": [0.0, 0.0, 0.0],
        "cuboid_status": "accepted-world-tracked-e24-v1",
    }
    evidence = {
        **_camera_evidence(None, "unknown", 0.0),
        "camera_motion_state": "dynamic",
        "camera_motion_confidence": 0.9,
        "camera_evidence_current": True,
    }
    fused = _fuse_camera_lidar_object(
        camera_object,
        lidar_object,
        evidence,
        profile,
        tracker,
    )
    assert fused["motion_state"] == "unknown"
    assert fused["motion_status"] == "e26-explicit-camera-lidar-conflict"
    assert fused["speed_mps"] is None
    assert fused["velocity_map_mps"] is None
    assert tracker.snapshot()["fusion_conflicts"] == 1


def test_e26_camera_only_dynamic_does_not_invent_metric_velocity() -> None:
    profile = _profile()
    tracker = CameraEgoMotionTracker(profile, _projection())
    camera_object = {
        "track_id": 12,
        "association_group": "person",
        "bbox_xyxy": [100.0, 100.0, 160.0, 190.0],
        "label": "person",
        "score": 0.9,
        "speed_mps": 42.0,
        "velocity_map_mps": [42.0, 0.0, 0.0],
    }
    evidence = {
        **_camera_evidence(None, "unknown", 0.0),
        "camera_motion_state": "dynamic",
        "camera_motion_confidence": 0.8,
        "camera_evidence_current": True,
    }
    fused = _fuse_camera_lidar_object(
        camera_object,
        None,
        evidence,
        profile,
        tracker,
    )
    assert fused["motion_state"] == "dynamic"
    assert fused["motion_status"] == (
        "e26-camera-relative-only-no-metric-velocity"
    )
    assert fused["speed_mps"] is None
    assert fused["velocity_map_mps"] is None
    assert fused["camera_only_metric_velocity_valid"] is False


def test_e26_benchmark_separates_dynamic_hits_and_static_false_alarm() -> None:
    benchmark = {
        "events": [
            {
                "id": "moving",
                "kind": "target-motion",
                "window_seconds": [0.0, 1.0],
                "class_group": "person",
                "target_source_track_ids": [7],
                "expected_motion": "dynamic",
                "minimum_hits": 2,
                "minimum_span_seconds": 0.1,
                "minimum_coverage_fraction": 0.5,
            },
            {
                "id": "parked",
                "kind": "static-control",
                "window_seconds": [0.0, 1.0],
                "class_group": "vehicle",
                "expected_motion": "static",
                "minimum_source_observations": 2,
                "minimum_coverage_fraction": 0.5,
                "minimum_classified_fraction": 0.5,
                "maximum_false_dynamic_fraction": 0.1,
            },
        ]
    }
    source_rows = []
    fusion_rows = []
    for index, timestamp in enumerate((0.0, 0.2)):
        source_rows.append(
            {
                "frame_index": index,
                "session_seconds": timestamp,
                "objects": [
                    {
                        "track_id": 7,
                        "association_group": "person",
                    },
                    {
                        "track_id": 8,
                        "association_group": "vehicle",
                    },
                ],
            }
        )
        fusion_rows.append(
            {
                "frame_index": index,
                "session_seconds": timestamp,
                "objects": [
                    {
                        "source_track_id": 7,
                        "association_group": "person",
                        "camera_evidence_current": True,
                        "motion_state": "dynamic",
                    },
                    {
                        "source_track_id": 8,
                        "association_group": "vehicle",
                        "camera_evidence_current": True,
                        "motion_state": "static",
                    },
                ],
            }
        )
    result = evaluate_camera_ego_motion_benchmark(
        source_rows,
        fusion_rows,
        benchmark,
    )
    assert result["passed"] is True
    assert result["passed_events"] == 2
    assert result["events"][1]["false_dynamic_fraction"] == 0.0
