import board
import pwmio
from adafruit_motor import servo
import digitalio
import time
import busio
import adafruit_vl53l1x
import neopixel
from adafruit_led_animation.animation.rainbow import Rainbow


# Setup NeoPixel strip
NUM_PIXELS = 30  # or however many you have
strip = neopixel.NeoPixel(board.GP9, NUM_PIXELS, brightness=0.3, auto_write=True)
GREEN = (0,255,0)
BLACK = (0,0,0)
BLUE = (0,0,255)

rainbow_strip = Rainbow(strip, speed=0.05, period=2)

#distance sensor
i2c = busio.I2C(board.GP1, board.GP0)
distance_sensor = adafruit_vl53l1x.VL53L1X(i2c)
distance_sensor.start_ranging()


#======== Release Servo =======
pwm = pwmio.PWMOut(board.GP21, frequency=50)
release_servo = servo.Servo(pwm, min_pulse = 750, max_pulse = 2250)

#======== Direction Servos ======= (left and right indicate which side of the chute they will be on
pwm_left = pwmio.PWMOut(board.GP17, frequency=50)
pwm_right = pwmio.PWMOut(board.GP16, frequency=50)
servo_left = servo.Servo(pwm_left, min_pulse=750, max_pulse=2250)
servo_right = servo.Servo(pwm_right, min_pulse=750, max_pulse=2250)
#function for their movment
def move_servos(stop_button):
    # Move servos from 0 to 180 degrees
    for i in range(50, 130):
        servo_left.angle = i
        servo_right.angle = i

        if not stop_button.value:
            print('Button Pressed - Checking distance')

            release_servo.angle = 90
            time.sleep(1.5)  # Lock

            while not stop_button.value:
                pass  # Wait until button released

            release_servo.angle = 0

            # Wait until a valid distance reading
            distance = None
            while distance is None:
                distance = distance_sensor.distance

            # IF CLOSE ENOUGH (<30cm), RETURN IMMEDIATELY
            if distance < 250:
                return value_return(distance)
            else:
                print('Distance too far, continuing movement')

        time.sleep(0.05)

    # Move servos from 180 back to 0 degrees
    for i in reversed(range(50, 130)):
        servo_left.angle = i
        servo_right.angle = i

        if not stop_button.value:
            print('Button Pressed - Checking distance')

            release_servo.angle = 90
            time.sleep(1.5)

            while not stop_button.value:
                pass

            release_servo.angle = 0

            distance = None
            while distance is None:
                distance = distance_sensor.distance

            if distance < 19:
                return value_return(distance)
            else:
                print('Distance too far, continuing movement')

        time.sleep(0.05)

    return None  # If no close press happened, return nothing



def value_return(distance):
    if distance is None:
        return 0
    elif distance < 2:
        return 2
    elif distance < 6:
        return 4
    elif distance < 10:
        return 6
    elif distance < 14:
        return 4
    elif distance < 20:
        return 2
    else:
        return 0

#======== Button Setup ======
player_1 = digitalio.DigitalInOut(board.GP14)
player_1.switch_to_input(pull = digitalio.Pull.UP)


# player_2 = digitalio.DigitalInOut(board.GP15)
# player_2.switch_to_input(digitalio.Pull.UP)

release_servo.angle = 90
lst_1 = [0]
strip.fill(BLACK)
strip.show()


while True:
    print(distance_sensor.distance)
    # # Move at full speed
    value_1 = move_servos(player_1) # move, but stop if player_1 presses the button
    if value_1 is not None:
        lst_1.append(value_1)
    print(lst_1)

    if lst_1 :

        num_pixels = sum(lst_1) # the 1, 2, or 3
        if num_pixels < 30:
            strip[:] = [GREEN if i < num_pixels else BLACK for i in range(len(strip))]
            strip.show()
        else:
            start_time = time.monotonic()
            while time.monotonic() - start_time < 5:  # Pulse for 5 seconds
                rainbow_strip.animate()
                time.sleep(0.05)  # Small delay to slow it down

            strip.fill(BLACK)
            strip.show()
            lst_1 = []