"""Session-aware Phase-A rotation/bias comparison and observability analysis."""

from __future__ import annotations

from collections import Counter
from collections.abc import Callable, Mapping
from dataclasses import dataclass, field, replace
from typing import Any

import numpy as np
from scipy.optimize import least_squares
from scipy.spatial.transform import Rotation

from .contracts import ImuSeries, MotionPair
from .geometry import (
    orthonormalize_rotation,
    rotation_angle_deg,
    rpy_deg_xyz,
    so3_exp,
    so3_log,
)
from .imu_preintegration import (
    apply_bias_jacobian_correction,
    preintegrate_gyro,
    preintegration_rotation_residual,
    residual_whiten_matrix,
)
from .rotation_handeye import (
    estimate_rotation_handeye_initial,
    select_strong_rotation_pairs,
)


@dataclass(frozen=True)
class PhaseASessionDiagnostics:
    session_id: str
    pair_count: int
    gyro_bias0_rad_s: np.ndarray
    gyro_bias_rad_s: np.ndarray
    residual_rms_deg: float
    residual_median_deg: float
    residual_p95_deg: float
    outlier_fraction_gt_5deg: float
    accepted: bool
    included_in_final: bool = True


@dataclass(frozen=True)
class PhaseAVariantResult:
    name: str
    R_IMU_lidar: np.ndarray
    rpy_deg_xyz: np.ndarray
    gyro_bias_rad_s_per_session: dict[str, np.ndarray]
    sessions: tuple[PhaseASessionDiagnostics, ...]
    residual_rms_deg: float
    residual_median_deg: float
    residual_p95_deg: float
    outlier_fraction_gt_5deg: float
    accepted: bool
    rotation_prior_used: bool
    optimizer_success: bool
    optimizer_nfev: int
    cost: float
    residual_count: int
    parameter_count: int
    jacobian: np.ndarray = field(repr=False)


@dataclass(frozen=True)
class PhaseAMarginalObservability:
    rank: int
    condition: float
    eigenvalues: np.ndarray
    weakest_direction_left_tangent: np.ndarray
    covariance_rotvec_rad2: np.ndarray
    yaw_std_deg: float
    direction_std_deg: np.ndarray
    precision_rank: int
    residual_variance_scale: float
    hessian_marginal: np.ndarray
    ok: bool
    notes: tuple[str, ...] = ()


@dataclass(frozen=True)
class PhaseALeaveOneOutResult:
    held_out_session: str
    data_only_rpy_deg_xyz: np.ndarray
    prior_rpy_deg_xyz: np.ndarray
    data_only_gyro_bias_rad_s_per_session: dict[str, np.ndarray]
    prior_gyro_bias_rad_s_per_session: dict[str, np.ndarray]
    data_only_training_rms_deg: float
    data_only_training_p95_deg: float
    prior_training_rms_deg: float
    prior_training_p95_deg: float
    held_out_static_bg_rms_deg: float
    held_out_static_bg_p95_deg: float
    held_out_fitted_bg_rms_deg: float
    held_out_fitted_bg_p95_deg: float
    held_out_fitted_bg_rad_s: np.ndarray


@dataclass(frozen=True)
class PhaseAPriorSensitivityResult:
    sigma_deg: float
    rpy_deg_xyz: np.ndarray
    rotation_difference_from_data_deg: float
    residual_rms_deg: float
    residual_p95_deg: float


@dataclass(frozen=True)
class PhaseAComparisonResult:
    fixed_bg_data_only: PhaseAVariantResult
    session_bg_data_only: PhaseAVariantResult
    session_bg_with_rotation_prior: PhaseAVariantResult
    observable_subspace_with_prior: PhaseAVariantResult | None
    marginal_observability: PhaseAMarginalObservability
    leave_one_out: tuple[PhaseALeaveOneOutResult, ...]
    strong_pair_counts_per_session: dict[str, int]
    excluded_sessions: tuple[str, ...]
    data_vs_prior_yaw_diff_deg: float
    data_vs_prior_geodesic_deg: float
    leave_one_out_yaw_range_deg: float
    leave_one_out_observable_max_deg: float
    prior_sensitivity: tuple[PhaseAPriorSensitivityResult, ...]
    strong_pair_candidate_count: int
    decorrelated_pair_count: int
    decorrelation_block_s: float
    solution_status: str
    recommended_result: str
    partial_accepted: bool
    accepted: bool
    acceptance_checks: dict[str, bool]
    notes: tuple[str, ...] = ()


ProgressCallback = Callable[[str, dict[str, Any]], None]


def _emit(callback: ProgressCallback | None, event: str, **fields: Any) -> None:
    if callback is not None:
        callback(event, fields)


def _pair_cov(pair: MotionPair) -> np.ndarray:
    raw = pair.metadata.get("cov")
    if raw is not None:
        return np.asarray(raw, dtype=float).reshape(3, 3)
    sigma = float(pair.metadata.get("preint_sigma_rad", 1e-2))
    return np.eye(3) * max(sigma, 1e-4) ** 2


def _pair_rotation_information(pair: MotionPair) -> float:
    """Approximate independent rotation information carried by one factor."""

    angle_rad = np.deg2rad(
        min(
            float(pair.metadata.get("rotation_deg_A", rotation_angle_deg(pair.R_A))),
            float(pair.metadata.get("rotation_deg_B", rotation_angle_deg(pair.R_B))),
        )
    )
    variance = max(float(np.trace(_pair_cov(pair))) / 3.0, 1e-12)
    fitness = float(np.clip(pair.fitness, 0.1, 1.0))
    return float((angle_rad * angle_rad / variance) * fitness)


def select_decorrelated_phase_a_pairs(
    pairs: list[MotionPair] | tuple[MotionPair, ...],
    *,
    block_s: float = 3.0,
    max_pairs_per_block: int = 1,
) -> list[MotionPair]:
    """Keep the most informative factors in non-overlapping time blocks.

    Sliding-window motion pairs share most IMU and LiDAR samples. Treating all
    of them as independent makes covariance and leave-one-out tests far too
    optimistic. A per-session time block is therefore the statistical unit.
    """

    values = list(pairs)
    if block_s <= 0.0 or max_pairs_per_block <= 0:
        return values
    grouped: dict[tuple[str, int], list[MotionPair]] = {}
    for pair in values:
        t0 = float(pair.metadata.get("t_i_imu_s", pair.t_i_s))
        t1 = float(pair.metadata.get("t_j_imu_s", pair.t_j_s))
        block = int(np.floor((0.5 * (t0 + t1)) / block_s))
        grouped.setdefault((pair.session_id, block), []).append(pair)
    selected: list[MotionPair] = []
    for key in sorted(grouped):
        ranked = sorted(
            grouped[key],
            key=lambda pair: (
                -_pair_rotation_information(pair),
                float(pair.metadata.get("duration_s", pair.t_j_s - pair.t_i_s)),
                pair.i,
                pair.j,
            ),
        )
        selected.extend(ranked[:max_pairs_per_block])
    return selected


def _pair_bias0(pair: MotionPair, fallback: np.ndarray) -> np.ndarray:
    raw = pair.metadata.get("gyro_bias0_rad_s")
    if raw is None:
        return np.asarray(fallback, dtype=float).reshape(3)
    return np.asarray(raw, dtype=float).reshape(3)


def phase_a_metadata_complete(pairs: list[MotionPair] | tuple[MotionPair, ...]) -> bool:
    """Return whether exact Phase-A bias correction metadata is present."""

    return all("J_bg" in pair.metadata and "cov" in pair.metadata for pair in pairs)


def rehydrate_phase_a_pairs(
    pairs: list[MotionPair] | tuple[MotionPair, ...],
    *,
    imu_by_session: Mapping[str, ImuSeries],
    bias0_by_session: Mapping[str, np.ndarray],
    progress_callback: ProgressCallback | None = None,
) -> tuple[list[MotionPair], dict[str, Any]]:
    """Recompute only gyro preintegration metadata; LiDAR registration is untouched."""

    enriched: list[MotionPair] = []
    errors_deg: list[float] = []
    total = len(pairs)
    for index, pair in enumerate(pairs, start=1):
        if pair.session_id not in imu_by_session:
            raise KeyError(f"missing IMU for session {pair.session_id!r}")
        imu = imu_by_session[pair.session_id]
        fallback = np.asarray(bias0_by_session[pair.session_id], dtype=float).reshape(3)
        bias0 = _pair_bias0(pair, fallback)
        try:
            t0 = float(pair.metadata["t_i_imu_s"])
            t1 = float(pair.metadata["t_j_imu_s"])
        except KeyError as exc:
            raise ValueError(
                f"pair {pair.session_id}:{pair.i}->{pair.j} lacks cached IMU time bounds"
            ) from exc
        preint = preintegrate_gyro(imu.t_s, imu.gyro_rad_s, t0, t1, bias0)
        error_deg = rotation_angle_deg(preint.delta_R.T @ pair.R_A)
        errors_deg.append(error_deg)
        metadata = dict(pair.metadata)
        metadata.update(
            {
                "J_bg": preint.J_bg.tolist(),
                "cov": preint.cov.tolist(),
                "duration_s": float(preint.duration_s),
                "mean_gyro_norm": float(preint.mean_gyro_norm),
                "preint_sigma_rad": float(preint.sigma_rad),
                "weight": float(preint.weight),
                "gyro_bias0_rad_s": bias0.tolist(),
                "phase_a_metadata_rehydrated": True,
                "rehydrated_R_A_error_deg": float(error_deg),
            }
        )
        enriched.append(replace(pair, metadata=metadata))
        if index == 1 or index == total or index % 500 == 0:
            _emit(
                progress_callback,
                "rehydrate_progress",
                processed=index,
                total=total,
                progress_pct=100.0 * index / max(total, 1),
                session=pair.session_id,
                max_R_A_error_deg=max(errors_deg),
            )
    report = {
        "pair_count": total,
        "max_R_A_error_deg": max(errors_deg) if errors_deg else 0.0,
        "rms_R_A_error_deg": (
            float(np.sqrt(np.mean(np.square(errors_deg)))) if errors_deg else 0.0
        ),
    }
    return enriched, report


def _bias_bases(
    pairs: list[MotionPair], supplied: Mapping[str, np.ndarray] | None
) -> dict[str, np.ndarray]:
    supplied_map = {} if supplied is None else supplied
    result: dict[str, np.ndarray] = {}
    for session_id in sorted({pair.session_id for pair in pairs}):
        if session_id in supplied_map:
            result[session_id] = np.asarray(supplied_map[session_id], dtype=float).reshape(3)
            continue
        pair = next(item for item in pairs if item.session_id == session_id)
        result[session_id] = _pair_bias0(pair, np.zeros(3))
    return result


def _rotation_distribution(
    errors_deg: list[float],
) -> tuple[float, float, float, float, bool]:
    if not errors_deg:
        return 1e9, 1e9, 1e9, 1.0, False
    values = np.asarray(errors_deg, dtype=float)
    rms = float(np.sqrt(np.mean(values**2)))
    median = float(np.median(values))
    p95 = float(np.percentile(values, 95.0))
    outlier_fraction = float(np.mean(values > 5.0))
    accepted = (
        len(values) >= 3
        and rms < 1.5
        and median < 0.5
        and p95 < 1.5
        and outlier_fraction <= 0.005
    )
    return rms, median, p95, outlier_fraction, accepted


def _corrected_delta_r(
    pair: MotionPair, bias: np.ndarray, bias0: np.ndarray
) -> np.ndarray:
    raw = pair.metadata.get("J_bg")
    if raw is None:
        raise ValueError(
            "Phase-A bias optimization requires J_bg; rehydrate the v1 cache from IMU first"
        )
    return apply_bias_jacobian_correction(
        pair.R_A,
        np.asarray(raw, dtype=float).reshape(3, 3),
        bias - bias0,
    )


def _wrap_angle_deg(value: float) -> float:
    return float((value + 180.0) % 360.0 - 180.0)


def _session_balance_scales(pairs: list[MotionPair]) -> dict[str, float]:
    information: dict[str, float] = {}
    for pair in pairs:
        information[pair.session_id] = (
            information.get(pair.session_id, 0.0)
            + _pair_rotation_information(pair)
        )
    target = float(np.mean(list(information.values())))
    return {
        session_id: float(
            np.clip(np.sqrt(target / max(value, 1e-12)), 0.25, 4.0)
        )
        for session_id, value in information.items()
    }


class _PhaseAProblem:
    def __init__(
        self,
        pairs: list[MotionPair],
        bias_bases: Mapping[str, np.ndarray],
        *,
        bias_prior_sigma_rad_s: float,
    ) -> None:
        if not pairs:
            raise ValueError("Phase-A requires at least one strong-rotation pair")
        self.pairs = list(pairs)
        self.session_ids = sorted({pair.session_id for pair in pairs})
        self.session_index = {sid: index for index, sid in enumerate(self.session_ids)}
        self.bias_bases = {
            sid: np.asarray(bias_bases[sid], dtype=float).reshape(3)
            for sid in self.session_ids
        }
        self.whiten = np.stack(
            [residual_whiten_matrix(_pair_cov(pair)) for pair in pairs]
        )
        self.session_scales = _session_balance_scales(pairs)
        self.balance_values = np.asarray(
            [self.session_scales[pair.session_id] for pair in pairs],
            dtype=float,
        )
        self.pair_session_indices = np.asarray(
            [self.session_index[pair.session_id] for pair in pairs],
            dtype=int,
        )
        self.r_a = np.stack([pair.R_A for pair in pairs])
        self.r_b = np.stack([pair.R_B for pair in pairs])
        self.j_bg = np.stack(
            [
                np.asarray(
                    pair.metadata.get("J_bg", np.zeros((3, 3))),
                    dtype=float,
                ).reshape(3, 3)
                for pair in pairs
            ]
        )
        self.pair_bias0 = np.stack(
            [
                _pair_bias0(pair, self.bias_bases[pair.session_id])
                for pair in pairs
            ]
        )
        self.bias_prior_sigma = max(float(bias_prior_sigma_rad_s), 1e-6)

    def _unpack(
        self,
        vector: np.ndarray,
        rotation_seed: np.ndarray,
        *,
        optimize_bias: bool,
        rotation_basis: np.ndarray,
    ) -> tuple[np.ndarray, dict[str, np.ndarray]]:
        rotation_dim = int(rotation_basis.shape[1])
        rotation_delta = rotation_basis @ vector[:rotation_dim]
        rotation = orthonormalize_rotation(so3_exp(rotation_delta) @ rotation_seed)
        if not optimize_bias:
            return rotation, {
                sid: value.copy() for sid, value in self.bias_bases.items()
            }
        biases = {
            sid: vector[
                rotation_dim + 3 * index : rotation_dim + 3 * (index + 1)
            ].copy()
            for sid, index in self.session_index.items()
        }
        return rotation, biases

    def _batch_error_vectors(
        self,
        rotation: np.ndarray,
        biases: Mapping[str, np.ndarray],
        *,
        optimize_bias: bool,
    ) -> np.ndarray:
        if optimize_bias:
            bias_matrix = np.stack(
                [biases[sid] for sid in self.session_ids]
            )
            delta_bias = (
                bias_matrix[self.pair_session_indices]
                - self.pair_bias0
            )
            correction_vectors = np.einsum(
                "nij,nj->ni", self.j_bg, delta_bias
            )
            corrections = Rotation.from_rotvec(
                correction_vectors
            ).as_matrix()
            delta_r = np.matmul(self.r_a, corrections)
        else:
            delta_r = self.r_a
        predicted = np.matmul(
            np.matmul(rotation[None, :, :], self.r_b),
            rotation.T[None, :, :],
        )
        error_matrices = np.matmul(
            np.swapaxes(delta_r, 1, 2),
            predicted,
        )
        return Rotation.from_matrix(error_matrices).as_rotvec()

    def raw_errors(
        self,
        rotation: np.ndarray,
        biases: Mapping[str, np.ndarray],
        pairs: list[MotionPair] | None = None,
    ) -> list[float]:
        if pairs is None:
            vectors = self._batch_error_vectors(
                rotation,
                biases,
                optimize_bias=True,
            )
            return np.degrees(np.linalg.norm(vectors, axis=1)).tolist()
        errors: list[float] = []
        for pair in pairs:
            bias0 = _pair_bias0(
                pair, self.bias_bases[pair.session_id]
            )
            delta_r = _corrected_delta_r(
                pair, biases[pair.session_id], bias0
            )
            error = preintegration_rotation_residual(
                delta_r, rotation, pair.R_B
            )
            errors.append(
                float(np.degrees(np.linalg.norm(error)))
            )
        return errors

    def solve(
        self,
        *,
        name: str,
        rotation_seed: np.ndarray,
        optimize_bias: bool,
        rotation_prior: np.ndarray | None = None,
        rotation_prior_sigma_deg: float = 15.0,
        bias_seed: Mapping[str, np.ndarray] | None = None,
        rotation_basis: np.ndarray | None = None,
        max_nfev: int = 200,
    ) -> PhaseAVariantResult:
        if optimize_bias and not phase_a_metadata_complete(self.pairs):
            raise ValueError(
                "Phase-A bias optimization requires J_bg/cov for every strong pair"
            )
        basis = (
            np.eye(3)
            if rotation_basis is None
            else np.asarray(rotation_basis, dtype=float).reshape(3, -1)
        )
        if basis.shape[1] and not np.allclose(
            basis.T @ basis, np.eye(basis.shape[1]), atol=1e-6
        ):
            raise ValueError("rotation_basis columns must be orthonormal")
        rotation_dim = int(basis.shape[1])
        dimension = rotation_dim + (
            3 * len(self.session_ids) if optimize_bias else 0
        )
        x0 = np.zeros(dimension)
        if optimize_bias:
            source = self.bias_bases if bias_seed is None else bias_seed
            for sid, index in self.session_index.items():
                x0[
                    rotation_dim + 3 * index : rotation_dim + 3 * (index + 1)
                ] = np.asarray(
                    source[sid], dtype=float
                ).reshape(3)

        prior = (
            None
            if rotation_prior is None
            else orthonormalize_rotation(
                np.asarray(rotation_prior, dtype=float).reshape(3, 3)
            )
        )
        prior_sigma_rad = max(
            np.deg2rad(float(rotation_prior_sigma_deg)), 1e-6
        )

        def residual(vector: np.ndarray) -> np.ndarray:
            rotation, biases = self._unpack(
                vector,
                rotation_seed,
                optimize_bias=optimize_bias,
                rotation_basis=basis,
            )
            error_vectors = self._batch_error_vectors(
                rotation,
                biases,
                optimize_bias=optimize_bias,
            )
            weighted = np.einsum(
                "nij,nj->ni", self.whiten, error_vectors
            )
            rows: list[np.ndarray] = [
                (
                    self.balance_values[:, None] * weighted
                ).reshape(-1)
            ]
            if optimize_bias:
                for sid in self.session_ids:
                    rows.append(
                        (biases[sid] - self.bias_bases[sid])
                        / self.bias_prior_sigma
                    )
            if prior is not None:
                rows.append(so3_log(prior.T @ rotation) / prior_sigma_rad)
            return np.concatenate(rows)

        optimum = least_squares(
            residual,
            x0,
            loss="huber",
            f_scale=1.0,
            max_nfev=max_nfev,
        )
        rotation, biases = self._unpack(
            optimum.x,
            rotation_seed,
            optimize_bias=optimize_bias,
            rotation_basis=basis,
        )
        global_errors = self.raw_errors(rotation, biases)
        rms, median, p95, outlier, global_accepted = _rotation_distribution(
            global_errors
        )
        sessions: list[PhaseASessionDiagnostics] = []
        for sid in self.session_ids:
            local_pairs = [
                pair for pair in self.pairs if pair.session_id == sid
            ]
            local_errors = self.raw_errors(rotation, biases, local_pairs)
            srms, smedian, sp95, soutlier, saccepted = _rotation_distribution(
                local_errors
            )
            sessions.append(
                PhaseASessionDiagnostics(
                    session_id=sid,
                    pair_count=len(local_pairs),
                    gyro_bias0_rad_s=self.bias_bases[sid].copy(),
                    gyro_bias_rad_s=biases[sid].copy(),
                    residual_rms_deg=srms,
                    residual_median_deg=smedian,
                    residual_p95_deg=sp95,
                    outlier_fraction_gt_5deg=soutlier,
                    accepted=saccepted,
                )
            )
        accepted = bool(
            optimum.success
            and global_accepted
            and all(item.accepted for item in sessions)
        )
        return PhaseAVariantResult(
            name=name,
            R_IMU_lidar=rotation,
            rpy_deg_xyz=rpy_deg_xyz(rotation),
            gyro_bias_rad_s_per_session=biases,
            sessions=tuple(sessions),
            residual_rms_deg=rms,
            residual_median_deg=median,
            residual_p95_deg=p95,
            outlier_fraction_gt_5deg=outlier,
            accepted=accepted,
            rotation_prior_used=prior is not None,
            optimizer_success=bool(optimum.success),
            optimizer_nfev=int(optimum.nfev),
            cost=float(optimum.cost),
            residual_count=int(optimum.fun.size),
            parameter_count=int(optimum.x.size),
            jacobian=np.asarray(optimum.jac, dtype=float),
        )


def _yaw_rad(rotation: np.ndarray) -> float:
    return float(np.arctan2(rotation[1, 0], rotation[0, 0]))


def _marginal_observability(
    variant: PhaseAVariantResult,
    *,
    direction_std_max_deg: float,
) -> PhaseAMarginalObservability:
    jacobian = np.asarray(variant.jacobian, dtype=float)
    hessian = jacobian.T @ jacobian
    h_rr = hessian[:3, :3]
    notes: list[str] = []
    if hessian.shape[0] > 3:
        h_rb = hessian[:3, 3:]
        h_bb = hessian[3:, 3:]
        singular_b = np.linalg.svd(h_bb, compute_uv=False)
        if singular_b[-1] <= max(singular_b[0] * 1e-12, 1e-12):
            notes.append(
                "H_bb is rank-deficient; Schur complement uses pseudoinverse"
            )
        h_marginal = (
            h_rr - h_rb @ np.linalg.pinv(h_bb, rcond=1e-12) @ h_rb.T
        )
    else:
        h_marginal = h_rr
    h_marginal = 0.5 * (h_marginal + h_marginal.T)
    eigenvalues, eigenvectors = np.linalg.eigh(h_marginal)
    max_eigen = max(float(eigenvalues[-1]), 1e-18)
    rank_threshold = max(max_eigen * 1e-6, 1e-9)
    rank = int(np.sum(eigenvalues > rank_threshold))
    positive = eigenvalues[eigenvalues > rank_threshold]
    condition = (
        float(max_eigen / positive[0])
        if positive.size == 3
        else float("inf")
    )
    dof = max(variant.residual_count - variant.parameter_count, 1)
    sigma2 = max(2.0 * variant.cost / dof, 1e-12)
    covariance = sigma2 * np.linalg.pinv(h_marginal, rcond=1e-12)
    direction_variances = np.full(3, np.inf)
    observable_eigen = eigenvalues > rank_threshold
    direction_variances[observable_eigen] = (
        sigma2 / eigenvalues[observable_eigen]
    )
    direction_std_deg = np.degrees(np.sqrt(direction_variances))
    precision_rank = int(
        np.sum(direction_std_deg <= float(direction_std_max_deg))
    )

    epsilon = 1e-6
    yaw0 = _yaw_rad(variant.R_IMU_lidar)
    yaw_gradient = np.zeros(3)
    for axis in range(3):
        delta = np.zeros(3)
        delta[axis] = epsilon
        perturbed = orthonormalize_rotation(
            so3_exp(delta) @ variant.R_IMU_lidar
        )
        difference = np.arctan2(
            np.sin(_yaw_rad(perturbed) - yaw0),
            np.cos(_yaw_rad(perturbed) - yaw0),
        )
        yaw_gradient[axis] = difference / epsilon
    yaw_coefficients = eigenvectors.T @ yaw_gradient
    if np.any(
        (~observable_eigen) & (np.abs(yaw_coefficients) > 1e-6)
    ):
        yaw_variance = float("inf")
    else:
        yaw_variance = float(
            np.sum(
                np.square(yaw_coefficients[observable_eigen])
                * direction_variances[observable_eigen]
            )
        )
    yaw_std_deg = float(np.degrees(np.sqrt(max(yaw_variance, 0.0))))
    if rank < 3:
        notes.append(f"marginalized rotation rank={rank}/3")
    if precision_rank < 3:
        notes.append(
            "only "
            f"{precision_rank}/3 rotation directions meet the "
            f"{direction_std_max_deg:.3f} deg precision threshold"
        )
    notes.append(
        "formal yaw std uses the robust, session-balanced linearized Phase-A "
        "model; leave-one-session-out remains the primary stability check"
    )
    return PhaseAMarginalObservability(
        rank=rank,
        condition=condition,
        eigenvalues=eigenvalues,
        weakest_direction_left_tangent=eigenvectors[:, 0],
        covariance_rotvec_rad2=covariance,
        yaw_std_deg=yaw_std_deg,
        direction_std_deg=direction_std_deg,
        precision_rank=precision_rank,
        residual_variance_scale=float(sigma2),
        hessian_marginal=h_marginal,
        ok=bool(precision_rank == 3 and np.isfinite(yaw_std_deg)),
        notes=tuple(notes),
    )


def _evaluate_static_bias(
    pairs: list[MotionPair], rotation: np.ndarray
) -> tuple[float, float]:
    errors = [
        float(
            np.degrees(
                np.linalg.norm(
                    preintegration_rotation_residual(
                        pair.R_A, rotation, pair.R_B
                    )
                )
            )
        )
        for pair in pairs
    ]
    rms, _, p95, _, _ = _rotation_distribution(errors)
    return rms, p95


def _solve_three_variants(
    pairs: list[MotionPair],
    bias_bases: Mapping[str, np.ndarray],
    *,
    rotation_prior: np.ndarray | None,
    rotation_prior_sigma_deg: float,
    bias_prior_sigma_rad_s: float,
    max_nfev: int,
    progress_callback: ProgressCallback | None = None,
    solve_label: str = "full",
) -> tuple[PhaseAVariantResult, PhaseAVariantResult, PhaseAVariantResult]:
    data_seed = estimate_rotation_handeye_initial(pairs)
    problem = _PhaseAProblem(
        pairs,
        bias_bases,
        bias_prior_sigma_rad_s=bias_prior_sigma_rad_s,
    )
    _emit(
        progress_callback,
        "variant_start",
        solve_label=solve_label,
        variant="A0_fixed_bg_data_only",
        pair_count=len(pairs),
    )
    fixed = problem.solve(
        name="A0_fixed_bg_data_only",
        rotation_seed=data_seed,
        optimize_bias=False,
        max_nfev=max_nfev,
    )
    _emit(
        progress_callback,
        "variant_complete",
        solve_label=solve_label,
        variant=fixed.name,
        rpy_deg=fixed.rpy_deg_xyz.tolist(),
        rms_deg=fixed.residual_rms_deg,
        p95_deg=fixed.residual_p95_deg,
    )
    _emit(
        progress_callback,
        "variant_start",
        solve_label=solve_label,
        variant="A1_session_bg_data_only",
        pair_count=len(pairs),
    )
    data_only = problem.solve(
        name="A1_session_bg_data_only",
        rotation_seed=fixed.R_IMU_lidar,
        optimize_bias=True,
        bias_seed=bias_bases,
        max_nfev=max_nfev,
    )
    _emit(
        progress_callback,
        "variant_complete",
        solve_label=solve_label,
        variant=data_only.name,
        rpy_deg=data_only.rpy_deg_xyz.tolist(),
        rms_deg=data_only.residual_rms_deg,
        p95_deg=data_only.residual_p95_deg,
    )
    _emit(
        progress_callback,
        "variant_start",
        solve_label=solve_label,
        variant="A2_session_bg_with_rotation_prior",
        pair_count=len(pairs),
    )
    with_prior = problem.solve(
        name="A2_session_bg_with_rotation_prior",
        rotation_seed=data_only.R_IMU_lidar,
        optimize_bias=True,
        rotation_prior=rotation_prior,
        rotation_prior_sigma_deg=rotation_prior_sigma_deg,
        bias_seed=data_only.gyro_bias_rad_s_per_session,
        max_nfev=max_nfev,
    )
    _emit(
        progress_callback,
        "variant_complete",
        solve_label=solve_label,
        variant=with_prior.name,
        rpy_deg=with_prior.rpy_deg_xyz.tolist(),
        rms_deg=with_prior.residual_rms_deg,
        p95_deg=with_prior.residual_p95_deg,
    )
    return fixed, data_only, with_prior


def solve_phase_a_comparison(
    pairs: list[MotionPair] | tuple[MotionPair, ...],
    *,
    gyro_bias_rad_s_by_session: Mapping[str, np.ndarray] | None = None,
    rotation_prior: np.ndarray | None = None,
    rotation_prior_sigma_deg: float = 15.0,
    preexcluded_session_ids: set[str] | None = None,
    strong_rotation_min_deg: float = 1.0,
    decorrelation_block_s: float = 0.0,
    max_pairs_per_block: int = 1,
    bias_prior_sigma_rad_s: float = 0.002,
    yaw_std_max_deg: float = 0.5,
    leave_one_out_yaw_range_max_deg: float = 1.0,
    data_prior_difference_max_deg: float = 1.0,
    run_leave_one_out: bool = True,
    max_nfev: int = 200,
    progress_callback: ProgressCallback | None = None,
) -> PhaseAComparisonResult:
    """Solve and compare data-only and installation-prior Phase-A variants."""

    all_pairs = list(pairs)
    strong_candidates = select_strong_rotation_pairs(
        all_pairs,
        min_rotation_deg=strong_rotation_min_deg,
    )
    counts = Counter(pair.session_id for pair in strong_candidates)
    excluded = (
        set() if preexcluded_session_ids is None else set(preexcluded_session_ids)
    )
    eligible_pairs = [
        pair for pair in strong_candidates if pair.session_id not in excluded
    ]
    active_pairs = select_decorrelated_phase_a_pairs(
        eligible_pairs,
        block_s=decorrelation_block_s,
        max_pairs_per_block=max_pairs_per_block,
    )
    if not active_pairs:
        raise ValueError(
            "no active strong-rotation pairs remain after session exclusion"
        )
    bases_all = _bias_bases(all_pairs, gyro_bias_rad_s_by_session)
    bases_active = {
        sid: bases_all[sid]
        for sid in sorted({pair.session_id for pair in active_pairs})
    }
    if not phase_a_metadata_complete(active_pairs):
        raise ValueError(
            "Phase-A A1/A2 require J_bg/cov metadata; rehydrate cache from raw IMU"
        )

    _emit(
        progress_callback,
        "phase_a_variants_start",
        all_pair_count=len(all_pairs),
        strong_pair_count=len(strong_candidates),
        active_pair_count=len(active_pairs),
        active_sessions=len(bases_active),
        excluded_sessions=sorted(excluded),
    )
    fixed, data_only, with_prior = _solve_three_variants(
        active_pairs,
        bases_active,
        rotation_prior=rotation_prior,
        rotation_prior_sigma_deg=rotation_prior_sigma_deg,
        bias_prior_sigma_rad_s=bias_prior_sigma_rad_s,
        max_nfev=max_nfev,
        progress_callback=progress_callback,
        solve_label="full",
    )
    _emit(
        progress_callback,
        "phase_a_variants_complete",
        A0_rpy_deg=fixed.rpy_deg_xyz.tolist(),
        A1_rpy_deg=data_only.rpy_deg_xyz.tolist(),
        A2_rpy_deg=with_prior.rpy_deg_xyz.tolist(),
    )
    marginal = _marginal_observability(
        data_only,
        direction_std_max_deg=yaw_std_max_deg,
    )
    _, marginal_directions = np.linalg.eigh(marginal.hessian_marginal)
    observable_mask = marginal.direction_std_deg <= yaw_std_max_deg
    observable_basis = marginal_directions[:, observable_mask]

    partial_result: PhaseAVariantResult | None = None
    if rotation_prior is not None and 0 < observable_basis.shape[1] < 3:
        partial_problem = _PhaseAProblem(
            active_pairs,
            bases_active,
            bias_prior_sigma_rad_s=bias_prior_sigma_rad_s,
        )
        partial_result = partial_problem.solve(
            name="A3_observable_subspace_with_prior",
            rotation_seed=rotation_prior,
            optimize_bias=True,
            bias_seed=data_only.gyro_bias_rad_s_per_session,
            rotation_basis=observable_basis,
            max_nfev=max_nfev,
        )

    prior_sensitivity: list[PhaseAPriorSensitivityResult] = []
    if rotation_prior is not None:
        sensitivity_problem = _PhaseAProblem(
            active_pairs,
            bases_active,
            bias_prior_sigma_rad_s=bias_prior_sigma_rad_s,
        )
        for sigma_deg in (15.0, 3.0, 1.0, 0.5, 0.2):
            sensitivity = sensitivity_problem.solve(
                name=f"prior_sensitivity_{sigma_deg:g}deg",
                rotation_seed=data_only.R_IMU_lidar,
                optimize_bias=True,
                rotation_prior=rotation_prior,
                rotation_prior_sigma_deg=sigma_deg,
                bias_seed=data_only.gyro_bias_rad_s_per_session,
                max_nfev=max_nfev,
            )
            difference = float(
                np.degrees(
                    np.linalg.norm(
                        so3_log(
                            data_only.R_IMU_lidar.T
                            @ sensitivity.R_IMU_lidar
                        )
                    )
                )
            )
            prior_sensitivity.append(
                PhaseAPriorSensitivityResult(
                    sigma_deg=sigma_deg,
                    rpy_deg_xyz=sensitivity.rpy_deg_xyz,
                    rotation_difference_from_data_deg=difference,
                    residual_rms_deg=sensitivity.residual_rms_deg,
                    residual_p95_deg=sensitivity.residual_p95_deg,
                )
            )

    leave_one_out: list[PhaseALeaveOneOutResult] = []
    leave_one_out_rotations: list[np.ndarray] = []
    active_session_ids = sorted(bases_active)
    if run_leave_one_out and len(active_session_ids) >= 3:
        for fold_index, held_out in enumerate(active_session_ids, start=1):
            train_pairs = [
                pair for pair in active_pairs if pair.session_id != held_out
            ]
            train_bases = {
                sid: value
                for sid, value in bases_active.items()
                if sid != held_out
            }
            _emit(
                progress_callback,
                "leave_one_out_start",
                fold=fold_index,
                folds=len(active_session_ids),
                held_out_session=held_out,
                training_pair_count=len(train_pairs),
            )
            _, fold_data, fold_prior = _solve_three_variants(
                train_pairs,
                train_bases,
                rotation_prior=rotation_prior,
                rotation_prior_sigma_deg=rotation_prior_sigma_deg,
                bias_prior_sigma_rad_s=bias_prior_sigma_rad_s,
                max_nfev=max_nfev,
                progress_callback=progress_callback,
                solve_label=f"loo_without_{held_out}",
            )
            held_out_pairs = [
                pair
                for pair in active_pairs
                if pair.session_id == held_out
            ]
            held_rms, held_p95 = _evaluate_static_bias(
                held_out_pairs, fold_data.R_IMU_lidar
            )
            held_problem = _PhaseAProblem(
                held_out_pairs,
                {held_out: bases_active[held_out]},
                bias_prior_sigma_rad_s=bias_prior_sigma_rad_s,
            )
            held_fit = held_problem.solve(
                name=f"held_out_bg_refit_{held_out}",
                rotation_seed=fold_data.R_IMU_lidar,
                optimize_bias=True,
                bias_seed={held_out: bases_active[held_out]},
                rotation_basis=np.zeros((3, 0)),
                max_nfev=max_nfev,
            )
            leave_one_out_rotations.append(fold_data.R_IMU_lidar)
            leave_one_out.append(
                PhaseALeaveOneOutResult(
                    held_out_session=held_out,
                    data_only_rpy_deg_xyz=fold_data.rpy_deg_xyz,
                    prior_rpy_deg_xyz=fold_prior.rpy_deg_xyz,
                    data_only_gyro_bias_rad_s_per_session=(
                        fold_data.gyro_bias_rad_s_per_session
                    ),
                    prior_gyro_bias_rad_s_per_session=(
                        fold_prior.gyro_bias_rad_s_per_session
                    ),
                    data_only_training_rms_deg=fold_data.residual_rms_deg,
                    data_only_training_p95_deg=fold_data.residual_p95_deg,
                    prior_training_rms_deg=fold_prior.residual_rms_deg,
                    prior_training_p95_deg=fold_prior.residual_p95_deg,
                    held_out_static_bg_rms_deg=held_rms,
                    held_out_static_bg_p95_deg=held_p95,
                    held_out_fitted_bg_rms_deg=held_fit.residual_rms_deg,
                    held_out_fitted_bg_p95_deg=held_fit.residual_p95_deg,
                    held_out_fitted_bg_rad_s=(
                        held_fit.gyro_bias_rad_s_per_session[held_out]
                    ),
                )
            )
            _emit(
                progress_callback,
                "leave_one_out_complete",
                fold=fold_index,
                folds=len(active_session_ids),
                held_out_session=held_out,
                data_only_yaw_deg=float(fold_data.rpy_deg_xyz[2]),
                prior_yaw_deg=float(fold_prior.rpy_deg_xyz[2]),
                held_out_static_bg_p95_deg=held_p95,
                held_out_fitted_bg_p95_deg=held_fit.residual_p95_deg,
            )

    yaw_values = [
        float(item.data_only_rpy_deg_xyz[2])
        for item in leave_one_out
    ]
    loo_yaw_range = (
        float(max(yaw_values) - min(yaw_values))
        if yaw_values
        else float("nan")
    )
    stability_basis = (
        np.eye(3)
        if marginal.precision_rank == 3
        else observable_basis
    )
    observable_loo_values = [
        float(
            np.degrees(
                np.linalg.norm(
                    stability_basis.T
                    @ so3_log(
                        item_rotation @ data_only.R_IMU_lidar.T
                    )
                )
            )
        )
        for item_rotation in leave_one_out_rotations
    ]
    loo_observable_max = (
        max(observable_loo_values)
        if observable_loo_values
        else float("nan")
    )
    yaw_difference = abs(
        _wrap_angle_deg(
            float(
                data_only.rpy_deg_xyz[2]
                - with_prior.rpy_deg_xyz[2]
            )
        )
    )
    geodesic_difference = float(
        np.degrees(
            np.linalg.norm(
                so3_log(
                    data_only.R_IMU_lidar.T
                    @ with_prior.R_IMU_lidar
                )
            )
        )
    )
    loo_required = run_leave_one_out and len(active_session_ids) >= 3
    sensitivity_half = next(
        (item for item in prior_sensitivity if item.sigma_deg == 0.5),
        None,
    )
    checks = {
        "data_only_quality": bool(data_only.accepted),
        "marginal_rotation_rank": bool(marginal.rank == 3),
        "marginal_rotation_precision_rank": bool(
            marginal.precision_rank == 3
        ),
        "marginal_yaw_std": bool(
            marginal.yaw_std_deg <= yaw_std_max_deg
        ),
        "data_vs_prior_yaw": bool(
            yaw_difference <= data_prior_difference_max_deg
        ),
        "data_vs_prior_geodesic": bool(
            geodesic_difference <= data_prior_difference_max_deg
        ),
        "leave_one_out_yaw_range": bool(
            (not loo_required)
            or (
                np.isfinite(loo_yaw_range)
                and loo_yaw_range
                <= leave_one_out_yaw_range_max_deg
            )
        ),
        "leave_one_out_observable_subspace": bool(
            (not loo_required)
            or (
                np.isfinite(loo_observable_max)
                and loo_observable_max
                <= leave_one_out_yaw_range_max_deg
            )
        ),
        "prior_sensitivity_0_5deg": bool(
            sensitivity_half is None
            or sensitivity_half.rotation_difference_from_data_deg
            <= data_prior_difference_max_deg
        ),
    }
    full_check_names = (
        "data_only_quality",
        "marginal_rotation_precision_rank",
        "marginal_yaw_std",
        "leave_one_out_observable_subspace",
        "prior_sensitivity_0_5deg",
    )
    full_accepted = all(checks[name] for name in full_check_names)
    partial_accepted = bool(
        not full_accepted
        and rotation_prior is not None
        and marginal.precision_rank >= 2
        and partial_result is not None
        and partial_result.accepted
        and checks["leave_one_out_observable_subspace"]
    )
    solution_status = (
        "phase_a_full_accepted"
        if full_accepted
        else (
            "phase_a_partial_accepted"
            if partial_accepted
            else "phase_a_rejected"
        )
    )
    recommended_result = (
        "A1_session_bg_data_only"
        if full_accepted
        else (
            "A3_observable_subspace_with_prior"
            if partial_accepted
            else "none"
        )
    )
    notes = (
        (
            "strong-rotation pairs require both IMU and LiDAR "
            f"relative rotation > {strong_rotation_min_deg:.3f} deg"
        ),
        (
            "A1 data-only result is primary; A2 is only a "
            "prior-sensitivity comparison"
        ),
        (
            "A3 is emitted only when one or more directions fail the precision "
            "gate; it optimizes the observable tangent subspace and explicitly "
            "inherits the complementary direction from the installation prior"
        ),
        (
            f"overlapping factors are reduced to at most {max_pairs_per_block} "
            f"pair(s) per {decorrelation_block_s:.3f}s session block"
        ),
        (
            f"thresholds: yaw_std<={yaw_std_max_deg:.3f} deg, "
            f"LOO yaw range<={leave_one_out_yaw_range_max_deg:.3f} deg, "
            f"data/prior difference<={data_prior_difference_max_deg:.3f} deg"
        ),
    )
    return PhaseAComparisonResult(
        fixed_bg_data_only=fixed,
        session_bg_data_only=data_only,
        session_bg_with_rotation_prior=with_prior,
        observable_subspace_with_prior=partial_result,
        marginal_observability=marginal,
        leave_one_out=tuple(leave_one_out),
        strong_pair_counts_per_session={
            sid: int(count) for sid, count in sorted(counts.items())
        },
        excluded_sessions=tuple(sorted(excluded)),
        data_vs_prior_yaw_diff_deg=yaw_difference,
        data_vs_prior_geodesic_deg=geodesic_difference,
        leave_one_out_yaw_range_deg=loo_yaw_range,
        leave_one_out_observable_max_deg=loo_observable_max,
        prior_sensitivity=tuple(prior_sensitivity),
        strong_pair_candidate_count=len(strong_candidates),
        decorrelated_pair_count=len(active_pairs),
        decorrelation_block_s=float(decorrelation_block_s),
        solution_status=solution_status,
        recommended_result=recommended_result,
        partial_accepted=partial_accepted,
        accepted=full_accepted,
        acceptance_checks=checks,
        notes=notes,
    )


def _array_map(
    values: Mapping[str, np.ndarray],
) -> dict[str, list[float]]:
    return {
        sid: np.asarray(value, dtype=float).reshape(3).tolist()
        for sid, value in values.items()
    }


def phase_a_session_to_dict(
    item: PhaseASessionDiagnostics,
) -> dict[str, Any]:
    return {
        "session_id": item.session_id,
        "pair_count": item.pair_count,
        "gyro_bias0_rad_s": item.gyro_bias0_rad_s.tolist(),
        "gyro_bias_rad_s": item.gyro_bias_rad_s.tolist(),
        "residual_rms_deg": item.residual_rms_deg,
        "residual_median_deg": item.residual_median_deg,
        "residual_p95_deg": item.residual_p95_deg,
        "outlier_fraction_gt_5deg": item.outlier_fraction_gt_5deg,
        "accepted": item.accepted,
        "included_in_final": item.included_in_final,
    }


def phase_a_variant_to_dict(
    item: PhaseAVariantResult,
) -> dict[str, Any]:
    return {
        "name": item.name,
        "R_IMU_lidar": item.R_IMU_lidar.tolist(),
        "rpy_deg_xyz": item.rpy_deg_xyz.tolist(),
        "gyro_bias_rad_s_per_session": _array_map(
            item.gyro_bias_rad_s_per_session
        ),
        "sessions": [
            phase_a_session_to_dict(value)
            for value in item.sessions
        ],
        "residual_rms_deg": item.residual_rms_deg,
        "residual_median_deg": item.residual_median_deg,
        "residual_p95_deg": item.residual_p95_deg,
        "outlier_fraction_gt_5deg": (
            item.outlier_fraction_gt_5deg
        ),
        "accepted": item.accepted,
        "rotation_prior_used": item.rotation_prior_used,
        "optimizer_success": item.optimizer_success,
        "optimizer_nfev": item.optimizer_nfev,
        "cost": item.cost,
        "residual_count": item.residual_count,
        "parameter_count": item.parameter_count,
    }


def phase_a_comparison_to_dict(
    result: PhaseAComparisonResult,
) -> dict[str, Any]:
    marginal = result.marginal_observability
    variants = {
        "A0_fixed_bg_data_only": phase_a_variant_to_dict(
            result.fixed_bg_data_only
        ),
        "A1_session_bg_data_only": phase_a_variant_to_dict(
            result.session_bg_data_only
        ),
        "A2_session_bg_with_rotation_prior": phase_a_variant_to_dict(
            result.session_bg_with_rotation_prior
        ),
    }
    if result.observable_subspace_with_prior is not None:
        variants["A3_observable_subspace_with_prior"] = (
            phase_a_variant_to_dict(
                result.observable_subspace_with_prior
            )
        )
    return {
        "status": result.solution_status,
        "accepted": result.accepted,
        "partial_accepted": result.partial_accepted,
        "recommended_result": result.recommended_result,
        "acceptance_checks": result.acceptance_checks,
        "variants": variants,
        "marginal_observability_A1": {
            "rank": marginal.rank,
            "condition": marginal.condition,
            "eigenvalues": marginal.eigenvalues.tolist(),
            "weakest_direction_left_tangent": (
                marginal.weakest_direction_left_tangent.tolist()
            ),
            "covariance_rotvec_rad2": (
                marginal.covariance_rotvec_rad2.tolist()
            ),
            "yaw_std_deg": marginal.yaw_std_deg,
            "direction_std_deg": marginal.direction_std_deg.tolist(),
            "precision_rank": marginal.precision_rank,
            "residual_variance_scale": (
                marginal.residual_variance_scale
            ),
            "hessian_marginal": (
                marginal.hessian_marginal.tolist()
            ),
            "ok": marginal.ok,
            "notes": list(marginal.notes),
        },
        "leave_one_out": [
            {
                "held_out_session": item.held_out_session,
                "data_only_rpy_deg_xyz": (
                    item.data_only_rpy_deg_xyz.tolist()
                ),
                "prior_rpy_deg_xyz": (
                    item.prior_rpy_deg_xyz.tolist()
                ),
                "data_only_gyro_bias_rad_s_per_session": (
                    _array_map(
                        item.data_only_gyro_bias_rad_s_per_session
                    )
                ),
                "prior_gyro_bias_rad_s_per_session": (
                    _array_map(
                        item.prior_gyro_bias_rad_s_per_session
                    )
                ),
                "data_only_training_rms_deg": (
                    item.data_only_training_rms_deg
                ),
                "data_only_training_p95_deg": (
                    item.data_only_training_p95_deg
                ),
                "prior_training_rms_deg": (
                    item.prior_training_rms_deg
                ),
                "prior_training_p95_deg": (
                    item.prior_training_p95_deg
                ),
                "held_out_static_bg_rms_deg": (
                    item.held_out_static_bg_rms_deg
                ),
                "held_out_static_bg_p95_deg": (
                    item.held_out_static_bg_p95_deg
                ),
                "held_out_fitted_bg_rms_deg": (
                    item.held_out_fitted_bg_rms_deg
                ),
                "held_out_fitted_bg_p95_deg": (
                    item.held_out_fitted_bg_p95_deg
                ),
                "held_out_fitted_bg_rad_s": (
                    item.held_out_fitted_bg_rad_s.tolist()
                ),
            }
            for item in result.leave_one_out
        ],
        "strong_pair_counts_per_session": (
            result.strong_pair_counts_per_session
        ),
        "excluded_sessions": list(result.excluded_sessions),
        "data_vs_prior_yaw_diff_deg": (
            result.data_vs_prior_yaw_diff_deg
        ),
        "data_vs_prior_geodesic_deg": (
            result.data_vs_prior_geodesic_deg
        ),
        "leave_one_out_yaw_range_deg": (
            result.leave_one_out_yaw_range_deg
        ),
        "leave_one_out_observable_max_deg": (
            result.leave_one_out_observable_max_deg
        ),
        "prior_sensitivity": [
            {
                "sigma_deg": item.sigma_deg,
                "rpy_deg_xyz": item.rpy_deg_xyz.tolist(),
                "rotation_difference_from_data_deg": (
                    item.rotation_difference_from_data_deg
                ),
                "residual_rms_deg": item.residual_rms_deg,
                "residual_p95_deg": item.residual_p95_deg,
            }
            for item in result.prior_sensitivity
        ],
        "strong_pair_candidate_count": result.strong_pair_candidate_count,
        "decorrelated_pair_count": result.decorrelated_pair_count,
        "decorrelation_block_s": result.decorrelation_block_s,
        "notes": list(result.notes),
    }
