# L13 - Spooroverweg - Erik De Jonghe
from machine import Pin, PWM
from time import sleep, ticks_ms,ticks_diff

DENDER_VALUE = 250
PWM_FREQ = 50
PWM_CYCLE = 1000/PWM_FREQ
PWM_MAX = 1024

# ms
ANGLE_OPEN = 50      # angle -90
ANGLE_CLOSED = 150   # angle 0
DELAY_GATE = 2000 
LED_SPEED_SLOW = 1200
LED_SPEED_FAST = 500
SERVO_STEP = 100

# ESP32 IN & OUT
led_rood_1 = Pin(19, Pin.OUT)
led_rood_2 = Pin(21, Pin.OUT)
led_geel = Pin(18, Pin.OUT)
druk_slagboom = Pin(32, Pin.IN, Pin.PULL_DOWN)
servo_pwm = PWM(Pin(33), freq=50)

# map ranges in - out
def map_range(num, in_min, in_max, out_min, out_max):
    return int((num - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)

# LED on-off with interval
def led_on_off(led, time_interval, ticks_ms_prev):
    if ticks_diff(ticks_ms(), ticks_ms_prev) > time_interval:
        ticks_ms_prev = ticks_ms()
        led.value(not led.value())
    return ticks_ms_prev

# START-STOP BUTTON Function (PULL_DOWN!) push_state = True when pressed
def push_start_stop(push_pin, ticks_ms_prev, push_state_prev):
    push_state = push_pin.value()
    if push_state != push_state_prev:
        if ticks_diff(ticks_ms(), ticks_ms_prev) > DENDER_VALUE:
            ticks_ms_prev = ticks_ms()
            push_state_prev = push_state
    return push_state, ticks_ms_prev, push_state_prev

def control_servo(servo_pin, pos):
    servo_duty = map_range(pos/100, 0, PWM_CYCLE, 0, PWM_MAX)
    servo_pin.duty(servo_duty)

def run_gate(servo_pin, mode, pos, ticks_ms_prev):
    if ticks_diff(ticks_ms(),ticks_ms_prev) > SERVO_STEP:
        ticks_ms_prev = ticks_ms()
        control_servo(servo_pin, pos)
        if mode == "CLOSE":
            if pos < ANGLE_CLOSED:
                pos += 1
        elif mode == "OPEN":
            if pos > ANGLE_OPEN:
                pos -= 1
    return pos, ticks_ms_prev

if __name__ == '__main__':
    push_prev_state = push_prev_time = 0
    led_geel_prev_ms = led_rood_1_prev_ms = led_rood_2_prev_ms = ticks_ms()

    state = 0
    positie = ANGLE_OPEN
    control_servo(servo_pwm, positie)

    while True:
        if state > 0:
            led_rood_1_prev_ms = led_on_off(led_rood_1, LED_SPEED_FAST,led_rood_1_prev_ms)
            led_rood_2_prev_ms = led_on_off(led_rood_2, LED_SPEED_FAST,led_rood_2_prev_ms)
        if state == 0:
            led_geel_prev_ms = led_on_off(led_geel, LED_SPEED_SLOW,led_geel_prev_ms)
            pushed, push_prev_time, push_prev_state = push_start_stop(druk_slagboom, push_prev_time, push_prev_state)
            if pushed:
                led_rood_1.value(1)
                led_rood_2.value(0)
                servo_prev_time = ticks_ms() + DELAY_GATE
                state += 1
        elif state == 1:
            led_geel.value(0)
            gate_mode = "CLOSE"
            positie, servo_prev_time = run_gate(servo_pwm, gate_mode, positie, servo_prev_time)
            if positie >= ANGLE_CLOSED:
                    state += 1
        elif state == 2:
            pushed, push_prev_time, push_prev_state = push_start_stop(druk_slagboom, push_prev_time, push_prev_state)
            if pushed:
                servo_prev_time = ticks_ms()
                positie = ANGLE_CLOSED
                state += 1
        elif state ==3:        
            gate_mode = "OPEN"
            positie, servo_prev_time = run_gate(servo_pwm, gate_mode, positie, servo_prev_time)
            if positie <= ANGLE_OPEN:
                led_rood_1.value(0)
                led_rood_2.value(0)
                state = 0