import math
from typing import Tuple, List

class SeguidorRuta:
    """
    Controlador para que el robot siga una serie de puntos (ruta A*).
    Utiliza un enfoque simplificado: gira hacia el waypoint y avanza.
    """

    def __init__(self, 
                 umbral_distancia: float = 0.15,
                 vel_max: float = 0.50,
                 vel_giro: float = 1.50):
        """
        Args:
            umbral_distancia: Distancia en metros a la que se considera alcanzado un waypoint.
            vel_max: Velocidad máxima de avance (m/s).
            vel_giro: Velocidad máxima de giro (rad/s).
        """
        self.umbral_distancia = umbral_distancia
        self.vel_max = vel_max
        self.vel_giro = vel_giro

    def calcular_velocidad(self, 
                           x_robot: float, 
                           y_robot: float, 
                           theta_robot: float, 
                           ruta: List[Tuple[float, float]]) -> Tuple[float, float, List[Tuple[float, float]]]:
        """
        Calcula la velocidad lineal y angular para dirigirse al siguiente waypoint.
        Retorna (v, w, ruta_actualizada).
        """
        if not ruta:
            return 0.0, 0.0, []

        wx, wy = ruta[0]
        
        # Calcular distancia al waypoint actual
        distancia = math.hypot(wy - y_robot, wx - x_robot)
        
        # Si estamos lo suficientemente cerca, pasar al siguiente punto
        if distancia < self.umbral_distancia:
            ruta.pop(0)
            if not ruta:
                # Llegamos al destino final
                return 0.0, 0.0, []
            # Tomar las coordenadas del nuevo waypoint
            wx, wy = ruta[0]

        # Calcular ángulo hacia el waypoint respecto al marco global
        angulo_objetivo = math.atan2(wy - y_robot, wx - x_robot)
        
        # Calcular error de ángulo (relativo al robot)
        error_angulo = angulo_objetivo - theta_robot
        
        # Normalizar el error al rango [-pi, pi] para girar por el lado más corto
        error_angulo = (error_angulo + math.pi) % (2 * math.pi) - math.pi

        # Lógica de movimiento
        # Si el error es grande (> 20 grados aprox), priorizar giro en el propio eje
        umbral_giro = 0.35
        
        if abs(error_angulo) > umbral_giro:
            v = 0.0
            # Girar a la velocidad máxima en la dirección correspondiente
            w = self.vel_giro if error_angulo > 0 else -self.vel_giro
        else:
            # Avanzar y corregir ligeramente la trayectoria (Control Proporcional suave)
            v = self.vel_max
            # Escala el giro suavemente pero sin superar el maximo
            w = error_angulo * (self.vel_giro / umbral_giro) 
            w = max(-self.vel_giro, min(self.vel_giro, w))

        return v, w, ruta