"""Eye-in-hand camera calibration and safe runtime workspace mapping.

Transform names follow the convention ``destination_T_source``.  Therefore:

* ``base_T_tool`` maps tool coordinates into the robot base frame;
* ``camera_T_target`` maps calibration-target coordinates into the camera;
* ``tool_T_camera`` maps camera coordinates into the robot tool frame.

For a static calibration target, every accepted sample should satisfy::

    base_T_target =
        base_T_tool @ tool_T_camera @ camera_T_target

At runtime a depth pixel is mapped with::

    base_point =
        base_T_tool @ tool_T_camera @ camera_point

OpenCV is imported only when solving a real hand-eye calibration or
undistorting a camera with non-zero distortion coefficients.
"""

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,
    List,
    Mapping,
    Optional,
    Protocol,
    Sequence,
    Tuple,
    Union,
)

import numpy as np

from .cameras import CameraRole


class CalibrationError(ValueError):
    """Base class for invalid eye-in-hand calibration operations."""


class CalibrationDependencyError(CalibrationError):
    """Raised when a lazily required calibration dependency is unavailable."""


class CalibrationNotReady(CalibrationError):
    """Raised when runtime mapping is attempted with an unapproved calibration."""


class CalibrationQualityError(CalibrationError):
    """A rejected solution and its machine-readable quality report."""

    def __init__(self, report: "HandEyeQualityReport") -> None:
        self.report = report
        super().__init__(
            "Eye-in-hand calibration rejected: "
            + "; ".join(report.reasons or ("quality gates failed",))
        )


@dataclass(frozen=True)
class RigidTransform:
    """Validated 4x4 proper rigid transform."""

    matrix: Tuple[Tuple[float, float, float, float], ...]

    def __post_init__(self) -> None:
        array = np.asarray(self.matrix, dtype=float)
        if array.shape != (4, 4) or not np.all(np.isfinite(array)):
            raise CalibrationError("Rigid transform must be a finite 4x4 matrix")
        if not np.allclose(array[3], (0.0, 0.0, 0.0, 1.0), atol=1e-8):
            raise CalibrationError("Rigid transform bottom row must be [0, 0, 0, 1]")
        rotation = array[:3, :3]
        if not np.allclose(rotation.T.dot(rotation), np.eye(3), atol=1e-5):
            raise CalibrationError("Rigid transform rotation is not orthonormal")
        determinant = float(np.linalg.det(rotation))
        if abs(determinant - 1.0) > 1e-5:
            raise CalibrationError("Rigid transform rotation must have determinant +1")
        object.__setattr__(
            self,
            "matrix",
            tuple(tuple(float(value) for value in row) for row in array.tolist()),
        )

    @classmethod
    def identity(cls) -> "RigidTransform":
        return cls.from_array(np.eye(4))

    @classmethod
    def from_array(cls, value: Sequence[Sequence[float]]) -> "RigidTransform":
        array = np.asarray(value, dtype=float)
        return cls(tuple(tuple(float(item) for item in row) for row in array.tolist()))

    @classmethod
    def from_rotation_translation(
        cls,
        rotation: Sequence[Sequence[float]],
        translation_xyz: Sequence[float],
    ) -> "RigidTransform":
        matrix = np.eye(4)
        matrix[:3, :3] = np.asarray(rotation, dtype=float)
        translation = np.asarray(translation_xyz, dtype=float).reshape(-1)
        if translation.shape != (3,):
            raise CalibrationError("Transform translation must contain x, y, and z")
        matrix[:3, 3] = translation
        return cls.from_array(matrix)

    @classmethod
    def from_xyz_rpy(
        cls,
        xyz: Sequence[float],
        rpy_radians: Sequence[float],
    ) -> "RigidTransform":
        xyz_array = np.asarray(xyz, dtype=float).reshape(-1)
        rpy = np.asarray(rpy_radians, dtype=float).reshape(-1)
        if xyz_array.shape != (3,) or rpy.shape != (3,):
            raise CalibrationError("xyz and rpy_radians must each have three values")
        roll, pitch, yaw = rpy
        cx, sx = math.cos(roll), math.sin(roll)
        cy, sy = math.cos(pitch), math.sin(pitch)
        cz, sz = math.cos(yaw), math.sin(yaw)
        rotation_x = np.asarray(((1.0, 0.0, 0.0), (0.0, cx, -sx), (0.0, sx, cx)))
        rotation_y = np.asarray(((cy, 0.0, sy), (0.0, 1.0, 0.0), (-sy, 0.0, cy)))
        rotation_z = np.asarray(((cz, -sz, 0.0), (sz, cz, 0.0), (0.0, 0.0, 1.0)))
        return cls.from_rotation_translation(
            rotation_z.dot(rotation_y).dot(rotation_x), xyz_array
        )

    @property
    def array(self) -> np.ndarray:
        return np.asarray(self.matrix, dtype=float)

    @property
    def rotation(self) -> np.ndarray:
        return self.array[:3, :3]

    @property
    def translation(self) -> np.ndarray:
        return self.array[:3, 3]

    def inverse(self) -> "RigidTransform":
        rotation = self.rotation
        inverse_rotation = rotation.T
        return RigidTransform.from_rotation_translation(
            inverse_rotation, -inverse_rotation.dot(self.translation)
        )

    def compose(self, other: "RigidTransform") -> "RigidTransform":
        return RigidTransform.from_array(self.array.dot(other.array))

    def transform_point(self, point_xyz: Sequence[float]) -> Tuple[float, float, float]:
        point = np.asarray(point_xyz, dtype=float).reshape(-1)
        if point.shape != (3,) or not np.all(np.isfinite(point)):
            raise CalibrationError("Point must contain finite x, y, and z")
        transformed = self.rotation.dot(point) + self.translation
        return tuple(float(value) for value in transformed)

    def to_list(self) -> List[List[float]]:
        return [list(row) for row in self.matrix]


@dataclass(frozen=True)
class CameraIntrinsics:
    """Pinhole intrinsics and optional OpenCV distortion coefficients."""

    width: int
    height: int
    fx: float
    fy: float
    cx: float
    cy: float
    distortion: Tuple[float, ...] = ()

    def __post_init__(self) -> None:
        if int(self.width) < 2 or int(self.height) < 2:
            raise CalibrationError("Camera image dimensions must be at least 2x2")
        numbers = tuple(float(value) for value in (self.fx, self.fy, self.cx, self.cy))
        if not all(math.isfinite(value) for value in numbers):
            raise CalibrationError("Camera intrinsics must be finite")
        fx, fy, cx, cy = numbers
        if fx <= 0 or fy <= 0:
            raise CalibrationError("Camera focal lengths must be positive")
        if not 0.0 <= cx < float(self.width) or not 0.0 <= cy < float(self.height):
            raise CalibrationError("Principal point must lie inside the image")
        distortion = tuple(float(value) for value in self.distortion)
        if not all(math.isfinite(value) for value in distortion):
            raise CalibrationError("Distortion coefficients must be finite")
        if distortion and len(distortion) not in (4, 5, 8, 12, 14):
            raise CalibrationError(
                "OpenCV distortion must have 4, 5, 8, 12, or 14 coefficients"
            )
        object.__setattr__(self, "width", int(self.width))
        object.__setattr__(self, "height", int(self.height))
        object.__setattr__(self, "fx", fx)
        object.__setattr__(self, "fy", fy)
        object.__setattr__(self, "cx", cx)
        object.__setattr__(self, "cy", cy)
        object.__setattr__(self, "distortion", distortion)

    @property
    def camera_matrix(self) -> np.ndarray:
        return np.asarray(
            (
                (self.fx, 0.0, self.cx),
                (0.0, self.fy, self.cy),
                (0.0, 0.0, 1.0),
            ),
            dtype=float,
        )

    def unproject(
        self, pixel_xy: Sequence[float], depth_mm: float
    ) -> Tuple[float, float, float]:
        pixel = np.asarray(pixel_xy, dtype=float).reshape(-1)
        if pixel.shape != (2,) or not np.all(np.isfinite(pixel)):
            raise CalibrationError("Pixel must contain finite x and y")
        x, y = float(pixel[0]), float(pixel[1])
        if not 0.0 <= x < self.width or not 0.0 <= y < self.height:
            raise CalibrationError("Pixel lies outside the calibrated image")
        depth = float(depth_mm)
        if not math.isfinite(depth) or depth <= 0:
            raise CalibrationError("Depth must be positive and finite")

        if any(abs(value) > 1e-15 for value in self.distortion):
            try:
                import cv2
            except ImportError as exc:  # pragma: no cover - deployment dependency
                raise CalibrationDependencyError(
                    "OpenCV is required to unproject distorted camera pixels"
                ) from exc
            points = np.asarray([[[x, y]]], dtype=np.float64)
            normalized = cv2.undistortPoints(
                points,
                self.camera_matrix,
                np.asarray(self.distortion, dtype=float),
            )[0, 0]
            normalized_x, normalized_y = float(normalized[0]), float(normalized[1])
        else:
            normalized_x = (x - self.cx) / self.fx
            normalized_y = (y - self.cy) / self.fy
        return (
            normalized_x * depth,
            normalized_y * depth,
            depth,
        )

    def to_dict(self) -> Dict[str, Any]:
        return {
            "width": self.width,
            "height": self.height,
            "fx": self.fx,
            "fy": self.fy,
            "cx": self.cx,
            "cy": self.cy,
            "distortion": list(self.distortion),
        }

    @classmethod
    def from_dict(cls, value: Mapping[str, Any]) -> "CameraIntrinsics":
        return cls(
            width=int(value["width"]),
            height=int(value["height"]),
            fx=float(value["fx"]),
            fy=float(value["fy"]),
            cx=float(value["cx"]),
            cy=float(value["cy"]),
            distortion=tuple(value.get("distortion", ())),
        )


@dataclass(frozen=True)
class HandEyeSample:
    """One synchronized robot pose and observed calibration-target pose."""

    base_T_tool: RigidTransform
    camera_T_target: RigidTransform
    image_captured_at_s: float
    tool_pose_at_s: float

    def __post_init__(self) -> None:
        if not isinstance(self.base_T_tool, RigidTransform):
            raise CalibrationError("base_T_tool must be a RigidTransform")
        if not isinstance(self.camera_T_target, RigidTransform):
            raise CalibrationError("camera_T_target must be a RigidTransform")
        image_time = float(self.image_captured_at_s)
        pose_time = float(self.tool_pose_at_s)
        if not math.isfinite(image_time) or not math.isfinite(pose_time):
            raise CalibrationError("Calibration sample timestamps must be finite")
        object.__setattr__(self, "image_captured_at_s", image_time)
        object.__setattr__(self, "tool_pose_at_s", pose_time)

    @property
    def skew_s(self) -> float:
        return abs(self.image_captured_at_s - self.tool_pose_at_s)


@dataclass(frozen=True)
class HandEyeQualityPolicy:
    min_samples: int = 10
    max_sample_skew_s: float = 0.03
    min_translation_span_mm: float = 60.0
    min_rotation_span_deg: float = 25.0
    min_rotation_axis_separation_deg: float = 15.0
    max_translation_rms_mm: float = 3.0
    max_translation_residual_mm: float = 7.0
    max_rotation_rms_deg: float = 1.5
    max_rotation_residual_deg: float = 4.0

    def __post_init__(self) -> None:
        if self.min_samples < 4:
            raise CalibrationError("Hand-eye calibration needs at least four samples")
        positive = (
            self.max_sample_skew_s,
            self.min_translation_span_mm,
            self.min_rotation_span_deg,
            self.min_rotation_axis_separation_deg,
            self.max_translation_rms_mm,
            self.max_translation_residual_mm,
            self.max_rotation_rms_deg,
            self.max_rotation_residual_deg,
        )
        if not all(math.isfinite(value) and value > 0 for value in positive):
            raise CalibrationError("All hand-eye quality thresholds must be positive")


@dataclass(frozen=True)
class HandEyeQualityReport:
    accepted: bool
    sample_count: int
    translation_span_mm: float
    rotation_span_deg: float
    rotation_axis_separation_deg: float
    translation_rms_mm: Optional[float] = None
    translation_max_mm: Optional[float] = None
    rotation_rms_deg: Optional[float] = None
    rotation_max_deg: Optional[float] = None
    reasons: Tuple[str, ...] = ()

    def __post_init__(self) -> None:
        sample_count = int(self.sample_count)
        if sample_count < 0:
            raise CalibrationError("Calibration sample count cannot be negative")
        required = (
            float(self.translation_span_mm),
            float(self.rotation_span_deg),
            float(self.rotation_axis_separation_deg),
        )
        if not all(math.isfinite(value) and value >= 0 for value in required):
            raise CalibrationError(
                "Calibration diversity values must be finite and non-negative"
            )
        residuals = (
            self.translation_rms_mm,
            self.translation_max_mm,
            self.rotation_rms_deg,
            self.rotation_max_deg,
        )
        converted_residuals = tuple(
            _optional_nonnegative_float(value) for value in residuals
        )
        reasons = tuple(str(reason) for reason in self.reasons)
        if self.accepted:
            if sample_count < 4 or any(value is None for value in converted_residuals):
                raise CalibrationError(
                    "Accepted calibration needs at least four samples and all "
                    "residual measurements"
                )
            if reasons:
                raise CalibrationError(
                    "Accepted calibration cannot contain rejection reasons"
                )
        object.__setattr__(self, "sample_count", sample_count)
        object.__setattr__(self, "translation_span_mm", required[0])
        object.__setattr__(self, "rotation_span_deg", required[1])
        object.__setattr__(self, "rotation_axis_separation_deg", required[2])
        object.__setattr__(self, "translation_rms_mm", converted_residuals[0])
        object.__setattr__(self, "translation_max_mm", converted_residuals[1])
        object.__setattr__(self, "rotation_rms_deg", converted_residuals[2])
        object.__setattr__(self, "rotation_max_deg", converted_residuals[3])
        object.__setattr__(self, "reasons", reasons)

    def to_dict(self) -> Dict[str, Any]:
        return {
            "accepted": self.accepted,
            "sample_count": self.sample_count,
            "translation_span_mm": self.translation_span_mm,
            "rotation_span_deg": self.rotation_span_deg,
            "rotation_axis_separation_deg": self.rotation_axis_separation_deg,
            "translation_rms_mm": self.translation_rms_mm,
            "translation_max_mm": self.translation_max_mm,
            "rotation_rms_deg": self.rotation_rms_deg,
            "rotation_max_deg": self.rotation_max_deg,
            "reasons": list(self.reasons),
        }

    @classmethod
    def from_dict(cls, value: Mapping[str, Any]) -> "HandEyeQualityReport":
        return cls(
            accepted=bool(value["accepted"]),
            sample_count=int(value["sample_count"]),
            translation_span_mm=float(value["translation_span_mm"]),
            rotation_span_deg=float(value["rotation_span_deg"]),
            rotation_axis_separation_deg=float(value["rotation_axis_separation_deg"]),
            translation_rms_mm=_optional_float(value.get("translation_rms_mm")),
            translation_max_mm=_optional_float(value.get("translation_max_mm")),
            rotation_rms_deg=_optional_float(value.get("rotation_rms_deg")),
            rotation_max_deg=_optional_float(value.get("rotation_max_deg")),
            reasons=tuple(str(item) for item in value.get("reasons", ())),
        )


@dataclass(frozen=True)
class EyeInHandCalibration:
    """Persistable, immutable eye-in-hand calibration artifact."""

    intrinsics: CameraIntrinsics
    camera_source_id: str
    tool_T_camera: RigidTransform
    quality: HandEyeQualityReport
    created_at_s: float
    schema_version: int = 1

    def __post_init__(self) -> None:
        if not isinstance(self.intrinsics, CameraIntrinsics):
            raise CalibrationError("Calibration intrinsics are invalid")
        camera_source_id = str(self.camera_source_id).strip()
        if not camera_source_id:
            raise CalibrationError("Calibration camera_source_id is required")
        object.__setattr__(self, "camera_source_id", camera_source_id)
        if not isinstance(self.tool_T_camera, RigidTransform):
            raise CalibrationError("Calibration tool_T_camera is invalid")
        if not isinstance(self.quality, HandEyeQualityReport):
            raise CalibrationError("Calibration quality report is invalid")
        if self.schema_version != 1:
            raise CalibrationError("Unsupported eye-in-hand schema version")
        if not math.isfinite(float(self.created_at_s)):
            raise CalibrationError("Calibration creation timestamp must be finite")
        object.__setattr__(self, "created_at_s", float(self.created_at_s))
        if not self.quality.accepted:
            raise CalibrationError("Rejected calibration cannot be persisted as ready")

    @property
    def ready(self) -> bool:
        return self.quality.accepted

    def to_dict(self) -> Dict[str, Any]:
        return {
            "schema_version": self.schema_version,
            "created_at_s": self.created_at_s,
            "camera_source_id": self.camera_source_id,
            "intrinsics": self.intrinsics.to_dict(),
            "tool_T_camera": self.tool_T_camera.to_list(),
            "quality": self.quality.to_dict(),
        }

    @classmethod
    def from_dict(cls, value: Mapping[str, Any]) -> "EyeInHandCalibration":
        return cls(
            schema_version=int(value.get("schema_version", 0)),
            created_at_s=float(value["created_at_s"]),
            camera_source_id=str(value["camera_source_id"]),
            intrinsics=CameraIntrinsics.from_dict(value["intrinsics"]),
            tool_T_camera=RigidTransform.from_array(value["tool_T_camera"]),
            quality=HandEyeQualityReport.from_dict(value["quality"]),
        )


class EyeInHandCalibrationStore:
    """Atomic, versioned JSON persistence for approved calibration artifacts."""

    def save(
        self,
        path: Union[str, os.PathLike],
        calibration: EyeInHandCalibration,
    ) -> Path:
        destination = Path(path)
        destination.parent.mkdir(parents=True, exist_ok=True)
        payload = json.dumps(
            calibration.to_dict(),
            ensure_ascii=False,
            allow_nan=False,
            indent=2,
            sort_keys=True,
        )
        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.chmod(temporary_name, 0o600)
            os.replace(temporary_name, destination)
            _fsync_directory(destination.parent)
        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,
    ) -> EyeInHandCalibration:
        source = Path(path)
        if source.is_symlink() or not source.is_file():
            raise CalibrationError(
                "Calibration path must be a regular, non-symlink file"
            )
        try:
            raw_payload = source.read_bytes()
        except OSError as exc:
            raise CalibrationError("Calibration file could not be read") from exc
        if not raw_payload:
            raise CalibrationError("Calibration file is empty")
        if len(raw_payload) > 1024 * 1024:
            raise CalibrationError("Calibration file exceeds 1 MiB limit")
        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 CalibrationError("Expected calibration SHA-256 is invalid")
            actual = hashlib.sha256(raw_payload).hexdigest()
            if not hmac.compare_digest(actual, expected):
                raise CalibrationError(
                    "Calibration SHA-256 does not match the approved artifact"
                )
        try:
            value = json.loads(raw_payload.decode("utf-8"))
        except (UnicodeError, json.JSONDecodeError) as exc:
            raise CalibrationError("Calibration file is not valid JSON") from exc
        if not isinstance(value, Mapping):
            raise CalibrationError("Calibration file must contain a JSON object")
        return EyeInHandCalibration.from_dict(value)


HandEyeSolver = Callable[[Sequence[HandEyeSample]], RigidTransform]


class EyeInHandCalibrator:
    """Collect synchronized samples and solve after strict diversity checks."""

    def __init__(
        self,
        intrinsics: CameraIntrinsics,
        *,
        camera_source_id: str,
        quality_policy: HandEyeQualityPolicy = HandEyeQualityPolicy(),
        hand_eye_solver: Optional[HandEyeSolver] = None,
        clock: Callable[[], float] = time.time,
    ) -> None:
        self.intrinsics = intrinsics
        self.camera_source_id = str(camera_source_id).strip()
        if not self.camera_source_id:
            raise CalibrationError("camera_source_id is required")
        self.quality_policy = quality_policy
        self.hand_eye_solver = hand_eye_solver
        self.clock = clock
        self._samples: List[HandEyeSample] = []

    @property
    def samples(self) -> Tuple[HandEyeSample, ...]:
        return tuple(self._samples)

    def add_sample(self, sample: HandEyeSample) -> None:
        if sample.skew_s > self.quality_policy.max_sample_skew_s:
            raise CalibrationError(
                "Robot pose and image skew {:.4f}s exceeds {:.4f}s".format(
                    sample.skew_s, self.quality_policy.max_sample_skew_s
                )
            )
        self._samples.append(sample)

    def clear(self) -> None:
        self._samples = []

    def solve(self) -> EyeInHandCalibration:
        diversity = _diversity_report(self._samples, self.quality_policy)
        if diversity.reasons:
            raise CalibrationQualityError(diversity)

        if self.hand_eye_solver is not None:
            tool_T_camera = self.hand_eye_solver(tuple(self._samples))
        else:
            tool_T_camera = _solve_with_opencv(self._samples)
        if not isinstance(tool_T_camera, RigidTransform):
            raise CalibrationError("Hand-eye solver must return a RigidTransform")

        report = _residual_report(
            self._samples,
            tool_T_camera,
            self.quality_policy,
            diversity,
        )
        if not report.accepted:
            raise CalibrationQualityError(report)
        return EyeInHandCalibration(
            intrinsics=self.intrinsics,
            camera_source_id=self.camera_source_id,
            tool_T_camera=tool_T_camera,
            quality=report,
            created_at_s=float(self.clock()),
        )


@dataclass(frozen=True)
class TimedToolPose:
    base_T_tool: RigidTransform
    timestamp_s: float

    def __post_init__(self) -> None:
        if not isinstance(self.base_T_tool, RigidTransform):
            raise CalibrationError("base_T_tool must be a RigidTransform")
        timestamp = float(self.timestamp_s)
        if not math.isfinite(timestamp):
            raise CalibrationError("Tool pose timestamp must be finite")
        object.__setattr__(self, "timestamp_s", timestamp)


def xarm_pose_to_base_T_tool(pose_mm_degrees: Any) -> RigidTransform:
    """Convert xArm ``get_position`` output into ``base_T_tool``.

    Accepted input is either the six-value payload
    ``[x_mm, y_mm, z_mm, roll_deg, pitch_deg, yaw_deg]`` or the SDK's common
    ``(return_code, payload)`` result.  Non-zero SDK return codes fail closed.
    """

    pose = pose_mm_degrees
    if isinstance(pose, (tuple, list)) and len(pose) == 2 and isinstance(pose[0], int):
        if pose[0] != 0:
            raise CalibrationError("xArm get_position returned code {}".format(pose[0]))
        pose = pose[1]
    try:
        values = tuple(float(value) for value in pose)
    except (TypeError, ValueError) as exc:
        raise CalibrationError("xArm pose must be a numeric sequence") from exc
    if len(values) < 6 or not all(math.isfinite(value) for value in values[:6]):
        raise CalibrationError("xArm pose must contain six finite mm/degree values")
    return RigidTransform.from_xyz_rpy(
        values[:3],
        tuple(math.radians(value) for value in values[3:6]),
    )


@dataclass(frozen=True)
class DepthObservation:
    depth_mm: float
    timestamp_s: float

    def __post_init__(self) -> None:
        depth = float(self.depth_mm)
        timestamp = float(self.timestamp_s)
        if not math.isfinite(depth):
            raise CalibrationError("Depth must be finite")
        if not math.isfinite(timestamp):
            raise CalibrationError("Depth timestamp must be finite")
        object.__setattr__(self, "depth_mm", depth)
        object.__setattr__(self, "timestamp_s", timestamp)


@dataclass(frozen=True)
class CalibrationReadiness:
    ready: bool
    reasons: Tuple[str, ...] = ()


class ToolPoseProvider(Protocol):
    def __call__(self, captured_at_s: float) -> TimedToolPose:
        """Return the closest synchronized base-to-tool pose."""


class TrackDepthProvider(Protocol):
    def __call__(self, track: Any) -> DepthObservation:
        """Return aligned depth at a tracked image location."""


class TimedToolPoseBuffer:
    """Thread-safe bounded history for xArm base-to-tool poses."""

    def __init__(self, *, maximum_poses: int = 256) -> None:
        if maximum_poses < 2:
            raise CalibrationError("maximum_poses must be at least two")
        self.maximum_poses = int(maximum_poses)
        self._poses: Deque[TimedToolPose] = deque(maxlen=self.maximum_poses)
        self._lock = threading.Lock()

    def add(self, pose: TimedToolPose) -> None:
        if not isinstance(pose, TimedToolPose):
            raise CalibrationError("Pose buffer accepts TimedToolPose values")
        with self._lock:
            if self._poses and pose.timestamp_s <= self._poses[-1].timestamp_s:
                raise CalibrationError(
                    "Tool pose timestamps must be strictly increasing"
                )
            self._poses.append(pose)

    def resolve(self, captured_at_s: float) -> TimedToolPose:
        timestamp = float(captured_at_s)
        if not math.isfinite(timestamp):
            raise CalibrationError("Requested tool pose timestamp must be finite")
        with self._lock:
            if not self._poses:
                raise CalibrationNotReady("Tool pose history is empty")
            return min(
                self._poses,
                key=lambda pose: abs(pose.timestamp_s - timestamp),
            )

    def __len__(self) -> int:
        with self._lock:
            return len(self._poses)


@dataclass(frozen=True)
class AlignedDepthFrame:
    """One aligned depth array kept outside serializable camera metadata."""

    timestamp_s: float
    depth_mm: Any = field(repr=False, compare=False)

    def __post_init__(self) -> None:
        timestamp = float(self.timestamp_s)
        if not math.isfinite(timestamp):
            raise CalibrationError("Depth-frame timestamp must be finite")
        array = np.asarray(self.depth_mm)
        if array.ndim != 2 or array.size == 0:
            raise CalibrationError("Aligned depth frame must be a non-empty 2-D array")
        if array.dtype.kind not in ("u", "i", "f"):
            raise CalibrationError("Aligned depth frame must contain numeric values")
        object.__setattr__(self, "timestamp_s", timestamp)


class SynchronizedDepthBuffer:
    """Bounded resolver for OAK-D depth aligned to the color image.

    Camera code calls :meth:`add` with the aligned depth ndarray and the same
    wall-clock timestamp used for its :class:`CameraFrame`.  No depth ndarray is
    copied into frame metadata or a telemetry payload.
    """

    def __init__(
        self,
        *,
        image_width: int,
        image_height: int,
        maximum_frames: int = 8,
        median_radius_px: int = 1,
    ) -> None:
        if image_width < 2 or image_height < 2:
            raise CalibrationError("Depth dimensions must be at least 2x2")
        if maximum_frames < 1:
            raise CalibrationError("maximum_frames must be positive")
        if median_radius_px < 0 or median_radius_px > 10:
            raise CalibrationError("median_radius_px must be between 0 and 10")
        self.image_width = int(image_width)
        self.image_height = int(image_height)
        self.median_radius_px = int(median_radius_px)
        self._frames: Deque[AlignedDepthFrame] = deque(maxlen=int(maximum_frames))
        self._lock = threading.Lock()

    def add(self, frame: AlignedDepthFrame) -> None:
        if not isinstance(frame, AlignedDepthFrame):
            raise CalibrationError("Depth buffer accepts AlignedDepthFrame values")
        array = np.asarray(frame.depth_mm)
        if array.shape != (self.image_height, self.image_width):
            raise CalibrationError(
                "Aligned depth dimensions do not match calibrated color image"
            )
        # Copy so a camera SDK cannot mutate or recycle the backing buffer.
        owned = AlignedDepthFrame(
            timestamp_s=float(frame.timestamp_s),
            depth_mm=np.array(array, copy=True),
        )
        with self._lock:
            if self._frames and owned.timestamp_s <= self._frames[-1].timestamp_s:
                raise CalibrationError(
                    "Aligned depth timestamps must be strictly increasing"
                )
            self._frames.append(owned)

    def resolve(self, track: Any) -> DepthObservation:
        captured_at = float(track.captured_at_s)
        with self._lock:
            if not self._frames:
                raise CalibrationNotReady("Aligned depth buffer is empty")
            frame = min(
                self._frames,
                key=lambda item: abs(item.timestamp_s - captured_at),
            )
            depth_image = frame.depth_mm

        x = int(round(float(track.center_xy[0])))
        y = int(round(float(track.center_xy[1])))
        if not 0 <= x < self.image_width or not 0 <= y < self.image_height:
            raise CalibrationError("Tracked pixel lies outside aligned depth image")
        radius = self.median_radius_px
        x_start, x_end = max(0, x - radius), min(self.image_width, x + radius + 1)
        y_start, y_end = max(0, y - radius), min(self.image_height, y + radius + 1)
        window = np.asarray(
            depth_image[y_start:y_end, x_start:x_end], dtype=float
        ).reshape(-1)
        valid = window[np.isfinite(window) & (window > 0.0)]
        if valid.size == 0:
            raise CalibrationError("Aligned depth has no valid value near target")
        return DepthObservation(
            depth_mm=float(np.median(valid)),
            timestamp_s=float(frame.timestamp_s),
        )

    def __len__(self) -> int:
        with self._lock:
            return len(self._frames)


class EyeInHandWorkspaceMapper:
    """Map tool-camera tracks into the robot base frame with synchronized depth."""

    def __init__(
        self,
        calibration: EyeInHandCalibration,
        *,
        runtime_camera_source_id: str,
        tool_pose_provider: ToolPoseProvider,
        depth_provider: TrackDepthProvider,
        workspace_lower_xyz_mm: Sequence[float],
        workspace_upper_xyz_mm: Sequence[float],
        max_frame_age_s: float = 0.30,
        max_pose_skew_s: float = 0.03,
        max_depth_skew_s: float = 0.03,
        min_depth_mm: float = 30.0,
        max_depth_mm: float = 3000.0,
        clock: Callable[[], float] = time.time,
    ) -> None:
        if not isinstance(calibration, EyeInHandCalibration):
            raise CalibrationNotReady(
                "A quality-approved EyeInHandCalibration is required"
            )
        if not calibration.ready:
            raise CalibrationNotReady("Eye-in-hand calibration quality is not approved")
        runtime_source = str(runtime_camera_source_id).strip()
        if not runtime_source:
            raise CalibrationNotReady("runtime_camera_source_id is required")
        if runtime_source != calibration.camera_source_id:
            raise CalibrationNotReady(
                "Runtime tool camera does not match calibrated camera source"
            )
        self.calibration = calibration
        self.runtime_camera_source_id = runtime_source
        self.tool_pose_provider = tool_pose_provider
        self.depth_provider = depth_provider
        self.lower = _xyz_tuple(workspace_lower_xyz_mm, "workspace lower bound")
        self.upper = _xyz_tuple(workspace_upper_xyz_mm, "workspace upper bound")
        if any(low >= high for low, high in zip(self.lower, self.upper)):
            raise CalibrationError(
                "Each workspace lower bound must be below its upper bound"
            )
        self.max_frame_age_s = _positive(max_frame_age_s, "max_frame_age_s")
        self.max_pose_skew_s = _positive(max_pose_skew_s, "max_pose_skew_s")
        self.max_depth_skew_s = _positive(max_depth_skew_s, "max_depth_skew_s")
        self.min_depth_mm = _positive(min_depth_mm, "min_depth_mm")
        self.max_depth_mm = _positive(max_depth_mm, "max_depth_mm")
        if self.min_depth_mm >= self.max_depth_mm:
            raise CalibrationError("Minimum depth must be below maximum depth")
        self.clock = clock

    @property
    def readiness(self) -> CalibrationReadiness:
        reasons = []
        if not self.calibration.ready:
            reasons.append("hand-eye calibration quality is not approved")
        if not callable(self.tool_pose_provider):
            reasons.append("tool pose provider is unavailable")
        if not callable(self.depth_provider):
            reasons.append("depth provider is unavailable")
        return CalibrationReadiness(not reasons, tuple(reasons))

    def is_ready(self) -> bool:
        return self.readiness.ready

    def to_workspace(self, track: Any) -> Tuple[float, float, float]:
        readiness = self.readiness
        if not readiness.ready:
            raise CalibrationNotReady("; ".join(readiness.reasons))
        if CameraRole(track.camera_role) != CameraRole.TOOL:
            raise CalibrationError(
                "Eye-in-hand mapping only accepts tool-camera tracks"
            )
        captured_at = float(track.captured_at_s)
        now = float(self.clock())
        age = now - captured_at
        if age < 0:
            raise CalibrationError("Tool-camera observation timestamp is in the future")
        if age > self.max_frame_age_s:
            raise CalibrationError("Tool-camera observation is stale")

        depth = self.depth_provider(track)
        if not isinstance(depth, DepthObservation):
            raise CalibrationError(
                "Depth provider must return a synchronized DepthObservation"
            )
        if abs(depth.timestamp_s - captured_at) > self.max_depth_skew_s:
            raise CalibrationError("Depth and tool-camera frame are not synchronized")
        if not self.min_depth_mm <= depth.depth_mm <= self.max_depth_mm:
            raise CalibrationError("Depth is outside configured sensor limits")

        tool_pose = self.tool_pose_provider(captured_at)
        if not isinstance(tool_pose, TimedToolPose):
            raise CalibrationError("Tool pose provider must return a TimedToolPose")
        if abs(tool_pose.timestamp_s - captured_at) > self.max_pose_skew_s:
            raise CalibrationError(
                "Robot pose and tool-camera frame are not synchronized"
            )

        camera_point = self.calibration.intrinsics.unproject(
            track.center_xy, depth.depth_mm
        )
        base_T_camera = tool_pose.base_T_tool.compose(self.calibration.tool_T_camera)
        base_point = base_T_camera.transform_point(camera_point)
        if not all(
            low <= value <= high
            for value, low, high in zip(base_point, self.lower, self.upper)
        ):
            raise CalibrationError(
                "Eye-in-hand point lies outside the configured robot workspace"
            )
        return base_point


def _solve_with_opencv(samples: Sequence[HandEyeSample]) -> RigidTransform:
    try:
        import cv2
    except ImportError as exc:  # pragma: no cover - deployment dependency
        raise CalibrationDependencyError(
            "OpenCV is required to solve eye-in-hand calibration"
        ) from exc

    rotations_gripper_to_base = [
        sample.base_T_tool.rotation.astype(np.float64) for sample in samples
    ]
    translations_gripper_to_base = [
        sample.base_T_tool.translation.reshape(3, 1).astype(np.float64)
        for sample in samples
    ]
    rotations_target_to_camera = [
        sample.camera_T_target.rotation.astype(np.float64) for sample in samples
    ]
    translations_target_to_camera = [
        sample.camera_T_target.translation.reshape(3, 1).astype(np.float64)
        for sample in samples
    ]
    try:
        rotation, translation = cv2.calibrateHandEye(
            rotations_gripper_to_base,
            translations_gripper_to_base,
            rotations_target_to_camera,
            translations_target_to_camera,
            method=cv2.CALIB_HAND_EYE_TSAI,
        )
    except Exception as exc:
        raise CalibrationError("OpenCV hand-eye solve failed") from exc
    return RigidTransform.from_rotation_translation(
        rotation, np.asarray(translation, dtype=float).reshape(3)
    )


def _diversity_report(
    samples: Sequence[HandEyeSample],
    policy: HandEyeQualityPolicy,
) -> HandEyeQualityReport:
    reasons = []
    if len(samples) < policy.min_samples:
        reasons.append(
            "need at least {} synchronized samples".format(policy.min_samples)
        )
    if any(sample.skew_s > policy.max_sample_skew_s for sample in samples):
        reasons.append("one or more samples exceed the synchronization limit")

    translations = [sample.base_T_tool.translation for sample in samples]
    translation_span = _maximum_pairwise_distance(translations)
    rotation_vectors = []
    rotation_span = 0.0
    for first_index, first in enumerate(samples):
        for second in samples[first_index + 1 :]:
            relative = first.base_T_tool.rotation.T.dot(second.base_T_tool.rotation)
            axis, angle = _rotation_axis_angle(relative)
            rotation_span = max(rotation_span, math.degrees(angle))
            if math.degrees(angle) >= 5.0:
                rotation_vectors.append(axis)
    axis_separation = _maximum_axis_separation_degrees(rotation_vectors)

    if translation_span < policy.min_translation_span_mm:
        reasons.append("robot translations do not span enough distance")
    if rotation_span < policy.min_rotation_span_deg:
        reasons.append("robot rotations do not span enough angle")
    if axis_separation < policy.min_rotation_axis_separation_deg:
        reasons.append("robot rotations do not cover sufficiently different axes")
    return HandEyeQualityReport(
        accepted=False,
        sample_count=len(samples),
        translation_span_mm=translation_span,
        rotation_span_deg=rotation_span,
        rotation_axis_separation_deg=axis_separation,
        reasons=tuple(reasons),
    )


def _residual_report(
    samples: Sequence[HandEyeSample],
    tool_T_camera: RigidTransform,
    policy: HandEyeQualityPolicy,
    diversity: HandEyeQualityReport,
) -> HandEyeQualityReport:
    base_targets = [
        sample.base_T_tool.compose(tool_T_camera).compose(sample.camera_T_target)
        for sample in samples
    ]
    translations = np.asarray(
        [target.translation for target in base_targets], dtype=float
    )
    translation_mean = np.mean(translations, axis=0)
    translation_errors = np.linalg.norm(translations - translation_mean, axis=1)
    mean_rotation = _mean_rotation([target.rotation for target in base_targets])
    rotation_errors = np.asarray(
        [
            math.degrees(_rotation_axis_angle(mean_rotation.T.dot(target.rotation))[1])
            for target in base_targets
        ],
        dtype=float,
    )
    translation_rms = float(np.sqrt(np.mean(np.square(translation_errors))))
    translation_max = float(np.max(translation_errors))
    rotation_rms = float(np.sqrt(np.mean(np.square(rotation_errors))))
    rotation_max = float(np.max(rotation_errors))
    reasons = list(diversity.reasons)
    if translation_rms > policy.max_translation_rms_mm:
        reasons.append("translation RMS residual exceeds quality limit")
    if translation_max > policy.max_translation_residual_mm:
        reasons.append("translation maximum residual exceeds quality limit")
    if rotation_rms > policy.max_rotation_rms_deg:
        reasons.append("rotation RMS residual exceeds quality limit")
    if rotation_max > policy.max_rotation_residual_deg:
        reasons.append("rotation maximum residual exceeds quality limit")
    return HandEyeQualityReport(
        accepted=not reasons,
        sample_count=diversity.sample_count,
        translation_span_mm=diversity.translation_span_mm,
        rotation_span_deg=diversity.rotation_span_deg,
        rotation_axis_separation_deg=diversity.rotation_axis_separation_deg,
        translation_rms_mm=translation_rms,
        translation_max_mm=translation_max,
        rotation_rms_deg=rotation_rms,
        rotation_max_deg=rotation_max,
        reasons=tuple(reasons),
    )


def _mean_rotation(rotations: Sequence[np.ndarray]) -> np.ndarray:
    summed = np.sum(np.asarray(rotations, dtype=float), axis=0)
    left, _singular, right = np.linalg.svd(summed)
    mean = left.dot(right)
    if np.linalg.det(mean) < 0:
        left[:, -1] *= -1
        mean = left.dot(right)
    return mean


def _rotation_axis_angle(rotation: np.ndarray) -> Tuple[np.ndarray, float]:
    cosine = min(1.0, max(-1.0, (float(np.trace(rotation)) - 1.0) / 2.0))
    angle = math.acos(cosine)
    if angle < 1e-9:
        return np.asarray((1.0, 0.0, 0.0)), 0.0
    if abs(math.pi - angle) < 1e-6:
        eigenvalues, eigenvectors = np.linalg.eig(rotation)
        index = int(np.argmin(np.abs(eigenvalues - 1.0)))
        axis = np.real(eigenvectors[:, index])
    else:
        axis = np.asarray(
            (
                rotation[2, 1] - rotation[1, 2],
                rotation[0, 2] - rotation[2, 0],
                rotation[1, 0] - rotation[0, 1],
            )
        ) / (2.0 * math.sin(angle))
    magnitude = float(np.linalg.norm(axis))
    if magnitude <= 1e-12:
        return np.asarray((1.0, 0.0, 0.0)), angle
    return axis / magnitude, angle


def _maximum_pairwise_distance(values: Sequence[np.ndarray]) -> float:
    maximum = 0.0
    for index, first in enumerate(values):
        for second in values[index + 1 :]:
            maximum = max(maximum, float(np.linalg.norm(first - second)))
    return maximum


def _maximum_axis_separation_degrees(axes: Sequence[np.ndarray]) -> float:
    maximum = 0.0
    for index, first in enumerate(axes):
        for second in axes[index + 1 :]:
            cosine = min(1.0, max(0.0, abs(float(np.dot(first, second)))))
            maximum = max(maximum, math.degrees(math.acos(cosine)))
    return maximum


def _xyz_tuple(values: Sequence[float], name: str) -> Tuple[float, float, float]:
    result = tuple(float(value) for value in values)
    if len(result) != 3 or not all(math.isfinite(value) for value in result):
        raise CalibrationError("{} must contain finite x, y, and z".format(name))
    return result


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


def _optional_float(value: Any) -> Optional[float]:
    if value is None:
        return None
    number = float(value)
    if not math.isfinite(number):
        raise CalibrationError("Calibration quality values must be finite")
    return number


def _optional_nonnegative_float(value: Any) -> Optional[float]:
    number = _optional_float(value)
    if number is not None and number < 0:
        raise CalibrationError("Calibration residuals must be non-negative")
    return number


def _fsync_directory(directory: Path) -> None:
    flags = getattr(os, "O_DIRECTORY", 0) | os.O_RDONLY
    descriptor = os.open(str(directory), flags)
    try:
        os.fsync(descriptor)
    finally:
        os.close(descriptor)
