import sys
import os
import time
import json
from queue import Queue
import threading

from enums import Comando, TipoComando

from mqtt_handler import enviar_mensagem_mqtt

sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), "..")))


# ─────────────────────────────
# 🔹 Fila de Mensagens para Processamento de Comandos
# ─────────────────────────────
fila_comandos = Queue()
def worker():
    while True:
        topico, payload = fila_comandos.get()
        try:
            processar_comando(topico, payload)
        except Exception as e:
            print("❌ Erro:", e)
        fila_comandos.task_done()
# 🔥 Cria 4 workers
for _ in range(4):
    threading.Thread(target=worker, daemon=True).start()


# ─────────────────────────────
# 🔹 Parâmetros de Execução
# ─────────────────────────────
mqtt_topic = sys.argv[1] if len(sys.argv) > 1 else "agrobot/operador/sonar"
camera_index = int(sys.argv[2]) if len(sys.argv) > 2 else 0


# ─────────────────────────────
# 🔹 Callback de Comando MQTT
# ─────────────────────────────
def ao_receber_comando(topico, payload):
    fila_comandos.put((topico, payload))

def processar_comando(topico, payload):
    try:
        comando = json.loads(payload.decode())
        tipo_cmd = int(comando.get("tipo_cmd"))
        codigo = int(comando.get("cmd"))

        # Ignora mensagens enviadas por si proprio
        if (tipo_cmd == TipoComando.RX):
            return

        resposta = {}

        if codigo == Comando.PING:
            resposta = {
                "ping": {
                    "timestamp": time.time(), 
                    "status": True
                }
            }

        elif codigo == Comando.GET_STATUS_DISPOSITIVO:
            from camera_manager import get_status_dispositivo
            resposta = {
                "device_status": get_status_dispositivo()
            }

        elif codigo == Comando.GET_RGB_FRAME:
            from camera_manager import get_rgb_frame
            frame, timestamp = get_rgb_frame()
            base64_img = None
            if frame is not None:
                from utils import encode_image_base64
                base64_img = encode_image_base64(frame)
            resposta = {
                "timestamp": timestamp, 
                "frame": base64_img 
            }

        elif codigo == Comando.GET_HEATMAP_FRAME:
            from camera_manager import get_heatmap_frame
            frame, timestamp = get_heatmap_frame()
            base64_img = None
            if frame is not None:
                from utils import encode_image_base64
                base64_img = encode_image_base64(frame)
            resposta = {
                "timestamp": timestamp, 
                "frame": base64_img 
            }

        elif codigo == Comando.CALIBRAR_GRADES:
            from camera_manager import get_depth_frame
            from processamento.obstaculos import calibrar_grades
            sucesso = calibrar_grades(get_depth_frame, n_frames=50)
            resposta = { 
                "calibragem": {
                    "timestamp": time.time(), 
                    "status": sucesso
                }
            }

        elif codigo == Comando.GET_OBSTACULOS:
            from camera_manager import analisar_obstaculos
            velocidade = float(comando.get("velocidade", 0.0))  # padrão = 0.0 m/s, parado
            precisao_micro = float(comando.get("precisao_micro", False))
            resposta = {
                "obstaculos": analisar_obstaculos(velocidade=velocidade, refinar=precisao_micro)
            }

        elif codigo == Comando.GET_LARGURA_CORREDOR:
            from camera_manager import analisar_corredor
            resposta = {
                "corredor": analisar_corredor()
            }

        elif codigo == Comando.GET_MAPA_PROFUNDIDADE:
            from camera_manager import get_depth_frame
            from utils import gerar_mapa_profundidade

            depth, timestamp = get_depth_frame()
            if depth is not None:
                resolucao = str(comando.get("resolucao", "10x10"))
                try:
                    linhas, colunas = map(int, resolucao.lower().split("x"))
                except:
                    linhas, colunas = 10, 10

            grid = gerar_mapa_profundidade(depth, linhas, colunas)
            grid["timestamp"] = timestamp
            resposta = {
                "mapa_profundidade": grid
            }
            
        elif codigo == Comando.GET_OBSTACULOS_3D:
            from camera_manager import analisar_obstaculos_3d
            resposta = {
                "obstaculos_3d": analisar_obstaculos_3d()
            }

        enviar_mensagem_mqtt(codigo, resposta)

    except Exception as e:
        print("❌ Erro ao processar comando MQTT:", e)

# ─────────────────────────────
# 🔹 Execução Principal
# ─────────────────────────────
if __name__ == "__main__":
    print("🚀 Iniciando Operário Visual com OAK-D Lite...")

    # 1. Inicia a câmera
    from camera_manager import iniciar_camera
    iniciar_camera(camera_index)

    from mqtt_handler import iniciar_mqtt, enviar_mensagem_script_carregado

    # 2. Inicia MQTT e registra callback
    iniciar_mqtt(mqtt_topic, ao_receber_comando)

    print(f"✅ Escutando comandos no tópico: {mqtt_topic}")

    enviar_mensagem_script_carregado()

    # 3. Loop principal
    try:
        while True:
            time.sleep(1)
    except KeyboardInterrupt:
        print("Encerrando...")