import RPi.GPIO as GPIO  # Import GPIO library
import time  # Import time library

GPIO.setmode(GPIO.BCM)  # Set GPIO pin numbering

TRIG = 23  # Associate pin 23 to TRIG
ECHO = 24  # Associate pin 24 to ECHO
LED1 = 9
LED2 = 16
LED3 = 6

GPIO.setup(LED1, GPIO.OUT)
GPIO.setup(LED2, GPIO.OUT)
GPIO.setup(LED3, GPIO.OUT)
GPIO.setup(TRIG, GPIO.OUT)  # Set pin as GPIO out
GPIO.setup(ECHO, GPIO.IN)  # Set pin as GPIO in


# Will return distance, if error -1
def check_distance():
    GPIO.output(TRIG, False)  # Set TRIG as LOW
    GPIO.output(TRIG, True)  # Set TRIG as HIGH
    time.sleep(0.00001)  # Delay of 0.00001 seconds
    GPIO.output(TRIG, False)  # Set TRIG as LOW

    while GPIO.input(ECHO) == 0:  # Check whether the ECHO is LOW
        pulse_start = time.time()  # Saves the last known time of LOW pulse

    while GPIO.input(ECHO) == 1:  # Check whether the ECHO is HIGH
        pulse_end = time.time()  # Saves the last known time of HIGH pulse

    pulse_duration = pulse_end - pulse_start  # Get pulse duration to a variable

    distance = pulse_duration * 17150  # Multiply pulse duration by 17150 to get distance
    distance = round(distance, 2)  # Round to two decimal points

    if 2 < distance < 400:  # Check whether the distance is within range
        # print("Distance:",distance - 0.5,"cm")  #Print distance with 0.5 cm calibration
        return distance - 0.5
    else:
        # print("Out Of Range")                   #display out of range
        return -1


def light_on(led):
    GPIO.output(led, GPIO.HIGH)


def light_off(led):
    GPIO.output(led, GPIO.LOW)


def main():
    while True:
        if 400 >= check_distance() > 250:
            light_on(LED1)
            light_off(LED2)
            light_off(LED3)
        elif 250 >= check_distance() > 150:
            light_on(LED2)
            light_off(LED1)
            light_off(LED3)
        elif 150 >= check_distance() > 2:
            light_on(LED3)
            light_off(LED1)