from machine import Pin, I2C
from time import sleep, sleep_us

# Since the nunchuck has a pullup on the I2C line,
# Make sure not to use any of the pins with funny behavior (e.g., D2)
i2c = I2C(0, scl=Pin(18), sda=Pin(19))

NUNCHUCK_ADDR = 0x52

# Initialize nunchuck
# Disable "encryption"
i2c.writeto(NUNCHUCK_ADDR, bytearray([0xF0, 0x55]))
i2c.writeto(NUNCHUCK_ADDR, bytearray([0xFB, 0x00]))

# Read the ID of the device
i2c.writeto(NUNCHUCK_ADDR, bytearray([0xFA]))
result = i2c.readfrom(NUNCHUCK_ADDR, 6)
print(f"Peripheral ID: {result}")

# Set up the first read
i2c.writeto(NUNCHUCK_ADDR, bytearray([0x00]))
sleep_us(100);

while True:
    # Read the controller data
    
    result = i2c.readfrom(NUNCHUCK_ADDR, 6)

    sleep_us(300)

    # Set up the next read (from the same address)
    # We could do this at the top of the loop, but this makes it easier
    # to trigger on the data payload with the Digital Discovery
    i2c.writeto(NUNCHUCK_ADDR, bytearray([0x00]))


    print(f"Stick X: {result[0] - 127}")
    print(f"Stick Y: {result[1] - 127}")
    print(f"AX: {(result[2] - 127) / 65}")
    print(f"AY: {(result[3] - 127) / 65}")
    print(f"AZ: {(result[4] - 127) / 65}")
    print(f"Button C: {result[5] & 0x02}")
    print(f"Button Z: {result[5] & 0x01}")

#     print(f"Stick X: {result[0] - 225}")
#     print(f"Stick Y: {result[1] - 225}")
#     print(f"Whammy bar: {result[3]}")
#     print(f"Buttons1: {result[4]:08b}")
#     print(f"Buttons2: {result[5]:08b}")

    sleep(0.2)