##############################################################
#          MPU6050 Accelerator + Gyroscope Interface         #
##############################################################
#
# Interface MPU6050 with Raspberry Pi Pico using MicroPython (Hardware & Simulation)
#
# Check out the link for Code explanation and Hardware details
# Link: 
#   http://tech.arunkumarn.in/blogs/raspberry-pi-pico/how-to-interface-mpu6050-imu-sensor-with-raspberry-pi-pico-using-micropython/
#
#


# Upload after imu.py and vector3d.py are added

from imu import MPU6050
from time import sleep
from machine import Pin, I2C

# Initialize I2C0 on GP0 (SDA) and GP1 (SCL) at 400kHz
i2c = I2C(0, sda=Pin(0), scl=Pin(1), freq=400000)

# Create MPU6050 instance
imu = MPU6050(i2c)

print("MPU6050 initialized. Reading sensor data...")
print("ax(accel X)\tay(accel Y)\taz(accel Z)\tgx(gyro X)\tgy(gyro Y)\tgz(gyro Z)\tTemp(°C)")
print("-" * 100)

try:
    while True:
        # Read and round sensor values for clean output
        ax = round(imu.accel.x, 2)
        ay = round(imu.accel.y, 2)
        az = round(imu.accel.z, 2)
        gx = round(imu.gyro.x, 1)
        gy = round(imu.gyro.y, 1)
        gz = round(imu.gyro.z, 1)
        temp = round(imu.temperature, 2)

        # Print formatted data (carriage return for in-place update)
        print(f"{ax}\t{ay}\t{az}\t{gx}\t{gy}\t{gz}\t{temp}", end="\r")

        # Small delay to stabilize readings and reduce console spam
        sleep(0.2)

except KeyboardInterrupt:
    print("\n\nSensor reading stopped by user.")
    # Optional: Deinitialize I2C if needed for power saving
    i2c.deinit()


# mpu = MPU6050(i2c)

# # wake up the MPU6050 from sleep
# mpu.wake()

# # continuously print the data
# while True:
#     gyro = mpu.read_gyro_data()
#     accel = mpu.read_accel_data()
#     print(f"Gyro: {gyro[0]:.2f}, {gyro[1]:.2f}, {gyro[2]:.2f} , Accel: {accel[0]:.2f}, {accel[1]:.2f}, {accel[2]:.2f}", end="\r")
#     sleep(0.1)