from machine import Pin, I2C
import time
from mpu6050 import MPU6050
from i2clcd import I2cLcd

# Encoders
left_clk = Pin(32, Pin.IN, Pin.PULL_UP)
left_dt = Pin(33, Pin.IN, Pin.PULL_UP)
right_clk = Pin(25, Pin.IN, Pin.PULL_UP)
right_dt = Pin(26, Pin.IN, Pin.PULL_UP)

left_count = 0
right_count = 0

# LED
green_led = Pin(2, Pin.OUT)
green_led.off()

# Encoder functions
def left_encoder(pin):
    global left_count
    if left_dt.value() != left_clk.value():
        left_count += 1
    else:
        left_count -= 1

def right_encoder(pin):
    global right_count
    if right_dt.value() != right_clk.value():
        right_count += 1
    else:
        right_count -= 1

# Interrupts
left_clk.irq(trigger=Pin.IRQ_FALLING, handler=left_encoder)
right_clk.irq(trigger=Pin.IRQ_FALLING, handler=right_encoder)

# I2C
i2c = I2C(0, scl=Pin(22), sda=Pin(21), freq=400000)

# MPU6050
mpu = MPU6050(i2c)

# LCD
devices = i2c.scan()
print("I2C devices:", devices)

lcd_address = None
for device in devices:
    if device != 104:
        lcd_address = device
        break

if lcd_address is not None:
    lcd = I2cLcd(i2c, lcd_address, 2, 16)
    lcd.clear()
    lcd.putstr("Robot Analyzer")
    lcd.move_to(0, 1)
    lcd.putstr("Starting...")
    time.sleep(2)
    lcd.clear()

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

# Main loop
while True:
    # Gyroscope
    gyro_z = mpu.read_gyro_z()
    
    # Time
    now = time.ticks_ms()
    dt = time.ticks_diff(now, last_time) / 1000
    last_time = now
    
    # Cumulative heading
    heading += gyro_z * dt
    
    # Encoder difference
    difference = left_count - right_count
    
    # Robot status (تعديل الشرط ليتوافق مع الروبك بالضبط)
    if abs(gyro_z) > 10:
        status = "LOST HEADING"
    elif abs(difference) <= 5:
        status = "STRAIGHT"
    elif difference > 5:
        status = "TURN RIGHT"
    else:
        status = "TURN LEFT"
        
    # LED
    if status == "STRAIGHT":
        green_led.on()
    else:
        green_led.off()
        
    # LCD
    if lcd_address is not None:
        line1 = "L:{} R:{}".format(left_count, right_count)
        lcd.move_to(0, 0)
        lcd.putstr((line1 + " " * 16)[:16])
        lcd.move_to(0, 1)
        lcd.putstr((status + " " * 16)[:16])
        
    # Serial monitor
    print(
        "L:", left_count,
        "| R:", right_count,
        "| Diff:", difference,
        "| GZ:", round(gyro_z, 1),
        "| Heading:", round(heading, 1),
        "| Status:", status
    )
    
    time.sleep_ms(100)