"""Cuadrados calibrados: 4 cuadrados de 0.5 m con r y t calibrados, mostrando la trayectoria.

Rojo: trayectoria estimada por odometría (lo que "cree" el robot).
Azul: trayectoria real (solo disponible en el simulador).
"""
import os
import sys

import matplotlib.pyplot as plt

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

RADIO = 0.0152   # reemplazar por r_efectivo
TROCHA = 0.1400  # reemplazar por t_efectiva

plt.ion()
fig, ax = plt.subplots()
ax.figure.set_size_inches(5, 5)
ax.grid(True)
ax.set_xlim([-0.2, 0.7])
ax.set_ylim([-0.2, 0.7])
ax.set_aspect('equal')

odo_x, odo_y = [0.0], [0.0]
real_x, real_y = [0.0], [0.0]
line_odo = ax.plot(odo_x, odo_y, 'r-', label='odometría')[0]
line_real = ax.plot(real_x, real_y, 'b-', label='real')[0]
pos = ax.plot(0.0, 0.0, 'ro')[0]
ax.legend()
fig.canvas.draw()


def plot_curr_pos(curr_pos: ev3.VehiclePosition):
    '''actualiza la posición en la gráfica'''
    odo_x.append(curr_pos.x)
    odo_y.append(curr_pos.y)
    real = ev3.medir_pose_real()
    real_x.append(real.x)
    real_y.append(real.y)
    line_odo.set_data(odo_x, odo_y)
    line_real.set_data(real_x, real_y)
    pos.set_data([curr_pos.x], [curr_pos.y])
    fig.canvas.flush_events()


with ev3.TwoWheelVehicle(
    RADIO,
    TROCHA,
    protocol=ev3.BLUETOOTH,
    host=MAC,
    speed=20,
    tracking_callback=plot_curr_pos,
) 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)
        real = ev3.medir_pose_real()
        print(f"Cuadrado {i + 1}: error de posición real = {real.distancia * 100:.1f} cm, "
              f"orientación {real.o:+.1f}°")

plt.ioff()
fig.savefig('cuadrados-calibrados.png', dpi=150, bbox_inches='tight')
plt.show()