from machine import Pin
import time

# Verkeerslichten

vl1_rood = Pin(25, Pin.OUT)    
vl1_oranje = Pin(26, Pin.OUT)  
vl1_groen = Pin(27, Pin.OUT)   

vl2_rood = Pin(21, Pin.OUT)      
vl2_oranje = Pin(19, Pin.OUT)    
vl2_groen = Pin(16, Pin.OUT)   

# Drukknoppen
btn_slow_fast = Pin(12, Pin.IN, Pin.PULL_UP)     
btn_knipperorange = Pin(4, Pin.IN, Pin.PULL_UP)   

# Status LEDs
led_slow = Pin(18, Pin.OUT)      
led_fast = Pin(17, Pin.OUT)   

print("Start sturing kruispunt")  
speed = 400

state = 0
last_state = -1

time_to_wait = 2500
time_stamp = 0

# init all lights
vl1_rood.value(1)   
vl1_oranje.value(0)     
vl1_groen.value(0)     

vl2_rood.value(1)       
vl2_oranje .value(0)     
vl2_groen.value(0)  

'''
R1  1   0   0   1   1   1   (1)
O1  0   0   1   0   0   0   (0)
G1  0   1   0   0   0   0   (0)

R2  1   1   1   1   0   0   (1)
O2  0   0   0   0   0   1   (0)
G2  0   0   0   0   1   0   (0) 
    0   1   2   3   4   5   (6=0 ) 
'''

# Hoofdloop
while True:

    if time.ticks_ms() - time_stamp > time_to_wait:
        time_stamp = time.ticks_ms()
        if state != last_state:
            last_state = state
            print("New state=", state)
            if  state == 0:
                vl2_oranje .value(0) 
                vl2_rood.value(1)
                time_to_wait = 2500  
            elif state == 1:
                vl1_rood.value(0)
                vl1_groen.value(1)   
            elif state == 2:
                vl1_groen.value(0)  
                vl1_oranje.value(1) 
                time_to_wait = 500         
            elif state == 3:
                vl1_oranje.value(0)  
                vl1_rood.value(1)
                time_to_wait = 2500                 
            elif state == 4:
                vl2_rood.value(0)
                vl2_groen.value(1)  
            elif state == 5:
                vl2_groen.value(0)   
                vl2_oranje .value(1) 
                time_to_wait = 500  

            state += 1
            if state > 5:
                state = 0