import time
from machine import Pin, ADC, PWM

light_sensor_1 = ADC(26)  # Side with servo on left
light_sensor_2 = ADC(27)  # Side with servo on right
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('WL_GPIO0', Pin.OUT)
led.high()

run_avg = [0, 0, 0, 0, 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)
    print("Adjusted Position: ", degrees)
    servo_pwm.duty_ns(int(new_duty))


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


# If this quits prematurely, add required buffer in final else statement
def move_to_light():
    print("Init 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 > 100 and cur_deg > 0:
            cur_deg = cur_deg - 1
            servo(cur_deg)
        elif offset < -100 and cur_deg < 180:
            cur_deg = cur_deg + 1
            servo(cur_deg)
        else:
            is_facing_light = True
        print("Current Degrees in move to light: ", cur_deg)
        time.sleep(.01)


# Deg+ = More Light to Right
# Deg- = More Light to Left
# Lower number = more 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()
    diff = light_sensor_1.read_u16() - light_sensor_2.read_u16()
    run_avg = shift_array(run_avg, diff)
    if calibrate_button.value() == 1:
        print("Button is being pressed")
        cal_threshold = int((light_sensor_1.read_u16() + light_sensor_2.read_u16()) / 2) - 50
        print("Sensor 1 Val: ", light_sensor_1.read_u16())
        print("Sensor 2 Val: ", light_sensor_2.read_u16())
        while calibrate_button.value() == 1:
            time.sleep(.1)
        print("New Threshold: ", cal_threshold)
    if 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
        print("Cur Num:", cur_num)
        if abs(cur_num) > 150:
            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):
                print("Hit")
                num_over += 1
    print("Num Over: ", num_over)
    print("Sign: ", sign)
    if num_over == 5:
        move_to_light()
        run_avg = [0, 0, 0, 0, 0]