#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_MPU6050.h>
#include <DHT.h>
#include <DHT_U.h>
#include <Adafruit_SSD1306.h> // สำหรับแสดงผล OLED

#define DHTPIN 4       // Pin connected to DHT22
#define DHTTYPE DHT22  // Define DHT type

#define I2C_SDA 21
#define I2C_SCL 22
#define OLED_WIDTH 128
#define OLED_HEIGHT 64
#define OLED_I2C_ADDRESS 0x3C // Address ของจอ OLED

DHT dht(DHTPIN, DHTTYPE);
Adafruit_MPU6050 mpu1, mpu2; // MPU6050 สองตัว
Adafruit_SSD1306 display(OLED_WIDTH, OLED_HEIGHT, &Wire);

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

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

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

struct s_temp {
  float temp, humidity;
};

// ฟังก์ชันล็อกและปลดล็อก I2C
static inline void i2c_lock() {
  BaseType_t rc = xSemaphoreTake(i2sem, portMAX_DELAY);
  assert(rc == pdPASS);
}

static inline void i2c_unlock() {
  BaseType_t rc = xSemaphoreGive(i2sem);
  assert(rc == pdPASS);
}

// Task อ่านค่า Accelerometer
static void accel_task(void *argp) {
  s_accel reading;
  for (;;) {
    sensors_event_t a, g, temp;

    i2c_lock();
    mpu1.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);

    delay(500);
  }
}

// Task อ่านค่า Gyroscope
static void rotation_task(void *argp) {
  s_gyro reading;
  for (;;) {
    sensors_event_t a, g, temp;

    i2c_lock();
    mpu2.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);

    delay(500);
  }
}

// Task อ่านค่า Temperature และ Humidity
static void temp_task(void *argp) {
  s_temp reading;
  reading.temp = 0.0;
  reading.humidity = 0.0;

  dht.begin();

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

    if (isnan(reading.temp) || isnan(reading.humidity)) {
      printf("Failed to read from DHT22 sensor!\n");
    } else {
      xQueueOverwrite(temph, &reading);
      xSemaphoreGive(chsem);
    }
    delay(2000);
  }
}

// Task แสดงผล
static void disp_task(void *argp) {
  s_temp temp_reading;
  s_accel accel_reading;
  s_gyro gyro_reading;

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

    // อ่านค่าจาก Queue
    xQueuePeek(temph, &temp_reading, 0);
    xQueuePeek(accelh, &accel_reading, 0);
    xQueuePeek(gyroh, &gyro_reading, 0);

    // แสดงผลใน Serial Monitor
    printf("Temp: %.2fC, Humidity: %.2f%%\n", temp_reading.temp, temp_reading.humidity);
    printf("Accel: X=%.2f Y=%.2f Z=%.2f\n", accel_reading.ax, accel_reading.ay, accel_reading.az);
    printf("Gyro: X=%.2f Y=%.2f Z=%.2f\n", gyro_reading.gx, gyro_reading.gy, gyro_reading.gz);

    // แสดงผลใน OLED
    display.clearDisplay();
    display.setCursor(0, 0);
    display.printf("Temp: %.2fC\nHumidity: %.2f%%\n", temp_reading.temp, temp_reading.humidity);
    display.printf("Accel: X=%.2f\nY=%.2f Z=%.2f\n", accel_reading.ax, accel_reading.ay, accel_reading.az);
    display.printf("Gyro: X=%.2f\nY=%.2f Z=%.2f\n", gyro_reading.gx, gyro_reading.gy, gyro_reading.gz);
    display.display();

    delay(500);
  }
}

void setup() {
  BaseType_t rc;

  app_cpu = xPortGetCoreID();

  chsem = xSemaphoreCreateBinary();
  assert(chsem);

  i2sem = xSemaphoreCreateBinary();
  assert(i2sem);
  xSemaphoreGive(i2sem);

  temph = xQueueCreate(1, sizeof(s_temp));
  assert(temph);

  accelh = xQueueCreate(1, sizeof(s_accel));
  assert(accelh);

  gyroh = xQueueCreate(1, sizeof(s_gyro));
  assert(gyroh);

  Wire.begin(I2C_SDA, I2C_SCL);

  // เริ่มต้น MPU6050
  i2c_lock();
  mpu1.begin(0x68);  // ตัวแรกที่ 0x68
  mpu2.begin(0x69);  // ตัวที่สองที่ 0x69
  i2c_unlock();

  // เริ่มต้น OLED
  display.begin(OLED_I2C_ADDRESS, OLED_WIDTH, OLED_HEIGHT);
  display.clearDisplay();

  delay(2000);
  printf("\nMultitasking with MPU6050 and DHT22:\n");

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

void loop() {
  vTaskDelete(nullptr);
}