"""Cuadrados con giroscopio.

Parte 1: comparar el ángulo del giroscopio con la orientación estimada por los encoders.
Parte 2: completar girar_con_giroscopio() y poner USAR_GIROSCOPIO = True.
"""
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   # r_efectivo de la calibración
TROCHA = 0.1400  # t_efectiva de la calibración
USAR_GIROSCOPIO = False

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')
x_values, y_values = [0.0], [0.0]
line = ax.plot(x_values, y_values, 'r-', label='odometría')[0]
pos = ax.plot(0.0, 0.0, 'ro')[0]
ax.legend()
fig.canvas.draw()


def plot_curr_pos(curr_pos: ev3.VehiclePosition):
    x_values.append(curr_pos.x)
    y_values.append(curr_pos.y)
    line.set_data(x_values, y_values)
    pos.set_data([curr_pos.x], [curr_pos.y])
    fig.canvas.flush_events()


def girar_con_giroscopio(vehicle, gyro, angulo):
    """Parte 2: girar `angulo` grados (positivo a la izquierda) usando el giroscopio.

    Pistas:
      - vehicle.move(velocidad, 200) gira a la izquierda en el sitio; vehicle.stop() lo detiene.
      - gyro.angle es positivo en sentido HORARIO: un giro a la izquierda lo hace decrecer.
      - Avance la simulación con ev3.sleep(0.01) mientras espera.
    """
    # TODO: reemplazar por un giro controlado con el giroscopio
    vehicle.drive_turn(angulo, 0.0).start(thread=False)


with ev3.EV3(protocol=ev3.BLUETOOTH, host=MAC) as robot:
    gyro = ev3.Gyro(ev3.PORT_1, ev3_obj=robot)
    with ev3.TwoWheelVehicle(
        RADIO,
        TROCHA,
        ev3_obj=robot,
        speed=20,
        tracking_callback=plot_curr_pos,
    ) as vehicle:
        gyro.reset()
        for cuadrado in range(4):
            for lado in range(4):
                vehicle.drive_straight(0.5).start(thread=False)
                if USAR_GIROSCOPIO:
                    girar_con_giroscopio(vehicle, gyro, 90)
                else:
                    vehicle.drive_turn(90, 0.0).start(thread=False)
                giro_gyro = -gyro.angle  # a antihorario, como la odometría
                giro_odo = vehicle.position.o
                print(f"cuadrado {cuadrado + 1}, esquina {lado + 1}: "
                      f"giroscopio {giro_gyro:+5d}°  odometría {giro_odo:+7.1f}°  "
                      f"real {ev3.medir_pose_real().giro_total:+7.1f}°")
        real = ev3.medir_pose_real()
        print(f"Posición final real: x={real.x:+.3f} m, y={real.y:+.3f} m")

plt.ioff()
plt.show()