import math
import numpy as np


class EvasorObstaculos:
    """
    Módulo de evasión de obstáculos para robot con cámara RealSense D415.
    Analiza el escaneo de profundidad y genera comandos de movimiento seguros.
    """

    # ── Zonas del campo visual (en fracción de los 640 píxeles) ──────────────
    ZONA_IZQ   = (0,   213)   # píxeles 0–212
    ZONA_CENT  = (213, 427)   # píxeles 213–426
    ZONA_DER   = (427, 640)   # píxeles 427–639

    # ── Umbrales de distancia (metros) ──────────────────────────────────────
    DIST_PELIGRO  = 0.45   # detener / maniobrar urgente
    DIST_PRECAUCION = 0.80 # reducir velocidad / preparar giro
    DIST_LIBRE    = 1.20   # camino despejado

    # ── Comandos de salida ───────────────────────────────────────────────────
    CMD_ADELANTE      = "ADELANTE"
    CMD_ADELANTE_LENTO= "ADELANTE_LENTO"
    CMD_GIRO_IZQ      = "GIRAR_IZQUIERDA"
    CMD_GIRO_DER      = "GIRAR_DERECHA"
    CMD_RETROCEDER    = "RETROCEDER"
    CMD_DETENIDO      = "DETENIDO"

    def __init__(self,
                 dist_peligro: float   = None,
                 dist_precaucion: float = None,
                 dist_libre: float     = None):
        self.dist_peligro    = dist_peligro    or self.DIST_PELIGRO
        self.dist_precaucion = dist_precaucion or self.DIST_PRECAUCION
        self.dist_libre      = dist_libre      or self.DIST_LIBRE

        # Último estado para logging / depuración
        self.ultimo_estado: dict = {}

    # ────────────────────────────────────────────────────────────────────────
    # Método principal
    # ────────────────────────────────────────────────────────────────────────
    def decidir_movimiento(self, escaner: np.ndarray) -> dict:
        """
        Recibe el array de 640 distancias (metros) de la cámara y
        devuelve un diccionario con el comando y métricas de la decisión.

        Returns
        -------
        {
            "comando"    : str,   # CMD_* constante
            "dist_izq"   : float, # distancia mínima zona izquierda
            "dist_centro": float, # distancia mínima zona central
            "dist_der"   : float, # distancia mínima zona derecha
            "obstaculo"  : bool,  # True si hay peligro inminente
            "razon"      : str,   # explicación legible
        }
        """
        if escaner is None or len(escaner) != 640:
            return self._estado(self.CMD_DETENIDO, 0, 0, 0, True,
                                "Escáner no disponible")

        # ── 1. Distancias mínimas por zona (ignorar lecturas inválidas) ─────
        dist_izq    = self._min_valida(escaner, *self.ZONA_IZQ)
        dist_centro = self._min_valida(escaner, *self.ZONA_CENT)
        dist_der    = self._min_valida(escaner, *self.ZONA_DER)

        # ── 2. Árbol de decisión ─────────────────────────────────────────────
        cmd, razon = self._arbol_decision(dist_izq, dist_centro, dist_der)

        hay_peligro = (dist_centro < self.dist_peligro or
                       dist_izq    < self.dist_peligro or
                       dist_der    < self.dist_peligro)

        self.ultimo_estado = self._estado(
            cmd, dist_izq, dist_centro, dist_der, hay_peligro, razon)
        return self.ultimo_estado

    # ────────────────────────────────────────────────────────────────────────
    # Árbol de decisión
    # ────────────────────────────────────────────────────────────────────────
    def _arbol_decision(self,
                        izq: float,
                        centro: float,
                        der: float) -> tuple[str, str]:
        """Lógica de prioridad para seleccionar comando."""

        # ── Zona central bloqueada ───────────────────────────────────────────
        if centro < self.dist_peligro:

            # Ambos lados bloqueados → retroceder
            if izq < self.dist_peligro and der < self.dist_peligro:
                return self.CMD_RETROCEDER, \
                       f"Todos los lados bloqueados ({centro:.2f}m)"

            # Izquierda libre → girar izquierda
            if izq >= der:
                return self.CMD_GIRO_IZQ, \
                       f"Centro bloqueado ({centro:.2f}m), izq más libre ({izq:.2f}m)"

            # Derecha libre → girar derecha
            return self.CMD_GIRO_DER, \
                   f"Centro bloqueado ({centro:.2f}m), der más libre ({der:.2f}m)"

        # ── Precaución central ───────────────────────────────────────────────
        if centro < self.dist_precaucion:

            if izq > self.dist_libre and der <= self.dist_precaucion:
                return self.CMD_GIRO_IZQ, \
                       f"Precaución centro ({centro:.2f}m), girando izq"

            if der > self.dist_libre and izq <= self.dist_precaucion:
                return self.CMD_GIRO_DER, \
                       f"Precaución centro ({centro:.2f}m), girando der"

            return self.CMD_ADELANTE_LENTO, \
                   f"Precaución ({centro:.2f}m), avanzando despacio"

        # ── Centro libre — revisar laterales ────────────────────────────────
        if izq < self.dist_peligro:
            return self.CMD_GIRO_DER, \
                   f"Obstáculo lateral izq ({izq:.2f}m)"

        if der < self.dist_peligro:
            return self.CMD_GIRO_IZQ, \
                   f"Obstáculo lateral der ({der:.2f}m)"

        # ── Camino despejado ─────────────────────────────────────────────────
        return self.CMD_ADELANTE, \
               f"Camino libre (centro={centro:.2f}m)"

    # ────────────────────────────────────────────────────────────────────────
    # Utilidades
    # ────────────────────────────────────────────────────────────────────────
    def _min_valida(self, escaner: np.ndarray,
                    inicio: int, fin: int) -> float:
        """Distancia mínima en la zona [inicio, fin) ignorando valores nulos."""
        zona = escaner[inicio:fin]
        validos = zona[(zona > 0.05) & (zona < 10.0)]
        return float(np.min(validos)) if len(validos) else 10.0

    @staticmethod
    def _estado(cmd, izq, centro, der, peligro, razon) -> dict:
        return {
            "comando"    : cmd,
            "dist_izq"   : round(float(izq),    3),
            "dist_centro": round(float(centro),  3),
            "dist_der"   : round(float(der),     3),
            "obstaculo"  : peligro,
            "razon"      : razon,
        }

    # ────────────────────────────────────────────────────────────────────────
    # Visualización en el mapa SLAM
    # ────────────────────────────────────────────────────────────────────────
    def dibujar_estado(self, mapa: np.ndarray,
                       pos_x: int = 10, pos_y: int = 20) -> None:
        """
        Superpone el estado de evasión sobre el mapa OpenCV existente.
        Llama a esto después de AlgoritmoSLAM.dibujar_robot_y_entorno().
        """
        import cv2

        e = self.ultimo_estado
        if not e:
            return

        COLOR_OK      = (0, 220, 0)
        COLOR_PRECAU  = (0, 200, 255)
        COLOR_PELIGRO = (0, 0, 255)

        color = COLOR_PELIGRO if e["obstaculo"] else (
                COLOR_PRECAU  if e["dist_centro"] < self.dist_precaucion
                else COLOR_OK)

        lineas = [
            f"CMD : {e['comando']}",
            f"IZQ : {e['dist_izq']:.2f}m",
            f"CENT: {e['dist_centro']:.2f}m",
            f"DER : {e['dist_der']:.2f}m",
            e["razon"],
        ]

        # Fondo semitransparente
        overlay = mapa.copy()
        cv2.rectangle(overlay,
                      (pos_x - 4,  pos_y - 16),
                      (pos_x + 250, pos_y + len(lineas) * 18 + 4),
                      (30, 30, 30), -1)
        cv2.addWeighted(overlay, 0.6, mapa, 0.4, 0, mapa)

        for i, linea in enumerate(lineas):
            cv2.putText(mapa, linea,
                        (pos_x, pos_y + i * 18),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.48, color, 1,
                        cv2.LINE_AA)