"""Safe trajectory demonstration, imitation training, and valve planning.

This module deliberately has no dependency on the xArm SDK, camera drivers, or
network clients.  Hardware state readers, robot command sinks, inverse
kinematics, camera calibration, and remote training submission are injected by
the application.  This keeps imports safe on developer machines and makes
hardware execution an explicit opt-in.

Joint values are expressed in radians, Cartesian values in millimetres, and
timestamps in seconds throughout this module.
"""

from __future__ import annotations

import hashlib
import hmac
import json
import math
import os
import tempfile
import threading
import time
from collections import deque
from dataclasses import dataclass, field
from pathlib import Path
from typing import (
    Any,
    Callable,
    Deque,
    Dict,
    Iterable,
    List,
    Mapping,
    Optional,
    Protocol,
    Sequence,
    Tuple,
    Union,
)

import numpy as np

NumberSequence = Sequence[float]
StateLike = Union["TrajectoryFrame", Mapping[str, Any]]


class TrajectoryError(ValueError):
    """Base class for invalid trajectory operations."""


class HardwareExecutionDisabled(RuntimeError):
    """Raised when a live hardware operation was not explicitly enabled."""


class TrajectoryExecutionStopped(RuntimeError):
    """Raised after a stop callback safely interrupts command dispatch."""

    def __init__(self, completed_commands: int) -> None:
        self.completed_commands = int(completed_commands)
        super().__init__(
            "Trajectory execution stopped after {} commands".format(
                self.completed_commands
            )
        )


class TrainingOffloadRequired(RuntimeError):
    """Raised when a training job is too large for the configured local budget."""


@dataclass(frozen=True)
class TrajectoryFrame:
    """One timestamped robot observation in a demonstration."""

    timestamp: float
    joints: Tuple[float, ...]
    tcp_pose: Optional[Tuple[float, ...]] = None
    observation: Mapping[str, float] = field(default_factory=dict)
    gripper: Optional[float] = None

    def __post_init__(self) -> None:
        joints = _finite_tuple(self.joints, "joints")
        if not joints:
            raise TrajectoryError("A trajectory frame must contain joint values")
        object.__setattr__(self, "joints", joints)

        timestamp = float(self.timestamp)
        if not math.isfinite(timestamp):
            raise TrajectoryError("Frame timestamp must be finite")
        object.__setattr__(self, "timestamp", timestamp)

        if self.tcp_pose is not None:
            tcp_pose = _finite_tuple(self.tcp_pose, "tcp_pose")
            if len(tcp_pose) < 3:
                raise TrajectoryError("tcp_pose must contain at least x, y, and z")
            object.__setattr__(self, "tcp_pose", tcp_pose)

        observation: Dict[str, float] = {}
        for key, value in self.observation.items():
            number = float(value)
            if not math.isfinite(number):
                raise TrajectoryError(
                    "Observation {!r} must be finite".format(str(key))
                )
            observation[str(key)] = number
        object.__setattr__(self, "observation", observation)

        if self.gripper is not None:
            gripper = float(self.gripper)
            if not math.isfinite(gripper):
                raise TrajectoryError("gripper must be finite")
            object.__setattr__(self, "gripper", gripper)

    @classmethod
    def from_state(
        cls, state: StateLike, *, timestamp: Optional[float] = None
    ) -> "TrajectoryFrame":
        """Normalise a frame or state mapping returned by an injected reader."""

        if isinstance(state, cls):
            if timestamp is None:
                return state
            return cls(
                timestamp=timestamp,
                joints=state.joints,
                tcp_pose=state.tcp_pose,
                observation=state.observation,
                gripper=state.gripper,
            )

        state_timestamp = timestamp if timestamp is not None else state.get("timestamp")
        if state_timestamp is None:
            raise TrajectoryError("State mapping is missing a timestamp")
        joints = state.get("joints", state.get("joint_positions"))
        if joints is None:
            raise TrajectoryError("State mapping is missing joints")
        return cls(
            timestamp=float(state_timestamp),
            joints=tuple(joints),
            tcp_pose=_optional_tuple(state.get("tcp_pose")),
            observation=dict(state.get("observation", {})),
            gripper=state.get("gripper"),
        )

    def to_dict(self) -> Dict[str, Any]:
        return {
            "timestamp": self.timestamp,
            "joints": list(self.joints),
            "tcp_pose": list(self.tcp_pose) if self.tcp_pose is not None else None,
            "observation": dict(self.observation),
            "gripper": self.gripper,
        }


@dataclass(frozen=True)
class Demonstration:
    """A validated sequence of robot observations."""

    frames: Tuple[TrajectoryFrame, ...]
    name: str = "demonstration"
    metadata: Mapping[str, Any] = field(default_factory=dict)

    def __post_init__(self) -> None:
        frames = tuple(self.frames)
        if len(frames) < 2:
            raise TrajectoryError("A demonstration needs at least two frames")
        joint_count = len(frames[0].joints)
        tcp_sizes = {
            len(frame.tcp_pose) for frame in frames if frame.tcp_pose is not None
        }
        if len(tcp_sizes) > 1:
            raise TrajectoryError("TCP pose dimensions must be consistent")
        previous = frames[0].timestamp
        for index, frame in enumerate(frames):
            if len(frame.joints) != joint_count:
                raise TrajectoryError(
                    "Frame {} has a different joint count".format(index)
                )
            if index and frame.timestamp <= previous:
                raise TrajectoryError("Frame timestamps must be strictly increasing")
            previous = frame.timestamp
        object.__setattr__(self, "frames", frames)
        object.__setattr__(self, "name", str(self.name))
        object.__setattr__(self, "metadata", dict(self.metadata))

    @property
    def duration(self) -> float:
        return self.frames[-1].timestamp - self.frames[0].timestamp

    @property
    def joint_count(self) -> int:
        return len(self.frames[0].joints)

    def to_dict(self) -> Dict[str, Any]:
        return {
            "schema_version": 1,
            "name": self.name,
            "metadata": dict(self.metadata),
            "frames": [frame.to_dict() for frame in self.frames],
        }

    @classmethod
    def from_dict(cls, value: Mapping[str, Any]) -> "Demonstration":
        version = int(value.get("schema_version", 1))
        if version != 1:
            raise TrajectoryError(
                "Unsupported demonstration schema version {}".format(version)
            )
        return cls(
            frames=tuple(
                TrajectoryFrame.from_state(frame) for frame in value["frames"]
            ),
            name=str(value.get("name", "demonstration")),
            metadata=dict(value.get("metadata", {})),
        )


class DemonstrationRecorder:
    """Record injected robot states without importing a hardware SDK.

    ``append`` and ``record_stream`` are always safe and useful for replayed
    state.  ``record_live`` calls a supplied state reader and therefore needs
    both ``dry_run=False`` at construction and ``allow_hardware=True`` at the
    call site.
    """

    def __init__(
        self,
        state_reader: Optional[Callable[[], StateLike]] = None,
        *,
        dry_run: bool = True,
        clock: Callable[[], float] = time.monotonic,
        sleeper: Callable[[float], None] = time.sleep,
    ) -> None:
        self._state_reader = state_reader
        self._dry_run = bool(dry_run)
        self._clock = clock
        self._sleeper = sleeper
        self._frames: List[TrajectoryFrame] = []

    def append(
        self, state: StateLike, *, timestamp: Optional[float] = None
    ) -> TrajectoryFrame:
        frame = TrajectoryFrame.from_state(state, timestamp=timestamp)
        if self._frames and frame.timestamp <= self._frames[-1].timestamp:
            raise TrajectoryError("Recorded timestamps must be strictly increasing")
        self._frames.append(frame)
        return frame

    def record_stream(
        self,
        states: Iterable[StateLike],
        *,
        name: str = "demonstration",
        metadata: Optional[Mapping[str, Any]] = None,
    ) -> Demonstration:
        for state in states:
            self.append(state)
        return self.finish(name=name, metadata=metadata)

    def live_recording_plan(
        self, *, duration: float, sample_rate_hz: float
    ) -> Mapping[str, float]:
        duration = _positive(duration, "duration")
        sample_rate_hz = _positive(sample_rate_hz, "sample_rate_hz")
        return {
            "duration": duration,
            "sample_rate_hz": sample_rate_hz,
            "planned_samples": float(math.floor(duration * sample_rate_hz) + 1),
        }

    def record_live(
        self,
        *,
        duration: float,
        sample_rate_hz: float,
        allow_hardware: bool = False,
        stop_requested: Optional[Callable[[], bool]] = None,
        name: str = "demonstration",
        metadata: Optional[Mapping[str, Any]] = None,
    ) -> Demonstration:
        """Poll the injected reader at a fixed rate after explicit opt-in."""

        plan = self.live_recording_plan(
            duration=duration, sample_rate_hz=sample_rate_hz
        )
        if self._dry_run or not allow_hardware:
            raise HardwareExecutionDisabled(
                "Live recording is disabled; set dry_run=False and "
                "allow_hardware=True explicitly"
            )
        if self._state_reader is None:
            raise TrajectoryError("Live recording requires an injected state_reader")

        period = 1.0 / plan["sample_rate_hz"]
        started = self._clock()
        deadline = started + plan["duration"]
        next_sample = started
        while True:
            now = self._clock()
            if now + 1e-12 < next_sample:
                self._sleeper(next_sample - now)
                now = self._clock()
            if now > deadline + period * 0.5:
                break
            if stop_requested is not None and stop_requested():
                break

            state = self._state_reader()
            self.append(state, timestamp=now - started)
            next_sample += period

        return self.finish(name=name, metadata=metadata)

    def finish(
        self,
        *,
        name: str = "demonstration",
        metadata: Optional[Mapping[str, Any]] = None,
        reset: bool = True,
    ) -> Demonstration:
        demonstration = Demonstration(
            frames=tuple(self._frames),
            name=name,
            metadata=dict(metadata or {}),
        )
        if reset:
            self._frames = []
        return demonstration


class DemonstrationStore:
    """Versioned JSON storage with atomic replacement."""

    def save(self, path: Union[str, os.PathLike], demo: Demonstration) -> Path:
        destination = Path(path)
        destination.parent.mkdir(parents=True, exist_ok=True)
        payload = json.dumps(
            demo.to_dict(), ensure_ascii=False, allow_nan=False, indent=2
        )
        descriptor, temporary_name = tempfile.mkstemp(
            prefix="." + destination.name + ".",
            suffix=".tmp",
            dir=str(destination.parent),
        )
        try:
            with os.fdopen(descriptor, "w", encoding="utf-8") as handle:
                handle.write(payload)
                handle.flush()
                os.fsync(handle.fileno())
            os.replace(temporary_name, destination)
        except Exception:
            try:
                os.unlink(temporary_name)
            except FileNotFoundError:
                pass
            raise
        return destination

    def load(self, path: Union[str, os.PathLike]) -> Demonstration:
        with Path(path).open("r", encoding="utf-8") as handle:
            payload = json.load(handle)
        if not isinstance(payload, Mapping):
            raise TrajectoryError("Demonstration file must contain a JSON object")
        return Demonstration.from_dict(payload)


@dataclass(frozen=True)
class JointLimits:
    lower: Tuple[float, ...]
    upper: Tuple[float, ...]
    max_velocity: Tuple[float, ...]
    max_acceleration: Tuple[float, ...]

    def __post_init__(self) -> None:
        lower = _finite_tuple(self.lower, "lower")
        upper = _finite_tuple(self.upper, "upper")
        max_velocity = _finite_tuple(self.max_velocity, "max_velocity")
        max_acceleration = _finite_tuple(self.max_acceleration, "max_acceleration")
        size = len(lower)
        if not size or any(
            len(values) != size for values in (upper, max_velocity, max_acceleration)
        ):
            raise TrajectoryError("All joint limit arrays must have equal length")
        if any(low >= high for low, high in zip(lower, upper)):
            raise TrajectoryError(
                "Each lower joint limit must be below its upper limit"
            )
        if any(value <= 0 for value in max_velocity + max_acceleration):
            raise TrajectoryError("Velocity and acceleration limits must be positive")
        object.__setattr__(self, "lower", lower)
        object.__setattr__(self, "upper", upper)
        object.__setattr__(self, "max_velocity", max_velocity)
        object.__setattr__(self, "max_acceleration", max_acceleration)

    @classmethod
    def xarm_lite6(cls) -> "JointLimits":
        """Conservative defaults; deployments should use controller limits."""

        lower = tuple(
            math.radians(value) for value in (-180, -135, -135, -180, -135, -180)
        )
        upper = tuple(math.radians(value) for value in (180, 135, 135, 180, 135, 180))
        # Conservative learning/replay limits, not the controller's maxima.
        velocity = tuple(math.radians(60) for _ in range(6))
        acceleration = tuple(math.radians(120) for _ in range(6))
        return cls(lower, upper, velocity, acceleration)


@dataclass(frozen=True)
class WorkspaceLimits:
    lower_xyz: Tuple[float, float, float]
    upper_xyz: Tuple[float, float, float]

    def __post_init__(self) -> None:
        lower = _finite_tuple(self.lower_xyz, "lower_xyz")
        upper = _finite_tuple(self.upper_xyz, "upper_xyz")
        if len(lower) != 3 or len(upper) != 3:
            raise TrajectoryError("Workspace limits must contain x, y, and z")
        if any(low >= high for low, high in zip(lower, upper)):
            raise TrajectoryError(
                "Each lower workspace limit must be below its upper limit"
            )
        object.__setattr__(self, "lower_xyz", lower)
        object.__setattr__(self, "upper_xyz", upper)

    def contains(self, xyz: NumberSequence, *, tolerance: float = 1e-9) -> bool:
        if len(xyz) < 3:
            return False
        return all(
            low - tolerance <= float(value) <= high + tolerance
            for value, low, high in zip(xyz[:3], self.lower_xyz, self.upper_xyz)
        )


@dataclass(frozen=True)
class SafetyViolation:
    kind: str
    frame_index: int
    dimension: Optional[int]
    actual: Optional[float]
    limit: Optional[float]
    message: str


@dataclass(frozen=True)
class SafetyReport:
    violations: Tuple[SafetyViolation, ...]
    max_velocity: Tuple[float, ...]
    max_acceleration: Tuple[float, ...]

    @property
    def safe(self) -> bool:
        return not self.violations


class TrajectorySafetyError(TrajectoryError):
    """An unsafe trajectory and its machine-readable validation report."""

    def __init__(self, report: SafetyReport) -> None:
        self.report = report
        summary = "; ".join(item.message for item in report.violations[:3])
        super().__init__("Unsafe trajectory: " + summary)


class TrajectorySafetyValidator:
    """Validate joints, derivatives, and Cartesian workspace bounds."""

    def __init__(
        self,
        joint_limits: JointLimits,
        *,
        workspace_limits: Optional[WorkspaceLimits] = None,
        forward_kinematics: Optional[
            Callable[[Tuple[float, ...]], NumberSequence]
        ] = None,
        tolerance: float = 1e-8,
    ) -> None:
        self.joint_limits = joint_limits
        self.workspace_limits = workspace_limits
        self.forward_kinematics = forward_kinematics
        self.tolerance = float(tolerance)

    def validate(self, demonstration: Demonstration) -> SafetyReport:
        violations: List[SafetyViolation] = []
        limits = self.joint_limits
        if demonstration.joint_count != len(limits.lower):
            violations.append(
                SafetyViolation(
                    "joint_count",
                    0,
                    None,
                    float(demonstration.joint_count),
                    float(len(limits.lower)),
                    "Trajectory joint count does not match configured limits",
                )
            )
            return SafetyReport(
                tuple(violations),
                tuple(0.0 for _ in limits.lower),
                tuple(0.0 for _ in limits.lower),
            )

        joints = np.asarray(
            [frame.joints for frame in demonstration.frames], dtype=float
        )
        times = np.asarray(
            [frame.timestamp for frame in demonstration.frames], dtype=float
        )
        for frame_index, row in enumerate(joints):
            for dimension, value in enumerate(row):
                low = limits.lower[dimension]
                high = limits.upper[dimension]
                if value < low - self.tolerance:
                    violations.append(
                        SafetyViolation(
                            "joint_lower",
                            frame_index,
                            dimension,
                            float(value),
                            float(low),
                            "Joint {} is below its lower limit".format(dimension),
                        )
                    )
                elif value > high + self.tolerance:
                    violations.append(
                        SafetyViolation(
                            "joint_upper",
                            frame_index,
                            dimension,
                            float(value),
                            float(high),
                            "Joint {} is above its upper limit".format(dimension),
                        )
                    )

        velocity = np.diff(joints, axis=0) / np.diff(times)[:, None]
        max_velocity = np.max(np.abs(velocity), axis=0)
        for interval, row in enumerate(velocity):
            for dimension, value in enumerate(row):
                limit = limits.max_velocity[dimension]
                if abs(value) > limit + self.tolerance:
                    violations.append(
                        SafetyViolation(
                            "velocity",
                            interval + 1,
                            dimension,
                            float(abs(value)),
                            float(limit),
                            "Joint {} velocity exceeds its limit".format(dimension),
                        )
                    )

        if len(velocity) >= 2:
            velocity_dt = (np.diff(times)[:-1] + np.diff(times)[1:]) / 2.0
            acceleration = np.diff(velocity, axis=0) / velocity_dt[:, None]
            max_acceleration_array = np.max(np.abs(acceleration), axis=0)
            for interval, row in enumerate(acceleration):
                for dimension, value in enumerate(row):
                    limit = limits.max_acceleration[dimension]
                    if abs(value) > limit + self.tolerance:
                        violations.append(
                            SafetyViolation(
                                "acceleration",
                                interval + 2,
                                dimension,
                                float(abs(value)),
                                float(limit),
                                "Joint {} acceleration exceeds its limit".format(
                                    dimension
                                ),
                            )
                        )
        else:
            max_acceleration_array = np.zeros(demonstration.joint_count)

        if self.workspace_limits is not None:
            for frame_index, frame in enumerate(demonstration.frames):
                pose: Optional[NumberSequence]
                if self.forward_kinematics is not None:
                    pose = self.forward_kinematics(frame.joints)
                else:
                    pose = frame.tcp_pose
                if pose is None:
                    violations.append(
                        SafetyViolation(
                            "workspace_unverifiable",
                            frame_index,
                            None,
                            None,
                            None,
                            "Workspace safety cannot be verified without TCP pose "
                            "or forward kinematics",
                        )
                    )
                    continue
                for dimension, (value, low, high) in enumerate(
                    zip(
                        pose[:3],
                        self.workspace_limits.lower_xyz,
                        self.workspace_limits.upper_xyz,
                    )
                ):
                    if (
                        float(value) < low - self.tolerance
                        or float(value) > high + self.tolerance
                    ):
                        violations.append(
                            SafetyViolation(
                                "workspace",
                                frame_index,
                                dimension,
                                float(value),
                                float(low if value < low else high),
                                "TCP coordinate {} is outside the workspace".format(
                                    dimension
                                ),
                            )
                        )

        return SafetyReport(
            tuple(violations),
            tuple(float(value) for value in max_velocity),
            tuple(float(value) for value in max_acceleration_array),
        )

    def require_safe(self, demonstration: Demonstration) -> SafetyReport:
        report = self.validate(demonstration)
        if not report.safe:
            raise TrajectorySafetyError(report)
        return report


def resample_demonstration(
    demonstration: Demonstration, *, sample_rate_hz: float
) -> Demonstration:
    """Linearly resample joints, TCP pose, and common numeric observations."""

    sample_rate_hz = _positive(sample_rate_hz, "sample_rate_hz")
    source_times = np.asarray(
        [frame.timestamp for frame in demonstration.frames], dtype=float
    )
    start = source_times[0]
    end = source_times[-1]
    step = 1.0 / sample_rate_hz
    target_times = np.arange(start, end + step * 0.25, step)
    target_times = target_times[target_times <= end + 1e-12]
    if target_times.size == 0 or target_times[-1] < end - 1e-12:
        target_times = np.append(target_times, end)
    else:
        target_times[-1] = min(target_times[-1], end)

    source_joints = np.asarray(
        [frame.joints for frame in demonstration.frames], dtype=float
    )
    joint_columns = [
        np.interp(target_times, source_times, source_joints[:, dimension])
        for dimension in range(demonstration.joint_count)
    ]
    target_joints = np.column_stack(joint_columns)

    poses = [frame.tcp_pose for frame in demonstration.frames]
    target_poses: Optional[np.ndarray] = None
    if all(pose is not None for pose in poses):
        source_poses = np.asarray(poses, dtype=float)
        pose_columns = [
            np.interp(target_times, source_times, source_poses[:, dimension])
            for dimension in range(source_poses.shape[1])
        ]
        target_poses = np.column_stack(pose_columns)

    common_observations = set(demonstration.frames[0].observation)
    for frame in demonstration.frames[1:]:
        common_observations.intersection_update(frame.observation)
    observation_values: Dict[str, np.ndarray] = {}
    for key in sorted(common_observations):
        values = np.asarray(
            [frame.observation[key] for frame in demonstration.frames], dtype=float
        )
        observation_values[key] = np.interp(target_times, source_times, values)

    grippers = [frame.gripper for frame in demonstration.frames]
    target_gripper: Optional[np.ndarray] = None
    if all(value is not None for value in grippers):
        target_gripper = np.interp(
            target_times, source_times, np.asarray(grippers, dtype=float)
        )

    frames = []
    for index, timestamp in enumerate(target_times):
        frames.append(
            TrajectoryFrame(
                timestamp=float(timestamp),
                joints=tuple(float(value) for value in target_joints[index]),
                tcp_pose=(
                    tuple(float(value) for value in target_poses[index])
                    if target_poses is not None
                    else None
                ),
                observation={
                    key: float(values[index])
                    for key, values in observation_values.items()
                },
                gripper=(
                    float(target_gripper[index]) if target_gripper is not None else None
                ),
            )
        )
    metadata = dict(demonstration.metadata)
    metadata["resampled_hz"] = sample_rate_hz
    return Demonstration(tuple(frames), demonstration.name, metadata)


def smooth_demonstration(
    demonstration: Demonstration, *, window_size: int = 5
) -> Demonstration:
    """Apply a centred moving average while preserving demonstrated endpoints."""

    if window_size < 1 or window_size % 2 == 0:
        raise TrajectoryError("window_size must be a positive odd integer")
    if window_size == 1 or len(demonstration.frames) < 3:
        return demonstration

    joints = np.asarray([frame.joints for frame in demonstration.frames], dtype=float)
    radius = window_size // 2
    padded = np.pad(joints, ((radius, radius), (0, 0)), mode="edge")
    kernel = np.ones(window_size, dtype=float) / float(window_size)
    smoothed = np.column_stack(
        [
            np.convolve(padded[:, dimension], kernel, mode="valid")
            for dimension in range(demonstration.joint_count)
        ]
    )
    smoothed[0] = joints[0]
    smoothed[-1] = joints[-1]

    path_changed = not np.allclose(smoothed, joints, rtol=0.0, atol=1e-12)
    frames = tuple(
        TrajectoryFrame(
            timestamp=frame.timestamp,
            joints=tuple(float(value) for value in smoothed[index]),
            # A joint-space filter invalidates the original measured TCP pose.
            # prepare_demonstration can reconstruct it with configured FK.
            tcp_pose=None if path_changed else frame.tcp_pose,
            observation=frame.observation,
            gripper=frame.gripper,
        )
        for index, frame in enumerate(demonstration.frames)
    )
    metadata = dict(demonstration.metadata)
    metadata["smoothing_window"] = window_size
    if path_changed:
        metadata["tcp_pose_invalidated"] = True
    return Demonstration(frames, demonstration.name, metadata)


def prepare_demonstration(
    demonstration: Demonstration,
    *,
    validator: TrajectorySafetyValidator,
    sample_rate_hz: float,
    smoothing_window: int = 5,
) -> Demonstration:
    """Resample, smooth, and slow a demonstration until derivative limits hold.

    Joint and workspace violations are never clipped.  They fail immediately.
    Velocity and acceleration violations can be made safe without changing the
    geometric path, so time is stretched by the required factor.
    """

    prepared = smooth_demonstration(
        resample_demonstration(demonstration, sample_rate_hz=sample_rate_hz),
        window_size=smoothing_window,
    )
    if (
        prepared.metadata.get("tcp_pose_invalidated")
        and validator.forward_kinematics is not None
    ):
        reconstructed_frames = tuple(
            TrajectoryFrame(
                timestamp=frame.timestamp,
                joints=frame.joints,
                tcp_pose=tuple(validator.forward_kinematics(frame.joints)),
                observation=frame.observation,
                gripper=frame.gripper,
            )
            for frame in prepared.frames
        )
        reconstructed_metadata = dict(prepared.metadata)
        reconstructed_metadata["tcp_pose_recomputed_with_fk"] = True
        prepared = Demonstration(
            reconstructed_frames, prepared.name, reconstructed_metadata
        )
    first_report = validator.validate(prepared)
    geometric = tuple(
        item
        for item in first_report.violations
        if item.kind not in ("velocity", "acceleration")
    )
    if geometric:
        raise TrajectorySafetyError(
            SafetyReport(
                geometric,
                first_report.max_velocity,
                first_report.max_acceleration,
            )
        )

    velocity_ratios = [
        actual / limit
        for actual, limit in zip(
            first_report.max_velocity, validator.joint_limits.max_velocity
        )
    ]
    acceleration_ratios = [
        math.sqrt(actual / limit) if actual > 0 else 0.0
        for actual, limit in zip(
            first_report.max_acceleration,
            validator.joint_limits.max_acceleration,
        )
    ]
    time_scale = max([1.0] + velocity_ratios + acceleration_ratios)
    if time_scale > 1.0:
        origin = prepared.frames[0].timestamp
        frames = tuple(
            TrajectoryFrame(
                timestamp=origin + (frame.timestamp - origin) * time_scale,
                joints=frame.joints,
                tcp_pose=frame.tcp_pose,
                observation=frame.observation,
                gripper=frame.gripper,
            )
            for frame in prepared.frames
        )
        metadata = dict(prepared.metadata)
        metadata["time_scale"] = time_scale
        prepared = Demonstration(frames, prepared.name, metadata)
    else:
        metadata = dict(prepared.metadata)
        metadata["time_scale"] = 1.0
        prepared = Demonstration(prepared.frames, prepared.name, metadata)

    validator.require_safe(prepared)
    return prepared


@dataclass(frozen=True)
class ImitationExample:
    features: Tuple[float, ...]
    action: Tuple[float, ...]


@dataclass(frozen=True)
class TrainingConfig:
    feature_names: Tuple[str, ...] = ()
    include_current_joints: bool = True
    action_mode: str = "joint_delta"
    neighbors: int = 3
    max_local_samples: int = 5000
    force_offload: bool = False
    evaluation_tolerance: float = math.radians(5)

    def __post_init__(self) -> None:
        if self.action_mode not in ("joint_delta", "joint_position"):
            raise TrajectoryError(
                "action_mode must be 'joint_delta' or 'joint_position'"
            )
        if self.neighbors < 1:
            raise TrajectoryError("neighbors must be at least one")
        if self.max_local_samples < 1:
            raise TrajectoryError("max_local_samples must be at least one")
        if self.evaluation_tolerance <= 0:
            raise TrajectoryError("evaluation_tolerance must be positive")
        object.__setattr__(
            self, "feature_names", tuple(str(name) for name in self.feature_names)
        )


def demonstrations_to_examples(
    demonstrations: Sequence[Demonstration], config: TrainingConfig
) -> Tuple[ImitationExample, ...]:
    examples: List[ImitationExample] = []
    for demonstration in demonstrations:
        for current, following in zip(demonstration.frames, demonstration.frames[1:]):
            features = []
            for feature_name in config.feature_names:
                if feature_name not in current.observation:
                    raise TrajectoryError(
                        "Observation feature {!r} is missing".format(feature_name)
                    )
                features.append(current.observation[feature_name])
            if config.include_current_joints:
                features.extend(current.joints)
            if not features:
                raise TrajectoryError(
                    "Training needs observation features or current joints"
                )

            if config.action_mode == "joint_delta":
                action = tuple(
                    end - start for start, end in zip(current.joints, following.joints)
                )
            else:
                action = following.joints
            examples.append(ImitationExample(tuple(features), tuple(action)))
    if not examples:
        raise TrajectoryError("No imitation examples could be built")
    feature_count = len(examples[0].features)
    action_count = len(examples[0].action)
    if any(
        len(item.features) != feature_count or len(item.action) != action_count
        for item in examples
    ):
        raise TrajectoryError("Training examples have inconsistent dimensions")
    return tuple(examples)


class ImitationPolicy(Protocol):
    def predict(self, features: NumberSequence) -> Tuple[float, ...]:
        """Predict the next joint action."""


@dataclass(frozen=True)
class NearestNeighborPolicy:
    """Small deterministic baseline suitable for Raspberry Pi evaluation."""

    features: Tuple[Tuple[float, ...], ...]
    actions: Tuple[Tuple[float, ...], ...]
    neighbors: int = 3

    def predict(self, features: NumberSequence) -> Tuple[float, ...]:
        query = np.asarray(features, dtype=float)
        training = np.asarray(self.features, dtype=float)
        if query.shape != (training.shape[1],):
            raise TrajectoryError(
                "Policy expected {} features, received {}".format(
                    training.shape[1], query.size
                )
            )
        distances = np.linalg.norm(training - query, axis=1)
        count = min(self.neighbors, len(distances))
        indices = np.argsort(distances, kind="stable")[:count]
        selected_distances = distances[indices]
        exact = indices[selected_distances <= 1e-12]
        actions = np.asarray(self.actions, dtype=float)
        if len(exact):
            prediction = np.mean(actions[exact], axis=0)
        else:
            weights = 1.0 / np.maximum(selected_distances, 1e-12)
            prediction = np.average(actions[indices], axis=0, weights=weights)
        return tuple(float(value) for value in prediction)


class LocalTrainingBackend(Protocol):
    def fit(
        self, examples: Sequence[ImitationExample], config: TrainingConfig
    ) -> ImitationPolicy:
        """Fit a local imitation policy."""


class NearestNeighborTrainer:
    def fit(
        self, examples: Sequence[ImitationExample], config: TrainingConfig
    ) -> NearestNeighborPolicy:
        return NearestNeighborPolicy(
            features=tuple(item.features for item in examples),
            actions=tuple(item.action for item in examples),
            neighbors=config.neighbors,
        )


@dataclass(frozen=True)
class TrainingJob:
    """Serializable request passed to an injected API gateway/job client."""

    schema_version: int
    config: Mapping[str, Any]
    examples: Tuple[ImitationExample, ...]

    def to_dict(self) -> Dict[str, Any]:
        return {
            "schema_version": self.schema_version,
            "config": dict(self.config),
            "examples": [
                {"features": list(item.features), "action": list(item.action)}
                for item in self.examples
            ],
        }


OffloadHook = Callable[[TrainingJob], Any]


@dataclass(frozen=True)
class TrainingOutcome:
    mode: str
    sample_count: int
    policy: Optional[ImitationPolicy] = None
    offload_result: Any = None


@dataclass(frozen=True)
class EvaluationMetrics:
    sample_count: int
    mean_absolute_error: float
    root_mean_squared_error: float
    max_absolute_error: float
    within_tolerance_fraction: float


class TrajectoryTrainingService:
    """Route small jobs locally and large jobs through an injected offload hook."""

    def __init__(
        self,
        *,
        local_backend: Optional[LocalTrainingBackend] = None,
        offload_hook: Optional[OffloadHook] = None,
    ) -> None:
        self.local_backend = local_backend or NearestNeighborTrainer()
        self.offload_hook = offload_hook

    def train(
        self,
        demonstrations: Sequence[Demonstration],
        config: Optional[TrainingConfig] = None,
    ) -> TrainingOutcome:
        config = config or TrainingConfig()
        examples = demonstrations_to_examples(demonstrations, config)
        should_offload = (
            config.force_offload or len(examples) > config.max_local_samples
        )
        if should_offload:
            if self.offload_hook is None:
                raise TrainingOffloadRequired(
                    "Training requires offload; inject an API gateway/job hook"
                )
            job = TrainingJob(
                schema_version=1,
                config={
                    "feature_names": list(config.feature_names),
                    "include_current_joints": config.include_current_joints,
                    "action_mode": config.action_mode,
                    "neighbors": config.neighbors,
                    "max_local_samples": config.max_local_samples,
                    "force_offload": config.force_offload,
                    "evaluation_tolerance": config.evaluation_tolerance,
                },
                examples=examples,
            )
            return TrainingOutcome(
                mode="offloaded",
                sample_count=len(examples),
                offload_result=self.offload_hook(job),
            )

        policy = self.local_backend.fit(examples, config)
        return TrainingOutcome(mode="local", sample_count=len(examples), policy=policy)

    def evaluate(
        self,
        policy: ImitationPolicy,
        demonstrations: Sequence[Demonstration],
        config: Optional[TrainingConfig] = None,
    ) -> EvaluationMetrics:
        config = config or TrainingConfig()
        examples = demonstrations_to_examples(demonstrations, config)
        predicted = np.asarray(
            [policy.predict(item.features) for item in examples], dtype=float
        )
        expected = np.asarray([item.action for item in examples], dtype=float)
        if predicted.shape != expected.shape:
            raise TrajectoryError(
                "Policy prediction dimensions do not match expected actions"
            )
        error = np.abs(predicted - expected)
        per_sample_max = np.max(error, axis=1)
        return EvaluationMetrics(
            sample_count=len(examples),
            mean_absolute_error=float(np.mean(error)),
            root_mean_squared_error=float(np.sqrt(np.mean(np.square(error)))),
            max_absolute_error=float(np.max(error)),
            within_tolerance_fraction=float(
                np.mean(per_sample_max <= config.evaluation_tolerance)
            ),
        )


@dataclass(frozen=True)
class ValveTarget:
    """A calibrated dynamic valve target in robot coordinates."""

    target_id: str
    center_xyz: Tuple[float, float, float]
    normal_xyz: Tuple[float, float, float]
    confidence: float
    timestamp: float
    coordinate_frame: str = "robot"
    orientation_radians: Optional[float] = None

    def __post_init__(self) -> None:
        center = _finite_tuple(self.center_xyz, "center_xyz")
        normal = np.asarray(_finite_tuple(self.normal_xyz, "normal_xyz"), dtype=float)
        if len(center) != 3 or normal.shape != (3,):
            raise TrajectoryError("Valve center and normal must be three-dimensional")
        magnitude = float(np.linalg.norm(normal))
        if magnitude <= 1e-12:
            raise TrajectoryError("Valve normal cannot be zero")
        confidence = float(self.confidence)
        timestamp = float(self.timestamp)
        if not 0.0 <= confidence <= 1.0:
            raise TrajectoryError("Valve confidence must be between zero and one")
        if not math.isfinite(timestamp):
            raise TrajectoryError("Valve timestamp must be finite")
        object.__setattr__(self, "center_xyz", center)
        object.__setattr__(
            self, "normal_xyz", tuple(float(value / magnitude) for value in normal)
        )
        object.__setattr__(self, "confidence", confidence)
        object.__setattr__(self, "timestamp", timestamp)
        object.__setattr__(self, "target_id", str(self.target_id))
        object.__setattr__(self, "coordinate_frame", str(self.coordinate_frame))
        if self.orientation_radians is not None:
            orientation = float(self.orientation_radians)
            if not math.isfinite(orientation):
                raise TrajectoryError("Valve orientation must be finite")
            object.__setattr__(self, "orientation_radians", orientation)


PointTransform = Callable[
    [Tuple[float, float, float], Tuple[float, float, float], str],
    Tuple[NumberSequence, NumberSequence],
]


def valve_target_from_observation(
    observation: Any,
    *,
    transform_to_robot: Optional[PointTransform] = None,
    default_timestamp: Optional[float] = None,
    default_normal_xyz: Optional[NumberSequence] = None,
) -> ValveTarget:
    """Adapt a camera/vision detection mapping or object to :class:`ValveTarget`.

    The adapter accepts common names such as ``center_xyz``, ``robot_xyz``,
    ``position_xyz``, ``normal_xyz``, and ``axis_xyz``.  A non-robot frame must
    be converted by the injected calibration transform.
    """

    get = _field_getter(observation)
    center = None
    center_field = ""
    for candidate in (
        "robot_xyz",
        "workspace_xyz_mm",
        "center_robot",
        "center_xyz",
        "position_xyz",
        "position",
    ):
        candidate_value = get(candidate)
        if candidate_value is not None:
            center = candidate_value
            center_field = candidate
            break
    if center is None:
        x, y, z = get("x"), get("y"), get("z")
        if None not in (x, y, z):
            center = (x, y, z)
            center_field = "xyz"
    if center is None:
        raise TrajectoryError(
            "Valve observation needs calibrated 3-D coordinates; pixel-only "
            "detections must be resolved by the vision/calibration layer"
        )

    normal = _first_present(get, ("normal_xyz", "axis_xyz", "normal", "axis"))
    if normal is None:
        normal = default_normal_xyz
    if normal is None:
        raise TrajectoryError(
            "Valve observation is missing a surface normal/axis; configure "
            "default_normal_xyz only when the workspace geometry guarantees it"
        )

    frame_value = _first_present(get, ("coordinate_frame", "frame_id", "frame"))
    if frame_value is None:
        if center_field not in ("robot_xyz", "workspace_xyz_mm", "center_robot"):
            raise TrajectoryError(
                "Ambiguous valve coordinates require an explicit coordinate_frame"
            )
        coordinate_frame = "robot"
    else:
        coordinate_frame = str(frame_value)
    center_tuple = _finite_tuple(center, "center")
    normal_tuple = _finite_tuple(normal, "normal")
    if coordinate_frame != "robot":
        if transform_to_robot is None:
            raise TrajectoryError(
                "A camera-to-robot transform is required for frame {!r}".format(
                    coordinate_frame
                )
            )
        transformed_center, transformed_normal = transform_to_robot(
            center_tuple, normal_tuple, coordinate_frame
        )
        center_tuple = _finite_tuple(transformed_center, "transformed center")
        normal_tuple = _finite_tuple(transformed_normal, "transformed normal")
        coordinate_frame = "robot"

    timestamp = _first_present(
        get,
        (
            "timestamp",
            "observed_at_s",
            "captured_at_s",
            "captured_at",
            "monotonic_time",
        ),
    )
    if timestamp is None:
        timestamp = default_timestamp
    if timestamp is None:
        raise TrajectoryError("Valve observation is missing a timestamp")
    confidence = _first_present(get, ("confidence", "score", "probability"))
    if confidence is None:
        confidence = 0.0
    target_id = _first_present(get, ("target_id", "track_id", "id", "label"))
    if target_id is None:
        target_id = "valve"
    orientation_radians = get("orientation_radians")
    if orientation_radians is None:
        orientation_degrees = _first_present(
            get, ("orientation_deg", "angle_deg", "rotation_deg")
        )
        if orientation_degrees is not None:
            orientation_radians = math.radians(float(orientation_degrees))
    return ValveTarget(
        target_id=str(target_id),
        center_xyz=center_tuple,
        normal_xyz=normal_tuple,
        confidence=float(confidence),
        timestamp=float(timestamp),
        coordinate_frame=coordinate_frame,
        orientation_radians=(
            float(orientation_radians) if orientation_radians is not None else None
        ),
    )


@dataclass(frozen=True)
class ValvePrimitiveFrame:
    """One valve-relative Cartesian waypoint in an imitation primitive."""

    timestamp: float
    local_xyz: Tuple[float, float, float]
    orientation_rpy: Tuple[float, float, float]
    gripper: Optional[float] = None

    def __post_init__(self) -> None:
        timestamp = float(self.timestamp)
        if not math.isfinite(timestamp):
            raise TrajectoryError("Primitive timestamp must be finite")
        local_xyz = _finite_tuple(self.local_xyz, "local_xyz")
        if len(local_xyz) != 3:
            raise TrajectoryError("local_xyz must have three values")
        orientation = _finite_tuple(self.orientation_rpy, "orientation_rpy")
        if len(orientation) != 3:
            raise TrajectoryError("orientation_rpy must have three values")
        object.__setattr__(self, "timestamp", timestamp)
        object.__setattr__(self, "local_xyz", local_xyz)
        object.__setattr__(self, "orientation_rpy", orientation)
        if self.gripper is not None:
            gripper = float(self.gripper)
            if not math.isfinite(gripper):
                raise TrajectoryError("Primitive gripper value must be finite")
            object.__setattr__(self, "gripper", gripper)


@dataclass(frozen=True)
class ValveMotionPrimitive:
    """A Cartesian demonstration expressed in the valve's local frame."""

    frames: Tuple[ValvePrimitiveFrame, ...]
    source_target_id: str = "valve"
    reference_orientation_radians: Optional[float] = None

    def __post_init__(self) -> None:
        frames = tuple(self.frames)
        if len(frames) < 2:
            raise TrajectoryError("A valve primitive needs at least two frames")
        previous = frames[0].timestamp
        for index, frame in enumerate(frames):
            if index and frame.timestamp <= previous:
                raise TrajectoryError(
                    "Primitive timestamps must be strictly increasing"
                )
            previous = frame.timestamp
        object.__setattr__(self, "frames", frames)
        if self.reference_orientation_radians is not None:
            reference_orientation = float(self.reference_orientation_radians)
            if not math.isfinite(reference_orientation):
                raise TrajectoryError("Primitive reference orientation must be finite")
            object.__setattr__(
                self, "reference_orientation_radians", reference_orientation
            )

    def to_dict(self) -> Dict[str, Any]:
        """Return a versioned, unit-explicit representation for deployment."""

        return {
            "schema": "labcobot.valve-motion-primitive.v1",
            "units": {
                "position": "millimetre",
                "orientation": "radian",
                "time": "second",
            },
            "source_target_id": self.source_target_id,
            "reference_orientation_radians": self.reference_orientation_radians,
            "frames": [
                {
                    "timestamp": frame.timestamp,
                    "local_xyz": list(frame.local_xyz),
                    "orientation_rpy": list(frame.orientation_rpy),
                    "gripper": frame.gripper,
                }
                for frame in self.frames
            ],
        }

    @classmethod
    def from_dict(cls, payload: Mapping[str, Any]) -> "ValveMotionPrimitive":
        """Load only the supported primitive schema and declared units."""

        if payload.get("schema") != "labcobot.valve-motion-primitive.v1":
            raise TrajectoryError("Unsupported valve primitive schema")
        units = payload.get("units")
        if units != {
            "position": "millimetre",
            "orientation": "radian",
            "time": "second",
        }:
            raise TrajectoryError("Valve primitive units are missing or unsupported")
        raw_frames = payload.get("frames")
        if not isinstance(raw_frames, list):
            raise TrajectoryError("Valve primitive frames must be a JSON list")
        if len(raw_frames) > 10_000:
            raise TrajectoryError("Valve primitive contains too many frames")
        frames = []
        for raw_frame in raw_frames:
            if not isinstance(raw_frame, Mapping):
                raise TrajectoryError("Valve primitive frames must be JSON objects")
            frames.append(
                ValvePrimitiveFrame(
                    timestamp=raw_frame.get("timestamp"),
                    local_xyz=raw_frame.get("local_xyz"),
                    orientation_rpy=raw_frame.get("orientation_rpy"),
                    gripper=raw_frame.get("gripper"),
                )
            )
        reference_orientation = payload.get("reference_orientation_radians")
        return cls(
            frames=tuple(frames),
            source_target_id=str(payload.get("source_target_id") or "valve"),
            reference_orientation_radians=(
                float(reference_orientation)
                if reference_orientation is not None
                else None
            ),
        )

    @classmethod
    def from_demonstration(
        cls, demonstration: Demonstration, target: ValveTarget
    ) -> "ValveMotionPrimitive":
        if any(frame.tcp_pose is None for frame in demonstration.frames):
            raise TrajectoryError(
                "Valve imitation learning requires a TCP pose in every frame"
            )
        basis = _valve_basis(
            target.normal_xyz, orientation_radians=target.orientation_radians
        )
        center = np.asarray(target.center_xyz, dtype=float)
        started = demonstration.frames[0].timestamp
        frames = []
        for frame in demonstration.frames:
            assert frame.tcp_pose is not None
            offset_world = np.asarray(frame.tcp_pose[:3], dtype=float) - center
            offset_local = basis.T.dot(offset_world)
            orientation = tuple(
                float(value)
                for value in (
                    frame.tcp_pose[3:6] if len(frame.tcp_pose) >= 6 else (0.0, 0.0, 0.0)
                )
            )
            frames.append(
                ValvePrimitiveFrame(
                    timestamp=frame.timestamp - started,
                    local_xyz=tuple(float(value) for value in offset_local),
                    orientation_rpy=orientation,
                    gripper=frame.gripper,
                )
            )
        return cls(
            tuple(frames),
            target.target_id,
            target.orientation_radians,
        )


class ValveMotionPrimitiveStore:
    """Atomically persist a reviewed valve-relative imitation primitive."""

    maximum_file_bytes = 8 * 1024 * 1024

    def save(
        self,
        path: Union[str, os.PathLike],
        primitive: ValveMotionPrimitive,
    ) -> Path:
        destination = Path(path)
        destination.parent.mkdir(parents=True, exist_ok=True)
        payload = json.dumps(
            primitive.to_dict(),
            ensure_ascii=False,
            allow_nan=False,
            indent=2,
        )
        if len(payload.encode("utf-8")) > self.maximum_file_bytes:
            raise TrajectoryError("Valve primitive exceeds the file-size limit")
        descriptor, temporary_name = tempfile.mkstemp(
            prefix="." + destination.name + ".",
            suffix=".tmp",
            dir=str(destination.parent),
        )
        try:
            with os.fdopen(descriptor, "w", encoding="utf-8") as handle:
                handle.write(payload)
                handle.flush()
                os.fsync(handle.fileno())
            os.replace(temporary_name, destination)
        except Exception:
            try:
                os.unlink(temporary_name)
            except FileNotFoundError:
                pass
            raise
        return destination

    def load(
        self,
        path: Union[str, os.PathLike],
        *,
        expected_sha256: Optional[str] = None,
    ) -> ValveMotionPrimitive:
        source = Path(path)
        try:
            raw_payload = source.read_bytes()
        except OSError as exc:
            raise TrajectoryError(
                "Valve primitive artifact is unavailable: {}".format(source)
            ) from exc
        if not raw_payload or len(raw_payload) > self.maximum_file_bytes:
            raise TrajectoryError("Valve primitive artifact has an invalid size")
        if expected_sha256 is not None:
            expected = str(expected_sha256).strip().lower()
            if len(expected) != 64 or any(
                character not in "0123456789abcdef" for character in expected
            ):
                raise TrajectoryError("Expected valve primitive SHA-256 is invalid")
            actual = hashlib.sha256(raw_payload).hexdigest()
            if not hmac.compare_digest(actual, expected):
                raise TrajectoryError(
                    "Valve primitive SHA-256 does not match the approved artifact"
                )
        try:
            payload = json.loads(raw_payload.decode("utf-8"))
        except (OSError, UnicodeError, json.JSONDecodeError) as exc:
            raise TrajectoryError("Valve primitive artifact is not valid JSON") from exc
        if not isinstance(payload, Mapping):
            raise TrajectoryError("Valve primitive file must contain a JSON object")
        return ValveMotionPrimitive.from_dict(payload)


def build_valve_turn_primitive(
    *,
    approach_distance: float = 80.0,
    contact_offset: float = 5.0,
    turn_radians: float = math.pi / 2,
    duration: float = 4.0,
    turn_steps: int = 8,
    orientation_rpy: NumberSequence = (math.pi, 0.0, 0.0),
) -> ValveMotionPrimitive:
    """Create a generic approach/contact/turn/retreat primitive.

    Local +Z follows the outward-facing valve normal.  The caller should tune
    offsets and orientation for its tool before enabling hardware execution.
    """

    approach_distance = _positive(approach_distance, "approach_distance")
    contact_offset = float(contact_offset)
    duration = _positive(duration, "duration")
    if turn_steps < 2:
        raise TrajectoryError("turn_steps must be at least two")
    orientation = _finite_tuple(orientation_rpy, "orientation_rpy")
    if len(orientation) != 3:
        raise TrajectoryError("orientation_rpy must contain roll, pitch, and yaw")

    frames = [
        ValvePrimitiveFrame(
            0.0,
            (0.0, 0.0, approach_distance),
            orientation,
            0.0,
        ),
        ValvePrimitiveFrame(
            duration * 0.2,
            (0.0, 0.0, contact_offset),
            orientation,
            1.0,
        ),
    ]
    for step in range(1, turn_steps + 1):
        fraction = step / float(turn_steps)
        timestamp = duration * (0.2 + 0.6 * fraction)
        frames.append(
            ValvePrimitiveFrame(
                timestamp,
                (0.0, 0.0, contact_offset),
                (
                    orientation[0],
                    orientation[1],
                    orientation[2] + turn_radians * fraction,
                ),
                1.0,
            )
        )
    frames.append(
        ValvePrimitiveFrame(
            duration,
            (0.0, 0.0, approach_distance),
            (
                orientation[0],
                orientation[1],
                orientation[2] + turn_radians,
            ),
            0.0,
        )
    )
    return ValveMotionPrimitive(tuple(frames), reference_orientation_radians=0.0)


@dataclass(frozen=True)
class CartesianWaypoint:
    timestamp: float
    tcp_pose: Tuple[float, float, float, float, float, float]
    gripper: Optional[float]


@dataclass(frozen=True)
class ValveMotionPlan:
    target: ValveTarget
    cartesian_waypoints: Tuple[CartesianWaypoint, ...]
    joint_trajectory: Optional[Demonstration]
    dry_run: bool = True


IKSolver = Callable[[Tuple[float, ...], Optional[Tuple[float, ...]]], NumberSequence]
OrientationResolver = Callable[
    [ValveTarget, ValveMotionPrimitive, ValvePrimitiveFrame], NumberSequence
]


class DynamicValvePlanner:
    """Retarget a learned primitive to each fresh valve observation."""

    def __init__(
        self,
        *,
        workspace_limits: WorkspaceLimits,
        joint_validator: Optional[TrajectorySafetyValidator] = None,
        inverse_kinematics: Optional[IKSolver] = None,
        orientation_resolver: Optional[OrientationResolver] = None,
        target_adapter: Callable[..., ValveTarget] = valve_target_from_observation,
        transform_to_robot: Optional[PointTransform] = None,
        default_normal_xyz: Optional[NumberSequence] = None,
        minimum_confidence: float = 0.7,
        maximum_target_age: float = 1.0,
        maximum_initial_joint_delta_radians: Optional[float] = None,
        clock: Callable[[], float] = time.time,
    ) -> None:
        self.workspace_limits = workspace_limits
        self.joint_validator = joint_validator
        self.inverse_kinematics = inverse_kinematics
        self.orientation_resolver = orientation_resolver
        self.target_adapter = target_adapter
        self.transform_to_robot = transform_to_robot
        self.default_normal_xyz = (
            _finite_tuple(default_normal_xyz, "default_normal_xyz")
            if default_normal_xyz is not None
            else None
        )
        if self.default_normal_xyz is not None and len(self.default_normal_xyz) != 3:
            raise TrajectoryError("default_normal_xyz must have three values")
        self.minimum_confidence = float(minimum_confidence)
        self.maximum_target_age = _positive(maximum_target_age, "maximum_target_age")
        self.maximum_initial_joint_delta_radians = (
            _positive(
                maximum_initial_joint_delta_radians,
                "maximum_initial_joint_delta_radians",
            )
            if maximum_initial_joint_delta_radians is not None
            else None
        )
        if (
            not math.isfinite(self.minimum_confidence)
            or not 0.0 <= self.minimum_confidence <= 1.0
        ):
            raise TrajectoryError("minimum_confidence must be between zero and one")
        self.clock = clock

    def plan(
        self,
        observation: Any,
        primitive: ValveMotionPrimitive,
        *,
        seed_joints: Optional[NumberSequence] = None,
        joint_sample_rate_hz: float = 25.0,
        smoothing_window: int = 3,
    ) -> ValveMotionPlan:
        now = self.clock()
        if isinstance(observation, ValveTarget):
            target = observation
        else:
            target = self.target_adapter(
                observation,
                transform_to_robot=self.transform_to_robot,
                default_timestamp=now,
                default_normal_xyz=self.default_normal_xyz,
            )
        if target.confidence < self.minimum_confidence:
            raise TrajectoryError(
                "Valve confidence {:.3f} is below {:.3f}".format(
                    target.confidence, self.minimum_confidence
                )
            )
        age = now - target.timestamp
        if age < -self.maximum_target_age:
            raise TrajectoryError(
                "Valve observation timestamp is too far in the future"
            )
        if age > self.maximum_target_age:
            raise TrajectoryError(
                "Valve observation is stale ({:.3f}s old)".format(age)
            )

        basis = _valve_basis(
            target.normal_xyz, orientation_radians=target.orientation_radians
        )
        center = np.asarray(target.center_xyz, dtype=float)
        waypoints: List[CartesianWaypoint] = []
        for frame in primitive.frames:
            xyz = center + basis.dot(np.asarray(frame.local_xyz, dtype=float))
            if not self.workspace_limits.contains(xyz):
                raise TrajectoryError(
                    "Retargeted valve waypoint is outside the configured workspace"
                )
            orientation = self._resolve_orientation(target, primitive, frame)
            pose = (
                float(xyz[0]),
                float(xyz[1]),
                float(xyz[2]),
                orientation[0],
                orientation[1],
                orientation[2],
            )
            waypoints.append(CartesianWaypoint(frame.timestamp, pose, frame.gripper))

        joint_trajectory: Optional[Demonstration] = None
        if self.inverse_kinematics is not None:
            frames: List[TrajectoryFrame] = []
            seed = (
                tuple(float(value) for value in seed_joints)
                if seed_joints is not None
                else None
            )
            initial_seed = seed
            for waypoint in waypoints:
                solved = tuple(
                    float(value)
                    for value in self.inverse_kinematics(waypoint.tcp_pose, seed)
                )
                frame = TrajectoryFrame(
                    timestamp=waypoint.timestamp,
                    joints=solved,
                    tcp_pose=waypoint.tcp_pose,
                    gripper=waypoint.gripper,
                    observation={
                        "valve_confidence": target.confidence,
                        "valve_x": target.center_xyz[0],
                        "valve_y": target.center_xyz[1],
                        "valve_z": target.center_xyz[2],
                    },
                )
                frames.append(frame)
                seed = solved
            raw = Demonstration(
                tuple(frames),
                name="valve-{}".format(target.target_id),
                metadata={"target_id": target.target_id},
            )
            if self.joint_validator is None:
                raise TrajectoryError(
                    "Inverse kinematics planning requires a joint safety validator"
                )
            joint_trajectory = prepare_demonstration(
                raw,
                validator=self.joint_validator,
                sample_rate_hz=joint_sample_rate_hz,
                smoothing_window=smoothing_window,
            )
            if self.maximum_initial_joint_delta_radians is not None:
                if initial_seed is None:
                    raise TrajectoryError(
                        "Live continuity validation requires current joint angles"
                    )
                first_command = joint_trajectory.frames[0].joints
                if len(initial_seed) != len(first_command):
                    raise TrajectoryError(
                        "Current joint count does not match the first command"
                    )
                largest_initial_delta = max(
                    abs(planned - current)
                    for planned, current in zip(first_command, initial_seed)
                )
                if largest_initial_delta > self.maximum_initial_joint_delta_radians:
                    raise TrajectoryError(
                        "First joint command is discontinuous from the current "
                        "robot state ({:.6f} rad exceeds {:.6f} rad)".format(
                            largest_initial_delta,
                            self.maximum_initial_joint_delta_radians,
                        )
                    )

        return ValveMotionPlan(
            target=target,
            cartesian_waypoints=tuple(waypoints),
            joint_trajectory=joint_trajectory,
            dry_run=True,
        )

    def _resolve_orientation(
        self,
        target: ValveTarget,
        primitive: ValveMotionPrimitive,
        frame: ValvePrimitiveFrame,
    ) -> Tuple[float, float, float]:
        if self.orientation_resolver is not None:
            orientation = _finite_tuple(
                self.orientation_resolver(target, primitive, frame),
                "resolved orientation",
            )
            if len(orientation) != 3:
                raise TrajectoryError(
                    "orientation_resolver must return roll, pitch, and yaw"
                )
            return orientation

        orientation = frame.orientation_rpy
        if (
            target.orientation_radians is not None
            and primitive.reference_orientation_radians is not None
        ):
            orientation_delta = (
                target.orientation_radians - primitive.reference_orientation_radians
            )
            return (
                orientation[0],
                orientation[1],
                orientation[2] + orientation_delta,
            )
        return orientation


@dataclass(frozen=True)
class JointCommand:
    timestamp: float
    joints: Tuple[float, ...]
    gripper: Optional[float]


@dataclass(frozen=True)
class ExecutionResult:
    executed: bool
    command_count: int
    commands: Tuple[JointCommand, ...]


JointCommandSink = Callable[[JointCommand], None]


class SafeTrajectoryExecutor:
    """Validate and optionally dispatch joint commands to an injected sink."""

    def __init__(
        self,
        *,
        validator: TrajectorySafetyValidator,
        command_sink: Optional[JointCommandSink] = None,
        dry_run: bool = True,
        clock: Callable[[], float] = time.monotonic,
        sleeper: Callable[[float], None] = time.sleep,
        maximum_sleep_interval: float = 0.02,
    ) -> None:
        self.validator = validator
        self.command_sink = command_sink
        self.dry_run = bool(dry_run)
        self.clock = clock
        self.sleeper = sleeper
        self.maximum_sleep_interval = _positive(
            maximum_sleep_interval, "maximum_sleep_interval"
        )
        if self.maximum_sleep_interval > 0.1:
            raise TrajectoryError("maximum_sleep_interval cannot exceed 0.1 seconds")

    def execute(
        self,
        demonstration: Demonstration,
        *,
        allow_hardware: bool = False,
        stop_requested: Optional[Callable[[], bool]] = None,
    ) -> ExecutionResult:
        self.validator.require_safe(demonstration)
        started = demonstration.frames[0].timestamp
        commands = tuple(
            JointCommand(
                timestamp=frame.timestamp - started,
                joints=frame.joints,
                gripper=frame.gripper,
            )
            for frame in demonstration.frames
        )
        if self.dry_run or not allow_hardware:
            return ExecutionResult(False, len(commands), commands)
        if self.command_sink is None:
            raise TrajectoryError(
                "Hardware execution requires an injected command_sink"
            )
        execution_started = self.clock()
        completed = 0
        for command in commands:
            if stop_requested is not None and stop_requested():
                raise TrajectoryExecutionStopped(completed)
            deadline = execution_started + command.timestamp
            while True:
                remaining = deadline - self.clock()
                if remaining <= 0:
                    break
                self.sleeper(min(remaining, self.maximum_sleep_interval))
                if stop_requested is not None and stop_requested():
                    raise TrajectoryExecutionStopped(completed)
            if stop_requested is not None and stop_requested():
                raise TrajectoryExecutionStopped(completed)
            self.command_sink(command)
            completed += 1
        return ExecutionResult(True, len(commands), commands)


@dataclass(frozen=True)
class ImitationCycleResult:
    """Outcome of one camera-to-motion controller cycle."""

    status: str
    frame_key: Optional[Tuple[str, str, int, float]] = None
    target_id: Optional[str] = None
    reasons: Tuple[str, ...] = ()
    tracks_considered: int = 0
    used_remote_inference: bool = False
    plan: Optional[ValveMotionPlan] = None
    execution: Optional[ExecutionResult] = None
    completed_commands: int = 0


PrimitiveProvider = Callable[[Any], ValveMotionPrimitive]
SeedJointsProvider = Callable[[], Optional[NumberSequence]]
TrajectoryPreflight = Callable[..., Any]


class DynamicValveImitationController:
    """Fail-closed camera-to-imitation orchestration.

    The injected collaborators are the hardware-free seams from
    :mod:`main.vision` and this module:

    ``frame_reader -> ValveTrackingService -> ValveTargetGate ->
    DynamicValvePlanner -> SafeTrajectoryExecutor``.

    ``remote_tracking_adapter`` may be a second ``ValveTrackingService`` whose
    detector calls an API gateway/ShortcutJob endpoint.  It is attempted at
    most once per frame, and only when local detection fails or yields no
    tracks.  The controller itself contains no network client.

    Hardware dispatch requires all of the following:

    * the controller was created with ``dry_run=False``;
    * ``process_once`` received ``armed=True``;
    * ``process_once`` received ``live=True``;
    * the executor itself was created with ``dry_run=False`` and has a sink.
    """

    def __init__(
        self,
        *,
        frame_reader: Callable[[], Any],
        tracking_service: Any,
        target_gate: Any,
        planner: DynamicValvePlanner,
        executor: SafeTrajectoryExecutor,
        primitive: Optional[ValveMotionPrimitive] = None,
        primitive_provider: Optional[PrimitiveProvider] = None,
        remote_tracking_adapter: Any = None,
        remote_fallback_on_empty: bool = True,
        require_remote_confirmation_for_live: bool = False,
        seed_joints_provider: Optional[SeedJointsProvider] = None,
        trajectory_preflight: Optional[TrajectoryPreflight] = None,
        dry_run: bool = True,
        clock: Callable[[], float] = time.time,
        frame_history_size: int = 256,
        execution_deduplication_seconds: float = 300.0,
        target_deduplication_distance_mm: float = 10.0,
        joint_sample_rate_hz: float = 100.0,
    ) -> None:
        if (primitive is None) == (primitive_provider is None):
            raise TrajectoryError(
                "Configure exactly one of primitive or primitive_provider"
            )
        if frame_history_size < 1:
            raise TrajectoryError("frame_history_size must be positive")
        self.frame_reader = frame_reader
        self.tracking_service = tracking_service
        self.target_gate = target_gate
        self.planner = planner
        self.executor = executor
        self.primitive = primitive
        self.primitive_provider = primitive_provider
        self.remote_tracking_adapter = remote_tracking_adapter
        self.remote_fallback_on_empty = bool(remote_fallback_on_empty)
        self.require_remote_confirmation_for_live = bool(
            require_remote_confirmation_for_live
        )
        if (
            self.require_remote_confirmation_for_live
            and self.remote_tracking_adapter is None
        ):
            raise TrajectoryError(
                "Live motion requires a remote/model valve-confirmation adapter"
            )
        self.seed_joints_provider = seed_joints_provider
        if trajectory_preflight is not None and not callable(trajectory_preflight):
            raise TrajectoryError("trajectory_preflight must be callable")
        self.trajectory_preflight = trajectory_preflight
        self.dry_run = bool(dry_run)
        self.clock = clock
        self.execution_deduplication_seconds = _positive(
            execution_deduplication_seconds,
            "execution_deduplication_seconds",
        )
        self.target_deduplication_distance_mm = _positive(
            target_deduplication_distance_mm,
            "target_deduplication_distance_mm",
        )
        self.joint_sample_rate_hz = _positive(
            joint_sample_rate_hz,
            "joint_sample_rate_hz",
        )
        if not 10.0 <= self.joint_sample_rate_hz <= 100.0:
            raise TrajectoryError("joint_sample_rate_hz must be between 10 and 100")

        self._lock = threading.Lock()
        self._frame_history_size = int(frame_history_size)
        self._seen_frame_order: Deque[Tuple[str, str, int, float]] = deque()
        self._seen_frames = set()
        self._last_observation_by_track: Dict[str, float] = {}
        self._executed_targets: List[Tuple[float, Tuple[float, float, float], str]] = []
        self._motion_fault_latched = False

    def process_once(
        self,
        *,
        armed: bool = False,
        live: bool = False,
        stop_requested: Optional[Callable[[], bool]] = None,
        authorization_generation: Any = None,
        authorization_validator: Optional[Callable[[Any], bool]] = None,
    ) -> ImitationCycleResult:
        """Process the newest frame once without ever implicitly arming motion."""

        if not self._lock.acquire(blocking=False):
            return ImitationCycleResult(
                status="busy", reasons=("another controller cycle is active",)
            )
        try:
            return self._process_once_locked(
                armed=bool(armed),
                live=bool(live),
                stop_requested=stop_requested,
                authorization_generation=authorization_generation,
                authorization_validator=authorization_validator,
            )
        finally:
            self._lock.release()

    def _process_once_locked(
        self,
        *,
        armed: bool,
        live: bool,
        stop_requested: Optional[Callable[[], bool]],
        authorization_generation: Any,
        authorization_validator: Optional[Callable[[Any], bool]],
    ) -> ImitationCycleResult:
        if self._motion_fault_latched:
            return ImitationCycleResult(
                status="motion_fault_latched",
                reasons=(
                    "motion was interrupted or failed; inspect the workcell and "
                    "explicitly clear the motion fault before retrying",
                ),
            )
        if stop_requested is not None and stop_requested():
            return ImitationCycleResult(
                status="stopped", reasons=("stop was requested before inference",)
            )

        try:
            frame = self.frame_reader()
        except Exception as exc:
            return ImitationCycleResult(
                status="frame_error",
                reasons=("frame reader failed: {}".format(type(exc).__name__),),
            )
        if frame is None:
            return ImitationCycleResult(
                status="no_frame", reasons=("no camera frame is available",)
            )
        try:
            frame_key = self._frame_key(frame)
        except Exception as exc:
            return ImitationCycleResult(
                status="frame_error",
                reasons=("invalid frame envelope: {}".format(type(exc).__name__),),
            )
        if frame_key in self._seen_frames:
            return ImitationCycleResult(
                status="duplicate_frame",
                frame_key=frame_key,
                reasons=("camera frame was already processed",),
            )
        self._remember_frame(frame_key)

        tracks: Sequence[Any] = ()
        local_error: Optional[Exception] = None
        try:
            tracks = tuple(self.tracking_service.process(frame))
        except Exception as exc:
            local_error = exc

        def authorization_is_valid() -> bool:
            if authorization_validator is None:
                return True
            try:
                return bool(authorization_validator(authorization_generation))
            except Exception:
                return False

        # Remote inference receives a camera frame.  It is therefore an
        # operator-authorized Search action, not a background fallback: local
        # inference failures while idle must remain local and fail closed.
        remote_authorized = bool(armed and live and authorization_is_valid())
        live_confirmation_required = (
            self.require_remote_confirmation_for_live
            and not self.dry_run
            and armed
            and live
        )
        use_remote = (
            self.remote_tracking_adapter is not None
            and remote_authorized
            and (
                live_confirmation_required
                or local_error is not None
                or (self.remote_fallback_on_empty and not tracks)
            )
        )
        if use_remote:
            if stop_requested is not None and stop_requested():
                return ImitationCycleResult(
                    status="stopped",
                    frame_key=frame_key,
                    reasons=("stop was requested before remote inference",),
                )
            if not authorization_is_valid():
                return ImitationCycleResult(
                    status="authorization_invalid",
                    frame_key=frame_key,
                    reasons=("Search authorization expired before remote inference",),
                )
            try:
                tracks = tuple(self.remote_tracking_adapter.process(frame))
            except Exception as exc:
                reasons = []
                if local_error is not None:
                    reasons.append(
                        "local inference failed: {}".format(type(local_error).__name__)
                    )
                reasons.append("remote inference failed: {}".format(type(exc).__name__))
                return ImitationCycleResult(
                    status="inference_error",
                    frame_key=frame_key,
                    reasons=tuple(reasons),
                    used_remote_inference=True,
                )
        elif local_error is not None:
            return ImitationCycleResult(
                status="inference_error",
                frame_key=frame_key,
                reasons=(
                    "local inference failed: {}".format(type(local_error).__name__),
                ),
            )

        now = float(self.clock())
        try:
            decision = self.target_gate.choose(tracks, now_s=now)
        except Exception as exc:
            return ImitationCycleResult(
                status="target_gate_error",
                frame_key=frame_key,
                reasons=("target gate failed: {}".format(type(exc).__name__),),
                tracks_considered=len(tracks),
                used_remote_inference=use_remote,
            )
        target = getattr(decision, "target", None)
        if target is None or not bool(getattr(decision, "accepted", False)):
            decision_reasons = tuple(
                str(reason)
                for reason in getattr(decision, "reasons", ("no safe valve target",))
            )
            return ImitationCycleResult(
                status="no_safe_target",
                frame_key=frame_key,
                reasons=decision_reasons or ("no safe valve target",),
                tracks_considered=len(tracks),
                used_remote_inference=use_remote,
            )

        target_id = str(target.track_id)
        target_role = getattr(target.camera_role, "value", target.camera_role)
        if str(target_role) == "tool" and not self._tool_calibration_ready():
            return ImitationCycleResult(
                status="tool_calibration_not_ready",
                frame_key=frame_key,
                target_id=target_id,
                reasons=(
                    "live tool-camera targeting requires a quality-approved "
                    "eye-in-hand mapper",
                ),
                tracks_considered=len(tracks),
                used_remote_inference=use_remote,
            )
        observed_at = float(target.observed_at_s)
        previous_observation = self._last_observation_by_track.get(target_id)
        if previous_observation is not None and observed_at <= previous_observation:
            return ImitationCycleResult(
                status="duplicate_observation",
                frame_key=frame_key,
                target_id=target_id,
                reasons=("valve observation was already processed",),
                tracks_considered=len(tracks),
                used_remote_inference=use_remote,
            )

        target_xyz = tuple(float(value) for value in target.workspace_xyz_mm)
        duplicate_execution = self._recent_execution(target_xyz, now)
        if duplicate_execution is not None:
            return ImitationCycleResult(
                status="target_already_executed",
                frame_key=frame_key,
                target_id=target_id,
                reasons=(
                    "a nearby valve target was already executed as {}".format(
                        duplicate_execution
                    ),
                ),
                tracks_considered=len(tracks),
                used_remote_inference=use_remote,
            )

        try:
            primitive = (
                self.primitive_provider(target)
                if self.primitive_provider is not None
                else self.primitive
            )
            if primitive is None:
                raise TrajectoryError("No valve primitive is available")
            seed_joints = (
                self.seed_joints_provider()
                if self.seed_joints_provider is not None
                else None
            )
            plan = self.planner.plan(
                target,
                primitive,
                seed_joints=seed_joints,
                joint_sample_rate_hz=self.joint_sample_rate_hz,
            )
        except Exception as exc:
            return ImitationCycleResult(
                status="planning_error",
                frame_key=frame_key,
                target_id=target_id,
                reasons=("motion planning failed: {}".format(type(exc).__name__),),
                tracks_considered=len(tracks),
                used_remote_inference=use_remote,
            )

        self._last_observation_by_track[target_id] = observed_at
        if plan.joint_trajectory is None:
            return ImitationCycleResult(
                status="cartesian_plan_only",
                frame_key=frame_key,
                target_id=target_id,
                reasons=(
                    "inverse kinematics is not configured; no joint commands "
                    "were generated",
                ),
                tracks_considered=len(tracks),
                used_remote_inference=use_remote,
                plan=plan,
            )

        if self.trajectory_preflight is not None:
            try:
                preflight = self.trajectory_preflight(
                    plan,
                    stop_requested=stop_requested,
                )
            except Exception as exc:
                return ImitationCycleResult(
                    status="clearance_blocked",
                    frame_key=frame_key,
                    target_id=target_id,
                    reasons=(
                        "dual-depth trajectory preflight failed: {}".format(
                            type(exc).__name__
                        ),
                    ),
                    tracks_considered=len(tracks),
                    used_remote_inference=use_remote,
                    plan=plan,
                )
            if getattr(preflight, "clear", False) is not True:
                blocker_values = getattr(preflight, "blockers", ())
                if not isinstance(blocker_values, (tuple, list)):
                    blocker_values = ()
                reasons = tuple(
                    "dual-depth trajectory preflight blocked: {}".format(value)
                    for value in blocker_values
                    if isinstance(value, str) and value
                )
                return ImitationCycleResult(
                    status="clearance_blocked",
                    frame_key=frame_key,
                    target_id=target_id,
                    reasons=reasons
                    or ("dual-depth trajectory preflight did not prove clearance",),
                    tracks_considered=len(tracks),
                    used_remote_inference=use_remote,
                    plan=plan,
                )

        authorization_valid = authorization_is_valid()
        allow_hardware = (
            (not self.dry_run)
            and armed
            and live
            and authorization_valid
            and (not self.require_remote_confirmation_for_live or use_remote)
        )

        def execution_stop_requested() -> bool:
            if stop_requested is not None and stop_requested():
                return True
            if authorization_validator is None:
                return False
            try:
                return not bool(authorization_validator(authorization_generation))
            except Exception:
                return True

        try:
            execution = self.executor.execute(
                plan.joint_trajectory,
                allow_hardware=allow_hardware,
                stop_requested=execution_stop_requested,
            )
        except TrajectoryExecutionStopped as exc:
            if allow_hardware and exc.completed_commands > 0:
                self._motion_fault_latched = True
            return ImitationCycleResult(
                status="stopped",
                frame_key=frame_key,
                target_id=target_id,
                reasons=(str(exc),),
                tracks_considered=len(tracks),
                used_remote_inference=use_remote,
                plan=plan,
                completed_commands=exc.completed_commands,
            )
        except Exception as exc:
            if allow_hardware:
                self._motion_fault_latched = True
            return ImitationCycleResult(
                status="execution_error",
                frame_key=frame_key,
                target_id=target_id,
                reasons=("trajectory execution failed: {}".format(type(exc).__name__),),
                tracks_considered=len(tracks),
                used_remote_inference=use_remote,
                plan=plan,
            )

        if execution.executed:
            self._executed_targets.append((now, target_xyz, target_id))
            self._prune_executed_targets(now)
            status = "executed"
        elif not armed:
            status = "dry_run_unarmed"
        elif not live:
            status = "dry_run_not_live"
        elif self.dry_run:
            status = "dry_run_controller"
        elif not authorization_valid:
            status = "authorization_invalid"
        elif self.require_remote_confirmation_for_live and not use_remote:
            status = "confirmation_required"
        else:
            status = "dry_run_executor"
        return ImitationCycleResult(
            status=status,
            frame_key=frame_key,
            target_id=target_id,
            tracks_considered=len(tracks),
            used_remote_inference=use_remote,
            plan=plan,
            execution=execution,
        )

    def run_loop(
        self,
        stop_event: Any,
        *,
        interval_seconds: float = 0.05,
        armed_gate: Union[bool, Callable[[], bool]] = False,
        live_gate: Union[bool, Callable[[], bool]] = False,
        authorization_provider: Optional[Callable[[], Any]] = None,
        authorization_validator: Optional[Callable[[Any], bool]] = None,
        motion_stop_requested: Optional[Callable[[], bool]] = None,
        on_result: Optional[Callable[[ImitationCycleResult], None]] = None,
        maximum_cycles: Optional[int] = None,
    ) -> int:
        """Run bounded cycles until stopped; both motion gates default closed."""

        interval_seconds = _positive(interval_seconds, "interval_seconds")
        if maximum_cycles is not None and maximum_cycles < 1:
            raise TrajectoryError("maximum_cycles must be positive")
        cycles = 0
        while not stop_event.is_set():

            def stop_requested() -> bool:
                return bool(
                    stop_event.is_set()
                    or (motion_stop_requested is not None and motion_stop_requested())
                )

            authorization_generation = (
                authorization_provider() if authorization_provider is not None else None
            )
            result = self.process_once(
                armed=_gate_value(armed_gate),
                live=_gate_value(live_gate),
                stop_requested=stop_requested,
                authorization_generation=authorization_generation,
                authorization_validator=authorization_validator,
            )
            cycles += 1
            if on_result is not None:
                on_result(result)
            if maximum_cycles is not None and cycles >= maximum_cycles:
                break
            stop_event.wait(interval_seconds)
        return cycles

    def clear_execution_history(self) -> None:
        """Explicitly allow previously completed spatial targets again."""

        with self._lock:
            self._executed_targets = []

    def clear_motion_fault(self) -> None:
        """Clear the motion latch after external workcell inspection/recovery."""

        with self._lock:
            self._motion_fault_latched = False

    def clear_observation_history(self) -> None:
        """Clear frame/track deduplication without arming hardware."""

        with self._lock:
            self._seen_frames.clear()
            self._seen_frame_order.clear()
            self._last_observation_by_track.clear()

    @staticmethod
    def _frame_key(frame: Any) -> Tuple[str, str, int, float]:
        role = getattr(frame.role, "value", frame.role)
        captured_at = float(frame.captured_at_s)
        if not math.isfinite(captured_at):
            raise TrajectoryError("Frame captured_at_s must be finite")
        sequence = int(frame.sequence)
        if sequence < 0:
            raise TrajectoryError("Frame sequence cannot be negative")
        return (str(frame.source_id), str(role), sequence, captured_at)

    def _remember_frame(self, frame_key: Tuple[str, str, int, float]) -> None:
        self._seen_frames.add(frame_key)
        self._seen_frame_order.append(frame_key)
        while len(self._seen_frame_order) > self._frame_history_size:
            expired = self._seen_frame_order.popleft()
            self._seen_frames.discard(expired)

    def _recent_execution(
        self, target_xyz: Tuple[float, float, float], now: float
    ) -> Optional[str]:
        self._prune_executed_targets(now)
        target_array = np.asarray(target_xyz, dtype=float)
        for _executed_at, previous_xyz, previous_id in self._executed_targets:
            distance = float(
                np.linalg.norm(target_array - np.asarray(previous_xyz, dtype=float))
            )
            if distance <= self.target_deduplication_distance_mm:
                return previous_id
        return None

    def _prune_executed_targets(self, now: float) -> None:
        self._executed_targets = [
            entry
            for entry in self._executed_targets
            if now - entry[0] <= self.execution_deduplication_seconds
        ]

    def _tool_calibration_ready(self) -> bool:
        mapper = getattr(self.target_gate, "mapper", None)
        readiness_check = getattr(mapper, "is_ready", None)
        if not callable(readiness_check):
            return False
        try:
            return bool(readiness_check())
        except Exception:
            return False


def _finite_tuple(values: NumberSequence, name: str) -> Tuple[float, ...]:
    try:
        result = tuple(float(value) for value in values)
    except (TypeError, ValueError) as exc:
        raise TrajectoryError("{} must be a numeric sequence".format(name)) from exc
    if not all(math.isfinite(value) for value in result):
        raise TrajectoryError("{} values must be finite".format(name))
    return result


def _optional_tuple(value: Any) -> Optional[Tuple[float, ...]]:
    if value is None:
        return None
    return tuple(value)


def _positive(value: float, name: str) -> float:
    number = float(value)
    if not math.isfinite(number) or number <= 0:
        raise TrajectoryError("{} must be positive and finite".format(name))
    return number


def _field_getter(value: Any) -> Callable[[str], Any]:
    if isinstance(value, Mapping):
        return value.get
    return lambda name: getattr(value, name, None)


def _first_present(getter: Callable[[str], Any], names: Sequence[str]) -> Any:
    for name in names:
        value = getter(name)
        if value is not None:
            return value
    return None


def _gate_value(value: Union[bool, Callable[[], bool]]) -> bool:
    return bool(value() if callable(value) else value)


def _valve_basis(
    normal_xyz: NumberSequence, *, orientation_radians: Optional[float] = None
) -> np.ndarray:
    """Return a stable right-handed basis with local +Z along the valve normal."""

    normal = np.asarray(normal_xyz, dtype=float)
    normal /= np.linalg.norm(normal)
    reference = np.asarray((0.0, 0.0, 1.0), dtype=float)
    if abs(float(np.dot(reference, normal))) > 0.9:
        reference = np.asarray((0.0, 1.0, 0.0), dtype=float)
    local_x = np.cross(reference, normal)
    local_x /= np.linalg.norm(local_x)
    local_y = np.cross(normal, local_x)
    local_y /= np.linalg.norm(local_y)
    if orientation_radians is not None:
        angle = float(orientation_radians)
        cosine = math.cos(angle)
        sine = math.sin(angle)
        rotated_x = cosine * local_x + sine * local_y
        rotated_y = -sine * local_x + cosine * local_y
        local_x = rotated_x
        local_y = rotated_y
    return np.column_stack((local_x, local_y, normal))
