#include <Arduino.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include <math.h> // untuk expf, sqrtf
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define PIN_SENSOR_SUHU 34
#define PIN_SENSOR_PM25 35
#define PIN_SENSOR_HUMID 32
#define PIN_SENSOR_GAS 33
#define PIN_BUZZER 23

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// ==================== Structs ====================
typedef struct {
  float gas;
  float pm25;
  float temperature;
  float humidity;
} SensorData;

typedef struct {
  SensorData raw;
  char quality[20];
} ClassifiedData;

// ==================== Queue Handles ====================
QueueHandle_t queueSensorToClassifier;
QueueHandle_t queueClassifierToDisplay;
QueueHandle_t queueClassifierToLogger;
QueueHandle_t queueClassifierToBuzzer;

// ==================== GNB Model ====================
#define NUM_CLASSES 3
#define NUM_FEATURES 4

typedef struct {
  const char* label;
  float prior;
  float mean[NUM_FEATURES];  // PM2.5, gas, suhu, RH
  float var[NUM_FEATURES];
} GNB_Class;

GNB_Class gnb_classes[NUM_CLASSES] = {
  { "BAIK",   0.35, {14, 406.428, 24, 58.142}, {6, 190.81, 1, 51.836} },
  { "SEDANG", 0.35, {43.714, 548.571, 27.785, 58.142}, {22.489, 1297.959, 0.632, 20.408} },
  { "BURUK",  0.3, {99.167, 698.33, 31.75,38.833}, {170.138, 980.555, 1.979, 16.472} }
};

float gaussian_prob(float x, float mean, float var) {
  float diff = x - mean;
  return (1.0 / sqrtf(2 * M_PI * var)) * expf(-(diff * diff) / (2 * var));
}

void classify_with_gnb(SensorData *data, char *output_class) {
  float max_prob = -1;
  int best_class = 0;

  float x[NUM_FEATURES] = { data->pm25, data->gas, data->temperature, data->humidity };

  for (int k = 0; k < NUM_CLASSES; k++) {
    float prob = gnb_classes[k].prior;
    for (int i = 0; i < NUM_FEATURES; i++) {
      prob *= gaussian_prob(x[i], gnb_classes[k].mean[i], gnb_classes[k].var[i]);
    }
    if (prob > max_prob) {
      max_prob = prob;
      best_class = k;
    }
  }

  strcpy(output_class, gnb_classes[best_class].label);
}

// ==================== Tasks ====================
void Task_Sensor(void *pvParameters) {
  SensorData data;
  while (1) {
    // Simulasi data sensor
    //data.gas = random(400, 1500);         // ppm
    //data.pm25 = random(10, 150);          // µg/m3
    //data.temperature = random(25, 35);    // °C
    //data.humidity = random(45, 75);       // %
    double gas = analogRead(PIN_SENSOR_GAS);
    double pm25 = analogRead(PIN_SENSOR_PM25);
    double temperature = analogRead(PIN_SENSOR_SUHU);
    double humidity = analogRead(PIN_SENSOR_HUMID);
    data.gas = (gas/4095)*750;
    data.pm25 = (pm25/4095)*120;
    data.temperature = (temperature/4095)*35;
    data.humidity = (humidity/4095)*70;
    //Serial.println(data.gas);


    xQueueSend(queueSensorToClassifier, &data, portMAX_DELAY);
    vTaskDelay(pdMS_TO_TICKS(1000)); // 2 detik
  }
}

void Task_Klasifikasi(void *pvParameters) {
  SensorData received;
  ClassifiedData result;

  while (1) {
    if (xQueueReceive(queueSensorToClassifier, &received, portMAX_DELAY)) {
      result.raw = received;
      classify_with_gnb(&received, result.quality);

      xQueueSend(queueClassifierToDisplay, &result, 0);
      xQueueSend(queueClassifierToLogger, &result, 0);
      xQueueSend(queueClassifierToBuzzer, &result, 0);
    }
  }
}

void Task_Display(void *pvParameters) {
  ClassifiedData data;
  while (1) {
    if (xQueueReceive(queueClassifierToDisplay, &data, portMAX_DELAY)) {
      Serial.printf("DISPLAY: PM2.5=%.1f µg/m³, GAS=%.1f ppm, Status=%s\n",
                    data.raw.pm25, data.raw.gas, data.quality);
      display.clearDisplay();
      display.setCursor(25, 0);
      display.setTextSize(2);
      display.print(data.quality);
      display.setTextSize(1);
      display.setCursor(15, 28);
      display.print("Gas: "); display.print(data.raw.gas, 2); display.print(" ppm");
      display.setCursor(15, 36);
      display.print("PM: "); display.print(data.raw.pm25); display.print(" mm/s");
      display.setCursor(15, 44);
      display.print("Suhu: "); display.print(data.raw.temperature); display.print(" C");
      display.display();
    }
  }
}

void Task_Logger(void *pvParameters) {
  ClassifiedData data;
  while (1) {
    if (xQueueReceive(queueClassifierToLogger, &data, portMAX_DELAY)) {
      Serial.printf("LOG: PM=%.1f, GAS=%.1f, Temp=%.1f°C, RH=%.1f%% -> %s\n",
                    data.raw.pm25, data.raw.gas, data.raw.temperature, data.raw.humidity, data.quality);
    }
  }
}

void Task_Buzzer(void *pvParameters) {
  ClassifiedData data;
  while (1) {
    if (xQueueReceive(queueClassifierToBuzzer, &data, portMAX_DELAY)) {
      if (strcmp(data.quality, "BURUK") == 0) {
        Serial.println("BUZZER ON (Simulasi)");
        digitalWrite(PIN_BUZZER, HIGH); // jika memakai buzzer nyata
      } else {
        Serial.println("BUZZER OFF");
        digitalWrite(PIN_BUZZER, LOW);
      }
    }
  }
}

// ==================== Setup dan Loop ====================
void setup() {
  Serial.begin(115200);
  delay(1000);

  // OLED setup
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 gagal start"));
    for (;;);
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);

  digitalWrite(PIN_BUZZER, LOW); // Jika memakai buzzer
  pinMode(PIN_BUZZER, OUTPUT);

  queueSensorToClassifier = xQueueCreate(5, sizeof(SensorData));
  queueClassifierToDisplay = xQueueCreate(5, sizeof(ClassifiedData));
  queueClassifierToLogger = xQueueCreate(5, sizeof(ClassifiedData));
  queueClassifierToBuzzer = xQueueCreate(5, sizeof(ClassifiedData));

  xTaskCreatePinnedToCore(Task_Sensor, "Task_Sensor", 4096, NULL, 5, NULL, 1);
  xTaskCreatePinnedToCore(Task_Klasifikasi, "Task_Klasifikasi", 4096, NULL, 5, NULL, 1);
  xTaskCreatePinnedToCore(Task_Display, "Task_Display", 4096, NULL, 4, NULL, 1);
  xTaskCreatePinnedToCore(Task_Logger, "Task_Logger", 4096, NULL, 3, NULL, 1);
  xTaskCreatePinnedToCore(Task_Buzzer, "Task_Buzzer", 4096, NULL, 3, NULL, 1);
}

void loop() {
  // Kosong karena semua pekerjaan dilakukan oleh task
}