from machine import Pin, I2C
import time

from lsm6ds3 import LSM6DS3, is_lsm6ds3


# XIAO ESP32C3 default I2C pins: SDA=GPIO6 (D4), SCL=GPIO7 (D5)
i2c = I2C(0, scl=Pin(23), sda=Pin(22), freq=400000)


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


def read_who_am_i(addr):
    try:
        sensor = LSM6DS3(i2c, addr)
        who = sensor.who_am_i()
        print("WHO_AM_I at", hex(addr), "=", hex(who))
        if is_lsm6ds3(who):
            print("Looks like an LSM6DS3")
        else:
            print("Unexpected WHO_AM_I value")
    except OSError as e:
        print("Failed to read WHO_AM_I from", hex(addr), "error:", e)


def basic_test():
    """Run an I2C scan and WHO_AM_I check on common LSM6DS3 addresses."""
    scan_i2c()
    for addr in (0x6A, 0x6B):
        read_who_am_i(addr)


def configure_and_read(addr=0x6A):
    """Configure accel/gyro and continuously print readings."""
    sensor = LSM6DS3(i2c, addr)
    sensor.configure()
    time.sleep_ms(100)

    while True:
        try:
            temp_c, accel_g, gyro_dps = sensor.read()
            ax_g, ay_g, az_g = accel_g
            gx_dps, gy_dps, gz_dps = gyro_dps

            print("Temp: {:.2f} C".format(temp_c))
            print(
                "Accel g:  ax={:.3f}, ay={:.3f}, az={:.3f}".format(
                    ax_g, ay_g, az_g
                )
            )
            print(
                "Gyro dps: gx={:.2f}, gy={:.2f}, gz={:.2f}".format(
                    gx_dps, gy_dps, gz_dps
                )
            )
            print("----")
        except OSError as e:
            print("I2C read error:", e)
        time.sleep(0.1)


if __name__ == "__main__":
    # First run a basic scan + WHO_AM_I test.
    basic_test()

    # Uncomment this to go straight into continuous sensor reading once
    # you know the correct I2C address for your board (0x6A or 0x6B).
    configure_and_read(addr=0x6B)