/**
 * 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
 * Modiefied November 12, 2024
 * Markus Pfeil
 * License kept
 */

#include <Arduino_FreeRTOS.h>
#include <task.h>
#include <semphr.h>  // add the FreeRTOS functions for Semaphores (or Flags).
#include <FreeRTOSConfig.h>


/* Define the traceTASK_SWITCHED_IN() macro to output the voltage associated
with the task being selected to run on port 0. */
// #define traceTASK_SWITCHED_IN() vSetAnalogueOutput( 0, (int)pxCurrentTCB->pxTaskTag )
//#define traceTASK_SWITCHED_IN() analogWrite( (int)pxCurrentTCB->pxTaskTag, 255 )
//#define traceTASK_SWITCHED_OUT() analogWrite((int)pxCurrentTCB->pxTaskTag, 0 )

/* Define the traceTASK_SWITCHED_IN() macro to switch ON and OFF the led on pin 13
with the task being selected to run. */ /*modified by Aradhana Dhumal*/
/* #define traceTASK_SWITCHED_IN() digitalWrite(13, (int)pxCurrentTCB->pxTaskTag); */ 
// Declare a Mutex Semaphore Handle which we will use to manage the Serial Port.
// It will be used to ensure only only one Task is accessing this resource at any time.


// Settings
enum {BUF_SIZE = 3};                  // Size of buffer array
static const int num_prod_tasks = 3;  // 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;
  Serial.println("Producer num");
  Serial.println(num);
  delay(10);
 //Set the output port for the taskSwitchMacro to 2+the task number
  int task_tag = num+2;
  vTaskSetApplicationTaskTag( NULL, ( void * )(task_tag));
  Serial.println("Producer Task Tag");
  Serial.println(task_tag);
  delay(10);
  // 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) {

  // Copy the parameters into a local variable
  int num = *(int*)parameters;
  Serial.println("Consumer num");
  Serial.println(num);
  delay(10);
  //Set the output port for the taskSwitchMacro to 8+the task number
  int task_tag = num+8;
  vTaskSetApplicationTaskTag( NULL, ( void * )(task_tag));
  Serial.println("Consumer Task Tag");
  Serial.println(task_tag);
  delay(100);
  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() {
  int f;
  for(f=2;f<=13;f++){
  pinMode(f, OUTPUT);
  }

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

  // Wait a moment to start (so we don't miss Serial output)
  delay(50);
  Serial.println();
  Serial.println("---FreeRTOS Semaphore Solution---");
  delay(500);
  // Create mutexes and semaphores before starting tasks
  bin_sem = xSemaphoreCreateBinary();
  if(bin_sem) Serial.println("bin_sem created");
  mutex = xSemaphoreCreateMutex();
  if(mutex) Serial.println("mutex created");
  sem_empty = xSemaphoreCreateCounting(BUF_SIZE, BUF_SIZE);
  if(sem_empty) Serial.println("sem_empty created");
  sem_filled = xSemaphoreCreateCounting(BUF_SIZE, 0);
  if(sem_filled) Serial.println("sem_filled created");

  // Start producer tasks (wait for each to read argument)
  for (int i = 0; i < num_prod_tasks; i++) {
    sprintf(task_name, "Producer_%i", i);
    xTaskCreate(producer,
                            task_name,
                            128,
                            (void *)&i,
                            1,
                            NULL);
    Serial.println(task_name);
    Serial.println(i);
    delay(10);
  }

  // Start consumer tasks
  for (int i = 0; i < num_cons_tasks; i++) {
    sprintf(task_name, "Consumer_%i", i);
    xTaskCreate(consumer,
                            task_name,
                            128,
                            (void *)&i,
                            1,
                            NULL);
    Serial.println(task_name);
    Serial.println(i);
    delay(10);
  }

  Serial.println("All tasks created");
  delay(100);
  // Now the Task scheduler, which takes over control of scheduling individual Tasks, is automatically started.
  vTaskStartScheduler();
}

void loop() {

  // Do nothing 

}