from machine import I2C, Pin
import time
import math

time.sleep(0.1)  # Wait for USB to become ready

# Initialize I2C
i2c = I2C(0, sda=Pin(0), scl=Pin(1), freq=400000)

# MPU6050 Registers and Address
MPU6050_ADDR = 0x68  # Default address (0x69 if AD0 pin high)
PWR_MGMT_1 = 0x6B
ACCEL_XOUT_H = 0x3B
GYRO_XOUT_H = 0x43

# Wake up the MPU6050
i2c.writeto_mem(MPU6050_ADDR, PWR_MGMT_1, b'\x00')

def read_raw_word(register):
    """Read a 16-bit word from the specified register."""
    data = i2c.readfrom_mem(MPU6050_ADDR, register, 2)
    return (data[0] << 8) | data[1]

def get_rotation():
    """Get gyroscope rotation values in degrees per second."""
    # Read gyroscope values
    gx = read_raw_word(GYRO_XOUT_H)
    gy = read_raw_word(GYRO_XOUT_H + 2)
    gz = read_raw_word(GYRO_XOUT_H + 4)
    
    # Convert to signed values
    gx = gx if gx < 32768 else gx - 65536
    gy = gy if gy < 32768 else gy - 65536
    gz = gz if gz < 32768 else gz - 65536
    
    # Apply sensitivity scale factor (131 LSB/°/s)
    gx_deg = gx / 131.0
    gy_deg = gy / 131.0
    gz_deg = gz / 131.0
    
    return gx_deg, gy_deg, gz_deg

# Pin setup for 74HC595 shift registers
data_pin = Pin(18, Pin.OUT)    # DS
clock_pin = Pin(20, Pin.OUT)   # SH_CP
latch_pin = Pin(19, Pin.OUT)   # ST_CP

def shift_out(byte1, byte2, byte3, byte4, byte5, byte6):
    """Send six bytes to daisy-chained 74HC595 registers."""
    latch_pin.off()  # Prepare to shift data

    for byte in [byte6, byte5, byte4, byte3, byte2, byte1]:
        for i in range(8):
            bit = (byte >> (7 - i)) & 0x01
            data_pin.value(bit)
            clock_pin.on()
            clock_pin.off()
    
    latch_pin.on()  # Latch the data

while True:
    # Get gyroscope rotation values
    x, y, z = get_rotation()
    print(f"Gyro Rotation: X={x:.2f}°/s, Y={y:.2f}°/s, Z={z:.2f}°/s")

    # Loop through 48 LEDs (6 registers × 8 bits)
    for i in range(41):
        # Initialize all bytes to 0
        byte1 = byte2 = byte3 = byte4 = byte5 = byte6 = 0x00

        if i < 8:
            byte1 = 1 << (7 - i)  # First register (LEDs 0–7)
        elif i < 16:
            byte2 = 1 << (15 - i)  # Second register (LEDs 8–15)
        elif i < 24:
            byte3 = 1 << (23 - i)  # Third register (LEDs 16–23)
        elif i < 32:
            byte4 = 1 << (31 - i)  # Fourth register (LEDs 24–31)
        elif i < 40:
            byte5 = 1 << (39 - i)  # Fifth register (LEDs 32–39)
        else:
            byte6 = 1 << (47 - i)  # Sixth register (LEDs 40–47)

        print(f"LED {i}: Bytes: {byte1:08b}, {byte2:08b}, {byte3:08b}, {byte4:08b}, {byte5:08b}, {byte6:08b}")

        # Shift out the data to the LEDs
        shift_out(byte1, byte2, byte3, byte4, byte5, byte6)
        time.sleep(0.1)