// #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
#define SIZE_OF_STRING    10

QueueHandle_t string_queue;
QueueHandle_t integer_queue;
QueueSetHandle_t queue_set;

// typedef enum{SENDER1,SENDER2}SENDER_ID;

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

void vInitializeQueue(void);
void vSenderTask1(void *params);
void vSenderTask2(void *params);
void vReceiverTask(void *params);
void vStringCopy(uint8_t *src, uint8_t *dest);

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

  xTaskCreate(vSenderTask2, NULL, 25000, (void *)100,2,NULL);
  xTaskCreate(vSenderTask1, NULL, 25000, (void *)"Hello !",2,NULL);

  

  xTaskCreate(vReceiverTask, NULL, 25000, NULL, 1, NULL);

  vTaskStartScheduler();

}

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

void vInitializeQueue(void)
{

  string_queue = xQueueCreate(1, sizeof(uint8_t *));
  integer_queue = xQueueCreate(1, sizeof(uint8_t *));

  if(string_queue != NULL && integer_queue != NULL)
  printf("Queue are created successfully!\n");
  else
  printf("Queue are not created!\n");

  queue_set = xQueueCreateSet(2);

  xQueueAddToSet((xQueueSetMemberHandle)string_queue, queue_set);
  xQueueAddToSet((xQueueSetMemberHandle)integer_queue, queue_set);

}

void vSenderTask1(void *params)
{
  uint8_t *temp = (uint8_t *)pvPortMalloc(SIZE_OF_STRING);
  BaseType_t result=0;

  vStringCopy((uint8_t *)params,temp);

  while(1)
  {
    result = xQueueSend(string_queue, &temp, 0);//passing the address of temp into queue

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

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

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

  while(1)
  {
    result = xQueueSend(integer_queue, (uint8_t *)&params, 0);//passing the address of temp into queue

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

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

void vReceiverTask(void *params)
{
  uint8_t *data;
  BaseType_t result;

  xQueueSetMemberHandle queue_handle=NULL;

  while(1)
  {
    queue_handle = xQueueSelectFromSet(queue_set, portMAX_DELAY);
    // result = xQueueReceive(general_queue, &data, pdMS_TO_TICKS(DELAY_TO_BE_USED));

    data = NULL;

    if(queue_handle == (xQueueSetMemberHandle)string_queue)
    {
      result = xQueueReceive(queue_handle,&data,pdMS_TO_TICKS(DELAY_TO_BE_USED));
      printf("Received -> %s\n",data);
    }
    else if(queue_handle == (xQueueSetMemberHandle)integer_queue)
    {
      result = xQueueReceive(queue_handle,&data,pdMS_TO_TICKS(DELAY_TO_BE_USED));
      printf("Received -> %d\n",data);
    }
  }
}

void vStringCopy(uint8_t *src, uint8_t *dest)
{
  while((*dest++ = *src++) != '\0');
}