// 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 uint8_t msg_queue_len = 5;

// Globals
static QueueHandle_t msg_queue;

// Tasks
// -------------------------------------------------------------

void printMessage(void *parameters)
{
  int item;

  //Loop Forever
  while(1)
  {
    if(xQueueReceive(msg_queue,(void *)&item,0)==pdTRUE)
    {
      
      // Serial.println(item);
    }
    Serial.println(item);
    vTaskDelay(1000 / portTICK_PERIOD_MS);

        // Get the number of elements in the queue
    unsigned portBASE_TYPE queueLength = uxQueueMessagesWaiting(msg_queue);

    // Print the number of elements
    Serial.print("Number of elements in msg_queue: ");
    Serial.println(queueLength);

  }
}

// ----------------------------------------------------
void setup() {

  // Configure Serial
  Serial.begin(115200);

  // Wait a moment to start (so we don't miss Serial output)
  vTaskDelay(1000 / portTICK_PERIOD_MS);

  Serial.println("---FreeRTOS Queue Demo---");
  
  // Create queue
  msg_queue= xQueueCreate(msg_queue_len,sizeof(int));


    xTaskCreatePinnedToCore(printMessage,
                          "Print Messages",
                          4096,
                          NULL,
                          1,
                          NULL,
                          app_cpu);

}

void loop() {
  
  static int num = 0;

  if (xQueueSend(msg_queue,(void *)&num,10)!=pdTRUE)
  {
    Serial.println("Queue Full");
  }
  num ++;

  // Wait a moment to start (so we don't miss Serial output)
  vTaskDelay(300 / portTICK_PERIOD_MS);


}