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)  

#

#Hulpgeheugens
last_btn_slow_fast_level = 1
last_btn_knipperorange_level = 1

is_wait_long = 1
is_knippering = 0
last_orange_state = 0 # for ornage blinking leds only

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

print("Start sturing kruispunt")  
speed = 400

state = 0
last_state = -1

time_to_wait = 750
time_stamp = 0

time_knipper_interval = 500


def set_lights(r1,o1,g1,r2,o2,g2):
    vl1_rood.value(r1)   
    vl1_oranje.value(o1)     
    vl1_groen.value(g1)     

    vl2_rood.value(r2)       
    vl2_oranje .value(o2)     
    vl2_groen.value(g2)  

# init all lights
set_lights(1,0,0,1,0,0)
 


# Hoofdloop
while True:

    level_btn_knipperorange = btn_knipperorange.value()

    if level_btn_knipperorange != last_btn_knipperorange_level:

        last_btn_knipperorange_level = level_btn_knipperorange
        if level_btn_knipperorange == 0:
            is_knippering = not is_knippering
            print("is_knippering is  now " , is_knippering)  
            if  is_knippering == 1:
                set_lights(0,0,0,0,0,0)
            else:
                set_lights(1,0,0,1,0,0)   
                state = 0 
                last_state = -1
                time_stamp = time.ticks_ms()   
        time.sleep(0.005)

    if is_knippering and time.ticks_ms() - time_stamp > 500:
        time_stamp = time.ticks_ms()
        last_orange_state = not last_orange_state
        vl1_oranje.value(last_orange_state)
        vl2_oranje.value(last_orange_state)   

    else:

        level_btn_slow_fast = btn_slow_fast.value()

        if level_btn_slow_fast != last_btn_slow_fast_level:
            last_btn_slow_fast_level = level_btn_slow_fast
            if level_btn_slow_fast == 0:
                is_wait_long = not is_wait_long
                print("speed fast now " , time_to_wait + time_to_wait * is_wait_long)
                if is_wait_long:
                    led_slow.value(1)
                    led_fast.value(0)
                else:
                    led_slow.value(0)
                    led_fast.value(1)

            time.sleep(0.005)

        if time.ticks_ms() - time_stamp > time_to_wait + time_to_wait * is_wait_long:
            time_stamp = time.ticks_ms()
            if state != last_state:
                last_state = state
                print("New state=", state)

                if  state == 0:
                    vl2_oranje.value(0) 
                    vl1_rood.value(1)
                    vl2_rood.value(1)
                elif  state == 1:
                    vl1_rood.value(0)
                    vl1_groen.value(1) 
                elif state == 2:
                    vl1_groen.value(0)  
                    vl1_oranje.value(1)  
                elif state == 3:
                    vl1_oranje.value(0)    
                    vl1_rood.value(1)         
                elif state == 4:
                    vl2_rood.value(0)
                    vl2_groen.value(1)  
                elif state == 5:
                    vl2_groen.value(0)   
                    vl2_oranje.value(1) 
                  

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