import time
import board
import busio
import adafruit_vl53l0x
import pyqtgraph as pg
from PyQt5.QtWidgets import QApplication
from PyQt5.QtCore import QThread, QTimer, pyqtSignal

timestamps = []
distances = []

i2c = busio.I2C(board.SCL, board.SDA)
sensor = adafruit_vl53l0x.VL53L0X(i2c)

class SensorThread(QThread):
    data_ready = pyqtSignal(float, float)

    def run(self):
        try:
            while True:
                start_time = time.monotonic()

                # Take a measurement
                distance = sensor.range - 15

                # Get current timestamp
                timestamp = time.monotonic()

                # Emit the data
                self.data_ready.emit(timestamp, distance)

                # Calculate the elapsed time
                elapsed_time = time.monotonic() - start_time

                # Delay between measurements
                #time.sleep(0.01)  # Adjust the delay as needed

        except KeyboardInterrupt:
            pass

def update_plot(timestamp, distance):
    timestamps.append(timestamp)
    distances.append(distance)

    # Convert timestamps to milliseconds
    timestamps_ms = [ts - timestamps[0] for ts in timestamps]
    curve.setData(timestamps_ms, distances)

    # Adjust x-axis range to display only the latest data
    x_range = max(timestamps_ms) - (max(timestamps_ms) - 1)
    plot.setXRange(max(timestamps_ms) - x_range, max(timestamps_ms))

sensor_thread = SensorThread()
sensor_thread.data_ready.connect(update_plot)
sensor_thread.start()

app = QApplication([])

win = pg.GraphicsLayoutWidget(title="Real-time Distance Plot")
plot = win.addPlot()
plot.setLabel('left', 'Distance (mm)')
plot.setLabel('bottom', 'Time (ms)')
y_range = 60
plot.setRange(yRange=(0, y_range))
curve = plot.plot(pen='b')

timer = QTimer()
timer.timeout.connect(lambda: None)  # Empty function to keep the event loop running
timer.start(0)

win.show()  # Show the plot window

app.exec_()