from __future__ import annotations

from pathlib import Path

import numpy as np
import pytest

from k1link.perception.contracts import (
    BoundingRegion2D,
    EvidenceBasis,
    EvidenceCurrentness,
    MetricGeometry,
    ObstacleObservation,
)
from k1link.perception.object_understanding import (
    AdvisoryResponse,
    AgencyState,
    SemanticResolution,
    StateBasis,
    load_object_semantic_vocabulary,
)
from k1link.perception.open_vocabulary_semantics import (
    OpenVocabularyDetection,
    OpenVocabularySemanticError,
    bind_object_understandings,
    fuse_open_vocabulary_detections,
    load_open_vocabulary_semantic_profile,
    parse_tao_grounding_dino_labels,
)

REPOSITORY_ROOT = Path(__file__).resolve().parents[1]
PROFILE_PATH = REPOSITORY_ROOT / "config/perception/open-vocabulary-semantic-shadow-v0.json"
VOCABULARY_PATH = REPOSITORY_ROOT / "config/perception/object-semantic-vocabulary-v0.json"
VALID_FOV_MASK = np.ones((600, 800), dtype=np.bool_)


def _detection(
    detection_id: str,
    raw_label: str,
    confidence: float,
    region: tuple[float, float, float, float],
    *,
    prompt_set_id: str = "urban-static/v0",
) -> OpenVocabularyDetection:
    return OpenVocabularyDetection(
        detection_id=detection_id,
        source_id="RAVNOVES00",
        frame_id="frame-000121",
        prompt_set_id=prompt_set_id,
        raw_label=raw_label,
        confidence=confidence,
        region=BoundingRegion2D(*region),
    )


def _observation(
    proposal_id: str | None,
    *,
    ordinal: int,
) -> ObstacleObservation:
    return ObstacleObservation(
        observation_id=f"frame-000121:observation-{ordinal}",
        occupancy_key=f"frame-000121:occupancy-{ordinal}",
        source_id="RAVNOVES00",
        frame_id="frame-000121",
        evidence_time_ns=121,
        basis=EvidenceBasis.FUSED if proposal_id else EvidenceBasis.LIDAR,
        currentness=EvidenceCurrentness.CURRENT,
        occupied_support=True,
        source_point_ids=(ordinal,),
        metric_geometry=MetricGeometry(
            coordinate_frame="map",
            centroid_xyz_m=(1.0, 2.0, 0.5),
            range_m=2.2,
            covariance_diagonal_m2=(0.1, 0.1, 0.1),
        ),
        proposal_ids=(proposal_id,) if proposal_id else (),
        semantic_hint="static.trash-bin" if proposal_id else None,
        reason_codes=("test-current-occupied-support",),
    )


def test_profile_is_raw_kb4_and_preserves_false_authority() -> None:
    profile = load_open_vocabulary_semantic_profile(PROFILE_PATH)

    assert profile.coordinate_space == "raw-kb4"
    assert (profile.width, profile.height) == (800, 600)
    assert profile.provider_id == "nvidia-tao-grounding-dino-trt/v1"
    assert profile.model_sha256 == (
        "6895acdc6b588e923f753e37b3bd18869e064256e5ecc1b2b9853e8c51125f94"
    )
    assert tuple(item.prompt_set_id for item in profile.prompt_groups) == (
        "urban-static/v0",
        "urban-agents/v0",
        "urban-vehicles/v0",
    )


def test_tao_parser_keeps_source_coordinates_and_rejects_rectified_boxes(
    tmp_path: Path,
) -> None:
    profile = load_open_vocabulary_semantic_profile(PROFILE_PATH)
    labels = tmp_path / "frame-000121.txt"
    labels.write_text(
        "trash bin 0.00 0 0.00 336.0 123.0 359.0 160.0 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.826\n",
        "utf-8",
    )

    parsed = parse_tao_grounding_dino_labels(
        labels,
        source_id="RAVNOVES00",
        frame_id="frame-000121",
        prompt_set_id="urban-static/v0",
        profile=profile,
    )

    assert len(parsed) == 1
    assert parsed[0].raw_label == "trash bin"
    assert parsed[0].region.as_tuple() == (336.0, 123.0, 359.0, 160.0)
    labels.write_text(
        "trash bin 0.00 0 0.00 336.0 123.0 900.0 160.0 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.826\n",
        "utf-8",
    )
    with pytest.raises(OpenVocabularySemanticError, match="raw image coordinate space"):
        parse_tao_grounding_dino_labels(
            labels,
            source_id="RAVNOVES00",
            frame_id="frame-000121",
            prompt_set_id="urban-static/v0",
            profile=profile,
        )


def test_tao_parser_translates_geometry_crop_back_to_raw_coordinates(tmp_path: Path) -> None:
    profile = load_open_vocabulary_semantic_profile(PROFILE_PATH)
    labels = tmp_path / "frame-000121-geometry-roi-000.txt"
    labels.write_text(
        "dog 0.00 0 0.00 10.0 20.0 50.0 70.0 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.714\n",
        "utf-8",
    )

    parsed = parse_tao_grounding_dino_labels(
        labels,
        source_id="RAVNOVES00",
        frame_id="frame-000121",
        prompt_set_id="urban-agents/v0",
        profile=profile,
        image_width=96,
        image_height=96,
        offset_x=200.0,
        offset_y=150.0,
        detection_scope_id="frame-000121:geometry-roi-000",
    )

    assert parsed[0].detection_id.startswith("frame-000121:geometry-roi-000")
    assert parsed[0].region.as_tuple() == (210.0, 170.0, 250.0, 220.0)


def test_prompt_collisions_fuse_before_geometry_and_background_box_is_removed() -> None:
    profile = load_open_vocabulary_semantic_profile(PROFILE_PATH)
    vocabulary = load_object_semantic_vocabulary(VOCABULARY_PATH)
    detections = (
        _detection("trash", "trash bin", 0.82, (300.0, 100.0, 360.0, 200.0)),
        _detection("cone", "traffic cone", 0.35, (301.0, 101.0, 361.0, 201.0)),
        _detection(
            "dog",
            "dog",
            0.71,
            (100.0, 250.0, 180.0, 340.0),
            prompt_set_id="urban-agents/v0",
        ),
        _detection(
            "background",
            "concrete hemisphere",
            0.66,
            (0.0, 100.0, 800.0, 600.0),
        ),
    )

    result = fuse_open_vocabulary_detections(
        detections,
        profile=profile,
        vocabulary=vocabulary,
        valid_fov_mask=VALID_FOV_MASK,
    )

    assert result.input_detection_count == 4
    assert result.invalid_area_count == 1
    assert len(result.bindings) == 2
    assert sum(len(item.detections) for item in result.bindings) == 3
    trash = next(
        item for item in result.bindings if item.proposal.semantic_hint == "static.trash-bin"
    )
    assert len(trash.detections) == 2
    assert trash.proposal.objectness == pytest.approx(0.82)


def test_binding_keeps_ranked_semantics_separate_from_state_risk_and_occupancy() -> None:
    profile = load_open_vocabulary_semantic_profile(PROFILE_PATH)
    vocabulary = load_object_semantic_vocabulary(VOCABULARY_PATH)
    fusion = fuse_open_vocabulary_detections(
        (
            _detection("trash", "trash bin", 0.82, (300.0, 100.0, 360.0, 200.0)),
            _detection("cone", "traffic cone", 0.35, (301.0, 101.0, 361.0, 201.0)),
        ),
        profile=profile,
        vocabulary=vocabulary,
        valid_fov_mask=VALID_FOV_MASK,
    )
    proposal = fusion.proposals[0]
    observations = (
        _observation(proposal.proposal_id, ordinal=1),
        _observation(None, ordinal=2),
    )

    understandings = bind_object_understandings(
        observations,
        bindings=fusion.bindings,
        profile=profile,
        vocabulary=vocabulary,
        generated_monotonic_ns=121,
    )

    semantic = understandings[0]
    assert tuple(item.class_id for item in semantic.hypotheses) == (
        "static.trash-bin",
        "static.traffic-cone",
    )
    assert semantic.semantic.resolution is SemanticResolution.SELECTED
    assert semantic.semantic.selected_class_id == "static.trash-bin"
    assert semantic.state.agency is AgencyState.INERT
    assert semantic.state.agency_basis is StateBasis.CLASS_PRIOR
    assert semantic.state.motion.value == "unknown"
    assert semantic.risk.level.value == "unknown"
    assert semantic.risk.responses == (AdvisoryResponse.ROUTE_AROUND,)
    assert semantic.authority.navigation_or_safety_accepted is False
    assert semantic.observation.source_point_ids == (1,)
    geometry_only = understandings[1]
    assert geometry_only.semantic.resolution is SemanticResolution.UNRESOLVED
    assert geometry_only.hypotheses == ()
    assert geometry_only.observation.source_point_ids == (2,)


def test_close_semantic_scores_remain_ambiguous() -> None:
    profile = load_open_vocabulary_semantic_profile(PROFILE_PATH)
    vocabulary = load_object_semantic_vocabulary(VOCABULARY_PATH)
    fusion = fuse_open_vocabulary_detections(
        (
            _detection(
                "adult",
                "adult person",
                0.64,
                (100.0, 50.0, 200.0, 350.0),
                prompt_set_id="urban-agents/v0",
            ),
            _detection(
                "child",
                "child",
                0.59,
                (101.0, 51.0, 201.0, 351.0),
                prompt_set_id="urban-agents/v0",
            ),
        ),
        profile=profile,
        vocabulary=vocabulary,
        valid_fov_mask=VALID_FOV_MASK,
    )
    proposal = fusion.proposals[0]

    result = bind_object_understandings(
        (_observation(proposal.proposal_id, ordinal=1),),
        bindings=fusion.bindings,
        profile=profile,
        vocabulary=vocabulary,
        generated_monotonic_ns=121,
    )[0]

    assert result.semantic.resolution is SemanticResolution.AMBIGUOUS
    assert result.semantic.selected_class_id is None
    assert result.state.agency is AgencyState.UNKNOWN
    assert result.state.agency_basis is StateBasis.UNKNOWN