from hcsr04 import HCSR04
from time import sleep
from sh1106 import SH1106_I2C
import math
from machine import Pin, PWM, I2C

# Pin assignments (adjust based on your ESP32 board)
trigger_pin = 4
echo_pin = 5
led_pin = 14
servo_pin = 15
oled_pin = 21

# Initialize hardware components
sensor = HCSR04(trigger_pin, echo_pin)
led = Pin(led_pin, Pin.OUT)
servo = PWM(Pin(servo_pin))
i2c = I2C(scl=Pin(oled_pin), sda=Pin(oled_pin + 1))

# Initialize OLED display
oled_width = 128
oled_height = 64
oled = SH1106_I2C(oled_width, oled_height, i2c)

# Set initial servo position
servo.duty(90)

def draw_radar(oled, distance):
    """Draws a radar on the OLED display.

    Args:
        oled: The SH1106_I2C object representing the OLED display.
        distance: The distance measured by the sensor in centimeters.
    """

    oled.fill(0)  # Clear the screen

    # Draw a circle for the radar
    oled.circle(64, 32, 30, 1)

    # Convert distance to a string before passing to oled.text
    distance_str = str(distance) + " cm"
    oled.text(distance_str, 0, 0)  # Update the display

while True:
    distance_cm = sensor.distance_cm()

    # Example interaction: Turn on LED and move servo if distance is close
    if distance_cm < 10:
        led.value(1)
        servo.duty(60)
        draw_radar(oled, distance_cm)
        sleep(1)
    else:
        led.value(0)
        servo.duty(90)  # Or set servo to another position based on your logic

    sleep(0.5)