import serial
import math


class ChasisESP32:
    """
    Interfaz UART con el ESP32.

    Protocolo de TX (RPi → ESP32):
        M:s1,f1,s2,f2,s3,f3,s4,f4\\n
            s = signo  → -1 | 0 | 1
            f = fracción de VEL_MAX → 0.000 – 1.000
        S:\\n   → Stop inmediato
        C:\\n   → Iniciar autocalibración

    Protocolo de RX (ESP32 → RPi):
        T:t1,t2,t3,t4\\n  → Odometría acumulada (ticks con signo)
        K:v1,v2,v3,v4\\n  → VEL_MAX_TICKS calibrada por motor (ticks/s)
        D:...\\n           → Mensajes de debug del ESP32
    """

    # ── Geometría ──────────────────────────────────────────────────────────────
    TICKS_POR_VUELTA = 20.0
    RADIO_RUEDA      = 0.03   # metros
    ANCHO_EJE        = 0.11   # metros (distancia entre ruedas del mismo eje)

    # VEL_MAX_TICKS se actualiza cuando el ESP32 envía la trama K:
    # Valor inicial conservador: 200 ticks/s
    # M1=FL, M2=FR, M3=RL, M4=RR
    _VEL_MAX_TICKS = [200.0, 200.0, 200.0, 200.0]

    def __init__(self, puerto: str = '/dev/ttyAMA0', baudrate: int = 921600):
        self.METROS_POR_TICK = (2.0 * math.pi * self.RADIO_RUEDA) / self.TICKS_POR_VUELTA

        try:
            self.puerto = serial.Serial(puerto, baudrate, timeout=0.05)
            print(f"✅ Conexión UART activa en {puerto}")
        except Exception as e:
            print(f"❌ Error UART: {e}")
            self.puerto = None

        self.x, self.y, self.theta = 0.0, 0.0, 0.0
        self.ticks_anteriores = [0, 0, 0, 0]
        self.primera_lectura  = True

    # ── Velocidad máxima (promedio por lado) ───────────────────────────────────
    @property
    def vel_max_izq(self) -> float:
        """Ticks/s máximos del lado izquierdo (promedio M1 y M3)."""
        return (self._VEL_MAX_TICKS[0] + self._VEL_MAX_TICKS[2]) / 2.0

    @property
    def vel_max_der(self) -> float:
        """Ticks/s máximos del lado derecho (promedio M2 y M4)."""
        return (self._VEL_MAX_TICKS[1] + self._VEL_MAX_TICKS[3]) / 2.0

    # ── Lectura de odometría y tramas de control ───────────────────────────────
    def leer_odometria(self) -> bool:
        """
        Lee una línea del ESP32 y la procesa.
        Retorna True si se actualizó la odometría.
        """
        if not (self.puerto and self.puerto.in_waiting > 0):
            return False

        try:
            linea = self.puerto.readline().decode('utf-8', errors='ignore').strip()
        except Exception:
            return False

        # ── T: odometría ──────────────────────────────────────────────────────
        if linea.startswith("T:"):
            partes = linea[2:].split(',')
            if len(partes) != 4:
                return False
            try:
                ticks_actuales = [int(p) for p in partes]
            except ValueError:
                return False

            if self.primera_lectura:
                self.ticks_anteriores = ticks_actuales
                self.primera_lectura  = False
                return True

            delta = [ticks_actuales[i] - self.ticks_anteriores[i] for i in range(4)]
            self.ticks_anteriores = ticks_actuales

            # Desplazamiento lineal de cada lado (metros)
            # M1(FL) y M3(RL) → izquierda
            # M2(FR) y M4(RR) → derecha
            d_izq = ((delta[0] + delta[2]) / 2.0) * self.METROS_POR_TICK
            d_der = ((delta[1] + delta[3]) / 2.0) * self.METROS_POR_TICK

            d_centro = (d_izq + d_der) / 2.0
            d_theta  = (d_der - d_izq) / self.ANCHO_EJE

            self.x     += d_centro * math.cos(self.theta + d_theta / 2.0)
            self.y     += d_centro * math.sin(self.theta + d_theta / 2.0)
            self.theta += d_theta
            return True

        # ── K: VEL_MAX calibrada por el ESP32 ─────────────────────────────────
        elif linea.startswith("K:"):
            partes = linea[2:].split(',')
            if len(partes) == 4:
                try:
                    nuevos = [float(p) for p in partes]
                    # Validar que los valores sean razonables (10–2000 ticks/s)
                    if all(10.0 < v < 2000.0 for v in nuevos):
                        self._VEL_MAX_TICKS = nuevos
                        print(
                            f"\n✅ VEL_MAX actualizada → "
                            f"M1={nuevos[0]:.1f}  M2={nuevos[1]:.1f}  "
                            f"M3={nuevos[2]:.1f}  M4={nuevos[3]:.1f}  ticks/s"
                        )
                    else:
                        print(f"\n⚠️  VEL_MAX recibida fuera de rango: {nuevos} — ignorada")
                except ValueError:
                    pass

        # ── D: debug del ESP32 ────────────────────────────────────────────────
        elif linea.startswith("D:"):
            print(f"\r[ESP32] {linea}                      ", end="")

        return False

    # ── Envío de velocidad diferencial ─────────────────────────────────────────
    def enviar_velocidad(self, v: float, w: float) -> None:
        """
        Convierte velocidad lineal (m/s) y angular (rad/s) al protocolo M:.

        Cinemática diferencial:
            v_izq = v - w * (ANCHO_EJE / 2)
            v_der = v + w * (ANCHO_EJE / 2)

        Cada rueda se envía con:
            s = signo  (-1, 0, 1)
            f = |vel_rueda_ticks| / VEL_MAX_TICKS del motor  (0.0 – 1.0)

        Trama resultante:
            M:s1,f1,s2,f2,s3,f3,s4,f4\\n
        """
        if not self.puerto:
            return

        # Velocidades en m/s para cada lado
        v_izq_ms = v - w * (self.ANCHO_EJE / 2.0)
        v_der_ms = v + w * (self.ANCHO_EJE / 2.0)

        # Convertir a ticks/s
        v_izq_ticks = v_izq_ms / self.METROS_POR_TICK
        v_der_ticks = v_der_ms / self.METROS_POR_TICK

        # Velocidades por motor (izq = M1, M3 / der = M2, M4)
        vel_motores = [v_izq_ticks, v_der_ticks, v_izq_ticks, v_der_ticks]

        # Umbral mínimo de movimiento: 0.5 ticks/s ≈ movimiento nulo
        UMBRAL = 0.5

        partes = []
        todos_cero = True
        for i, vel in enumerate(vel_motores):
            if abs(vel) < UMBRAL:
                s, f = 0, 0.0
            else:
                s = 1 if vel > 0 else -1
                # Normalizar respecto a la VEL_MAX de cada motor específico
                f = min(abs(vel) / self._VEL_MAX_TICKS[i], 1.0)
                todos_cero = False
            partes.append(f"{s},{f:.3f}")

        if todos_cero:
            cmd = "S:\n"
        else:
            cmd = "M:" + ",".join(partes) + "\n"

        print(f"\r[UART TX] → {cmd.strip():<55}", end="", flush=True)
        try:
            self.puerto.write(cmd.encode('utf-8'))
        except Exception as e:
            print(f"\n❌ Error al escribir UART: {e}")

    # ── Calibración ───────────────────────────────────────────────────────────
    def calibrar_pid(self) -> None:
        """Solicita autocalibración al ESP32. La respuesta K: actualizará VEL_MAX."""
        if self.puerto:
            try:
                self.puerto.write(b"C:\n")
                print("\n⚙️  Solicitud de calibración enviada al ESP32.")
            except Exception as e:
                print(f"\n❌ Error al enviar calibración: {e}")