from machine import Pin, I2C
import time

# I2C inicializálás Pico-n (GPIO14 = SCL, GPIO15 = SDA)
i2c = I2C(1, scl=Pin(15), sda=Pin(14), freq=100000)

DEVICE_ADDR = 0x36
ANGLE_REG = 0x0E

# Ellenőrzés: látjuk-e az eszközt?
devices = i2c.scan()
print("I2C eszközök:", [hex(d) for d in devices])
if DEVICE_ADDR not in devices:
    print("HIBA: Nem található az AS5600 a buszon!")
else:
    print("AS5600 megtalálva a 0x36 címen.")

turns = 0
prev_angle = 0

while True:
    try:
        # Regiszter cím küldése
        i2c.writeto(DEVICE_ADDR, bytes([ANGLE_REG]))
        # 2 bájt olvasása
        data = i2c.readfrom(DEVICE_ADDR, 2)
        angle = ((data[0] << 8) | data[1]) & 0x0FFF  # 12 bites szög

        # Fordulat detektálás (átmenet 4095 → 0)
        if angle < 100 and prev_angle > 4000:
            turns += 1
        prev_angle = angle

        # Teljes szög = fordulatok * 360° + aktuális szög
        total_angle = turns * 360.0 + (angle * 360.0 / 4096.0)

        print("Fordulat:", turns, "Szög:", total_angle, "°")

        time.sleep(0.2)

    except OSError as e:
        print("I2C hiba:", e)
        time.sleep(1)