from machine import Pin, Timer
import time

# Configuratie van TRIG- en ECHO-pinnen
trig_pins = [2, 4, 6, 8]   # TRIG-pinnen
echo_pins = [3, 5, 7, 9]   # ECHO-pinnen
trig = [Pin(pin, Pin.OUT) for pin in trig_pins]
echo = [Pin(pin, Pin.IN) for pin in echo_pins]

# Configuratie van het waarschuwingssignaal
alert_pin = Pin(15, Pin.OUT)  # GPIO 15 voor signaal (bijv. LED)

# Variabelen
distances = [0] * 4
threshold = 10  # Drempel in cm
obstacle_detected = [False] * 4  # Voor elke sensor een detectie-status
start_time = [-1] * 4  # Tijdstempel voor elke sensor

# TRIG-puls genereren
def send_pulse(idx):
    trig[idx].low()
    time.sleep_us(2)
    trig[idx].high()
    time.sleep_us(10)
    trig[idx].low()

# ECHO-afhandeling
def handle_echo(pin):
    for idx in range(4):
        if pin == echo[idx]:
            if pin.value() == 1:  # Rising edge detected
                start_time[idx] = time.ticks_us()
            else:  # Falling edge detected
                if start_time[idx] != -1:  # Valid start time
                    duration = time.ticks_diff(time.ticks_us(), start_time[idx])
                    distances[idx] = (duration / 2) / 29.1  # Afstand in cm

                    # Obstakeldetectie
                    if distances[idx] < threshold:
                        obstacle_detected[idx] = True
                        alert_pin.high()  # Zet het signaal hoog
                    else:
                        obstacle_detected[idx] = False
                        if not any(obstacle_detected):  # Alleen uitschakelen als geen enkele sensor detecteert
                            alert_pin.low()
                start_time[idx] = -1  # Reset start time

# Timer-callback
def periodic_trigger(timer):
    for i in range(4):
        send_pulse(i)

# Setup
for t in trig:
    t.low()  # Zorg dat TRIG laag begint

for i in range(4):
    echo[i].irq(trigger=Pin.IRQ_RISING | Pin.IRQ_FALLING, handler=handle_echo)

# Timer-initialisatie
measurement_timer = Timer()
measurement_timer.init(period=100, mode=Timer.PERIODIC, callback=periodic_trigger)

# Hoofdprogramma
while True:
    detected_sensors = [i + 1 for i, detected in enumerate(obstacle_detected) if detected]
    if detected_sensors:
        print(f"Obstakel gedetecteerd door sensor(en): {', '.join(map(str, detected_sensors))}")
        print(f"Afstanden (cm): {distances}")
    else:
        print("Geen obstakel. Afstanden (cm):", distances)
    time.sleep(0.5)  # Andere taken kunnen hier worden uitgevoerd