"""Calibración del radio de las ruedas: avanzar 2 m y medir la distancia real."""
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

DISTANCIA = 2.0  # m

with ev3.TwoWheelVehicle(
    0.0152,  # radius_wheel_measured  -> reemplazar por r_efectivo
    0.1400,  # tread
    protocol=ev3.BLUETOOTH,
    host=MAC,
) as vehicle:
    vehicle.drive_straight(DISTANCIA, speed=20).start(thread=False)

    # En el laboratorio se mide con una regla; en el simulador se lee la pose real
    real = ev3.medir_pose_real()
    print(f"Distancia pedida: {DISTANCIA:.3f} m")
    print(f"Distancia real:   {real.distancia:.4f} m  (desvío lateral y = {real.y:+.4f} m, "
          f"orientación final {real.o:+.2f}°)")