#include <Arduino.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <freertos/semphr.h>

SemaphoreHandle_t countingSemaphore;
SemaphoreHandle_t binarySemaphore;
SemaphoreHandle_t recursiveMutex;
SemaphoreHandle_t regularMutex;
StaticSemaphore_t countingSemaphoreBuffer;
StaticSemaphore_t binarySemaphoreBuffer;
StaticSemaphore_t recursiveMutexBuffer;
StaticSemaphore_t regularMutexBuffer;

void Task1(void *pvParameters);
void Task2(void *pvParameters);
void Task3(void *pvParameters);
void Task4(void *pvParameters);  // New task using mutexes
void cleanup();

void setup() {
    Serial.begin(115200);
    
    // Create a counting semaphore with a maximum count of 5
    countingSemaphore = xSemaphoreCreateCounting(5, 0);
    
    // Create a binary semaphore using static allocation
    binarySemaphore = xSemaphoreCreateBinaryStatic(&binarySemaphoreBuffer);
    
    // Create a recursive mutex
    recursiveMutex = xSemaphoreCreateRecursiveMutexStatic(&recursiveMutexBuffer);
    
    // Create a regular mutex
    regularMutex = xSemaphoreCreateMutexStatic(&regularMutexBuffer);
    
    // Create tasks
    xTaskCreate(Task1, "Task 1", 1000, NULL, 1, NULL);
    xTaskCreate(Task2, "Task 2", 1000, NULL, 1, NULL);
    xTaskCreate(Task3, "Task 3", 1000, NULL, 1, NULL);
    xTaskCreate(Task4, "Task 4", 1000, NULL, 1, NULL); // New task
}

void loop() {
    // Nothing to do here
}

void Task1(void *pvParameters) {
    for (;;) {
        // Give the counting semaphore
        xSemaphoreGive(countingSemaphore);
        Serial.println("Task 1: Gave counting semaphore");
        
        // Check the count of the counting semaphore
        UBaseType_t count = uxSemaphoreGetCount(countingSemaphore);
        Serial.print("Task 1: Counting semaphore count = ");
        Serial.println(count);
        
        vTaskDelay(pdMS_TO_TICKS(1000));
    }
}

void Task2(void *pvParameters) {
    for (;;) {
        // Wait for the counting semaphore
        if (xSemaphoreTake(countingSemaphore, pdMS_TO_TICKS(500)) == pdTRUE) {
            Serial.println("Task 2: Took counting semaphore");
        } else {
            Serial.println("Task 2: Failed to take counting semaphore");
        }
        
        vTaskDelay(pdMS_TO_TICKS(1500));
    }
}

void Task3(void *pvParameters) {
    for (;;) {
        // Give the binary semaphore
        xSemaphoreGive(binarySemaphore);
        Serial.println("Task 3: Gave binary semaphore");

        // Wait for the binary semaphore
        if (xSemaphoreTake(binarySemaphore, pdMS_TO_TICKS(5000)) == pdTRUE) {
            Serial.println("Task 3: Took binary semaphore");
        } else {
            Serial.println("Task 3: Failed to take binary semaphore");
        }
        
        vTaskDelay(pdMS_TO_TICKS(2000));
    }
}

// New Task using mutexes
void Task4(void *pvParameters) {
    for (;;) {
        // Take the regular mutex
        if (xSemaphoreTake(regularMutex, pdMS_TO_TICKS(500)) == pdTRUE) {
            Serial.println("Task 4: Took regular mutex");
            // Simulate some work
            vTaskDelay(pdMS_TO_TICKS(1000));
            xSemaphoreGive(regularMutex);
            Serial.println("Task 4: Released regular mutex");
        } else {
            Serial.println("Task 4: Failed to take regular mutex");
        }

        // Use the recursive mutex
        if (xSemaphoreTake(recursiveMutex, pdMS_TO_TICKS(500)) == pdTRUE) {
            Serial.println("Task 4: Took recursive mutex");
            // Simulate some recursive work
            if (xSemaphoreTake(recursiveMutex, pdMS_TO_TICKS(500)) == pdTRUE) {
                Serial.println("Task 4: Took recursive mutex again");
                xSemaphoreGive(recursiveMutex);
            }
            xSemaphoreGive(recursiveMutex);
            Serial.println("Task 4: Released recursive mutex");
        } else {
            Serial.println("Task 4: Failed to take recursive mutex");
        }

        vTaskDelay(pdMS_TO_TICKS(2000));
    }
}

// Clean up resources
void cleanup() {
    vSemaphoreDelete(countingSemaphore);
    vSemaphoreDelete(binarySemaphore);
    vSemaphoreDelete(recursiveMutex);
    vSemaphoreDelete(regularMutex);
}