import ev3_dc as ev3
import keyboard
import matplotlib.pyplot as plt
from thread_task import Periodic

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))

angleList = []  # Lista para almacenar los ángulos
gyroList = []  # Lista para almacenar las velocidades angulares

with ev3.EV3(protocol=ev3.BLUETOOTH, host=MAC) as robot:
    # Conectar el sensor giroscópico en el Puerto 1
    sensor = ev3.Gyro(ev3.PORT_1, ev3_obj=robot)

    def read_sensor():
        gyro = sensor.state
        angleList.append(gyro.angle)
        gyroList.append(gyro.rate)
        print(f"Angulo: {gyro.angle}, Velocidad: {gyro.rate}")

    print("Presiona 'q' para interrumpir el programa")
    sensor_task = Periodic(0.5, read_sensor)
    sensor_task.start()
    while not exit:
        plt.clf()
        plt.title('Ángulo y Velocidad Angular del Giroscopio')  
        plt.plot(angleList, 'b-')
        plt.plot(gyroList, 'r-')
        plt.show(block=False)
        plt.draw()
        plt.pause(1)
    sensor_task.stop()
    print("Programa terminado")
    plt.close()