"""Cuadrados con encoder: 4 cuadrados de 0.5 m usando drive_straight y drive_turn."""
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

with ev3.TwoWheelVehicle(
    0.0152,  # radius_wheel_measured
    0.1400,  # tread_measured
    protocol=ev3.BLUETOOTH,
    host=MAC,
    speed=20,
) as vehicle:
    lado = vehicle.drive_straight(0.5) + vehicle.drive_turn(90, 0.0)
    cuadrado = lado + lado + lado + lado
    for i in range(4):
        cuadrado.start(thread=False)
        odo = vehicle.position
        real = ev3.medir_pose_real()
        print(f"Cuadrado {i + 1}: odometría x={odo.x:+.3f} y={odo.y:+.3f} o={odo.o:+6.1f}° | "
              f"real x={real.x:+.3f} y={real.y:+.3f} o={real.o:+6.1f}°")