#include <Arduino_FreeRTOS.h>
#include <queue.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// Definisi antrian (queue)
QueueHandle_t textQueue;
QueueHandle_t delayQueue;

// Inisialisasi LCD
LiquidCrystal_I2C lcd(0x27, 16, 2); // Alamat I2C dan ukuran LCD

// Deklarasi prototipe fungsi
void displayText(String text);

// Tugas untuk menampilkan teks melalui Serial dan LCD
void vTaskDisplayText(void *pvParameters) {
  (void)pvParameters;

  for (;;) {
    // Baca teks dari antrian
    String text;
    if (xQueueReceive(textQueue, &text, portMAX_DELAY) == pdPASS) {
      // Baca delay dari antrian
      int delayValue;
      if (xQueueReceive(delayQueue, &delayValue, portMAX_DELAY) == pdPASS) {
        // Tampilkan teks melalui Serial
        displayText(text);
        
        // Tampilkan teks melalui LCD
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print(text);

        // Tunggu sesuai dengan nilai delay
        vTaskDelay(pdMS_TO_TICKS(delayValue));
      } else {
        Serial.println("Gagal membaca antrian delayQueue");
      }
    } else {
      Serial.println("Gagal membaca antrian textQueue");
    }
  }
}

// Tugas untuk membaca input Serial
void vTaskReadSerial(void *pvParameters) {
  (void)pvParameters;

  for (;;) {
    // Baca teks dari Serial1
    if (Serial1.available() > 0) {
      String inputText1 = Serial1.readStringUntil('\n');

      // Baca angka dari Serial1
      while (Serial1.available() == 0) {}
      int inputDelay1 = Serial1.parseInt();

      // Baca teks kedua dari Serial1
      while (Serial1.available() == 0) {}
      String inputText2 = Serial1.readStringUntil('\n');

      // Baca angka kedua dari Serial1
      while (Serial1.available() == 0) {}
      int inputDelay2 = Serial1.parseInt();

      // Periksa apakah nilai angka valid
      if (Serial1.available() == 0) {
        // Tambahkan teks dan angka ke dalam antrian
        if (xQueueSend(textQueue, &inputText1, portMAX_DELAY) == pdPASS &&
            xQueueSend(delayQueue, &inputDelay1, portMAX_DELAY) == pdPASS) {
          Serial.println("Input set 1 diterima dan dikirim ke antrian");
        } else {
          Serial.println("Gagal mengirim set 1 ke antrian");
        }

        if (xQueueSend(textQueue, &inputText2, portMAX_DELAY) == pdPASS &&
            xQueueSend(delayQueue, &inputDelay2, portMAX_DELAY) == pdPASS) {
          Serial.println("Input set 2 diterima dan dikirim ke antrian");
        } else {
          Serial.println("Gagal mengirim set 2 ke antrian");
        }
      } else {
        // Bersihkan sisa karakter di input
        Serial1.read();
        Serial.println("Gagal membaca nilai angka dari input");
      }
    }

    // Tunggu sejenak untuk menghindari busy-waiting
    vTaskDelay(pdMS_TO_TICKS(100));
  }
}

void setup() {
  Serial.begin(9600);  // Ini untuk Serial Monitor di Arduino IDE
  Serial1.begin(9600); // Ini untuk simulasi Wokwi menggunakan Serial1

  // Inisialisasi antrian dengan kapasitas maksimum 10 elemen
  textQueue = xQueueCreate(10, sizeof(String));
  delayQueue = xQueueCreate(10, sizeof(int));

  if (textQueue != NULL && delayQueue != NULL) {
    Serial.println("Antrian berhasil dibuat");
  } else {
    Serial.println("Gagal membuat antrian");
  }

  // Inisialisasi LCD
  lcd.begin(16, 2);
  lcd.print("Hello LCD!");

  // Buat tugas untuk menampilkan teks melalui Serial dan LCD
  if (xTaskCreate(vTaskDisplayText, "DisplayTask", 1000, NULL, 1, NULL) == pdPASS) {
    Serial.println("Tugas DisplayTask berhasil dibuat");
  } else {
    Serial.println("Gagal membuat tugas DisplayTask");
  }

  // Buat tugas untuk membaca input Serial
  if (xTaskCreate(vTaskReadSerial, "SerialTask", 1000, NULL, 2, NULL) == pdPASS) {
    Serial.println("Tugas SerialTask berhasil dibuat");
  } else {
    Serial.println("Gagal membuat tugas SerialTask");
  }

  // Mulai FreeRTOS scheduler
  vTaskStartScheduler();
}

void loop() {
  // Loop kosong, kode utama dijalankan dalam tugas-tugas FreeRTOS
}

// Implementasi fungsi displayText
void displayText(String text) {
  Serial.println(text);
}