import time
import board
import busio
import pwmio
import adafruit_vl53l1x
import sdcardio
import storage
from adafruit_motor import servo
from audiopwmio import PWMAudioOut as AudioOut
from audiomp3 import MP3Decoder

# SPI and SD setup
spi = busio.SPI(board.GP10, board.GP11, board.GP12)
cs = board.GP13
sdcard = sdcardio.SDCard(spi, cs)
vfs = storage.VfsFat(sdcard)
storage.mount(vfs, "/sd")

# Audio setup
audio = AudioOut(board.GP15)
path = "/sd/toy_story/"

# Servo setup
pwm_left = pwmio.PWMOut(board.GP7, frequency=50)
pwm_right = pwmio.PWMOut(board.GP8, frequency=50)
woody_servo = servo.Servo(pwm_left)
buzz_servo = servo.Servo(pwm_right)

# Distance sensor
i2c = busio.I2C(board.GP5, board.GP4)
vl53 = adafruit_vl53l1x.VL53L1X(i2c)
vl53.start_ranging()

# Movement speed
sweep_speed = 0.02

# Play MP3 function while moving servos
def play_mp3_with_motion(filename):
    file_path = path + filename
    print("Playing:", file_path)
    with open(file_path, "rb") as mp3_file:
        decoder = MP3Decoder(mp3_file)
        audio.play(decoder)

        # Move while music is playing
        while audio.playing:
            # Sweep forward
            for buzz_angle, woody_angle in zip(range(120, 45, -1), range(50, 121, 1)):
                if not audio.playing:
                    break
                if check_human_nearby():
                    return
                buzz_servo.angle = buzz_angle
                woody_servo.angle = woody_angle
                time.sleep(sweep_speed)

            # Sweep backward
            for buzz_angle, woody_angle in zip(range(45, 121, 1), range(120, 49, -1)):
                if not audio.playing:
                    break
                if check_human_nearby():
                    return
                buzz_servo.angle = buzz_angle
                woody_servo.angle = woody_angle
                time.sleep(sweep_speed)

# Check for human nearby
def check_human_nearby():
    if vl53.data_ready:
        distance_cm = vl53.distance
        vl53.clear_interrupt()
        if distance_cm is not None and distance_cm < 90:
            print("🚨 Human detected at", distance_cm, "cm!")
            # Freeze servos
            woody_servo.angle = 90
            buzz_servo.angle = 90
            # Stop music
            audio.stop()
            time.sleep(0.1)
            # Play "Andy is coming"
            play_mp3_simple('andy_coming.mp3')
            return True
    return False

# Play MP3 simply without motion
def play_mp3_simple(filename):
    file_path = path + filename
    with open(file_path, "rb") as mp3_file:
        decoder = MP3Decoder(mp3_file)
        audio.play(decoder)
        while audio.playing:
            time.sleep(0.1)

# --- Main program ---
try:
    print("Starting Toy Story Scene!")

    while True:
        play_mp3_with_motion('friend_in_me.mp3')

finally:
    vl53.stop_ranging()