#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_MPU6050.h>
#include <DHT.h>
#include <DHT_U.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define DHTPIN 4       
#define DHTTYPE DHT22  

#define I2C_SDA 25
#define I2C_SCL 26
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
#define SCREEN_ADDRESS 0x3C  

DHT dht(DHTPIN, DHTTYPE);
Adafruit_MPU6050 mpu_accel;
Adafruit_MPU6050 mpu_gyro;
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

static int app_cpu = 0;
static QueueHandle_t accelh = nullptr;
static QueueHandle_t temph = nullptr;
static QueueHandle_t gyroh = nullptr;
static SemaphoreHandle_t chsem = nullptr;
static SemaphoreHandle_t i2sem = nullptr;

struct s_accel {
  float ax, ay, az;
};

struct s_temp {
  float temp, humidity;
};

struct s_gyro {
  float gx, gy, gz;
};

static inline void i2c_lock() { xSemaphoreTake(i2sem, portMAX_DELAY); }
static inline void i2c_unlock() { xSemaphoreGive(i2sem); }

static void temp_task(void *argp) {
  s_temp reading;
  dht.begin();

  for (;;) {
    i2c_lock();
    reading.temp = dht.readTemperature();
    reading.humidity = dht.readHumidity();
    i2c_unlock();

    if (!isnan(reading.temp) && !isnan(reading.humidity)) {
      xQueueOverwrite(temph, &reading);
      xSemaphoreGive(chsem);
    }
    vTaskDelay(pdMS_TO_TICKS(2000));
  }
}

static void accel_task(void *argp) {
  s_accel reading;

  i2c_lock();
  if (!mpu_accel.begin(0x68)) { 
    i2c_unlock();
    Serial.println("Failed to initialize MPU6050 (accel)!");
    vTaskDelete(nullptr);
  }
  i2c_unlock();

  for (;;) {
    sensors_event_t a, g, temp;
    i2c_lock();
    mpu_accel.getEvent(&a, &g, &temp);
    i2c_unlock();

    reading.ax = a.acceleration.x;
    reading.ay = a.acceleration.y;
    reading.az = a.acceleration.z;

    xQueueOverwrite(accelh, &reading);
    xSemaphoreGive(chsem);

    vTaskDelay(pdMS_TO_TICKS(500));
  }
}

static void rotation_task(void *argp) {
  s_gyro reading;

  i2c_lock();
  if (!mpu_gyro.begin(0x69)) { 
    i2c_unlock();
    Serial.println("Failed to initialize MPU6050 (gyro)!");
    vTaskDelete(nullptr);
  }
  i2c_unlock();

  for (;;) {
    sensors_event_t a, g, temp;
    i2c_lock();
    mpu_gyro.getEvent(&a, &g, &temp);
    i2c_unlock();

    reading.gx = g.gyro.x;
    reading.gy = g.gyro.y;
    reading.gz = g.gyro.z;

    xQueueOverwrite(gyroh, &reading);
    xSemaphoreGive(chsem);

    vTaskDelay(pdMS_TO_TICKS(500));
  }
}

static void disp_task(void *argp) {
  s_temp temp_reading = {0, 0};
  s_accel accel_reading = {0, 0, 0};
  s_gyro gyro_reading = {0, 0, 0};

  for (;;) {
    xSemaphoreTake(chsem, portMAX_DELAY);

    s_temp temp_buf;
    s_accel accel_buf;
    s_gyro gyro_buf;

    if (xQueueReceive(temph, &temp_buf, 0) == pdPASS) {
      temp_reading = temp_buf;
    }
    if (xQueueReceive(accelh, &accel_buf, 0) == pdPASS) {
      accel_reading = accel_buf;
    }
    if (xQueueReceive(gyroh, &gyro_buf, 0) == pdPASS) {
      gyro_reading = gyro_buf;
    }

    display.clearDisplay();
    display.setCursor(0, 0);
    display.printf("Temp: %.2f C\n", temp_reading.temp);
    display.printf("Humidity: %.2f %%\n", temp_reading.humidity);
    display.printf("Accel X,Y,Z:\n%.2f : %.2f : %.2f\n", accel_reading.ax, accel_reading.ay, accel_reading.az);
    display.printf("Gyro X,Y,Z:\n%.2f : %.2f : %.2f\n", gyro_reading.gx, gyro_reading.gy, gyro_reading.gz);
    display.display();

    Serial.printf("Temp: %.2f C, Humidity: %.2f %%\n", temp_reading.temp, temp_reading.humidity);
    Serial.printf("Accel X: %.2f, Y: %.2f, Z: %.2f\n", accel_reading.ax, accel_reading.ay, accel_reading.az);
    Serial.printf("Gyro X: %.2f, Y: %.2f, Z: %.2f\n\n", gyro_reading.gx, gyro_reading.gy, gyro_reading.gz);

    vTaskDelay(pdMS_TO_TICKS(500));
  }
}

void setup() {
  Serial.begin(115200);
  Wire.begin(I2C_SDA, I2C_SCL);

  if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Initializing...");
  display.display();
  delay(1000);

  app_cpu = xPortGetCoreID();

  chsem = xSemaphoreCreateBinary();
  i2sem = xSemaphoreCreateBinary();
  xSemaphoreGive(i2sem);

  temph = xQueueCreate(1, sizeof(s_temp));
  accelh = xQueueCreate(1, sizeof(s_accel));
  gyroh = xQueueCreate(1, sizeof(s_gyro));

  Serial.println("\nESP32 DHT22 & Dual MPU6050 Display");

  xTaskCreatePinnedToCore(temp_task, "temp_task", 2400, nullptr, 1, nullptr, app_cpu);
  xTaskCreatePinnedToCore(accel_task, "accel_task", 2400, nullptr, 1, nullptr, app_cpu);
  xTaskCreatePinnedToCore(rotation_task, "rotation_task", 2400, nullptr, 1, nullptr, app_cpu);
  xTaskCreatePinnedToCore(disp_task, "disp_task", 4000, nullptr, 1, nullptr, app_cpu);
}

void loop() {
  vTaskDelete(nullptr);
}