"""Hash-pinned, fail-closed motion for a fifteen-second capture run.

This module is intentionally only an integration seam.  Importing it never
connects to, arms, or moves a robot.  A live deployment must explicitly load a
reviewed primitive by its exact SHA-256 digest and inject an already-created
``XArmMotionAdapter`` (or an adapter with the same narrow interface).

The primitive owns its safety envelope.  Consequently, changing a joint
limit, derivative limit, start-continuity limit, workspace bound, command
interval, or joint waypoint changes the reviewed digest.  The envelope may
only narrow the repository's conservative Lite6 limits; it can never widen
them and joint angles are compared as absolute values without wrapping.
"""

from __future__ import annotations

import hashlib
import hmac
import json
import math
import os
import re
import stat
import threading
import time
from dataclasses import dataclass
from pathlib import Path
from typing import Any, Callable, Mapping, Optional, Protocol, Sequence, Tuple, Union

try:
    from .capture_runs import MotionExecutionEvidence
except ImportError:  # Direct ``main/runtime.py`` execution on the Pi.
    from capture_runs import MotionExecutionEvidence  # type: ignore[no-redef]

MOTION_SCHEMA = "labcobot.capture-motion-primitive.v1"
MOTION_DURATION_SECONDS = 15.0
JOINT_COUNT = 6
MAX_ARTIFACT_BYTES = 1024 * 1024
MAX_FRAME_COUNT = 4096
COMMAND_INTERVAL_SECONDS = 0.01
MAX_COMMAND_INTERVAL_SECONDS = COMMAND_INTERVAL_SECONDS
REQUIRED_FRAME_COUNT = 1501
MAX_START_DELTA_RADIANS = math.radians(5.0)
MAX_START_LEAD_SECONDS = 2.0
ROBOT_MODEL = "xarm_lite6"
CONTROLLER_DEVICE_TYPE = 9
JOINT_LIMIT_POLICY = "xarm-lite6-conservative-v1"

# These are the same conservative learning/replay limits used by
# JointLimits.xarm_lite6.  They are repeated as immutable module constants so
# loading a commissioning artifact does not import NumPy or training code.
HARD_JOINT_LOWER_RADIANS = tuple(
    math.radians(value) for value in (-180, -135, -135, -180, -135, -180)
)
HARD_JOINT_UPPER_RADIANS = tuple(
    math.radians(value) for value in (180, 135, 135, 180, 135, 180)
)
HARD_MAX_VELOCITY_RADIANS_PER_SECOND = tuple(
    math.radians(60.0) for _ in range(JOINT_COUNT)
)
HARD_MAX_ACCELERATION_RADIANS_PER_SECOND_SQUARED = tuple(
    math.radians(120.0) for _ in range(JOINT_COUNT)
)

_SHA256 = re.compile(r"^[0-9a-f]{64}$")
_PROFILE_ID = re.compile(r"^[A-Za-z0-9._-]{1,64}$")
_VALIDATED_ARTIFACT = object()


class CaptureMotionError(RuntimeError):
    """Base class with a bounded machine-readable failure code."""

    def __init__(self, code: str, message: str) -> None:
        self.code = str(code)
        super().__init__(str(message))


class MotionArtifactError(CaptureMotionError):
    """A primitive file is malformed or does not match its approved digest."""


class MotionSafetyError(CaptureMotionError):
    """A primitive or live start state violates a commissioned safety bound."""


class MotionExecutionError(CaptureMotionError):
    """A controller, scheduler, watchdog, or halt operation failed."""


class MotionAdapter(Protocol):
    """Narrow subset implemented by ``XArmMotionAdapter``."""

    @property
    def prepared(self) -> bool:
        """Whether the software command gate is open."""

    def read_joint_angles_radians(self) -> Sequence[float]:
        """Return exactly six finite physical joint angles."""

    def forward_kinematics(self, joints: Sequence[float]) -> Sequence[float]:
        """Return a finite six-value TCP pose in millimetres/radians."""

    def prepare_motion(self) -> None:
        """Validate controller readiness and explicitly open the command gate."""

    def send_joint_command(self, command: Any) -> None:
        """Dispatch one validated absolute-radian command."""

    def lock_commands(self) -> None:
        """Close the software command gate."""

    def halt(self) -> None:
        """Stop the controller and disable motion."""


@dataclass(frozen=True)
class MotionSafetyEnvelope:
    """All mutable commissioning policy embedded in the approved artifact."""

    joint_lower_radians: Tuple[float, ...]
    joint_upper_radians: Tuple[float, ...]
    max_joint_velocity_radians_per_second: Tuple[float, ...]
    max_joint_acceleration_radians_per_second_squared: Tuple[float, ...]
    max_start_delta_radians: Tuple[float, ...]
    workspace_lower_xyz_mm: Tuple[float, float, float]
    workspace_upper_xyz_mm: Tuple[float, float, float]
    max_command_interval_seconds: float


@dataclass(frozen=True)
class MotionFrame:
    """One reviewed absolute joint-space waypoint."""

    time_seconds: float
    joints: Tuple[float, ...]


@dataclass(frozen=True, init=False)
class CaptureMotionPrimitive:
    """An immutable primitive created only by the hash-checking loader."""

    profile_id: str
    profile_sha256: str
    robot_model: str
    axis_count: int
    controller_device_type: int
    joint_limit_policy: str
    duration_seconds: float
    safety: MotionSafetyEnvelope
    frames: Tuple[MotionFrame, ...]
    tcp_poses: Tuple[Tuple[float, ...], ...]
    _validation_token: object

    @classmethod
    def _create(
        cls,
        *,
        profile_id: str,
        profile_sha256: str,
        safety: MotionSafetyEnvelope,
        frames: Tuple[MotionFrame, ...],
        tcp_poses: Tuple[Tuple[float, ...], ...],
    ) -> "CaptureMotionPrimitive":
        instance = object.__new__(cls)
        object.__setattr__(instance, "profile_id", profile_id)
        object.__setattr__(instance, "profile_sha256", profile_sha256)
        object.__setattr__(instance, "robot_model", ROBOT_MODEL)
        object.__setattr__(instance, "axis_count", JOINT_COUNT)
        object.__setattr__(instance, "controller_device_type", CONTROLLER_DEVICE_TYPE)
        object.__setattr__(instance, "joint_limit_policy", JOINT_LIMIT_POLICY)
        object.__setattr__(instance, "duration_seconds", MOTION_DURATION_SECONDS)
        object.__setattr__(instance, "safety", safety)
        object.__setattr__(instance, "frames", frames)
        object.__setattr__(instance, "tcp_poses", tcp_poses)
        object.__setattr__(instance, "_validation_token", _VALIDATED_ARTIFACT)
        return instance


@dataclass(frozen=True)
class AbsoluteJointCommand:
    """Command shape accepted by the existing xArm motion adapter."""

    timestamp: float
    joints: Tuple[float, ...]
    gripper: None = None


def _finite_number(value: Any, name: str) -> float:
    if isinstance(value, bool):
        raise MotionArtifactError("artifact_invalid", "{} must be numeric".format(name))
    try:
        number = float(value)
    except (TypeError, ValueError, OverflowError) as exc:
        raise MotionArtifactError(
            "artifact_invalid", "{} must be numeric".format(name)
        ) from exc
    if not math.isfinite(number):
        raise MotionArtifactError("artifact_invalid", "{} must be finite".format(name))
    return number


def _finite_vector(value: Any, length: int, name: str) -> Tuple[float, ...]:
    if not isinstance(value, (list, tuple)) or len(value) != length:
        raise MotionArtifactError(
            "artifact_invalid",
            "{} must contain exactly {} values".format(name, length),
        )
    return tuple(
        _finite_number(item, "{}[{}]".format(name, index))
        for index, item in enumerate(value)
    )


def _runtime_vector(value: Any, name: str) -> Tuple[float, ...]:
    try:
        result = tuple(float(item) for item in value)
    except (TypeError, ValueError, OverflowError) as exc:
        raise MotionExecutionError(
            "controller_state_invalid",
            "{} must contain exactly six finite values".format(name),
        ) from exc
    if len(result) != JOINT_COUNT or not all(math.isfinite(item) for item in result):
        raise MotionExecutionError(
            "controller_state_invalid",
            "{} must contain exactly six finite values".format(name),
        )
    return result


def _exact_keys(value: Any, expected: Sequence[str], name: str) -> Mapping[str, Any]:
    if not isinstance(value, Mapping):
        raise MotionArtifactError(
            "artifact_invalid", "{} must be a JSON object".format(name)
        )
    actual = set(value)
    wanted = set(expected)
    if actual != wanted:
        raise MotionArtifactError(
            "artifact_invalid",
            "{} fields do not match schema".format(name),
        )
    if not all(isinstance(key, str) for key in value):
        raise MotionArtifactError(
            "artifact_invalid", "{} contains a non-string field".format(name)
        )
    return value


def _reject_duplicate_keys(pairs: Sequence[Tuple[str, Any]]) -> Mapping[str, Any]:
    result = {}
    for key, value in pairs:
        if key in result:
            raise MotionArtifactError(
                "artifact_invalid", "Artifact contains duplicate JSON fields"
            )
        result[key] = value
    return result


def _reject_nonstandard_number(value: str) -> None:
    raise MotionArtifactError(
        "artifact_invalid", "Artifact contains a non-standard JSON number"
    )


def _read_regular_file(path: Path) -> bytes:
    flags = os.O_RDONLY | getattr(os, "O_CLOEXEC", 0)
    flags |= getattr(os, "O_NOFOLLOW", 0)
    descriptor: Optional[int] = None
    try:
        descriptor = os.open(str(path), flags)
        info = os.fstat(descriptor)
        if not stat.S_ISREG(info.st_mode):
            raise MotionArtifactError(
                "artifact_unsafe_path", "Motion artifact must be a regular file"
            )
        if info.st_size < 2 or info.st_size > MAX_ARTIFACT_BYTES:
            raise MotionArtifactError(
                "artifact_size_invalid", "Motion artifact size is invalid"
            )
        chunks = []
        remaining = MAX_ARTIFACT_BYTES + 1
        while remaining > 0:
            chunk = os.read(descriptor, min(65536, remaining))
            if not chunk:
                break
            chunks.append(chunk)
            remaining -= len(chunk)
        payload = b"".join(chunks)
        if len(payload) != info.st_size or len(payload) > MAX_ARTIFACT_BYTES:
            raise MotionArtifactError(
                "artifact_size_invalid", "Motion artifact changed while being read"
            )
        return payload
    except MotionArtifactError:
        raise
    except OSError as exc:
        raise MotionArtifactError(
            "artifact_read_failed", "Motion artifact could not be read"
        ) from exc
    finally:
        if descriptor is not None:
            os.close(descriptor)


def _parse_safety(value: Any) -> MotionSafetyEnvelope:
    value = _exact_keys(
        value,
        (
            "joint_lower_radians",
            "joint_upper_radians",
            "max_joint_velocity_radians_per_second",
            "max_joint_acceleration_radians_per_second_squared",
            "max_start_delta_radians",
            "workspace_lower_xyz_mm",
            "workspace_upper_xyz_mm",
            "max_command_interval_seconds",
        ),
        "safety",
    )
    lower = _finite_vector(value["joint_lower_radians"], JOINT_COUNT, "joint lower")
    upper = _finite_vector(value["joint_upper_radians"], JOINT_COUNT, "joint upper")
    velocity = _finite_vector(
        value["max_joint_velocity_radians_per_second"],
        JOINT_COUNT,
        "joint velocity limits",
    )
    acceleration = _finite_vector(
        value["max_joint_acceleration_radians_per_second_squared"],
        JOINT_COUNT,
        "joint acceleration limits",
    )
    start_delta = _finite_vector(
        value["max_start_delta_radians"], JOINT_COUNT, "start continuity limits"
    )
    workspace_lower = _finite_vector(
        value["workspace_lower_xyz_mm"], 3, "workspace lower"
    )
    workspace_upper = _finite_vector(
        value["workspace_upper_xyz_mm"], 3, "workspace upper"
    )
    command_interval = _finite_number(
        value["max_command_interval_seconds"], "maximum command interval"
    )

    for index in range(JOINT_COUNT):
        if lower[index] >= upper[index]:
            raise MotionSafetyError(
                "joint_limits_invalid", "Each lower joint limit must be below upper"
            )
        if (
            lower[index] < HARD_JOINT_LOWER_RADIANS[index]
            or upper[index] > HARD_JOINT_UPPER_RADIANS[index]
        ):
            raise MotionSafetyError(
                "joint_limits_widened",
                "Artifact joint limits may only narrow conservative Lite6 limits",
            )
        if not 0.0 < velocity[index] <= HARD_MAX_VELOCITY_RADIANS_PER_SECOND[index]:
            raise MotionSafetyError(
                "velocity_limit_invalid",
                "Artifact velocity limits exceed conservative Lite6 limits",
            )
        if not (
            0.0
            < acceleration[index]
            <= HARD_MAX_ACCELERATION_RADIANS_PER_SECOND_SQUARED[index]
        ):
            raise MotionSafetyError(
                "acceleration_limit_invalid",
                "Artifact acceleration limits exceed conservative Lite6 limits",
            )
        if not 0.0 < start_delta[index] <= MAX_START_DELTA_RADIANS:
            raise MotionSafetyError(
                "start_continuity_limit_invalid",
                "Start-continuity limits must be positive and at most five degrees",
            )

    if any(low >= high for low, high in zip(workspace_lower, workspace_upper)) or any(
        abs(value) > 2000.0 for value in workspace_lower + workspace_upper
    ):
        raise MotionSafetyError(
            "workspace_limits_invalid", "Workspace bounds are invalid"
        )
    if command_interval != COMMAND_INTERVAL_SECONDS:
        raise MotionSafetyError(
            "command_interval_invalid",
            "Command interval must be exactly 0.01 seconds (100 Hz)",
        )
    return MotionSafetyEnvelope(
        joint_lower_radians=lower,
        joint_upper_radians=upper,
        max_joint_velocity_radians_per_second=velocity,
        max_joint_acceleration_radians_per_second_squared=acceleration,
        max_start_delta_radians=start_delta,
        workspace_lower_xyz_mm=workspace_lower,
        workspace_upper_xyz_mm=workspace_upper,
        max_command_interval_seconds=command_interval,
    )


def _workspace_contains(safety: MotionSafetyEnvelope, pose: Sequence[float]) -> bool:
    return all(
        low <= value <= high
        for value, low, high in zip(
            pose[:3],
            safety.workspace_lower_xyz_mm,
            safety.workspace_upper_xyz_mm,
        )
    )


def _validate_frames(
    value: Any,
    safety: MotionSafetyEnvelope,
    forward_kinematics: Callable[[Sequence[float]], Sequence[float]],
) -> Tuple[Tuple[MotionFrame, ...], Tuple[Tuple[float, ...], ...]]:
    if not isinstance(value, list) or len(value) != REQUIRED_FRAME_COUNT:
        raise MotionArtifactError(
            "artifact_invalid",
            "Motion artifact needs exactly 1501 frames for 15 seconds at 100 Hz",
        )
    frames = []
    tcp_poses = []
    for index, raw_frame in enumerate(value):
        raw_frame = _exact_keys(
            raw_frame, ("time_seconds", "joints"), "frame {}".format(index)
        )
        frame = MotionFrame(
            time_seconds=_finite_number(
                raw_frame["time_seconds"], "frame {} time".format(index)
            ),
            joints=_finite_vector(
                raw_frame["joints"], JOINT_COUNT, "frame {} joints".format(index)
            ),
        )
        expected_time = index * COMMAND_INTERVAL_SECONDS
        if abs(frame.time_seconds - expected_time) > 1e-12:
            raise MotionSafetyError(
                "motion_timing_invalid",
                "Frame {} is outside the exact 100 Hz schedule".format(index),
            )
        for joint, low, high in zip(
            frame.joints,
            safety.joint_lower_radians,
            safety.joint_upper_radians,
        ):
            if not low <= joint <= high:
                raise MotionSafetyError(
                    "joint_limit_exceeded",
                    "Frame {} exceeds a commissioned joint limit".format(index),
                )
        try:
            pose = tuple(float(item) for item in forward_kinematics(frame.joints))
        except Exception as exc:
            raise MotionSafetyError(
                "workspace_unverifiable",
                "Forward kinematics failed for frame {}".format(index),
            ) from exc
        if len(pose) != 6 or not all(math.isfinite(item) for item in pose):
            raise MotionSafetyError(
                "workspace_unverifiable",
                "Forward kinematics returned invalid data for frame {}".format(index),
            )
        if not _workspace_contains(safety, pose):
            raise MotionSafetyError(
                "workspace_exceeded",
                "Frame {} leaves the commissioned workspace".format(index),
            )
        frames.append(frame)
        tcp_poses.append(pose)

    if frames[0].time_seconds != 0.0:
        raise MotionSafetyError(
            "motion_timing_invalid", "The first motion frame must be at 0 seconds"
        )
    if frames[-1].time_seconds != MOTION_DURATION_SECONDS:
        raise MotionSafetyError(
            "motion_timing_invalid", "The terminal motion frame must be at 15 seconds"
        )

    velocities = []
    intervals = []
    for index, (previous, current) in enumerate(zip(frames, frames[1:]), start=1):
        interval = current.time_seconds - previous.time_seconds
        if interval <= 0.0:
            raise MotionSafetyError(
                "motion_timing_invalid", "Motion timestamps must strictly increase"
            )
        if abs(interval - COMMAND_INTERVAL_SECONDS) > 1e-12:
            raise MotionSafetyError(
                "command_interval_exceeded",
                "Frame {} is outside the exact 100 Hz command interval".format(index),
            )
        velocity = tuple(
            (current_joint - previous_joint) / interval
            for previous_joint, current_joint in zip(previous.joints, current.joints)
        )
        for dimension, actual in enumerate(velocity):
            if (
                abs(actual)
                > safety.max_joint_velocity_radians_per_second[dimension] + 1e-12
            ):
                raise MotionSafetyError(
                    "velocity_exceeded",
                    "Frame {} exceeds a joint velocity limit".format(index),
                )
        intervals.append(interval)
        velocities.append(velocity)

    # Validate acceleration from rest into the first segment and between every
    # pair of segment velocities.  The terminal hold requirement below makes
    # the final segment exactly zero velocity before the watchdog halt.
    for dimension, actual in enumerate(velocities[0]):
        acceleration = abs(actual) / (intervals[0] / 2.0)
        if (
            acceleration
            > safety.max_joint_acceleration_radians_per_second_squared[dimension]
            + 1e-12
        ):
            raise MotionSafetyError(
                "acceleration_exceeded", "Motion accelerates too quickly from rest"
            )
    for index in range(1, len(velocities)):
        acceleration_interval = (intervals[index - 1] + intervals[index]) / 2.0
        for dimension, (previous, current) in enumerate(
            zip(velocities[index - 1], velocities[index])
        ):
            acceleration = abs(current - previous) / acceleration_interval
            if (
                acceleration
                > safety.max_joint_acceleration_radians_per_second_squared[dimension]
                + 1e-12
            ):
                raise MotionSafetyError(
                    "acceleration_exceeded",
                    "Frame {} exceeds a joint acceleration limit".format(index + 1),
                )

    if frames[-1].joints != frames[-2].joints:
        raise MotionSafetyError(
            "terminal_hold_required",
            "The 15-second terminal frame must repeat the prior absolute joints",
        )
    if not any(
        abs(frame.joints[dimension] - frames[0].joints[dimension]) > 1e-6
        for frame in frames[1:-1]
        for dimension in range(JOINT_COUNT)
    ):
        raise MotionSafetyError(
            "motion_required", "The commissioned primitive contains no movement"
        )
    return tuple(frames), tuple(tcp_poses)


def load_commissioned_motion_primitive(
    path: Union[os.PathLike, str],
    *,
    expected_sha256: str,
    forward_kinematics: Callable[[Sequence[float]], Sequence[float]],
) -> CaptureMotionPrimitive:
    """Load, hash-check, and fully safety-validate one reviewed primitive.

    ``expected_sha256`` is compared with the raw file bytes before JSON is
    parsed.  The parser rejects duplicate fields, NaN/Infinity, unknown fields,
    and gripper data.  No values are clipped, wrapped, interpolated, or fixed.
    """

    if not isinstance(expected_sha256, str) or not _SHA256.fullmatch(expected_sha256):
        raise MotionArtifactError(
            "artifact_hash_invalid", "Expected SHA-256 must be 64 lowercase hex"
        )
    if not callable(forward_kinematics):
        raise MotionArtifactError(
            "workspace_unverifiable", "Forward kinematics is required"
        )
    payload = _read_regular_file(Path(path))
    actual_sha256 = hashlib.sha256(payload).hexdigest()
    if not hmac.compare_digest(actual_sha256, expected_sha256):
        raise MotionArtifactError(
            "artifact_hash_mismatch", "Motion artifact does not match approved SHA-256"
        )
    try:
        decoded = payload.decode("utf-8")
        value = json.loads(
            decoded,
            object_pairs_hook=_reject_duplicate_keys,
            parse_constant=_reject_nonstandard_number,
        )
    except CaptureMotionError:
        raise
    except (UnicodeDecodeError, json.JSONDecodeError) as exc:
        raise MotionArtifactError(
            "artifact_invalid", "Motion artifact is not strict UTF-8 JSON"
        ) from exc
    value = _exact_keys(
        value,
        (
            "schema",
            "profile_id",
            "robot_model",
            "axis_count",
            "controller_device_type",
            "joint_limit_policy",
            "duration_seconds",
            "joint_space",
            "safety",
            "frames",
        ),
        "artifact",
    )
    if value["schema"] != MOTION_SCHEMA:
        raise MotionArtifactError(
            "artifact_schema_invalid", "Motion artifact schema is unsupported"
        )
    if value["joint_space"] != "absolute_radians":
        raise MotionArtifactError(
            "artifact_joint_space_invalid",
            "Motion artifact must contain absolute radian joints",
        )
    profile_id = value["profile_id"]
    if not isinstance(profile_id, str) or not _PROFILE_ID.fullmatch(profile_id):
        raise MotionArtifactError(
            "artifact_identity_invalid", "Motion profile id is invalid"
        )
    if (
        value["robot_model"] != ROBOT_MODEL
        or value["joint_limit_policy"] != JOINT_LIMIT_POLICY
        or type(value["axis_count"]) is not int
        or value["axis_count"] != JOINT_COUNT
        or type(value["controller_device_type"]) is not int
        or value["controller_device_type"] != CONTROLLER_DEVICE_TYPE
    ):
        raise MotionArtifactError(
            "artifact_identity_invalid",
            "Motion artifact does not identify the commissioned Lite6 policy",
        )
    duration = _finite_number(value["duration_seconds"], "duration")
    if duration != MOTION_DURATION_SECONDS:
        raise MotionArtifactError(
            "artifact_duration_invalid", "Motion artifact must last exactly 15 seconds"
        )
    safety = _parse_safety(value["safety"])
    frames, poses = _validate_frames(value["frames"], safety, forward_kinematics)
    return CaptureMotionPrimitive._create(
        profile_id=profile_id,
        profile_sha256=actual_sha256,
        safety=safety,
        frames=frames,
        tcp_poses=poses,
    )


class _HaltGate:
    """Call the physical halt seam at most once for one start generation."""

    def __init__(self, adapter: MotionAdapter) -> None:
        self.adapter = adapter
        self._lock = threading.Lock()
        self._attempted = False
        self._error: Optional[MotionExecutionError] = None

    def halt(self) -> None:
        with self._lock:
            if self._attempted:
                if self._error is not None:
                    raise self._error
                return
            self._attempted = True
            failures = []
            for name, callback in (
                ("command gate", self.adapter.lock_commands),
                ("controller halt", self.adapter.halt),
            ):
                try:
                    callback()
                except Exception as exc:
                    failures.append("{} ({})".format(name, type(exc).__name__))
            if failures:
                self._error = MotionExecutionError(
                    "motion_halt_failed",
                    "Motion halt failed: {}".format(", ".join(failures)),
                )
                raise self._error


class CommissionedMotionSession:
    """One pending/running primitive with an independent deadline watchdog."""

    def __init__(
        self,
        *,
        primitive: CaptureMotionPrimitive,
        adapter: MotionAdapter,
        halt_gate: _HaltGate,
        start_at_monotonic_s: float,
        stop_requested: Callable[[], bool],
        clock: Callable[[], float],
        sleeper: Callable[[float], None],
        maximum_poll_interval: float,
        maximum_dispatch_lateness_seconds: float,
    ) -> None:
        self.primitive = primitive
        self.adapter = adapter
        self.halt_gate = halt_gate
        self.start_at_monotonic_s = start_at_monotonic_s
        self.stop_requested = stop_requested
        self.clock = clock
        self.sleeper = sleeper
        self.maximum_poll_interval = maximum_poll_interval
        self.maximum_dispatch_lateness_seconds = maximum_dispatch_lateness_seconds
        self.deadline_monotonic_s = start_at_monotonic_s + primitive.duration_seconds
        self._stop_event = threading.Event()
        self._commands_done = threading.Event()
        self._done = threading.Event()
        self._state_lock = threading.Lock()
        self._command_count = 0
        self._error: Optional[MotionExecutionError] = None
        self._evidence: Optional[MotionExecutionEvidence] = None
        suffix = primitive.profile_sha256[:8]
        self._watchdog_thread = threading.Thread(
            target=self._watchdog,
            name="capture-motion-watchdog-{}".format(suffix),
            daemon=True,
        )
        self._execution_thread = threading.Thread(
            target=self._execute,
            name="capture-motion-executor-{}".format(suffix),
            daemon=True,
        )

    def launch(self) -> None:
        """Start the watchdog before command execution."""

        try:
            self._watchdog_thread.start()
            self._execution_thread.start()
        except Exception as exc:
            self._fail(
                MotionExecutionError(
                    "motion_worker_start_failed", "Motion workers could not start"
                )
            )
            raise MotionExecutionError(
                "motion_worker_start_failed", "Motion workers could not start"
            ) from exc

    def is_active(self) -> bool:
        return not self._done.is_set()

    def wait(self, timeout_seconds: float) -> MotionExecutionEvidence:
        try:
            timeout = float(timeout_seconds)
        except (TypeError, ValueError, OverflowError) as exc:
            raise MotionExecutionError(
                "motion_wait_invalid", "Motion wait timeout is invalid"
            ) from exc
        if not math.isfinite(timeout) or timeout < 0.0:
            raise MotionExecutionError(
                "motion_wait_invalid", "Motion wait timeout is invalid"
            )
        if not self._done.wait(timeout):
            self._fail(
                MotionExecutionError(
                    "motion_wait_timeout", "Motion completion watchdog timed out"
                )
            )
        with self._state_lock:
            error = self._error
            evidence = self._evidence
        if error is not None:
            raise error
        if evidence is None:
            self._fail(
                MotionExecutionError(
                    "motion_evidence_missing", "Motion completion evidence is missing"
                )
            )
            with self._state_lock:
                missing_error = self._error
            if missing_error is not None:
                raise missing_error
            raise MotionExecutionError(
                "motion_evidence_missing", "Motion completion evidence is missing"
            )
        return evidence

    def request_halt(self) -> None:
        self._fail(
            MotionExecutionError(
                "motion_stop_requested", "Motion was stopped before completion"
            )
        )
        with self._state_lock:
            error = self._error
        if error is not None and error.code == "motion_halt_failed":
            raise error

    def _external_stop_requested(self) -> bool:
        try:
            result = self.stop_requested()
        except Exception as exc:
            raise MotionExecutionError(
                "motion_stop_check_failed", "Motion stop callback failed"
            ) from exc
        if not isinstance(result, bool):
            raise MotionExecutionError(
                "motion_stop_check_failed", "Motion stop callback must return bool"
            )
        return result

    def _wait_until(self, deadline: float) -> None:
        while True:
            if self._stop_event.is_set():
                raise MotionExecutionError(
                    "motion_stop_requested", "Motion stop was requested"
                )
            if self._external_stop_requested():
                raise MotionExecutionError(
                    "motion_stop_requested", "Motion stop was requested"
                )
            remaining = deadline - float(self.clock())
            if remaining <= 0.0:
                return
            try:
                self.sleeper(min(remaining, self.maximum_poll_interval))
            except Exception as exc:
                raise MotionExecutionError(
                    "motion_clock_failed", "Motion scheduler wait failed"
                ) from exc

    def _execute(self) -> None:
        try:
            # The final 15-second frame is a mandatory duplicate hold marker.
            # It is deliberately not dispatched at the same instant that the
            # independent watchdog closes the command gate and halts motion.
            for frame in self.primitive.frames[:-1]:
                scheduled = self.start_at_monotonic_s + frame.time_seconds
                self._wait_until(scheduled)
                now = float(self.clock())
                if now - scheduled > self.maximum_dispatch_lateness_seconds:
                    raise MotionExecutionError(
                        "motion_dispatch_late",
                        "A reviewed joint command missed its dispatch deadline",
                    )
                if now >= self.deadline_monotonic_s:
                    raise MotionExecutionError(
                        "motion_deadline_exceeded",
                        "A joint command reached the 15-second watchdog deadline",
                    )
                command = AbsoluteJointCommand(
                    timestamp=frame.time_seconds,
                    joints=frame.joints,
                )
                try:
                    self.adapter.send_joint_command(command)
                except Exception as exc:
                    raise MotionExecutionError(
                        "motion_command_failed", "xArm joint command failed"
                    ) from exc
                with self._state_lock:
                    self._command_count += 1
            self._commands_done.set()
        except MotionExecutionError as exc:
            self._fail(exc)
        except Exception:
            self._fail(
                MotionExecutionError(
                    "motion_execution_failed", "Motion execution failed"
                )
            )

    def _watchdog(self) -> None:
        try:
            self._wait_until(self.deadline_monotonic_s)
            if not self._commands_done.is_set():
                raise MotionExecutionError(
                    "motion_commands_incomplete",
                    "Not all reviewed commands completed before the watchdog",
                )
            with self._state_lock:
                expected = len(self.primitive.frames) - 1
                if self._command_count != expected:
                    raise MotionExecutionError(
                        "motion_commands_incomplete",
                        "Motion command evidence is incomplete",
                    )
            self.halt_gate.halt()
            finished = max(float(self.clock()), self.deadline_monotonic_s)
            evidence = MotionExecutionEvidence(
                executed=True,
                profile_sha256=self.primitive.profile_sha256,
                started_at_monotonic_s=self.start_at_monotonic_s,
                finished_at_monotonic_s=finished,
                command_count=expected,
            )
            with self._state_lock:
                if self._error is None:
                    self._evidence = evidence
                    self._done.set()
        except MotionExecutionError as exc:
            self._fail(exc)
        except Exception:
            self._fail(
                MotionExecutionError("motion_watchdog_failed", "Motion watchdog failed")
            )

    def _fail(self, error: MotionExecutionError) -> None:
        with self._state_lock:
            if self._done.is_set():
                return
            self._stop_event.set()
        halt_error = None
        try:
            self.halt_gate.halt()
        except MotionExecutionError as exc:
            halt_error = exc
        with self._state_lock:
            if self._done.is_set():
                return
            self._error = halt_error or error
            self._done.set()


class CommissionedMotionRunner:
    """CaptureRunController-compatible runner for one reviewed primitive."""

    commissioned = True

    def __init__(
        self,
        primitive: CaptureMotionPrimitive,
        motion_adapter: MotionAdapter,
        *,
        clock: Callable[[], float] = time.monotonic,
        sleeper: Callable[[float], None] = time.sleep,
        maximum_poll_interval: float = 0.02,
        maximum_dispatch_lateness_seconds: float = 0.05,
    ) -> None:
        if not isinstance(primitive, CaptureMotionPrimitive) or (
            primitive._validation_token is not _VALIDATED_ARTIFACT
        ):
            raise MotionArtifactError(
                "motion_not_commissioned",
                "Runner requires a hash-validated commissioning primitive",
            )
        required = (
            "read_joint_angles_radians",
            "forward_kinematics",
            "prepare_motion",
            "send_joint_command",
            "lock_commands",
            "halt",
        )
        if any(not callable(getattr(motion_adapter, name, None)) for name in required):
            raise MotionExecutionError(
                "motion_adapter_invalid", "Motion adapter is incomplete"
            )
        prepared = getattr(motion_adapter, "prepared", None)
        if prepared is not True and prepared is not False:
            raise MotionExecutionError(
                "motion_adapter_invalid", "Motion adapter readiness state is invalid"
            )
        controller = getattr(motion_adapter, "arm", motion_adapter)
        reported_axis = getattr(
            controller, "axis", getattr(motion_adapter, "axis_count", None)
        )
        reported_device_type = getattr(
            controller,
            "device_type",
            getattr(motion_adapter, "controller_device_type", None),
        )
        if (
            isinstance(reported_axis, bool)
            or reported_axis != primitive.axis_count
            or isinstance(reported_device_type, bool)
            or reported_device_type != primitive.controller_device_type
        ):
            raise MotionExecutionError(
                "controller_identity_mismatch",
                "Motion adapter is not the commissioned Lite6 controller type",
            )
        self.primitive = primitive
        self.motion_adapter = motion_adapter
        self.profile_sha256 = primitive.profile_sha256
        self.duration_seconds = primitive.duration_seconds
        self.clock = clock
        self.sleeper = sleeper
        self.maximum_poll_interval = self._bounded_positive(
            maximum_poll_interval, "maximum poll interval", 0.05
        )
        self.maximum_dispatch_lateness_seconds = self._bounded_positive(
            maximum_dispatch_lateness_seconds,
            "maximum dispatch lateness",
            0.1,
        )
        self._lock = threading.Lock()
        self._arming_lock = threading.Lock()
        self._current_session: Optional[CommissionedMotionSession] = None
        self._halt_gate: Optional[_HaltGate] = None
        self._start_in_progress = False
        self._start_cancel_event = threading.Event()

    @staticmethod
    def _bounded_positive(value: Any, name: str, maximum: float) -> float:
        if isinstance(value, bool):
            raise MotionExecutionError(
                "motion_configuration_invalid", "{} is invalid".format(name)
            )
        try:
            number = float(value)
        except (TypeError, ValueError, OverflowError) as exc:
            raise MotionExecutionError(
                "motion_configuration_invalid", "{} is invalid".format(name)
            ) from exc
        if not math.isfinite(number) or not 0.0 < number <= maximum:
            raise MotionExecutionError(
                "motion_configuration_invalid", "{} is invalid".format(name)
            )
        return number

    def start(
        self,
        *,
        start_at_monotonic_s: float,
        duration_seconds: float,
        stop_requested: Callable[[], bool],
    ) -> CommissionedMotionSession:
        """Validate the live start state, arm, and launch the watchdog/session."""

        if isinstance(start_at_monotonic_s, bool) or isinstance(duration_seconds, bool):
            raise MotionExecutionError(
                "motion_start_invalid", "Motion start parameters are invalid"
            )
        try:
            start_at = float(start_at_monotonic_s)
            duration = float(duration_seconds)
            now = float(self.clock())
        except (TypeError, ValueError, OverflowError) as exc:
            raise MotionExecutionError(
                "motion_start_invalid", "Motion start parameters are invalid"
            ) from exc
        if not all(math.isfinite(value) for value in (start_at, duration, now)):
            raise MotionExecutionError(
                "motion_start_invalid", "Motion start parameters are invalid"
            )
        if duration != MOTION_DURATION_SECONDS:
            raise MotionExecutionError(
                "motion_duration_invalid", "Only the reviewed 15-second run is allowed"
            )
        if start_at < now or start_at - now > MAX_START_LEAD_SECONDS:
            raise MotionExecutionError(
                "motion_start_invalid",
                "Motion start must be a future monotonic deadline within two seconds",
            )
        if not callable(stop_requested):
            raise MotionExecutionError(
                "motion_start_invalid", "Motion stop callback is required"
            )

        with self._lock:
            if self._start_in_progress or (
                self._current_session is not None and self._current_session.is_active()
            ):
                raise MotionExecutionError(
                    "motion_busy", "A commissioned motion is already active"
                )
            halt_gate = _HaltGate(self.motion_adapter)
            start_cancel_event = threading.Event()
            self._halt_gate = halt_gate
            self._start_cancel_event = start_cancel_event
            self._start_in_progress = True

        def combined_stop_requested() -> bool:
            if start_cancel_event.is_set():
                return True
            try:
                result = stop_requested()
            except Exception as exc:
                raise MotionExecutionError(
                    "motion_stop_check_failed", "Motion stop callback failed"
                ) from exc
            if not isinstance(result, bool):
                raise MotionExecutionError(
                    "motion_stop_check_failed",
                    "Motion stop callback must return bool",
                )
            return result

        session: Optional[CommissionedMotionSession] = None
        try:
            self._require_start_not_stopped(combined_stop_requested, "before arming")
            # halt() sets the cancellation event before waiting on this lock.
            # If SDK preparation is already in progress, halt therefore closes
            # the gate immediately after that atomic adapter operation returns;
            # execution can never be launched afterward.
            with self._arming_lock:
                self._require_start_not_stopped(
                    combined_stop_requested, "before controller preparation"
                )
                self._preflight(start_at, combined_stop_requested)
            self._require_start_not_stopped(combined_stop_requested, "while arming")
            if float(self.clock()) - start_at > (
                self.maximum_dispatch_lateness_seconds
            ):
                raise MotionExecutionError(
                    "motion_start_late",
                    "Controller readiness missed the reviewed start deadline",
                )
            session = CommissionedMotionSession(
                primitive=self.primitive,
                adapter=self.motion_adapter,
                halt_gate=halt_gate,
                start_at_monotonic_s=start_at,
                stop_requested=combined_stop_requested,
                clock=self.clock,
                sleeper=self.sleeper,
                maximum_poll_interval=self.maximum_poll_interval,
                maximum_dispatch_lateness_seconds=(
                    self.maximum_dispatch_lateness_seconds
                ),
            )
            with self._lock:
                self._require_start_not_stopped(
                    combined_stop_requested, "before worker launch"
                )
                self._current_session = session
                session.launch()
                self._start_in_progress = False
            return session
        except Exception as exc:
            try:
                with self._arming_lock:
                    halt_gate.halt()
            except MotionExecutionError as halt_exc:
                raise halt_exc from exc
            if isinstance(exc, CaptureMotionError):
                raise
            raise MotionExecutionError(
                "motion_start_failed", "Commissioned motion could not start"
            ) from exc
        finally:
            with self._lock:
                self._start_in_progress = False

    @staticmethod
    def _require_start_not_stopped(
        stop_requested: Callable[[], bool], phase: str
    ) -> None:
        if stop_requested():
            raise MotionExecutionError(
                "motion_stop_requested",
                "Motion was stopped {}".format(phase),
            )

    def _preflight(
        self,
        start_at: float,
        stop_requested: Callable[[], bool],
    ) -> None:
        if getattr(self.motion_adapter, "prepared", None) is not False:
            raise MotionExecutionError(
                "controller_not_locked",
                "xArm command gate must be locked before commissioning preflight",
            )
        before = _runtime_vector(
            self.motion_adapter.read_joint_angles_radians(), "xArm joint state"
        )
        self._validate_live_start(before)
        self._require_start_not_stopped(stop_requested, "during state validation")
        try:
            self.motion_adapter.prepare_motion()
        except Exception as exc:
            raise MotionExecutionError(
                "controller_not_ready", "xArm controller readiness check failed"
            ) from exc
        if getattr(self.motion_adapter, "prepared", None) is not True:
            raise MotionExecutionError(
                "controller_not_ready", "xArm command gate did not open"
            )
        after = _runtime_vector(
            self.motion_adapter.read_joint_angles_radians(), "armed xArm joint state"
        )
        self._validate_live_start(after)
        self._require_start_not_stopped(stop_requested, "during controller preparation")
        for first, second, limit in zip(
            before, after, self.primitive.safety.max_start_delta_radians
        ):
            if abs(second - first) > limit:
                raise MotionSafetyError(
                    "controller_moved_during_arming",
                    "xArm joint state changed during controller arming",
                )
        if float(self.clock()) - start_at > self.maximum_dispatch_lateness_seconds:
            raise MotionExecutionError(
                "motion_start_late",
                "Controller readiness missed the reviewed start deadline",
            )

    def _validate_live_start(self, current: Tuple[float, ...]) -> None:
        safety = self.primitive.safety
        first = self.primitive.frames[0].joints
        for value, target, low, high, delta in zip(
            current,
            first,
            safety.joint_lower_radians,
            safety.joint_upper_radians,
            safety.max_start_delta_radians,
        ):
            if not low <= value <= high:
                raise MotionSafetyError(
                    "start_joint_limit_exceeded",
                    "Current xArm state is outside commissioned joint limits",
                )
            # Deliberately use absolute subtraction.  Joint values are never
            # normalized or wrapped across +/-pi.
            if abs(value - target) > delta:
                raise MotionSafetyError(
                    "start_discontinuous",
                    "Current xArm state is not continuous with the first waypoint",
                )
        try:
            pose = tuple(
                float(item) for item in self.motion_adapter.forward_kinematics(current)
            )
        except Exception as exc:
            raise MotionSafetyError(
                "workspace_unverifiable", "Live start FK could not be verified"
            ) from exc
        if len(pose) != 6 or not all(math.isfinite(item) for item in pose):
            raise MotionSafetyError(
                "workspace_unverifiable", "Live start FK returned invalid data"
            )
        if not _workspace_contains(safety, pose):
            raise MotionSafetyError(
                "start_workspace_exceeded",
                "Current xArm TCP is outside the commissioned workspace",
            )

    def halt(self) -> None:
        """Idempotently close the gate and halt the current start generation."""

        with self._lock:
            self._start_cancel_event.set()
            session = self._current_session
            halt_gate = self._halt_gate
        if session is not None and session.is_active():
            session.request_halt()
            return
        if halt_gate is not None:
            with self._arming_lock:
                halt_gate.halt()


__all__ = [
    "AbsoluteJointCommand",
    "CaptureMotionError",
    "CaptureMotionPrimitive",
    "CommissionedMotionRunner",
    "CommissionedMotionSession",
    "HARD_JOINT_LOWER_RADIANS",
    "HARD_JOINT_UPPER_RADIANS",
    "HARD_MAX_ACCELERATION_RADIANS_PER_SECOND_SQUARED",
    "HARD_MAX_VELOCITY_RADIANS_PER_SECOND",
    "JOINT_COUNT",
    "MAX_COMMAND_INTERVAL_SECONDS",
    "MAX_START_DELTA_RADIANS",
    "MOTION_DURATION_SECONDS",
    "MOTION_SCHEMA",
    "MotionArtifactError",
    "MotionExecutionError",
    "MotionFrame",
    "MotionSafetyEnvelope",
    "MotionSafetyError",
    "load_commissioned_motion_primitive",
]
