"""
camera_api.py — RealSense D435I camera thread
"""
import base64
import io
import time
import threading
import numpy as np
from PIL import Image
from Core.config_loader import load_config
from Core.logger import log

_cfg = load_config("Camera")

CAM_WIDTH   = _cfg["width"]
CAM_HEIGHT  = _cfg["height"]
CAM_FPS     = _cfg["fps"]
CAM_QUALITY = _cfg["jpeg_quality"]

# Shared state
latest_frame_b64     = [None]
_raw_frame           = [None]
camera_lock          = threading.Lock()
_raw_lock            = threading.Lock()
camera_alive         = [True]
_cam_last_frame_time = [0.0]
_cam_connected       = [False]


def get_raw_refs():
    """Return (raw_frame_ref, raw_lock) for YOLO to share."""
    return _raw_frame, _raw_lock


def camera_loop():
    """Capture RealSense frames continuously with auto-reconnect."""
    import pyrealsense2 as rs

    backoff = 2.0
    while camera_alive[0]:
        pipeline = None
        try:
            pipeline = rs.pipeline()
            cfg = rs.config()
            cfg.enable_stream(rs.stream.color, CAM_WIDTH, CAM_HEIGHT, rs.format.bgr8, CAM_FPS)
            pipeline.start(cfg)
            backoff = 2.0
            _cam_connected[0] = True
            print("Camera connected")
            log(f"Camera connected {CAM_WIDTH}x{CAM_HEIGHT}@{CAM_FPS}", "info", "camera")

            while camera_alive[0]:
                try:
                    frames = pipeline.wait_for_frames(timeout_ms=5000)
                    color_frame = frames.get_color_frame()
                    if not color_frame:
                        continue

                    frame = np.asanyarray(color_frame.get_data())
                    if frame is None or frame.size == 0:
                        continue

                    with _raw_lock:
                        _raw_frame[0] = frame.copy()

                    img = Image.fromarray(frame[:, :, ::-1])
                    buf = io.BytesIO()
                    img.save(buf, format="JPEG", quality=CAM_QUALITY)
                    with camera_lock:
                        latest_frame_b64[0] = base64.b64encode(buf.getvalue()).decode()

                    _cam_last_frame_time[0] = time.time()

                except Exception:
                    if time.time() - _cam_last_frame_time[0] > 10.0:
                        print("  [Camera] No frame for 10s — reconnecting...")
                        break

        except Exception as e:
            if _cam_connected[0]:
                print(f"  [Camera] Disconnected ({type(e).__name__}) — retrying in {backoff:.0f}s...")
                _cam_connected[0] = False
            try:
                pipeline.stop()
            except Exception:
                pass
            time.sleep(backoff)
            backoff = min(backoff * 2, 10.0)


def start_camera():
    """Start camera thread. Returns (raw_frame_ref, raw_lock)."""
    threading.Thread(target=camera_loop, daemon=True).start()
    time.sleep(3.0)
    return _raw_frame, _raw_lock


def stop_camera():
    """Stop camera thread."""
    camera_alive[0] = False


def get_frame():
    """Return latest base64 JPEG frame for LLaVA. None if not ready."""
    with camera_lock:
        return latest_frame_b64[0]


def get_frame_age() -> float:
    """Return seconds since last camera frame."""
    return time.time() - _cam_last_frame_time[0] if _cam_last_frame_time[0] > 0 else 999.0