from RPi import GPIO
import time
import threading

class UltrasonicSensor:
    def __init__(self, trigger_pin, echo_pin):
        self.trigger_pin = trigger_pin
        self.echo_pin = echo_pin
        self.distance = None
        self._stop_event = threading.Event()

        # Set up GPIO
        GPIO.setmode(GPIO.BCM)
        GPIO.setup(self.trigger_pin, GPIO.OUT)
        GPIO.setup(self.echo_pin, GPIO.IN)

        # Ensure the trigger pin is low initially
        GPIO.output(self.trigger_pin, GPIO.LOW)
        #print("Waiting for sensor to settle...")
        time.sleep(2)
        #print("Sensor settled...")

        # Start the distance measurement thread
        self.thread = threading.Thread(target=self._measure_distance_thread, daemon=True)
        self.thread.start()

    def _measure_distance_thread(self):
        """Thread to continuously measure the distance."""
        while not self._stop_event.is_set():
            self.distance = self._measure_distance()
            time.sleep(0.1)  # Adjust the sleep time as needed

    def _measure_distance(self):
        """Measures the distance using the ultrasonic sensor."""
        # Send a 10us pulse to trigger the measurement
        GPIO.output(self.trigger_pin, True)
        time.sleep(0.00001)
        GPIO.output(self.trigger_pin, False)
        #print("Trigger set")

        # Initialize pulse_start and pulse_end
        pulse_start = time.time()
        pulse_end = time.time()
        #print("Pulse done")

        # Wait for the echo to start
        timeout_start = time.time()
        while GPIO.input(self.echo_pin) == 0:
            pulse_start = time.time()
            if time.time() - timeout_start > 1:
                #print("Timeout waiting for echo start")
                return None  # Timeout

        # Wait for the echo to end
        timeout_start = time.time()
        while GPIO.input(self.echo_pin) == 1:
            pulse_end = time.time()
            if time.time() - timeout_start > 1:
                #print("Timeout waiting for echo end")
                return None  # Timeout

        # Calculate the duration of the pulse
        pulse_duration = pulse_end - pulse_start

        #print("Calculating distance")
        # Calculate the distance in cm
        distance = pulse_duration * 17150
        distance = round(distance, 2)

        #print("Returning distance")
        return distance

    def get_distance(self):
        """Get the last measured distance."""
        return self.distance

    def cleanup(self):
        """Clean up GPIO pins and stop the thread."""
        self._stop_event.set()
        self.thread.join()
        GPIO.cleanup(self.trigger_pin)
        GPIO.cleanup(self.echo_pin)