#include <MPU6050_tockn.h>
#include <Wire.h>
#include <math.h>
#include <ESP32Servo.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>

#define NOTE_C4  262
#define NOTE_E4  330
#define NOTE_G4  392
#define NOTE_A4  440
#define NOTE_C5  523

Servo servo;
MPU6050 mpu6050(Wire);
TaskHandle_t DieuKhienServoHandle = NULL; // lưu trữ handle của tác vụ -> DieuKhienServo

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

float velocityX = 0, velocityY = 0, velocityZ = 0; 
unsigned long previousTime = 0; 
float dt = 0.1; 

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

  xTaskCreate(HienTTSerial, "HienTTSerial", 2048, NULL, 1, NULL);
  xTaskCreate(DetectChuyenDong, "DetectChuyenDong", 2048, NULL, 3, NULL);
  xTaskCreate(DieuKhienServo, "DieuKhienServo", 2048, NULL, 3, &DieuKhienServoHandle);
  xTaskCreate(CanhBaoNhiet, "CanhBaoNhiet", 2048, NULL, 2, NULL);

}

void loop() {

}


void HienTTSerial(void *pvParameters) {
  while (true) {
    mpu6050.update();

    unsigned long currentTime = millis(); 
    dt = (currentTime - previousTime) / 1000.0; 
    previousTime = currentTime; 


    float ax = mpu6050.getAccX();
    float ay = mpu6050.getAccY();
    float az = mpu6050.getAccZ();

 
    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);

    Serial.print("Acceleration X: ");
    Serial.print(ax);
    Serial.print(", Y: ");
    Serial.print(ay);
    Serial.print(", Z: ");
    Serial.println(az);

    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("==================================================================");

    vTaskDelay(200 / portTICK_PERIOD_MS);
  }
}

void DetectChuyenDong(void *pvParameters) {
  while (true) {
    mpu6050.update();
    unsigned long currentTime = millis(); 
    dt = (currentTime - previousTime) / 1000.0; 
    previousTime = currentTime; 


    float ax = mpu6050.getAccX();
    float ay = mpu6050.getAccY();
    float az = mpu6050.getAccZ();


    velocityX += ax * dt;
    velocityY += ay * dt;
    velocityZ += az * dt;

    if (fabs(velocityX) > 0.1 || fabs(velocityY) > 0.1 || fabs(velocityZ) > 0.1) {
      digitalWrite(pin_led, HIGH); 
    } else {
      digitalWrite(pin_led, LOW); 
    }

    vTaskDelay(100 / portTICK_PERIOD_MS); 
  }
}

void DieuKhienServo(void *pvParameters) {
  while (true) {
    mpu6050.update();
    int pos = map(mpu6050.getAngleZ(), -180, 180, 0, 180);
    servo.write(pos);
    
    vTaskDelay(100 / portTICK_PERIOD_MS); 
  }
}

void CanhBaoNhiet(void *pvParameters) {
  while (true) {
    if (mpu6050.getTemp() > 50) {
      CanhBao();
    }
    float temperature = mpu6050.getTemp();

    if (temperature > 70) {
      CanhBao(); 
      vTaskSuspend(DieuKhienServoHandle); 
      Serial.println("Da dung Dieu khien Servo");
    } else if (temperature <= 70 && eTaskGetState(DieuKhienServoHandle) == eSuspended) {
      vTaskResume(DieuKhienServoHandle); 
      Serial.println("Da tiep tuc chuc nang Dieu khien Servo");
    }

    vTaskDelay(1000 / portTICK_PERIOD_MS); 
  }
}

void CanhBao() {
  tone(pin_speaker, NOTE_C4, 200); 
  delay(200);
  tone(pin_speaker, NOTE_E4, 200); 
  delay(200);
  tone(pin_speaker, NOTE_G4, 200); 
  delay(200);
  tone(pin_speaker, NOTE_C5, 200); 
  delay(200);

  noTone(pin_speaker); 
}