/**
 * FreeRTOS Queue Alternate Solution
 * 
 * Demonstrate how it's often easier to use queues instead of counting
 * semaphores to pass information between tasks.
 * 
 * Date: April 16, 2025
 * Author: Mike
 * License: 0BSD
 */

// You'll likely need this on vanilla FreeRTOS
//#include <semphr.h>

// Use only core 1 for demo purposes
#if CONFIG_FREERTOS_UNICORE
  static const BaseType_t app_cpu = 0;
#else
  static const BaseType_t app_cpu = 1;
#endif

// Settings
static const int q_len = 10;
static const uint8_t queue_len = 10;  // Size of queue
static const int num_prod_tasks = 5;  // Number of producer tasks
static const int num_cons_tasks = 2;  // Number of consumer tasks
static const int num_writes = 3;      // Num times each producer writes to buf

// Globals
static QueueHandle_t q;               // Send data from producer to consumer.
static SemaphoreHandle_t mutex;       // Lock access to the serial.
static SemaphoreHandle_t bin_sem;


//*****************************************************************************
// Tasks

// Producer: write a given number of times to shared buffer
void producer(void *parameters) {

  // Copy the parameters into a local variable
  int num = *(int *)parameters;

  // Release the binary semaphore.
  xSemaphoreGive(bin_sem);

  for (int i = 0; i < num_writes; i++){
    // Send the number into the queue. (Wait max time if the queue is full)
    xQueueSend(q, (void*)&num, portMAX_DELAY);
  }

  // Delete self task
  vTaskDelete(NULL);
}

// Consumer: continuously read from shared buffer
void consumer(void *parameters) {

  int data;

  // Read from buffer
  while (1) {
    // Retrive data from the queue. (Wait max time if the queue is empty)
    xQueueReceive(q, (void*)&data, portMAX_DELAY);

    // Use mutex to protect the Serial.
    xSemaphoreTake(mutex, portMAX_DELAY);
    Serial.println(data);
    xSemaphoreGive(mutex);
  }
}

//*****************************************************************************
// Main (runs as its own task with priority 1 on core 1)

void setup() {

  char task_name[12];
  
  // Configure Serial
  Serial.begin(115200);

  // Wait a moment to start (so we don't miss Serial output)
  vTaskDelay(1000 / portTICK_PERIOD_MS);
  Serial.println();
  Serial.println("---FreeRTOS Semaphore Alternate Solution---");

  // Create queue.
  q = xQueueCreate(q_len, sizeof(int));

  // Create semaphore and mutex before start tasks.
  mutex = xSemaphoreCreateMutex();
  bin_sem = xSemaphoreCreateBinary();

  // Start producer tasks (wait for each to read argument)
  for (int i = 0; i < num_prod_tasks; i++) {
    sprintf(task_name, "Producer %i", i);
    xTaskCreatePinnedToCore(producer,
                            task_name,
                            1024,
                            (void *)&i,
                            1,
                            NULL,
                            app_cpu);
    xSemaphoreTake(bin_sem, portMAX_DELAY);
  }

  // Start consumer tasks
  for (int i = 0; i < num_cons_tasks; i++) {
    sprintf(task_name, "Consumer %i", i);
    xTaskCreatePinnedToCore(consumer,
                            task_name,
                            1024,
                            NULL,
                            1,
                            NULL,
                            app_cpu);
  }

  // Use mutex to protect the Serial.
  xSemaphoreTake(mutex, portMAX_DELAY);
  Serial.println("All tasks created");
  xSemaphoreGive(mutex);
}

void loop() {
  
  // Do nothing but allow yielding to lower-priority tasks
  vTaskDelay(1000 / portTICK_PERIOD_MS);
}