#include <Wire.h>
#include <MPU6050.h>
#include <Kalman.h>

// Initialize two MPU6050 objects with different I2C addresses
MPU6050 mpu1(0x68);
MPU6050 mpu2(0x69);

// Kalman filter objects for both sensors
Kalman kalmanX1;
Kalman kalmanY1;
Kalman kalmanX2;
Kalman kalmanY2;

const float alpha = 0.98; // Complementary filter constant


unsigned long lastTime = 0; // Last time the loop was updated
const float dt = 0.01; // Time interval between readings (10 ms)

void setup() {
  Serial.begin(115200);
  Wire.begin();
  
  mpu1.initialize();
  mpu2.initialize();
  
  if (mpu1.testConnection()) {
    Serial.println("MPU6050 #1 connection successful");
  } else {
    Serial.println("MPU6050 #1 connection failed");
  }

  if (mpu2.testConnection()) {
    Serial.println("MPU6050 #2 connection successful");
  } else {
    Serial.println("MPU6050 #2 connection failed");
  }

  // Initialize Kalman filters
  kalmanX1.setAngle(0); // Set initial angle for X-axis
  kalmanY1.setAngle(0); // Set initial angle for Y-axis
  kalmanX2.setAngle(0); // Set initial angle for X-axis
  kalmanY2.setAngle(0); // Set initial angle for Y-axis
}

void loop() {
  int16_t ax1, ay1, az1, gx1, gy1, gz1;
  int16_t ax2, ay2, az2, gx2, gy2, gz2;
  float accelAngleX1, accelAngleY1, accelAngleX2, accelAngleY2;
  float gyroRateX1, gyroRateY1, gyroRateX2, gyroRateY2;
  float angleX1, angleY1, angleX2, angleY2;
  float combinedAngleX, combinedAngleY;

  unsigned long currentTime = millis();
  float deltaTime = (currentTime - lastTime) / 1000.0; // Convert to seconds
  lastTime = currentTime;

  mpu1.getMotion6(&ax1, &ay1, &az1, &gx1, &gy1, &gz1);
  mpu2.getMotion6(&ax2, &ay2, &az2, &gx2, &gy2, &gz2);

  // Calculate accelerometer angles (conversion factor 180/PI)
  accelAngleX1 = atan2(ay1, az1) * RAD_TO_DEG;
  accelAngleY1 = atan2(ax1, az1) * RAD_TO_DEG;
  accelAngleX2 = atan2(ay2, az2) * RAD_TO_DEG;
  accelAngleY2 = atan2(ax2, az2) * RAD_TO_DEG;

  // Calculate gyroscope rates
  gyroRateX1 = gx1 / 131.0;
  gyroRateY1 = gy1 / 131.0;
  gyroRateX2 = gx2 / 131.0;
  gyroRateY2 = gy2 / 131.0;

  // Integrate gyroscope data and apply complementary filter
  angleX1 = alpha * (angleX1 + gyroRateX1 * deltaTime) + (1 - alpha) * accelAngleX1;
  angleY1 = alpha * (angleY1 + gyroRateY1 * deltaTime) + (1 - alpha) * accelAngleY1;
  angleX2 = alpha * (angleX2 + gyroRateX2 * deltaTime) + (1 - alpha) * accelAngleX2;
  angleY2 = alpha * (angleY2 + gyroRateY2 * deltaTime) + (1 - alpha) * accelAngleY2;

  // Apply Kalman filter to the angles
  float kalAngleX1 = kalmanX1.getAngle(angleX1, gyroRateX1, deltaTime);
  float kalAngleY1 = kalmanY1.getAngle(angleY1, gyroRateY1, deltaTime);
  float kalAngleX2 = kalmanX2.getAngle(angleX2, gyroRateX2, deltaTime);
  float kalAngleY2 = kalmanY2.getAngle(angleY2, gyroRateY2, deltaTime);

  // Average the angles from both sensors
  combinedAngleX = (kalAngleX1 + kalAngleX2) * 0.5;
  combinedAngleY = (kalAngleY1 + kalAngleY2) * 0.5;

  // Print debug information
  Serial.print("Accel AngleX1: ");
  Serial.print(accelAngleX1);
  Serial.print("\tGyro RateX1: ");
  Serial.print(gyroRateX1);
  Serial.print("\tAngleX1: ");
  Serial.print(angleX1);
  Serial.print("\tKalman AngleX1: ");
  Serial.print(kalAngleX1);

  Serial.print("\tAccel AngleY1: ");
  Serial.print(accelAngleY1);
  Serial.print("\tGyro RateY1: ");
  Serial.print(gyroRateY1);
  Serial.print("\tAngleY1: ");
  Serial.print(angleY1);
  Serial.print("\tKalman AngleY1: ");
  Serial.print(kalAngleY1);

  Serial.print("\tAccel AngleX2: ");
  Serial.print(accelAngleX2);
  Serial.print("\tGyro RateX2: ");
  Serial.print(gyroRateX2);
  Serial.print("\tAngleX2: ");
  Serial.print(angleX2);
  Serial.print("\tKalman AngleX2: ");
  Serial.print(kalAngleX2);

  Serial.print("\tAccel AngleY2: ");
  Serial.print(accelAngleY2);
  Serial.print("\tGyro RateY2: ");
  Serial.print(gyroRateY2);
  Serial.print("\tAngleY2: ");
  Serial.print(angleY2);
  Serial.print("\tKalman AngleY2: ");
  Serial.print(kalAngleY2);

  Serial.print("\tCombined AngleX: ");
  Serial.print(combinedAngleX);
  Serial.print("\tCombined AngleY: ");
  Serial.println(combinedAngleY);

  delay(1000); // Adjust delay as needed
}