"""
camera_controller.py — D435i parameter service client + IMX219 v4l2 control.

D435i (realsense2_camera node):
  depth_module.emitter_enabled  int  0=off  1=on  2=auto
  depth_module.emitter_always_on bool  keep emitter on between frames

IMX219 (CSI cameras via v4l2):
  exposure_auto     0=manual  1=auto
  exposure          microseconds (manual)
  analogue_gain     raw sensor gain (16–170 typical range)

The IMX219 controls are exercised via subprocess `v4l2-ctl`, which is
always available on JetPack 6.
"""

from __future__ import annotations

import subprocess
import logging
from dataclasses import dataclass
from typing import Optional

import rclpy
from rclpy.node import Node
from rcl_interfaces.msg import Parameter, ParameterValue, ParameterType
from rcl_interfaces.srv import SetParameters

_LOG = logging.getLogger(__name__)

# D435i camera node (realsense2_camera default namespace)
_RS_NODE = '/camera/camera'

# IMX219 v4l2 device paths for each camera position
# Front camera is the primary source for ambient-light detection
_IMX219_DEVICES: dict[str, str] = {
    'front': '/dev/video0',
    'right': '/dev/video2',
    'rear':  '/dev/video4',
    'left':  '/dev/video6',
}

# Auto-exposure values for IMX219
_IMX219_AE_AUTO   = 1
_IMX219_AE_MANUAL = 0


@dataclass
class CameraProfile:
    """Parameter set for a given vision mode."""
    emitter_enabled: int         # 0=off, 1=on, 2=auto
    imx_ae_mode: int             # 0=manual, 1=auto
    imx_exposure_us: Optional[int]  # None → leave AE to control it
    imx_gain: Optional[int]         # None → leave AE to control it


# Per-mode camera profiles
PROFILES: dict[str, CameraProfile] = {
    'day': CameraProfile(
        emitter_enabled=2,          # RS depth auto
        imx_ae_mode=_IMX219_AE_AUTO,
        imx_exposure_us=None,
        imx_gain=None,
    ),
    'twilight': CameraProfile(
        emitter_enabled=1,          # emitter on for better depth
        imx_ae_mode=_IMX219_AE_MANUAL,
        imx_exposure_us=20_000,     # 20 ms
        imx_gain=64,
    ),
    'night': CameraProfile(
        emitter_enabled=1,
        imx_ae_mode=_IMX219_AE_MANUAL,
        imx_exposure_us=50_000,     # 50 ms (sacrifice motion sharpness)
        imx_gain=128,
    ),
    'ir_only': CameraProfile(
        emitter_enabled=1,          # emitter critical for IR stereo in dark
        imx_ae_mode=_IMX219_AE_MANUAL,
        imx_exposure_us=80_000,
        imx_gain=170,               # near-max analogue gain
    ),
}


class CameraController:
    """
    Non-blocking camera controller.

    set_profile() sends async ROS2 parameter requests and v4l2 commands.
    It should be called from within a ROS2 node (pass the node reference).
    """

    def __init__(self, node: Node):
        self._node = node
        self._cli: Optional[rclpy.client.Client] = None
        self._pending_mode: Optional[str] = None
        self._current_mode: Optional[str] = None
        self._init_rs_client()

    # ── Public API ────────────────────────────────────────────────────────────

    def set_profile(self, mode: str) -> None:
        """Apply the camera profile for `mode`. Fire-and-forget async."""
        if mode not in PROFILES:
            self._node.get_logger().warning(f'[camera_ctrl] Unknown mode: {mode}')
            return
        if mode == self._current_mode:
            return
        profile = PROFILES[mode]
        self._set_rs_emitter(profile.emitter_enabled)
        self._set_imx219(profile)
        self._current_mode = mode
        self._node.get_logger().info(
            f'[camera_ctrl] Applied profile={mode} '
            f'emitter={profile.emitter_enabled} '
            f'gain={profile.imx_gain} exp={profile.imx_exposure_us}'
        )

    # ── D435i (RealSense) ─────────────────────────────────────────────────────

    def _init_rs_client(self) -> None:
        self._cli = self._node.create_client(
            SetParameters, f'{_RS_NODE}/set_parameters'
        )

    def _set_rs_emitter(self, value: int) -> None:
        if not self._cli.service_is_ready():
            self._node.get_logger().debug(
                '[camera_ctrl] RS parameter service not ready — skipping emitter set'
            )
            return
        pval = ParameterValue(type=ParameterType.PARAMETER_INTEGER, integer_value=value)
        param = Parameter(name='depth_module.emitter_enabled', value=pval)
        req = SetParameters.Request(parameters=[param])
        self._cli.call_async(req)  # fire-and-forget

    # ── IMX219 via v4l2-ctl ──────────────────────────────────────────────────

    def _set_imx219(self, profile: CameraProfile) -> None:
        for dev in _IMX219_DEVICES.values():
            self._v4l2_set(dev, 'exposure_auto', profile.imx_ae_mode)
            if profile.imx_exposure_us is not None:
                self._v4l2_set(dev, 'exposure', profile.imx_exposure_us)
            if profile.imx_gain is not None:
                self._v4l2_set(dev, 'analogue_gain', profile.imx_gain)

    def _v4l2_set(self, dev: str, ctrl: str, value: int) -> None:
        cmd = ['v4l2-ctl', '-d', dev, f'--set-ctrl={ctrl}={value}']
        try:
            result = subprocess.run(cmd, capture_output=True, timeout=1.0)
            if result.returncode != 0:
                self._node.get_logger().debug(
                    f'[camera_ctrl] v4l2 {dev} {ctrl}={value}: '
                    + result.stderr.decode().strip()
                )
        except (subprocess.TimeoutExpired, FileNotFoundError) as exc:
            self._node.get_logger().debug(f'[camera_ctrl] v4l2-ctl unavailable: {exc}')
