"""Small causal fixtures for fault injection and heading-free entry."""

from dataclasses import replace

import numpy as np
import pytest

from k1link.missions.causal_tracking import CausalTracking
from k1link.missions.entry_acquisition import choose_entry, entry_seeds
from k1link.missions.replay_faults import drop_receipts
from k1link.missions.stationary_entry import STATIONARY_POLICY, stationary_prefix
from k1link.sessions.live_planning import PlanningLiveEvent


def test_receipt_fault_keeps_identity_time_and_payload_of_survivors():
    e = PlanningLiveEvent("B", 1, 1, 1_000_000_000, 1, "pose", position=(0, 0, 0))
    events = [
        replace(e, sequence=i + 1, monotonic_ns=int((t + 1) * 1e9))
        for i, t in enumerate([0, 43.9, 44, 45, 46.99, 47, 48])
    ]
    audit = {}
    kept = list(drop_receipts(iter(events), 44, 47, audit))
    assert kept == [events[i] for i in [0, 1, 5, 6]]
    assert all(x is events[x.sequence - 1] for x in kept)
    assert [x["sequence"] for x in audit["dropped"]] == [3, 4, 5]
    for start, end in [(0, 2), (4, 3), (1, 121), (1, float("nan"))]:
        with pytest.raises(ValueError):
            list(drop_receipts(iter(events), start, end, {}))


def stationary_events():
    points = np.random.default_rng(23).uniform([-4, -4, -1], [4, 4, 3], (1500, 3))
    e = PlanningLiveEvent("B", 1, 1, 1_000_000_000, 1, "pose", position=(0, 0, 0))
    for i in range(21):
        t = i * 0.5
        yield replace(
            e, sequence=2 * i + 1, monotonic_ns=int((t + 1) * 1e9), position=(0.001 * i / 20, 0, 0)
        )
        yield replace(
            e, sequence=2 * i + 2, monotonic_ns=int((t + 1.001) * 1e9), kind="points", points=points
        )
    yield replace(e, sequence=100, monotonic_ns=12_000_000_000, position=(999, 999, 999))


def test_stationary_prefix_has_no_future_motion_or_heading():
    path = np.array([[10, 20, 0], [14, 20, 0]])
    sample, initial, basis, meta = stationary_prefix(stationary_events(), path)
    assert meta["maximum_motion_m"] == pytest.approx(0.001)
    assert max(x["time_s"] for x in meta["source_events"]) <= 10
    assert np.allclose(initial[:3, :3], np.eye(3))
    assert np.allclose(initial[:3, 3], [10, 20, 0])
    assert len(sample["path"]) == 1 and len(sample["points"]) >= 300
    assert np.allclose(basis, [4, 0, 0])


def test_stationary_prefix_rejects_motion_even_if_buffer_would_thin_it():
    events = list(stationary_events())
    events[2] = replace(events[2], position=(0.11, 0, 0))
    with pytest.raises(ValueError, match="not stationary"):
        stationary_prefix(events, np.array([[0, 0, 0], [4, 0, 0]]))
    with pytest.raises(ValueError, match="Incomplete"):
        stationary_prefix(events[:2], np.array([[0, 0, 0], [4, 0, 0]]))


def test_stationary_all_yaws_rotate_at_entry_and_require_complete_search():
    anchor = np.array([40.0, 30.0, 2.0])
    initial = np.eye(4)
    initial[:3, 3] = [7, 8, 0]
    seeds = list(entry_seeds(initial, anchor, [1, 0, 0], policy=STATIONARY_POLICY))
    assert len(seeds) == 108 and set(x["yaw_deg"] for x in seeds) == set(range(0, 360, 30))
    attempts = []
    for seed in seeds:
        matrix = seed.pop("matrix")
        assert np.allclose(
            (matrix @ np.r_[anchor, 1])[:3], anchor + [7 + seed["along_m"], 8 + seed["across_m"], 0]
        )
        # All fits converge to the same half-turn solution at the entry anchor.
        final = initial.copy()
        final[:2, :2] = -np.eye(2)
        final[:3, 3] = (initial @ np.r_[anchor, 1])[:3] - final[:3, :3] @ anchor
        attempts.append(
            {
                **seed,
                "result": dict(
                    status="candidate",
                    T_reference_query=final.tolist(),
                    overlap=0.95,
                    inlier_rmse_m=0.1,
                    matched_query_indices=[0],
                ),
            }
        )
    assert (
        choose_entry(attempts, initial, anchor, policy=STATIONARY_POLICY)["status"] == "candidate"
    )
    assert choose_entry(attempts[:27], initial, anchor, policy=STATIONARY_POLICY)["reasons"] == [
        "incomplete-search"
    ]


def test_post_tracking_gap_requires_three_new_segment_windows():
    gate = CausalTracking()
    fit = dict(status="candidate", T_reference_query=np.eye(4).tolist())

    def sample(t, segment):
        return dict(
            monotonic_ns=int(t * 1e9), segment=segment, path=np.array([[0, 0, 0], [25, 0, 0]])
        )

    for t in (30, 35, 40):
        assert gate.accept(fit, sample(t, 0), int((t + 0.2) * 1e9), 0)["accepted"]
    assert gate.state == "tracking"
    gate.tick(44_000_000_000, 1)
    assert gate.state == "lost" and gate.matrix is None
    stale = gate.accept(fit, sample(41, 0), 44_100_000_000, 1)
    assert not stale["accepted"] and gate.streak == 0 and gate.matrix is None
    for i, t in enumerate((45, 50, 55)):
        assert gate.accept(fit, sample(t, 1), int((t + 0.2) * 1e9), 1)["accepted"]
        assert gate.streak == i + 1
        assert gate.state == ("tracking" if i == 2 else "acquiring")
    gate.clear("input-ended")
    assert gate.matrix is None and gate.state == "lost"