from __future__ import annotations

from types import SimpleNamespace

import numpy as np
import pytest
from scipy.spatial.transform import Rotation

from imu_lidar.contracts import ImuSeries
from rtk_imu.rtk_imu_engineering import (
    G0,
    HPR_DIRECT_ANGULAR_SIGMA_RAD,
    ResidualAudit,
    _all_hpr,
    _engineering_gates,
    _enu,
    _height_reference,
    _hpr_angular_sigma_rad,
    _marginal_lever_information,
    _motion_flags,
    _nodes,
    _resample_segments_with_multiplicity,
    _segments,
    solve_engineering_6dof,
)
from rtk_imu.rtk_imu_multisource import UnifiedSession
from rtk_imu.rtk_imu_node_graph import (
    _additive_marginal_lever_information,
    _free_residual,
    _free_sparsity,
    _hpr_sigma,
    build_problem,
    fit_states_at_fixed_lever,
    initial_parameters as node_graph_initial_parameters,
    jacobian_sparsity as node_graph_jacobian_sparsity,
    residual as node_graph_residual,
    solve_free_lever_many,
)
from tools.run_rtk_imu_mechanical_prior_heldout import _gate as heldout_gate
from tools.audit_rtk_imu_innovation_noise import _summarize as innovation_summary
from tools.audit_rtk_imu_propagation_bias_root_cause import _root_report

EARTH_RADIUS_M = 6_378_137.0


def _rows_from_motion(
    session_id: str,
    lever: np.ndarray,
    R_RTK_IMU: np.ndarray,
    rotation_at,
    *,
    duration_s: float = 10.0,
    gga_altitude_offset_m: float | None = None,
) -> UnifiedSession:
    imu_t = np.arange(0.0, duration_s + 0.005, 0.01)
    rotations = Rotation.from_matrix(np.asarray([rotation_at(t) for t in imu_t]))
    matrices = rotations.as_matrix()
    relative = Rotation.from_matrix(np.einsum("nij,njk->nik", matrices[:-1].transpose(0, 2, 1), matrices[1:]))
    gyro = np.empty((imu_t.size, 3))
    gyro[:-1] = relative.as_rotvec() / 0.01
    gyro[-1] = gyro[-2]
    accel = np.einsum("nji,j->ni", matrices, np.array([0.0, 0.0, G0]))

    hpr_t = np.arange(0.0, duration_s + 0.001, 0.1)
    rows_hpr: list[dict[str, str]] = []
    baseline_I = R_RTK_IMU.T[:, 0]
    for t in hpr_t:
        baseline = rotation_at(t) @ baseline_I
        heading = np.degrees(np.arctan2(baseline[0], baseline[1])) % 360.0
        pitch = np.degrees(np.arcsin(np.clip(baseline[2], -1.0, 1.0)))
        rows_hpr.append({
            "checksum_valid": "1", "heading_quality": "4", "t_device_s": str(t),
            "heading_deg": str(heading), "pitch_deg": str(pitch),
        })

    rows_best: list[dict[str, str]] = []
    rows_gga: list[dict[str, str]] = []
    best_t = np.arange(0.0, duration_s + 0.001, 0.5)
    dt_velocity = 1e-3
    for t in best_t:
        R_WI = rotation_at(t)
        position = R_WI @ lever
        before = rotation_at(max(t - dt_velocity, 0.0)) @ lever
        after = rotation_at(min(t + dt_velocity, duration_s)) @ lever
        denominator = min(t + dt_velocity, duration_s) - max(t - dt_velocity, 0.0)
        velocity = (after - before) / denominator
        lat = position[1] / EARTH_RADIUS_M * 180.0 / np.pi
        lon = position[0] / EARTH_RADIUS_M * 180.0 / np.pi
        rows_best.append({
            "checksum_valid": "1", "position_fixed": "1", "t_device_s": str(t),
            "lat_deg": str(lat), "lon_deg": str(lon), "altitude_m": str(50.0 + position[2]),
            "doppler_velocity_valid": "1", "velocity_east_m_s": str(velocity[0]),
            "velocity_north_m_s": str(velocity[1]), "vertical_speed_m_s": str(velocity[2]),
        })
        if gga_altitude_offset_m is not None:
            rows_gga.append({
                "checksum_valid": "1", "fix_quality": "4", "t_device_s": str(t),
                "lat_deg": str(lat), "lon_deg": str(lon),
                "altitude_msl_m": str(50.0 + position[2] + gga_altitude_offset_m),
            })

    rtk = {"BESTNAVA": rows_best, "GNHPR": rows_hpr}
    if rows_gga:
        rtk["GGA"] = rows_gga
    return UnifiedSession(
        session_id=session_id,
        batch_id="synthetic",
        imu=ImuSeries(t_s=imu_t, gyro_rad_s=gyro, acc_m_s2=accel),
        imu_rpy_deg=np.zeros((imu_t.size, 3)),
        imu_quaternion_wxyz=np.tile([1.0, 0.0, 0.0, 0.0], (imu_t.size, 1)),
        imu_host_receive_utc_s=imu_t,
        rtk_by_type=rtk,
    )


def _constant_velocity_session(speed_m_s: float = 3.0) -> UnifiedSession:
    imu_t = np.arange(0.0, 5.01, 0.01)
    rows_best = []
    rows_hpr = []
    for t in np.arange(0.0, 5.01, 0.1):
        rows_hpr.append({
            "checksum_valid": "1", "heading_quality": "4", "t_device_s": str(t),
            "heading_deg": "90", "pitch_deg": "0",
        })
    for t in np.arange(0.0, 5.01, 0.5):
        rows_best.append({
            "checksum_valid": "1", "position_fixed": "1", "t_device_s": str(t),
            "lat_deg": "0", "lon_deg": str(speed_m_s * t / EARTH_RADIUS_M * 180.0 / np.pi),
            "altitude_m": "50", "doppler_velocity_valid": "1",
            "velocity_east_m_s": str(speed_m_s), "velocity_north_m_s": "0",
            "vertical_speed_m_s": "0",
        })
    return UnifiedSession(
        session_id="constant_velocity", batch_id="synthetic",
        imu=ImuSeries(
            t_s=imu_t, gyro_rad_s=np.zeros((imu_t.size, 3)),
            acc_m_s2=np.tile([0.0, 0.0, G0], (imu_t.size, 1)),
        ),
        imu_rpy_deg=np.zeros((imu_t.size, 3)),
        imu_quaternion_wxyz=np.tile([1.0, 0.0, 0.0, 0.0], (imu_t.size, 1)),
        imu_host_receive_utc_s=imu_t,
        rtk_by_type={"BESTNAVA": rows_best, "GNHPR": rows_hpr},
    )


def _audit(values: list[float]) -> ResidualAudit:
    vector = np.asarray(values, dtype=float)
    return ResidualAudit(20, vector, vector, float(np.linalg.norm(vector)), float(np.linalg.norm(vector)))


def test_constant_speed_straight_is_gravity_candidate_but_not_zupt() -> None:
    session = _constant_velocity_session()
    times = np.asarray([float(row["t_device_s"]) for row in session.rtk_by_type["BESTNAVA"]])
    velocities = np.asarray([[3.0, 0.0, 0.0]] * len(times))
    gravity_candidate, zupt_static = _motion_flags(session, 2.5, times, velocities)
    assert gravity_candidate
    assert not zupt_static


def test_bestnava_is_not_suppressed_by_earlier_gga_epochs() -> None:
    session = _rows_from_motion(
        "best_preferred", np.array([0.3, -0.2, 0.1]), np.eye(3),
        lambda _: np.eye(3), gga_altitude_offset_m=100.0,
    )
    for row in session.rtk_by_type["GGA"]:
        row["t_device_s"] = str(float(row["t_device_s"]) - 0.05)
    reference = _height_reference([session])
    assert reference is not None
    nodes = _nodes(session, reference, 0.5)
    assert nodes and all(node.source == "BESTNAVA" for node in nodes)
    assert all(node.velocity_enu_m_s is not None for node in nodes)

def test_gga_msl_altitude_never_enters_bestnava_z_reference() -> None:
    lever = np.array([0.3, -0.2, 0.4])
    session = _rows_from_motion("mixed_height", lever, np.eye(3), lambda _: np.eye(3),
                                gga_altitude_offset_m=123.0)
    reference = _height_reference([session])
    assert reference is not None and reference[2] == pytest.approx(50.4)
    best, best_mask = _enu(session.rtk_by_type["BESTNAVA"][1], "BESTNAVA", reference)
    gga, gga_mask = _enu(session.rtk_by_type["GGA"][1], "GGA", reference)
    assert best_mask.tolist() == [True, True, True]
    assert gga_mask.tolist() == [True, True, False]
    assert best[2] == pytest.approx(0.0)
    assert gga[2] == pytest.approx(0.0)


def test_schur_observability_uses_marginal_lever_information() -> None:
    J_l = np.array([
        [1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 0.1],
        [1.0, 1.0, 0.0], [1.0, 0.0, 0.0],
    ])
    J_n = np.array([[1.0], [0.0], [0.0], [1.0], [0.0]])
    J = np.column_stack([J_l, J_n])
    marginal, singular, condition, rank, weakest, covariance = _marginal_lever_information(J, np.ones(4))
    H = J.T @ J
    expected = H[:3, :3] - H[:3, 3:] @ np.linalg.pinv(H[3:, 3:]) @ H[3:, :3]
    assert np.allclose(marginal, expected)
    assert singular.shape == (3,) and rank == 3 and np.isfinite(condition)
    assert abs(weakest[2]) > 0.99
    assert covariance.shape == (3, 3)


def test_bootstrap_resampling_preserves_session_multiplicity() -> None:
    segments = [SimpleNamespace(session_id="a"), SimpleNamespace(session_id="b")]
    sampled = _resample_segments_with_multiplicity(segments, ["a", "a", "b"])
    assert [segment.session_id for segment in sampled] == ["a", "a", "b"]


@pytest.mark.parametrize("fault", ["q5", "time_gap", "baseline_jump"])
def test_isolated_hpr_faults_do_not_split_r0_trajectory(fault: str) -> None:
    session = _constant_velocity_session(0.0)
    if fault == "q5":
        session.rtk_by_type["GNHPR"][25]["heading_quality"] = "5"
    elif fault == "time_gap":
        session.rtk_by_type["GNHPR"] = [
            row for row in session.rtk_by_type["GNHPR"]
            if not 2.1 <= float(row["t_device_s"]) <= 2.9
        ]
    else:
        session.rtk_by_type["GNHPR"][25]["heading_deg"] = "270"
    reference = _height_reference([session])
    assert reference is not None
    nodes = _nodes(session, reference, 0.5)
    assert nodes
    assert all(
        right.continuity_id == left.continuity_id
        for left, right in zip(nodes[:-1], nodes[1:])
    )
    if fault == "baseline_jump":
        assert any(not node.hpr_factor_valid and node.hpr_factor_method == "isolated_outlier" for node in nodes)


def test_hpr_bridge_covariance_is_weaker_than_direct_and_limited_to_half_second() -> None:
    assert _hpr_angular_sigma_rad("nearest_q4", 0.0) == pytest.approx(HPR_DIRECT_ANGULAR_SIGMA_RAD)
    assert _hpr_angular_sigma_rad("bracket_interpolation", 0.2) > HPR_DIRECT_ANGULAR_SIGMA_RAD
    assert _hpr_angular_sigma_rad("bracket_interpolation", 0.5) >= _hpr_angular_sigma_rad(
        "bracket_interpolation", 0.2
    )
    assert np.isinf(_hpr_angular_sigma_rad("bracket_interpolation", 0.500001))

def test_short_hpr_dropout_uses_interpolated_factor_without_splitting_r0() -> None:
    session = _constant_velocity_session(0.0)
    session.rtk_by_type["GNHPR"] = [
        row for row in session.rtk_by_type["GNHPR"]
        if not 2.4 <= float(row["t_device_s"]) <= 2.6
    ]
    reference = _height_reference([session])
    assert reference is not None
    nodes = _nodes(session, reference, 0.5)
    bridged = [node for node in nodes if abs(node.t_s - 2.5) < 1e-9]
    assert len(bridged) == 1
    assert bridged[0].hpr_factor_valid
    assert bridged[0].hpr_factor_method == "bracket_interpolation"
    assert all(right.continuity_id == left.continuity_id for left, right in zip(nodes[:-1], nodes[1:]))

def test_position_time_backwards_still_splits_r0_trajectory() -> None:
    session = _constant_velocity_session(0.0)
    session.rtk_by_type["BESTNAVA"][5]["t_device_s"] = "1.75"
    reference = _height_reference([session])
    assert reference is not None
    nodes = _nodes(session, reference, 0.5)
    assert any(
        right.continuity_id != left.continuity_id
        for left, right in zip(nodes[:-1], nodes[1:])
    )


def test_one_hz_timestamp_jitter_does_not_create_artificial_two_second_r0_gaps() -> None:
    session = _constant_velocity_session(0.0)
    rows = session.rtk_by_type["BESTNAVA"]
    # Retain strict device-time monotonicity while making every second sample early.
    for index, row in enumerate(rows):
        row["t_device_s"] = str(float(row["t_device_s"]) - (0.002 if index % 2 else 0.0))
    reference = _height_reference([session])
    assert reference is not None
    nodes = _nodes(session, reference, 1.0)
    intervals = np.diff([node.t_s for node in nodes])
    assert len(nodes) == 6
    assert np.all(intervals > 0.75)
    assert np.all(intervals < 1.25)
    assert all(right.continuity_id == left.continuity_id for left, right in zip(nodes[:-1], nodes[1:]))

def test_large_residuals_fail_engineering_acceptance() -> None:
    gates = _engineering_gates(
        np.array([0.01, 0.01, 0.01]), np.array([100.0, 10.0, 1.0]), 3, 100.0,
        _audit([1.0, 1.0]), _audit([1.0, 1.0, 1.0]), _audit([2.0, 2.0, 2.0]),
        {"a": 0.01, "b": 0.02, "c": 0.03}, np.array([0.01, 0.01, 0.01]), True,
        0.01, True, None, False, None,
    )
    assert not gates["gga_xy_rms_p95"]
    assert not gates["bestnava_xyz_rms_p95"]
    assert not gates["doppler_velocity_rms_p95"]
    assert not all(gates.values())


def test_unobservable_free_solution_does_not_block_mechanical_prior_with_euclidean_delta() -> None:
    gates = _engineering_gates(
        np.array([0.01, 0.01, 0.01]), np.array([100.0, 10.0, 1.0]), 3, 100.0,
        _audit([0.01, 0.01]), _audit([0.01, 0.01, 0.01]), _audit([0.01, 0.01, 0.01]),
        {"a": 0.01, "b": 0.02, "c": 0.03}, np.array([0.01, 0.01, 0.01]), True,
        0.01, True, np.array([10.0, 10.0, 10.0]), False, None,
    )
    assert gates["manual_lever_consistency"] is False
    assert gates["free_manual_mahalanobis_consistency"] is True
    observable_gates = _engineering_gates(
        np.array([0.01, 0.01, 0.01]), np.array([100.0, 10.0, 1.0]), 3, 100.0,
        _audit([0.01, 0.01]), _audit([0.01, 0.01, 0.01]), _audit([0.01, 0.01, 0.01]),
        {"a": 0.01, "b": 0.02, "c": 0.03}, np.array([0.01, 0.01, 0.01]), True,
        0.01, True, None, True, 12.0,
    )
    assert observable_gates["free_manual_mahalanobis_consistency"] is False
def test_yaw_only_recovers_xy_but_weak_z_is_rejected_and_transforms_are_inverse() -> None:
    lever = np.array([0.30, -0.20, 0.10])
    session = _rows_from_motion(
        "yaw", lever, np.eye(3), lambda t: Rotation.from_euler("z", 0.20 * t).as_matrix()
    )
    result = solve_engineering_6dof(
        [session], R_RTK_IMU=np.eye(3), sample_period_s=0.5,
        run_loo=False, run_bootstrap=False, run_rotation_sensitivity=False,
    )
    assert result.l_I_m is not None
    assert np.allclose(result.l_I_m[:2], lever[:2], atol=0.05)
    assert result.lever_precision_rank < 3 or not result.engineering_acceptance_gates["lever_marginal_std"]
    assert not result.engineering_6dof_accepted
    assert result.T_RTK_IMU is not None and result.T_IMU_RTK is not None
    assert np.allclose(result.T_RTK_IMU @ result.T_IMU_RTK, np.eye(4), atol=1e-8)


def test_pitch_excitation_recovers_vertical_lever_arm() -> None:
    lever = np.array([0.28, -0.16, 0.42])
    rotation_at = lambda t: Rotation.from_euler(
        "xyz", [0.0, 0.12 * np.sin(0.55 * t), 0.16 * t]
    ).as_matrix()
    session = _rows_from_motion("pitch", lever, np.eye(3), rotation_at)
    result = solve_engineering_6dof(
        [session], R_RTK_IMU=np.eye(3), sample_period_s=0.5,
        run_loo=False, run_bootstrap=False, run_rotation_sensitivity=False,
    )
    assert result.l_I_m is not None
    assert result.l_I_m[2] == pytest.approx(lever[2], abs=0.08)


def test_full_3d_excitation_with_nonzero_fixed_r2g_recovers_l_i() -> None:
    lever = np.array([0.24, -0.18, 0.36])
    fixed_rotation = Rotation.from_euler("xyz", [0.454, -0.003, 0.012], degrees=True).as_matrix()
    rotation_at = lambda t: Rotation.from_euler(
        "xyz", [0.16 * np.sin(0.37 * t), 0.18 * np.sin(0.51 * t), 0.18 * t]
    ).as_matrix()
    session = _rows_from_motion(
        "full_3d", lever, fixed_rotation, rotation_at, duration_s=15.0
    )
    result = solve_engineering_6dof(
        [session], R_RTK_IMU=fixed_rotation, sample_period_s=0.5,
        run_loo=False, run_bootstrap=False, run_rotation_sensitivity=False,
    )
    assert result.l_I_m is not None
    assert np.allclose(result.l_I_m, lever, atol=0.08)
    assert np.allclose(result.R_RTK_IMU, fixed_rotation)
    assert result.T_RTK_IMU is not None and result.T_IMU_RTK is not None
    assert np.allclose(result.T_RTK_IMU @ result.T_IMU_RTK, np.eye(4), atol=1e-8)

def test_mechanical_soft_prior_keeps_free_solution_and_reports_prior_solution() -> None:
    lever = np.array([0.24, -0.18, 0.36])
    reference = np.array([0.30, -0.18, 0.36])
    rotation_at = lambda t: Rotation.from_euler(
        "xyz", [0.14 * np.sin(0.37 * t), 0.16 * np.sin(0.51 * t), 0.18 * t]
    ).as_matrix()
    session = _rows_from_motion("prior", lever, np.eye(3), rotation_at, duration_s=12.0)
    result = solve_engineering_6dof(
        [session], R_RTK_IMU=np.eye(3),
        manual_l_I_m=reference,
        manual_l_I_covariance_m2=np.diag([0.02**2, 0.02**2, 0.02**2]),
        sample_period_s=0.5, run_loo=False, run_bootstrap=False,
        run_rotation_sensitivity=False,
    )
    assert result.free_solution is not None
    assert result.prior_constrained_solution is not None
    assert result.translation_prior_applied
    assert np.allclose(result.mechanical_reference_l_I_m, reference)
    assert np.linalg.norm(result.prior_to_mechanical_delta_m) < np.linalg.norm(result.free_to_mechanical_delta_m)
    assert np.allclose(result.l_I_m, result.prior_constrained_solution.l_I_m)
    assert result.mechanical_reference_solution is not None
    assert result.residual_comparison is not None
    assert result.posterior_to_prior_covariance_ratio is not None


def test_manual_mean_and_covariance_must_be_provided_together() -> None:
    session = _constant_velocity_session(0.0)
    with pytest.raises(ValueError, match="must be provided together"):
        solve_engineering_6dof([session], R_RTK_IMU=np.eye(3), manual_l_I_m=np.zeros(3))


def test_blockwise_schur_information_is_additive_across_independent_segments() -> None:
    rng = np.random.default_rng(7)
    lever_blocks = []
    nuisance_blocks = []
    row_count = 30
    for scale in (1.0, 1e-3, 20.0):
        lever_blocks.append(rng.normal(size=(row_count, 3)) * scale)
        nuisance_blocks.append(rng.normal(size=(row_count, 15)) * scale)
    jacobian = np.zeros((3 * row_count, 3 + 3 * 15))
    for index, (lever, nuisance) in enumerate(zip(lever_blocks, nuisance_blocks)):
        rows = slice(index * row_count, (index + 1) * row_count)
        columns = slice(3 + index * 15, 3 + (index + 1) * 15)
        jacobian[rows, :3] = lever
        jacobian[rows, columns] = nuisance
    residual = np.ones(jacobian.shape[0])
    combined = _marginal_lever_information(jacobian, residual)[0]
    expected = np.zeros((3, 3))
    for lever, nuisance in zip(lever_blocks, nuisance_blocks):
        local = np.column_stack([lever, nuisance])
        expected += _marginal_lever_information(local, np.ones(row_count))[0]
    assert np.allclose(combined, expected, rtol=1e-9, atol=1e-9)


def test_per_node_graph_has_finite_residual_and_matching_sparse_structure() -> None:
    lever = np.array([0.24,-0.18,0.36])
    rotation_at = lambda t: Rotation.from_euler('z',.12*t).as_matrix()
    session = _rows_from_motion('node_graph',lever,np.eye(3),rotation_at,duration_s=10.)
    segments = _segments([session],.5)
    assert segments
    problem = build_problem(segments[0],np.eye(3),lever)
    x0 = node_graph_initial_parameters(problem)
    value = node_graph_residual(problem,x0)
    sparsity = node_graph_jacobian_sparsity(problem,x0)
    assert x0.size == 15*len(problem.segment.nodes)
    assert np.all(np.isfinite(value))
    assert sparsity.shape == (value.size,x0.size)
    assert sparsity.nnz > value.size


def test_node_graph_hpr_override_retains_bridge_extra_variance() -> None:
    problem=SimpleNamespace(hpr_direct_angular_sigma_rad=.006)
    direct=SimpleNamespace(hpr_angular_sigma_rad=HPR_DIRECT_ANGULAR_SIGMA_RAD)
    assert _hpr_sigma(problem,direct)==pytest.approx(.006)
    old=_hpr_angular_sigma_rad('bracket_interpolation',.4)
    bridge=SimpleNamespace(hpr_angular_sigma_rad=old)
    expected=np.sqrt(.006**2+old**2-HPR_DIRECT_ANGULAR_SIGMA_RAD**2)
    assert _hpr_sigma(problem,bridge)==pytest.approx(expected)


def test_node_graph_free_lever_layout_extends_fixed_state_by_three() -> None:
    lever=np.array([.24,-.18,.36])
    rotation_at=lambda t: Rotation.from_euler('z',.12*t).as_matrix()
    session=_rows_from_motion('node_graph_free',lever,np.eye(3),rotation_at,duration_s=5.)
    problem=build_problem(_segments([session],.5)[0],np.eye(3),lever,.006)
    value=np.concatenate([np.zeros(3),node_graph_initial_parameters(problem,np.zeros(3))])
    residual=_free_residual(problem,value)
    sparsity=_free_sparsity(problem,value)
    assert np.all(np.isfinite(residual))
    assert sparsity.shape==(residual.size,value.size)


def test_node_graph_block_schur_information_is_additive() -> None:
    rng=np.random.default_rng(7); rows=24; nuisance=5
    local=[rng.normal(size=(rows,3+nuisance)) for _ in range(2)]
    global_jac=np.zeros((2*rows,3+2*nuisance))
    global_jac[:rows,:3]=local[0][:,:3]
    global_jac[:rows,3:3+nuisance]=local[0][:,3:]
    global_jac[rows:,:3]=local[1][:,:3]
    global_jac[rows:,3+nuisance:]=local[1][:,3:]
    actual=_additive_marginal_lever_information(
        global_jac,[0,rows,2*rows],[3,3+nuisance,3+2*nuisance])
    expected=np.zeros((3,3))
    for jac in local:
        H=jac.T@jac
        expected+=H[:3,:3]-H[:3,3:]@np.linalg.pinv(H[3:,3:])@H[3:,:3]
    assert np.allclose(actual,expected,rtol=1e-10,atol=1e-10)


def test_node_graph_soft_lever_prior_adds_exact_information() -> None:
    lever=np.array([.24,-.18,.36])
    rotation_at=lambda t: Rotation.from_euler('z',.12*t).as_matrix()
    session=_rows_from_motion('node_graph_prior',lever,np.eye(3),rotation_at,
                              duration_s=5.)
    problem=build_problem(_segments([session],.5)[0],np.eye(3),lever,.006)
    free=solve_free_lever_many([problem],lever,max_nfev=1)
    covariance=np.diag([.02**2,.02**2,.03**2])
    prior=solve_free_lever_many([problem],lever,max_nfev=1,
        lever_prior_mean_m=lever,lever_prior_covariance_m2=covariance)
    free_information=np.linalg.pinv(free.lever_covariance_m2,rcond=1e-9)
    prior_information=np.linalg.pinv(prior.lever_covariance_m2,rcond=1e-9)
    assert np.allclose(prior_information-free_information,
                       np.linalg.inv(covariance),rtol=1e-6,atol=1e-5)


def test_heldout_gate_does_not_accept_underdispersed_statistics() -> None:
    vector={'count':1,'axis_rms':[0.,0.,0.],'axis_p95_abs':[0.,0.,0.],
            'vector_rms':.01,'vector_p95':.02}
    summary={'optimizer_converged_fraction':1.,
        'global_chi_square_per_dof':.127,
        'best_position_physical_m':vector,
        'doppler_physical_m_s':vector,
        'residual_by_factor':{
            'hpr':{'p95_abs':1.3},
            'imu_preintegration':{'p95_abs':.28}}}
    result=heldout_gate(summary)
    assert not result['passed']
    assert not result['checks']['global_chi_square_per_dof_in_0p25_4']


def test_fixed_lever_retry_starts_from_previous_final_state() -> None:
    lever=np.array([.24,-.18,.36])
    rotation_at=lambda t: Rotation.from_euler('z',.12*t).as_matrix()
    session=_rows_from_motion('node_graph_retry',lever,np.eye(3),rotation_at,
                              duration_s=5.)
    problem=build_problem(_segments([session],.5)[0],np.eye(3),lever,.006)
    state,first=fit_states_at_fixed_lever(problem,lever,max_nfev=1)
    _,retry=fit_states_at_fixed_lever(
        problem,lever,max_nfev=1,initial_state_values=state)
    assert retry['initial_cost']==pytest.approx(first['cost'])


def test_innovation_summary_reports_vector_and_temporal_metrics() -> None:
    records=[{'residual':np.array([float(i),0.,0.]),
              'normalized':np.array([float(i),0.,0.]),
              'normalized_factor_only':np.array([float(i),0.,0.]),
              't_s':float(i)} for i in range(4)]
    result=innovation_summary(records)
    assert result['vector_p95']>result['vector_p50']
    assert result['temporal_linear_drift_per_s'][0]==pytest.approx(1.)
    assert np.asarray(result['empirical_covariance']).shape==(3,3)


def test_propagation_root_report_detects_common_constant_acceleration() -> None:
    acceleration=np.array([.2,-.02,-.01]); position=[]; velocity=[]
    for index,dt in enumerate((.8,1.,1.2,1.4)):
        base={'interval_id':str(index),'session':'s','motion':'m','t_s':index,
              'speed_bin':'slow','gyro_bin':'low','dt_s':dt,'R0_WI':np.eye(3)}
        position.append({**base,'residual':.5*acceleration*dt*dt})
        velocity.append({**base,'residual':acceleration*dt})
    report,_=_root_report(position,velocity)
    assert report['common_constant_acceleration_error_detected']
    assert np.allclose(
        report['overall']['difference_velocity_minus_position_m_s2']['bias'],0.)
