from machine import Pin, PWM
import time
from hcsr04 import HCSR04

# Initialize ultrasonic sensor
ultrasonic = HCSR04(trig_pin=4, echo_pin=5)

# Initialize buzzer
buzzer_pin = Pin(12, Pin.OUT)
buzzer_pwm = PWM(buzzer_pin)

# Parking sensor settings
WARNING_DISTANCE = 70  # cm (distance at which warnings start)
MAX_BEEP_INTERVAL = 1.0  # seconds (slowest beep interval)
MIN_BEEP_INTERVAL = 0.1  # seconds (fastest beep interval)
BEEP_DURATION = 0.1  # seconds
BEEP_FREQUENCY = 1000  # Hz

def play_beep():
    buzzer_pwm.freq(BEEP_FREQUENCY)
    buzzer_pwm.duty(512)  # 50% duty cycle
    time.sleep(BEEP_DURATION)
    buzzer_pwm.duty(0)  # Turn off

def calculate_beep_interval(distance):
    if distance >= WARNING_DISTANCE:
        return None  # No beeping
    
    # Map distance to beep interval (closer = faster beeps)
    distance = max(0, min(WARNING_DISTANCE, distance))
    ratio = distance / WARNING_DISTANCE
    interval = MIN_BEEP_INTERVAL + (MAX_BEEP_INTERVAL - MIN_BEEP_INTERVAL) * ratio
    return interval

try:
    while True:
        distance = ultrasonic.measure_distance()
        print(f"Distance: {distance} cm")
        
        if distance < 0:
            print("Measurement error")
            time.sleep(0.1)
            continue
            
        beep_interval = calculate_beep_interval(distance)
        
        if beep_interval is None:
            # Distance > 70cm - no beeping
            buzzer_pwm.duty(0)
            time.sleep(0.1)
        else:
            # Play beep and wait according to interval
            play_beep()
            time.sleep(beep_interval - BEEP_DURATION)
            
except KeyboardInterrupt:
    print("Program stopped")
finally:
    buzzer_pwm.deinit()