from machine import Pin, PWM
import time, uasyncio as asyncio, network

# ===== Wi-Fi Credentials (Edit these) =====
SSID = "YourWiFiSSID"
PASSWORD = "YourWiFiPassword"
ESP32_CAM_IP = "192.168.x.xxx"  # Replace with actual ESP32-CAM IP from serial monitor

# ===== Pins Setup =====
IN1,IN2,IN3,IN4 = Pin(0,Pin.OUT),Pin(1,Pin.OUT),Pin(2,Pin.OUT),Pin(3,Pin.OUT)
ENA,ENB = PWM(Pin(4)),PWM(Pin(5)); ENA.freq(1000); ENB.freq(1000)
irL,irR = Pin(6,Pin.IN),Pin(7,Pin.IN)
trig,echo = Pin(8,Pin.OUT),Pin(9,Pin.IN)
servo = PWM(Pin(10)); servo.freq(50)
front_led,back_led,buzzer = Pin(11,Pin.OUT),Pin(12,Pin.OUT),Pin(13,Pin.OUT)

def servo_angle(angle):
    duty = int((angle/180*5000) + 2500)
    servo.duty_u16(duty)

def horn(duration=0.3):
    buzzer.value(1)
    time.sleep(duration)
    buzzer.value(0)

def stop():
    for p in (IN1, IN2, IN3, IN4):
        p.value(0)
    ENA.duty_u16(0)
    ENB.duty_u16(0)
    front_led.value(0)
    back_led.value(1)

def forward(speed=50000):
    IN1.value(1)
    IN2.value(0)
    IN3.value(1)
    IN4.value(0)
    ENA.duty_u16(speed)
    ENB.duty_u16(speed)
    front_led.value(1)
    back_led.value(0)

def backward(speed=50000):
    IN1.value(0)
    IN2.value(1)
    IN3.value(0)
    IN4.value(1)
    ENA.duty_u16(speed)
    ENB.duty_u16(speed)
    back_led.value(1)
    front_led.value(0)

def left(speed=50000):
    IN1.value(0)
    IN2.value(1)
    IN3.value(1)
    IN4.value(0)
    ENA.duty_u16(speed)
    ENB.duty_u16(speed)

def right(speed=50000):
    IN1.value(1)
    IN2.value(0)
    IN3.value(0)
    IN4.value(1)
    ENA.duty_u16(speed)
    ENB.duty_u16(speed)

def get_distance():
    trig.value(0)
    time.sleep_us(2)
    trig.value(1)
    time.sleep_us(10)
    trig.value(0)
    while echo.value() == 0:
        start = time.ticks_us()
    while echo.value() == 1:
        end = time.ticks_us()
    duration = time.ticks_diff(end, start)
    distance = (duration * 0.0343) / 2
    return distance

# Global mode variable
mode = "free"

# Line follower task
async def line_follow():
    global mode
    while mode == "line":
        left_val = irL.value()
        right_val = irR.value()
        if left_val == 0 and right_val == 0:
            forward()
        elif left_val == 0 and right_val == 1:
            left()
        elif left_val == 1 and right_val == 0:
            right()
        else:
            stop()
        await asyncio.sleep_ms(50)
    stop()

# Obstacle avoidance task
async def obstacle_avoid():
    global mode
    while mode == "obstacle":
        dist = get_distance()
        if dist < 20:
            stop()
            horn()
            servo_angle(140)
            time.sleep(0.4)
            left_dist = get_distance()
            servo_angle(40)
            time.sleep(0.4)
            right_dist = get_distance()
            servo_angle(90)
            if left_dist > right_dist:
                left()
                time.sleep(0.5)
            else:
                right()
                time.sleep(0.5)
        else:
            forward()
        await asyncio.sleep_ms(100)
    stop()

# Connect to Wi-Fi
def connect_wifi():
    wlan = network.WLAN(network.STA_IF)
    wlan.active(True)
    wlan.connect(SSID, PASSWORD)
    while not wlan.isconnected():
        pass
    ip = wlan.ifconfig()[0]
    print("Connected! IP address:", ip)
    return ip

# Web server handler
async def serve(reader, writer):
    global mode
    request_line = await reader.readline()
    request = request_line.decode()
    while await reader.readline() != b'\r\n':  # Consume headers
        pass

    if "/cmd" in request:
        cmd = request.split("cmd=")[1].split(" ")[0]
        if cmd == "forward": forward()
        elif cmd == "backward": backward()
        elif cmd == "left": left()
        elif cmd == "right": right()
        elif cmd == "stop": stop()
        elif cmd == "horn": horn()
        response = "HTTP/1.1 200 OK\r\n\r\nOK"
    elif "/mode" in request:
        m = request.split("mode=")[1].split(" ")[0]
        mode = m
        stop()
        if mode == "line":
            asyncio.create_task(line_follow())
        elif mode == "obstacle":
            asyncio.create_task(obstacle_avoid())
        response = f"HTTP/1.1 200 OK\r\n\r\nMode set to: {m}"
    else:
        cam_url = f"http://{ESP32_CAM_IP}:81/stream"
        html = f"""
        HTTP/1.1 200 OK\r\nContent-Type: text/html\r\n\r\n
        <html><head><title>Robot Car Control</title></head><body style='text-align:center;'>
        <h2>Raspberry Pi Pico 2 W Robot Car</h2>
        <img src='{cam_url}' width='70%'><br><br>
        <button onclick="fetch('/cmd?cmd=forward')">Forward ⬆️</button>
        <button onclick="fetch('/cmd?cmd=left')">Left ⬅️</button>
        <button onclick="fetch('/cmd?cmd=stop')">Stop ⏹️</button>
        <button onclick="fetch('/cmd?cmd=right')">Right ➡️</button>
        <button onclick="fetch('/cmd?cmd=backward')">Backward ⬇️</button><br><br>
        <button onclick="fetch('/cmd?cmd=horn')">Horn 🔊</button><br><br>
        <button onclick="fetch('/mode?mode=free')">Free Mode</button>
        <button onclick="fetch('/mode?mode=line')">Line Follower</button>
        <button onclick="fetch('/mode?mode=obstacle')">Obstacle Avoid</button>
        </body></html>
        """
        response = html
    writer.write(response.encode())
    await writer.drain()
    await writer.wait_closed()

async def main():
    ip = connect_wifi()
    print("Open this URL in browser: http://" + ip)
    server = await asyncio.start_server(serve, "0.0.0.0", 80)
    while True:
        await asyncio.sleep(1)

asyncio.run(main())