"""Continuous-time LiDAR-pair extrinsic and clock-offset refinement.

This module deliberately accepts a supplied odometry trajectory. It profiles a
scalar clock offset while re-solving the six-DoF LiDAR extrinsic at every
candidate, so the temporal estimate cannot be credited with a residual change
that is only caused by holding the extrinsic fixed. A future spline/trajectory
solver can replace :class:`OdometryTrack` without changing the profile/holdout
contract.
"""

from __future__ import annotations

import math
import random
from collections import OrderedDict
from collections.abc import Callable, Sequence
from concurrent.futures import ThreadPoolExecutor
from dataclasses import dataclass, field
from os import cpu_count
from typing import TYPE_CHECKING, Literal

import numpy as np

if TYPE_CHECKING:
    from calibrex.data.adaptive_voxel import AdaptivePlaneQueryContext
    from calibrex.solvers.cuda_continuous_time_adapter import CudaRigLinearizer
    from calibrex.solvers.cuda_plane_query_adapter import CudaPlaneQueries
    from calibrex.solvers.numba_continuous_time_adapter import (
        FloatArray,
        IntArray,
        NumbaContinuousTimeQueryContext,
        _CapturedTimestampRows,
    )

from calibrex.core.continuous_time_lidar_train_diagnostics import (
    ContinuousTimeLidarTrainDiagnosticsData,
    TrainProfilePointData,
    build_train_range_bins,
    summarize_train_residuals,
)
from calibrex.core.geometry import SE3, Vector3
from calibrex.core.time import apply_time_offset_ns
from calibrex.data import adaptive_voxel as adaptive_voxel_module
from calibrex.data.adaptive_voxel import (
    AdaptiveVoxelPlaneMap,
    AdaptiveVoxelPolicy,
    build_adaptive_voxel_plane_map,
)
from calibrex.data.livox import (
    LivoxPointRecord,
    VoxelPlaneMap,
    VoxelPlaneQueryContext,
    build_voxel_plane_map,
)
from calibrex.data.odometry_track import OdometryTrack
from calibrex.evaluation.lidar import build_rig_point_to_plane_observations
from calibrex.evaluation.packed_lidar_observations import _PackedObservationBuilder
from calibrex.graph.lidar_point_to_plane import (
    _NATIVE_CORRECTION,
    _NATIVE_LINEARIZE,
    _NATIVE_RESIDUALS,
    LidarRigPointToPlaneEvaluation,
    LidarRigPointToPlaneFactor,
    _ObservationArrays,
    _PackedObservations,
    _PreparedObservationBuilder,
)
from calibrex.solvers.fixed_trajectory_se3_solver import (
    FixedTrajectorySe3ExtrinsicSolver,
    FixedTrajectorySe3SolverOptions,
    FixedTrajectorySe3SolverResult,
)
from calibrex.solvers.robust_lidar_observations import (
    RobustObservationFilterResult,
    _filter_packed_factor_mad,
    filter_lidar_observations_mad,
)

ContinuousTimeLidarPairStatus = Literal[
    "converged",
    "max_iterations",
    "insufficient_constraints",
    "rejected",
]


@dataclass(frozen=True)
class ContinuousTimeLidarPairProblem:
    """Inputs for one continuous-time LiDAR-pair refinement."""

    source_records: Sequence[LivoxPointRecord]
    target_points: Sequence[Vector3]
    target_capture_timestamps_ns: Sequence[int]
    odometry_track: OdometryTrack
    t_base_source: SE3
    initial_t_source_target: SE3
    variable: str
    sensor: str
    voxel_size_m: float
    correspondence_gate_m: float
    train_indices: Sequence[int] | None = None
    holdout_indices: Sequence[int] | None = None


@dataclass(frozen=True)
class ContinuousTimeLidarPairOptions:
    """Controls for deterministic profile optimization over clock offset."""

    initial_time_offset_sec: float = 0.0
    max_abs_time_offset_sec: float = 0.20
    initial_time_step_sec: float = 0.02
    minimum_time_step_sec: float = 0.001
    max_iterations: int = 8
    min_correspondences: int = 6
    max_odometry_extrapolation_s: float = 0.25
    max_source_records: int | None = 6000
    max_target_points_per_split: int | None = 6000
    sampling_seed: int = 0
    holdout_start_fraction: float = 0.8
    outlier_policy: Literal["none", "mad"] = "mad"
    outlier_mad_scale: float = 3.5
    outlier_min_threshold_m: float = 0.02
    outlier_min_inlier_fraction: float = 0.25
    voxel_strategy: Literal["uniform", "adaptive"] = "uniform"
    adaptive_use_uniform_fallback: bool = True
    adaptive_range_reference_m: float = 10.0
    adaptive_range_exponent: float = 0.5
    adaptive_min_voxel_size_m: float = 0.25
    adaptive_max_voxel_size_m: float = 1.5
    adaptive_min_points_per_voxel: int = 3
    adaptive_range_origin_m: Vector3 = (0.0, 0.0, 0.0)
    correspondence_refinement_iterations: int = 1
    solver: FixedTrajectorySe3SolverOptions = field(
        default_factory=FixedTrajectorySe3SolverOptions
    )
    query_backend: Literal["python", "numba_cpu", "cuda"] = "python"
    # Experimental: whole-pipeline gains have not been established.
    cuda_captured_summary: bool = False


@dataclass(frozen=True)
class ContinuousTimeLidarPairIteration:
    """One accepted/rejected time-profile iteration."""

    iteration: int
    candidate_offsets_sec: tuple[float, ...]
    selected_offset_sec: float
    train_rmse_m: float | None
    holdout_rmse_m: float | None
    accepted: bool
    candidate_train_rmse_m: tuple[float | None, ...] = ()
    candidate_holdout_rmse_m: tuple[float | None, ...] = ()
    train_correspondence_count: int = 0
    train_outlier_rejected_count: int = 0


@dataclass(frozen=True)
class ContinuousTimeLidarPairResult:
    """Joint profile result with train/holdout and observability evidence."""

    status: ContinuousTimeLidarPairStatus
    reason: str
    initial_time_offset_sec: float
    estimated_time_offset_sec: float
    initial_t_source_target: SE3
    refined_t_source_target: SE3
    initial_train_rmse_m: float | None
    final_train_rmse_m: float | None
    final_holdout_rmse_m: float | None
    train_correspondence_count: int
    holdout_correspondence_count: int
    outlier_rejected_count: int
    observability: LidarRigPointToPlaneEvaluation | None
    iterations: tuple[ContinuousTimeLidarPairIteration, ...]
    train_diagnostics: ContinuousTimeLidarTrainDiagnosticsData | None = None
    trajectory_model: Literal["piecewise_se3_fixed_odometry"] = (
        "piecewise_se3_fixed_odometry"
    )
    time_offset_sign_convention: str = (
        "positive offset evaluates target capture at odometry time "
        "t_capture + offset"
    )
    query_backend: str = "python"


@dataclass(frozen=True)
class _OffsetEvaluation:
    offset_sec: float
    solver_result: FixedTrajectorySe3SolverResult
    train_factor: LidarRigPointToPlaneFactor
    holdout_factor: LidarRigPointToPlaneFactor | None
    train_correspondence_count: int
    holdout_correspondence_count: int
    holdout_rmse_m: float | None
    train_outlier_rejected_count: int
    holdout_outlier_rejected_count: int
    train_candidate_correspondence_count: int
    outlier_rejected_count: int


@dataclass(frozen=True)
class _FactorBuild:
    factor: LidarRigPointToPlaneFactor
    candidate_correspondence_count: int
    outlier_rejected_count: int


@dataclass(frozen=True)
class _PackedSelection:
    original: tuple[int, ...]
    selected: tuple[int, ...]
    positions: IntArray
    timestamps: _CapturedTimestampRows


@dataclass
class _QueryReuse:
    planes: VoxelPlaneQueryContext | None
    poses: OrderedDict[int, tuple[SE3, SE3, SE3]] = field(default_factory=OrderedDict)
    compiled: NumbaContinuousTimeQueryContext | None = None
    adaptive: AdaptivePlaneQueryContext | None = None
    used_numba: bool = False
    observation_builder: _PreparedObservationBuilder | None = None
    packed_builder: _PackedObservationBuilder | None = None
    executor: ThreadPoolExecutor | None = None
    summary_executor: ThreadPoolExecutor | None = None
    cuda_rig: CudaRigLinearizer | None = None
    cuda_planes: CudaPlaneQueries | None = None
    used_cuda: bool = False
    used_cuda_summary: bool = False
    used_parallel_summary: bool = False
    used_cpu_linearization_cache: bool = False
    used_cpu_parallel_linearization: bool = False
    selections: dict[int, _PackedSelection] = field(default_factory=dict)

    def identity(self) -> str:
        if self.used_cuda or (self.cuda_planes is not None and self.cuda_planes.query_calls > 0):
            from calibrex.solvers.cuda_continuous_time_adapter import cuda_continuous_time_identity

            return cuda_continuous_time_identity(
                associations_gpu=self.cuda_planes is not None and self.cuda_planes.query_calls > 0,
                captured_summary_gpu=self.used_cuda_summary,
                summary_parallel_cpu=self.used_parallel_summary,
            )
        if self.used_numba or (self.compiled is not None and self.compiled.used):
            from calibrex.solvers.numba_continuous_time_adapter import (
                numba_continuous_time_identity,
            )

            return numba_continuous_time_identity(
                summary_parallel_cpu=self.used_parallel_summary,
                linearization_cache_cpu=self.used_cpu_linearization_cache,
                linearization_parallel_cpu=self.used_cpu_parallel_linearization,
            )
        return "python"

    def source_pose(self, timestamp_ns: int, pose: SE3, base: SE3) -> SE3:
        eligible = type(pose) is SE3 and type(base) is SE3
        cached = self.poses.get(timestamp_ns) if eligible else None
        if cached is not None and cached[0] is pose and cached[1] is base:
            self.poses.move_to_end(timestamp_ns)
            return cached[2]
        result = pose.compose(base)
        if eligible:
            self.poses[timestamp_ns] = (pose, base, result)
            if len(self.poses) > 8192:
                self.poses.popitem(last=False)
        return result


class ContinuousTimeLidarPairSolver:
    """Profile a scalar clock offset while re-solving a LiDAR extrinsic."""

    def solve(
        self,
        problem: ContinuousTimeLidarPairProblem,
        options: ContinuousTimeLidarPairOptions | None = None,
    ) -> ContinuousTimeLidarPairResult:
        """Return a deterministic continuous-time profile/holdout result."""

        settings = options or ContinuousTimeLidarPairOptions()
        track = problem.odometry_track
        parallel = (
            settings.query_backend in ("numba_cpu", "cuda") and len(problem.target_points) >= 16384
            and (not settings.max_target_points_per_split
                 or settings.max_target_points_per_split >= 8192)
            and type(self) is ContinuousTimeLidarPairSolver
            and "_evaluate_offset" not in self.__dict__
            and type(self)._evaluate_offset is _NATIVE_OFFSET_EVALUATION
            and type(track) is OdometryTrack
            and "interpolate" not in track.__dict__
            and OdometryTrack.interpolate is _NATIVE_INTERPOLATION
            and "_record_extrapolation" not in track.__dict__
            and OdometryTrack._record_extrapolation is _NATIVE_EXTRAPOLATION
            and LidarRigPointToPlaneFactor.residuals is _NATIVE_RESIDUALS
            and LidarRigPointToPlaneFactor.linearize is _NATIVE_LINEARIZE
            and LidarRigPointToPlaneFactor._corrected_transform is _NATIVE_CORRECTION
            and FixedTrajectorySe3ExtrinsicSolver.solve is _NATIVE_FIXED_SOLVE
        )
        if parallel:
            with ThreadPoolExecutor(max_workers=3, thread_name_prefix="calibrex-lidar") as executor:
                if (settings.query_backend == "numba_cpu"
                    and len(problem.target_points) >= 32768 and (cpu_count() or 1) >= 6
                    and (not settings.max_target_points_per_split
                         or settings.max_target_points_per_split >= 32768)):
                    # Use a separate bounded pool: candidate workers must never
                    # wait on tasks queued to their own occupied executor.
                    with ThreadPoolExecutor(
                        max_workers=3, thread_name_prefix="calibrex-summary",
                    ) as summary_executor:
                        return self._solve(problem, settings, executor, summary_executor)
                return self._solve(problem, settings, executor)
        return self._solve(problem, settings, None)

    def _solve(
        self, problem: ContinuousTimeLidarPairProblem, settings: ContinuousTimeLidarPairOptions,
        executor: ThreadPoolExecutor | None,
        summary_executor: ThreadPoolExecutor | None = None,
    ) -> ContinuousTimeLidarPairResult:

        self._validate(problem, settings)
        train_indices, holdout_indices = _resolve_indices(problem)
        initial_offset = _clip(
            settings.initial_time_offset_sec,
            -settings.max_abs_time_offset_sec,
            settings.max_abs_time_offset_sec,
        )
        source_records = _sample_source_records(
            problem.source_records,
            settings.max_source_records,
            seed=settings.sampling_seed,
        )
        source_plane_map: VoxelPlaneMap | None = (
            build_voxel_plane_map(source_records, problem.voxel_size_m)
            if settings.voxel_strategy == "uniform" or settings.adaptive_use_uniform_fallback
            else None
        )
        adaptive_plane_map = (
            build_adaptive_voxel_plane_map(
                source_records,
                AdaptiveVoxelPolicy(
                    base_voxel_size_m=problem.voxel_size_m,
                    range_reference_m=settings.adaptive_range_reference_m,
                    range_exponent=settings.adaptive_range_exponent,
                    min_voxel_size_m=settings.adaptive_min_voxel_size_m,
                    max_voxel_size_m=settings.adaptive_max_voxel_size_m,
                    min_points_per_voxel=settings.adaptive_min_points_per_voxel,
                    range_origin_m=settings.adaptive_range_origin_m,
                ),
            )
            if settings.voxel_strategy == "adaptive"
            else None
        )
        query_reuse = _QueryReuse(
            VoxelPlaneQueryContext(source_plane_map) if source_plane_map is not None else None,
        )
        query_reuse.executor = executor
        query_reuse.summary_executor = summary_executor
        query_reuse.observation_builder = _PreparedObservationBuilder(problem.target_points)
        if adaptive_plane_map is not None:
            # Historical adaptive-only benchmark archives predate the batch context.
            factory = getattr(adaptive_voxel_module, "AdaptivePlaneQueryContext", None)
            if factory is not None:
                query_reuse.adaptive = factory(adaptive_plane_map)
        if settings.query_backend in ("numba_cpu", "cuda"):
            try:
                from calibrex.solvers.numba_continuous_time_adapter import (
                    NumbaContinuousTimeQueryContext,
                )
            except ImportError as exc:
                raise ValueError(
                    "compiled queries require the optional calibrex[numba] dependency"
                ) from exc

            query_reuse.compiled = NumbaContinuousTimeQueryContext.capture(problem.odometry_track)
            if query_reuse.compiled is not None:
                query_reuse.packed_builder = _PackedObservationBuilder.capture(
                    problem.target_points, query_reuse.planes, query_reuse.adaptive,
                )
                if (query_reuse.packed_builder is not None
                    and apply_time_offset_ns is _NATIVE_TIME_OFFSET
                    and _select_evenly_spaced_indices is _NATIVE_SELECTION
                    and FixedTrajectorySe3ExtrinsicSolver.solve is _NATIVE_FIXED_SOLVE
                    and LidarRigPointToPlaneFactor.residuals is _NATIVE_RESIDUALS
                    and LidarRigPointToPlaneFactor.linearize is _NATIVE_LINEARIZE
                    and OdometryTrack.interpolate is _NATIVE_INTERPOLATION
                    and OdometryTrack._record_extrapolation is _NATIVE_EXTRAPOLATION
                    and "_record_extrapolation" not in problem.odometry_track.__dict__):
                    from calibrex.solvers.numba_continuous_time_adapter import (
                        _CapturedTimestampRows,
                    )

                    for original in (train_indices, holdout_indices):
                        captured_indices = _select_evenly_spaced_indices(
                            original, settings.max_target_points_per_split,
                            seed=settings.sampling_seed,
                        )
                        times = _CapturedTimestampRows.capture(
                            [problem.target_capture_timestamps_ns[i] for i in captured_indices],
                        )
                        if times is not None:
                            positions = np.asarray(captured_indices, dtype=np.int64)
                            positions.flags.writeable = False
                            query_reuse.selections[id(original)] = _PackedSelection(
                                original, captured_indices, positions, times,
                            )
            # Small capture budgets remain on CPU, avoiding CUDA import/context
            # costs. Once captured, GPU batches retain even MAD-trimmed rows.
            if (settings.query_backend == "cuda" and query_reuse.packed_builder is not None
                and min(len(problem.target_points), settings.max_target_points_per_split
                        or len(problem.target_points)) >= 8192):
                try:
                    from calibrex.solvers.cuda_continuous_time_adapter import CudaRigLinearizer
                except ImportError as exc:
                    raise ValueError("CUDA rig queries require calibrex[cuda,numba]") from exc
                query_reuse.cuda_rig = CudaRigLinearizer()
                from calibrex.solvers.cuda_plane_query_adapter import CudaPlaneQueries

                query_reuse.cuda_planes = CudaPlaneQueries.capture(
                    query_reuse.planes, query_reuse.adaptive,
                )
        initial = self._evaluate_offset(
            problem,
            settings,
            source_records,
            source_plane_map,
            adaptive_plane_map,
            initial_offset,
            train_indices,
            holdout_indices,
            problem.initial_t_source_target,
            query_reuse,
        )
        if initial is None:
            return ContinuousTimeLidarPairResult(
                status="insufficient_constraints",
                reason="initial time offset produced too few point-to-plane correspondences",
                initial_time_offset_sec=initial_offset,
                estimated_time_offset_sec=initial_offset,
                initial_t_source_target=problem.initial_t_source_target,
                refined_t_source_target=problem.initial_t_source_target,
                initial_train_rmse_m=None,
                final_train_rmse_m=None,
                final_holdout_rmse_m=None,
                train_correspondence_count=0,
                holdout_correspondence_count=0,
                outlier_rejected_count=0,
                observability=None,
                iterations=(),
                query_backend=query_reuse.identity(),
            )

        current = initial
        initial_train_rmse = initial.solver_result.final_rmse_m
        step = max(settings.minimum_time_step_sec, settings.initial_time_step_sec)
        trace: list[ContinuousTimeLidarPairIteration] = []
        status: ContinuousTimeLidarPairStatus = "max_iterations"
        reason = "maximum time-profile iterations reached"
        for iteration in range(1, settings.max_iterations + 1):
            candidates = _candidate_offsets(
                current.offset_sec,
                step,
                settings.max_abs_time_offset_sec,
            )
            def evaluate_candidate(
                offset: float, initial_transform: SE3 = current.solver_result.refined_transform,
            ) -> _OffsetEvaluation | None:
                return self._evaluate_offset(
                    problem,
                    settings,
                    source_records,
                    source_plane_map,
                    adaptive_plane_map,
                    offset,
                    train_indices,
                    holdout_indices,
                    initial_transform,
                    query_reuse,
                )
            if query_reuse.executor is not None and query_reuse.packed_builder is not None:
                # Results retain candidate order, so all selection and trace ties
                # follow the serial contract. Track counters are recorded under
                # its existing lock and immutable numeric batches are shared.
                candidate_results = list(query_reuse.executor.map(evaluate_candidate, candidates))
            else:
                candidate_results = [evaluate_candidate(offset) for offset in candidates]
            evaluated = [candidate for candidate in candidate_results if candidate is not None]
            if not evaluated:
                step *= 0.5
                if step < settings.minimum_time_step_sec:
                    status = "rejected"
                    reason = "time-profile candidates became insufficiently constrained"
                    break
                continue
            selected = min(
                evaluated,
                key=lambda item: (
                    float("inf")
                    if item.solver_result.final_rmse_m is None
                    else item.solver_result.final_rmse_m,
                    abs(item.offset_sec),
                    item.offset_sec,
                ),
            )
            current_rmse = current.solver_result.final_rmse_m
            selected_rmse = selected.solver_result.final_rmse_m
            accepted = (
                selected_rmse is not None
                and (current_rmse is None or selected_rmse < current_rmse - 1.0e-10)
            )
            trace.append(
                ContinuousTimeLidarPairIteration(
                    iteration=iteration,
                    candidate_offsets_sec=tuple(candidates),
                    candidate_train_rmse_m=tuple(
                        next(
                            (
                                candidate.solver_result.final_rmse_m
                                for candidate in evaluated
                                if candidate.offset_sec == offset
                            ),
                            None,
                        )
                        for offset in candidates
                    ),
                    candidate_holdout_rmse_m=tuple(
                        next(
                            (
                                candidate.holdout_rmse_m
                                for candidate in evaluated
                                if candidate.offset_sec == offset
                            ),
                            None,
                        )
                        for offset in candidates
                    ),
                    selected_offset_sec=selected.offset_sec,
                    train_rmse_m=selected_rmse,
                    holdout_rmse_m=selected.holdout_rmse_m,
                    accepted=accepted,
                    train_correspondence_count=selected.train_correspondence_count,
                    train_outlier_rejected_count=selected.train_outlier_rejected_count,
                )
            )
            if accepted:
                current = selected
            else:
                step *= 0.5
            if step < settings.minimum_time_step_sec:
                status = "converged"
                reason = "time-profile step reached the configured minimum"
                break
        else:
            if step < settings.minimum_time_step_sec:
                status = "converged"
                reason = "time-profile step reached the configured minimum"

        for _ in range(settings.correspondence_refinement_iterations):
            refined = self._evaluate_offset(
                problem,
                settings,
                source_records,
                source_plane_map,
                adaptive_plane_map,
                current.offset_sec,
                train_indices,
                holdout_indices,
                current.solver_result.refined_transform,
                query_reuse,
            )
            if refined is None:
                break
            current_rmse = current.solver_result.final_rmse_m
            refined_rmse = refined.solver_result.final_rmse_m
            if (
                refined_rmse is None
                or (current_rmse is not None and refined_rmse > current_rmse + 1.0e-10)
            ):
                break
            current = refined

        evaluation = current.train_factor.evaluate(current.solver_result.correction)
        final_residuals = current.train_factor.residuals(
            current.solver_result.correction
        )
        final_ranges = current.train_factor._point_ranges()
        train_diagnostics = ContinuousTimeLidarTrainDiagnosticsData(
            sampled_source_record_count=len(source_records),
            train_target_point_count=len(train_indices),
            train_candidate_correspondence_count=(
                current.train_candidate_correspondence_count
            ),
            train_correspondence_count=current.train_correspondence_count,
            train_outlier_rejected_count=current.train_outlier_rejected_count,
            selected_time_offset_sec=current.offset_sec,
            final_residuals=summarize_train_residuals(final_residuals),
            range_bins=build_train_range_bins(final_ranges, final_residuals),
            profile=tuple(
                TrainProfilePointData(
                    iteration=item.iteration,
                    candidate_offsets_sec=item.candidate_offsets_sec,
                    candidate_train_rmse_m=item.candidate_train_rmse_m,
                    selected_offset_sec=item.selected_offset_sec,
                    train_rmse_m=item.train_rmse_m,
                    train_correspondence_count=item.train_correspondence_count,
                    train_outlier_rejected_count=item.train_outlier_rejected_count,
                    accepted=item.accepted,
                )
                for item in trace
            ),
        )
        return ContinuousTimeLidarPairResult(
            status=status,
            reason=reason,
            initial_time_offset_sec=initial_offset,
            estimated_time_offset_sec=current.offset_sec,
            initial_t_source_target=problem.initial_t_source_target,
            refined_t_source_target=current.solver_result.refined_transform,
            initial_train_rmse_m=initial_train_rmse,
            final_train_rmse_m=current.solver_result.final_rmse_m,
            final_holdout_rmse_m=current.holdout_rmse_m,
            train_correspondence_count=current.train_correspondence_count,
            holdout_correspondence_count=current.holdout_correspondence_count,
            outlier_rejected_count=current.outlier_rejected_count,
            observability=evaluation,
            iterations=tuple(trace),
            train_diagnostics=train_diagnostics,
            query_backend=query_reuse.identity(),
        )

    def _evaluate_offset(
        self,
        problem: ContinuousTimeLidarPairProblem,
        settings: ContinuousTimeLidarPairOptions,
        source_records: Sequence[LivoxPointRecord],
        source_plane_map: VoxelPlaneMap | None,
        adaptive_plane_map: AdaptiveVoxelPlaneMap | None,
        offset_sec: float,
        train_indices: Sequence[int],
        holdout_indices: Sequence[int],
        initial_transform: SE3,
        query_reuse: _QueryReuse | None = None,
    ) -> _OffsetEvaluation | None:
        train_build = self._factor_for_indices(
            problem,
            settings,
            source_records,
            source_plane_map,
            adaptive_plane_map,
            offset_sec,
            train_indices,
            initial_transform,
            query_reuse,
        )
        if train_build is None:
            return None
        train_factor = train_build.factor
        solver_result = FixedTrajectorySe3ExtrinsicSolver().solve(
            train_factor,
            settings.solver,
        )
        holdout_factor = None
        holdout_rmse = None
        holdout_outlier_rejected_count = 0
        if holdout_indices:
            holdout_build = self._factor_for_indices(
                problem,
                settings,
                source_records,
                source_plane_map,
                adaptive_plane_map,
                offset_sec,
                holdout_indices,
                initial_transform,
                query_reuse,
            )
            if holdout_build is not None:
                holdout_factor = holdout_build.factor
                holdout_outlier_rejected_count = holdout_build.outlier_rejected_count
                holdout_rmse = _factor_rmse(holdout_factor, solver_result.correction)
        return _OffsetEvaluation(
            offset_sec=offset_sec,
            solver_result=solver_result,
            train_factor=train_factor,
            holdout_factor=holdout_factor,
            train_correspondence_count=train_factor._observation_count(),
            holdout_correspondence_count=(
                holdout_factor._observation_count() if holdout_factor is not None else 0
            ),
            holdout_rmse_m=holdout_rmse,
            train_outlier_rejected_count=train_build.outlier_rejected_count,
            holdout_outlier_rejected_count=holdout_outlier_rejected_count,
            train_candidate_correspondence_count=(
                train_build.candidate_correspondence_count
            ),
            outlier_rejected_count=(
                train_build.outlier_rejected_count + holdout_outlier_rejected_count
            ),
        )

    def _factor_for_indices(
        self,
        problem: ContinuousTimeLidarPairProblem,
        settings: ContinuousTimeLidarPairOptions,
        source_records: Sequence[LivoxPointRecord],
        source_plane_map: VoxelPlaneMap | None,
        adaptive_plane_map: AdaptiveVoxelPlaneMap | None,
        offset_sec: float,
        indices: Sequence[int],
        initial_transform: SE3,
        query_reuse: _QueryReuse | None = None,
    ) -> _FactorBuild | None:
        captured = query_reuse.selections.get(id(indices)) if query_reuse is not None else None
        if (captured is not None and (captured.original is not indices
            or apply_time_offset_ns is not _NATIVE_TIME_OFFSET
            or _select_evenly_spaced_indices is not _NATIVE_SELECTION)):
            captured = None
        selected_indices = (captured.selected if captured is not None else
                            _select_evenly_spaced_indices(
                                indices, settings.max_target_points_per_split,
                                seed=settings.sampling_seed,
                            ))
        target_points: list[Vector3] | None = (
            None if captured is not None else
            [problem.target_points[index] for index in selected_indices]
        )
        target_poses: list[SE3] = []
        coordinates = None
        compiled = None
        packed: _PackedObservations | None = None
        rows_evaluated = False
        if (query_reuse is not None and query_reuse.compiled is not None
            and query_reuse.packed_builder is not None and len(selected_indices) >= 64):
            positions = (captured.positions if captured is not None
                         else np.asarray(selected_indices, dtype=np.int64))
            prepared = (captured.timestamps.shifted(
                apply_time_offset_ns(0, offset_sec), int(query_reuse.compiled.times[0]),
            ) if captured is not None else None)
            if prepared is not None:
                batch = query_reuse.compiled._evaluate_prepared_rows(
                    problem.odometry_track, prepared, query_reuse.packed_builder.points[positions],
                    problem.t_base_source, initial_transform,
                    settings.max_odometry_extrapolation_s,
                )
            else:
                batch = query_reuse.compiled._evaluate_rows(
                    problem.odometry_track,
                    [apply_time_offset_ns(problem.target_capture_timestamps_ns[index], offset_sec)
                     for index in selected_indices], query_reuse.packed_builder.points[positions],
                    problem.t_base_source, initial_transform,
                    settings.max_odometry_extrapolation_s,
                )
            if batch is not None:
                rows, coordinates, usable = batch
                if not usable:
                    return None
                rows_evaluated = True
                from calibrex.solvers.numba_continuous_time_adapter import (
                    _pack_rig_observations,
                )

                packed = query_reuse.packed_builder.build(
                    positions, rows, coordinates, voxel_size_m=problem.voxel_size_m,
                    correspondence_gate_m=problem.correspondence_gate_m,
                    _position_query=(query_reuse.cuda_planes.query
                                     if query_reuse.cuda_planes is not None else None),
                    _array_packer=_pack_rig_observations,
                )
                if packed is None:
                    # Query counters were already recorded. Materialize those
                    # same normalized rows without interpolating a second time.
                    from calibrex.solvers.numba_continuous_time_adapter import (
                        _materialize_pose_rows,
                    )
                    target_poses = _materialize_pose_rows(rows)
        if not rows_evaluated and query_reuse is not None and query_reuse.compiled is not None:
            target_points = [problem.target_points[index] for index in selected_indices]
            compiled = query_reuse.compiled.evaluate(
                problem.odometry_track,
                [apply_time_offset_ns(problem.target_capture_timestamps_ns[index], offset_sec)
                 for index in selected_indices], target_points, problem.t_base_source,
                initial_transform, settings.max_odometry_extrapolation_s,
            )
        if compiled is not None:
            target_poses, coordinates, usable = compiled
            if not usable:
                return None
        for index in (() if compiled is not None or rows_evaluated else selected_indices):
            capture_ns = apply_time_offset_ns(
                problem.target_capture_timestamps_ns[index], offset_sec
            )
            pose, _clamped, extrapolation_s = problem.odometry_track.interpolate(capture_ns)
            if extrapolation_s > settings.max_odometry_extrapolation_s:
                return None
            target_poses.append(
                query_reuse.source_pose(capture_ns, pose, problem.t_base_source)
                if query_reuse is not None else pose.compose(problem.t_base_source)
            )
        if packed is None and target_points is None:
            target_points = [problem.target_points[index] for index in selected_indices]
        observations = packed if packed is not None else build_rig_point_to_plane_observations(
            source_records=list(source_records),
            target_points=target_points or [],
            initial_t_source_target=initial_transform,
            target_t_world_source=target_poses,
            voxel_size_m=problem.voxel_size_m,
            correspondence_gate_m=problem.correspondence_gate_m,
            source_plane_map=source_plane_map,
            adaptive_plane_map=adaptive_plane_map,
            source_plane_queries=query_reuse.planes if query_reuse is not None else None,
            _prepared_world_coordinates=coordinates,
            _adaptive_queries=query_reuse.adaptive if query_reuse is not None else None,
            _observation_builder=(
                query_reuse.observation_builder if query_reuse is not None else None
            ),
        )
        if len(observations) < settings.min_correspondences:
            return None
        candidate_correspondence_count = len(observations)
        outlier_rejected_count = 0
        array_kernel: Callable[[_ObservationArrays, SE3], list[float]] | None = None
        array_rmse_kernel: Callable[[_ObservationArrays, SE3], tuple[float] | None] | None = None
        array_linearizer: Callable[
            [_ObservationArrays, Sequence[SE3], Sequence[float]], tuple[FloatArray, FloatArray],
        ] | None = None
        array_normal_kernel: Callable[
            [FloatArray, FloatArray, bool, float], tuple[list[list[float]], list[float]],
        ] | None = None
        if settings.query_backend in ("numba_cpu", "cuda"):
            from calibrex.solvers.numba_continuous_time_adapter import (
                NumbaRigLinearizer,
                _numba_parallel_rig_rmse,
                _numba_rig_rmse,
                _numba_rmse_policy,
                _numba_values_rmse,
                numba_rig_linearization,
                numba_rig_normal_equations,
                numba_rig_residuals,
            )
            gpu = (query_reuse.cuda_rig.fork() if query_reuse is not None
                   and query_reuse.cuda_rig is not None else None)
            cpu = (NumbaRigLinearizer(query_reuse.summary_executor)
                   if gpu is None and query_reuse is not None
                   and query_reuse.summary_executor is not None else None)

            def compiled_residuals(arrays: _ObservationArrays, transform: SE3) -> list[float]:
                # Single evaluations are cheaper on CPU. Capture only the
                # retained MAD batch for repeated thirteen-way GPU differences.
                result = numba_rig_residuals(arrays, transform)
                if query_reuse is not None:
                    query_reuse.used_numba = True
                return result

            array_kernel = compiled_residuals

            def compiled_rmse(arrays: _ObservationArrays, transform: SE3) -> tuple[float] | None:
                if (settings.cuda_captured_summary and gpu is not None
                        and _numba_rmse_policy() is not None):
                    values = gpu._captured_residuals(arrays, transform)
                    if values is not None:
                        result = _numba_values_rmse(values)
                        if result is not None:
                            if query_reuse is not None:
                                query_reuse.used_cuda = query_reuse.used_cuda_summary = True
                            return result
                if query_reuse is not None and query_reuse.summary_executor is not None:
                    result = _numba_parallel_rig_rmse(
                        arrays, transform, query_reuse.summary_executor,
                    )
                    if result is not None:
                        query_reuse.used_parallel_summary = True
                        return result
                return _numba_rig_rmse(arrays, transform)

            array_rmse_kernel = compiled_rmse

            def compiled_linearization(
                arrays: _ObservationArrays, transforms: Sequence[SE3], steps: Sequence[float],
            ) -> tuple[FloatArray, FloatArray]:
                if gpu is not None:
                    result = gpu.linearize(arrays, transforms, steps)
                    if query_reuse is not None:
                        query_reuse.used_cuda = True
                else:
                    result = (cpu.linearize(arrays, transforms, steps) if cpu is not None
                              else numba_rig_linearization(arrays, transforms, steps))
                    if cpu is not None and query_reuse is not None:
                        if cpu.cache_hits:
                            query_reuse.used_cpu_linearization_cache = True
                        if cpu.parallel_calls:
                            query_reuse.used_cpu_parallel_linearization = True
                if query_reuse is not None:
                    query_reuse.used_numba = True
                return result

            array_linearizer = compiled_linearization
            array_normal_kernel = numba_rig_normal_equations
        factor = LidarRigPointToPlaneFactor(
            variable=problem.variable, t_ego_lidar=initial_transform, observations=observations,
            sensor=problem.sensor, _array_kernel=array_kernel, _array_linearizer=array_linearizer,
            _array_rmse_kernel=array_rmse_kernel,
            _array_normal_kernel=array_normal_kernel,
        )
        if settings.outlier_policy == "mad":
            rejected = _filter_packed_factor_mad(
                factor, mad_scale=settings.outlier_mad_scale,
                minimum_threshold_m=settings.outlier_min_threshold_m,
                minimum_inlier_fraction=settings.outlier_min_inlier_fraction,
                minimum_inliers=settings.min_correspondences,
            )
            if rejected is None:
                filtered: RobustObservationFilterResult = filter_lidar_observations_mad(
                    factor.observations, variable=problem.variable, t_ego_lidar=initial_transform,
                    sensor=problem.sensor, mad_scale=settings.outlier_mad_scale,
                    minimum_threshold_m=settings.outlier_min_threshold_m,
                    minimum_inlier_fraction=settings.outlier_min_inlier_fraction,
                    minimum_inliers=settings.min_correspondences, _factor=factor,
                )
                rejected = filtered.rejected_count
            outlier_rejected_count = rejected
        if factor._observation_count() < settings.min_correspondences:
            return None
        return _FactorBuild(
            factor=factor,
            candidate_correspondence_count=candidate_correspondence_count,
            outlier_rejected_count=outlier_rejected_count,
        )

    @staticmethod
    def _validate(
        problem: ContinuousTimeLidarPairProblem,
        settings: ContinuousTimeLidarPairOptions,
    ) -> None:
        if settings.query_backend not in ("python", "numba_cpu", "cuda"):
            raise ValueError("unsupported continuous-time query_backend")
        if not isinstance(settings.cuda_captured_summary, bool):
            raise ValueError("cuda_captured_summary must be a boolean")
        if len(problem.target_points) != len(problem.target_capture_timestamps_ns):
            raise ValueError("target points and capture timestamps must have equal length")
        if not problem.target_points:
            raise ValueError("continuous-time LiDAR pair requires target points")
        if problem.voxel_size_m <= 0.0 or problem.correspondence_gate_m <= 0.0:
            raise ValueError("voxel size and correspondence gate must be positive")
        if settings.max_abs_time_offset_sec < 0.0:
            raise ValueError("max_abs_time_offset_sec must be non-negative")
        if settings.initial_time_step_sec <= 0.0 or settings.minimum_time_step_sec <= 0.0:
            raise ValueError("time-profile steps must be positive")
        if settings.minimum_time_step_sec > settings.initial_time_step_sec:
            raise ValueError("minimum_time_step_sec cannot exceed initial_time_step_sec")
        if not 0.0 < settings.holdout_start_fraction < 1.0:
            raise ValueError("holdout_start_fraction must be in (0, 1)")
        if settings.max_iterations < 1 or settings.min_correspondences < 1:
            raise ValueError("max_iterations and min_correspondences must be positive")
        if settings.outlier_policy not in ("none", "mad"):
            raise ValueError("outlier_policy must be 'none' or 'mad'")
        if settings.voxel_strategy not in ("uniform", "adaptive"):
            raise ValueError("voxel_strategy must be 'uniform' or 'adaptive'")
        if settings.outlier_mad_scale < 0.0:
            raise ValueError("outlier_mad_scale must be non-negative")
        if settings.outlier_min_threshold_m < 0.0:
            raise ValueError("outlier_min_threshold_m must be non-negative")
        if not 0.0 < settings.outlier_min_inlier_fraction <= 1.0:
            raise ValueError("outlier_min_inlier_fraction must be in (0, 1]")
        if settings.adaptive_range_reference_m <= 0.0:
            raise ValueError("adaptive_range_reference_m must be positive")
        if settings.adaptive_range_exponent < 0.0:
            raise ValueError("adaptive_range_exponent must be non-negative")
        if settings.adaptive_min_voxel_size_m <= 0.0:
            raise ValueError("adaptive_min_voxel_size_m must be positive")
        if settings.adaptive_max_voxel_size_m < settings.adaptive_min_voxel_size_m:
            raise ValueError("adaptive voxel size bounds are invalid")
        if settings.adaptive_min_points_per_voxel < 1:
            raise ValueError("adaptive_min_points_per_voxel must be positive")
        if settings.correspondence_refinement_iterations < 0:
            raise ValueError("correspondence_refinement_iterations cannot be negative")
        if (
            settings.max_source_records is not None
            and settings.max_source_records < 1
        ):
            raise ValueError("max_source_records must be positive when configured")
        if (
            settings.max_target_points_per_split is not None
            and settings.max_target_points_per_split < 1
        ):
            raise ValueError(
                "max_target_points_per_split must be positive when configured"
            )


def _sample_source_records(
    records: Sequence[LivoxPointRecord],
    limit: int | None,
    *,
    seed: int = 0,
) -> list[LivoxPointRecord]:
    """Select a stable evenly spaced source subset for repeated profiling."""

    indices = _select_evenly_spaced_indices(range(len(records)), limit, seed=seed)
    return [records[index] for index in indices]


def _select_evenly_spaced_indices(
    indices: Sequence[int],
    limit: int | None,
    *,
    seed: int = 0,
) -> tuple[int, ...]:
    """Return deterministic boundary-preserving indices at most ``limit`` long."""

    if limit is None or len(indices) <= limit:
        return tuple(indices)
    if limit < 1:
        raise ValueError("sampling limit must be positive")
    if limit == 1:
        return (indices[0],)
    if seed != 0:
        selected = {indices[0], indices[-1]}
        interior = list(indices[1:-1])
        remaining = max(0, limit - len(selected))
        if remaining:
            selected.update(random.Random(seed).sample(interior, min(remaining, len(interior))))
        return tuple(sorted(selected))
    last_position = len(indices) - 1
    return tuple(
        indices[(position * last_position) // (limit - 1)]
        for position in range(limit)
    )


def _resolve_indices(
    problem: ContinuousTimeLidarPairProblem,
) -> tuple[tuple[int, ...], tuple[int, ...]]:
    count = len(problem.target_points)
    train = tuple(problem.train_indices or range(count))
    holdout = tuple(problem.holdout_indices or ())
    all_indices = set(train) | set(holdout)
    if not all(0 <= index < count for index in all_indices):
        raise ValueError("train/holdout indices are outside target point range")
    if set(train) & set(holdout):
        raise ValueError("train and holdout indices must be disjoint")
    if not train:
        raise ValueError("continuous-time LiDAR pair requires train indices")
    return train, holdout


def _candidate_offsets(current: float, step: float, bound: float) -> list[float]:
    candidates = [
        _clip(current - step, -bound, bound),
        _clip(current, -bound, bound),
        _clip(current + step, -bound, bound),
    ]
    return list(dict.fromkeys(round(value, 12) for value in candidates))


def _clip(value: float, lower: float, upper: float) -> float:
    return min(upper, max(lower, float(value)))


def _rmse(values: Sequence[float]) -> float | None:
    if not values:
        return None
    return math.sqrt(sum(value * value for value in values) / len(values))


_NATIVE_RMSE = _rmse
_NATIVE_SUM = sum


def _factor_rmse(factor: LidarRigPointToPlaneFactor, correction: Sequence[float]) -> float | None:
    summary = (factor._compiled_rmse(correction)
               if _rmse is _NATIVE_RMSE and sum is _NATIVE_SUM else None)
    return summary[0] if summary is not None else _rmse(factor.residuals(correction))


_NATIVE_OFFSET_EVALUATION = ContinuousTimeLidarPairSolver._evaluate_offset
_NATIVE_INTERPOLATION = OdometryTrack.interpolate
_NATIVE_EXTRAPOLATION = OdometryTrack._record_extrapolation
_NATIVE_FIXED_SOLVE = FixedTrajectorySe3ExtrinsicSolver.solve
_NATIVE_TIME_OFFSET = apply_time_offset_ns
_NATIVE_SELECTION = _select_evenly_spaced_indices
