from machine import Pin, PWM, I2C
import ssd1306
import time
import network
from umqtt.simple import MQTTClient
import ujson

# Wi-Fi and MQTT Config
WIFI_SSID = "Wokwi-GUEST"
MQTT_BROKER = "broker.hivemq.com"
MQTT_PORT = 8883
ALERT_TOPIC = "car/alert"
DISTANCE_TOPIC = "car/distance"

# Hardware Setup
trigger = Pin(5, Pin.OUT)
echo = Pin(18, Pin.IN)
led = Pin(4, Pin.OUT)  # LED moved to pin 4
buzzer = PWM(Pin(23))  # Use PWM for the buzzer (moved to pin 23)
servo = PWM(Pin(19), freq=50)  # Servo motor
servo.duty(0)  # Initialize servo to 0 degrees

# OLED Setup
i2c = I2C(scl=Pin(22), sda=Pin(21))  # I2C for OLED (unchanged)
oled = ssd1306.SSD1306_I2C(128, 64, i2c)

# Wi-Fi Connection
def connect_wifi():
    wlan = network.WLAN(network.STA_IF)
    wlan.active(True)
    wlan.connect(WIFI_SSID)
    while not wlan.isconnected():
        print("Connecting to WiFi...")
        time.sleep(1)
    print("WiFi Connected!")

# Measure Distance
def measure_distance():
    trigger.on()
    time.sleep_us(10)
    trigger.off()
    while echo.value() == 0:
        pass
    t1 = time.ticks_us()
    while echo.value() == 1:
        pass
    t2 = time.ticks_us()
    distance = (t2 - t1) * 0.0343 / 2  # Distance in cm
    return distance

# MQTT Connection
def mqtt_connect():
    client = MQTTClient("ESP32_Car", MQTT_BROKER, MQTT_PORT, ssl=True)  # No username/password
    client.connect()
    print("MQTT Connected!")
    return client

# Function to Play a Tone
def play_tone(frequency, duration):
    buzzer.freq(frequency)  # Set frequency (in Hz)
    buzzer.duty(512)  # 50% duty cycle
    time.sleep(duration)
    buzzer.duty(0)  # Turn off the buzzer

# Servo Control
def set_servo_angle(angle):
    duty = int((angle / 180.0) * 102 + 26)  # Map angle to duty cycle (26-128)
    servo.duty(duty)

# Main Loop
connect_wifi()
client = mqtt_connect()

while True:
    distance = measure_distance()
    print("Distance:", distance, "cm")

    # Display distance on OLED
    oled.fill(0)  # Clear screen
    oled.text("Distance:", 0, 0)
    oled.text(f"{distance:.2f} cm", 0, 20)
    
    if distance < 10:  # Collision warning threshold
        led.on()
        oled.text("Collision Alert!", 0, 40)
        set_servo_angle(90)  # Activate servo (e.g., block path)
        
        print("Collision Alert! Playing tone...")
        play_tone(1000, 0.5)
        time.sleep(0.2)
        play_tone(1200, 0.5)
        
        alert_msg = ujson.dumps({"status": "Collision Alert!", "distance": distance})
        client.publish(ALERT_TOPIC, alert_msg)
    else:
        led.off()
        set_servo_angle(0)  # Reset servo position

    # Publish distance updates to MQTT
    distance_msg = ujson.dumps(distance)
    client.publish(DISTANCE_TOPIC, distance_msg)
    
    oled.show()
    time.sleep(1)