#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

// Define your I2C pins, buzzer pin, and LED pin
const int SDA_PIN = 18; // Usually GPIO4
const int SCL_PIN = 19; // Usually GPIO5
const int BUZZER_PIN = 3; // Connect your buzzer to this pin
const int LED_PIN = 2; // Connect your LED to this pin

// Create an MPU6050 object
Adafruit_MPU6050 mpu;

// Variables to store the calibration values
float baseAccelX = 0;
float baseAccelY = 0;
float baseAccelZ = 0;
float baseGyroX = 0;
float baseGyroY = 0;
float baseGyroZ = 0;

// Variable to track the shake duration
unsigned long shakeStartTime = 0;
bool isShaking = false;

void setup() {
  // Initialize serial communication
  Serial.begin(115200);
  
  // Initialize I2C communication
  Wire.begin(SDA_PIN, SCL_PIN);
  
  // Initialize the MPU6050
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  
  // Set up the accelerometer and gyroscope range
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  
  // Calibrate the accelerometer and gyroscope
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  baseAccelX = a.acceleration.x;
  baseAccelY = a.acceleration.y;
  baseAccelZ = a.acceleration.z;
  baseGyroX = g.gyro.x;
  baseGyroY = g.gyro.y;
  baseGyroZ = g.gyro.z;
  
  // Set up the buzzer and LED
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(LED_PIN, OUTPUT);
  
  Serial.println("MPU6050 ready and calibrated!");
}

void loop() {
  // Read the sensor values
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  
  // Calculate the difference from the calibrated values for acceleration
  float diffAccelX = abs(a.acceleration.x - baseAccelX);
  float diffAccelY = abs(a.acceleration.y - baseAccelY);
  float diffAccelZ = abs(a.acceleration.z - baseAccelZ);
  float maxDiffAccel = max(diffAccelX, max(diffAccelY, diffAccelZ)) * 2;
  
  // Calculate the difference from the calibrated values for gyroscope
  float diffGyroX = abs(g.gyro.x - baseGyroX);
  float diffGyroY = abs(g.gyro.y - baseGyroY);
  float diffGyroZ = abs(g.gyro.z - baseGyroZ);
  float maxDiffGyro = max(diffGyroX, max(diffGyroY, diffGyroZ)) * 5;
  
  // Use the larger of the differences between gyro and accel
  float maxDiff = max(maxDiffAccel, maxDiffGyro);
  
  // Check if the difference exceeds the threshold
  if (maxDiff > 1) {
    // Record the start time of the shake
    if (!isShaking) {
      shakeStartTime = millis();
      isShaking = true;
    }
    
    // Calculate the number of beeps based on the shake force
    int beeps = (int)pow(2, (maxDiff - 1));
    
    // Perform the beeps
    for (int i = 0; i < beeps; i++) {
      tone(BUZZER_PIN, 1000, 10); // Beep for 100 milliseconds
      delay(200); // Wait for 150 milliseconds between beeps
    }
    
    // Turn on the LED
    digitalWrite(LED_PIN, HIGH);
  } else {
    // If shaking has stopped, reset the shake start time and shaking status
    if (isShaking) {
      shakeStartTime = 0;
      isShaking = false;
      
      // Turn off the LED
      digitalWrite(LED_PIN, LOW);
    }
  }
  
  // Small delay before the next reading
  delay(100);
}