# pip install ev3-dc
# Comando para instalar la librería del robot EV3
"""Cuadrados de 50 cm solo con los encoders (punto de partida del ejercicio de la semana 13).

El robot parte sobre el marcador 1, mirando hacia el marcador 2, y recorre el cuadrado
girando 90° a la izquierda en cada esquina (4 giros por vuelta). Después de cada esquina
se imprime lo que cree el robot (odometría); en el simulador también la pose real.

Simulador: abra worlds/ejercicio_cuadrado.wbt, ejecútelo (▶) y cambie la importación por
import ev3_sim as ev3 (con ev3_sim.py en la misma carpeta de este programa).
"""
import math
# Importamos math para calcular distancias

import ev3_dc as ev3
# Importamos la librería del robot EV3 (en el simulador: import ev3_sim as ev3)

# ===== CONFIGURA AQUÍ TU ROBOT =====
MAC = '00:16:53:50:8A:86'   # dirección MAC de tu ladrillo EV3 (se ignora en el simulador)
RADIUS_WHEEL = 0.0152       # m, radio de la rueda
TREAD_DISTANCE = 0.1400     # m, distancia completa entre las ruedas
# ====================================

LADO = 0.50      # m, lado del cuadrado
VUELTAS = 3      # vueltas completas al cuadrado
VELOCIDAD = 30   # % de la velocidad máxima de los motores

# Esquinas del cuadrado en el marco de partida (x adelante, y a la izquierda)
ESQUINAS = [(LADO, 0.0), (LADO, LADO), (0.0, LADO), (0.0, 0.0)]


def reportar(etiqueta, vehicle, esperado):
    """Imprime la pose de la odometría y, en el simulador, la pose real y el error."""
    odo = vehicle.position
    linea = f"{etiqueta:16s} odometría x={odo.x:+.3f} y={odo.y:+.3f} o={odo.o:+7.1f}°"
    if hasattr(ev3, "medir_pose_real"):  # solo existe en el simulador
        real = ev3.medir_pose_real()
        error = math.hypot(real.x - esperado[0], real.y - esperado[1])
        linea += f" | real x={real.x:+.3f} y={real.y:+.3f} o={real.o:+7.1f}° | error {error * 100:5.1f} cm"
    print(linea)


with ev3.TwoWheelVehicle(
    RADIUS_WHEEL,    # radius_wheel
    TREAD_DISTANCE,  # tread
    protocol=ev3.BLUETOOTH,
    host=MAC,
    speed=VELOCIDAD
) as vehicle:
    for vuelta in range(1, VUELTAS + 1):
        for k, esquina in enumerate(ESQUINAS):
            # Avanzamos un lado y giramos 90° a la izquierda en el sitio
            vehicle.drive_straight(LADO).start(thread=False)
            vehicle.drive_turn(90, 0.0).start(thread=False)
            reportar(f"vuelta {vuelta} esq. {k + 1}", vehicle, esquina)