calibration/imu_lidar/pipeline.py

"""Executable LiDAR–IMU calibration pipeline (V1)."""

from __future__ import annotations

from collections.abc import Callable
from dataclasses import asdict, dataclass, replace
from pathlib import Path
from time import perf_counter
from typing import Any

import numpy as np

from .contracts import (
    CalibrationMode,
    CalibrationRequest,
    CalibrationResult,
    CalibrationStatus,
    MotionPair,
    SessionInput,
)
from .finalize import finalize_result
from .imu_audit import audit_imu
from .imu_io import load_imu_samples
from .joint_optimizer import solve_joint_extrinsic
from .keyframes import build_keyframes
from .lidar_deskew import deskew_lidar_frames
from .lidar_io import load_lidar_frames
from .motion_pairs import build_motion_pairs
from .motion_pairs_io import build_motion_pairs_payload
from .rotation_handeye import solve_rotation_handeye
from .time_offset import TimeOffsetResult, estimate_time_offset, refine_time_offset_signed
from .timestamp_audit import audit_timestamps
from .vehicle_config import load_vehicle_config, prior_enabled

# Remap keyframe indices so multi-session Phase-C graphs do not collide.
_SESSION_INDEX_OFFSET = 1_000_000


def _merge_time_offset(previous: TimeOffsetResult, refined: TimeOffsetResult) -> TimeOffsetResult:
    return TimeOffsetResult(
        delta_t_s=refined.delta_t_s,
        correlation_peak=refined.correlation_peak,
        search_s=previous.search_s,
        notes=tuple(list(previous.notes) + list(refined.notes)),
        ok=True,
    )


@dataclass(frozen=True)
class PipelineStage:
    name: str
    responsibility: str


STAGES = (
    PipelineStage("vehicle_config", "加载并校验当前车辆安装配置"),
    PipelineStage("timestamp_audit", "审查 IMU 与 LiDAR 时间域"),
    PipelineStage("imu_audit", "审查单位、轴向启发与静止零偏"),
    PipelineStage("time_offset", "各会话独立粗估/精修 δt"),
    PipelineStage("lidar_motion", "各会话关键帧、可选去畸变与 LiDAR 相对运动"),
    PipelineStage("motion_pairs", "各会话构造运动对，再合并"),
    PipelineStage("rotation_handeye", "用全部会话运动对联合求解旋转外参"),
    PipelineStage("joint_optimizer", "Phase-A 会话级零偏联合精修；Phase-B/C 暂时门控"),
    PipelineStage("finalize", "写出结果与质量报告"),
)


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


def _emit_progress(
    callback: ProgressCallback | None,
    stage_index: int,
    event: str,
    **fields: Any,
) -> None:
    if callback is None:
        return
    callback(
        {
            "stage_index": stage_index,
            "stage_total": len(STAGES),
            "stage": STAGES[stage_index - 1].name,
            "event": event,
            **fields,
        }
    )


def describe_pipeline(_: CalibrationRequest) -> tuple[PipelineStage, ...]:
    """Return the planned stages."""

    return STAGES


def _build_pairs_and_handeye(
    *,
    session_id: str,
    working_frames,
    imu,
    delta_t_s: float,
    gyro_bias_rad_s: np.ndarray,
    request: CalibrationRequest,
    R_prior: np.ndarray | None = None,
    prior_sigma_deg: float | None = None,
    progress_callback: ProgressCallback | None = None,
):
    keyframes = build_keyframes(
        working_frames,
        min_translation_m=request.min_pair_translation_m,
        min_rotation_deg=request.min_pair_rotation_deg,
        min_registration_fitness=request.min_registration_fitness,
    )
    if progress_callback is not None:
        progress_callback(
            {
                "event": "keyframes_ready",
                "keyframe_count": len(keyframes.indices),
                "lidar_frame_count": len(working_frames),
            }
        )
    pair_set = build_motion_pairs(
        session_id=session_id,
        keyframes=list(keyframes.frames),
        keyframe_indices=keyframes.indices,
        imu=imu,
        delta_t_s=delta_t_s,
        gyro_bias_rad_s=gyro_bias_rad_s,
        min_rotation_deg=request.min_pair_rotation_deg,
        min_translation_m=request.min_pair_translation_m,
        min_registration_fitness=request.min_registration_fitness,
        max_imu_gap_s=request.max_imu_gap_s,
        max_lidar_gap_s=request.max_lidar_gap_s,
        all_frame_times_s=np.asarray([frame.t_mid_s for frame in working_frames], dtype=float),
        progress_callback=progress_callback,
    )
    handeye = solve_rotation_handeye(
        pair_set.pairs,
        R_prior=R_prior,
        prior_sigma_deg=prior_sigma_deg,
    )
    return keyframes, pair_set, handeye


def _translation_prior_from_config(
    vehicle_config: dict[str, Any] | None,
) -> tuple[np.ndarray | None, np.ndarray | float | None]:
    if vehicle_config is None or not prior_enabled(vehicle_config, "translation_prior"):
        return None, None
    init_cfg = vehicle_config.get("initialization") or {}
    tp = init_cfg.get("translation_prior") or {}
    if tp.get("t_IMU_lidar_m") is None:
        return None, None
    return np.asarray(tp["t_IMU_lidar_m"], dtype=float).reshape(3), tp.get("sigma_m", [0.05, 0.05, 0.05])


def _rotation_prior_from_config(
    vehicle_config: dict[str, Any] | None,
) -> tuple[np.ndarray | None, float | None]:
    if vehicle_config is None or not prior_enabled(vehicle_config, "rotation_prior"):
        return None, None
    init_cfg = vehicle_config.get("initialization") or {}
    rp = init_cfg.get("rotation_prior") or {}
    if rp.get("R_IMU_lidar") is None:
        return None, None
    return np.asarray(rp["R_IMU_lidar"], dtype=float).reshape(3, 3), float(rp.get("sigma_deg", 15.0))


def _prepare_session_pairs(
    session: SessionInput,
    request: CalibrationRequest,
    *,
    R_prior: np.ndarray | None = None,
    prior_sigma_deg: float | None = None,
    progress_callback: ProgressCallback | None = None,
    session_index: int = 1,
    session_total: int = 1,
) -> dict[str, Any]:
    """Per-session: audit, δt, keyframes/pairs. No joint extrinsic yet."""

    started_at = perf_counter()

    def emit(stage_index: int, event: str, **fields: Any) -> None:
        _emit_progress(
            progress_callback,
            stage_index,
            event,
            session=session.session_id,
            session_index=session_index,
            session_total=session_total,
            **fields,
        )

    emit(
        2,
        "session_start",
        imu_source=str(session.imu_source),
        lidar_source=str(session.lidar_source),
    )
    imu = load_imu_samples(session.imu_source)
    frames = load_lidar_frames(session.lidar_source)
    emit(
        2,
        "data_loaded",
        imu_samples=int(imu.t_s.size),
        lidar_frames=len(frames),
        imu_span_s=float(imu.t_s[-1] - imu.t_s[0]) if imu.t_s.size >= 2 else 0.0,
        lidar_span_s=(
            float(frames[-1].t_mid_s - frames[0].t_mid_s) if len(frames) >= 2 else 0.0
        ),
        elapsed_s=perf_counter() - started_at,
    )

    ts = audit_timestamps(imu, frames)
    emit(2, "audit_complete", ok=ts.ok)
    if not ts.ok:
        emit(2, "blocked", reason="timestamp_audit")
        return {"ok": False, "stage": "timestamp_audit", "session_id": session.session_id, "report": asdict(ts)}

    imu_report = audit_imu(imu)
    emit(
        3,
        "audit_complete",
        ok=imu_report.ok,
        gyro_bias_norm_rad_s=float(np.linalg.norm(imu_report.gyro_bias_rad_s)),
    )
    if not imu_report.ok:
        emit(3, "blocked", reason="imu_audit")
        return {"ok": False, "stage": "imu_audit", "session_id": session.session_id, "report": asdict(imu_report)}

    fixed_time_offset_s = (
        session.fixed_time_offset_s
        if session.fixed_time_offset_s is not None
        else request.fixed_time_offset_s
    )
    if fixed_time_offset_s is not None:
        offset_source = "fixed"
        offset = TimeOffsetResult(
            delta_t_s=float(fixed_time_offset_s),
            correlation_peak=1.0,
            search_s=0.0,
            notes=(
                f"fixed_time_offset_s={float(fixed_time_offset_s):.6f} "
                "(skip |ω| search; intended for host-UTC-bridged sessions)",
            ),
            ok=True,
        )
    else:
        offset_source = "estimated"
        offset = estimate_time_offset(
            imu,
            frames,
            gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
            search_s=request.time_offset_search_s,
        )
        if not offset.ok:
            emit(
                4,
                "blocked",
                reason="time_offset",
                time_offset_s=float(offset.delta_t_s),
                correlation_peak=float(offset.correlation_peak),
            )
            return {"ok": False, "stage": "time_offset", "session_id": session.session_id, "report": asdict(offset)}

    emit(
        4,
        "offset_ready",
        source=offset_source,
        time_offset_s=float(offset.delta_t_s),
        correlation_peak=float(offset.correlation_peak),
    )
    coarse_delta_t = float(offset.delta_t_s)
    working_frames = frames
    r_x = np.eye(3) if R_prior is None else np.asarray(R_prior, dtype=float).reshape(3, 3)
    handeye = None
    pair_set = None
    keyframes = None
    pairs_notes: list[str] = []
    pair_count = 0

    iterations_total = max(1, request.max_iterations)
    build_pass = "outer"

    def on_build_progress(payload: dict[str, Any]) -> None:
        event = str(payload.get("event", "running"))
        stage_index = 5 if event == "keyframes_ready" else 6
        fields = {key: value for key, value in payload.items() if key != "event"}
        emit(
            stage_index,
            event,
            iteration=iteration + 1,
            iterations_total=iterations_total,
            build_pass=build_pass,
            **fields,
        )

    for iteration in range(iterations_total):
        build_pass = "outer"
        emit(
            5,
            "iteration_start",
            iteration=iteration + 1,
            iterations_total=iterations_total,
            deskew=iteration > 0,
            time_offset_s=float(offset.delta_t_s),
        )
        if iteration > 0:
            deskew_started_at = perf_counter()
            emit(5, "deskew_start", iteration=iteration + 1)
            working_frames = deskew_lidar_frames(
                frames,
                imu,
                delta_t_s=offset.delta_t_s,
                R_IMU_lidar=r_x,
                gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
            )
            emit(
                5,
                "deskew_complete",
                iteration=iteration + 1,
                lidar_frames=len(working_frames),
                elapsed_s=perf_counter() - deskew_started_at,
            )
        keyframes, pair_set, handeye = _build_pairs_and_handeye(
            session_id=session.session_id,
            working_frames=working_frames,
            imu=imu,
            delta_t_s=offset.delta_t_s,
            gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
            request=request,
            R_prior=R_prior,
            prior_sigma_deg=prior_sigma_deg,
            progress_callback=on_build_progress,
        )
        pairs_notes = list(pair_set.notes)
        pair_count = len(pair_set.pairs)
        emit(
            7,
            "local_handeye",
            iteration=iteration + 1,
            build_pass=build_pass,
            keyframes=len(keyframes.indices),
            pair_count=pair_count,
            rms_deg=float(handeye.residual_rms_deg),
            p95_deg=float(handeye.residual_p95_deg),
            outlier_fraction_gt_5deg=float(handeye.outlier_fraction_gt_5deg),
            ok=handeye.ok,
        )
        if pair_count < 3:
            emit(
                6,
                "blocked",
                reason="insufficient_motion_pairs",
                iteration=iteration + 1,
                keyframes=len(keyframes.indices),
                pair_count=pair_count,
            )
            return {
                "ok": False,
                "stage": "motion_pairs",
                "session_id": session.session_id,
                "iteration": iteration,
                "time_offset": asdict(offset),
                "imu_audit": asdict(imu_report),
                "timestamp_audit": asdict(ts),
                "keyframes": 0 if keyframes is None else len(keyframes.indices),
                "pair_notes": pairs_notes,
                "handeye": asdict(handeye),
            }
        r_x = handeye.R_IMU_lidar

        if not request.enable_signed_time_refine:
            continue

        for refine_step in range(1, 3):
            emit(
                4,
                "signed_refine_start",
                iteration=iteration + 1,
                refine_step=refine_step,
                time_offset_s=float(offset.delta_t_s),
            )
            refined = refine_time_offset_signed(
                imu,
                frames,
                delta_t_s=offset.delta_t_s,
                R_IMU_lidar=r_x,
                gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
                search_s=min(0.12, max(0.04, 0.25 * request.time_offset_search_s)),
                max_shift_s=request.max_signed_refine_shift_s,
            )
            # Also bound total walk away from the original coarse estimate.
            if abs(refined.delta_t_s - coarse_delta_t) > request.max_signed_refine_shift_s:
                refined = TimeOffsetResult(
                    delta_t_s=float(offset.delta_t_s),
                    correlation_peak=refined.correlation_peak,
                    search_s=refined.search_s,
                    notes=tuple(
                        list(refined.notes)
                        + [
                            f"signed refine clamped: |δt-coarse| would exceed "
                            f"{request.max_signed_refine_shift_s:.3f}s"
                        ]
                    ),
                    ok=True,
                )
            delta_shift = abs(refined.delta_t_s - offset.delta_t_s)
            offset = _merge_time_offset(offset, refined)
            emit(
                4,
                "signed_refine_complete",
                iteration=iteration + 1,
                refine_step=refine_step,
                time_offset_s=float(offset.delta_t_s),
                shift_s=float(delta_shift),
                correlation_peak=float(refined.correlation_peak),
            )
            if delta_shift < 1e-3:
                break
            build_pass = f"signed_refine_{refine_step}"
            keyframes, pair_set, handeye = _build_pairs_and_handeye(
                session_id=session.session_id,
                working_frames=working_frames,
                imu=imu,
                delta_t_s=offset.delta_t_s,
                gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
                request=request,
                R_prior=R_prior,
                prior_sigma_deg=prior_sigma_deg,
                progress_callback=on_build_progress,
            )
            pairs_notes = list(pair_set.notes)
            pair_count = len(pair_set.pairs)
            emit(
                7,
                "local_handeye",
                iteration=iteration + 1,
                build_pass=build_pass,
                keyframes=len(keyframes.indices),
                pair_count=pair_count,
                rms_deg=float(handeye.residual_rms_deg),
                p95_deg=float(handeye.residual_p95_deg),
                outlier_fraction_gt_5deg=float(handeye.outlier_fraction_gt_5deg),
                ok=handeye.ok,
            )
            if pair_count < 3:
                emit(
                    6,
                    "blocked",
                    reason="insufficient_motion_pairs_after_signed_refine",
                    iteration=iteration + 1,
                    keyframes=len(keyframes.indices),
                    pair_count=pair_count,
                )
                return {
                    "ok": False,
                    "stage": "motion_pairs",
                    "session_id": session.session_id,
                    "iteration": iteration,
                    "time_offset": asdict(offset),
                    "imu_audit": asdict(imu_report),
                    "timestamp_audit": asdict(ts),
                    "keyframes": 0 if keyframes is None else len(keyframes.indices),
                    "pair_notes": pairs_notes,
                    "handeye": asdict(handeye),
                }
            r_x = handeye.R_IMU_lidar
    assert handeye is not None and pair_set is not None and keyframes is not None
    acc_mean = np.asarray(imu_report.static_acc_mean_m_s2, dtype=float).reshape(3)
    acc_n = float(np.linalg.norm(acc_mean))
    if acc_n > 1e-6:
        gravity_init = -acc_mean * (9.80665 / acc_n)
    else:
        gravity_init = np.array([0.0, 0.0, -9.80665])

    emit(
        7,
        "session_complete",
        keyframes=len(keyframes.indices),
        pair_count=pair_count,
        time_offset_s=float(offset.delta_t_s),
        local_handeye_ok=handeye.ok,
        elapsed_s=perf_counter() - started_at,
    )
    return {
        "ok": True,
        "session_id": session.session_id,
        "pairs": tuple(pair_set.pairs),
        "gyro_bias_rad_s": np.asarray(imu_report.gyro_bias_rad_s, dtype=float).reshape(3),
        "gravity_init_m_s2": gravity_init,
        "timestamp_audit": asdict(ts),
        "imu_audit": {
            **asdict(imu_report),
            "gyro_bias_rad_s": imu_report.gyro_bias_rad_s.tolist(),
            "static_acc_mean_m_s2": imu_report.static_acc_mean_m_s2.tolist(),
        },
        "time_offset": asdict(offset),
        "time_offset_s": float(offset.delta_t_s),
        "keyframes": len(keyframes.indices),
        "pair_count": pair_count,
        "pair_notes": pairs_notes,
        "handeye_local": {
            "residual_rms_deg": handeye.residual_rms_deg,
            "residual_median_deg": handeye.residual_median_deg,
            "residual_p95_deg": handeye.residual_p95_deg,
            "outlier_fraction_gt_5deg": handeye.outlier_fraction_gt_5deg,
            "pair_count": handeye.pair_count,
            "ok": handeye.ok,
            "notes": handeye.notes,
            "R_IMU_lidar": handeye.R_IMU_lidar.tolist(),
        },
    }


def _remap_pairs_for_joint(prepared: list[dict[str, Any]]) -> list[MotionPair]:
    merged: list[MotionPair] = []
    for index, prep in enumerate(prepared):
        id_offset = (index + 1) * _SESSION_INDEX_OFFSET
        for pair in prep["pairs"]:
            merged.append(
                replace(
                    pair,
                    i=int(pair.i) + id_offset,
                    j=int(pair.j) + id_offset,
                )
            )
    return merged


def run_calibration(
    request: CalibrationRequest,
    *,
    progress_callback: ProgressCallback | None = None,
) -> CalibrationResult:
    """Run the V1 calibration pipeline for one or more sessions.

    Multi-session: each session estimates its own δt and builds motion pairs;
    rotation hand-eye and joint SE3 are solved once on the merged pair set.
    """

    overall_started_at = perf_counter()

    def finish(
        *,
        status: CalibrationStatus,
        message: str,
        details: dict[str, Any],
        T_IMU_lidar: np.ndarray | None = None,
        time_offset_s: float | None = None,
        motion_pairs_payload: dict[str, Any] | None = None,
    ) -> CalibrationResult:
        _emit_progress(
            progress_callback,
            9,
            "writing_result",
            status=status.value,
            output_directory=str(request.output_directory),
        )
        result = finalize_result(
            status=status,
            message=message,
            details=details,
            T_IMU_lidar=T_IMU_lidar,
            time_offset_s=time_offset_s,
            output_directory=request.output_directory,
            motion_pairs_payload=motion_pairs_payload,
        )
        _emit_progress(
            progress_callback,
            9,
            "complete",
            status=result.status.value,
            elapsed_s=perf_counter() - overall_started_at,
        )
        return result

    _emit_progress(
        progress_callback,
        1,
        "pipeline_start",
        mode=request.requested_mode.value,
        session_count=len(request.sessions),
        max_iterations=max(1, request.max_iterations),
        output_directory=str(request.output_directory),
    )
    if not request.sessions:
        return finish(
            status=CalibrationStatus.BLOCKED,
            message="no sessions provided",
            details={},
        )

    vehicle_config = None
    if request.vehicle_config is not None:
        _emit_progress(
            progress_callback,
            1,
            "loading_vehicle_config",
            path=str(request.vehicle_config),
        )
        try:
            vehicle_config = load_vehicle_config(request.vehicle_config)
        except Exception as exc:  # noqa: BLE001 - surface config problems as blocked
            _emit_progress(
                progress_callback,
                1,
                "blocked",
                reason="vehicle_config",
                error=str(exc),
            )
            return finish(
                status=CalibrationStatus.BLOCKED,
                message=f"vehicle config failed: {exc}",
                details={},
            )
    _emit_progress(
        progress_callback,
        1,
        "vehicle_config_ready",
        loaded=vehicle_config is not None,
    )

    r_prior, prior_sigma_deg = _rotation_prior_from_config(vehicle_config)

    prepared: list[dict[str, Any]] = []
    session_total = len(request.sessions)
    for session_index, session in enumerate(request.sessions, start=1):
        prep = _prepare_session_pairs(
            session,
            request,
            R_prior=r_prior,
            prior_sigma_deg=prior_sigma_deg,
            progress_callback=progress_callback,
            session_index=session_index,
            session_total=session_total,
        )
        if not prep.get("ok"):
            return finish(
                status=CalibrationStatus.BLOCKED,
                message=f"blocked at stage {prep.get('stage')} ({prep.get('session_id')})",
                details={"sessions": [prep]},
            )
        prepared.append(prep)

    all_pairs = _remap_pairs_for_joint(prepared)
    pair_counts_per_session = {
        p["session_id"]: int(p["pair_count"]) for p in prepared
    }
    _emit_progress(
        progress_callback,
        7,
        "joint_handeye_start",
        session_count=len(prepared),
        merged_pair_count=len(all_pairs),
        pair_counts_per_session=pair_counts_per_session,
    )
    handeye_started_at = perf_counter()
    handeye = solve_rotation_handeye(
        all_pairs,
        R_prior=r_prior,
        prior_sigma_deg=prior_sigma_deg,
    )
    _emit_progress(
        progress_callback,
        7,
        "joint_handeye_complete",
        pair_count=handeye.pair_count,
        rms_deg=float(handeye.residual_rms_deg),
        p95_deg=float(handeye.residual_p95_deg),
        outlier_fraction_gt_5deg=float(handeye.outlier_fraction_gt_5deg),
        ok=handeye.ok,
        elapsed_s=perf_counter() - handeye_started_at,
    )
    if handeye.pair_count < 3:
        return finish(
            status=CalibrationStatus.BLOCKED,
            message="blocked at stage rotation_handeye (joint)",
            details={
                "sessions": [_public_session(p) for p in prepared],
                "joint_handeye": asdict(handeye),
                "merged_pair_count": len(all_pairs),
            },
        )

    force_rotation_only = request.requested_mode == CalibrationMode.ROTATION_ONLY
    t_prior, t_prior_sigma = _translation_prior_from_config(vehicle_config)
    gyro_bias_by_session = {
        p["session_id"]: np.asarray(p["gyro_bias_rad_s"], dtype=float) for p in prepared
    }
    time_offset_by_session = {
        p["session_id"]: float(p["time_offset_s"]) for p in prepared
    }

    preexcluded_session_ids = {
        p["session_id"] for p in prepared if not p["handeye_local"]["ok"]
    }
    if len(preexcluded_session_ids) == len(prepared):
        _emit_progress(
            progress_callback,
            8,
            "phase_a_complete",
            accepted=False,
            reason="all_sessions_failed_local_handeye_gate",
            excluded_sessions=sorted(preexcluded_session_ids),
        )
        return finish(
            status=CalibrationStatus.BLOCKED,
            message=(
                "Phase-A blocked: all sessions failed the local "
                "rotation residual gate"
            ),
            details={
                "sessions": [_public_session(p) for p in prepared],
                "joint_handeye": asdict(handeye),
                "merged_pair_count": len(all_pairs),
                "excluded_sessions": sorted(
                    preexcluded_session_ids
                ),
            },
        )
    _emit_progress(
        progress_callback,
        8,
        "phase_a_start",
        session_count=len(prepared),
        merged_pair_count=len(all_pairs),
        preexcluded_sessions=sorted(preexcluded_session_ids),
    )
    phase_a_started_at = perf_counter()

    def on_phase_a_progress(
        event: str,
        fields: dict[str, Any],
    ) -> None:
        _emit_progress(
            progress_callback,
            8,
            event,
            **fields,
        )

    joint = solve_joint_extrinsic(
        all_pairs,
        handeye.R_IMU_lidar,
        force_rotation_only=force_rotation_only,
        imu=None,
        gyro_bias_rad_s_by_session=gyro_bias_by_session,
        time_offset_s_by_session=time_offset_by_session,
        preexcluded_session_ids=preexcluded_session_ids,
        rotation_prior=r_prior,
        rotation_prior_sigma_deg=(
            15.0 if prior_sigma_deg is None else prior_sigma_deg
        ),
        phase_a_progress_callback=on_phase_a_progress,
        enable_phase_c=not force_rotation_only,
        t_init_m=t_prior,
        t_prior_m=t_prior,
        t_prior_sigma_m=t_prior_sigma,
    )
    included_sessions = [
        item.session_id for item in joint.phase_a_sessions if item.included_in_final
    ]
    excluded_sessions = [
        item.session_id for item in joint.phase_a_sessions if not item.included_in_final
    ]
    _emit_progress(
        progress_callback,
        8,
        "phase_a_complete",
        accepted=joint.phase_a_accepted,
        joint_rms_deg=float(joint.residual_rms_rot_deg),
        rotation_observable=joint.observability.rotation_observable,
        included_sessions=included_sessions,
        excluded_sessions=excluded_sessions,
        elapsed_s=perf_counter() - phase_a_started_at,
    )
    for item in joint.phase_a_sessions:
        _emit_progress(
            progress_callback,
            8,
            "phase_a_session",
            session=item.session_id,
            included=item.included_in_final,
            accepted=item.accepted,
            pair_count=item.pair_count,
            rms_deg=float(item.residual_rms_deg),
            p95_deg=float(item.residual_p95_deg),
            bias_delta_norm_rad_s=float(
                np.linalg.norm(item.gyro_bias_rad_s - item.gyro_bias0_rad_s)
            ),
            gyro_bias_rad_s=np.asarray(item.gyro_bias_rad_s, dtype=float).round(8).tolist(),
        )

    phase_a_by_session = {
        item.session_id: item for item in joint.phase_a_sessions
    }
    session_results = []
    for prep in prepared:
        phase_a = phase_a_by_session.get(prep["session_id"])
        session_bias = joint.gyro_bias_rad_s_per_session.get(prep["session_id"])
        session_results.append(
            {
                **_public_session(prep),
                "vehicle_config_loaded": vehicle_config is not None,
                "handeye": {
                    "residual_rms_deg": handeye.residual_rms_deg,
                    "residual_median_deg": handeye.residual_median_deg,
                    "residual_p95_deg": handeye.residual_p95_deg,
                    "outlier_fraction_gt_5deg": handeye.outlier_fraction_gt_5deg,
                    "pair_count": handeye.pair_count,
                    "ok": handeye.ok,
                    "notes": tuple(list(handeye.notes) + [f"joint over {len(request.sessions)} sessions"]),
                    "R_IMU_lidar": handeye.R_IMU_lidar.tolist(),
                },
                "joint": {
                    "translation_accepted": joint.translation_accepted,
                    "residual_rms_rot_deg": joint.residual_rms_rot_deg,
                    "residual_rms_trans_m": joint.residual_rms_trans_m,
                    "observability": asdict(joint.observability),
                    "notes": joint.notes,
                    "T_IMU_lidar": joint.T_IMU_lidar.tolist(),
                    "phase_a": None if phase_a is None else asdict(phase_a),
                    "gyro_bias_rad_s": None
                    if session_bias is None
                    else np.asarray(session_bias, dtype=float).tolist(),
                    "accel_bias_m_s2": None
                    if joint.accel_bias_m_s2 is None
                    else np.asarray(joint.accel_bias_m_s2, dtype=float).tolist(),
                    "gravity_m_s2": None
                    if joint.gravity_m_s2 is None
                    else np.asarray(joint.gravity_m_s2, dtype=float).tolist(),
                },
                "translation_accepted": joint.translation_accepted,
                "rotation_ok": (
                    phase_a is not None
                    and phase_a.included_in_final
                    and phase_a.accepted
                    and joint.phase_a_accepted
                    and joint.observability.rotation_observable
                ),
                "rotation_prior_constrained": (
                    phase_a is not None
                    and phase_a.included_in_final
                    and phase_a.accepted
                    and joint.phase_a_accepted
                    and not joint.observability.rotation_observable
                    and r_prior is not None
                ),
            }
        )

    T = np.asarray(joint.T_IMU_lidar, dtype=float)
    if request.requested_mode == CalibrationMode.ROTATION_ONLY:
        # A rotation-only result must never expose a seed/prior translation,
        # including when the rotation itself is rejected by a later gate.
        T = T.copy()
        T[:3, 3] = 0.0
    # Multi-session offsets stay in details; the legacy scalar is single-session only.
    delta_t = float(prepared[0]["time_offset_s"]) if len(prepared) == 1 else None
    joint_rotation_ok = joint.phase_a_accepted
    if not joint_rotation_ok:
        status = CalibrationStatus.BLOCKED
        message = (
            f"joint rotation rejected: RMS={joint.residual_rms_rot_deg:.3f} deg "
            "or a retained session failed the Phase-A residual gates"
        )
    elif request.requested_mode == CalibrationMode.FULL_SE3:
        if joint.translation_accepted:
            status = CalibrationStatus.FULL_SE3_ACCEPTED
            message = f"full SE3 accepted (joint {len(prepared)} sessions, {len(all_pairs)} pairs)"
        else:
            status = CalibrationStatus.FULL_SE3_REJECTED
            message = (
                f"rotation accepted jointly ({len(prepared)} sessions); "
                "translation deferred until Phase-B/C session-state redesign"
            )
    elif joint.observability.rotation_observable:
        status = CalibrationStatus.ROTATION_ONLY_ACCEPTED
        message = (
            f"rotation-only calibration accepted "
            f"(joint {len(prepared)} sessions, {len(all_pairs)} pairs)"
        )
        T = T.copy()
        T[:3, 3] = 0.0
    elif r_prior is not None:
        status = CalibrationStatus.ROTATION_ONLY_PRIOR_CONSTRAINED
        message = (
            "rotation residuals passed, but motion does not independently observe all "
            "rotation axes; result remains constrained by the installation prior"
        )
        T = T.copy()
        T[:3, 3] = 0.0
    else:
        status = CalibrationStatus.BLOCKED
        message = "rotation residuals passed but rotation observability failed without a prior"
        T = T.copy()
        T[:3, 3] = 0.0

    return finish(
        status=status,
        message=message,
        details={
            "sessions": session_results,
            "joint": {
                "session_count": len(prepared),
                "merged_pair_count": len(all_pairs),
                "pair_counts_per_session": {p["session_id"]: p["pair_count"] for p in prepared},
                "time_offset_s_per_session": {p["session_id"]: p["time_offset_s"] for p in prepared},
                "handeye_rms_deg": handeye.residual_rms_deg,
                "handeye_p95_deg": handeye.residual_p95_deg,
                "handeye_outlier_fraction_gt_5deg": handeye.outlier_fraction_gt_5deg,
                "phase_a_accepted": joint.phase_a_accepted,
                "phase_a_comparison": joint.phase_a_comparison,
                "phase_a_sessions": [asdict(item) for item in joint.phase_a_sessions],
                "gyro_bias_rad_s_per_session": {
                    sid: np.asarray(value, dtype=float).tolist()
                    for sid, value in joint.gyro_bias_rad_s_per_session.items()
                },
                "excluded_sessions": [
                    item.session_id for item in joint.phase_a_sessions if not item.included_in_final
                ],
                "joint_rotation_rms_deg": joint.residual_rms_rot_deg,
                "rotation_observable": joint.observability.rotation_observable,
                "translation_accepted": joint.translation_accepted,
            },
            "joint_handeye": asdict(handeye),
        },
        T_IMU_lidar=None if status == CalibrationStatus.BLOCKED else T,
        time_offset_s=delta_t,
        motion_pairs_payload=build_motion_pairs_payload(prepared_sessions=prepared),
    )


def _public_session(session_result: dict[str, Any]) -> dict[str, Any]:
    payload = dict(session_result)
    payload.pop("T_IMU_lidar", None)
    payload.pop("pairs", None)
    payload.pop("gyro_bias_rad_s", None)
    payload.pop("gravity_init_m_s2", None)
    return payload