from __future__ import annotations

from pathlib import Path

from k1link.perception.m48s_replay_timeline import _actionable_camera_metric_rows
from k1link.perception.threat import ReplayBodyFrame, load_replay_threat_profile

REPOSITORY_ROOT = Path(__file__).resolve().parents[1]


def test_camera_obstacles_keep_only_added_cells_inside_an_accepted_threat() -> None:
    profile = load_replay_threat_profile(
        REPOSITORY_ROOT / "config/perception/m4-replay-threat-v3.json"
    )
    body_frame = ReplayBodyFrame(
        frame_id="frame-000001",
        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="test",
        camera_forward_alignment_deg=0.0,
    )
    threat = {
        "component_id": "rolling-actionable",
        "cells": [
            {"x": 0, "y": 0, "z": 0},
            {"x": 1, "y": 4, "z": 0},
            {"x": 2, "y": 0, "z": 0},
        ],
        "assessment": {
            "decision": "threat",
            "corridor_intersection": "intersects",
        },
    }
    unknown = {
        **threat,
        "component_id": "rolling-unknown",
        "assessment": {
            "decision": "unknown",
            "corridor_intersection": "unknown",
        },
    }

    selected = _actionable_camera_metric_rows(
        [threat, unknown],
        added_cells=frozenset({(0, 0, 0), (1, 4, 0)}),
        body_frame=body_frame,
        profile=profile,
    )

    assert [row["component_id"] for row in selected] == ["rolling-actionable"]
    assert selected[0]["cells"] == [{"x": 0, "y": 0, "z": 0}]