print("Hello, Pi Pico W!")

# =========================
# IMPORTS
# =========================
from machine import Pin, I2C
import utime, time
import network
from umqttsimple import MQTTClient
from imu import MPU6050
from servo import Servo


# =========================
# WIFI CONNECTION
# =========================
print("Connecting to WiFi", end="")
wlan = network.WLAN(network.STA_IF)
wlan.active(True)
wlan.connect("Wokwi-GUEST", "")

while not wlan.isconnected():
    print(".", end="")
    time.sleep(0.1)

print("\nWiFi Connected!")
print(wlan.ifconfig())


# =========================
# MQTT (NO SSL)
# =========================
MQTT_CLIENT_ID = "picow-01"
MQTT_BROKER    = "broker.emqx.io"
MQTT_TOPIC     = "wokwi"

client = MQTTClient(MQTT_CLIENT_ID, MQTT_BROKER, port=1883)
client.connect()
print("MQTT Connected!")


# =========================
# MPU6050 SETUP
# =========================
i2c = I2C(0, sda=Pin(4), scl=Pin(5), freq=400000)
imu = MPU6050(i2c)

def read_imu():
    ax = round(imu.accel.x, 2)
    ay = round(imu.accel.y, 2)
    az = round(imu.accel.z, 2)
    temp = round(imu.temperature, 2)
    return ax, ay, az, temp


# =========================
# RELAY SETUP
# =========================
relay = Pin(16, Pin.OUT)


# =========================
# ULTRASONIC SENSOR
# =========================
trigger = Pin(26, Pin.OUT)
echo = Pin(27, Pin.IN)

def read_ultrasonic():
    trigger.low()
    utime.sleep_us(2)
    trigger.high()
    utime.sleep_us(10)
    trigger.low()

    while echo.value() == 0:
        signaloff = utime.ticks_us()

    while echo.value() == 1:
        signalon = utime.ticks_us()

    timepassed = signalon - signaloff
    distance = (timepassed * 0.0343) / 2
    return distance


# =========================
# PIR SENSOR
# =========================
pir = Pin(0, Pin.IN)

def read_pir():
    return pir.value()


# =========================
# SERVO SETUP
# =========================
servo = Servo(pin=22)


# =========================
# MAIN LOGIC
# =========================
def main():
    while True:
        # === READ SENSOR ===
        dist = read_ultrasonic()
        motion = read_pir()
        ax, ay, az, temp = read_imu()

        # =============================
        # SMART PARKING CONDITIONS
        # =============================

        # 1️⃣ Parkir tersedia jika jarak > 20cm
        if dist > 20:
            relay.high()
            relay_status = "Available"
        else:
            relay.low()
            relay_status = "Full"

        # 2️⃣ Servo otomatis
        if dist < 20:
            servo.move(90)
            servo_status = "Open"
        else:
            servo.move(0)
            servo_status = "Closed"

        # =============================
        # PRINT STATUS
        # =============================
        print("Distance:", dist, "cm")
        print("PIR Motion:", motion)
        print("Relay:", relay_status)
        print("Servo:", servo_status)
        print("IMU:", ax, ay, az, temp)

        # =============================
        # MQTT PUBLISH
        # =============================
        msg = "Dist={} PIR={} Relay={} Servo={} IMU={} {} {} Temp={}".format(
            round(dist,1), motion, relay_status,
            servo_status, ax, ay, az, temp
        )

        client.publish(MQTT_TOPIC, msg)

        utime.sleep(1)


# =========================
# RUN PROGRAM
# =========================
main()