import machine
import utime
import math

# --- Configuration ---
# I2C Pins for MPU6050 (User specified)
I2C_SDA = 8
I2C_SCL = 9

# GPIO Pins (User specified)
LED_PIN = 15

# Thresholds for Gyroscope Logic
# We use Gyroscope (Yaw / Z-axis) instead of the Accelerometer for a bike!
# A value around 2000-4000 in raw MPU6050 gyro units indicates a steady turn.
TURN_RATE_THRESHOLD = 2000  
STRAIGHT_RATE_THRESHOLD = 500

# Timers
TIMEOUT_LIMIT = 30 # Seconds (Max time the indicator can be ON before a turn starts)
TURN_TIMEOUT_LIMIT = 7 # Seconds (Max time allowed to complete a turn once it starts)

# Filter configuration
# Alpha determines the smoothing. 1.0 = no filtering, 0.1 = heavily smoothed (slow response)
# 0.2 means the new value relies 20% on the current reading and 80% on past history.
FILTER_ALPHA = 0.2

# --- I2C Initialization ---
i2c = machine.I2C(0, sda=machine.Pin(I2C_SDA), scl=machine.Pin(I2C_SCL), freq=400000)

MPU_ADDR = 0x68

def init_mpu():
    try:
        i2c.writeto_mem(MPU_ADDR, 0x6B, b'\x00') # Wake up MPU6050
        print("MPU6050 Initialized")
    except Exception as e:
        print(f"MPU6050 not found: {e}")

def get_gyro_z_data():
    """Reads raw Gyroscope Z-axis data from MPU6050 to detect turning/yaw."""
    try:
        # Read 2 bytes for GYRO_ZOUT_H and L (Registers 0x47 and 0x48)
        data = i2c.readfrom_mem(MPU_ADDR, 0x47, 2)
        
        # Bulletproof MicroPython 16-bit signed conversion
        z = (data[0] << 8) | data[1]
        if z > 32767:
            z -= 65536
        return z
    except Exception as e:
        print(f"Error reading Gyro: {e}")
        return 0

# --- Hardware Setup ---
indicator_led = machine.Pin(LED_PIN, machine.Pin.OUT)

# --- State Management ---
is_active = True
start_time = 0
turn_detected = False
turn_detected_time = 0 # Track when the turn actually started
filtered_gyro_z = 0  # Store the smoothed value

def stop_indicator():
    global is_active, turn_detected, turn_detected_time
    indicator_led.value(0)
    is_active = False
    turn_detected = False
    turn_detected_time = 0
    print("Indicator OFF. Restart the Pico to test again.")

init_mpu()

print("System Ready. Turning light ON directly...")
indicator_led.value(1)
start_time = utime.time()

while True:
    current_time = utime.time()
    gyro_z = get_gyro_z_data()
    
    # Apply Exponential Moving Average (EMA) Filter to smooth vibrations
    filtered_gyro_z = (FILTER_ALPHA * gyro_z) + ((1.0 - FILTER_ALPHA) * filtered_gyro_z)
    
    # Monitor Active Turn Logic
    if is_active:
        # Check overall 30-second Cutoff Timer
        if not turn_detected and (current_time - start_time) >= TIMEOUT_LIMIT:
            print("Cutoff: 30s limit reached without turning.")
            stop_indicator()
            continue

        # Turn Detection Logic using Gyroscope Z-Axis (Yaw / Rotation Rate)
        rotation_speed = abs(filtered_gyro_z)
        
        if rotation_speed > TURN_RATE_THRESHOLD:
            if not turn_detected:
                print("Turn in progress (Rotation detected)...")
                turn_detected = True
                turn_detected_time = current_time # Start the 5-second completion timer
        
        # If we previously detected a turn, handle completion or timeout
        if turn_detected:
            # Condition A: Bike straightens out normally
            if rotation_speed < STRAIGHT_RATE_THRESHOLD:
                utime.sleep_ms(500) # Ensure stability before turning off
                print("Turn completed (Moving straight).")
                stop_indicator()
            
            # Condition B: Turn completion safety timer (5 seconds) expires
            elif (current_time - turn_detected_time) >= TURN_TIMEOUT_LIMIT:
                print(f"Failsafe: Turn did not complete within {TURN_TIMEOUT_LIMIT}s.")
                stop_indicator()

    utime.sleep_ms(50)