"""LiDAR keyframe selection."""

from __future__ import annotations

from dataclasses import dataclass

import numpy as np

from .contracts import LidarFrame
from .registration import register_lidar_pair


@dataclass(frozen=True)
class KeyframeSet:
    indices: tuple[int, ...]
    frames: tuple[LidarFrame, ...]


def build_keyframes(
    frames: list[LidarFrame],
    *,
    min_translation_m: float = 0.3,
    min_rotation_deg: float = 3.0,
    min_registration_fitness: float = 0.5,
    max_frame_gap: int = 8,
) -> KeyframeSet:
    """Select keyframes with enough relative motion for hand-eye pairs."""

    if not frames:
        return KeyframeSet((), ())

    selected = [0]
    last = 0
    for index in range(1, len(frames)):
        if index - last > max_frame_gap:
            selected.append(index)
            last = index
            continue
        result = register_lidar_pair(frames[index].points_xyz, frames[last].points_xyz)
        if not result.ok or result.fitness < min_registration_fitness:
            continue
        if result.translation_m >= min_translation_m or result.rotation_deg >= min_rotation_deg:
            selected.append(index)
            last = index

    if selected[-1] != len(frames) - 1 and len(frames) > 1:
        selected.append(len(frames) - 1)

    unique = tuple(dict.fromkeys(selected))
    return KeyframeSet(indices=unique, frames=tuple(frames[i] for i in unique))