from machine import Pin, ADC, I2C
import ssd1306
import time

# ESP32 Pin assignment for I2C
i2c = I2C(scl=Pin(22), sda=Pin(21))

# OLED display setup
oled_width = 128
oled_height = 64
oled = ssd1306.SSD1306_I2C(oled_width, oled_height, i2c)

# ADC setup for potentiometers on GPIO 33 for "up" and GPIO 35 for "down"
pot_up = ADC(Pin(33))
pot_down = ADC(Pin(35))

# Configure ADC resolution and attenuation for full range (0-3.3V)
pot_up.atten(ADC.ATTN_11DB)
pot_down.atten(ADC.ATTN_11DB)

# Current gear, starting at Neutral ("N")
current_gear = 'N'
gears = ['1', 'N', '2', '3', '4', '5']  # Define the gear sequence

# Flags to determine if the lever is in the reset position
up_reset = True
down_reset = True

# Neutral threshold values specific to transitioning between 1st and 2nd gears
neutral_up_threshold = 1500  # Value for entering Neutral from 1st gear
neutral_down_threshold = 1500  # Value for entering Neutral from 2nd gear

# Function to update the display with the current gear, displayed large
def update_display(gear):
    oled.fill(0)  # Clear the display
    oled.text(gear, 10, 10)  # Display gear prominently; adjust size if possible
    oled.show()

# Function to shift gear up or down based on potentiometer values
def shift_gear():
    global current_gear, up_reset, down_reset
    up_value = pot_up.read()  # Read value from "up" potentiometer
    down_value = pot_down.read()  # Read value from "down" potentiometer

    # Main gear shift thresholds
    up_threshold = 3048
    down_threshold = 3048
    reset_threshold = 1000  # Value below which the lever must return before another shift
    
    current_index = gears.index(current_gear)
    
    # Handling Neutral specifically for 1 to N to 2 transitions without applying reset
    if current_gear == '1' and up_value > neutral_up_threshold:
        current_gear = 'N'
        up_reset = down_reset = False  # Reset both to allow direct transition between 1, N, and 2
    elif current_gear == 'N' and up_value > up_threshold and not up_reset:
        current_gear = '2'
        up_reset = down_reset = True  # Reset both after moving out of N
    elif current_gear == 'N' and down_value > down_threshold and not down_reset:
        current_gear = '1'
        up_reset = down_reset = True  # Reset both after moving out of N
    elif current_gear == '2' and down_value > neutral_down_threshold:
        current_gear = 'N'
        up_reset = down_reset = False  # Reset both to allow direct transition between 1, N, and 2

    # Ensure reset is respected for all other transitions
    if current_gear != 'N' or (current_gear == 'N' and (up_reset or down_reset)):
        if up_value > up_threshold and up_reset and current_index < len(gears) - 1:
            current_gear = gears[current_index + 1]
            up_reset = False
        elif down_value > down_threshold and down_reset and current_index > 0:
            current_gear = gears[current_index - 1]
            down_reset = False

    # Reset condition for "up" and "down" levers
    if up_value < reset_threshold:
        up_reset = True
    if down_value < reset_threshold:
        down_reset = True

    update_display(current_gear)


# Main loop
while True:
    shift_gear()
    # Short delay to limit update speed and allow for potentiometer adjustments
    time.sleep(0.005)