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

static QueueHandle_t msg_queue; 


//Task: Wait for item on queue and print it
void printMessages(void *parameters){
  int item;
  // Loop
  while(1){
    if(xQueueReceive(msg_queue, (void *)&item, 0) == pdTRUE){
      //Serial.println(item);
    }
    Serial.println(item);
    //Wait
    vTaskDelay(500/portTICK_PERIOD_MS);
  }
}


void setup() {
  // Configure serial and wait a second
  Serial.begin(115200);
  vTaskDelay(1000 / portTICK_PERIOD_MS);
  Serial.println();
  Serial.println("---FreeRTOS Queue Demo---");
  //Create queue
  msg_queue = xQueueCreate(msg_queue_len, sizeof(int));

  // Start blink task
  xTaskCreatePinnedToCore(  // Use xTaskCreate() in vanilla FreeRTOS
            printMessages,      // Function to be called
            "Print Messages",   // Name of task
            1024,           // Stack size (bytes in ESP32, words in FreeRTOS)
            NULL,           // Parameter to pass
            1,              // Task priority
            NULL,           // Task handle
            app_cpu);       // Run on one core for demo purposes (ESP32 only)
    
}

void loop() {
  static int num = 0;
  // Try to add item to queue for 10 ticks, fail if queue is full
  if (xQueueSend(msg_queue,(void *)&num,10)!=pdTRUE)
  {
    Serial.println("Queue Full");
  }
  num ++;
  //Wait
  vTaskDelay(1000/portTICK_PERIOD_MS);
}