agrobot_base/Python/raspi/cam_3/pi/frame_service.py

import time
import threading
import base64
import numpy as np


class FrameService:
    def __init__(self, state, trigger_manager, camera_manager):
        self.state = state
        self.trigger = trigger_manager
        self.camera = camera_manager
        self._capture_lock = threading.RLock()
        self.raw_processors = {}

    # =========================================================
    # Helpers
    # =========================================================

    def _get_camera_spec(self, cam_id):
        cam = self.state.get_camera(cam_id)
        if cam is None:
            raise RuntimeError(f"Câmera '{cam_id}' não encontrada no state")
        return cam

    def _ensure_raw_processor_for_camera(self, cam_id):
        cam = self._get_camera_spec(cam_id)
        rp = self.raw_processors.get(cam_id)

        if (
            rp is None or
            rp.sensor_width != cam.width or
            rp.sensor_height != cam.height or
            rp.bayer_pattern.upper() != cam.bayer_pattern.upper()
        ):
            from raw_processor_core import RawProcessorCore
            rp = RawProcessorCore(
                sensor_width=cam.width,
                sensor_height=cam.height,
                bayer_pattern=cam.bayer_pattern,
            )
            self.raw_processors[cam_id] = rp

        return rp

    def _capture_with_retry(self, required_sources, previous_ids=None, max_attempts=10, retry_delay_s=0.02):
        last_frames = None

        for _ in range(max_attempts):
            frames = self.camera.capture_raw_frames()

            ok = True
            for cam_id in required_sources:
                info = frames.get(cam_id)
                if info is None:
                    ok = False
                    break

                frame, width, height, channels, frame_id, frame_ts, last_read_ts = info
                if frame is None or width <= 0 or height <= 0 or channels <= 0:
                    ok = False
                    break
                if previous_ids is not None:
                    prev_id = previous_ids.get(cam_id)
                    if prev_id is not None and frame_id <= prev_id:
                        ok = False
                        break

            if ok:
                return frames

            last_frames = frames
            time.sleep(retry_delay_s)

        raise RuntimeError(
            f"Capture não atingiu frames válidos/novos após {max_attempts} tentativas... "
            f"required_sources={required_sources}, received_sources={list((last_frames or {}).keys())}"
        )

    def _is_usb_rgb_camera(self, cam):
        return getattr(cam, "interface", "").upper() == "USB" and cam.role == "rgb"

    def _is_csi_multispec_camera(self, cam):
        return getattr(cam, "interface", "").upper() == "CSI" and cam.role in ("re", "nir")

    def _normalize_raw16_to_float(self, raw16: np.ndarray, bit_depth: int) -> np.ndarray:
        max_val = float((1 << int(bit_depth)) - 1)
        arr = raw16.astype(np.float32) / max_val
        return np.clip(arr, 0.0, 1.0)

    def _convert_usb_bgr_to_rgb_chw_float(self, frame_bgr: np.ndarray) -> np.ndarray:
        if frame_bgr.ndim != 3 or frame_bgr.shape[2] != 3:
            raise RuntimeError(f"Frame RGB USB inválido para conversão BGR->RGB: shape={frame_bgr.shape}")

        rgb = frame_bgr[:, :, ::-1]  # BGR -> RGB
        rgb = rgb.astype(np.float32) / 255.0
        chw = np.transpose(rgb, (2, 0, 1))
        return np.clip(chw, 0.0, 1.0)

    def _convert_output_dtype(self, arr, bit_depth=None):
        """
        Converte arrays float32 normalizados [0..1], uint8, uint16 ou inteiros crus
        para self.state.output_dtype.
        """
        if bit_depth is None:
            bit_depth = 10

        max_sensor_value = float((1 << int(bit_depth)) - 1)

        if self.state.output_dtype == "uint8":
            if arr.dtype == np.uint8:
                return arr

            if arr.dtype == np.float32:
                return (arr * 255.0).clip(0, 255).astype(np.uint8)

            if arr.dtype.kind in ("u", "i"):
                max_val = arr.max() if arr.size > 0 else 0
                if max_val <= 255:
                    return arr.astype(np.uint8)
                return ((arr.astype(np.float32) / max_sensor_value) * 255.0).clip(0, 255).astype(np.uint8)

            raise RuntimeError(f"dtype não suportado para uint8: {arr.dtype}")

        if self.state.output_dtype == "uint16":
            if arr.dtype == np.uint16:
                return arr

            if arr.dtype == np.uint8:
                return (arr.astype(np.uint16) << 8)

            if arr.dtype == np.float32:
                return (arr * 65535.0).clip(0, 65535).astype(np.uint16)

            if arr.dtype.kind in ("u", "i"):
                return arr.astype(np.uint16)

            raise RuntimeError(f"dtype não suportado para uint16: {arr.dtype}")

        if self.state.output_dtype == "float32":
            if arr.dtype == np.float32:
                return arr

            if arr.dtype == np.uint8:
                return arr.astype(np.float32) / 255.0

            if arr.dtype.kind in ("u", "i"):
                max_val = arr.max() if arr.size > 0 else 0
                if max_val <= 255:
                    return arr.astype(np.float32) / 255.0
                return arr.astype(np.float32) / max_sensor_value

            raise RuntimeError(f"dtype não suportado para float32: {arr.dtype}")

        raise RuntimeError(f"output_dtype inválido: {self.state.output_dtype}")

    # =========================================================
    # Captura principal
    # =========================================================

    def capture_frame_raw(self):
        with self._capture_lock:
            if not self.state.initialized:
                raise RuntimeError("Módulo não inicializado")

            t0_perf = time.perf_counter()
            t0_unix = time.time()

            dt_trigger = 0.0
            dt_settle = 0.0
            dt_capture = 0.0
            dt_process = 0.0

            required_sources = list(self.state.payload.sources)
            required_cams = [self._get_camera_spec(cam_id) for cam_id in required_sources]
            has_csi_source = any(getattr(cam, "interface", "").upper() == "CSI" for cam in required_cams)

            try:
                previous_ids = {}
                frames_before = self.camera.capture_raw_frames()
                for cam_id in required_sources:
                    info = frames_before.get(cam_id)
                    if info is not None:
                        previous_ids[cam_id] = info[4]  # frame_id

                # Trigger só vale para as CSI RE/NIR.
                # A USB RGB não responde a esse trigger.
                if self.state.trigger_enabled and has_csi_source:
                    trig_start = time.perf_counter()
                    self.trigger.pulse()
                    trig_end = time.perf_counter()
                    dt_trigger = trig_end - trig_start

                    settle_ms = float(getattr(self.state, "trigger_settle_delay_ms", 0.0) or 0.0)
                    if settle_ms > 0:
                        settle_start = time.perf_counter()
                        time.sleep(settle_ms / 1000.0)
                        settle_end = time.perf_counter()
                        dt_settle = settle_end - settle_start

                cap_start = time.perf_counter()
                raw_frames = self._capture_with_retry(required_sources, previous_ids=previous_ids)
                cap_end = time.perf_counter()
                dt_capture = cap_end - cap_start

                proc_start = time.perf_counter()
                frame_out, meta_extra = self._build_output_frame(raw_frames)
                proc_end = time.perf_counter()
                dt_process = proc_end - proc_start

            except Exception as e:
                raise RuntimeError(f"Falha durante captura/processamento de frame: {e}") from e

            if frame_out is None:
                raise RuntimeError("Frame processado retornou nulo")

            total_end = time.perf_counter()

            self.state.stream_frame_id += 1
            frame_id = self.state.stream_frame_id

            camera_frames_meta = {}
            for cam_id, info in raw_frames.items():
                frame, width, height, channels, cam_frame_id, cam_frame_ts, cam_last_read_ts = info
                cam = self._get_camera_spec(cam_id)

                camera_frames_meta[cam_id] = {
                    "camera_frame_id": int(cam_frame_id),
                    "camera_frame_ts": cam_frame_ts,
                    "camera_last_read_ts": cam_last_read_ts,
                    "width": int(width),
                    "height": int(height),
                    "channels": int(channels),
                    "role": cam.role,
                    "interface": getattr(cam, "interface", None),
                    "bayer_pattern": cam.bayer_pattern,
                    "bit_depth": int(cam.bit_depth),
                    "device_path": getattr(cam, "device_path", None),
                }

            meta = {
                "frame_id": frame_id,
                "ts_pi": t0_unix,
                "ts_pi_monotonic": t0_perf,

                "dt_trigger": dt_trigger,
                "dt_settle": dt_settle,
                "dt_capture": dt_capture,
                "dt_process": dt_process,
                "dt_total_pi": total_end - t0_perf,

                "camera_frames": camera_frames_meta,
                "capture_mode_resolved": self.state.resolve_capture_mode(),
                "payload_format_version": self.state.payload_format_version,
            }

            meta.update(meta_extra)
            return frame_out, meta

    def capture_frame_base64(self):
        frame, meta = self.capture_frame_raw()

        if isinstance(frame, dict):
            encoded = {}
            total_size = 0

            for cam_id, arr in frame.items():
                frame_bytes = arr.tobytes()
                total_size += len(frame_bytes)
                encoded[cam_id] = {
                    "encoding": "base64",
                    "size": len(frame_bytes),
                    "data": base64.b64encode(frame_bytes).decode("ascii"),
                }

            meta["encoding"] = "base64-multi"
            meta["size"] = total_size
            meta["frames"] = encoded
            return meta

        frame_bytes = frame.tobytes()
        meta["encoding"] = "base64"
        meta["size"] = len(frame_bytes)
        meta["data"] = base64.b64encode(frame_bytes).decode("ascii")
        return meta

    # =========================================================
    # Roteamento por frame_type
    # =========================================================

    def _build_output_frame(self, raw_frames):
        if self.state.frame_type == "RAW_BRUTO":
            return self._process_raw_bruto(raw_frames)

        if self.state.frame_type == "RGB":
            return self._process_rgb(raw_frames)

        if self.state.frame_type == "MULTISPEC":
            return self._process_multispec(raw_frames)

        raise RuntimeError(f"frame_type inválido: {self.state.frame_type}")

    # =========================================================
    # RAW_BRUTO
    # =========================================================

    def _process_raw_bruto(self, raw_frames):
        sources = list(self.state.payload.sources)

        if len(sources) == 1:
            cam_id = sources[0]
            frame, width, height, channels, cam_frame_id, cam_frame_ts, cam_last_read_ts = raw_frames[cam_id]
            cam = self._get_camera_spec(cam_id)

            if channels == 1:
                output_layout = "HW"
                output_channel_names = ["RAW_NATIVE"]
            else:
                output_layout = "HWC"
                output_channel_names = [f"C{i}" for i in range(channels)]

            meta_extra = {
                "frame_type": self.state.frame_type,
                "output_dtype": str(frame.dtype),
                "dtype": str(frame.dtype),
                "output_layout": output_layout,
                "output_channels": int(channels),
                "output_channel_names": output_channel_names,
                "output_width": int(width),
                "output_height": int(height),
                "width": int(width),
                "height": int(height),
                "channels": int(channels),
                "payload_sources": [cam_id],
                "source_camera": {
                    "id": cam_id,
                    "role": cam.role,
                    "interface": getattr(cam, "interface", None),
                    "bayer_pattern": cam.bayer_pattern,
                    "bit_depth": int(cam.bit_depth),
                    "device_path": getattr(cam, "device_path", None),
                },
            }
            return frame, meta_extra

        frames_out = {}
        sources_meta = []

        for cam_id in sources:
            frame, width, height, channels, cam_frame_id, cam_frame_ts, cam_frame_last_read_ts = raw_frames[cam_id]
            cam = self._get_camera_spec(cam_id)
            frames_out[cam_id] = frame

            sources_meta.append({
                "id": cam_id,
                "role": cam.role,
                "interface": getattr(cam, "interface", None),
                "width": int(width),
                "height": int(height),
                "channels": int(channels),
                "bayer_pattern": cam.bayer_pattern,
                "bit_depth": int(cam.bit_depth),
                "device_path": getattr(cam, "device_path", None),
            })

        meta_extra = {
            "frame_type": self.state.frame_type,
            "output_dtype": "multi",
            "dtype": "multi",
            "output_layout": "MULTI_NATIVE",
            "output_channels": len(frames_out),
            "output_channel_names": [f"NATIVE_{cam_id.upper()}" for cam_id in sources],
            "output_width": None,
            "output_height": None,
            "width": None,
            "height": None,
            "channels": len(frames_out),
            "payload_sources": sources,
            "raw_sources": sources_meta,
        }

        return frames_out, meta_extra

    # =========================================================
    # RGB
    # =========================================================

    def _process_rgb(self, raw_frames):
        cam_id = self.state.payload.sources[0]
        frame, width, height, channels, _, _, _ = raw_frames[cam_id]
        cam = self._get_camera_spec(cam_id)

        if not self._is_usb_rgb_camera(cam):
            raise RuntimeError(
                f"Modo RGB espera câmera USB role=rgb, mas recebeu cam_id={cam_id}, "
                f"role={cam.role}, interface={getattr(cam, 'interface', None)}"
            )

        rgb_chw = self._convert_usb_bgr_to_rgb_chw_float(frame)
        rgb_chw = self._convert_output_dtype(rgb_chw, bit_depth=8)

        meta_extra = {
            "frame_type": self.state.frame_type,
            "output_dtype": self.state.output_dtype,
            "dtype": str(rgb_chw.dtype),
            "output_layout": "CHW",
            "output_channels": 3,
            "output_channel_names": ["R", "G", "B"],
            "output_width": int(rgb_chw.shape[2]),
            "output_height": int(rgb_chw.shape[1]),
            "width": int(rgb_chw.shape[2]),
            "height": int(rgb_chw.shape[1]),
            "channels": 3,
            "payload_sources": [cam_id],
            "source_camera": {
                "id": cam_id,
                "role": cam.role,
                "interface": getattr(cam, "interface", None),
                "bit_depth": int(cam.bit_depth),
                "source_width": int(width),
                "source_height": int(height),
                "device_path": getattr(cam, "device_path", None),
            },
            "rgb_note": "RGB derivado diretamente de frame USB BGR8",
        }

        return rgb_chw, meta_extra

    # =========================================================
    # MULTISPEC
    # =========================================================

    def _process_multispec(self, raw_frames):
        sources = list(self.state.payload.sources)
        if len(sources) < 2:
            raise RuntimeError("MULTISPEC requer pelo menos RGB + RE ou RGB + NIR")

        rgb_cam = self.state.get_active_camera_by_role("rgb")
        re_cam = self.state.get_active_camera_by_role("re")
        nir_cam = self.state.get_active_camera_by_role("nir")

        if rgb_cam is None:
            raise RuntimeError("MULTISPEC requer uma câmera RGB ativa")

        # RGB USB
        rgb_frame, rgb_w, rgb_h, rgb_channels, _, _, _ = raw_frames[rgb_cam.id]
        if rgb_channels != 3:
            raise RuntimeError(f"Frame RGB USB inválido: channels={rgb_channels}, shape={rgb_frame.shape}")

        rgb_chw = self._convert_usb_bgr_to_rgb_chw_float(rgb_frame)

        spectral_parts = []
        source_cameras = [{
            "id": rgb_cam.id,
            "role": rgb_cam.role,
            "interface": getattr(rgb_cam, "interface", None),
            "bit_depth": int(rgb_cam.bit_depth),
            "device_path": getattr(rgb_cam, "device_path", None),
        }]
        channel_names = ["R", "G", "B"]

        if re_cam is not None and re_cam.id in raw_frames:
            re_packed, *_ = raw_frames[re_cam.id]
            if re_packed.ndim == 3 and re_packed.shape[2] == 1:
                re_packed = re_packed[:, :, 0]

            rp_re = self._ensure_raw_processor_for_camera(re_cam.id)
            re_raw16 = rp_re.unpack_raw10_packed(re_packed)
            re_single = self._normalize_raw16_to_float(re_raw16, re_cam.bit_depth)[None, :, :]
            spectral_parts.append(re_single)
            channel_names.append("RE")
            source_cameras.append({
                "id": re_cam.id,
                "role": re_cam.role,
                "interface": getattr(re_cam, "interface", None),
                "bayer_pattern": re_cam.bayer_pattern,
                "bit_depth": int(re_cam.bit_depth),
            })

        if nir_cam is not None and nir_cam.id in raw_frames:
            nir_packed, *_ = raw_frames[nir_cam.id]
            if nir_packed.ndim == 3 and nir_packed.shape[2] == 1:
                nir_packed = nir_packed[:, :, 0]

            rp_nir = self._ensure_raw_processor_for_camera(nir_cam.id)
            nir_raw16 = rp_nir.unpack_raw10_packed(nir_packed)
            nir_single = self._normalize_raw16_to_float(nir_raw16, nir_cam.bit_depth)[None, :, :]
            spectral_parts.append(nir_single)
            channel_names.append("NIR")
            source_cameras.append({
                "id": nir_cam.id,
                "role": nir_cam.role,
                "interface": getattr(nir_cam, "interface", None),
                "bayer_pattern": nir_cam.bayer_pattern,
                "bit_depth": int(nir_cam.bit_depth),
            })

        if not spectral_parts:
            raise RuntimeError("MULTISPEC requer pelo menos um canal espectral CSI além do RGB")

        arrays = [rgb_chw] + spectral_parts

        min_h = min(arr.shape[1] for arr in arrays)
        min_w = min(arr.shape[2] for arr in arrays)

        arrays = [arr[:, :min_h, :min_w] for arr in arrays]
        multispec = np.concatenate(arrays, axis=0)
        multispec = self._convert_output_dtype(multispec)

        meta_extra = {
            "frame_type": self.state.frame_type,
            "output_dtype": self.state.output_dtype,
            "dtype": str(multispec.dtype),
            "output_layout": "CHW",
            "output_channels": int(multispec.shape[0]),
            "output_channel_names": channel_names,
            "output_width": int(multispec.shape[2]),
            "output_height": int(multispec.shape[1]),
            "width": int(multispec.shape[2]),
            "height": int(multispec.shape[1]),
            "channels": int(multispec.shape[0]),
            "payload_sources": [cam["id"] for cam in source_cameras],
            "source_cameras": source_cameras,
            "multispec_note": "Modo adaptativo: RGB USB + 1 ou 2 canais espectrais CSI",
        }

        return multispec, meta_extra