// /**
//  * FreeRTOS Counting Semaphore Challenge
//  * 
//  * Challenge: use a mutex and counting semaphores to protect the shared buffer 
//  * so that each number (0 throguh 4) is printed exactly 3 times to the Serial 
//  * monitor (in any order). Do not use queues to do this!
//  * 
//  * Hint: you will need 2 counting semaphores in addition to the mutex, one for 
//  * remembering number of filled slots in the buffer and another for 
//  * remembering the number of empty slots in the buffer.
//  * 
//  * Date: January 24, 2021
//  * Author: Shawn Hymel
//  * 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
// enum {BUF_SIZE = 5};                  // Size of buffer array
// 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 int buf[BUF_SIZE];             // Shared buffer
// static int head = 0;                  // Writing index to buffer
// static int tail = 0;                  // Reading index to buffer
// static SemaphoreHandle_t bin_sem;     // Waits for parameter to be read
// static SemaphoreHandle_t sem_empty;   // Counts number of empty slots in bug=f
// static SemaphoreHandle_t sem_filled;  // Counts number of filled slots in bug=f
// static SemaphoreHandle_t mutex;       // Lock buffer and Serial

// //*****************************************************************************
// // 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);

//   // Fill shared buffer with task number
//   for (int i = 0; i < num_writes; i++) {

//     // Wait until there is an empty slot in the buffer.
//     xSemaphoreTake(sem_empty, portMAX_DELAY);

//     // Protect shared resource.
//     xSemaphoreTake(mutex, portMAX_DELAY);

//     // Critical section (accessing shared buffer)
//     buf[head] = num;
//     head = (head + 1) % BUF_SIZE;

//     // Release the mutex.
//     xSemaphoreGive(mutex);

//     // Signal the consumer know that a slot in the buffer is ready.
//     xSemaphoreGive(sem_filled);
//   }

//   // Delete self task
//   vTaskDelete(NULL);
// }

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

//   int val;

//   // Read from buffer
//   while (1) {

//     // Wait until there is an available slot in buffer to read the data.
//     xSemaphoreTake(sem_filled, portMAX_DELAY);

//     // Protect the shared resource.
//     xSemaphoreTake(mutex, portMAX_DELAY);
    
//     // Critical section (accessing shared buffer and Serial)
//     val = buf[tail];
//     tail = (tail + 1) % BUF_SIZE;
//     Serial.println(val);

//     // Release the mutex.
//     xSemaphoreGive(mutex);

//     // Signal the porducer that there is an available slot to write.
//     xSemaphoreGive(sem_empty);
//   }
// }

// //*****************************************************************************
// // 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 mutexes and semaphores before starting tasks
//   bin_sem = xSemaphoreCreateBinary();
//   mutex = xSemaphoreCreateMutex();
//   sem_empty = xSemaphoreCreateCounting(BUF_SIZE, BUF_SIZE);
//   sem_filled = xSemaphoreCreateCounting(BUF_SIZE, 0);

//   // 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);
//   }

//   // vTaskDelay(portTICK_PERIOD_MS);

//   // Notify that all tasks have been created
//   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);
// }



/**
 * FreeRTOS Counting Semaphore Solution
 * 
 * Use producer tasks (writing to shared memory) and consumer tasks (reading 
 * from shared memory) to demonstrate counting semaphores.
 * 
 * Date: January 24, 2021
 * Author: Shawn Hymel
 * 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
enum {BUF_SIZE = 5};                  // Size of buffer array
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 int buf[BUF_SIZE];             // Shared buffer
static int head = 0;                  // Writing index to buffer
static int tail = 0;                  // Reading index to buffer
static SemaphoreHandle_t bin_sem;     // Waits for parameter to be read
static SemaphoreHandle_t mutex;       // Lock access to buffer and Serial
static SemaphoreHandle_t sem_empty;   // Counts number of empty slots in buf
static SemaphoreHandle_t sem_filled;  // Counts number of filled slots in buf

//*****************************************************************************
// 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);

  // Fill shared buffer with task number
  for (int i = 0; i < num_writes; i++) {

    // Wait for empty slot in buffer to be available
    xSemaphoreTake(sem_empty, portMAX_DELAY);

    // Lock critical section with a mutex
    xSemaphoreTake(mutex, portMAX_DELAY);
    buf[head] = num;
    head = (head + 1) % BUF_SIZE;
    xSemaphoreGive(mutex);

    // Signal to consumer tasks that a slot in the buffer has been filled
    xSemaphoreGive(sem_filled);
  }

  // Delete self task
  vTaskDelete(NULL);
}

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

  int val;

  // Read from buffer
  while (1) {

    // Wait for at least one slot in buffer to be filled
    xSemaphoreTake(sem_filled, portMAX_DELAY);

    // Lock critical section with a mutex
    xSemaphoreTake(mutex, portMAX_DELAY);
    val = buf[tail];
    tail = (tail + 1) % BUF_SIZE;
    Serial.println(val);
    xSemaphoreGive(mutex);

    // Signal to producer thread that a slot in the buffer is free
    xSemaphoreGive(sem_empty);
  }
}

//*****************************************************************************
// 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 Solution---");

  // Create mutexes and semaphores before starting tasks
  bin_sem = xSemaphoreCreateBinary();
  mutex = xSemaphoreCreateMutex();
  sem_empty = xSemaphoreCreateCounting(BUF_SIZE, BUF_SIZE);
  sem_filled = xSemaphoreCreateCounting(BUF_SIZE, 0);

  // 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);
  }

  // Notify that all tasks have been created (lock Serial with mutex)
  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);
}