#include <MPU6050_tockn.h>
#include <Wire.h>
#include <math.h>
#include <ESP32Servo.h>
#include <pitches.h>

Servo servo;
MPU6050 mpu6050(Wire);

const int pin_servo = 17;
const int pin_speaker = 18;
const int pin_led = 5;

int pos;

float velocityX = 0, velocityY = 0, velocityZ = 0; // Khởi tạo vận tốc
unsigned long previousTime = 0; // Thời gian trước đó
float dt = 0.1; // Giá trị mặc định, sẽ được tính lại

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu6050.begin();
  servo.attach(pin_servo);
  pinMode(pin_led, OUTPUT);
  pinMode(pin_speaker, OUTPUT);
  mpu6050.calcGyroOffsets(true);
}

void loop() {
  mpu6050.update();

  pos = map(mpu6050.getAngleZ(), -180, 180, 0, 180);
  servo.write(pos);

  unsigned long currentTime = millis(); // Thời gian hiện tại
  dt = (currentTime - previousTime) / 1000.0; // Tính toán thời gian trôi qua (đổi sang giây)
  previousTime = currentTime; // Cập nhật thời gian trước đó

  mpu6050.update();

  // Đọc gia tốc
  float ax = mpu6050.getAccX();
  float ay = mpu6050.getAccY();
  float az = mpu6050.getAccZ();

  // Tính toán vận tốc từ gia tốc
  velocityX += ax * dt;
  velocityY += ay * dt;
  velocityZ += az * dt;

  Serial.print("Velocity X:");
  Serial.print(velocityX);
  Serial.print(", Y: ");
  Serial.print(velocityY);
  Serial.print(", Z: ");
  Serial.println(velocityZ);

  if (velocityX > 0 || velocityY > 0 || velocityZ > 0) {
    digitalWrite(pin_led, HIGH); // Phát hiện chuyển động
  }else{
    digitalWrite(pin_led, LOW);
  }; // Không phát hiện chuyển động

  Serial.print("Acceleration X: ");
  Serial.print(mpu6050.getAccX());
  Serial.print(", Y: ");
  Serial.print(mpu6050.getAccY());
  Serial.print(", Z: ");
  Serial.print(mpu6050.getAccX());
  Serial.println(" m/s^2");

  Serial.print("Angle X: ");
  Serial.print(mpu6050.getAngleX());
  Serial.print(", Y: ");
  Serial.print(mpu6050.getAngleY());
  Serial.print(", Z:");
  Serial.print(mpu6050.getAngleZ());
  Serial.println(" deg");

  Serial.print("Temperature: ");
  Serial.print(mpu6050.getTemp());
  Serial.println(" degrees C");
  Serial.println("==================================================================");


  // Nếu nhiệt độ lớn hơn 50°C, phát âm thanh cảnh báo nhẹ nhàng
  if (mpu6050.getTemp() > 50) {
    playSoftAlertSound();
  }

  delay(1000); // Đợi trước khi lặp lại
}

// Âm thanh cảnh báo nhẹ nhàng
void playSoftAlertSound() {
  tone(pin_speaker, NOTE_C4, 200); // Nốt C4
  delay(200);
  tone(pin_speaker, NOTE_E4, 200); // Nốt E4
  delay(200);
  tone(pin_speaker, NOTE_G4, 200); // Nốt G4
  delay(200);
  tone(pin_speaker, NOTE_C5, 200); // Nốt C5
  delay(200);
  
  noTone(pin_speaker); // Dừng phát âm
  delay(1000); // Nghỉ ngơi 1 giây trước khi lặp lại
}