from __future__ import annotations

from pathlib import Path

import pytest

from k1link.compute.world_motion import (
    WorldMotionTracker,
    evaluate_motion_benchmark,
    read_motion_benchmark,
    read_world_motion_profile,
)


def _profile() -> dict[str, object]:
    root = Path(__file__).resolve().parents[1]
    profile, digest = read_world_motion_profile(
        root / "experiments" / "perception" / "e24_world_motion_profile.json"
    )
    assert len(digest) == 64
    return profile


def _object(track_id: int, x: float, *, group: str = "vehicle") -> dict[str, object]:
    label = "person" if group == "person" else "car"
    half_size = [0.35, 0.35, 0.9] if group == "person" else [2.25, 0.925, 0.775]
    return {
        "association_group": group,
        "bbox_xyxy": [100.0, 100.0, 200.0, 200.0],
        "clustered_points": 32,
        "cuboid_center_map": [x, 2.0, 0.8],
        "cuboid_half_size": half_size,
        "cuboid_quaternion_xyzw": [0.0, 0.0, 0.0, 1.0],
        "cuboid_status": "accepted-class-prior-amodal-v1",
        "distance_smoothed_m": 10.0,
        "geometry": "class-prior-completed-from-visible-lidar-support",
        "label": label,
        "score": 0.9,
        "track_id": track_id,
    }


def _provisional_person(track_id: int, x: float) -> dict[str, object]:
    value = _object(track_id, x, group="person")
    value.update(
        {
            "cuboid_center_map": None,
            "cuboid_half_size": None,
            "cuboid_quaternion_xyzw": None,
            "cuboid_status": "rejected-amodal-completion-support-coverage-below-threshold",
            "observed_cuboid_center_map": [x, 2.0, 0.8],
            "observed_cuboid_quaternion_xyzw": [0.0, 0.0, 0.0, 1.0],
        }
    )
    return value


def test_e24_profile_is_bounded_and_has_no_control_authority() -> None:
    profile = _profile()

    assert profile["source"]["coordinate_frame"] == "k1-map"
    assert profile["bounds"]["maximum_tracks"] == 192
    assert profile["authority"] == {
        "commands_enabled": False,
        "navigation_or_safety_accepted": False,
    }


def test_e24_reassociates_changed_source_id_and_classifies_static() -> None:
    tracker = WorldMotionTracker(_profile())
    last = []
    for frame in range(30):
        source_id = 10 if frame < 12 else 77
        jitter = 0.025 if frame % 2 else -0.025
        last, world = tracker.update(
            frame_index=frame,
            session_seconds=frame * 0.1,
            objects=[_object(source_id, 10.0 + jitter)],
        )

    assert last[0]["track_id"] == 240001
    assert last[0]["source_track_id"] == 77
    assert world[0]["motion_state"] == "static"
    assert tracker.snapshot()["reassociated_source_ids"] == 1


def test_e24_classifies_sustained_world_motion_dynamic() -> None:
    tracker = WorldMotionTracker(_profile())
    world = []
    for frame in range(35):
        _, world = tracker.update(
            frame_index=frame,
            session_seconds=frame * 0.1,
            objects=[_object(20, 5.0 + frame * 0.12, group="person")],
        )

    assert world[0]["motion_state"] == "dynamic"
    assert world[0]["speed_mps"] is not None
    assert world[0]["speed_mps"] > 0.75


def test_e24_confirms_sparse_visible_support_without_promoting_first_hit() -> None:
    tracker = WorldMotionTracker(_profile())
    first, first_world = tracker.update(
        frame_index=0,
        session_seconds=0.0,
        objects=[_provisional_person(21, 5.0)],
    )
    tracker.update(
        frame_index=1,
        session_seconds=0.5,
        objects=[_provisional_person(21, 5.5)],
    )
    third, third_world = tracker.update(
        frame_index=2,
        session_seconds=1.0,
        objects=[_provisional_person(21, 6.0)],
    )

    assert first[0]["cuboid_status"].startswith("rejected-")
    assert first_world == []
    assert third[0]["cuboid_status"] == "accepted-world-track-provisional-e24-v1"
    assert third_world[0]["track_hits"] == 3


def test_e24_holds_confirmed_track_briefly_then_expires() -> None:
    tracker = WorldMotionTracker(_profile())
    for frame in range(12):
        tracker.update(
            frame_index=frame,
            session_seconds=frame * 0.1,
            objects=[_object(30, 3.0)],
        )
    held, held_world = tracker.update(
        frame_index=12,
        session_seconds=1.2,
        objects=[],
    )
    expired, expired_world = tracker.update(
        frame_index=20,
        session_seconds=2.0,
        objects=[],
    )

    assert held[0]["cuboid_status"] == "accepted-world-track-hold-e24-v1"
    assert held_world[0]["observation_age_ms"] == pytest.approx(100.0)
    assert expired == []
    assert expired_world == []


def test_e24_benchmark_contract_and_evaluator() -> None:
    root = Path(__file__).resolve().parents[1]
    benchmark, digest = read_motion_benchmark(
        root / "experiments" / "perception" / "e24_motion_benchmark.json"
    )
    event = benchmark["events"][0]
    rows = [
        {
            "session_seconds": 55.0 + index * 0.2,
            "objects": [
                {
                    "track_id": 1,
                    "class": event["class_group"],
                    "motion_state": event["expected_motion"],
                    "observation_age_ms": 0.0,
                }
            ],
        }
        for index in range(5)
    ]
    one_event = {**benchmark, "events": [event]}
    result = evaluate_motion_benchmark(rows, one_event)

    assert len(digest) == 64
    assert result["passed"] is True
    assert result["events"][0]["best_hits"] == 5
