# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

"""
Provides the RealSenseCamera class for capturing frames from Intel RealSense cameras.
"""

import logging
import sys
import time
from threading import Event, Lock, Thread
from typing import TYPE_CHECKING, Any

import cv2
import numpy as np
from numpy.typing import NDArray

from lerobot.utils.import_utils import _pyrealsense2_available, require_package

if TYPE_CHECKING or _pyrealsense2_available:
    import pyrealsense2 as rs
else:
    rs = None

from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from lerobot.utils.errors import DeviceNotConnectedError

from ..camera import Camera
from ..configs import ColorMode
from ..utils import get_cv2_rotation
from .configuration_realsense import RealSenseCameraConfig

logger = logging.getLogger(__name__)
pkg_name = "pyrealsense2-macosx" if sys.platform == "darwin" else "pyrealsense2"


class RealSenseCamera(Camera):
    """
    Manages interactions with Intel RealSense cameras for frame and depth recording.

    This class provides an interface similar to `OpenCVCamera` but tailored for
    RealSense devices, leveraging the `pyrealsense2` library. It uses the camera's
    unique serial number for identification, offering more stability than device
    indices, especially on Linux. It also supports capturing depth maps alongside
    color frames.

    Use the provided utility script to find available camera indices and default profiles:
    ```bash
    lerobot-find-cameras realsense
    ```

    A `RealSenseCamera` instance requires a configuration object specifying the
    camera's serial number or a unique device name. If using the name, ensure only
    one camera with that name is connected.

    The camera's default settings (FPS, resolution, color mode) from the stream
    profile are used unless overridden in the configuration.

    Example:
        ```python
        from lerobot.cameras.realsense import RealSenseCamera, RealSenseCameraConfig
        from lerobot.cameras import ColorMode, Cv2Rotation

        # Basic usage with serial number
        config = RealSenseCameraConfig(serial_number_or_name="0123456789") # Replace with actual SN
        camera = RealSenseCamera(config)
        camera.connect()

        # Read 1 frame synchronously (blocking)
        color_image = camera.read()

        # Read 1 frame asynchronously (waits for new frame with a timeout)
        async_image = camera.async_read()

        # Get the latest frame immediately (no wait, returns timestamp)
        latest_image, timestamp = camera.read_latest()

        # Example with depth capture and custom settings
        custom_config = RealSenseCameraConfig(
            serial_number_or_name="0123456789", # Replace with actual SN
            fps=30,
            width=1280,
            height=720,
            color_mode=ColorMode.BGR, # Request BGR output
            rotation=Cv2Rotation.NO_ROTATION,
            use_depth=True
        )
        depth_camera = RealSenseCamera(custom_config)
        depth_camera.connect()

        # Read 1 depth frame
        depth_map = depth_camera.read_depth()

        # Example using a unique camera name
        name_config = RealSenseCameraConfig(serial_number_or_name="Intel RealSense D435") # If unique
        name_camera = RealSenseCamera(name_config)
        # ... connect, read, disconnect ...
        ```
    """

    # Maximum number of warmup attempts made by connect(). A failed attempt is first
    # retried with a plain pipeline stop/start, which is usually enough to recover the
    # stream; a USB hardware reset is performed before the final attempt as a last resort.
    _MAX_CONNECT_ATTEMPTS = 3

    def __init__(self, config: RealSenseCameraConfig):
        """
        Initializes the RealSenseCamera instance.

        Args:
            config: The configuration settings for the camera.
        """
        require_package(pkg_name, extra="intelrealsense", import_name="pyrealsense2")
        super().__init__(config)

        self.config = config

        self.width: int | None = config.width
        self.height: int | None = config.height

        if config.serial_number_or_name.isdigit():
            self.serial_number = config.serial_number_or_name
        else:
            self.serial_number = self._find_serial_number_from_name(config.serial_number_or_name)

        self.fps = config.fps
        self.color_mode = config.color_mode
        self.use_rgb = config.use_rgb
        self.use_depth = config.use_depth
        self.warmup_s = config.warmup_s
        self.exposure: int | None = config.exposure
        self.gain: int | None = config.gain
        self.white_balance: int | None = config.white_balance

        self.rs_pipeline: rs.pipeline | None = None
        self.rs_profile: rs.pipeline_profile | None = None

        self.thread: Thread | None = None
        self.stop_event: Event | None = None
        self.frame_lock: Lock = Lock()
        self.latest_color_frame: NDArray[Any] | None = None
        self.latest_depth_frame: NDArray[Any] | None = None
        self.latest_timestamp: float | None = None
        self.new_frame_event: Event = Event()

        self.rotation: int | None = get_cv2_rotation(config.rotation)

        self.capture_width: int | None = None
        self.capture_height: int | None = None
        self._reset_connection_settings()

    def __str__(self) -> str:
        return f"{self.__class__.__name__}({self.serial_number})"

    def _reset_connection_settings(self) -> None:
        """Restore settings that may have been auto-detected during a failed connection."""
        self.fps = self.config.fps
        self.width = self.config.width
        self.height = self.config.height
        self.warmup_s = self.config.warmup_s
        self.capture_width, self.capture_height = self.width, self.height
        if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
            self.capture_width, self.capture_height = self.height, self.width

    @property
    def is_connected(self) -> bool:
        """Checks if the camera pipeline is started and streams are active."""
        return self.rs_pipeline is not None and self.rs_profile is not None

    def _hardware_reset(self, wait_s: float = 5.0) -> None:
        """Issue a USB hardware reset to recover an unresponsive device (common on D405)."""
        context = rs.context()
        for device in context.query_devices():
            if device.get_info(rs.camera_info.serial_number) == self.serial_number:
                logger.info(f"{self} performing hardware reset.")
                device.hardware_reset()
                time.sleep(wait_s)
                return
        logger.warning(f"{self} device not found for hardware reset, skipping.")

    def _open_pipeline(self) -> None:
        """Initializes the RealSense pipeline, starts it, and starts the background read thread.

        Raises:
            ValueError: If the configuration is invalid, a requested sensor option is unsupported,
                or a requested sensor value is invalid.
            ConnectionError: If the camera is found but fails to start the pipeline or no RealSense devices are detected at all.
            RuntimeError: If the pipeline starts but fails to apply requested settings.
        """
        rs_pipeline = rs.pipeline()
        rs_config = rs.config()
        self._configure_rs_pipeline_config(rs_config)

        try:
            rs_profile = rs_pipeline.start(rs_config)
        except RuntimeError as e:
            raise ConnectionError(
                f"Failed to open {self}.Run `lerobot-find-cameras realsense` to find available cameras."
            ) from e

        self.rs_pipeline = rs_pipeline
        self.rs_profile = rs_profile

        try:
            self._configure_capture_settings()
            self._configure_sensor_options()
            self._start_read_thread()
        except BaseException:
            self._release_after_failed_setup()
            raise

    def _run_warmup(self) -> None:
        """Blocks until at least one valid frame has been captured by the background thread.

        Raises:
            ConnectionError: If no frame arrives before ``warmup_s`` elapses.
        """
        # NOTE(Steven/Caroline): Enforcing at least one second of warmup as RS cameras need a bit of time before the first read. If we don't wait, the first read from the warmup will raise.
        self.warmup_s = max(self.warmup_s, 1)

        warmup_read = self.async_read if self.use_rgb else self.async_read_depth
        start_time = time.time()
        while time.time() - start_time < self.warmup_s:
            warmup_read(timeout_ms=self.warmup_s * 1000)
            time.sleep(0.1)
        with self.frame_lock:
            if (self.use_rgb and self.latest_color_frame is None) or (
                self.use_depth and self.latest_depth_frame is None
            ):
                raise ConnectionError(f"{self} failed to capture frames during warmup.")

    def _release_after_failed_setup(self) -> None:
        """Releases the device handle and restores auto-detected settings after a failed attempt."""
        try:
            self._cleanup_resources()
        except Exception:
            logger.exception(f"Failed to fully clean up {self} after connect() failed.")
        self._reset_connection_settings()

    @check_if_already_connected
    def connect(self, warmup: bool = True) -> None:
        """
        Connects to the RealSense camera specified in the configuration.

        Initializes the RealSense pipeline, configures the required streams (color
        and optionally depth), starts the pipeline, and validates the actual stream settings.

        If the pipeline starts but no frames arrive during warmup, retries up to
        ``_MAX_CONNECT_ATTEMPTS`` times, performing a USB hardware reset before the
        final attempt.

        Args:
            warmup (bool): If True, waits at connect() time until at least one valid frame
                           has been captured by the background thread. Defaults to True.

        Raises:
            DeviceAlreadyConnectedError: If the camera is already connected.
            ValueError: If the configuration is invalid (e.g., missing serial/name, name not unique).
            ConnectionError: If the camera is found but fails to start the pipeline or no RealSense devices are detected at all.
            RuntimeError: If the pipeline starts but fails to apply requested settings.
        """

        if not warmup:
            self._open_pipeline()
            logger.info(f"{self} connected.")
            return

        last_error: Exception | None = None

        for attempt in range(1, self._MAX_CONNECT_ATTEMPTS + 1):
            if attempt == self._MAX_CONNECT_ATTEMPTS:
                self._hardware_reset()

            self._open_pipeline()

            connected = False
            try:
                self._run_warmup()
                connected = True
            except (TimeoutError, ConnectionError) as e:
                last_error = e
            finally:
                if not connected:
                    self._release_after_failed_setup()

            if connected:
                logger.info(f"{self} connected.")
                return

            logger.warning(f"{self} warmup failed (attempt {attempt}/{self._MAX_CONNECT_ATTEMPTS}).")

        raise ConnectionError(
            f"{self} failed to capture frames after {self._MAX_CONNECT_ATTEMPTS} attempts."
        ) from last_error

    @staticmethod
    def find_cameras() -> list[dict[str, Any]]:
        """
        Detects available Intel RealSense cameras connected to the system.

        Returns:
            List[Dict[str, Any]]: A list of dictionaries,
            where each dictionary contains 'type', 'id' (serial number), 'name',
            firmware version, USB type, and other available specs, and the default profile properties (width, height, fps, format).

        Raises:
            OSError: If pyrealsense2 is not installed.
            ImportError: If pyrealsense2 is not installed.
        """
        found_cameras_info = []
        context = rs.context()
        devices = context.query_devices()

        for device in devices:
            camera_info = {
                "name": device.get_info(rs.camera_info.name),
                "type": "RealSense",
                "id": device.get_info(rs.camera_info.serial_number),
                "firmware_version": device.get_info(rs.camera_info.firmware_version),
                "usb_type_descriptor": device.get_info(rs.camera_info.usb_type_descriptor),
                "physical_port": device.get_info(rs.camera_info.physical_port),
                "product_id": device.get_info(rs.camera_info.product_id),
                "product_line": device.get_info(rs.camera_info.product_line),
            }

            # Get stream profiles for each sensor
            sensors = device.query_sensors()
            for sensor in sensors:
                profiles = sensor.get_stream_profiles()

                for profile in profiles:
                    if profile.is_video_stream_profile() and profile.is_default():
                        vprofile = profile.as_video_stream_profile()
                        stream_info = {
                            "stream_type": vprofile.stream_name(),
                            "format": vprofile.format().name,
                            "width": vprofile.width(),
                            "height": vprofile.height(),
                            "fps": vprofile.fps(),
                        }
                        camera_info["default_stream_profile"] = stream_info

            found_cameras_info.append(camera_info)

        return found_cameras_info

    def _find_serial_number_from_name(self, name: str) -> str:
        """Finds the serial number for a given unique camera name."""
        camera_infos = self.find_cameras()
        found_devices = [cam for cam in camera_infos if str(cam["name"]) == name]

        if not found_devices:
            available_names = [cam["name"] for cam in camera_infos]
            raise ValueError(
                f"No RealSense camera found with name '{name}'. Available camera names: {available_names}"
            )

        if len(found_devices) > 1:
            serial_numbers = [dev["id"] for dev in found_devices]
            raise ValueError(
                f"Multiple RealSense cameras found with name '{name}'. "
                f"Please use a unique serial number instead. Found SNs: {serial_numbers}"
            )

        serial_number = str(found_devices[0]["id"])
        return serial_number

    def _configure_rs_pipeline_config(self, rs_config: Any) -> None:
        """Creates and configures the RealSense pipeline configuration object."""
        rs.config.enable_device(rs_config, self.serial_number)

        if self.width and self.height and self.fps:
            if self.use_rgb:
                rs_config.enable_stream(
                    rs.stream.color, self.capture_width, self.capture_height, rs.format.rgb8, self.fps
                )
            if self.use_depth:
                rs_config.enable_stream(
                    rs.stream.depth, self.capture_width, self.capture_height, rs.format.z16, self.fps
                )
        else:
            if self.use_rgb:
                rs_config.enable_stream(rs.stream.color)
            if self.use_depth:
                rs_config.enable_stream(rs.stream.depth)

    @check_if_not_connected
    def _configure_capture_settings(self) -> None:
        """Sets fps, width, and height from device stream if not already configured.

        Uses the color stream profile (or the depth stream profile when the color
        stream is disabled) to update unset attributes. Handles rotation by swapping
        width/height when needed. Original capture dimensions are always stored.

        Raises:
            DeviceNotConnectedError: If device is not connected.
        """

        if self.rs_profile is None:
            raise RuntimeError(f"{self}: rs_profile must be initialized before use.")

        rs_stream = rs.stream.color if self.use_rgb else rs.stream.depth
        stream = self.rs_profile.get_stream(rs_stream).as_video_stream_profile()

        if self.fps is None:
            self.fps = stream.fps()

        if self.width is None or self.height is None:
            actual_width = int(round(stream.width()))
            actual_height = int(round(stream.height()))
            if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
                self.width, self.height = actual_height, actual_width
                self.capture_width, self.capture_height = actual_width, actual_height
            else:
                self.width, self.height = actual_width, actual_height
                self.capture_width, self.capture_height = actual_width, actual_height

    def _read(self, read_depth: bool = False) -> NDArray[Any]:
        """Shared helper for :meth:`read`/:meth:`read_depth`: wait for a fresh color or depth frame."""
        if self.thread is None or not self.thread.is_alive():
            raise RuntimeError(f"{self} read thread is not running.")

        self.new_frame_event.clear()
        return self._async_read(timeout_ms=10000, read_depth=read_depth)

    def _get_color_sensor(self) -> "rs.sensor":
        """Returns the dedicated "RGB Camera" sensor that controls the color stream.

        Manual color controls are only applied to a dedicated RGB module. Cameras
        without one (e.g. the D405, whose color stream comes from the shared
        "Stereo Module") are unsupported, so we never fall back to another sensor
        to avoid altering the depth stream.
        """
        if self.rs_profile is None:
            raise RuntimeError(f"{self}: rs_profile must be initialized before use.")

        device = self.rs_profile.get_device()
        sensors = {s.get_info(rs.camera_info.name): s for s in device.query_sensors()}

        if "RGB Camera" in sensors:
            return sensors["RGB Camera"]

        available = list(sensors.keys())
        raise RuntimeError(
            f"{self}: manual color controls require a dedicated 'RGB Camera' module, which this camera does not have. ",
            f"Available sensors: {available}.",
        )

    def _set_sensor_option(self, sensor: "rs.sensor", option: "rs.option", value: float, label: str) -> None:
        """Sets a sensor option, re-raising range errors with actionable diagnostics."""
        try:
            sensor.set_option(option, value)
        except Exception as e:
            range_info = ""
            try:
                option_range = sensor.get_option_range(option)
                range_info = (
                    f" (supported range: min={option_range.min}, max={option_range.max}, "
                    f"step={option_range.step}, default={option_range.default})"
                )
            except Exception:
                range_info = " (option range unavailable)"
            raise ValueError(
                f"{self}: failed to set {label} to {value}{range_info}. Original error: {e}"
            ) from e

    def _configure_sensor_options(self) -> None:
        """Applies manual sensor options (exposure, gain, white balance) to the color sensor.

        When exposure or gain is set, auto-exposure is disabled first. When white_balance
        is set, auto white balance is disabled first. An omitted option is left unchanged,
        and configuration is skipped entirely if all options are omitted.

        Raises:
            ValueError: If the sensor does not support a requested option or a requested
                value is invalid. Invalid-value errors include the option name, requested
                value, and supported range when available.
        """
        if self.exposure is None and self.gain is None and self.white_balance is None:
            return

        color_sensor = self._get_color_sensor()

        requested_options = (
            (rs.option.exposure, self.exposure, "exposure"),
            (rs.option.gain, self.gain, "gain"),
            (rs.option.white_balance, self.white_balance, "white balance"),
        )
        unsupported_options = [
            label
            for option, value, label in requested_options
            if value is not None and not color_sensor.supports(option)
        ]
        if unsupported_options:
            raise ValueError(
                f"{self}: color sensor does not support requested manual options: {unsupported_options}."
            )

        manual_exposure_requested = self.exposure is not None or self.gain is not None
        if manual_exposure_requested:
            if color_sensor.supports(rs.option.enable_auto_exposure):
                self._set_sensor_option(color_sensor, rs.option.enable_auto_exposure, 0, "auto-exposure")
                logger.info(f"{self} auto-exposure disabled.")
            else:
                logger.warning(
                    f"{self} sensor does not support disabling auto-exposure; "
                    "applying manual exposure/gain directly."
                )

        if self.exposure is not None:
            self._set_sensor_option(color_sensor, rs.option.exposure, self.exposure, "exposure")
            logger.info(f"{self} exposure set to {self.exposure}.")

        if self.gain is not None:
            self._set_sensor_option(color_sensor, rs.option.gain, self.gain, "gain")
            logger.info(f"{self} gain set to {self.gain}.")

        if self.white_balance is not None:
            if color_sensor.supports(rs.option.enable_auto_white_balance):
                self._set_sensor_option(
                    color_sensor, rs.option.enable_auto_white_balance, 0, "auto white balance"
                )
                logger.info(f"{self} auto white balance disabled.")
            else:
                logger.warning(
                    f"{self} sensor does not support disabling auto white balance; "
                    "applying manual white balance directly."
                )
            self._set_sensor_option(
                color_sensor, rs.option.white_balance, self.white_balance, "white balance"
            )
            logger.info(f"{self} white balance set to {self.white_balance}.")

    @check_if_not_connected
    def read_depth(self, timeout_ms: int = 200) -> NDArray[Any]:
        """
        Reads a single frame (depth) synchronously from the camera.

        This is a blocking call. It waits for a coherent set of frames (depth)
        from the camera hardware via the RealSense pipeline.

        Returns:
            np.ndarray: The depth map as a NumPy array (height, width, 1)
                  of type `np.uint16` (raw depth values in millimeters).

        Raises:
            DeviceNotConnectedError: If the camera is not connected.
            RuntimeError: If reading frames from the pipeline fails or frames are invalid.
        """
        if timeout_ms:
            logger.warning(
                f"{self} read() timeout_ms parameter is deprecated and will be removed in future versions."
            )

        if not self.use_depth:
            raise RuntimeError(
                f"Failed to capture depth frame '.read_depth()'. Depth stream is not enabled for {self}."
            )

        return self._read(read_depth=True)

    def _read_from_hardware(self):
        if self.rs_pipeline is None:
            raise RuntimeError(f"{self}: rs_pipeline must be initialized before use.")

        ret, frame = self.rs_pipeline.try_wait_for_frames(timeout_ms=10000)

        if not ret or frame is None:
            raise RuntimeError(f"{self} read failed (status={ret}).")

        return frame

    @check_if_not_connected
    def read(self, color_mode: ColorMode | None = None, timeout_ms: int = 0) -> NDArray[Any]:
        """
        Reads a single frame (color) synchronously from the camera.

        This is a blocking call. It waits for a coherent set of frames (color)
        from the camera hardware via the RealSense pipeline.

        Returns:
            np.ndarray: The captured color frame as a NumPy array
              (height, width, channels), processed according to `color_mode` and rotation.

        Raises:
            DeviceNotConnectedError: If the camera is not connected.
            RuntimeError: If reading frames from the pipeline fails or frames are invalid.
            ValueError: If an invalid `color_mode` is requested.
        """

        start_time = time.perf_counter()

        if color_mode is not None:
            logger.warning(
                f"{self} read() color_mode parameter is deprecated and will be removed in future versions."
            )

        if timeout_ms:
            logger.warning(
                f"{self} read() timeout_ms parameter is deprecated and will be removed in future versions."
            )

        if not self.use_rgb:
            raise RuntimeError(f"{self}: cannot read color — camera was configured with use_rgb=False.")

        frame = self._read()

        read_duration_ms = (time.perf_counter() - start_time) * 1e3
        logger.debug(f"{self} read took: {read_duration_ms:.1f}ms")

        return frame

    def _postprocess_image(self, image: NDArray[Any], depth_frame: bool = False) -> NDArray[Any]:
        """
        Applies color conversion, dimension validation, and rotation to a raw color frame.

        Args:
            image (np.ndarray): The raw image frame (expected RGB format from RealSense).

        Returns:
            np.ndarray: The processed image frame according to `self.color_mode` and `self.rotation`.

        Raises:
            ValueError: If the requested `color_mode` is invalid.
            RuntimeError: If the raw frame dimensions do not match the configured
                          `width` and `height`.
        """

        if self.color_mode and self.color_mode not in (ColorMode.RGB, ColorMode.BGR):
            raise ValueError(
                f"Invalid requested color mode '{self.color_mode}'. Expected {ColorMode.RGB} or {ColorMode.BGR}."
            )

        if depth_frame:
            h, w = image.shape
        else:
            h, w, c = image.shape

            if c != 3:
                raise RuntimeError(f"{self} frame channels={c} do not match expected 3 channels (RGB/BGR).")

        if h != self.capture_height or w != self.capture_width:
            raise RuntimeError(
                f"{self} frame width={w} or height={h} do not match configured width={self.capture_width} or height={self.capture_height}."
            )

        processed_image = image
        if not depth_frame and self.color_mode == ColorMode.BGR:
            processed_image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)

        if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE, cv2.ROTATE_180]:
            processed_image = cv2.rotate(processed_image, self.rotation)

        return processed_image

    def _read_loop(self) -> None:
        """
        Internal loop run by the background thread for asynchronous reading.

        On each iteration:
        1. Reads a color/depth frame (blocking call with 10s timeout)
        2. Stores result in latest_color_frame/latest_depth_frame and updates timestamp (thread-safe)
        3. Sets new_frame_event to notify listeners

        Stops on DeviceNotConnectedError, logs other errors and continues.
        """
        stop_event = self.stop_event
        if stop_event is None:
            raise RuntimeError(f"{self}: stop_event is not initialized before starting read loop.")

        failure_count = 0
        while not stop_event.is_set():
            try:
                frame = self._read_from_hardware()

                if self.use_rgb:
                    color_frame_raw = frame.get_color_frame()
                    color_frame = np.asanyarray(color_frame_raw.get_data())
                    processed_color_frame = self._postprocess_image(color_frame)

                if self.use_depth:
                    depth_frame_raw = frame.get_depth_frame()
                    depth_frame = np.asanyarray(depth_frame_raw.get_data())
                    processed_depth_frame = self._postprocess_image(depth_frame, depth_frame=True)
                    if processed_depth_frame.ndim == 2:  # (H, W) -> (H, W, 1)
                        processed_depth_frame = processed_depth_frame[..., np.newaxis]

                capture_time = time.perf_counter()

                with self.frame_lock:
                    # Under the lock, so a late frame cannot resurrect the buffer _stop_read_thread() cleared.
                    if stop_event.is_set():
                        break
                    if self.use_rgb:
                        self.latest_color_frame = processed_color_frame
                    if self.use_depth:
                        self.latest_depth_frame = processed_depth_frame
                    self.latest_timestamp = capture_time
                self.new_frame_event.set()
                failure_count = 0

            except DeviceNotConnectedError:
                break
            except Exception as e:
                if failure_count <= 10:
                    failure_count += 1
                    logger.warning(f"Error reading frame in background thread for {self}: {e}")
                else:
                    raise RuntimeError(f"{self} exceeded maximum consecutive read failures.") from e

    def _start_read_thread(self) -> None:
        """Starts or restarts the background read thread if it's not running."""
        self._stop_read_thread()

        self.stop_event = Event()
        self.thread = Thread(target=self._read_loop, args=(), name=f"{self}_read_loop")
        self.thread.daemon = True
        self.thread.start()

    def _stop_read_thread(self) -> None:
        """Signals the background read thread to stop and waits for it to join."""
        if self.stop_event is not None:
            self.stop_event.set()

        if self.thread is not None and self.thread.is_alive():
            self.thread.join(timeout=2.0)
            if self.thread.is_alive():  # pragma: no cover
                logger.warning(f"{self} read thread did not terminate within timeout.")

        self.thread = None
        self.stop_event = None

        with self.frame_lock:
            self.latest_color_frame = None
            self.latest_depth_frame = None
            self.latest_timestamp = None
            self.new_frame_event.clear()

    def _cleanup_resources(self) -> None:
        """Stop background reads and stop the pipeline, including after partial setup."""
        read_thread = self.thread
        rs_pipeline = self.rs_pipeline

        try:
            self._stop_read_thread()
        finally:
            self.rs_pipeline = None
            self.rs_profile = None
            try:
                if rs_pipeline is not None:
                    rs_pipeline.stop()
            finally:
                # Stopping the pipeline may unblock a hardware read that outlived
                # the first bounded join in _stop_read_thread().
                if read_thread is not None and read_thread.is_alive():
                    read_thread.join(timeout=2.0)
                    if read_thread.is_alive():  # pragma: no cover
                        logger.warning(f"{self} read thread remained alive after stopping the pipeline.")

    def _async_read(self, timeout_ms: float, read_depth: bool = False) -> NDArray[Any]:
        """Shared helper for :meth:`async_read`/:meth:`async_read_depth`: return the latest buffered frame."""
        if self.thread is None or not self.thread.is_alive():
            raise RuntimeError(f"{self} read thread is not running.")

        if not self.new_frame_event.wait(timeout=timeout_ms / 1000.0):
            raise TimeoutError(
                f"Timed out waiting for frame from camera {self} after {timeout_ms} ms. "
                f"Read thread alive: {self.thread.is_alive()}."
            )

        with self.frame_lock:
            frame = self.latest_depth_frame if read_depth else self.latest_color_frame
            self.new_frame_event.clear()

        if frame is None:
            raise RuntimeError(f"Internal error: Event set but no frame available for {self}.")

        return frame

    @check_if_not_connected
    def async_read(self, timeout_ms: float = 200) -> NDArray[Any]:
        """
        Reads the latest available frame data (color) asynchronously.

        This method retrieves the most recent color frame captured by the background
        read thread. It does not block waiting for the camera hardware directly,
        but may wait up to timeout_ms for the background thread to provide a frame.
        It is “best effort” under high FPS.

        Args:
            timeout_ms (float): Maximum time in milliseconds to wait for a frame
                to become available. Defaults to 200ms (0.2 seconds).

        Returns:
            np.ndarray:
            The latest captured frame data (color image), processed according to configuration.

        Raises:
            DeviceNotConnectedError: If the camera is not connected.
            TimeoutError: If no frame data becomes available within the specified timeout.
            RuntimeError: If the background thread died unexpectedly or another error occurs.
        """

        if not self.use_rgb:
            raise RuntimeError(f"{self}: cannot read color — camera was configured with use_rgb=False.")

        return self._async_read(timeout_ms=timeout_ms)

    def _read_latest(self, max_age_ms: int, read_depth: bool = False) -> NDArray[Any]:
        """Shared helper for :meth:`read_latest`/:meth:`read_latest_depth`: peek the latest buffered frame."""
        if self.thread is None or not self.thread.is_alive():
            raise RuntimeError(f"{self} read thread is not running.")

        with self.frame_lock:
            frame = self.latest_depth_frame if read_depth else self.latest_color_frame
            timestamp = self.latest_timestamp

        if frame is None or timestamp is None:
            raise RuntimeError(f"{self} has not captured any frames yet.")

        age_ms = (time.perf_counter() - timestamp) * 1e3
        if age_ms > max_age_ms:
            raise TimeoutError(
                f"{self} latest frame is too old: {age_ms:.1f} ms (max allowed: {max_age_ms} ms)."
            )

        return frame

    @check_if_not_connected
    def read_latest(self, max_age_ms: int = 500) -> NDArray[Any]:
        """Return the most recent (color) frame captured immediately (Peeking).

        This method is non-blocking and returns whatever is currently in the
        memory buffer. The frame may be stale,
        meaning it could have been captured a while ago (hanging camera scenario e.g.).

        Returns:
            NDArray[Any]: The frame image (numpy array).

        Raises:
            TimeoutError: If the latest frame is older than `max_age_ms`.
            DeviceNotConnectedError: If the camera is not connected.
            RuntimeError: If the camera is connected but has not captured any frames yet.
        """
        if not self.use_rgb:
            raise RuntimeError(f"{self}: cannot read color — camera was configured with use_rgb=False.")

        return self._read_latest(max_age_ms=max_age_ms)

    @check_if_not_connected
    def async_read_depth(self, timeout_ms: float = 200) -> NDArray[np.uint16]:
        """Read the latest depth frame asynchronously, in millimeters.

        Mirrors :meth:`async_read` but returns the depth stream rather than the
        color stream. Output is ``np.uint16`` of shape ``(H, W, 1)``, where each
        pixel is the distance from the sensor in millimeters.

        Raises:
            DeviceNotConnectedError: If the camera is not connected.
            RuntimeError: If ``use_depth`` is ``False`` for this camera, or if
                the background read thread is not running.
            TimeoutError: If no frame becomes available within ``timeout_ms``.
        """
        if not self.use_depth:
            raise RuntimeError(f"{self}: cannot read depth — camera was configured with use_depth=False.")

        return self._async_read(timeout_ms=timeout_ms, read_depth=True)

    @check_if_not_connected
    def read_latest_depth(self, max_age_ms: int = 500) -> NDArray[Any]:
        """Return the most recent depth frame in millimeters (peeking).

        Non-blocking counterpart of :meth:`read_latest` for the depth stream.
        Output is ``np.uint16`` of shape ``(H, W, 1)``, where each pixel is the
        distance from the sensor in millimeters.

        Raises:
            DeviceNotConnectedError: If the camera is not connected.
            RuntimeError: If ``use_depth`` is ``False`` for this camera, or if
                no depth frame has been captured yet.
            TimeoutError: If the latest depth frame is older than ``max_age_ms``.
        """
        if not self.use_depth:
            raise RuntimeError(f"{self}: cannot read depth — camera was configured with use_depth=False.")

        return self._read_latest(max_age_ms=max_age_ms, read_depth=True)

    def disconnect(self) -> None:
        """
        Disconnects from the camera, stops the pipeline, and cleans up resources.

        Stops the background read thread (if running) and stops the RealSense pipeline.

        Raises:
            DeviceNotConnectedError: If the camera is already disconnected (pipeline not running).
        """

        if not self.is_connected and self.thread is None:
            raise DeviceNotConnectedError(
                f"Attempted to disconnect {self}, but it appears already disconnected."
            )

        self._cleanup_resources()

        logger.info(f"{self} disconnected.")
