#include "freertos/FreeRTOS.h"
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

const int trigPin = 18;
const int echoPin = 5;
const int ledPin = 13;
const int buzzerPin = 32;
const int buttonPin = 35; // Chân nút nhấn
const int distanceThreshold1 = 20; // Ngưỡng khoảng cách 20 cm
const int distanceThreshold2 = 70; // Ngưỡng khoảng cách 70 cm

#define SOUND_SPEED 0.034
#define CM_TO_INCH 0.393701

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

long duration;
int distanceCm;
int distanceInch;
bool buzzerState = false; // Trạng thái buzzer (true: đang kêu, false: tắt)

TaskHandle_t sensorTaskHandle;
TaskHandle_t ledBuzzerTaskHandle;
TaskHandle_t buttonTaskHandle;

void buttonTask(void *pvParameters) {
    (void)pvParameters;
    pinMode(buttonPin, INPUT_PULLUP);

    for (;;) {
        if (digitalRead(buttonPin) == LOW) {
            buzzerState = !buzzerState; // Đảo trạng thái buzzer
            vTaskDelay(pdMS_TO_TICKS(200)); // Delay để tránh nhiễu
        }
        vTaskDelay(pdMS_TO_TICKS(100));
    }
}

void sensorTask(void *pvParameters) {
    (void)pvParameters;
    pinMode(trigPin, OUTPUT);
    pinMode(echoPin, INPUT);

    for (;;) {
        digitalWrite(trigPin, LOW);
        delayMicroseconds(2);
        digitalWrite(trigPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(trigPin, LOW);

        duration = pulseIn(echoPin, HIGH);
        distanceCm = duration * SOUND_SPEED / 2;
        distanceInch = distanceCm * CM_TO_INCH;

        Serial.print("Distance (cm): ");
        Serial.println(distanceCm);
        Serial.print("Distance (inch): ");
        Serial.println(distanceInch);

        display.clearDisplay();
        display.setTextSize(2);
        display.setTextColor(WHITE);
        display.setCursor(0, 25);

        if (distanceCm <= distanceThreshold1) {
            display.print("con");
        } else if (distanceCm > distanceThreshold2) {
            display.print("100");
        } else {
            display.print(distanceCm);
            display.print(" cm");
        }

        display.display();

        vTaskDelay(pdMS_TO_TICKS(500));
    }
}

void ledBuzzerTask(void *pvParameters) {
    (void)pvParameters;
    pinMode(ledPin, OUTPUT);
    pinMode(buzzerPin, OUTPUT);

    for (;;) {
        if (distanceCm < distanceThreshold1) {
            digitalWrite(ledPin, HIGH);
        } else {
            digitalWrite(ledPin, LOW);
        }

        if (distanceCm > distanceThreshold2 && buzzerState) {
            digitalWrite(buzzerPin, HIGH);
        } else {
            digitalWrite(buzzerPin, LOW);
        }

        vTaskDelay(pdMS_TO_TICKS(100));
    }
}

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

    if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
        Serial.println(F("SSD1306 allocation failed"));
        for (;;);
    }

    delay(500);
    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(WHITE);

    xTaskCreatePinnedToCore(sensorTask, "SensorTask", 2048, NULL, 1, &sensorTaskHandle, 0);
    xTaskCreatePinnedToCore(ledBuzzerTask, "LedBuzzerTask", 2048, NULL, 1, &ledBuzzerTaskHandle, 1);
    xTaskCreatePinnedToCore(buttonTask, "ButtonTask", 2048, NULL, 1, &buttonTaskHandle, 0);
}

void loop() {
    // Tasks are running independently
}