// #include <avr/io.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include <stdio.h>
// #include <Arduino_FreeRTOS.h>
// #include <queue.h>

#define LENGHT_OF_QUEUE   05
#define DELAY_TO_BE_USED  500

QueueHandle_t general_queue;

typedef enum{SENDER1,SENDER2}SENDER_ID;

typedef struct {
  SENDER_ID sendId;
  uint8_t data;
}QueueData;

void vInitializeQueue(void);
void vSenderTask(void *params);
void vReceiverTask(void *params);

QueueData dummy_data_sender1 = {SENDER1, 55};
QueueData dummy_data_sender2 = {SENDER2, 35};

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
  
  vInitializeQueue();

  xTaskCreate(vSenderTask, NULL, 50000, (void *)&(dummy_data_sender1),1,NULL);

  xTaskCreate(vSenderTask, NULL, 50000, (void *)&(dummy_data_sender2),1,NULL);

  xTaskCreate(vReceiverTask, NULL, 50000, NULL, 2, NULL);

  vTaskStartScheduler();

}

void loop() {
  // put your main code here, to run repeatedly:
  delay(10); // this speeds up the simulation
}

void vInitializeQueue(void)
{
  general_queue = xQueueCreate(LENGHT_OF_QUEUE, sizeof(QueueData));

  if(general_queue != NULL)
  printf("Queue is created successfully!\n");
  else
  printf("Queue is not created!\n");

}

void vSenderTask(void *params)
{
  BaseType_t result=0;

  while(1)
  {
    result = xQueueSend(general_queue, params, pdMS_TO_TICKS(DELAY_TO_BE_USED));//0);

    if(result != pdPASS)
      printf("Unable to send data over queue!\n");

      // Introduce a delay or yield here
      // vTaskDelay(pdMS_TO_TICKS(50));  // Adjust the delay as needed
      //  taskYIELD();
  }
}

void vReceiverTask(void *params)
{
  QueueData received_data;
  BaseType_t result;

  while(1)
  {
    result = xQueueReceive(general_queue, &received_data, 0);//pdMS_TO_TICKS(DELAY_TO_BE_USED));

    if(result == pdPASS)
    {
      switch(received_data.sendId)
      {
        case SENDER1: 
          printf("Data received from Sender 1 -> %d\n",received_data.data);
        break;

        case SENDER2: 
          printf("Data received from Sender 2 -> %d\n",received_data.data);
        break;

        default: break;
      }
    }
  }
}