#include <Arduino.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <freertos/queue.h>

// Definisi struct untuk perintah dengan panjang body maksimal 8 karakter
struct Command{
  char body[8];
};

// Definisi struct untuk output dengan panjang body maksimal 128 karakter
struct Output{
  char body[128];
};

// Buat queue untuk Command dan Output, masing-masing dengan panjang 10
QueueHandle_t xCommandQueue = xQueueCreate(10, sizeof(Command*));
QueueHandle_t xOutputQueue = xQueueCreate(10, sizeof(Output*));

// Variabel global untuk menyimpan status LED
bool LEDStatus = false;

// Task untuk menerima input dari Serial Monitor
void Input(void *pvParameters){
  Command *cmd;  // Pointer untuk menyimpan perintah baru
  Output *out;   // Pointer untuk menyimpan output pesan error jika perintah tidak valid
  while (1){
    // Membaca input dari Serial Monitor hingga ada newline ('\n')
    String input = Serial.readStringUntil('\n');
    if (!input.isEmpty()){
      // Jika perintah adalah "led on" atau "led off"
      if (input == "led on" || input == "led off"){
        // Alokasikan memori untuk command menggunakan vPortMalloc
        cmd = (Command *)pvPortMalloc(sizeof(Command));
        // Salin input ke dalam body dari struct Command
        input.toCharArray(cmd->body, sizeof(cmd->body));
        // Kirim command ke Command Queue
        xQueueSend(xCommandQueue, &cmd, portMAX_DELAY);
      }else{
        // Jika input tidak valid, kirim pesan error ke Output Queue
        out = (Output *)pvPortMalloc(sizeof(Output));
        strcpy(out->body, "Error: Invalid command");
        xQueueSend(xOutputQueue, &out, portMAX_DELAY);
      }
    }
  }
}

// Task untuk memproses perintah dari Command Queue
void ProcessCmd(void *pvParameters){
  Command *cmd;   // Pointer untuk menyimpan command dari queue
  Output *out;    // Pointer untuk menyimpan output pesan sukses atau error
  int commandCode = 0;  // Kode numerik untuk switch-case

  while (1){
    // Cek apakah ada command baru dalam Command Queue
    if(xQueueReceive(xCommandQueue, &cmd, portMAX_DELAY)){
      // Tentukan case berdasarkan command string
      if (strcmp(cmd->body, "led on") == 0) {
        commandCode = 1;  // Kode untuk "led on"
      } else if (strcmp(cmd->body, "led off") == 0) {
        commandCode = 2;  // Kode untuk "led off"
      } else {
        commandCode = 0;  // Kode untuk perintah tidak valid
      }

      // Gunakan switch-case untuk menangani perintah
      switch (commandCode) {
        case 1:  // Case untuk "led on"
          if (!LEDStatus) {  // Jika LED saat ini mati
            digitalWrite(LED_BUILTIN, HIGH);
            LEDStatus = true;  // Ubah status LED menjadi menyala
            // Buat pesan sukses dan kirim ke Output Queue
            out = (Output *)pvPortMalloc(sizeof(Output));
            strcpy(out->body, "Success: led on");
            xQueueSend(xOutputQueue, &out, portMAX_DELAY);
          } else {
            // Kirim pesan error jika LED sudah menyala
            out = (Output *)pvPortMalloc(sizeof(Output));
            strcpy(out->body, "Error: LED already in the requested state (on)");
            xQueueSend(xOutputQueue, &out, portMAX_DELAY);
          }
          break;

        case 2:  // Case untuk "led off"
          if (LEDStatus) {  // Jika LED saat ini menyala
            digitalWrite(LED_BUILTIN, LOW);
            LEDStatus = false;  // Ubah status LED menjadi mati
            // Buat pesan sukses dan kirim ke Output Queue
            out = (Output *)pvPortMalloc(sizeof(Output));
            strcpy(out->body, "Success: led off");
            xQueueSend(xOutputQueue, &out, portMAX_DELAY);
          } else {
            // Kirim pesan error jika LED sudah mati
            out = (Output *)pvPortMalloc(sizeof(Output));
            strcpy(out->body, "Error: LED already in the requested state (off)");
            xQueueSend(xOutputQueue, &out, portMAX_DELAY);
          }
          break;

        default:  // Case untuk perintah tidak valid
          // Kirim pesan error untuk perintah yang tidak valid
          out = (Output *)pvPortMalloc(sizeof(Output));
          strcpy(out->body, "Error: Invalid command");
          xQueueSend(xOutputQueue, &out, portMAX_DELAY);
          break;
      }

      // Bebaskan memori yang digunakan oleh Task
      vPortFree(cmd);
    }
  }
}

// Task untuk menampilkan output dari Output Queue ke Serial Monitor
void OutputS(void *pvParameters){
  Output *out;  // Pointer untuk menyimpan output dari queue
  while (1){
    // Cek apakah ada output baru dalam Output Queue
    if(xQueueReceive(xOutputQueue, &out, portMAX_DELAY)){
      // Tampilkan pesan di Serial Monitor
      Serial.println(out->body);
      // Bebaskan memori yang digunakan oleh output
      vPortFree(out);
    }
  }
}

// Fungsi setup untuk inisialisasi program
void setup(){
  Serial.begin(115200);              // Inisialisasi Serial Monitor dengan baud rate 115200
  pinMode(LED_BUILTIN, OUTPUT);      // Set pin LED_BUILTIN sebagai output
  // Membuat task Input, ProcessCmd, dan OutputS, semuanya berjalan di core 1
  xTaskCreatePinnedToCore(Input, "Input", 1024, NULL, 1, NULL, 1);
  xTaskCreatePinnedToCore(ProcessCmd, "ProcessCmd", 1024, NULL, 1, NULL, 1);
  xTaskCreatePinnedToCore(OutputS, "OutputS", 1024, NULL, 1, NULL, 1);
}

// Loop utama kosong karena semua kerjaan dilakukan di task FreeRTOS
void loop() {}