#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 q_len = 5;

// Global
static QueueHandle_t q;

// Task: wait for item on queue and print it.
void printItem(void* arg){
  
  int item;

  // Loop forever.
  while (1) {
    if (xQueueReceive(q, (void*)&item, 0) == pdTRUE) {
      Serial.println(item);
    }
      // Serial.println(item);

    // Wait before trying again.
    vTaskDelay(500 / portTICK_PERIOD_MS);
  }
}

void setup() {
  // put your setup code here, to run once:

  // Serial configuration
  Serial.begin(115200);
  // Serial.begin(115200);

  // Wait a moment to start so that we won't miss output.
  vTaskDelay(1000 / portTICK_PERIOD_MS);
  Serial.println();
  Serial.println("---FreeRTOS Queue Demo---");

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

  // Task to receive the item from queue.
  xTaskCreatePinnedToCore(  // Use xTaskCreate() in vanilla FreeRTOS.
    printItem,              // Function to be called.
    "Task 1",               // Name of task.
    1024,                   // Stack size (bytes in ESP32, words in FreeRTOS).
    NULL,                   // Parameter to pass to function.
    1,                      // Task priority (o to configMAX_PRIORITIES - 1)
    NULL,                   // Task handle
    app_cpu);               // Run on one core for demo purposes (ESP32 only)

  // // Delete "setup and loop task"
  // vTaskDelete(NULL);
}

void loop() {
  // put your main code here, to run repeatedly:

  // Initialization.
  static int num = 0;

  // Try to add item to queue for 10 ticks, fail if queue is full.
  if (xQueueSend(q, (void*)&num, 10) != pdTRUE) {
    Serial.println("Queue is full");
  }

  num++;

  // Wait before trying again.
  vTaskDelay(1000 / portTICK_PERIOD_MS);
}