from machine import I2C, Pin
import time

# I2C1 inicializálása GP14/15 lábakon
i2c = I2C(1, sda=Pin(14), scl=Pin(15), freq=1000000)
AS5600_ADDR = 0x36

# Konstansok a negyedekhez
RESOLUTION = 4096
QUARTER = RESOLUTION // 4 

def get_raw_angle():
    try:
        i2c.writeto(AS5600_ADDR, b'\x0E')
        data = i2c.readfrom(AS5600_ADDR, 2)
        return (data[0] << 8) | data[1]
    except:
        return None

print("AS5600 Számláló - Csak változáskor ír ki")

# Kezdőértékek beállítása
current_raw = get_raw_angle()
while current_raw is None:
    current_raw = get_raw_angle()
    #time.sleep(0.1)

last_raw = current_raw
last_abs_pos = 0 #-1 # Kezdőérték, ami biztosan más, mint az első mért adat
turns = 0
current_raw = get_raw_angle()
abs_pos=0
while True:
   
    # CSAK AKKOR ÍRUNK KI, HA VÁLTOZOTT AZ ABSZOLÚT POZÍCIÓ
    if get_raw_angle() != current_raw:
        #deg = (current_raw * 360) / RESOLUTION
        #print("Abszolút: {:6d} | Kör: {:2d} | Szög: {:5.1f}°".format(
        #    current_raw, turns, deg
        #))
        print("val: ",current_raw)
        current_raw=get_raw_angle()

"""

    if current_raw is not None:
        # Negyedek meghatározása
        last_quarter = last_raw // QUARTER
        current_quarter = current_raw // QUARTER
        
        # Körváltás detektálása
        if last_quarter == 3 and current_quarter == 0:
            turns += 1
        elif last_quarter == 0 and current_quarter == 3:
            turns -= 1
            
        last_raw = current_raw
        
        # 32-bites folyamatos pozíció számítása
        abs_pos = (turns * RESOLUTION) + current_raw
        
        # CSAK AKKOR ÍRUNK KI, HA VÁLTOZOTT AZ ABSZOLÚT POZÍCIÓ
        if abs_pos != last_abs_pos:
            deg = (current_raw * 360) / RESOLUTION
            print("Abszolút: {:6d} | Kör: {:2d} | Szög: {:5.1f}°".format(
                abs_pos, turns, deg
            ))
            last_abs_pos = abs_pos # Frissítjük az utolsó ismert pozíciót

    time.sleep(0.01) # Gyorsabb mintavételezés a simább mozgáskövetéshez

 """