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

// Initialize MPU6050
MPU6050 mpu;

// Variables for storing the previous and current time
unsigned long prevTime;
unsigned long currTime;

// Variables for storing velocity and displacement
float velocityX = 0, velocityY = 0, velocityZ = 0;
float displacementX = 0, displacementY = 0, displacementZ = 0;
float accelX, accelY, accelZ;
float gyroX, gyroY, gyroZ;

struct VelocityDisplacement {
  float velX;
  float velY;
  float velZ;
  float dispX;
  float dispY;
  float dispZ;
};

void updateVelocityDisplacement(VelocityDisplacement &data) {
  data.velX = velocityX;
  data.velY = velocityY;
  data.velZ = velocityZ;
  data.dispX = displacementX;
  data.dispY = displacementY;
  data.dispZ = displacementZ;
}

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();
  prevTime = millis();
}

void loop() {
  readAndPrintSensorData();
  delay(500); // Delay between readings
}

// Function to read and print sensor data
void readAndPrintSensorData() {
  // Read raw accelerometer and gyroscope data
  int16_t ax, ay, az;
  int16_t gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Convert raw values to meaningful units
  accelX = convertAccelerometer(ax);
  accelY = convertAccelerometer(ay);
  accelZ = convertAccelerometer(az);
  gyroX = convertGyroscope(gx);
  gyroY = convertGyroscope(gy);
  gyroZ = convertGyroscope(gz);
  
  
  // Calculate velocity and displacement
  calculateVelocityDisplacement();

  VelocityDisplacement velDispData;
  updateVelocityDisplacement(velDispData);

  float Xvelocity= velDispData.velX;
  float Yvelocity= velDispData.velY;
  float Zvelocity= velDispData.velZ;
  float Xdiplacement= velDispData.dispX;
  float Ydiplacement= velDispData.dispY;
  float Zdiplacement= velDispData.dispZ;

  // Print sensor data to serial monitor
  Serial.print("Velocity (m/s): ");
  Serial.print(velDispData.velX);
  Serial.print(", ");
  Serial.print(velDispData.velY);
  Serial.print(", ");
  Serial.println(velDispData.velZ);

  Serial.print("Displacement (m): ");
  Serial.print(velDispData.dispX);
  Serial.print(", ");
  Serial.print(velDispData.dispY);
  Serial.print(", ");
  Serial.println(velDispData.dispZ);

  Serial.println();
}

// Function to convert raw accelerometer data to g
float convertAccelerometer(int16_t rawValue) {
  return (float)rawValue / 16384.0; // 16384 LSB per g for ± 2g
  //return (float)rawValue / 8192.0; // 8192 LSB per g for ± 4g
  //return (float)rawValue / 4096.0; // 4096 LSB per g for ± 8g
  //return (float)rawValue / 2048.0; // 2048 LSB per g for ± 16g
}

// Function to convert raw gyroscope data to degrees/s
float convertGyroscope(int16_t rawValue) {
  return (float)rawValue / 131.0; // 131 LSB per degree/s full-scale range of ±250 DPS
  //return (float)rawValue / 65.5; // 65.5 LSB per degree/s full-scale range of ±500 DPS
  //return (float)rawValue / 32.8; // 32.8 LSB per degree/s full-scale range of ±1000 DPS
  //return (float)rawValue / 16.4; // 16.4 LSB per degree/s full-scale range of ±2000 DPS
}

// Function to calculate velocity and displacement
void calculateVelocityDisplacement() {
  currTime = millis();
  float dt = (currTime - prevTime) / 1000.0; // Convert to seconds
  prevTime = currTime;

  // Integrate acceleration to get velocity
  velocityX += (accelX * dt) /*+ (gyroX * dt)*/;
  velocityY += (accelY * dt) /*+ (gyroY * dt)*/;
  velocityZ += (accelZ * dt)/*+ (gyroZ * dt)*/;

  // Integrate velocity to get displacement
  displacementX += velocityX * dt;
  displacementY += velocityY * dt;
  displacementZ += velocityZ * dt;
}