// Arduino Nano MPU-6050 IMU - PROFESSIONAL 6-AXIS TRACKING
// A4=SDA, A5=SCL | Accel/Gyro/Temp/Tilt | NO LIBRARY REQUIRED

#include <Wire.h>
#define MPU6050_ADDR 0x68

// Sensor data
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
float accelAngleX, accelAngleY, gyroAngleX, gyroAngleY;
float roll, pitch;
unsigned long timer;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x6B); Wire.write(0); Wire.endTransmission();
  
  Serial.println("=== MPU-6050 6-AXIS IMU PROFESSIONAL MONITOR ===");
  Serial.println("AcX AcY AcZ | GyX GyY GyZ | Temp | Pitch Roll");
  timer = micros();
}

void loop() {
  readMPU6050();
  calculateAngles();
  printData();
  delay(50);
}

void readMPU6050() {
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDR, 14, true);
  
  AcX = Wire.read() << 8 | Wire.read();
  AcY = Wire.read() << 8 | Wire.read();  
  AcZ = Wire.read() << 8 | Wire.read();
  Tmp = Wire.read() << 8 | Wire.read();
  GyX = Wire.read() << 8 | Wire.read();
  GyY = Wire.read() << 8 | Wire.read();
  GyZ = Wire.read() << 8 | Wire.read();
}

void calculateAngles() {
  float accelX = AcX / 16384.0;  // ±2g scale
  float accelY = AcY / 16384.0;
  float accelZ = AcZ / 16384.0;
  
  accelAngleX = atan2(accelY, sqrt(accelX*accelX + accelZ*accelZ)) * 180/PI;
  accelAngleY = atan2(-accelX, sqrt(accelY*accelY + accelZ*accelZ)) * 180/PI;
  
  float gyroXrate = GyX / 131.0;  // ±250°/s
  float gyroYrate = GyY / 131.0;
  
  float dt = (micros() - timer) / 1000000.0;
  gyroAngleX += gyroXrate * dt;
  gyroAngleY += gyroYrate * dt;
  timer = micros();
  
  // Complementary filter (90% gyro, 10% accel)
  roll = 0.9 * gyroAngleX + 0.1 * accelAngleX;
  pitch = 0.9 * gyroAngleY + 0.1 * accelAngleY;
}

void printData() {
  float temp = (Tmp / 340.0) + 36.53;
  
  Serial.printf("%6d %6d %6d | %6d %6d %6d | %5.1fC | %6.1f %6.1f\n",
                AcX, AcY, AcZ, GyX, GyY, GyZ, temp, pitch, roll);
  
  // Motion detection
  if(abs(AcX)>8000 || abs(AcY)>8000 || abs(AcZ)>16000) {
    Serial.println("*** MOTION DETECTED! ***");
    digitalWrite(13, HIGH);
  } else {
    digitalWrite(13, LOW);
  }
}

// Add to loop() after printData()
static int shakeCount = 0;
if(abs(AcX)>12000 || abs(AcY)>12000 || abs(AcZ)>18000) {
  shakeCount++;
  if(shakeCount > 10) {
    gyroAngleX = 0; gyroAngleY = 0;  // Reset drift
    Serial.println("*** GYRO RESET ***");
    shakeCount = 0;
  }
}

// Add to setup() after Wire setup
calibrateMPU6050(1000);  // 1000ms calibration

void calibrateMPU6050(int samples) {
  long ax=0, ay=0, az=0, gx=0, gy=0, gz=0;
  for(int i=0; i<samples; i++) {
    readMPU6050();
    ax += AcX; ay += AcY; az += AcZ;
    gx += GyX; gy += GyY; gz += GyZ;
    delay(10);
  }
  // Store offsets for drift removal
}