from __future__ import annotations

from pathlib import Path

import pytest

from k1link.perception.contracts import (
    BoundingRegion2D,
    CorridorIntersection,
    GridCell,
    HistorySample,
    LocalObstacleMap,
    MotionState,
    ObjectProposal2D,
    SourceAccounting,
    TemporalObstacle,
    TemporalState,
    ThreatDecision,
)
from k1link.perception.geometry import RecordedGeometryStore
from k1link.perception.graph_validation import validate_threats
from k1link.perception.threat import (
    DualEvidenceReplayThreatProvider,
    RecordedReplayBodyFrameResolver,
    ReplayBodyFrame,
    load_replay_threat_profile,
)

REPOSITORY_ROOT = Path(__file__).resolve().parents[1]
PROFILE_PATH = REPOSITORY_ROOT / "config/perception/m4-replay-threat-v2.json"
ROLLING_PROFILE_PATH = REPOSITORY_ROOT / "config/perception/m4-replay-threat-v3.json"


class _BodyFrames:
    def body_frame_for_frame(self, frame_id: str) -> ReplayBodyFrame:
        return ReplayBodyFrame(
            frame_id=frame_id,
            origin_map_xyz_m=(0.0, 0.0, 0.0),
            basis_map_from_body=((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0)),
            sensor_height_m=1.25,
            surface_slope_deg=0.0,
            forward_source="fixture",
            camera_forward_alignment_deg=0.0,
        )


def _obstacle(
    component_id: str,
    cell: GridCell,
    *,
    motion: MotionState,
    history: tuple[tuple[str, int, tuple[float, float, float]], ...],
    state: TemporalState = TemporalState.CURRENT,
    semantic_hint: str | None = None,
) -> TemporalObstacle:
    samples = tuple(
        HistorySample(frame_id=frame_id, evidence_time_ns=time_ns, centroid_xyz_m=point)
        for frame_id, time_ns, point in history
    )
    current = samples[-1]
    return TemporalObstacle(
        component_id=component_id,
        identity_scope="ephemeral",
        state=state,
        ttl_ns=(3_000_000_000 if state is TemporalState.RETAINED else 750_000_000),
        last_hit_ns=current.evidence_time_ns,
        age_ns=0 if state is TemporalState.CURRENT else 100_000_000,
        association_basis="test-spatial-support",
        history=samples,
        cells=() if state is TemporalState.EXPIRED else (cell,),
        coordinate_frame=None if state is TemporalState.EXPIRED else "map",
        last_centroid_xyz_m=None if state is TemporalState.EXPIRED else current.centroid_xyz_m,
        motion=MotionState.UNKNOWN if state is not TemporalState.CURRENT else motion,
        motion_confidence=(
            0.0 if state is not TemporalState.CURRENT or motion is MotionState.UNKNOWN else 1.0
        ),
        motion_reason=(
            "stale-support"
            if state is not TemporalState.CURRENT
            else "bounded-map-history-moving"
            if motion is MotionState.MOVING
            else "bounded-map-history-stationary"
            if motion is MotionState.STATIONARY
            else "insufficient-history"
        ),
        semantic_hint=semantic_hint,
    )


def _proposal(frame_id: str = "frame-000002") -> ObjectProposal2D:
    return ObjectProposal2D(
        proposal_id="proposal-camera-only",
        source_id="RAVNOVES00",
        frame_id=frame_id,
        region=BoundingRegion2D(10.0, 10.0, 20.0, 20.0),
        objectness=0.8,
        provider_id="test-detector/v1",
        model_id="test-model/v1",
        preprocess_id="test-preprocess/v1",
        semantic_hint="person",
    )


def _map(
    *,
    occupied: tuple[TemporalObstacle, ...] = (),
    unknown: tuple[TemporalObstacle, ...] = (),
    camera: tuple[ObjectProposal2D, ...] = (),
    frame_id: str = "frame-000002",
) -> LocalObstacleMap:
    return LocalObstacleMap(
        source_id="RAVNOVES00",
        session_id="20260720T065719Z_viewer_live",
        frame_id=frame_id,
        graph_id="reference-perception-graph/v1",
        generated_monotonic_ns=0,
        output_age_ns=0,
        occupied=occupied,
        unknown=unknown,
        camera_uncertainty=camera,
        accounting=SourceAccounting(1, 1, 0, 0),
    )


def test_replay_threat_profile_freezes_virtual_authority_and_dual_evidence_policy() -> None:
    profile = load_replay_threat_profile(PROFILE_PATH)

    assert profile.rig.body_length_m == 1.0
    assert profile.rig.body_width_m == 0.6
    assert profile.rig.nominal_sensor_height_m == 1.25
    assert profile.corridor.forward_length_m == 8.0
    assert profile.calibration_content_sha256 == (
        "05f3ad9b38b3a4fc95388a8ec83da83c745e217709e51787b3d5aad0969f6fa9"
    )


def test_recorded_body_frame_is_grounded_and_does_not_inherit_handheld_roll_pitch() -> None:
    profile = load_replay_threat_profile(PROFILE_PATH)
    resolver = RecordedReplayBodyFrameResolver(
        RecordedGeometryStore.from_repository(REPOSITORY_ROOT),
        profile=profile.body_frame,
    )

    start = resolver.body_frame_for_frame("frame-000000")
    middle = resolver.body_frame_for_frame("frame-000138")
    later = resolver.body_frame_for_frame("frame-000274")

    assert start is None  # the opening surface height is not qualified evidence
    assert middle is not None and later is not None
    assert tuple(row[2] for row in middle.basis_map_from_body) == (0.0, 0.0, 1.0)
    assert tuple(row[2] for row in later.basis_map_from_body) == (0.0, 0.0, 1.0)
    assert middle.sensor_height_m == pytest.approx(1.2509065924)
    assert later.sensor_height_m == pytest.approx(1.2838213430)
    assert middle.camera_forward_alignment_deg < 7.0
    assert later.camera_forward_alignment_deg < 2.0

    summary = resolver.qualification_summary()
    assert summary["available"] == 3928
    assert summary["qualified"] == 3861
    assert summary["rejected"] == 67


def test_static_crossing_approaching_and_geometry_only_critical_cases_are_never_safe() -> None:
    provider = DualEvidenceReplayThreatProvider(
        body_frame_resolver=_BodyFrames(),
        profile=load_replay_threat_profile(PROFILE_PATH),
    )
    current_frame = "frame-000002"
    critical = (
        _obstacle(
            "static-in-corridor",
            GridCell(6, 0, 0),
            motion=MotionState.STATIONARY,
            history=(
                ("frame-000000", 0, (2.925, 0.225, 0.225)),
                (current_frame, 300_000_000, (2.925, 0.225, 0.225)),
            ),
        ),
        _obstacle(
            "crossing",
            GridCell(6, 3, 0),
            motion=MotionState.MOVING,
            history=(
                ("frame-000000", 0, (2.925, 2.575, 0.225)),
                (current_frame, 300_000_000, (2.925, 1.575, 0.225)),
            ),
        ),
        _obstacle(
            "approaching",
            GridCell(9, 0, 0),
            motion=MotionState.MOVING,
            history=(
                ("frame-000000", 0, (6.275, 0.225, 0.225)),
                (current_frame, 300_000_000, (4.275, 0.225, 0.225)),
            ),
            semantic_hint="car",
        ),
        _obstacle(
            "geometry-only",
            GridCell(4, 0, 0),
            motion=MotionState.STATIONARY,
            history=(
                ("frame-000000", 0, (2.025, 0.225, 0.225)),
                (current_frame, 300_000_000, (2.025, 0.225, 0.225)),
            ),
            semantic_hint=None,
        ),
    )

    result = provider.assess(_map(occupied=critical))

    assert {item.decision for item in result} == {ThreatDecision.THREAT}
    assert all(item.corridor_intersection is CorridorIntersection.INTERSECTS for item in result)
    assert (
        next(item for item in result if item.component_id == "approaching").ttc_seconds is not None
    )
    assert (
        "geometry-only-evidence"
        in next(item for item in result if item.component_id == "geometry-only").reason_codes
    )


def test_receding_and_static_outside_are_clear_but_incomplete_evidence_is_unknown() -> None:
    provider = DualEvidenceReplayThreatProvider(
        body_frame_resolver=_BodyFrames(),
        profile=load_replay_threat_profile(PROFILE_PATH),
    )
    current_frame = "frame-000002"
    clear = (
        _obstacle(
            "static-outside",
            GridCell(6, 7, 0),
            motion=MotionState.STATIONARY,
            history=(
                ("frame-000000", 0, (2.925, 3.375, 0.225)),
                (current_frame, 300_000_000, (2.925, 3.375, 0.225)),
            ),
        ),
        _obstacle(
            "receding-behind",
            GridCell(-5, 0, 0),
            motion=MotionState.MOVING,
            history=(
                ("frame-000000", 0, (-1.025, 0.225, 0.225)),
                (current_frame, 300_000_000, (-2.025, 0.225, 0.225)),
            ),
        ),
    )
    incomplete = _obstacle(
        "unknown-motion",
        GridCell(6, 7, 0),
        motion=MotionState.UNKNOWN,
        history=((current_frame, 300_000_000, (2.925, 3.375, 0.225)),),
    )
    held = _obstacle(
        "occluded-held",
        GridCell(6, 0, 0),
        motion=MotionState.UNKNOWN,
        history=((current_frame, 300_000_000, (2.925, 0.225, 0.225)),),
        state=TemporalState.HELD,
    )
    obstacle_map = _map(
        occupied=(*clear, incomplete),
        unknown=(held,),
        camera=(_proposal(),),
    )

    result = provider.assess(obstacle_map)
    by_id = {item.component_id: item for item in result}

    assert by_id["static-outside"].decision is ThreatDecision.NOT_THREAT
    assert by_id["receding-behind"].decision is ThreatDecision.NOT_THREAT
    assert by_id["unknown-motion"].decision is ThreatDecision.UNKNOWN
    assert by_id["occluded-held"].decision is ThreatDecision.UNKNOWN
    assert by_id["proposal-camera-only"].decision is ThreatDecision.UNKNOWN
    assert by_id["proposal-camera-only"].closest_approach_m is None
    validate_threats(obstacle_map, result)


def test_semantic_hint_and_ephemeral_component_name_do_not_change_threat_geometry() -> None:
    provider = DualEvidenceReplayThreatProvider(
        body_frame_resolver=_BodyFrames(),
        profile=load_replay_threat_profile(PROFILE_PATH),
    )
    history = (
        ("frame-000000", 0, (2.925, 0.225, 0.225)),
        ("frame-000002", 300_000_000, (2.925, 0.225, 0.225)),
    )
    first = _obstacle(
        "ephemeral-a",
        GridCell(6, 0, 0),
        motion=MotionState.STATIONARY,
        history=history,
        semantic_hint="car",
    )
    second = _obstacle(
        "ephemeral-b",
        GridCell(6, 0, 0),
        motion=MotionState.STATIONARY,
        history=history,
        semantic_hint=None,
    )

    values = provider.assess(_map(occupied=(first, second)))

    assert values[0].decision == values[1].decision
    assert values[0].corridor_intersection == values[1].corridor_intersection
    assert values[0].relative_speed_mps == values[1].relative_speed_mps
    assert values[0].closest_approach_m == values[1].closest_approach_m
    assert values[0].ttc_seconds == values[1].ttc_seconds


def test_retained_map_can_block_but_cannot_claim_clear_or_motion() -> None:
    provider = DualEvidenceReplayThreatProvider(
        body_frame_resolver=_BodyFrames(),
        profile=load_replay_threat_profile(ROLLING_PROFILE_PATH),
    )
    retained_in = _obstacle(
        "retained-in",
        GridCell(6, 0, 0),
        motion=MotionState.UNKNOWN,
        history=(("frame-000002", 300_000_000, (2.925, 0.225, 0.225)),),
        state=TemporalState.RETAINED,
    )
    retained_out = _obstacle(
        "retained-out",
        GridCell(6, 7, 0),
        motion=MotionState.UNKNOWN,
        history=(("frame-000002", 300_000_000, (2.925, 3.375, 0.225)),),
        state=TemporalState.RETAINED,
    )

    values = provider.assess(_map(occupied=(retained_in, retained_out)))
    by_id = {item.component_id: item for item in values}

    assert by_id["retained-in"].decision is ThreatDecision.THREAT
    assert by_id["retained-in"].relative_speed_mps is None
    assert by_id["retained-out"].decision is ThreatDecision.UNKNOWN
    assert by_id["retained-out"].corridor_intersection is CorridorIntersection.UNKNOWN