from machine import Pin, I2C, PWM
from machine_i2c_lcd import I2cLcd
import time
import struct

# =========================================================
# HARDWARE SETUP & PIN CONFIGURATION
# =========================================================
i2c = I2C(0, scl=Pin(22), sda=Pin(21))
MPU_ADDR = 0x68

# Wake up MPU6050
try:
    i2c.writeto_mem(MPU_ADDR, 0x6B, b'\x00')
except Exception as e:
    print("MPU6050 Init Error:", e)

# LCD Setup (16x2 I2C)
devices = i2c.scan()
lcd_addr = [addr for addr in devices if addr != MPU_ADDR]
if lcd_addr:
    lcd = I2cLcd(i2c, lcd_addr[0], 2, 16)
else:
    print("Warning: LCD not detected!")
    lcd = None

# Encoders Pin Setup
left_clk = Pin(32, Pin.IN)
left_dt = Pin(33, Pin.IN)

right_clk = Pin(34, Pin.IN)
right_dt = Pin(35, Pin.IN)

# LED Outputs (Green on GPIO 2, Red on GPIO 4)
green_led = Pin(2, Pin.OUT)
red_led = Pin(4, Pin.OUT)

# Buzzer Setup (GPIO 15)
buzzer = PWM(Pin(15))
buzzer.duty(0)

# =========================================================
# ENCODER PULSE COUNTERS
# =========================================================
left_ticks = 0
right_ticks = 0
gyro_z_offset = 0.0

last_left_clk = left_clk.value()
last_right_clk = right_clk.value()

def left_encoder_isr(pin):
    global left_ticks, last_left_clk
    clk_val = left_clk.value()
    if clk_val != last_left_clk and clk_val == 0:
        if left_dt.value() != clk_val:
            left_ticks += 1
        else:
            left_ticks -= 1
    last_left_clk = clk_val

def right_encoder_isr(pin):
    global right_ticks, last_right_clk
    clk_val = right_clk.value()
    if clk_val != last_right_clk and clk_val == 0:
        if right_dt.value() != clk_val:
            right_ticks += 1
        else:
            right_ticks -= 1
    last_right_clk = clk_val

# Attach Quadrature Interrupts
left_clk.irq(trigger=Pin.IRQ_FALLING | Pin.IRQ_RISING, handler=left_encoder_isr)
right_clk.irq(trigger=Pin.IRQ_FALLING | Pin.IRQ_RISING, handler=right_encoder_isr)

# =========================================================
# SENSOR READINGS & LCD UPDATE FUNCTIONS
# =========================================================
def read_raw_gyro_z():
    """Reads raw Z-axis gyro reading in deg/s."""
    try:
        data = i2c.readfrom_mem(MPU_ADDR, 0x47, 2)
        raw_gz = struct.unpack(">h", data)[0]
        return raw_gz / 131.0  # Convert to deg/s (+/-250dps range)
    except:
        return 0.0

def calibrate_gyro():
    """Calculates zero-rate offset by averaging samples at startup."""
    global gyro_z_offset
    total = 0.0
    samples = 50
    for _ in range(samples):
        total += read_raw_gyro_z()
        time.sleep(0.01)
    gyro_z_offset = total / samples

def read_gyro_z_dps():
    """Returns calibrated angular velocity."""
    return read_raw_gyro_z() - gyro_z_offset

def update_lcd(l_count, r_count, status_str):
    if not lcd:
        return
    
    line1 = "L:{:<4} R:{:<4}".format(l_count, r_count)
    if len(line1) < 16:
        line1 += " " * (16 - len(line1))
    lcd.move_to(0, 0)
    lcd.putstr(line1[:16])
    
    line2 = "Status: {}".format(status_str)
    if len(line2) < 16:
        line2 += " " * (16 - len(line2))
    lcd.move_to(0, 1)
    lcd.putstr(line2[:16])

# =========================================================
# MAIN EVALUATION LOOP
# =========================================================
if lcd:
    lcd.clear()
    lcd.move_to(0, 0)
    lcd.putstr("Calibrating Gyro")
    calibrate_gyro()
    lcd.clear()
    lcd.move_to(0, 0)
    lcd.putstr("System Ready...")
    time.sleep(0.5)

while True:
    # Read Calibrated Gyroscope Z-axis (deg/s)
    gz_dps = read_gyro_z_dps()
    
    # Calculate Pulse Difference: (Left - Right)
    encoder_diff = left_ticks - right_ticks

    # Lost Heading (|Z-gyro| > 10 deg/s)
    if abs(gz_dps) > 10.0:
        status = "LOST HDG"
        green_led.value(0)
        red_led.value(1)
        buzzer.freq(1000)
        buzzer.duty(512)
    else:
        green_led.value(1)
        red_led.value(0)
        buzzer.duty(0)

        # Straight (-5 <= L - R <= 5)
        if -5 <= encoder_diff <= 5:
            status = "STRAIGHT"
        # Turning Right (Left > Right + 5)
        elif encoder_diff > 5:
            status = "TURN R"
        # Turning Left (Right > Left + 5)
        elif encoder_diff < -5:
            status = "TURN L"

    # Refresh LCD Display
    update_lcd(left_ticks, right_ticks, status)
    
    time.sleep(0.05)