#include <Wire.h>
#include <MPU6050.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <freertos/queue.h>

MPU6050 mpu;

#define SDA_PIN 21   // Define SDA pin
#define SCL_PIN 22   // Define SCL pin

QueueHandle_t accelQueue;
QueueHandle_t gyroQueue;

void setup() {
  Serial.begin(9600);

  // Initialize MPU6050
  Wire.begin(SDA_PIN, SCL_PIN);
  mpu.initialize();

  // Create queues
  accelQueue = xQueueCreate(10, sizeof(int16_t) * 3); // 3 values for accelerometer (x, y, z)
  gyroQueue = xQueueCreate(10, sizeof(int16_t) * 3); // 3 values for gyroscope (x, y, z)

  // Create tasks
  xTaskCreate(readDataTask, "ReadData", 10000, NULL, 1, NULL);
  xTaskCreate(processDataTask, "ProcessData", 10000, NULL, 2, NULL);
}

void loop() {
  vTaskDelete(NULL); // This loop is not needed in FreeRTOS
}

void readDataTask(void *pvParameters) {
  while (true) {
    // Read accelerometer and gyroscope data
    int16_t accelData[3], gyroData[3];
    mpu.getAcceleration(&accelData[0], &accelData[1], &accelData[2]);
    mpu.getRotation(&gyroData[0], &gyroData[1], &gyroData[2]);

    // Send data to respective queues
    xQueueSend(accelQueue, &accelData, portMAX_DELAY);
    xQueueSend(gyroQueue, &gyroData, portMAX_DELAY);

    // Delay for some time before next reading
    vTaskDelay(pdMS_TO_TICKS(100)); // Delay for 100 milliseconds
  }
}

void processDataTask(void *pvParameters) {
  QueueSetHandle_t queueSet;
  queueSet = xQueueCreateSet(10);

  // Add queues to queue set
  xQueueAddToSet(accelQueue, queueSet);
  xQueueAddToSet(gyroQueue, queueSet);

  while (true) {
    // Wait for data on either queue
    QueueSetMemberHandle_t xActivatedMember = xQueueSelectFromSet(queueSet, portMAX_DELAY);

    // Transfer data to UART channels based on data source
    if (xActivatedMember == accelQueue) {
      int16_t accelData[3];
      xQueueReceive(accelQueue, &accelData, portMAX_DELAY);
      Serial.print("Accelerometer: ");
      Serial.print("X = "); Serial.print(accelData[0]);
      Serial.print(", Y = "); Serial.print(accelData[1]);
      Serial.print(", Z = "); Serial.println(accelData[2]);
      // Send data to UART channel for accelerometer
    } else if (xActivatedMember == gyroQueue) {
      int16_t gyroData[3];
      xQueueReceive(gyroQueue, &gyroData, portMAX_DELAY);
      Serial.print("Gyroscope: ");
      Serial.print("X = "); Serial.print(gyroData[0]);
      Serial.print(", Y = "); Serial.print(gyroData[1]);
      Serial.print(", Z = "); Serial.println(gyroData[2]);
      // Send data to UART channel for gyroscope
    }
  }
}