# Motor configuration for a quadcopter
MOTOR_CONFIG = {
    'M1': {'position': 'front-left',  'rotation': 'CW',  'esc_pin': 'PWM1', 'kv': 2300},
    'M2': {'position': 'front-right', 'rotation': 'CCW', 'esc_pin': 'PWM2', 'kv': 2300},
    'M3': {'position': 'rear-left',   'rotation': 'CCW', 'esc_pin': 'PWM3', 'kv': 2300},
    'M4': {'position': 'rear-right',  'rotation': 'CW',  'esc_pin': 'PWM4', 'kv': 2300},
}

# Motor mixing formula (how FC translates pilot input to motor speeds)
def motor_mix(roll, pitch, yaw, throttle):
    """Compute motor speeds from attitude commands"""
    m1 = throttle + roll - pitch + yaw  # Front-left
    m2 = throttle - roll - pitch - yaw  # Front-right
    m3 = throttle + roll + pitch - yaw  # Rear-left
    m4 = throttle - roll + pitch + yaw  # Rear-right
    return [clamp(m1), clamp(m2), clamp(m3), clamp(m4)]

def clamp(value, min_val=1000, max_val=2000):
    return max(min_val, min(max_val, int(value)))

import smbus2
import time

class MPU6050:
    """Read accelerometer and gyroscope from MPU6050"""
    def __init__(self, bus=1, addr=0x68):
        self.bus = smbus2.SMBus(bus)
        self.addr = addr
        # Wake up sensor
        self.bus.write_byte_data(self.addr, 0x6B, 0x00)
        # Set accelerometer range to ±4g
        self.bus.write_byte_data(self.addr, 0x1C, 0x08)
        # Set gyro range to ±500°/s
        self.bus.write_byte_data(self.addr, 0x1B, 0x08)

    def read_raw(self, reg):
        high = self.bus.read_byte_data(self.addr, reg)
        low = self.bus.read_byte_data(self.addr, reg + 1)
        val = (high << 8) | low
        return val - 65536 if val > 32767 else val

    def read_all(self):
        return {
            'ax': self.read_raw(0x3B) / 8192.0,
            'ay': self.read_raw(0x3D) / 8192.0,
            'az': self.read_raw(0x3F) / 8192.0,
            'gx': self.read_raw(0x43) / 65.5,
            'gy': self.read_raw(0x45) / 65.5,
            'gz': self.read_raw(0x47) / 65.5,
        }

# Usage
imu = MPU6050()
while True:
    data = imu.read_all()
    print(f"Accel: {data['ax']:.2f}, {data['ay']:.2f}, {data['az']:.2f} g")
    print(f"Gyro:  {data['gx']:.1f}, {data['gy']:.1f}, {data['gz']:.1f} °/s")
    time.sleep(0.01)