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

MPU6050 mpu;

// Hàm để đo thời gian đọc dữ liệu
unsigned long measureReadTime() {
  unsigned long startTime = micros(); // Thời gian bắt đầu
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az); // Đọc dữ liệu
  return micros() - startTime; // Trả về thời gian đã trôi qua
}

void setup() {
  Serial.begin(115200);
  
  // Khởi động MPU6050
  Wire.begin(21, 22); // SDA, SCL
  mpu.initialize();
  
  // Kiểm tra kết nối
  if (!mpu.testConnection()) {
    Serial.println("Kết nối với MPU6050 thất bại!");
    while (1);
  }
  
  // Thay đổi tốc độ I2C
  Wire.setClock(100000); // 100 kHz
  Serial.println("Tốc độ giao tiếp I2C: 100 kHz");
  
  // Đo thời gian đọc dữ liệu
  unsigned long readTime = measureReadTime();
  Serial.print("Thời gian đọc dữ liệu (100 kHz): ");
  Serial.print(readTime);
  Serial.println(" µs");

  // Thay đổi tốc độ lên 400 kHz
  Wire.setClock(400000); // 400 kHz
  Serial.println("Tốc độ giao tiếp I2C: 400 kHz");
  
  // Đo thời gian đọc dữ liệu
  readTime = measureReadTime();
  Serial.print("Thời gian đọc dữ liệu (400 kHz): ");
  Serial.print(readTime);
  Serial.println(" µs");
}

void loop() {
  // Không làm gì trong loop
}