"""Selected-route retrieval is distinct from the old start-neighbourhood search."""

import numpy as np
import pytest

from k1link.missions.route_relocalization import (
    ROUTE_RELOCALIZATION_POLICY,
    ReferenceGrid,
    choose_route_location,
    local_submap,
    rank_route_candidates,
    relocalize_route,
    relocalize_start_then_route,
)
from k1link.missions.stationary_bootstrap import BOOTSTRAP_POLICY, StationaryBootstrap


def scene():
    """Three deliberately different local places along one 90-m route."""
    rng = np.random.default_rng(92)
    parts = []
    for center, scale in (
        (10.0, (2.0, 0.7, 1.0)),
        (45.0, (0.8, 4.0, 2.0)),
        (80.0, (4.0, 1.2, 0.4)),
    ):
        points = rng.normal(size=(900, 3)) * scale
        points[:, 0] += center
        parts.append(points)
    return np.concatenate(parts), np.array([[0.0, 0.0, 0.0], [90.0, 0.0, 0.0]])


def candidate_result(matrix, *, overlap=0.95, rmse=0.1):
    return dict(
        status="candidate",
        T_reference_query=np.asarray(matrix).tolist(),
        initial_T_reference_query=np.asarray(matrix).tolist(),
        matched_query_indices=[1],
        overlap=overlap,
        inlier_rmse_m=rmse,
        registration_seconds=0.01,
        localization_confirmed=False,
        vehicle_control=False,
    )


def attempt(index, progress, matrix, *, overlap=0.95, rmse=0.1):
    return dict(
        candidate=dict(
            index=index,
            position=[progress, 0, 0],
            progress_m=float(progress),
            descriptor_distance=0.1,
        ),
        yaw_deg=0.0,
        result=candidate_result(matrix, overlap=overlap, rmse=rmse),
    )


def test_descriptor_retrieval_covers_full_selected_route_not_its_start():
    reference, path = scene()
    # The stationary K1 cloud is at the middle place in another local SLAM frame.
    query = reference[(reference[:, 0] > 37) & (reference[:, 0] < 53)] - [45.0, 3.0, 0.0]
    ranked, coverage = rank_route_candidates(reference, path, query)

    assert coverage["descriptor_scope"] == "entire-selected-route"
    assert coverage["route_anchor_count"] > 10
    assert ranked
    assert len(ranked) == coverage["descriptor_covered_anchor_count"]
    assert len(ranked) > ROUTE_RELOCALIZATION_POLICY["candidate_batch_size"]
    assert abs(ranked[0].progress_m - 45.0) <= ROUTE_RELOCALIZATION_POLICY["anchor_spacing_m"]


def test_full_route_atlas_is_not_rejected_by_one_gicp_target_cap():
    reference, _ = scene()
    full_route = np.tile(reference, (40, 1))  # 108,000 map points, not one GICP target.
    atlas = ReferenceGrid(full_route)
    target = local_submap(atlas, [45.0, 0.0, 0.0], 28.0, maximum_points=100_000)

    assert len(full_route) > 100_000
    assert 300 <= len(target) <= 100_000


def test_dense_eighty_metre_submap_keeps_its_footprint_under_the_gicp_budget():
    x, y = np.meshgrid(np.arange(-40.0, 40.0, 0.2), np.arange(-40.0, 40.0, 0.2))
    reference = np.column_stack((x.ravel(), y.ravel(), np.zeros(x.size)))
    target = local_submap(reference, [0.0, 0.0, 0.0], 80.0, maximum_points=1_000)

    assert 300 <= len(target) <= 1_000


def test_middle_of_route_relocalisation_qualifies_with_real_gicp():
    pytest.importorskip("small_gicp")
    reference, path = scene()
    query = reference[(reference[:, 0] > 37) & (reference[:, 0] < 53)] - [45.0, 3.0, 0.0]
    output = relocalize_route(reference, path, query, [0.0, 0.0, 0.0])

    assert output["status"] == "candidate", output["reasons"]
    assert output["initialization"]["complete"]
    assert abs(output["initialization"]["selected_route_progress_m"] - 45.0) <= 5.0
    assert output["overlap"] > 0.9 and output["inlier_rmse_m"] < 0.05
    assert not output["localization_confirmed"] and not output["vehicle_control"]


def test_hybrid_accepts_the_dense_start_before_route_retrieval(monkeypatch):
    """A normal start must not be degraded by global target voxelisation."""
    import k1link.missions.route_relocalization as module

    reference, path = scene()
    query = reference[:700]
    local = candidate_result(np.eye(4))
    local["initialization"] = dict(complete=True, reason=None, attempts=[{}] * 108)
    monkeypatch.setattr(module, "acquire_entry", lambda *args, **kwargs: dict(local))

    def unexpected_route(*args, **kwargs):
        raise AssertionError("A qualified dense start must not enter route recovery.")

    monkeypatch.setattr(module, "relocalize_route", unexpected_route)
    output = relocalize_start_then_route(reference, path, query, [0.0, 0.0, 0.0])

    assert output["status"] == "candidate"
    assert output["initialization"]["strategy"] == "dense-start-first-then-route-recovery/v1"
    assert output["initialization"]["expected_attempts"] == 108
    assert output["initialization"]["stages"][0]["name"] == "dense-start"


def test_hybrid_records_a_route_recovery_only_after_a_dense_start_rejection(monkeypatch):
    import k1link.missions.route_relocalization as module

    reference, path = scene()
    query = reference[:700]
    local = candidate_result(np.eye(4))
    local.update(status="rejected", reasons=["no-admissible-entry"])
    local["initialization"] = dict(complete=True, reason="no-admissible-entry", attempts=[{}])
    recovered = candidate_result(np.eye(4))
    recovered["initialization"] = dict(
        complete=True,
        reason=None,
        attempts=[{}],
        selected_candidate_index=4,
        selected_route_progress_m=20.0,
    )
    monkeypatch.setattr(module, "acquire_entry", lambda *args, **kwargs: dict(local))
    monkeypatch.setattr(module, "relocalize_route", lambda *args, **kwargs: dict(recovered))

    output = relocalize_start_then_route(reference, path, query, [0.0, 0.0, 0.0])

    assert output["status"] == "candidate"
    assert [stage["name"] for stage in output["initialization"]["stages"]] == [
        "dense-start",
        "route-recovery",
    ]
    assert output["initialization"]["expected_attempts"] == 2


def test_equally_good_distinct_route_places_are_rejected_as_ambiguous():
    first, second = np.eye(4), np.eye(4)
    second[0, 3] = 50.0
    output = choose_route_location(
        [attempt(0, 0.0, first), attempt(10, 50.0, second, overlap=0.94, rmse=0.11)],
        [0.0, 0.0, 0.0],
        complete=True,
    )

    assert output["status"] == "rejected"
    assert output["reasons"] == ["ambiguous-route-location"]
    assert output["matched_query_indices"] == []
    assert not output["localization_confirmed"] and not output["vehicle_control"]


def test_no_qualified_candidate_is_a_route_location_failure_not_a_fake_prior():
    output = choose_route_location([], [0.0, 0.0, 0.0], complete=True)

    assert output["status"] == "rejected"
    assert output["reasons"] == ["no-route-location"]
    assert output["initialization"]["complete"]
    assert output["initialization"]["expected_attempts"] == 0


def test_global_result_must_account_for_every_generated_hypothesis_before_prior():
    boot = StationaryBootstrap(
        np.array([[0.0, 0, 0], [40.0, 0, 0]]),
        initialization_policy=ROUTE_RELOCALIZATION_POLICY,
    )
    # Exercise the policy contract directly; no old 108-seed local-start
    # assumption is permitted for whole-route retrieval.
    initialization = dict(
        policy=ROUTE_RELOCALIZATION_POLICY,
        scope="selected-route",
        complete=True,
        expected_attempts=2,
        attempts=[{}, {}],
    )
    boot.phase = "searching"
    boot.initialization_sample = dict(monotonic_ns=1, segment=0)
    boot.search_started_ns = 1
    accepted = boot.offer_prior(
        dict(
            status="candidate",
            T_reference_query=np.eye(4).tolist(),
            initialization=initialization,
        ),
        1_000_000_000,
        0,
    )
    assert accepted["provisional"]

    second = StationaryBootstrap(
        np.array([[0.0, 0, 0], [40.0, 0, 0]]),
        initialization_policy=ROUTE_RELOCALIZATION_POLICY,
    )
    second.phase = "searching"
    second.initialization_sample = dict(monotonic_ns=1, segment=0)
    second.search_started_ns = 1
    initialization["expected_attempts"] = 3
    rejected = second.offer_prior(
        dict(
            status="candidate",
            T_reference_query=np.eye(4).tolist(),
            initialization=initialization,
        ),
        1_000_000_000,
        0,
    )
    assert not rejected["provisional"] and rejected["reason"] == "initialization-incomplete"


def test_global_ambiguity_has_a_specific_operator_message():
    from k1link.missions.stationary_live import phase_message

    boot = StationaryBootstrap(
        np.array([[0.0, 0, 0], [40.0, 0, 0]]),
        initialization_policy=ROUTE_RELOCALIZATION_POLICY,
    )
    boot.phase, boot.reason = "lost", "initialization-ambiguous"
    assert "несколько похожих участков" in phase_message(boot)


def test_route_search_deadline_cannot_outlive_the_stationary_prefix_freshness_fence():
    assert (
        ROUTE_RELOCALIZATION_POLICY["deadline_s"] < BOOTSTRAP_POLICY["maximum_prior_source_age_s"]
    )
    assert (
        ROUTE_RELOCALIZATION_POLICY["maximum_search_wall_s"]
        <= BOOTSTRAP_POLICY["maximum_prior_source_age_s"]
    )


def test_precise_search_reaches_the_last_ranked_anchor(monkeypatch):
    import k1link.missions.route_relocalization as module

    reference, path = scene()
    query = reference[:700]
    ranked, _ = rank_route_candidates(reference, path, query)
    calls = []
    prepared_targets = []
    original_prepare = module.PreparedReference.__init__

    def prepare(self, target):
        prepared_targets.append(target)
        original_prepare(self, target)

    def register(self, query, initial, *, policy):
        calls.append(policy)
        result = candidate_result(np.eye(4))
        if len(calls) < (len(ranked) - 1) * 3 + 1:
            result.update(status="rejected", reasons=["fixture mismatch"])
        return result

    monkeypatch.setattr(module.PreparedReference, "register", register)
    monkeypatch.setattr(module.PreparedReference, "__init__", prepare)
    result = relocalize_route(reference, path, query, [0, 0, 0])
    info = result["initialization"]
    assert result["status"] == "candidate"
    assert info["selected_candidate_index"] == ranked[-1].index
    assert len(calls) == info["expected_attempts"] == len(ranked) * 3
    assert len(prepared_targets) == len(ranked)
    assert not info["remaining_candidate_indices"]
    assert len(info["candidate_batches"]) > 1
    assert all(policy == ROUTE_RELOCALIZATION_POLICY["registration_policy"] for policy in calls)


def test_budget_exhaustion_is_not_a_completed_negative_or_provisional_fit(monkeypatch):
    import k1link.missions.route_relocalization as module

    reference, path = scene()
    elapsed = [0.0]

    def register(self, query, initial, *, policy):
        elapsed[0] += 12
        return candidate_result(np.eye(4))

    monkeypatch.setattr(module.PreparedReference, "register", register)
    result = relocalize_route(reference, path, reference[:700], [0, 0, 0],
                              clock=lambda: elapsed[0])
    info = result["initialization"]
    assert result["reasons"] == ["incomplete-route-search"]
    assert not info["complete"] and not info["candidate_queue_exhausted"]
    assert info["remaining_candidate_indices"]
    assert info["expected_attempts"] > len(info["attempts"])
    assert info["candidate_queue"] == []


def test_ambiguity_uses_fitted_places_even_when_retrieval_anchor_is_same():
    first, second = np.eye(4), np.eye(4)
    second[0, 3] = 40
    result = choose_route_location([attempt(0, 0, first), attempt(0, 0, second)],
                                    [0, 0, 0], complete=True)
    assert result["reasons"] == ["ambiguous-route-location"]


def test_fresh_confirmation_fallback_skips_dense_search(monkeypatch):
    import k1link.missions.route_relocalization as module

    reference, path = scene()
    def no_dense(*args, **kwargs):
        raise AssertionError("Already disproved dense-start prior must not repeat.")
    monkeypatch.setattr(module, "acquire_entry", no_dense)
    monkeypatch.setattr(module, "relocalize_route", lambda *a, **k: {"route_only": True})
    assert relocalize_start_then_route(reference, path, reference[:700], [0, 0, 0],
                                      route_only=True) == {"route_only": True}