from machine import I2C, Pin
import time

# Setup standard ESP32 hardware I2C peripheral bus
i2c = I2C(0, scl=Pin(22), sda=Pin(21), freq=100000)
MPU_ADDR = 0x68  # MPU6050 default I2C address (AD0 pin tied LOW)

# MPU6050 register map (from the official Register Map document)
REG_PWR_MGMT_1   = 0x6B  # Power management — needed to wake the device
REG_WHO_AM_I     = 0x75  # Device identity — should return 0x68
REG_ACCEL_XOUT_H = 0x3B  # First accelerometer data register (6 bytes total)
REG_TEMP_OUT_H   = 0x41  # Temperature data register (2 bytes)
REG_GYRO_XOUT_H  = 0x43  # First gyroscope data register (6 bytes total)

# Sensitivity scale factors for the default full-scale ranges
ACCEL_SCALE = 16384.0  # LSB/g       (±2g range, default)
GYRO_SCALE  = 131.0    # LSB/(°/s)   (±250 °/s range, default)


def write_register(reg_addr, value):
    """Write a single byte to a register on the MPU6050."""
    #****************** Write your Code Here ******************
    #****************** Start ******************


    #****************** End *******************

def read_register(reg_addr, num_bytes):
    """Read num_bytes from a register on the MPU6050.

    Step 1: Send the register pointer (write phase, no STOP — pass False)
    Step 2: Issue a repeated START and read the requested bytes
    """
    #****************** Write your Code Here ******************
    #****************** Start ******************


    #****************** End *******************
    return i2c.readfrom(MPU_ADDR, num_bytes)


def to_signed_16(high_byte, low_byte):
    """Combine two bytes into a signed 16-bit value (two's complement).
    The MPU6050 reports sensor data as big-endian 16-bit two's complement.
    """
    raw = (high_byte << 8) | low_byte
    if raw & 0x8000:
        raw -= 65536
    return raw


# --- Bus verification ---
print("Scanning for I2C devices...")
devices = i2c.scan()
if MPU_ADDR in devices:
    print(f"Success: MPU6050 detected at address {hex(MPU_ADDR)}")
else:
    print("Error: MPU6050 not responding. Check wiring and pull-ups.")

# --- Device wake-up ---
# The MPU6050 powers up in sleep mode. Writing 0x00 to PWR_MGMT_1
# clears the SLEEP bit and selects the internal 8 MHz oscillator.
write_register(REG_PWR_MGMT_1, 0x00)
time.sleep(0.1)  # Allow the internal oscillator to stabilize

# --- Identity check ---
chip_id = read_register(REG_WHO_AM_I, 1)[0]
print(f"WHO_AM_I register returned: {hex(chip_id)} (expected 0x68)")

# --- Main acquisition loop ---
while True:
    try:
        # Accelerometer: 6 consecutive bytes from REG_ACCEL_XOUT_H
        accel_raw = read_register(REG_ACCEL_XOUT_H, 6)
        ax = to_signed_16(accel_raw[0], accel_raw[1]) / ACCEL_SCALE
        ay = to_signed_16(accel_raw[2], accel_raw[3]) / ACCEL_SCALE
        az = to_signed_16(accel_raw[4], accel_raw[5]) / ACCEL_SCALE

        # Temperature: 2 bytes from REG_TEMP_OUT_H
        # Datasheet formula: T_celsius = (TEMP_OUT / 340) + 36.53
        temp_raw = read_register(REG_TEMP_OUT_H, 2)
        t_value = to_signed_16(temp_raw[0], temp_raw[1])
        temperature = (t_value / 340.0) + 36.53

        # Gyroscope: 6 consecutive bytes from REG_GYRO_XOUT_H
        gyro_raw = read_register(REG_GYRO_XOUT_H, 6)
        gx = to_signed_16(gyro_raw[0], gyro_raw[1]) / GYRO_SCALE
        gy = to_signed_16(gyro_raw[2], gyro_raw[3]) / GYRO_SCALE
        gz = to_signed_16(gyro_raw[4], gyro_raw[5]) / GYRO_SCALE

        print(f"Accel(g): X={ax:+.2f} Y={ay:+.2f} Z={az:+.2f}  |  ",
              f"Gyro(°/s): X={gx:+7.1f} Y={gy:+7.1f} Z={gz:+7.1f}  |  ",
              f"Temp: {temperature:.2f}°C")

    except Exception as e:
        print("Communication interface fault:", e)

    time.sleep(0.5)