"""Joint camera--LiDAR extrinsic and clock-offset refinement.

The solver consumes explicit probabilistic 2D--3D correspondences, per-point
LiDAR firing times, and body twists.  It is ROS-independent and deliberately
keeps trajectory estimation outside this boundary.
"""

from __future__ import annotations

import math
import struct
from collections.abc import Callable, Sequence
from dataclasses import asdict, dataclass
from functools import lru_cache
from typing import Literal, TypeAlias

import numpy as np
from numpy.typing import NDArray

from calibrex.core.capture_time import (
    ConstantBodyTwist,
    DeskewedLidarPoint,
    LidarCaptureTimePolicy,
    TimedLidarPoint,
    _static_point_at_later_body_frame,
    deskew_lidar_points_to_reference,
)
from calibrex.core.continuous_trajectory import PiecewiseSE3Trajectory
from calibrex.core.geometry import (
    SE3,
    Vector3,
    _quaternion_multiply_raw_xyzw,
    _tuple3,
    normalize_quaternion_xyzw,
    quaternion_conjugate_xyzw,
)
from calibrex.data.depth import DepthCameraIntrinsics
from calibrex.evaluation.holdout import split_indices
from calibrex.solvers.borer_six_dof_solver import apply_local_se3_delta

FloatArray: TypeAlias = NDArray[np.float64]
ContinuousTimeStatus = Literal[
    "converged",
    "max_evaluations",
    "insufficient_correspondences",
    "missing_holdout",
    "degenerate_motion",
    "at_bound",
]


@dataclass(frozen=True)
class TimedProbabilisticImageCorrespondence:
    """One LiDAR firing and its image-plane probability distribution."""

    correspondence_id: str
    point_lidar_m: tuple[float, float, float]
    capture_offset_sec: float
    image_mean_px: tuple[float, float]
    image_covariance_px2: tuple[float, float, float, float]
    outlier_probability: float = 0.0
    reliability: float = 1.0

    def __post_init__(self) -> None:
        values = (
            *self.point_lidar_m,
            self.capture_offset_sec,
            *self.image_mean_px,
            *self.image_covariance_px2,
            self.outlier_probability,
            self.reliability,
        )
        if not self.correspondence_id or not all(
            math.isfinite(value) for value in values
        ):
            raise ValueError("timed correspondence values must be finite")
        a, b, c, d = self.image_covariance_px2
        if not math.isclose(b, c, rel_tol=1.0e-9, abs_tol=1.0e-12):
            raise ValueError("image covariance must be symmetric")
        if a <= 0.0 or d <= 0.0 or a * d - b * c <= 0.0:
            raise ValueError("image covariance must be positive definite")
        if not 0.0 <= self.outlier_probability <= 1.0:
            raise ValueError("outlier_probability must be in [0, 1]")
        if not 0.0 <= self.reliability <= 1.0:
            raise ValueError("reliability must be in [0, 1]")


@dataclass(frozen=True)
class ContinuousTimeCameraLidarCapture:
    """One camera exposure paired with a sequential LiDAR scan."""

    capture_id: str
    lidar_message_stamp_sec: float
    camera_exposure_time_sec: float
    correspondences: tuple[TimedProbabilisticImageCorrespondence, ...]
    twist: ConstantBodyTwist
    transform_body_lidar: SE3
    camera: DepthCameraIntrinsics
    rolling_shutter_readout_sec: float = 0.0
    rolling_shutter_direction: Literal[
        "top_to_bottom", "bottom_to_top"
    ] = "top_to_bottom"
    body_trajectory: PiecewiseSE3Trajectory | None = None

    def __post_init__(self) -> None:
        if not self.capture_id:
            raise ValueError("capture_id must not be empty")
        if not math.isfinite(self.lidar_message_stamp_sec) or not math.isfinite(
            self.camera_exposure_time_sec
        ):
            raise ValueError("capture timestamps must be finite")
        identifiers = [item.correspondence_id for item in self.correspondences]
        if len(identifiers) != len(set(identifiers)):
            raise ValueError("correspondence IDs must be unique within a capture")
        if self.camera.projection != "pinhole":
            raise ValueError("continuous-time solver v0.1 supports pinhole cameras")
        if (
            not math.isfinite(self.rolling_shutter_readout_sec)
            or self.rolling_shutter_readout_sec < 0.0
        ):
            raise ValueError("rolling_shutter_readout_sec must be non-negative")


@dataclass(frozen=True)
class ContinuousTimeCameraLidarOptions:
    """Bounds, deterministic pattern steps, and evidence gates."""

    holdout_ratio: float = 0.25
    split_seed: int = 0
    minimum_train_correspondences: int = 24
    minimum_holdout_correspondences: int = 8
    minimum_confidence: float = 0.25
    rotation_bound_deg: float = 2.0
    translation_bound_m: float = 0.25
    time_offset_bound_sec: float = 0.05
    initial_rotation_step_deg: float = 0.5
    initial_translation_step_m: float = 0.05
    initial_time_step_sec: float = 0.01
    minimum_rotation_step_deg: float = 0.02
    minimum_translation_step_m: float = 0.002
    minimum_time_step_sec: float = 0.0002
    max_evaluations: int = 500
    cauchy_scale: float = 3.0
    time_sensitivity_probe_sec: float = 0.005
    minimum_time_sensitivity_px_per_sec: float = 0.5
    estimate_time_offset: bool = True
    use_per_point_time: bool = True
    use_covariance: bool = True
    use_rolling_shutter: bool = True

    def __post_init__(self) -> None:
        if not 0.0 < self.holdout_ratio < 1.0:
            raise ValueError("holdout_ratio must be in (0, 1)")
        if self.minimum_train_correspondences < 4:
            raise ValueError("minimum_train_correspondences must be at least four")
        if self.minimum_holdout_correspondences < 4:
            raise ValueError("minimum_holdout_correspondences must be at least four")
        if not 0.0 <= self.minimum_confidence <= 1.0:
            raise ValueError("minimum_confidence must be in [0, 1]")
        bounds = (
            self.rotation_bound_deg,
            self.translation_bound_m,
            self.time_offset_bound_sec,
        )
        if any(value <= 0.0 for value in bounds):
            raise ValueError("continuous-time parameter bounds must be positive")
        if not (
            0.0
            < self.minimum_rotation_step_deg
            <= self.initial_rotation_step_deg
            <= self.rotation_bound_deg
        ):
            raise ValueError("rotation steps and bound are inconsistent")
        if not (
            0.0
            < self.minimum_translation_step_m
            <= self.initial_translation_step_m
            <= self.translation_bound_m
        ):
            raise ValueError("translation steps and bound are inconsistent")
        if not (
            0.0
            < self.minimum_time_step_sec
            <= self.initial_time_step_sec
            <= self.time_offset_bound_sec
        ):
            raise ValueError("time steps and bound are inconsistent")
        if self.max_evaluations < 15:
            raise ValueError("max_evaluations must be at least 15")
        if self.cauchy_scale <= 0.0:
            raise ValueError("cauchy_scale must be positive")
        if self.time_sensitivity_probe_sec <= 0.0:
            raise ValueError("time_sensitivity_probe_sec must be positive")
        if self.minimum_time_sensitivity_px_per_sec <= 0.0:
            raise ValueError(
                "minimum_time_sensitivity_px_per_sec must be positive"
            )


@dataclass(frozen=True)
class ContinuousTimeEvaluation:
    """Uncertainty-normalized reprojection evidence."""

    objective: float
    weighted_reprojection_rmse_px: float | None
    mean_mahalanobis_error: float | None
    valid_correspondence_count: int
    skipped_correspondence_count: int


_DeskewEvaluator: TypeAlias = Callable[
    [
        ContinuousTimeCameraLidarCapture,
        TimedProbabilisticImageCorrespondence,
        LidarCaptureTimePolicy,
        ContinuousTimeCameraLidarOptions,
    ],
    DeskewedLidarPoint,
]
_DESKEW_CACHE_MAX_BYTES = 128 * 1024 * 1024
_DESKEW_CACHE_BYTES_PER_POINT = 4096


@dataclass(frozen=True)
class ContinuousTimeIteration:
    """One evaluated bounded correction."""

    evaluation: int
    delta_rotation_deg_xyz: tuple[float, float, float]
    delta_translation_m_xyz: tuple[float, float, float]
    time_offset_sec: float
    objective: float
    accepted: bool


@dataclass(frozen=True)
class ContinuousTimeCameraLidarResult:
    """Joint estimate with train/holdout evidence and observability."""

    status: ContinuousTimeStatus
    reason: str
    initial_transform_camera_lidar: SE3
    transform_camera_lidar: SE3
    initial_time_offset_sec: float
    estimated_time_offset_sec: float
    train_capture_ids: tuple[str, ...]
    holdout_capture_ids: tuple[str, ...]
    initial_train_evaluation: ContinuousTimeEvaluation
    final_train_evaluation: ContinuousTimeEvaluation
    initial_holdout_evaluation: ContinuousTimeEvaluation
    final_holdout_evaluation: ContinuousTimeEvaluation
    time_sensitivity_px_per_sec: float
    time_observability_rank: int
    trajectory_model: Literal["constant_body_twist", "piecewise_se3"]
    trace: tuple[ContinuousTimeIteration, ...]
    options: ContinuousTimeCameraLidarOptions

    def as_dict(self) -> dict[str, object]:
        """Return a provenance-rich solver record."""

        return {
            "status": self.status,
            "reason": self.reason,
            "initial_transform_camera_lidar": (
                self.initial_transform_camera_lidar.as_dict()
            ),
            "transform_camera_lidar": self.transform_camera_lidar.as_dict(),
            "initial_time_offset_sec": self.initial_time_offset_sec,
            "estimated_time_offset_sec": self.estimated_time_offset_sec,
            "train_capture_ids": list(self.train_capture_ids),
            "holdout_capture_ids": list(self.holdout_capture_ids),
            "initial_train_evaluation": asdict(self.initial_train_evaluation),
            "final_train_evaluation": asdict(self.final_train_evaluation),
            "initial_holdout_evaluation": asdict(
                self.initial_holdout_evaluation
            ),
            "final_holdout_evaluation": asdict(self.final_holdout_evaluation),
            "time_sensitivity_px_per_sec": self.time_sensitivity_px_per_sec,
            "time_observability_rank": self.time_observability_rank,
            "trajectory_model": self.trajectory_model,
            "trace": [asdict(item) for item in self.trace],
            "options": asdict(self.options),
            "method": "continuous_time_probabilistic_camera_lidar/v0.1",
            "clock_convention": (
                "lidar_sensor_time + dt_lidar = camera_reference_time"
            ),
            "trajectory_boundary": (
                "body motion is supplied by a bounded piecewise SE(3) "
                "trajectory when present, otherwise by a per-capture "
                "constant body twist; trajectory estimation remains external"
            ),
            "papers": [
                {
                    "title": (
                        "Motion-Based Calibration of Multimodal Sensor "
                        "Extrinsics and Timing Offset Estimation"
                    ),
                    "doi": "10.1109/TRO.2016.2596771",
                },
                {
                    "title": (
                        "Spatiotemporal Camera-LiDAR Calibration: A "
                        "Targetless and Structureless Approach"
                    ),
                    "doi": "10.1109/LRA.2020.2969164",
                },
            ],
            "implementation": (
                "independent ROS-free bounded optimizer; no paper code copied"
            ),
        }


class ContinuousTimeCameraLidarSolver:
    """Jointly refine one extrinsic and one LiDAR-to-camera clock offset."""

    def solve(
        self,
        captures: Sequence[ContinuousTimeCameraLidarCapture],
        initial_transform_camera_lidar: SE3,
        capture_time_policy: LidarCaptureTimePolicy,
        options: ContinuousTimeCameraLidarOptions | None = None,
    ) -> ContinuousTimeCameraLidarResult:
        """Optimize training captures and report disjoint holdout evidence."""

        settings = options or ContinuousTimeCameraLidarOptions()
        identifiers = [item.capture_id for item in captures]
        if len(identifiers) != len(set(identifiers)):
            raise ValueError("capture IDs must be unique")
        trajectory_presence = {
            item.body_trajectory is not None for item in captures
        }
        if len(trajectory_presence) > 1:
            raise ValueError(
                "captures must use one consistent trajectory model"
            )
        _validate_trajectory_temporal_support(
            captures,
            capture_time_policy,
            settings,
        )
        ordered = sorted(captures, key=lambda item: item.capture_id)
        train_indices, holdout_indices = split_indices(
            len(ordered), settings.holdout_ratio, settings.split_seed
        )
        train = tuple(ordered[index] for index in train_indices)
        holdout = tuple(ordered[index] for index in holdout_indices)
        deskew = _make_deskew_evaluator(ordered, settings)
        batches = _make_capture_batches(ordered, settings, deskew)

        def evaluate(
            selected: Sequence[ContinuousTimeCameraLidarCapture],
            transform: SE3,
            offset: float,
        ) -> ContinuousTimeEvaluation:
            return _evaluate(
                selected, transform, capture_time_policy, offset, settings, deskew, batches
            )

        initial_time = capture_time_policy.sensor_time_offset_sec
        initial_train = evaluate(
            train,
            initial_transform_camera_lidar,
            initial_time,
        )
        initial_holdout = evaluate(
            holdout,
            initial_transform_camera_lidar,
            initial_time,
        )
        if (
            initial_train.valid_correspondence_count
            < settings.minimum_train_correspondences
        ):
            return _terminal_result(
                "insufficient_correspondences",
                "too few training correspondences passed confidence/projection gates",
                initial_transform_camera_lidar,
                initial_time,
                train,
                holdout,
                initial_train,
                initial_holdout,
                0.0,
                settings,
            )
        if (
            initial_holdout.valid_correspondence_count
            < settings.minimum_holdout_correspondences
        ):
            return _terminal_result(
                "missing_holdout",
                "too few disjoint holdout correspondences passed the gates",
                initial_transform_camera_lidar,
                initial_time,
                train,
                holdout,
                initial_train,
                initial_holdout,
                0.0,
                settings,
            )
        sensitivity = _time_sensitivity(
            train,
            initial_transform_camera_lidar,
            capture_time_policy,
            initial_time,
            settings,
            deskew,
            batches,
        )
        if (
            settings.estimate_time_offset
            and sensitivity < settings.minimum_time_sensitivity_px_per_sec
        ):
            return _terminal_result(
                "degenerate_motion",
                "weak motion makes the clock offset unobservable",
                initial_transform_camera_lidar,
                initial_time,
                train,
                holdout,
                initial_train,
                initial_holdout,
                sensitivity,
                settings,
            )

        delta: FloatArray = np.zeros(7, dtype=float)
        best = initial_train
        steps: FloatArray = np.asarray(
            [
                settings.initial_rotation_step_deg,
                settings.initial_rotation_step_deg,
                settings.initial_rotation_step_deg,
                settings.initial_translation_step_m,
                settings.initial_translation_step_m,
                settings.initial_translation_step_m,
                settings.initial_time_step_sec,
            ],
            dtype=float,
        )
        minimum_steps: FloatArray = np.asarray(
            [
                settings.minimum_rotation_step_deg,
                settings.minimum_rotation_step_deg,
                settings.minimum_rotation_step_deg,
                settings.minimum_translation_step_m,
                settings.minimum_translation_step_m,
                settings.minimum_translation_step_m,
                settings.minimum_time_step_sec,
            ],
            dtype=float,
        )
        bounds: FloatArray = np.asarray(
            [
                settings.rotation_bound_deg,
                settings.rotation_bound_deg,
                settings.rotation_bound_deg,
                settings.translation_bound_m,
                settings.translation_bound_m,
                settings.translation_bound_m,
                settings.time_offset_bound_sec,
            ],
            dtype=float,
        )
        active: NDArray[np.bool_] = np.ones(7, dtype=bool)
        active[6] = settings.estimate_time_offset
        trace = [
            _iteration(1, delta, initial_time, best.objective, accepted=True)
        ]
        evaluations = 1
        while (
            np.any((steps >= minimum_steps) & active)
            and evaluations < settings.max_evaluations
        ):
            candidates: list[
                tuple[float, tuple[float, ...], FloatArray, ContinuousTimeEvaluation]
            ] = []
            for axis in range(7):
                if not active[axis] or steps[axis] < minimum_steps[axis]:
                    continue
                for direction in (-1.0, 1.0):
                    candidate = delta.copy()
                    candidate[axis] = float(
                        np.clip(
                            candidate[axis] + direction * steps[axis],
                            -bounds[axis],
                            bounds[axis],
                        )
                    )
                    if np.array_equal(candidate, delta):
                        continue
                    transform = apply_local_se3_delta(
                        initial_transform_camera_lidar,
                        candidate[:3],
                        candidate[3:6],
                    )
                    result = evaluate(
                        train,
                        transform,
                        initial_time + float(candidate[6]),
                    )
                    evaluations += 1
                    candidates.append(
                        (
                            result.objective,
                            tuple(candidate.tolist()),
                            candidate,
                            result,
                        )
                    )
                    if evaluations >= settings.max_evaluations:
                        break
                if evaluations >= settings.max_evaluations:
                    break
            if not candidates:
                break
            candidate_objective, _tie, candidate_delta, candidate_result = min(
                candidates,
                key=lambda item: (item[0], item[1]),
            )
            accepted = candidate_objective < best.objective - 1.0e-12
            for objective, _tie, candidate, _result in candidates:
                trace.append(
                    _iteration(
                        len(trace) + 1,
                        candidate,
                        initial_time,
                        objective,
                        accepted=(
                            accepted
                            and np.array_equal(candidate, candidate_delta)
                        ),
                    )
                )
            if accepted:
                delta = candidate_delta
                best = candidate_result
            else:
                steps *= 0.5

        transform = apply_local_se3_delta(
            initial_transform_camera_lidar, delta[:3], delta[3:6]
        )
        estimated_time = initial_time + float(delta[6])
        final_holdout = evaluate(
            holdout,
            transform,
            estimated_time,
        )
        at_bound = bool(
            np.any(
                np.isclose(np.abs(delta), bounds, atol=1.0e-12) & active
            )
        )
        converged = bool(np.all((steps < minimum_steps) | ~active))
        status: ContinuousTimeStatus = (
            "at_bound"
            if at_bound
            else "converged"
            if converged
            else "max_evaluations"
        )
        reason = {
            "at_bound": "joint optimum reached a configured correction bound",
            "converged": "all joint parameter steps reached their minima",
            "max_evaluations": "maximum joint objective evaluations reached",
        }[status]
        return ContinuousTimeCameraLidarResult(
            status=status,
            reason=reason,
            initial_transform_camera_lidar=initial_transform_camera_lidar,
            transform_camera_lidar=transform,
            initial_time_offset_sec=initial_time,
            estimated_time_offset_sec=estimated_time,
            train_capture_ids=tuple(item.capture_id for item in train),
            holdout_capture_ids=tuple(item.capture_id for item in holdout),
            initial_train_evaluation=initial_train,
            final_train_evaluation=best,
            initial_holdout_evaluation=initial_holdout,
            final_holdout_evaluation=final_holdout,
            time_sensitivity_px_per_sec=sensitivity,
            time_observability_rank=int(
                sensitivity >= settings.minimum_time_sensitivity_px_per_sec
            ),
            trajectory_model=(
                "piecewise_se3"
                if any(item.body_trajectory is not None for item in captures)
                else "constant_body_twist"
            ),
            trace=tuple(trace),
            options=settings,
        )


def evaluate_continuous_time_camera_lidar(
    captures: Sequence[ContinuousTimeCameraLidarCapture],
    transform_camera_lidar: SE3,
    capture_time_policy: LidarCaptureTimePolicy,
    time_offset_sec: float,
    options: ContinuousTimeCameraLidarOptions | None = None,
) -> ContinuousTimeEvaluation:
    """Evaluate robust covariance-normalized reprojection loss."""

    settings = options or ContinuousTimeCameraLidarOptions()
    return _evaluate(
        captures, transform_camera_lidar, capture_time_policy, time_offset_sec,
        settings, _deskew_correspondence,
    )


def _evaluate(
    captures: Sequence[ContinuousTimeCameraLidarCapture],
    transform_camera_lidar: SE3,
    capture_time_policy: LidarCaptureTimePolicy,
    time_offset_sec: float,
    settings: ContinuousTimeCameraLidarOptions,
    deskew: _DeskewEvaluator,
    batches: dict[int, _CaptureBatch] | None = None,
) -> ContinuousTimeEvaluation:
    policy = LidarCaptureTimePolicy(
        point_offset_unit=capture_time_policy.point_offset_unit,
        stamp_reference=capture_time_policy.stamp_reference,
        sensor_time_offset_sec=time_offset_sec,
    )
    if batches is not None:
        return _evaluate_batches(captures, transform_camera_lidar, policy, settings, batches)
    squared_pixel_errors: list[float] = []
    mahalanobis_errors: list[float] = []
    weights: list[float] = []
    skipped = 0
    transform_point = _prepare_point_transform(transform_camera_lidar)
    for capture in captures:
        selected = [
            item
            for item in capture.correspondences
            if item.reliability * (1.0 - item.outlier_probability)
            >= settings.minimum_confidence
        ]
        for correspondence in selected:
            point = deskew(
                capture, correspondence, policy, settings
            )
            point_camera = transform_point(
                point.position_lidar_at_reference_m
            )
            if point_camera[2] <= 1.0e-9:
                skipped += 1
                continue
            u = capture.camera.fx * point_camera[0] / point_camera[2] + capture.camera.cx
            v = capture.camera.fy * point_camera[1] / point_camera[2] + capture.camera.cy
            if not (
                0.0 <= u < capture.camera.width
                and 0.0 <= v < capture.camera.height
            ):
                skipped += 1
                continue
            residual_u = u - correspondence.image_mean_px[0]
            residual_v = v - correspondence.image_mean_px[1]
            if settings.use_covariance:
                a, b, c, d = correspondence.image_covariance_px2
                determinant = a * d - b * c
                mahalanobis_squared = (
                    d * residual_u * residual_u
                    - (b + c) * residual_u * residual_v
                    + a * residual_v * residual_v
                ) / determinant
            else:
                mahalanobis_squared = (
                    residual_u * residual_u + residual_v * residual_v
                )
            confidence = correspondence.reliability * (
                1.0 - correspondence.outlier_probability
            )
            squared_pixel_errors.append(
                residual_u * residual_u + residual_v * residual_v
            )
            mahalanobis_errors.append(math.sqrt(max(0.0, mahalanobis_squared)))
            weights.append(confidence)
    if not weights:
        return ContinuousTimeEvaluation(
            objective=math.inf,
            weighted_reprojection_rmse_px=None,
            mean_mahalanobis_error=None,
            valid_correspondence_count=0,
            skipped_correspondence_count=skipped,
        )
    weight_array: FloatArray = np.asarray(weights, dtype=float)
    mahalanobis_array: FloatArray = np.asarray(
        mahalanobis_errors, dtype=float
    )
    pixel_error_array: FloatArray = np.asarray(
        squared_pixel_errors, dtype=float
    )
    robust = np.log1p(
        (mahalanobis_array / settings.cauchy_scale) ** 2
    )
    return ContinuousTimeEvaluation(
        objective=float(np.sum(weight_array * robust) / np.sum(weight_array)),
        weighted_reprojection_rmse_px=math.sqrt(
            float(np.sum(weight_array * pixel_error_array))
            / float(np.sum(weight_array))
        ),
        mean_mahalanobis_error=float(
            np.sum(weight_array * mahalanobis_array) / np.sum(weight_array)
        ),
        valid_correspondence_count=len(weights),
        skipped_correspondence_count=skipped,
    )


def _time_sensitivity(
    captures: Sequence[ContinuousTimeCameraLidarCapture],
    transform: SE3,
    policy: LidarCaptureTimePolicy,
    time_offset: float,
    settings: ContinuousTimeCameraLidarOptions,
    deskew: _DeskewEvaluator | None = None,
    batches: dict[int, _CaptureBatch] | None = None,
) -> float:
    step = settings.time_sensitivity_probe_sec
    left = _projected_pixels(
        captures, transform, policy, time_offset - step, settings, deskew, batches
    )
    right = _projected_pixels(
        captures, transform, policy, time_offset + step, settings, deskew, batches
    )
    common = sorted(set(left).intersection(right))
    if not common:
        return 0.0
    squared = [
        (right[key][0] - left[key][0]) ** 2
        + (right[key][1] - left[key][1]) ** 2
        for key in common
    ]
    return math.sqrt(float(np.mean(squared))) / (2.0 * step)


def _projected_pixels(
    captures: Sequence[ContinuousTimeCameraLidarCapture],
    transform: SE3,
    policy: LidarCaptureTimePolicy,
    time_offset: float,
    settings: ContinuousTimeCameraLidarOptions,
    deskew: _DeskewEvaluator | None = None,
    batches: dict[int, _CaptureBatch] | None = None,
) -> dict[tuple[str, str], tuple[float, float]]:
    resolved_policy = LidarCaptureTimePolicy(
        policy.point_offset_unit,
        policy.stamp_reference,
        time_offset,
    )
    output: dict[tuple[str, str], tuple[float, float]] = {}
    transform_point = _prepare_point_transform(transform)
    evaluate_deskew = deskew or _deskew_correspondence
    for capture in captures:
        if batches is not None:
            batch = batches[id(capture)]
            indices, u, v = batch.project(transform, resolved_policy)
            for index, x, y in zip(indices, u, v, strict=True):
                output[(capture.capture_id, batch.selected[index].correspondence_id)] = (
                    float(x), float(y)
                )
            continue
        selected = [
            item
            for item in capture.correspondences
            if item.reliability * (1.0 - item.outlier_probability)
            >= settings.minimum_confidence
        ]
        for correspondence in selected:
            point = evaluate_deskew(
                capture, correspondence, resolved_policy, settings
            )
            point_camera = transform_point(
                point.position_lidar_at_reference_m
            )
            if point_camera[2] <= 1.0e-9:
                continue
            output[(capture.capture_id, point.point_id)] = (
                capture.camera.fx * point_camera[0] / point_camera[2]
                + capture.camera.cx,
                capture.camera.fy * point_camera[1] / point_camera[2]
                + capture.camera.cy,
            )
    return output


def _normalize_quaternions(quaternions: FloatArray) -> FloatArray:
    x, y, z, w = quaternions
    norm = np.sqrt(x * x + y * y + z * z + w * w)
    return np.asarray((x / norm, y / norm, z / norm, w / norm))


def _rotate_points(
    points: FloatArray, quaternions: FloatArray, translation: Vector3,
) -> FloatArray:
    # Use the scalar core's two Hamilton products, including zero terms and
    # both normalizations. A rotation matrix would change rounding and ties.
    x, y, z = points.T
    qx, qy, qz, qw = _normalize_quaternions(quaternions)
    cx, cy, cz, cw = _normalize_quaternions(np.asarray((qx, qy, qz, qw)))
    cx, cy, cz = -cx, -cy, -cz
    ax = qw * x + qx * 0.0 + qy * z - qz * y
    ay = qw * y - qx * z + qy * 0.0 + qz * x
    az = qw * z + qx * y - qy * x + qz * 0.0
    aw = qw * 0.0 - qx * x - qy * y - qz * z
    tx, ty, tz = translation
    return np.column_stack((
        aw * cx + ax * cw + ay * cz - az * cy + tx,
        aw * cy - ax * cz + ay * cw + az * cx + ty,
        aw * cz + ax * cy - ay * cx + az * cw + tz,
    ))


def _transform_points(points: FloatArray, transform: SE3) -> FloatArray:
    return _rotate_points(
        points, np.asarray(transform.rotation_quat_xyzw)[:, None], transform.translation_m
    )


def _axis_angle_quaternions(twist: ConstantBodyTwist, delta: FloatArray) -> FloatArray:
    quaternions = np.zeros((4, delta.size))
    quaternions[3] = 1.0
    if not delta.size:
        return quaternions
    wx, wy, wz = twist.angular_velocity_body_radps
    speed = math.sqrt(wx * wx + wy * wy + wz * wz)
    if speed < 1.0e-15:
        return quaternions
    moving = delta != 0.0
    x, y, z = wx / speed, wy / speed, wz / speed
    angle = speed * -delta[moving]
    # NumPy's vector trig can differ from libm by an ulp. Keep the core's
    # math.sin/cos and batch the remaining ordered arithmetic.
    cosine = np.fromiter(map(math.cos, angle), dtype=np.float64, count=angle.size)
    sine = np.fromiter(map(math.sin, angle), dtype=np.float64, count=angle.size)
    one_minus = 1.0 - cosine
    m00 = cosine + x * x * one_minus
    m01 = x * y * one_minus - z * sine
    m02 = x * z * one_minus + y * sine
    m10 = y * x * one_minus + z * sine
    m11 = cosine + y * y * one_minus
    m12 = y * z * one_minus - x * sine
    m20 = z * x * one_minus - y * sine
    m21 = z * y * one_minus + x * sine
    m22 = cosine + z * z * one_minus
    trace = m00 + m11 + m22
    trace_branch = trace > 0.0
    x_branch = ~trace_branch & (m00 > m11) & (m00 > m22)
    y_branch = ~trace_branch & ~x_branch & (m11 > m22)
    z_branch = ~(trace_branch | x_branch | y_branch)
    result = np.empty((4, angle.size))
    for branch, mask in enumerate((trace_branch, x_branch, y_branch, z_branch)):
        if not np.any(mask):
            continue
        if branch == 0:
            scale = np.sqrt(trace[mask] + 1.0) * 2.0
            result[:, mask] = (
                (m21[mask] - m12[mask]) / scale,
                (m02[mask] - m20[mask]) / scale,
                (m10[mask] - m01[mask]) / scale,
                0.25 * scale,
            )
        elif branch == 1:
            scale = np.sqrt(1.0 + m00[mask] - m11[mask] - m22[mask]) * 2.0
            result[:, mask] = (
                0.25 * scale,
                (m01[mask] + m10[mask]) / scale,
                (m02[mask] + m20[mask]) / scale,
                (m21[mask] - m12[mask]) / scale,
            )
        elif branch == 2:
            scale = np.sqrt(1.0 + m11[mask] - m00[mask] - m22[mask]) * 2.0
            result[:, mask] = (
                (m01[mask] + m10[mask]) / scale,
                0.25 * scale,
                (m12[mask] + m21[mask]) / scale,
                (m02[mask] - m20[mask]) / scale,
            )
        else:
            scale = np.sqrt(1.0 + m22[mask] - m00[mask] - m11[mask]) * 2.0
            result[:, mask] = (
                (m02[mask] + m20[mask]) / scale,
                (m12[mask] + m21[mask]) / scale,
                0.25 * scale,
                (m10[mask] - m01[mask]) / scale,
            )
    # Match quaternion_xyzw_from_rotation_matrix and SE3 construction before
    # the two normalizations performed by rotate_vector_xyzw.
    quaternions[:, moving] = _normalize_quaternions(_normalize_quaternions(result))
    return quaternions


def _constant_twist_points(
    points_body: FloatArray, twist: ConstantBodyTwist, delta: FloatArray,
) -> FloatArray:
    rotated = _rotate_points(points_body, _axis_angle_quaternions(twist, delta), (0.0, 0.0, 0.0))
    return rotated - np.asarray(twist.linear_velocity_body_mps) * delta[:, None]


@dataclass(frozen=True)
class _CaptureBatch:
    capture: ContinuousTimeCameraLidarCapture
    selected: tuple[TimedProbabilisticImageCorrespondence, ...]
    means: FloatArray
    covariances: FloatArray
    confidences: FloatArray
    points: Callable[[bytes, LidarCaptureTimePolicy], FloatArray]

    def project(
        self, transform: SE3, policy: LidarCaptureTimePolicy,
    ) -> tuple[NDArray[np.intp], FloatArray, FloatArray]:
        points = self.points(struct.pack("d", policy.sensor_time_offset_sec), policy)
        x, y, z = _transform_points(points, transform).T
        # Preserve the scalar depth gate, including its NaN behavior.
        positive = ~(z <= 1.0e-9)
        camera = self.capture.camera
        u = camera.fx * x[positive] / z[positive] + camera.cx
        v = camera.fy * y[positive] / z[positive] + camera.cy
        return np.flatnonzero(positive), u, v


def _prepare_capture_batch(
    capture: ContinuousTimeCameraLidarCapture,
    settings: ContinuousTimeCameraLidarOptions,
    deskew: _DeskewEvaluator,
) -> _CaptureBatch:
    selected = tuple(
        point for point in capture.correspondences
        if point.reliability * (1.0 - point.outlier_probability) >= settings.minimum_confidence
    )
    means = np.asarray([p.image_mean_px for p in selected], dtype=np.float64).reshape(-1, 2)
    covariances = np.asarray(
        [p.image_covariance_px2 for p in selected], dtype=np.float64
    ).reshape(-1, 4)
    confidences = np.asarray(
        [p.reliability * (1.0 - p.outlier_probability) for p in selected], dtype=np.float64
    )
    points_lidar = np.asarray([p.point_lidar_m for p in selected], dtype=np.float64).reshape(-1, 3)
    points_body = _transform_points(points_lidar, capture.transform_body_lidar)
    inverse = capture.transform_body_lidar.inverse()
    offsets = np.asarray(
        [p.capture_offset_sec if settings.use_per_point_time else 0.0 for p in selected],
        dtype=np.float64,
    )
    reference_times = np.asarray(
        [_camera_row_exposure_time(capture, p.image_mean_px[1], settings) for p in selected],
        dtype=np.float64,
    )

    @lru_cache(maxsize=2)
    def points(_bits: bytes, policy: LidarCaptureTimePolicy) -> FloatArray:
        if capture.body_trajectory is not None:
            result = np.asarray([
                deskew(capture, p, policy, settings).position_lidar_at_reference_m for p in selected
            ], dtype=np.float64).reshape(-1, 3)
        else:
            scale = 1.0 if policy.point_offset_unit == "seconds" else 1.0e-9
            capture_times = (
                capture.lidar_message_stamp_sec + offsets * scale + policy.sensor_time_offset_sec
            )
            result = _transform_points(
                _constant_twist_points(points_body, capture.twist, reference_times - capture_times),
                inverse,
            )
        result.setflags(write=False)
        return result

    return _CaptureBatch(capture, selected, means, covariances, confidences, points)


def _make_capture_batches(
    captures: Sequence[ContinuousTimeCameraLidarCapture],
    settings: ContinuousTimeCameraLidarOptions,
    deskew: _DeskewEvaluator,
) -> dict[int, _CaptureBatch] | None:
    # The existing conservative allowance also covers the static arrays and
    # two contiguous point arrays per capture. Larger problems keep scalar IO.
    count = sum(len(capture.correspondences) for capture in captures)
    if count * _DESKEW_CACHE_BYTES_PER_POINT > _DESKEW_CACHE_MAX_BYTES:
        return None
    return {id(c): _prepare_capture_batch(c, settings, deskew) for c in captures}


def _evaluate_batches(
    captures: Sequence[ContinuousTimeCameraLidarCapture],
    transform: SE3,
    policy: LidarCaptureTimePolicy,
    settings: ContinuousTimeCameraLidarOptions,
    batches: dict[int, _CaptureBatch],
) -> ContinuousTimeEvaluation:
    errors: list[FloatArray] = []
    mahalanobis: list[FloatArray] = []
    weights: list[FloatArray] = []
    skipped = 0
    for capture in captures:
        batch = batches[id(capture)]
        indices, u, v = batch.project(transform, policy)
        inside = (u >= 0.0) & (u < capture.camera.width) & (v >= 0.0) & (v < capture.camera.height)
        indices, u, v = indices[inside], u[inside], v[inside]
        skipped += len(batch.selected) - indices.size
        residual_u = u - batch.means[indices, 0]
        residual_v = v - batch.means[indices, 1]
        squared_pixel = residual_u * residual_u + residual_v * residual_v
        if settings.use_covariance:
            a, b, c, d = batch.covariances[indices].T
            mahalanobis_squared = (
                d * residual_u * residual_u - (b + c) * residual_u * residual_v
                + a * residual_v * residual_v
            ) / (a * d - b * c)
        else:
            mahalanobis_squared = squared_pixel
        errors.append(squared_pixel)
        # max(0.0, NaN) and signed zeros follow the scalar core's behavior.
        mahalanobis.append(np.sqrt(np.where(mahalanobis_squared > 0.0, mahalanobis_squared, 0.0)))
        weights.append(batch.confidences[indices])
    weight_array = np.concatenate(weights) if weights else np.empty(0, dtype=np.float64)
    if not weight_array.size:
        return ContinuousTimeEvaluation(math.inf, None, None, 0, skipped)
    mahalanobis_array = np.concatenate(mahalanobis)
    pixel_error_array = np.concatenate(errors)
    robust = np.log1p((mahalanobis_array / settings.cauchy_scale) ** 2)
    return ContinuousTimeEvaluation(
        objective=float(np.sum(weight_array * robust) / np.sum(weight_array)),
        weighted_reprojection_rmse_px=math.sqrt(
            float(np.sum(weight_array * pixel_error_array)) / float(np.sum(weight_array))
        ),
        mean_mahalanobis_error=float(
            np.sum(weight_array * mahalanobis_array) / np.sum(weight_array)
        ),
        valid_correspondence_count=weight_array.size,
        skipped_correspondence_count=skipped,
    )


def _prepare_point_transform(transform: SE3) -> Callable[[Vector3], Vector3]:
    rotation = normalize_quaternion_xyzw(transform.rotation_quat_xyzw)
    conjugate = quaternion_conjugate_xyzw(rotation)
    translation = transform.translation_m

    def transform_point(point: Vector3) -> Vector3:
        # Preserve SE3.transform_point's conversion, normalization and Hamilton
        # product order, moving only the point-independent work out of the loop.
        px, py, pz = _tuple3(point)
        rotated = _quaternion_multiply_raw_xyzw(
            _quaternion_multiply_raw_xyzw(rotation, (px, py, pz, 0.0)), conjugate
        )
        return (
            rotated[0] + translation[0],
            rotated[1] + translation[1],
            rotated[2] + translation[2],
        )

    return transform_point


def _prepared_deskew(
    capture: ContinuousTimeCameraLidarCapture,
    correspondence: TimedProbabilisticImageCorrespondence,
    settings: ContinuousTimeCameraLidarOptions,
    inverse: SE3,
) -> Callable[[bytes, LidarCaptureTimePolicy], DeskewedLidarPoint]:
    point_offset = correspondence.capture_offset_sec if settings.use_per_point_time else 0.0
    reference_time = _camera_row_exposure_time(capture, correspondence.image_mean_px[1], settings)
    position_body = capture.transform_body_lidar.transform_point(correspondence.point_lidar_m)

    @lru_cache(maxsize=2)
    def deskew(_offset_bits: bytes, policy: LidarCaptureTimePolicy) -> DeskewedLidarPoint:
        capture_time = policy.point_reference_time_sec(
            capture.lidar_message_stamp_sec, point_offset
        )
        delta = reference_time - capture_time
        if capture.body_trajectory is not None:
            position_reference = capture.body_trajectory.transform_static_point_between_body_frames(
                position_body, source_time_sec=capture_time, target_time_sec=reference_time,
            )
        else:
            position_reference = _static_point_at_later_body_frame(
                position_body, capture.twist, delta
            )
        return DeskewedLidarPoint(
            correspondence.correspondence_id, inverse.transform_point(position_reference),
            capture_time, delta,
        )

    return deskew


def _make_deskew_evaluator(
    captures: Sequence[ContinuousTimeCameraLidarCapture],
    settings: ContinuousTimeCameraLidarOptions,
) -> _DeskewEvaluator:
    count = sum(len(capture.correspondences) for capture in captures)
    if count * _DESKEW_CACHE_BYTES_PER_POINT > _DESKEW_CACHE_MAX_BYTES:
        return _deskew_correspondence
    # Retain the original objects with each key; identities cannot be recycled.
    # The entry set is fixed at construction, even if a foreign point is queried.
    entries = {
        (id(capture), id(item)): (capture, item) for capture in captures
        for item in capture.correspondences
    }
    kernels: dict[
        tuple[int, int], Callable[[bytes, LidarCaptureTimePolicy], DeskewedLidarPoint]
    ] = {}
    inverses: dict[int, SE3] = {}

    def deskew(
        capture: ContinuousTimeCameraLidarCapture,
        correspondence: TimedProbabilisticImageCorrespondence,
        policy: LidarCaptureTimePolicy,
        options: ContinuousTimeCameraLidarOptions,
    ) -> DeskewedLidarPoint:
        key = (id(capture), id(correspondence))
        if options is not settings or key not in entries:
            return _deskew_correspondence(capture, correspondence, policy, options)
        kernel = kernels.get(key)
        if kernel is None:
            inverse = inverses.get(id(capture))
            if inverse is None:
                inverse = capture.transform_body_lidar.inverse()
                inverses[id(capture)] = inverse
            kernel = _prepared_deskew(capture, correspondence, settings, inverse)
            kernels[key] = kernel
        # The policy includes units and stamp semantics. Byte keys additionally
        # preserve signed-zero offsets. Each point retains only two clock states.
        return kernel(struct.pack("d", policy.sensor_time_offset_sec), policy)

    return deskew


def _deskew_correspondence(
    capture: ContinuousTimeCameraLidarCapture,
    correspondence: TimedProbabilisticImageCorrespondence,
    policy: LidarCaptureTimePolicy,
    settings: ContinuousTimeCameraLidarOptions,
) -> DeskewedLidarPoint:
    point_offset = (
        correspondence.capture_offset_sec
        if settings.use_per_point_time
        else 0.0
    )
    reference_time = _camera_row_exposure_time(
        capture, correspondence.image_mean_px[1], settings
    )
    if capture.body_trajectory is not None:
        capture_time = policy.point_reference_time_sec(
            capture.lidar_message_stamp_sec,
            point_offset,
        )
        position_body_capture = capture.transform_body_lidar.transform_point(
            correspondence.point_lidar_m
        )
        position_body_reference = (
            capture.body_trajectory.transform_static_point_between_body_frames(
                position_body_capture,
                source_time_sec=capture_time,
                target_time_sec=reference_time,
            )
        )
        position_lidar_reference = (
            capture.transform_body_lidar.inverse().transform_point(
                position_body_reference
            )
        )
        return DeskewedLidarPoint(
            point_id=correspondence.correspondence_id,
            position_lidar_at_reference_m=position_lidar_reference,
            capture_time_reference_sec=capture_time,
            deskew_delta_sec=reference_time - capture_time,
        )
    result = deskew_lidar_points_to_reference(
        [
            TimedLidarPoint(
                correspondence.correspondence_id,
                correspondence.point_lidar_m,
                point_offset,
            )
        ],
        message_stamp_sec=capture.lidar_message_stamp_sec,
        reference_time_sec=reference_time,
        policy=policy,
        twist=capture.twist,
        transform_body_lidar=capture.transform_body_lidar,
    )
    return result.points[0]


def _camera_row_exposure_time(
    capture: ContinuousTimeCameraLidarCapture,
    image_row_px: float,
    settings: ContinuousTimeCameraLidarOptions,
) -> float:
    if (
        not settings.use_rolling_shutter
        or capture.rolling_shutter_readout_sec == 0.0
    ):
        return capture.camera_exposure_time_sec
    fraction = min(
        1.0,
        max(0.0, image_row_px / float(capture.camera.height - 1)),
    )
    if capture.rolling_shutter_direction == "bottom_to_top":
        fraction = 1.0 - fraction
    return (
        capture.camera_exposure_time_sec
        + fraction * capture.rolling_shutter_readout_sec
    )


def _validate_trajectory_temporal_support(
    captures: Sequence[ContinuousTimeCameraLidarCapture],
    policy: LidarCaptureTimePolicy,
    settings: ContinuousTimeCameraLidarOptions,
) -> None:
    clock_margin = (
        settings.time_offset_bound_sec if settings.estimate_time_offset else 0.0
    )
    for capture in captures:
        trajectory = capture.body_trajectory
        if trajectory is None:
            continue
        required_times = [
            _camera_row_exposure_time(
                capture,
                correspondence.image_mean_px[1],
                settings,
            )
            for correspondence in capture.correspondences
        ]
        for correspondence in capture.correspondences:
            point_offset = (
                correspondence.capture_offset_sec
                if settings.use_per_point_time
                else 0.0
            )
            nominal_time = policy.point_reference_time_sec(
                capture.lidar_message_stamp_sec,
                point_offset,
            )
            required_times.extend(
                (nominal_time - clock_margin, nominal_time + clock_margin)
            )
        if (
            required_times
            and (
                min(required_times) < trajectory.minimum_time_sec
                or max(required_times) > trajectory.maximum_time_sec
            )
        ):
            raise ValueError(
                f"capture {capture.capture_id!r} plus the configured clock "
                "search exceeds the supplied trajectory support "
                f"[{trajectory.minimum_time_sec:.9g}, "
                f"{trajectory.maximum_time_sec:.9g}] seconds"
            )


def _iteration(
    evaluation: int,
    delta: FloatArray,
    initial_time: float,
    objective: float,
    *,
    accepted: bool,
) -> ContinuousTimeIteration:
    return ContinuousTimeIteration(
        evaluation=evaluation,
        delta_rotation_deg_xyz=(
            float(delta[0]),
            float(delta[1]),
            float(delta[2]),
        ),
        delta_translation_m_xyz=(
            float(delta[3]),
            float(delta[4]),
            float(delta[5]),
        ),
        time_offset_sec=initial_time + float(delta[6]),
        objective=objective,
        accepted=accepted,
    )


def _terminal_result(
    status: ContinuousTimeStatus,
    reason: str,
    transform: SE3,
    time_offset: float,
    train: Sequence[ContinuousTimeCameraLidarCapture],
    holdout: Sequence[ContinuousTimeCameraLidarCapture],
    train_evaluation: ContinuousTimeEvaluation,
    holdout_evaluation: ContinuousTimeEvaluation,
    sensitivity: float,
    options: ContinuousTimeCameraLidarOptions,
) -> ContinuousTimeCameraLidarResult:
    return ContinuousTimeCameraLidarResult(
        status=status,
        reason=reason,
        initial_transform_camera_lidar=transform,
        transform_camera_lidar=transform,
        initial_time_offset_sec=time_offset,
        estimated_time_offset_sec=time_offset,
        train_capture_ids=tuple(item.capture_id for item in train),
        holdout_capture_ids=tuple(item.capture_id for item in holdout),
        initial_train_evaluation=train_evaluation,
        final_train_evaluation=train_evaluation,
        initial_holdout_evaluation=holdout_evaluation,
        final_holdout_evaluation=holdout_evaluation,
        time_sensitivity_px_per_sec=sensitivity,
        time_observability_rank=0,
        trajectory_model=(
            "piecewise_se3"
            if any(item.body_trajectory is not None for item in (*train, *holdout))
            else "constant_body_twist"
        ),
        trace=(),
        options=options,
    )
