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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#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
TaskHandle_t CanhBaoTaskHandle = NULL; // Handle cho task cảnh báo
TaskHandle_t CanhBaoOledHandle = NULL;
TaskHandle_t HienThiOledHandle = NULL;
Adafruit_SSD1306 display = Adafruit_SSD1306(128, 64, &Wire);

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; 
float angleZ = 0;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu6050.begin();
  servo.attach(pin_servo); 
  pinMode(pin_led, OUTPUT); 
  pinMode(pin_speaker, OUTPUT); 
  mpu6050.calcGyroOffsets(true);
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for (;;)
      ; // Don't proceed, loop forever
  }
  display.display();
  delay(500); // Pause for 2 seconds
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setRotation(0);

  xTaskCreate(DetectChuyenDong, "DetectChuyenDong", 4096, NULL, 3, NULL);
  xTaskCreate(DieuKhienServo, "DieuKhienServo", 4096, NULL, 3, &DieuKhienServoHandle);
  xTaskCreate(CanhBaoNhiet, "CanhBaoNhiet", 4096, NULL, 2, NULL);
  xTaskCreate(HienThiOled, "HienThiOled", 4096, NULL, 1, &HienThiOledHandle);
  xTaskCreate(CanhBao, "CanhBao", 2048, NULL, 1, &CanhBaoTaskHandle);
  xTaskCreate(CanhBaoOled, "CanhBaoOled", 4096, NULL, 1, &CanhBaoOledHandle);

  vTaskSuspend(CanhBaoOledHandle);
  vTaskSuspend(CanhBaoTaskHandle);  // Tạm dừng task cảnh báo lúc đầu
}

void loop() {
}

void DetectChuyenDong(void *pvParameters) {
  while (true) {
    mpu6050.update();
    angleZ = mpu6050.getAngleZ();
    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); 
    }
    Serial.print("\nTask detect");
    vTaskDelay(100 / portTICK_PERIOD_MS); 
  }
}

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

void CanhBaoNhiet(void *pvParameters) {
  while (true) {
    Serial.print("\nTask dieu khien canh bao");
    float temperature = mpu6050.getTemp();
    if (temperature > 70) {
      vTaskSuspend(DieuKhienServoHandle);
      vTaskResume(CanhBaoTaskHandle);
      vTaskResume(CanhBaoOledHandle);
      
      Serial.println("\nDa dung Dieu khien Servo");
    } else if (temperature <= 70 && eTaskGetState(DieuKhienServoHandle) == eSuspended ) {
      servo.attach(pin_servo);  
      servo.write(90);
      vTaskResume(DieuKhienServoHandle);

      Serial.println("\nDa tiep tuc chuc nang Dieu khien Servo");
    }

    vTaskDelay(1000 / portTICK_PERIOD_MS); 
  }
}


void HienThiOled(void *pvParameters) {
    while (true) {
      Serial.print("\nTask hien thi Oled");
      mpu6050.update(); 
      float angleX = mpu6050.getAngleX();
      float angleY = mpu6050.getAngleY();
      float angleZ = mpu6050.getAngleZ();
      float temperature = mpu6050.getTemp();

      display.clearDisplay();
      display.setCursor(0, 0); 
      display.print("Angle X: "); display.println(angleX);
      display.print("Angle Y: "); display.println(angleY);
      display.print("Angle Z: "); display.println(angleZ);
      display.print("Temp: "); display.print(temperature); display.println(" C");
      display.display(); 
      
      vTaskDelay(200 / portTICK_PERIOD_MS);
    }
}

void CanhBaoOled(void *pvParameters) {
  while(true) {
      Serial.print("\nTask hien thi canh bao len Oled");
      display.clearDisplay();
      display.setTextSize(1); 
      display.setTextColor(WHITE); 
      display.setCursor(10, 30); 
      display.println("Da dung hoat dong");
      display.setCursor(10, 40); 
      display.println("Servo do qua tai nhiet");
      display.display(); 
      
      vTaskDelay(200 / portTICK_PERIOD_MS);
      vTaskSuspend(NULL); 
  }
}

void CanhBao(void *pvParameters) {
  while (true) {
    Serial.print("\nTask phat di canh bao bang Buzzer");
    tone(pin_speaker, NOTE_C4, 200); 
    vTaskDelay(200 / portTICK_PERIOD_MS); 
    tone(pin_speaker, NOTE_E4, 200); 
    vTaskDelay(200 / portTICK_PERIOD_MS);
    tone(pin_speaker, NOTE_G4, 200); 
    vTaskDelay(200 / portTICK_PERIOD_MS); 
    tone(pin_speaker, NOTE_C5, 200); 
    vTaskDelay(200 / portTICK_PERIOD_MS); 
    noTone(pin_speaker); 
    
    vTaskSuspend(NULL); 
  }
}