"""R1 head video-service control via the robot_state DDS service (api_id 1001).

Phone-free equivalent of the Unitree app's Settings → Service Status toggle for
the stereo camera: `RobotStateClient.ServiceSwitch("stereo_patch_pc1", on)`.
VIDEO ONLY — this never touches motion/loco services.

Reuses the app's already-initialized DDS ChannelFactory (arm_controller.init()
calls ChannelFactoryInitialize once per process), so the client is created
lazily on first use — by then the arm subsystem has set the factory up. If the
SDK/DDS isn't available it degrades to a no-op (returns False) instead of raising.
"""
from __future__ import annotations

import threading
from typing import Optional

from Project.Sanad.core.logger import get_logger

log = get_logger("stream_service")

STEREO_SERVICE = "stereo_patch_pc1"


class StreamService:
    """Thin, thread-safe wrapper around RobotStateClient for the stereo stream."""

    def __init__(self) -> None:
        self._lock = threading.Lock()
        self._client = None
        self._unavailable = False

    def _client_or_none(self):
        if self._client is not None:
            return self._client
        if self._unavailable:
            return None
        try:
            # Reuses the process-wide ChannelFactory (already initialized by the
            # arm controller). Does NOT re-init it — that's the single-init rule.
            from unitree_sdk2py.go2.robot_state.robot_state_client import (
                RobotStateClient,
            )
            c = RobotStateClient()
            c.Init()
            c.SetTimeout(3.0)
            self._client = c
            log.info("robot_state client ready (stereo video-service control)")
            return c
        except Exception as exc:
            # No SDK, DDS not up yet, or robot link down — mark unavailable so we
            # don't retry-spam; a process restart re-attempts.
            log.info("robot_state client unavailable (%s) — stream auto-control off", exc)
            self._unavailable = True
            return None

    def set_stereo(self, on: bool) -> bool:
        """Start (on=True) / stop (on=False) the stereo stream. Idempotent —
        starting an already-running service is harmless. Returns True on success."""
        with self._lock:
            c = self._client_or_none()
            if c is None:
                return False
            try:
                code = c.ServiceSwitch(STEREO_SERVICE, bool(on))
                if code == 0:
                    log.info("stereo stream -> %s", "ON" if on else "OFF")
                    return True
                log.warning("stereo ServiceSwitch(%s) returned code=%s", on, code)
                return False
            except Exception as exc:
                log.warning("stereo ServiceSwitch(%s) failed: %s", on, exc)
                return False


_instance: Optional[StreamService] = None


def get_stream_service() -> StreamService:
    global _instance
    if _instance is None:
        _instance = StreamService()
    return _instance