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)
led = Pin(25, Pin.OUT)
led.high()
run_avg = [0, 0, 0, 0, 0]

light_1_offset = 0
light_2_offset = 0


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():
    print("Moving 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 > 75 and cur_deg > 0:
            cur_deg = cur_deg - 1
            servo(cur_deg)
        elif offset < -75 and cur_deg < 180:
            cur_deg = cur_deg + 1
            servo(cur_deg)
        else:
            is_facing_light = True
        time.sleep(.01)
    print("Moved to light!")


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()
        print("Reset")
        continue
    if calibrate_button.value() == 1:
        sens_1 = light_sensor_1.read_u16()
        sens_2 = light_sensor_2.read_u16()
        if sens_1 > sens_2:
            light_1_offset = sens_1 - sens_2
        elif sens_1 < sens_2:
            light_2_offset = sens_2 - sens_1
        while calibrate_button.value() == 1:
            time.sleep(.1)
    diff = light_sensor_1.read_u16() - light_sensor_2.read_u16() - (light_1_offset - light_2_offset)
    run_avg = shift_array(run_avg, diff)
    num_over = 0
    first_iter = True
    sign = True
    for cur_num in run_avg:
        if abs(cur_num) > 90:
            if first_iter:
                print("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):
                print("Reset")
                num_over = 0
                first_iter = True
                break
            elif (cur_num >= 0 and sign) or (cur_num < 0 and not sign):
                print("+1")
                num_over += 1
    if num_over == 5:
        print("Try move")
        move_to_light()
        run_avg = [0, 0, 0, 0, 0]