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

MPU6050 mpu;

float accelX, accelY, accelZ;  // قراءات التسارع في المحاور الثلاثة
float gyroX, gyroY, gyroZ;  // قراءات الجيروسكوب (الدوران الزاوي) في المحاور الثلاثة
float temperature;  // درجة الحرارة
float velocityX = 0, velocityY = 0, velocityZ = 0;  // السرعات في المحاور الثلاثة
float dt;  // الفرق الزمني بين القراءات

// Kalman filter variables for X, Y, and Z axes
float x_hatX = 0, x_hatY = 0, x_hatZ = 0;  // التقديرات المتوقعة للسرعة في المحاور الثلاثة
float P_X = 1, P_Y = 1, P_Z = 1;    // مصفوفة التباين للمحاور الثلاثة
float Q = 0.01;  // ضوضاء العملية (ثابت لكل المحاور)
float R = 0.1;   // ضوضاء المستشعر (ثابت لكل المحاور)
float K_X, K_Y, K_Z;  // كسب كالمان للمحاور الثلاثة

unsigned long lastTime = 0;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed!");
    while (1);
  }
}

void loop() {
  unsigned long currentTime = millis();
  dt = (currentTime - lastTime) / 1000.0;  // تحويل الوقت إلى ثواني
  lastTime = currentTime;

  // الحصول على بيانات التسارع من MPU6050
  accelX = mpu.getAccelerationX() / 16384.0;  // قراءة التسارع في المحور X
  accelY = mpu.getAccelerationY() / 16384.0;  // قراءة التسارع في المحور Y
  accelZ = mpu.getAccelerationZ() / 16384.0;  // قراءة التسارع في المحور Z

  // الحصول على بيانات الجيروسكوب (الدوران الزاوي)
  gyroX = mpu.getRotationX() / 131.0;  // قراءة الدوران الزاوي في المحور X
  gyroY = mpu.getRotationY() / 131.0;  // قراءة الدوران الزاوي في المحور Y
  gyroZ = mpu.getRotationZ() / 131.0;  // قراءة الدوران الزاوي في المحور Z

  // الحصول على درجة الحرارة من المستشعر
  temperature = mpu.getTemperature() / 340.0 + 36.53;  // تحويل قيمة القراءة إلى درجة مئوية

  // *** فلتر كالمان للمحور X ***
  x_hatX += accelX * dt;  // التنبؤ بالسرعة بناءً على التسارع
  P_X += Q;  // تحديث التباين بناءً على الضوضاء
  K_X = P_X / (P_X + R);  // حساب كسب كالمان
  x_hatX = x_hatX + K_X * (accelX - x_hatX);  // تحديث السرعة المتوقعة
  P_X = (1 - K_X) * P_X;  // تحديث التباين

  // *** فلتر كالمان للمحور Y ***
  x_hatY += accelY * dt;  // التنبؤ بالسرعة بناءً على التسارع
  P_Y += Q;  // تحديث التباين بناءً على الضوضاء
  K_Y = P_Y / (P_Y + R);  // حساب كسب كالمان
  x_hatY = x_hatY + K_Y * (accelY - x_hatY);  // تحديث السرعة المتوقعة
  P_Y = (1 - K_Y) * P_Y;  // تحديث التباين

  // *** فلتر كالمان للمحور Z ***
  x_hatZ += accelZ * dt;  // التنبؤ بالسرعة بناءً على التسارع
  P_Z += Q;  // تحديث التباين بناءً على الضوضاء
  K_Z = P_Z / (P_Z + R);  // حساب كسب كالمان
  x_hatZ = x_hatZ + K_Z * (accelZ - x_hatZ);  // تحديث السرعة المتوقعة
  P_Z = (1 - K_Z) * P_Z;  // تحديث التباين

  // عرض البيانات على الشاشة
  Serial.print("Acceleration X: ");
  Serial.print(accelX);
  Serial.print("t Y: ");
  Serial.print(accelY);
  Serial.print("t Z: ");
  Serial.println(accelZ);
  
  
  Serial.print("Estimated Speed X: ");
  Serial.print(x_hatX);
  Serial.print("t Y: ");
  Serial.print(x_hatY);
  Serial.print("t Z: ");
  Serial.println(x_hatZ);
  

  // عرض الدوران الزاوي
  Serial.print("Gyro X: ");
  Serial.print(gyroX);
  Serial.print("t Y: ");
  Serial.print(gyroY);
  Serial.print("t Z: ");
  Serial.println(gyroZ);

  // عرض درجة الحرارة
  Serial.print("Temperature: ");
  Serial.println(temperature);

  delay(100);  // تأخير لتحديث البيانات
}