// Written By Chayathon Rungrueang
//ID 6530300082

//Disclaimer in Question it say print and show_oled is combine Task(you can see in Line 185++)

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

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

#define I2C_SDA 25
#define I2C_SCL 26
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

DHT dht(DHTPIN, DHTTYPE);
Adafruit_MPU6050 mpu;
Adafruit_MPU6050 mpu2;
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire);



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

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

struct r_rotation {
  float rx;
  float ry;
  float rz;
};

struct s_temp {
  float temp;
  float humidity;
};

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);
}

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);
  }
}

static void rotation_task(void *argp)
{
  r_rotation read_rotate;
  i2c_lock();
  if (!mpu2.begin(0x69)) {
    i2c_unlock();
    printf("Failed to initialize MPU6050!\n");
    vTaskDelete(nullptr);
  }
  i2c_unlock();
  for (;;)
  {
    sensors_event_t accel, gyro, temp;
    i2c_lock();
    mpu2.getEvent(&accel, &gyro, &temp);
    i2c_unlock();

    read_rotate.rx = gyro.gyro.x;
    read_rotate.ry = gyro.gyro.y;
    read_rotate.rz = gyro.gyro.z;

    xQueueOverwrite(gyroqh, &read_rotate);
    xSemaphoreGive(chsem);
    delay(500);

  }

}

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

  i2c_lock();
  if (!mpu.begin(0x68)) {
    i2c_unlock();
    printf("Failed to initialize MPU6050!\n");
    vTaskDelete(nullptr);
  }
  i2c_unlock();

  for (;;) {
    sensors_event_t a, g, temp;

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

static void disp_task(void *argp) {
  float temp,hum,acc_x,acc_y,acc_z,gy_x,gy_y,gy_z;
  s_temp temp_reading;
  s_accel accel_reading;
  r_rotation rotation_reading;
  BaseType_t rc;

  for (;;) {
    rc = xSemaphoreTake(chsem, portMAX_DELAY);
    assert(rc == pdPASS);

    rc = xQueuePeek(temph, &temp_reading, 0);
    if (rc == pdPASS) {
      printf("Temperature: %.2fC, Humidity: %.2f%%\n", temp_reading.temp, temp_reading.humidity);
      temp = temp_reading.temp;
      hum = temp_reading.humidity;
    } else {
      printf("No temperature data available.\n");
    }

    rc = xQueuePeek(accelh, &accel_reading, 0);
    if (rc == pdPASS) {
      printf("Acceleration - X: %.2f, Y: %.2f, Z: %.2f\n", accel_reading.ax, accel_reading.ay, accel_reading.az);
      acc_x = accel_reading.ax;
      acc_y = accel_reading.ay;
      acc_z = accel_reading.az;

    } else {
      printf("No acceleration data available.\n");
    }

    rc = xQueuePeek(gyroqh, &rotation_reading, 0);
    if (rc == pdPASS) {
      printf("Gyro - X: %.2f, Y: %.2f, Z: %.2f\n", rotation_reading.rx, rotation_reading.ry, rotation_reading.rz);
      gy_x = rotation_reading.rx;
      gy_y = rotation_reading.ry;
      gy_z = rotation_reading.rz;

    } else {
      printf("No Gyro data available.\n");
    }
    display.clearDisplay();
    display.setTextSize(0.3);
    display.setTextColor(WHITE);
    display.setCursor(0, 0);
    display.println(F("Temp  "));
    display.setCursor(30, 0);
    display.println(temp);
    display.setCursor(0, 10);
    display.println("Hum : ");
    display.setCursor(30, 10);
    display.println(hum);
    display.setCursor(0, 20);
    display.println("Acc X: " + String(acc_x) + ",Y: " + String(acc_y));  
    display.setCursor(25, 30);
    display.println("Z: " + String(acc_z));   
    display.setCursor(0, 40);
    display.println("Gyro X: " + String(gy_x) + ",Y: " + String(gy_y));  
    display.setCursor(25, 50);
    display.println("Z: " + String(gy_z));   



    display.display();
  }
}

void setup() {
  BaseType_t rc;

  app_cpu = xPortGetCoreID();

  chsem = xSemaphoreCreateBinary();
  assert(chsem);

  i2sem = xSemaphoreCreateBinary();
  assert(i2sem);

  rc = xSemaphoreGive(i2sem);
  assert(rc == pdPASS);

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

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

  gyroqh = xQueueCreate(1, sizeof(r_rotation));
  assert(gyroqh);

  // showoled = xQueueCreate(1, 2048);
  // assert(showoled);

  Wire.begin(I2C_SDA, I2C_SCL);
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
  }
  delay(2000);
  printf("\nmailbox_dht22_mpu6050.ino:\n");

  rc = xTaskCreatePinnedToCore(
         temp_task, "temp_task", 2400, nullptr, 1, nullptr, app_cpu);
  assert(rc == pdPASS);

  rc = xTaskCreatePinnedToCore(
         accel_task, "accel_task", 2400, nullptr, 1, nullptr, app_cpu);
  assert(rc == pdPASS);

  rc = xTaskCreatePinnedToCore(
         disp_task, "disp_task", 4000, nullptr, 1, nullptr, app_cpu);
  assert(rc == pdPASS);

  rc = xTaskCreatePinnedToCore(
         rotation_task, "rotation_task", 4000, nullptr, 1, nullptr, app_cpu);
  assert(rc == pdPASS);
}

void loop() {
  vTaskDelete(nullptr);
}