#include <stdio.h>
#include "pico/stdlib.h"
#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"

SemaphoreHandle_t xMutex;

void taskA(void *params) {
    while (1) {
        // Attempt to take the mutex, waiting indefinitely if necessary
        if (xSemaphoreTake(xMutex, portMAX_DELAY) == pdTRUE) {
            // Critical section: Access the shared resource
            printf("Task A: Accessing shared resource\n");
            vTaskDelay(500 / portTICK_PERIOD_MS); // Simulate work by delaying
            
            // Release the mutex after finishing work
            xSemaphoreGive(xMutex);
        }
        
        vTaskDelay(1000 / portTICK_PERIOD_MS); // Delay before trying to access again
    }
}

void taskB(void *params) {
    while (1) {
        // Attempt to take the mutex, waiting indefinitely if necessary
        if (xSemaphoreTake(xMutex, portMAX_DELAY) == pdTRUE) {
            // Critical section: Access the shared resource
            printf("Task B: Accessing shared resource\n");
            vTaskDelay(500 / portTICK_PERIOD_MS); // Simulate work by delaying
            
            // Release the mutex after finishing work
            xSemaphoreGive(xMutex);
        }

        vTaskDelay(1500 / portTICK_PERIOD_MS); // Delay before trying to access again
    }
}

int main() {
    stdio_init_all(); // Initialize stdio over USB

    // Create a semaphore
    xMutex = xSemaphoreCreateMutex();
    if (xMutex == NULL) {
        printf("Failed to create mutex\n");
        while (1); // Loop forever if mutex creation fails
    }

    xTaskCreate(taskA, "Task A", 1024, NULL, 1, NULL);
    xTaskCreate(taskB, "Task B", 1024, NULL, 1, NULL);

    vTaskStartScheduler(); // Start FreeRTOS scheduler

    for (;;);
}