import time
from machine import Pin, ADC, PWM

light_sensor_1 = ADC(26)
light_sensor_2 = ADC(27)
servo_pwm = PWM(Pin(28, Pin.OUT))
servo_pwm.freq(50)
cur_deg = 90
reset_button = Pin(14, Pin.IN)
calibrate_button = Pin(15, Pin.IN)
cal_threshold = 0
led = Pin(0, Pin.OUT)
led.high()
run_avg = [0, 0, 0, 0, 0]

sensitivity = 10


def servo(degrees):
    if degrees > 180:
        degrees = 180
    if degrees < 0:
        degrees = 0
    max_duty = 2500000
    min_duty = 500000
    new_duty = min_duty + (max_duty - min_duty) * (degrees / 180)
    servo_pwm.duty_ns(int(new_duty))


def shift_array(arr, num_to_add):
    arr.pop(0)
    arr.append(num_to_add)
    return arr


def move_to_light():
    global cur_deg
    is_facing_light = False
    while not is_facing_light:
        offset = light_sensor_1.read_u16() - light_sensor_2.read_u16()
        if offset > (sensitivity - 5) and cur_deg > 0:
            cur_deg = cur_deg - 1
            servo(cur_deg - 5)
        elif offset < -(sensitivity - 5) and cur_deg < 180:
            cur_deg = cur_deg + 1
            servo(cur_deg)
        else:
            is_facing_light = True
        time.sleep(.01)


servo(90)
while True:
    if reset_button.value() == 1:
        led.low()
        cur_deg = 90
        servo(90)
        cal_threshold = 0
        run_avg = [0, 0, 0, 0, 0]
        while reset_button.value() == 1:
            time.sleep(.1)
        led.high()
        continue
    diff = light_sensor_1.read_u16() - light_sensor_2.read_u16()
    run_avg = shift_array(run_avg, diff)
    if calibrate_button.value() == 1:
        cal_threshold = int((light_sensor_1.read_u16() + light_sensor_2.read_u16()) / 2) - 25
        while calibrate_button.value() == 1:
            time.sleep(.1)
    if cal_threshold == 0 or (light_sensor_1.read_u16() > cal_threshold and light_sensor_2.read_u16() > cal_threshold):
        continue
    num_over = 0
    first_iter = True
    sign = True
    for i in run_avg:
        cur_num = i
        if abs(cur_num) > sensitivity:
            if first_iter:
                if cur_num >= 0:
                    sign = True
                else:
                    sign = False
                first_iter = False
            if (cur_num >= 0 and not sign) or (cur_num < 0 and sign):
                num_over = 0
                first_iter = True
                break
            elif (cur_num >= 0 and sign) or (cur_num < 0 and not sign):
                num_over += 1
    if num_over == 5:
        move_to_light()
        run_avg = [0, 0, 0, 0, 0]