from machine import Pin, I2C
import time

# Pins
lclk, ldt = Pin(32, Pin.IN, Pin.PULL_UP), Pin(33, Pin.IN, Pin.PULL_UP)
rclk, rdt = Pin(25, Pin.IN, Pin.PULL_UP), Pin(26, Pin.IN, Pin.PULL_UP)
led = Pin(2, Pin.OUT)

# I2C
i2c = I2C(0, scl=Pin(22), sda=Pin(21), freq=100000)
MPU, LCD = 0x68, 0x27

# MPU6050
i2c.writeto_mem(MPU, 0x6B, b'\x00')
i2c.writeto_mem(MPU, 0x1B, b'\x00')

# LCD
BL, EN, RS = 0x08, 0x04, 0x01

def write(x):
    i2c.writeto(LCD, bytes([x | BL]))

def pulse(x):
    write(x | EN)
    time.sleep_us(1)
    write(x & ~EN)
    time.sleep_us(50)

def send(x, mode=0):
    pulse((x & 0xF0) | mode)
    pulse(((x << 4) & 0xF0) | mode)

def cmd(x):
    send(x)
    time.sleep_ms(2)

def text(s):
    for c in s:
        send(ord(c), RS)

def lcd_init():
    time.sleep_ms(50)
    for _ in range(3):
        pulse(0x30)
        time.sleep_ms(1)
    pulse(0x20)
    cmd(0x28)
    cmd(0x0C)
    cmd(0x01)
    cmd(0x06)

def cursor(row, col):
    cmd((0x80 if row == 0 else 0xC0) + col)

def lcd_show(l, r, status):
    cursor(0, 0)
    text("{:<16}".format("L:{} R:{}".format(l, r))[:16])
    cursor(1, 0)
    text("{:<16}".format(status)[:16])

lcd_init()

# Counters
left = right = 0
last_l = lclk.value()
last_r = rclk.value()

# Heading
heading = 0.0
last_time = time.ticks_ms()

def gyro_z():
    d = i2c.readfrom_mem(MPU, 0x47, 2)
    return int.from_bytes(d, "big", True) / 131.0

last_display = 0

while True:

    # Left Encoder
    c = lclk.value()
    if c != last_l:
        if c == 0:
            left += 1 if ldt.value() != c else -1
        last_l = c

    # Right Encoder
    c = rclk.value()
    if c != last_r:
        if c == 0:
            right += 1 if rdt.value() != c else -1
        last_r = c

    # Gyroscope
    gz = gyro_z()

    # Heading
    now = time.ticks_ms()
    dt = time.ticks_diff(now, last_time) / 1000
    heading += gz * dt
    last_time = now

    # Difference
    diff = abs(left) - abs(right)

    # Status
    if abs(gz) > 10:
        status = "LOST HEADING"
        led.value(0)
    elif abs(diff) <= 5:
        status = "STRAIGHT"
        led.value(1)
    elif diff > 5:
        status = "TURN RIGHT"
        led.value(1)
    else:
        status = "TURN LEFT"
        led.value(1)

    # LCD every 200 ms
    if time.ticks_diff(time.ticks_ms(), last_display) >= 200:
        lcd_show(left, right, status)

        print(
            "L:", left,
            "R:", right,
            "Diff:", diff,
            "GZ:", round(gz, 2),
            "Heading:", round(heading, 2),
            "Status:", status
        )

        last_display = time.ticks_ms()

    time.sleep_ms(1)