#include <Arduino.h>
#include <ESP32Servo.h>  // su dung ESP32-compatible Servo library
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

const int BUTTON_PIN = 18;  
const int LED_PIN = 17;     
const int SERVO_PIN1 = 13;   
const int SERVO_PIN2 = 16;
const int trigPin = 2;
const int echoPin = 4;
const long emptyDistance = 30;

LiquidCrystal_I2C lcd(0x27, 16, 2);

String lastDisplayedMessage = "";

// biến mô phỏng mức độ pH
volatile float ph_value = 8.5;
Servo myservo1;  
Servo myservo2;  

volatile bool hopperEmpty = false;

// ham mo phong cam bien pH bang nut bam
void read_ph_sensor_task(void *pvParameter) {
    while (1) {
        // Mô phỏng việc đọc cảm biến pH dựa trên nút nhấn
        if (digitalRead(BUTTON_PIN) == LOW) {
            ph_value += 0.5;  // tang gia tri pH
            if (ph_value > 14.0) {
                ph_value = 14.0;
            }
        } else {
            ph_value -= 0.5;  // giam gia tri pH
            if (ph_value < 0.0) {
                ph_value = 0.0;
            }
        }
        
        Serial.println(ph_value);
        
        // Delay 
        vTaskDelay(1000 / portTICK_PERIOD_MS); 
    }
}

void control_led_task(void *pvParameter) {
    while (1) {
        // Control logic
        if (ph_value < 7.0) {
            // pH is too low, turn on LED
            Serial.println("pH thap.");
            digitalWrite(LED_PIN, HIGH);  // Turn on LED
        } else if (ph_value > 9.5) {
            // pH is too high, turn on LED
            Serial.println("pH cao.");
            digitalWrite(LED_PIN, HIGH);  // Turn on LED
        } else {
            // pH is within the desired range
            Serial.println("pH trong khoang cho phep.");
            digitalWrite(LED_PIN, LOW);  // Turn off LED
        }

        // Delay 
        vTaskDelay(1000 / portTICK_PERIOD_MS);  
    }
}

void control_servo_task1(void *pvParameter) {
    while (1) { 
        if (hopperEmpty) {
            Serial.println("Hopper is empty, stopping servo 1.");
            myservo1.write(90); // Stop the servo
        } else if (ph_value < 7.0) {
            // pH thap, nap thung tron voi hoat dong
            Serial.println("Nap thung tron voi bat.");
            // tan suat tron voi 0.5 giay
            myservo1.write(90);  
            vTaskDelay(500 / portTICK_PERIOD_MS);  
            myservo1.write(0);   
            vTaskDelay(500 / portTICK_PERIOD_MS);
        } else {
            // pH trong khoang cho phep
            Serial.println("Nap thung tron voi tat.");
            myservo1.write(0);   
        }

        // delay
        vTaskDelay(1000 / portTICK_PERIOD_MS);  
    }
}

void control_servo_task2(void *pvParameter) {
    while (1) { 
        if (hopperEmpty) {
            Serial.println("Hopper is empty, stopping servo 2.");
            myservo2.write(90); // Stop the servo
        } else if (ph_value > 9.5) {
            // pH thap, nap thung tron voi hoat dong
            Serial.println("Nap thung tron duong bat.");
            // tan suat tron duong 0.5 giay
            myservo2.write(90);  
            vTaskDelay(500 / portTICK_PERIOD_MS);  
            myservo2.write(0);   
            vTaskDelay(500 / portTICK_PERIOD_MS);
        } else {
            // pH trong khoang cho phep
            Serial.println("Nap thung tron duong tat.");
            myservo2.write(0);   
        }

        // delay
        vTaskDelay(1000 / portTICK_PERIOD_MS);  
    }
}

void read_ultrasonic_sensor() {
    long duration, distance;
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    duration = pulseIn(echoPin, HIGH);
    distance = (duration / 2) / 29.1;
    Serial.print("Distance: ");
    Serial.print(distance);
    Serial.println(" cm");

    if (distance > emptyDistance) {
        Serial.println("pheu dung day!"); // Hopper is full
    } 
    else if (distance < 58 && distance != 2) { 
        Serial.println("pheu dung voi!"); // Hopper is partially full
    }
    else if (distance == 2) { // Use double equals for comparison
        Serial.println("pheu dung rong!"); // Hopper is empty
    }
}

void setup() {
    // Initialize serial communication
    Serial.begin(115200);

    // Initialize GPIO pins
    pinMode(BUTTON_PIN, INPUT_PULLUP);  // Button input with pull-up resistor
    pinMode(LED_PIN, OUTPUT);           // LED output
    pinMode(trigPin, OUTPUT);           // Ultrasonic sensor trig pin
    pinMode(echoPin, INPUT);            // Ultrasonic sensor echo pin

    myservo1.attach(SERVO_PIN1);
    myservo2.attach(SERVO_PIN2);

    lcd.init();
    lcd.backlight();

    // Create tasks
    xTaskCreate(
        read_ph_sensor_task,   
        "ReadPH",              
        1024,                  
        NULL,                  
        1,                     
        NULL                   
    );

    xTaskCreate(
        control_led_task,      
        "ControlLED",         
        1024,               
        NULL,                  
        1,                     
        NULL                   
    );

    xTaskCreate(
        control_servo_task1,    
        "ControlServo1",        
        2048,                  
        NULL,                 
        1,                     
        NULL                  
    );

    xTaskCreate(
        control_servo_task2,    
        "ControlServo2",       
        2048,                  
        NULL,                  
        1,                    
        NULL                  
    );
}

void loop() {
    String message;

    if (ph_value < 7.0) {
        message = "pH thap.";
    } else if (ph_value > 9.5) {
        message = "pH cao.";
    } else {
        message = "pH trong khoang cho phep.";
    }

    if (message != lastDisplayedMessage) {
        lcd.clear();
        lcd.print(message);
        lastDisplayedMessage = message;

        if (ph_value < 7.0) {
            delay(250);
            lcd.clear();
            lcd.print("tang do pH.");
        } else if (ph_value > 9.5) {
            delay(250);
            lcd.clear();
            lcd.print("giam do pH.");
        } else {
            delay(250);
            lcd.setCursor(0, 0);
            lcd.print("tang pH, tat.");
            lcd.setCursor(1, 1);
            lcd.print("giam pH, tat.");
        }
    }

    read_ultrasonic_sensor();  // Read the ultrasonic sensor in the main loop
    delay(2000);
}