from machine import Pin, I2C
import time
import math

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

MPU_ADDR = 0x68

# MPU9250 / MPU6050-style registers
PWR_MGMT_1   = 0x6B
WHO_AM_I     = 0x75
ACCEL_XOUT_H = 0x3B
GYRO_XOUT_H  = 0x43

ACCEL_CONFIG = 0x1C
GYRO_CONFIG  = 0x1B

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(addr, reg):
    data = read_reg(addr, reg, 2)
    value = (data[0] << 8) | data[1]
    if value & 0x8000:
        value -= 65536
    return value

def init_mpu():
    # Wake up MPU
    write_reg(MPU_ADDR, PWR_MGMT_1, 0x00)
    time.sleep_ms(100)

    # Accelerometer full scale = ±2g
    write_reg(MPU_ADDR, ACCEL_CONFIG, 0x00)
    time.sleep_ms(10)

    # Gyroscope full scale = ±250 deg/s
    write_reg(MPU_ADDR, GYRO_CONFIG, 0x00)
    time.sleep_ms(10)

def scan_i2c():
    devices = i2c.scan()
    print("I2C devices:", [hex(d) for d in devices])

def read_who_am_i():
    try:
        who = read_reg(MPU_ADDR, WHO_AM_I, 1)[0]
        print("WHO_AM_I:", hex(who))
    except Exception as e:
        print("WHO_AM_I read failed:", e)

def read_accel():
    ax_raw = read_int16(MPU_ADDR, ACCEL_XOUT_H)
    ay_raw = read_int16(MPU_ADDR, ACCEL_XOUT_H + 2)
    az_raw = read_int16(MPU_ADDR, ACCEL_XOUT_H + 4)

    # ±2g => 16384 LSB/g
    ax = ax_raw / 16384.0
    ay = ay_raw / 16384.0
    az = az_raw / 16384.0

    return ax, ay, az

def read_gyro():
    gx_raw = read_int16(MPU_ADDR, GYRO_XOUT_H)
    gy_raw = read_int16(MPU_ADDR, GYRO_XOUT_H + 2)
    gz_raw = read_int16(MPU_ADDR, GYRO_XOUT_H + 4)

    # ±250 dps => 131 LSB/(deg/s)
    gx = gx_raw / 131.0
    gy = gy_raw / 131.0
    gz = gz_raw / 131.0

    return gx, gy, gz

def calc_tilt(ax, ay, az):
    # Simple tilt estimate from accelerometer
    roll = math.degrees(math.atan2(ay, az))
    pitch = math.degrees(math.atan2(-ax, math.sqrt(ay * ay + az * az)))
    return roll, pitch

print("Starting GY-91 test...")
scan_i2c()
read_who_am_i()
init_mpu()

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

        # Send CSV line to PC
        # Format:
        # gx,gy,gz,ax,ay,az,roll,pitch
        print("{:.2f},{:.2f},{:.2f},{:.3f},{:.3f},{:.3f},{:.2f},{:.2f}".format(
            gx, gy, gz, ax, ay, az, roll, pitch
        ))

    except Exception as e:
        print("ERROR,", e)

    time.sleep(0.05)   # 20 updates per second