from machine import Pin, I2C
from ssd1306 import SSD1306_I2C
import network
import time
import urequests

# OLED
i2c = I2C(0, scl=Pin(22), sda=Pin(21))
oled = SSD1306_I2C(128, 64, i2c)

# Button
btn_a = Pin(12, Pin.IN, Pin.PULL_UP)
btn_b = Pin(13, Pin.IN, Pin.PULL_UP)
btn_c = Pin(14, Pin.IN, Pin.PULL_UP)

# Motor LED
motor_left = Pin(25, Pin.OUT)
motor_right = Pin(26, Pin.OUT)

# Status
current_pos = "HOME"
battery = 85
task = "IDLE"
is_moving = False

# GANTI DENGAN URL ANDA (tanpa / di akhir)
SERVER_URL = "https://webhook.site/de7fd5c6-b431-4500-ba09-825955f17e52"

def update_display(cmd=""):
    oled.fill(0)
    oled.text("Hello Lukman", 20, 0)
    oled.hline(0, 10, 128, 1)
    oled.text("AGV-01", 45, 14)
    oled.text("BAT: " + str(battery) + "%", 30, 26)
    oled.text("POS: " + current_pos, 25, 38)
    if cmd:
        oled.text("CMD: " + cmd, 25, 50)
    else:
        oled.text("TASK: " + task, 25, 50)
    oled.show()

def move_forward():
    global is_moving
    is_moving = True
    motor_left.value(1)
    motor_right.value(1)
    print("MOTOR ON")

def stop():
    global is_moving
    is_moving = False
    motor_left.value(0)
    motor_right.value(0)
    print("MOTOR OFF")

def send_data():
    try:
        data = {
            "agv_id": "AGV-01",
            "position": current_pos,
            "task": task,
            "battery": battery,
            "status": "ARRIVED"
        }
        urequests.post(SERVER_URL, json=data)
        print("Data sent!")
    except Exception as e:
        print("Send error:", e)

def check_command():
    try:
        # Ambil data terbaru
        response = urequests.get(SERVER_URL)
        text = response.text
        response.close()
        
        print("Received:", text[:100])  # Debug
        
        # Cek jika ada command
        if "START" in text:
            print("COMMAND: START")
            update_display("START")
            move_forward()
            time.sleep(3)
            stop()
            update_display()
            return True
        elif "STOP" in text:
            print("COMMAND: STOP")
            update_display("STOP")
            stop()
            return True
        elif "CHARGE" in text:
            print("COMMAND: CHARGE")
            update_display("CHARGE")
            stop()
            return True
            
    except Exception as e:
        print("Check error:", e)
    
    return False

def check_station():
    global current_pos, task
    
    if btn_a.value() == 0:
        current_pos = "STA-A"
        task = "PICKUP"
        return True
    elif btn_b.value() == 0:
        current_pos = "STA-B"
        task = "DELIVER"
        return True
    elif btn_c.value() == 0:
        current_pos = "STA-C"
        task = "CHARGE"
        return True
    return False

# WiFi
wlan = network.WLAN(network.STA_IF)
wlan.active(True)
wlan.connect("Wokwi-GUEST", "")

update_display()

while not wlan.isconnected():
    time.sleep(0.5)

print("WiFi OK! IP:", wlan.ifconfig()[0])

# Loop utama
counter = 0
while True:
    # Cek command tiap 3 detik
    if counter % 6 == 0:
        check_command()
    
    # Cek button
    if check_station():
        move_forward()
        time.sleep(2)
        stop()
        send_data()
        time.sleep(1)
    
    counter += 1
    time.sleep(0.5)