import ev3_dc as ev3
import keyboard
import matplotlib.pyplot as plt
import numpy as np
from pyparsing import line

MAC = '00:16:53:50:8A:86'  # Cambia esto por la dirección MAC de tu ladrillo EV3

exit = False
keyboard.add_hotkey('q', lambda: globals().update(exit=True))

indice = -1
indice_incremento = 1

angle_list = np.arange(-90, 91, 5)  # Lista para almacenar los ángulos
radian_list = np.deg2rad(angle_list)  # Convertir ángulos a radianes
# Lista para almacenar las distancias
distance_list = np.zeros_like(angle_list)

plt.ion()
fig, ax = plt.subplots(subplot_kw={'projection': 'polar'})
ax.figure.set_size_inches(5, 5)
ax.grid(True)

ax.set_rgrids(np.arange(0, 255, 25))  # Set specific radial grid lines
# Set specific angular grid lines (in degrees)
ax.set_thetagrids([-90, -60, -30, 0, 30, 60, 90])
ax.set_rlim(0, 50)  # Set radial limits
ax.set_thetamin(-90)  # Minimum theta value
ax.set_thetamax(90)  # Maximum theta value
ax.set_theta_zero_location("N")  # Set the zero angle to the top (North)
ax.set_theta_direction(-1)  # Set the angular direction to clockwise

radar = ax.plot(radian_list, distance_list, '.-', color='k')[0]
radar_fill = ax.fill_between(
    radian_list, 255, distance_list, color='#f7e600', alpha=1)
fig.canvas.draw()

with ev3.EV3(protocol=ev3.BLUETOOTH, host=MAC) as robot:
    # Conectar el sensor ultrasónico en el Puerto 2
    sensor = ev3.Ultrasonic(ev3.PORT_2, ev3_obj=robot)
    motor = ev3.Motor(ev3.PORT_B, ev3_obj=robot)
    motor.move_to(int(angle_list[0]), brake=True).start(thread=False)

    print("Presiona 'q' para interrumpir el programa")
    while not exit:

        indice = indice + indice_incremento
        if indice >= len(angle_list) - 1:
            indice_incremento = -1
        elif indice <= 0:
            indice_incremento = 1

        motor.move_to(int(angle_list[indice]), brake=True).start(thread=False)

        distance = sensor.distance
        distance = 10.0*distance if isinstance(distance, float) else distance
        distance = distance
        distance_list[indice] = distance

        radar.set_ydata(distance_list)
        radar_fill.set_data(radian_list, 255, distance_list)
        max_distance = max(max([d for d in distance_list if d <= 250]), 40)
        # Set radial limits to nearest 10 multiple
        ax.set_rlim(0, ((max_distance // 10) + 1) * 10)
        fig.canvas.flush_events()

        plt.pause(0.25)

    motor.stop(brake=False)
    plt.ioff()
    fig.savefig('radar.png',
                dpi=300,
                bbox_inches='tight',
                transparent=False)
    print("Programa terminado")