from time import sleep_ms
from machine import I2C, Pin, UART, PWM
from i2c_lcd import I2cLcd
from hcsr04 import HCSR04
import network
import time
import dht
import ujson
from umqtt.simple import MQTTClient

# MQTT Server Parameters
MQTT_CLIENT_ID = "micropython-distance-demo-shaiful"
MQTT_BROKER    = "broker.mqttdashboard.com"
MQTT_USER      = ""
MQTT_PASSWORD  = ""
MQTT_TOPIC     = "wokwi-distance/shaiful"

print("Connecting to WiFi", end="")
sta_if = network.WLAN(network.STA_IF)
sta_if.active(True)
sta_if.connect('Wokwi-GUEST', '')
while not sta_if.isconnected():
  print(".", end="")
  time.sleep(0.1)
print(" Connected!")

print("Connecting to MQTT server... ", end="")
client = MQTTClient(MQTT_CLIENT_ID, MQTT_BROKER, user=MQTT_USER, password=MQTT_PASSWORD)
client.connect()
print("Connected!")

# Define I2C address of the LCD
lcd_address = 0x27

# Initialize I2C bus
i2c = I2C(scl=Pin(22), sda=Pin(21), freq=400000)  # SCL Pin 22, SDA Pin 21

# Initialize LCD
lcd = I2cLcd(i2c, lcd_address, 2, 16)  # 2 lines, 16 characters per line

# Initialize HC-SR04 Sensor (Trigger Pin, Echo Pin)
sensor = HCSR04(trigger_pin=14, echo_pin=13)  # Example GPIOs, adjust as needed

# Initialize UART for serial display
uart = UART(1, baudrate=9600, tx=Pin(1), rx=Pin(3))  # tx and rx pins can be adjusted based on your ESP32 setup

# Initialize Servo
servo_pin = 12  # Example GPIO, adjust as needed
servo = PWM(Pin(servo_pin), freq=50)  # Initialize PWM on the servo pin with a frequency of 50Hz

def read_distance():
    try:
        distance = sensor.distance_cm()
        return distance
    except OSError:
        return None

def display_data_on_lcd(distance):
    lcd.clear()
    lcd.move_to(0, 0)
    lcd.putstr("Distance (cm):")
    lcd.move_to(0, 1)
    if distance is not None:
        lcd.putstr("{:.2f}".format(distance))
    else:
        lcd.putstr("Error")

def send_data_over_uart(distance):
    if distance is not None:
        uart.write("Distance: {:.2f} cm\r\n".format(distance))
    else:
        uart.write("Error reading distance\r\n")

def send_data_over_mqtt(distance):
    if distance is not None:
        try:
            client.publish(MQTT_TOPIC, ujson.dumps({"distance_cm": distance}))
        except Exception as e:
            print("Error publishing to MQTT:", e)

def control_servo(distance):
    if distance is not None:
        if distance < 100:  # Activate servo if distance is below 100cm
            servo.duty(115)  # Duty cycle for desired position
        else:
            servo.duty(77)  # Duty cycle for default position
    else:
        servo.duty(0)  # Turn off the servo if distance reading fails

def main():
    while True:
        distance = read_distance()
        display_data_on_lcd(distance)
        send_data_over_uart(distance)
        control_servo(distance)
        send_data_over_mqtt(distance)  # New line to send data over MQTT
        sleep_ms(500)  # Update display and servo control every 500 milliseconds

if __name__ == "__main__":
    main()