import sys
import select
import tty
import termios
from modulo_esp32 import ChasisESP32

# Mensaje de interfaz
MSG = """
Control Manual del Robot (Modo Python Puro)
-------------------------------------------
Controles de movimiento:
        w
   a    s    d

Espacio o 'x' : FRENAR DE GOLPE
'q'           : SALIR Y APAGAR MOTORES

Aumentos por pulsación:
w/s : +- 0.05 m/s (Velocidad Lineal)
a/d : +- 0.10 rad/s (Velocidad Angular)
"""

# Límites de seguridad para que el robot no salga volando
VEL_LINEAL_MAX = 0.5  # Metros por segundo
VEL_ANGULAR_MAX = 1.0 # Radianes por segundo
PASO_LINEAL = 0.05
PASO_ANGULAR = 0.1

def obtener_tecla(settings):
    """Magia de Linux para leer una sola tecla sin presionar Enter"""
    tty.setraw(sys.stdin.fileno())
    select.select([sys.stdin], [], [], 0)
    key = sys.stdin.read(1)
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key

def main():
    # Guardar la configuración de la terminal para restaurarla al final
    settings = termios.tcgetattr(sys.stdin)
    
    # Inicializar la conexión UART con la ESP32
    print("Iniciando conexión con el chasis...")
    robot = ChasisESP32(puerto='/dev/ttyAMA0')
    
    v = 0.0
    w = 0.0
    
    try:
        print(MSG)
        while True:
            tecla = obtener_tecla(settings)
            
            # Lógica de movimiento
            if tecla == 'w':
                v += PASO_LINEAL
            elif tecla == 's':
                v -= PASO_LINEAL
            elif tecla == 'a':
                w += PASO_ANGULAR  # Giro a la izquierda (positivo en regla de mano derecha)
            elif tecla == 'd':
                w -= PASO_ANGULAR  # Giro a la derecha
            elif tecla == ' ' or tecla == 'x':
                v = 0.0
                w = 0.0
            elif tecla == 'q':
                break
            
            # Aplicar límites de seguridad
            v = max(min(v, VEL_LINEAL_MAX), -VEL_LINEAL_MAX)
            w = max(min(w, VEL_ANGULAR_MAX), -VEL_ANGULAR_MAX)
            
            # Imprimir estado actual sobrescribiendo la misma línea
            print(f"\r🚀 Velocidad -> Lineal: {v:.2f} m/s | Angular: {w:.2f} rad/s    ", end='')
            
            # Enviar el comando matemático a la ESP32
            robot.enviar_velocidad(v, w)

    except Exception as e:
        print(f"\n❌ Error en teleop: {e}")
        
    finally:
        # 1. Por seguridad, frenar el robot al salir
        print("\nDeteniendo motores...")
        robot.enviar_velocidad(0.0, 0.0)
        # 2. Restaurar la terminal a su estado normal
        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
        print("Teleop cerrado exitosamente.")

if __name__ == "__main__":
    main()