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

# ---------- OLED ----------
i2c = I2C(0, scl=Pin(1), sda=Pin(0), freq=400_000)
oled = ssd1306.SSD1306_I2C(128, 64, i2c, addr=0x3C)

# ---------- Inputs ----------
joy_y = ADC(26)  # VRy -> GP26 (ADC0)
brake = Pin(15, Pin.IN, Pin.PULL_UP)  # botón a GND

# ---------- Output ----------
brake_led = Pin(16, Pin.OUT)  # LED (con resistencia) a GND

# ---------- Params ----------
MAX_KMH = 30.0
DT_MS = 50

ACCEL_KMH_PER_S = 14.0
COAST_KMH_PER_S = 6.0
BRAKE_KMH_PER_S = 30.0

DEADZONE = 0.08
SMOOTH = 0.15

speed = 0.0
y_filt = 0.5

def clamp(x, a, b):
    return a if x < a else (b if x > b else x)

while True:
    dt = DT_MS / 1000.0

    # leer joystick (0..1)
    y = joy_y.read_u16() / 65535.0
    y_filt = (1.0 - SMOOTH) * y_filt + SMOOTH * y

    # comando -1..+1 (arriba positivo)
    cmd = (y_filt - 0.5) * 2.0
    if abs(cmd) < DEADZONE:
        cmd = 0.0

    braking = (brake.value() == 0)
    brake_led.value(1 if braking else 0)

    # dinámica de velocidad
    if braking:
        speed -= BRAKE_KMH_PER_S * dt
    else:
        if cmd > 0:
            speed += (cmd * ACCEL_KMH_PER_S) * dt
        else:
            speed -= COAST_KMH_PER_S * dt  # sueltas o jalas abajo: desacelera

    speed = clamp(speed, 0.0, MAX_KMH)

    # ---------- OLED ----------
    oled.fill(0)
    oled.text("JOY SPD", 0, 0)
    oled.text(f"{int(speed + 0.5)} km/h", 0, 20)
    oled.text(f"cmd:{cmd:+.2f}", 0, 48)
    if braking:
        oled.text("BRK", 100, 0)
    oled.show()

    time.sleep_ms(DT_MS)