#include <Wire.h>

#define MPU1_ADDR 0x68   // Bus0 - AD0 = GND
#define MPU2_ADDR 0x69   // Bus0 - AD0 = VCC
#define MPU3_ADDR 0x68   // Bus1 - AD0 = GND
#define MPU4_ADDR 0x69   // Bus1 - AD0 = VCC

#define SENSITIVITY 4096.0f
const float g_const = 9.81;

// Pins - MUST match your physical wiring
#define SDA0 21
#define SCL0 22
#define SDA1 19
#define SCL1 18

// --- MPU6050 Initialisierung ---
bool mpuInit(TwoWire &bus, uint8_t addr, const char* sensorName) {
  Serial.print("Initializing ");
  Serial.print(sensorName);
  Serial.print(" (0x");
  Serial.print(addr, HEX);
  Serial.println(")...");
  
  bus.beginTransmission(addr);
  bus.write(0x6B); // PWR_MGMT_1
  bus.write(0x00); // Wake up
  if (bus.endTransmission(true) != 0) {
    Serial.print("ERROR: Init failed for ");
    Serial.println(sensorName);
    return false;
  }
  delay(10);

  bus.beginTransmission(addr);
  bus.write(0x1C); // ACCEL_CONFIG
  bus.write(0x10); // ±8g
  if (bus.endTransmission(true) != 0) {
    Serial.print("ERROR: Config failed for ");
    Serial.println(sensorName);
    return false;
  }
  delay(10);
  
  Serial.print(sensorName);
  Serial.println(" initialized successfully!");
  return true;
}

// --- MPU6050 Z-Achse auslesen ---
float readAccZ(TwoWire &bus, uint8_t addr, const char* sensorName) {
  bus.beginTransmission(addr);
  bus.write(0x3F); // ACCEL_ZOUT_H
  if (bus.endTransmission(false) != 0) {
    Serial.print("ERROR: Communication failed with ");
    Serial.println(sensorName);
    return 0.0f;
  }

  uint8_t bytesReceived = bus.requestFrom(addr, 2, true);
  if (bytesReceived < 2) {
    Serial.print("ERROR: Incomplete data from ");
    Serial.println(sensorName);
    return 0.0f;
  }

  uint8_t highByte = bus.read();
  uint8_t lowByte = bus.read();
  int16_t raw = (highByte << 8) | lowByte;
  return (raw / SENSITIVITY) * g_const;
}

void scanI2CBus(TwoWire &bus, const char* busName) {
  byte error, address;
  int nDevices = 0;
  
  Serial.print("Scanning ");
  Serial.print(busName);
  Serial.println("...");

  for(address = 1; address < 127; address++ ) {
    bus.beginTransmission(address);
    error = bus.endTransmission();
    
    if (error == 0) {
      Serial.print("Device found at 0x");
      if (address < 16) Serial.print("0");
      Serial.print(address, HEX);
      Serial.println();
      nDevices++;
    }
  }
  
  if (nDevices == 0) {
    Serial.println("No I2C devices found on this bus!");
  }
}

void setup() {
  Serial.begin(115200);
  while (!Serial) {
    ; // wait for serial port to connect
  }
  delay(2000);
  Serial.println("\n=== MPU6050 Sensor Array Test ===");

  // Initialize I2C buses with proper error checking
  Serial.println("Initializing I2C Bus 0 (Wire)...");
  Wire.begin(SDA0, SCL0, 100000);
  Serial.println("I2C Bus 0 initialized successfully!");
  delay(100);

  Serial.println("Initializing I2C Bus 1 (Wire1)...");
  Wire1.begin(SDA1, SCL1, 100000);
  Serial.println("I2C Bus 1 initialized successfully!");
  delay(100);

  // Scan buses to detect devices
  Serial.println("\nScanning I2C Bus 0...");
  scanI2CBus(Wire, "Bus 0");
  
  Serial.println("Scanning I2C Bus 1...");
  scanI2CBus(Wire1, "Bus 1");
  delay(1000);

  // Initialize sensors
  Serial.println("\nInitializing sensors...");
  bool allSensorsOK = true;
  
  if (!mpuInit(Wire, MPU1_ADDR, "MPU1 (Bus0, Addr 0x68)")) allSensorsOK = false;
  delay(50);
  
  if (!mpuInit(Wire, MPU2_ADDR, "MPU2 (Bus0, Addr 0x69)")) allSensorsOK = false;
  delay(50);
  
  if (!mpuInit(Wire1, MPU3_ADDR, "MPU3 (Bus1, Addr 0x68)")) allSensorsOK = false;
  delay(50);
  
  if (!mpuInit(Wire1, MPU4_ADDR, "MPU4 (Bus1, Addr 0x69)")) allSensorsOK = false;
  delay(50);

  if (allSensorsOK) {
    Serial.println("\nAll sensors initialized successfully!");
    Serial.println("Z1\tZ2\tZ3\tZ4");
    Serial.println("----------------------------------------");
  } else {
    Serial.println("\nWARNING: Some sensors failed to initialize!");
  }
}

void loop() {
  // Read sensors with error handling
  float z1 = readAccZ(Wire, MPU1_ADDR, "MPU1");
  delay(2);
  float z2 = readAccZ(Wire, MPU2_ADDR, "MPU2");
  delay(2);
  float z3 = readAccZ(Wire1, MPU3_ADDR, "MPU3");
  delay(2);
  float z4 = readAccZ(Wire1, MPU4_ADDR, "MPU4");

  // Print readings
  Serial.print(z1, 3); Serial.print('\t');
  Serial.print(z2, 3); Serial.print('\t');
  Serial.print(z3, 3); Serial.print('\t');
  Serial.println(z4, 3);

  delay(100); // 10 Hz for stability
}