#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Địa chỉ I2C của MPU6050
#define MPU6050_ADDR 0x68

// Các thanh ghi của MPU6050
#define PWR_MGMT_1 0x6B
#define ACCEL_X 0x3B
#define ACCEL_Y 0x3D
#define ACCEL_Z 0x3F

void setup() {
  Serial.begin(115200);

  // Khởi tạo I2C
  Wire.begin();

  // Khởi tạo màn hình OLED
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);

  // Khởi động MPU6050
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(PWR_MGMT_1);
  Wire.write(0); // Đặt MPU6050 ở chế độ hoạt động
  Wire.endTransmission();
}

void loop() {
  // Đọc giá trị gia tốc
  int16_t ax, ay, az;

  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(ACCEL_X); // Địa chỉ của gia tốc kế trục X
  Wire.endTransmission();
  Wire.requestFrom(MPU6050_ADDR, 6); // Yêu cầu 6 byte dữ liệu
  
  ax = (Wire.read() << 8) | Wire.read(); // Trục X
  ay = (Wire.read() << 8) | Wire.read(); // Trục Y
  az = (Wire.read() << 8) | Wire.read(); // Trục Z

  // Hiển thị kết quả trên Serial Monitor
  Serial.print("Gia tốc X: "); Serial.print(ax);
  Serial.print(", Y: "); Serial.print(ay);
  Serial.print(", Z: "); Serial.println(az);

  // Hiển thị kết quả trên màn hình OLED
  display.clearDisplay();
  display.setCursor(0, 0);
  display.print("Gia tốc X: "); display.println(ax);
  display.print("Gia tốc Y: "); display.println(ay);
  display.print("Gia tốc Z: "); display.println(az);
  display.display();

  delay(500); // Đợi 500 ms trước khi đọc lại
}