from machine import Pin, I2C
import time

# XIAO RP2040: SDA=GPIO6 (D4), SCL=GPIO7 (D5)
i2c = I2C(1, sda=Pin(6), scl=Pin(7), freq=400000)

# Common I2C addresses for this family depend on SA0 wiring
POSSIBLE_ADDRS = [0x6A, 0x6B]

WHO_AM_I = 0x0F
CTRL1_XL = 0x10
CTRL2_G  = 0x11
CTRL3_C  = 0x12

OUTX_L_G  = 0x22
OUTX_L_XL = 0x28

def write_reg(addr, reg, value):
    i2c.writeto_mem(addr, reg, bytes([value]))

def read_reg(addr, reg, n=1):
    return i2c.readfrom_mem(addr, reg, n)

def read_int16_le(addr, reg):
    data = read_reg(addr, reg, 2)
    value = data[0] | (data[1] << 8)
    if value & 0x8000:
        value -= 65536
    return value

def find_sensor():
    found = i2c.scan()
    print("I2C scan:", [hex(x) for x in found])

    for addr in POSSIBLE_ADDRS:
        if addr in found:
            try:
                who = read_reg(addr, WHO_AM_I, 1)[0]
                print("Address", hex(addr), "WHO_AM_I =", hex(who))
                return addr
            except Exception as e:
                print("Read failed at", hex(addr), e)

    return None

def init_sensor(addr):
    # CTRL3_C: IF_INC=1 so multi-byte reads auto-increment
    write_reg(addr, CTRL3_C, 0x04)

    # CTRL1_XL: accel ODR 104 Hz, ±2g, 100 Hz filter
    write_reg(addr, CTRL1_XL, 0x40)

    # CTRL2_G: gyro ODR 104 Hz, 250 dps
    write_reg(addr, CTRL2_G, 0x40)

def read_accel(addr):
    ax_raw = read_int16_le(addr, OUTX_L_XL)
    ay_raw = read_int16_le(addr, OUTX_L_XL + 2)
    az_raw = read_int16_le(addr, OUTX_L_XL + 4)

    # ±2g -> 0.061 mg/LSB = 0.000061 g/LSB
    ax = ax_raw * 0.000061
    ay = ay_raw * 0.000061
    az = az_raw * 0.000061
    return ax, ay, az

def read_gyro(addr):
    gx_raw = read_int16_le(addr, OUTX_L_G)
    gy_raw = read_int16_le(addr, OUTX_L_G + 2)
    gz_raw = read_int16_le(addr, OUTX_L_G + 4)

    # 250 dps -> 8.75 mdps/LSB = 0.00875 dps/LSB
    gx = gx_raw * 0.00875
    gy = gy_raw * 0.00875
    gz = gz_raw * 0.00875
    return gx, gy, gz

addr = find_sensor()

if addr is None:
    print("ISM330DLC not found")
else:
    init_sensor(addr)
    print("Streaming gyro + accel data from", hex(addr))

    while True:
        try:
            gx, gy, gz = read_gyro(addr)
            ax, ay, az = read_accel(addr)

            # Same CSV style your PC GUI can read
            print("{:.2f},{:.2f},{:.2f},{:.3f},{:.3f},{:.3f}".format(
                gx, gy, gz, ax, ay, az
            ))
        except Exception as e:
            print("ERROR,", e)

        time.sleep(0.05)