from machine import Pin, I2C, PWM
from time import sleep_ms
from mpu6050 import MPU6050
from hcsr04 import HCSR04
from neopixel import NeoPixel

i2c = I2C(0, scl=Pin(22), sda=Pin(21), freq=400000)
mpu = MPU6050(i2c)
ultrasonic = HCSR04(trigger_pin=5, echo_pin=18, echo_timeout_us=1000000)
pin_neopixel = Pin(12, Pin.OUT)
np = NeoPixel(pin_neopixel, 16)
buzzer = PWM(14)
buzzer.duty_u16(32768)

red = (255,0,0)
yellow = (255,255,0)
orange = (255,69,0)

def play_tone(frequency, duration_ms):
#Plays a tone at a given frequency for a specified duration.
    buzzer.freq(frequency)
    sleep_ms(duration_ms)
    buzzer.freq(0)

while True:
    accel = mpu.get_accel()
    gyro = mpu.get_gyro()
    distance = ultrasonic.distance_cm()
    
    print("Accelerometer (g): x={:.2f}, y={:.2f}, z={:.2f}".format(*accel))
    print("Gyroscope (°/s): x={:.2f}, y={:.2f}, z={:.2f}".format(*gyro))
    print("Distance = ", distance, "cm")
    print("-" * 50)

    if distance >= 50 and distance < 100:
        np.fill((yellow))
        np.write()
        buzzer.freq(440)
        sleep_ms(200)
        buzzer.freq(1)
        sleep_ms(200)
    elif distance >= 25 and distance < 50:
        np.fill((orange))
        np.write()
        buzzer.freq(523)
        sleep_ms(50)
        buzzer.freq(1)
        sleep_ms(50)
    elif distance < 25:
        np.fill((red))
        np.write()
        buzzer.freq(659)
        sleep_ms(1000)
        buzzer.freq(1)
        sleep_ms(1000)
    
    buzzer.freq(1)
    np.fill((0,0,0))
    np.write()
    sleep_ms(200)