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

MPU6050 mpu;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu.initialize();
  
  if (!mpu.testConnection()) {
    Serial.println("Kết nối với MPU6050 thất bại");
    while (1);
  }
  
  Serial.println("MPU6050 đã kết nối thành công");
}

void loop() {
  // Variables to hold accelerometer data
  int16_t ax, ay, az;
  
  // Get raw accelerometer measurements
  mpu.getAcceleration(&ax, &ay, &az);
  
  // Calculate tilt angles
  float xAngle = atan2(ay, az) * 180.0 / PI;
  float yAngle = atan2(ax, az) * 180.0 / PI;
  
  // Print tilt information
  Serial.print("Góc nghiêng X: ");
  Serial.print(xAngle);
  Serial.print(" độ, Góc nghiêng Y: ");
  Serial.print(yAngle);
  Serial.println(" độ");
  
  // Determine tilt direction
  if (abs(xAngle) > 10) {
    Serial.print("Nghiêng theo trục X: ");
    if (xAngle > 0) {
      Serial.println("Nghiêng sang phải");
    } else {
      Serial.println("Nghiêng sang trái");
    }
  } else if (abs(yAngle) > 10) {
    Serial.print("Nghiêng theo trục Y: ");
    if (yAngle > 0) {
      Serial.println("Nghiêng về phía trước");
    } else {
      Serial.println("Nghiêng về phía sau");
    }
  } else {
    Serial.println("Không nghiêng");
  }
  
  delay(500); // Delay between readings
}