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

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

oled_width = 128
oled_height = 64
oled = ssd1306.SSD1306_I2C(oled_width, oled_height, i2c)

joy_x = ADC(25)
joy_y = ADC(33)

sprite = [
    [0,0,1,0,0],
    [0,1,1,1,0],
    [1,1,1,1,1],
    [0,0,1,0,0],
    [1,1,1,1,1]
]

# ------- Joystick calibratie -------
def calibrate_joystick():
    samples = 100
    sum_x = 0
    sum_y = 0

    for _ in range(samples):
        sum_x += joy_x.read_u16()
        sum_y += joy_y.read_u16()
        time.sleep(0.005)

    center_x = sum_x // samples
    center_y = sum_y // samples

    return center_x, center_y

center_x, center_y = calibrate_joystick()

# Toon calibratie op scherm
oled.fill(0)
oled.text("Joystick", 0, 0)
oled.text("Calibrated", 0, 12)
oled.show()
time.sleep(1)

while True:
    oled.fill(0)

    # Lees joystick waarden
    raw_x = joy_x.read_u16()
    raw_y = joy_y.read_u16()

    # Gecentreerde waarde (positief of negatief)
    delta_x = raw_x - center_x
    delta_y = raw_y - center_y

    # Clamp delta naar 0–65535 schaal voor eenvoud
    # (negatief = minimum, positief = maximum)
    scaled_x = max(0, min(65535, delta_x + 32768))
    scaled_y = max(0, min(65535, delta_y + 32768))

    # Schaal naar schermcoördinaten
    pos_x = int((scaled_x / 65535) * (oled_width - len(sprite[0])))
    pos_y = int((scaled_y / 65535) * (oled_height - len(sprite) - 6))

    # Teken sprite
    for y in range(len(sprite)):
        for x in range(len(sprite[y])):
            oled.pixel(pos_x + x, pos_y + y, sprite[y][x])

    oled.text("x", pos_x - 1, pos_y + 7)
    oled.show()