import time
import RPi.GPIO as GPIO

trigger_pin = 27
echo_pin = 22
speed_of_sound_mps = 340 # in m/s

rising_found = False
rising_time = 0
falling_found = False
falling_time = 0

class Ultrasonic:

    @staticmethod
    def __send_trigger():
        GPIO.setup(trigger_pin, GPIO.OUT)
        GPIO.output(trigger_pin, False) # Ensure a clean transition
        time.sleep(0.000005)
        GPIO.output(trigger_pin, True)  # Trigger now
        time.sleep(0.000010)
        GPIO.output(trigger_pin, False)

    @staticmethod
    def __signal_start_callback(channel):
        global rising_found
        global rising_time
        rising_found = True
        rising_time = time.process_time_ns()

    @staticmethod
    def __signal_end_callback(channel):
        global falling_found
        global falling_time
        falling_found = True
        falling_time = time.process_time_ns()

    @staticmethod
    def __measure_pulse(timeout = 1000000) -> int:
        """ Measure the length of a pulse in us. When either the pulse doesn't start in [timeout] us
            or the pulse is longer than [timeout] us, 0 is returned.

        :param timeout: Timeout in us
        :return: The length of the pulse or 0 on timeout
        :rtype: int
        """

        global rising_found
        global rising_time
        global falling_found
        global falling_time

        # Setup pin
        GPIO.setup(echo_pin, GPIO.IN)

        # Wait for the rising edge
        rising_found = False
        GPIO.add_event_detect(echo_pin, GPIO.RISING, callback = Ultrasonic.__signal_start_callback)
        start = time.process_time_ns()
        while (time.process_time_ns() < start + timeout * 1000 and not rising_found):
            pass

        GPIO.remove_event_detect(echo_pin)
        if (not rising_found):
            return 0

        # Wait for the falling edge
        falling_found = False
        GPIO.add_event_detect(echo_pin, GPIO.FALLING, callback = Ultrasonic.__signal_end_callback)
        start = time.process_time_ns()
        while (time.process_time_ns() < start + timeout * 1000 and not falling_found):
            pass

        GPIO.remove_event_detect(echo_pin)
        if (not falling_found):
            return 0

        # Finally, return the time difference
        return int((falling_time - rising_time) / 1000)
    
    @staticmethod
    def get_distance() -> float:
        """ Get the distance from the Ultrasonic sensor in cm. 
            Returns 'NaN' when the result is invalid.

        :return: The distance in cm or 'NaN'
        :rtype: float
        """
        GPIO.setmode(GPIO.BCM)

        # Trigger the measurement
        Ultrasonic.__send_trigger()

        # The pulse length in us
        pulse = Ultrasonic.__measure_pulse()

        if (pulse == 0):
            print("invalid")
            return float('NaN')

        # Convert the speed of sound from m/s to cm/us for convenience
        speed_of_sound_cmpus = speed_of_sound_mps / 10000

        # Convert us time to cm distance
        distance = (pulse / 2) * speed_of_sound_cmpus

        return distance