import smbus
import time

# I2C setup
bus = smbus.SMBus(1)  # Raspberry Pi I2C bus 1

# I2C Addresses
ACCEL_ADDR = 0x53  # ADXL345 Accelerometer
GYRO_ADDR = 0x68   # ITG3205 Gyroscope
MAG_ADDR = 0x0D    # QMC5883L Magnetometer

### Initialize ADXL345 ###
def initialize_adxl345():
    try:
        bus.write_byte_data(ACCEL_ADDR, 0x2D, 0x08)  # Enable measurement mode
        print("ADXL345 Initialized.")
    except OSError:
        print("Error: Could not initialize ADXL345.")

def read_accelerometer():
    try:
        data = bus.read_i2c_block_data(ACCEL_ADDR, 0x32, 6)
        x = (data[1] << 8) | data[0]
        y = (data[3] << 8) | data[2]
        z = (data[5] << 8) | data[4]

        # Convert to signed values
        x = x if x < 32768 else x - 65536
        y = y if y < 32768 else y - 65536
        z = z if z < 32768 else z - 65536

        return (x, y, z)
    except OSError:
        return None

### Initialize ITG3205 ###
def initialize_itg3205():
    try:
        bus.write_byte_data(GYRO_ADDR, 0x3E, 0x00)  # Power management - wake up sensor
        bus.write_byte_data(GYRO_ADDR, 0x15, 0x07)  # Set sample rate divider
        print("ITG3205 Initialized.")
    except OSError:
        print("Error: Could not initialize ITG3205.")

def read_gyroscope():
    try:
        data = bus.read_i2c_block_data(GYRO_ADDR, 0x1D, 6)
        x = (data[0] << 8) | data[1]
        y = (data[2] << 8) | data[3]
        z = (data[4] << 8) | data[5]

        # Convert to signed values
        x = x if x < 32768 else x - 65536
        y = y if y < 32768 else y - 65536
        z = z if z < 32768 else z - 65536

        return (x, y, z)
    except OSError:
        return None

### Initialize QMC5883L ###
def initialize_qmc5883l():
    try:
        bus.write_byte_data(MAG_ADDR, 0x09, 0x1D)  # 10Hz, Continuous mode, 2G range
        time.sleep(0.1)
        print("QMC5883L Initialized.")
    except OSError:
        print("Error: Could not initialize QMC5883L.")

def read_magnetometer():
    try:
        data = bus.read_i2c_block_data(MAG_ADDR, 0x00, 6)
        x = (data[1] << 8) | data[0]
        y = (data[3] << 8) | data[2]
        z = (data[5] << 8) | data[4]

        # Convert to signed values
        x = x if x < 32768 else x - 65536
        y = y if y < 32768 else y - 65536
        z = z if z < 32768 else z - 65536

        return (x, y, z)
    except OSError:
        return None

### Main Function ###
if __name__ == "__main__":
    initialize_adxl345()
    initialize_itg3205()
    initialize_qmc5883l()
    
    while True:
        accel_data = read_accelerometer()
        gyro_data = read_gyroscope()
        mag_data = read_magnetometer()

        print(f"Accelerometer: X={accel_data[0]}, Y={accel_data[1]}, Z={accel_data[2]}")
        print(f"Gyroscope: X={gyro_data[0]}, Y={gyro_data[1]}, Z={gyro_data[2]}")
        print(f"Magnetometer: X={mag_data[0]}, Y={mag_data[1]}, Z={mag_data[2]}")
        print("-" * 50)

        time.sleep(1)  # Read every second