"""Cuadrados por tiempo: 4 cuadrados de 0.5 m, cambiando entre avanzar y girar por tiempo."""
import os
import sys

sys.path.insert(0, os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "..", "python"))
import ev3_sim as ev3  # en el robot real: import ev3_dc as ev3

MAC = '00:16:53:50:8A:86'  # se ignora en el simulador

VELOCIDAD = 20   # % de la velocidad máxima
T_RECTO = 9.40   # s para recorrer 0.5 m   (ajustar)
T_GIRO = 2.07    # s para girar 90° en el sitio (ajustar)
T_PAUSA = 0.3    # s detenido entre movimientos

with ev3.TwoWheelVehicle(
    0.0152,  # radius_wheel (no se usa para mover por tiempo, solo para la odometría)
    0.1400,  # tread
    protocol=ev3.BLUETOOTH,
    host=MAC,
) as vehicle:
    for cuadrado in range(4):
        for lado in range(4):
            vehicle.move(VELOCIDAD, 0)      # recto
            ev3.sleep(T_RECTO)
            vehicle.stop()
            ev3.sleep(T_PAUSA)
            vehicle.move(VELOCIDAD, 200)    # giro a la izquierda en el sitio
            ev3.sleep(T_GIRO)
            vehicle.stop()
            ev3.sleep(T_PAUSA)
        real = ev3.medir_pose_real()
        print(f"Cuadrado {cuadrado + 1}: posición real x={real.x:+.3f} m, y={real.y:+.3f} m, "
              f"orientación {real.o:+.1f}°")