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

distanceList = []  # Lista para almacenar las distancias

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

    def read_sensor():
        distance = sensor.distance 
        distance = 10.0*distance if isinstance(distance, float) else distance
        distanceList.append(distance)
        print(f"Distancia: {distance} cm")

    print("Presiona 'q' para interrumpir el programa")
    sensor_task = Periodic(0.5, read_sensor)
    sensor_task.start()
    while not exit:
        plt.clf()
        plt.title('Distancia del Sensor Ultrasónico')
        plt.plot(distanceList, 'r-')
        plt.show(block=False)
        plt.draw()
        plt.pause(1)
    sensor_task.stop()
    print("Programa terminado")