# camera_manager.py

import depthai as dai
import cv2
import numpy as np
import time

# Parâmetros da câmera
RGB_WIDTH, RGB_HEIGHT = 640, 480
DEPTH_WIDTH, DEPTH_HEIGHT = 320, 240
FX = 440.0        # distância focal em pixels (aproximado)
BASELINE = 0.075  # distância entre câmeras estéreo (em metros)

# Variáveis globais
device = None
device_info = None
rgb_queue = None
depth_queue = None
ultimo_frame_depth = None
timestamp_ultimo_depth_frame = None
ultimo_frame_rgb = None
timestamp_ultimo_rgb_frame = None

def iniciar_camera(index=0):
    global device, device_info, rgb_queue, depth_queue

    print("Iniciando câmera OAK-D Lite...")

    pipeline = dai.Pipeline()

    # RGB
    cam_rgb = pipeline.create(dai.node.ColorCamera)
    cam_rgb.setPreviewSize(RGB_WIDTH, RGB_HEIGHT)
    cam_rgb.setInterleaved(False)
    cam_rgb.setBoardSocket(dai.CameraBoardSocket.RGB)

    xout_rgb = pipeline.create(dai.node.XLinkOut)
    xout_rgb.setStreamName("rgb")
    cam_rgb.preview.link(xout_rgb.input)

    # Profundidade
    mono_left = pipeline.create(dai.node.MonoCamera)
    mono_right = pipeline.create(dai.node.MonoCamera)
    stereo = pipeline.create(dai.node.StereoDepth)

    mono_left.setResolution(dai.MonoCameraProperties.SensorResolution.THE_480_P)
    mono_right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_480_P)
    mono_left.setBoardSocket(dai.CameraBoardSocket.LEFT)
    mono_right.setBoardSocket(dai.CameraBoardSocket.RIGHT)

    stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY)

    mono_left.out.link(stereo.left)
    mono_right.out.link(stereo.right)

    xout_depth = pipeline.create(dai.node.XLinkOut)
    xout_depth.setStreamName("depth")
    stereo.depth.link(xout_depth.input)

    # Dispositivo
    available_devices = dai.Device.getAllAvailableDevices()
    if index >= len(available_devices):
        raise ValueError(f"Câmera de índice {index} não encontrada.")

    device_info = available_devices[index]
    device = dai.Device(pipeline, device_info)

    rgb_queue = device.getOutputQueue(name="rgb", maxSize=1, blocking=False)
    depth_queue = device.getOutputQueue(name="depth", maxSize=1, blocking=False)

    print(f"Câmera iniciada: {device_info.name} (ID: {device_info.getMxId()})")

    get_camera_calib()

def get_camera_calib():
    global FX, BASELINE
    calib = device.readCalibration()

    # Obtem matriz intrínseca da câmera LEFT (com resolução padrão 640x400)
    intrinsics = calib.getCameraIntrinsics(dai.CameraBoardSocket.LEFT, 640, 400)
    FX = intrinsics[0][0]  # fx
    BASELINE = calib.getBaselineDistance() / 1000.0  # de mm → m

    print(f"[CALIB] FX: {FX:.2f} px, BASELINE: {BASELINE:.4f} m")

def get_rgb_frame():
    global ultimo_frame_rgb, timestamp_ultimo_rgb_frame
    if rgb_queue is None:
        return None, None
    frame = rgb_queue.tryGet()
    if frame is not None:
        ultimo_frame_rgb = frame.getCvFrame()
        timestamp_ultimo_rgb_frame = time.time()
    return ultimo_frame_rgb, timestamp_ultimo_rgb_frame

def get_heatmap_frame():
    frame = get_depth_frame()
    if frame is not None:
        return gerar_heatmap(frame), timestamp_ultimo_depth_frame
    return None, None

def gerar_heatmap(depth_frame):
    # Normaliza e aplica colormap
    normalized = cv2.normalize(depth_frame, None, 0, 255, cv2.NORM_MINMAX)
    heatmap = cv2.applyColorMap(normalized.astype(np.uint8), cv2.COLORMAP_JET)
    return heatmap

def get_depth_frame():
    global ultimo_frame_depth, timestamp_ultimo_depth_frame
    if depth_queue is None:
        return None, None

    frame = depth_queue.tryGet()
    if frame is not None:
        ultimo_frame_depth = frame.getFrame()
        timestamp_ultimo_depth_frame = time.time()
    return ultimo_frame_depth, timestamp_ultimo_depth_frame

def get_status_dispositivo():
    from depthai import UsbSpeed

    try:
        memory = device.getDdrMemoryUsage()
        memory_info = {
            "used": memory.used,
            "remaining": memory.remaining,
            "total": memory.total
        }
    except:
        memory_info = None

    try:
        temp = device.getChipTemperature()
        temp_info = {
            "css": temp.css,
            "mss": temp.mss,
            "upa": temp.upa,
            "dss": temp.dss
        }
    except:
        temp_info = None

    try:
        info = device.getDeviceInfo()
        protocol = str(info.protocol)
    except:
        protocol = None

    try:
        bootloader = str(device.getBootloaderVersion())
    except:
        bootloader = None

    try:
        usb_speed = str(device.getUsbSpeed().name)
    except:
        usb_speed = None

    try:
        pipeline_running = device.isPipelineRunning()
    except:
        pipeline_running = None

    try:
        cameras = [sensor.name for sensor in device.getConnectedCameras()]
    except:
        cameras = None

    return {
        "id": device_info.getMxId(),
        "name": device_info.name,
        "state": device_info.state.name,
        "usb_speed": usb_speed,
        "available_camera_sensors": cameras,
        "version": protocol,
        "bootloader_version": bootloader,
        "is_pipeline_running": pipeline_running,
        "memory_usage": memory_info,
        "temperature": temp_info
    }

def analisar_obstaculos(velocidade=0.0, refinar=False):
    from processamento.obstaculos import analisar_macro_grid, analisar_micro_grid
    global timestamp_ultimo_depth_frame

    frame = get_depth_frame()

    if frame is None:
        if ultimo_frame_depth is None:
            return { "erro": "Sem frame disponível", "timestamp": None }
        frame = ultimo_frame_depth

    macro = analisar_macro_grid(frame, velocidade)

    if macro["precisa_micro"] and refinar:
        micro = analisar_micro_grid(frame)
        micro["timestamp"] = timestamp_ultimo_depth_frame
        return micro

    macro["timestamp"] = timestamp_ultimo_depth_frame
    return macro

def analisar_corredor():
    from processamento.corredor import estimar_largura_corredor
    global timestamp_ultimo_depth_frame  # precisa garantir que fx e baseline existam

    frame = get_depth_frame()

    if frame is None:
        if ultimo_frame_depth is None:
            return { "erro": "Sem frame disponível", "timestamp": None }
        frame = ultimo_frame_depth

    largura_mm, pos_central, pontos_debug = estimar_largura_corredor(frame, FX, BASELINE)

    return {
        "largura_corredor_mm": float(largura_mm) if largura_mm is not None else None,
        "posicao_central_fracao": float(pos_central) if pos_central is not None else None,
        "timestamp": timestamp_ultimo_depth_frame,
        "pontos_debug": [ [int(x), float(y)] for x, y in pontos_debug ]
    }

def analisar_obstaculos_3d():
    from processamento.visao3d import detectar_obstaculos_em_frente
    global timestamp_ultimo_depth_frame

    frame = get_depth_frame()

    if frame is None:
        if ultimo_frame_depth is None:
            return { "erro": "Sem frame disponível", "timestamp": None }
        frame = ultimo_frame_depth

    lista = detectar_obstaculos_em_frente(frame, FX, BASELINE)

    return {
        "obstaculos_detectados": lista,
        "timestamp": timestamp_ultimo_depth_frame
    }