#include "I2Cdev.h"
#include "MPU6050.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  #include "Wire.h"
#endif

int sensitivity = 20;

MPU6050 mpu1(0x68);  // MPU6050 with AD0 pin connected to GND;
MPU6050 mpu2(0x69);  // MPU6050 with AD0 pin connected to VCC

int16_t ax1, ay1, az1, gx1, gy1, gz1;
int16_t ax2, ay2, az2, gx2, gy2, gz2;

#define LED_PIN 2
#define buzzer 12

bool State = true;

void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif
    Serial.begin(115200);

    Serial.println("Initializing I2C devices...");
    mpu1.initialize();
    mpu2.initialize();
    
    pinMode(LED_PIN, OUTPUT);
    pinMode(buzzer, OUTPUT);
}

void loop() {
  
    // read raw accel/gyro measurements from device
    mpu1.getMotion6(&ax1, &ay1, &az1, &gx1, &gy1, &gz1);
    mpu2.getMotion6(&ax2, &ay2, &az2, &gx2, &gy2, &gz2);

  float pitch1 = calculatePitch(ax1, ay1, az1);
  float roll1 = calculateRoll(ax1, ay1, az1);

  float pitch2 = calculatePitch(ax2, ay2, az2);
  float roll2 = calculateRoll(ax2, ay2, az2);
  
  Serial.print("Pitch1: ");
  Serial.print(pitch1); 
  Serial.print(" degrees | ");
  Serial.print("Roll1: ");
  Serial.print(roll1); 
  Serial.print(" degrees");

  Serial.print("   |   Pitch2: ");
  Serial.print(pitch2); 
  Serial.print(" degrees | ");
  Serial.print("Roll2: ");
  Serial.print(roll2); 
  Serial.println(" degrees");
  int p1=pitch1,p2=pitch2;
  int r1=roll1,r2=roll2;  
  if ((p1<=p2+sensitivity && p1>=p2-sensitivity) && (r1<=r2+sensitivity && r1>=r2-sensitivity)){
    State = true;
  }
  else
  State = false;
    digitalWrite(LED_PIN, State);
    digitalWrite(buzzer, State);
    delay(100);
}
float calculatePitch(int16_t ax, int16_t ay, int16_t az) {
  return atan2(-ax, sqrt(ay * ay + az * az)) * 180.0 / PI;
}

// Function to calculate roll angle
float calculateRoll(int16_t ax, int16_t ay, int16_t az) {
  return atan2(ay, az) * 180.0 / PI;
}