"""Deterministic entry hypotheses, ambiguity and bounded numeric qualification."""

import numpy as np
import pytest

from k1link.missions.entry_acquisition import acquire_entry, choose_entry, entry_seeds
from k1link.missions.registration import angle_deg, transform


def attempts(matrix=None):
    matrix = np.eye(4) if matrix is None else matrix
    return [
        dict(
            index=i,
            along_m=(i // 9 - 1) * 3,
            across_m=(i // 3 % 3 - 1) * 3,
            yaw_deg=(i % 3 - 1) * 15,
            result=dict(
                status="candidate",
                T_reference_query=matrix.tolist(),
                overlap=0.95,
                inlier_rmse_m=0.1,
                matched_query_indices=[0],
                registration_seconds=0.01,
            ),
        )
        for i in range(27)
    ]


def test_seeds_rotate_about_query_entry_and_span_route_basis():
    initial = np.eye(4)
    initial[:3, 3] = [10, 20, 1]
    anchor = np.array([100, 200, 3.0])
    seeds = list(entry_seeds(initial, anchor, [0, 4, 0]))
    assert len(seeds) == 27
    for seed in seeds:
        expected = transform(anchor[None], initial)[0] + [-seed["across_m"], seed["along_m"], 0]
        assert np.allclose(transform(anchor[None], seed["matrix"])[0], expected)
        assert np.linalg.det(seed["matrix"][:3, :3]) == pytest.approx(1)
    with pytest.raises(ValueError):
        list(entry_seeds(initial, anchor, [0, 0, 0]))


def test_single_supported_solution_does_not_relax_local_rejection():
    data = attempts()
    data[0]["result"]["status"] = "rejected"
    result = choose_entry(data, np.eye(4), [0, 0, 0])
    assert result["status"] == "candidate"
    assert result["initialization"]["clusters"][0]["support"] == 26
    assert not result["initialization"]["attempts"][0]["entry_admitted"]


def test_two_near_equal_place_solutions_are_ambiguous_even_with_unequal_support():
    data = attempts()
    alternative = np.eye(4)
    alternative[0, 3] = 2
    data[-1]["result"].update(
        T_reference_query=alternative.tolist(), overlap=0.94, inlier_rmse_m=0.11
    )
    result = choose_entry(data, np.eye(4), [0, 0, 0])
    assert result["status"] == "rejected" and result["reasons"] == ["ambiguous-entry"]
    assert result["matched_query_indices"] == []


def test_cluster_is_pairwise_not_a_chain_between_distant_places():
    data = attempts()
    for i, item in enumerate(data):
        matrix = np.eye(4)
        matrix[0, 3] = (i % 3) * 0.4
        item["result"]["T_reference_query"] = matrix.tolist()
    result = choose_entry(data, np.eye(4), [0, 0, 0])
    assert len(result["initialization"]["clusters"]) == 2
    assert result["reasons"] == ["ambiguous-entry"]


@pytest.mark.parametrize("offset", [[6, 0, 0], [0, 0, 1.1]])
def test_solution_outside_entry_region_is_rejected(offset):
    matrix = np.eye(4)
    matrix[:3, 3] = offset
    result = choose_entry(attempts(matrix), np.eye(4), [0, 0, 0])
    assert result["reasons"] == ["no-admissible-entry"]


def test_angles_and_multiple_translation_starts_required():
    data = attempts()
    for item in data[3:]:
        item["result"]["status"] = "rejected"
    assert choose_entry(data, np.eye(4), [0, 0, 0])["reasons"] == [
        "insufficient-multistart-support"
    ]
    matrix = np.eye(4)
    a = np.radians(31)
    matrix[:2, :2] = [[np.cos(a), -np.sin(a)], [np.sin(a), np.cos(a)]]
    assert choose_entry(attempts(matrix), np.eye(4), [0, 0, 0])["status"] == "rejected"


def test_partial_search_and_deadline_cannot_claim_unique_solution():
    assert choose_entry(attempts(), np.eye(4), [0, 0, 0], complete=False)["reasons"] == [
        "incomplete-search"
    ]
    times = iter([0, 1, 26, 26])
    points = np.random.default_rng(1).normal(size=(400, 3))
    output = acquire_entry(
        points,
        points,
        np.eye(4),
        [0, 0, 0],
        [1, 0, 0],
        fitter=lambda *args: attempts()[0]["result"],
        clock=lambda: next(times),
    )
    assert len(output["initialization"]["attempts"]) == 1
    assert output["reasons"] == ["incomplete-search"]


def test_recovers_known_transform_from_several_starts():
    pytest.importorskip("small_gicp")
    from test_mission_registration import geometry

    ref = geometry()
    truth = np.eye(4)
    a = np.radians(10)
    truth[:2, :2] = [[np.cos(a), -np.sin(a)], [np.sin(a), np.cos(a)]]
    truth[:3, 3] = [0.5, 2.6, 0.1]
    query = transform(ref, np.linalg.inv(truth))
    result = acquire_entry(ref, query, np.eye(4), [0, 0, 0], [1, 0, 0])
    assert result["status"] == "candidate", result["reasons"]
    found = np.asarray(result["T_reference_query"])
    assert np.linalg.norm(found[:3, 3] - truth[:3, 3]) < 0.03
    assert angle_deg(found[:3, :3] @ truth[:3, :3].T) < 0.3
    assert result["policy"]["maximum_correction_m"] == 3
    assert not result["localization_confirmed"] and not result["vehicle_control"]


def test_centre_first_search_preserves_all_seed_identities():
    from k1link.missions.stationary_entry import STATIONARY_POLICY
    seeds = list(entry_seeds(np.eye(4), [0, 0, 0], [1, 0, 0], policy=STATIONARY_POLICY))
    assert (seeds[0]["along_m"], seeds[0]["across_m"], seeds[0]["yaw_deg"]) == (0, 0, 0)
    assert {s["index"] for s in seeds} == set(range(108))
    assert len({(s["along_m"], s["across_m"], s["yaw_deg"]) for s in seeds}) == 108


def test_prepared_target_is_identical_across_rejected_and_accepted_fits():
    from k1link.missions.registration import PreparedReference, register
    from test_mission_registration import geometry
    ref = geometry()
    prepared = PreparedReference(ref)
    query = ref + [.7, -.5, .2]
    first = prepared.register(query, np.eye(4))
    assert prepared.register(query + [100, 0, 0], np.eye(4))["status"] == "rejected"
    second = prepared.register(query, np.eye(4))
    standalone = register(ref, query, np.eye(4))
    for result in (first, second, standalone):
        result.pop("registration_seconds")
    assert first == second == standalone
    assert first["status"] == "candidate"
    assert np.allclose(np.asarray(first["T_reference_query"])[:3, 3], [-.7, .5, -.2], atol=.02)