/*
 * Demo FreeRTOS pada ESP32 di Platform Wokwi
 * 
 * Demonstrasi:
 * 1. Dua task mengirim dan menerima pesan via queue
 * 2. Menggunakan semaphore untuk melindungi resource bersama (LED, memori)
 * 3. Mensimulasikan deadlock dan menyelesaikannya
 */

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

// Definisi pin
#define LED_BUILTIN 2  // LED bawaan ESP32
#define LED_RED 27     // LED merah tambahan
#define LED_GREEN 26   // LED hijau tambahan
#define BUTTON_PIN 0   // Button untuk simulasi kesalahan

// Struktur pesan untuk queue
typedef struct {
  int sender_id;
  int value;
  unsigned long timestamp;
} Message;

// Handle untuk FreeRTOS objects
QueueHandle_t messageQueue;
SemaphoreHandle_t ledSemaphore;
SemaphoreHandle_t resourceASemaphore;
SemaphoreHandle_t resourceBSemaphore;

// Shared resource untuk simulasi
volatile int sharedCounter = 0;
volatile bool simulateDeadlock = false;

// Deklarasi task
void senderTask1(void *parameter);
void senderTask2(void *parameter);
void receiverTask(void *parameter);
void ledControlTask(void *parameter);
void watchdogTask(void *parameter);
void resourceATask(void *parameter);
void resourceBTask(void *parameter);

void setup() {
  // Inisialisasi serial
  Serial.begin(115200);
  delay(1000); // Waktu untuk serial monitor terbuka
  
  Serial.println("\n=== Demo FreeRTOS pada ESP32 (Wokwi) ===");
  Serial.println("Menginisialisasi sistem...");
  
  // Inisialisasi pin
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(LED_RED, OUTPUT);
  pinMode(LED_GREEN, OUTPUT);
  pinMode(BUTTON_PIN, INPUT_PULLUP);
  
  // Buat queue dengan kapasitas 10 pesan
  messageQueue = xQueueCreate(10, sizeof(Message));
  if (messageQueue == NULL) {
    Serial.println("Error: Gagal membuat queue!");
    while(1); // Hentikan eksekusi jika gagal
  }
  
  // Buat semaphore untuk LED
  ledSemaphore = xSemaphoreCreateMutex();
  if (ledSemaphore == NULL) {
    Serial.println("Error: Gagal membuat LED semaphore!");
    while(1);
  }
  
  // Buat semaphore untuk simulasi deadlock
  resourceASemaphore = xSemaphoreCreateMutex();
  resourceBSemaphore = xSemaphoreCreateMutex();
  if (resourceASemaphore == NULL || resourceBSemaphore == NULL) {
    Serial.println("Error: Gagal membuat resource semaphores!");
    while(1);
  }
  
  Serial.println("Sistem dimulai. Membuat tasks...");
  
  // Buat tasks
  xTaskCreate(
    senderTask1,          // Fungsi task
    "SenderTask1",        // Nama untuk debugging
    2048,                 // Stack size (bytes)
    NULL,                 // Parameter
    1,                    // Prioritas (1 = terendah)
    NULL                  // Task handle
  );
  
  xTaskCreate(
    senderTask2,
    "SenderTask2",
    2048,
    NULL,
    1,
    NULL
  );
  
  xTaskCreate(
    receiverTask,
    "ReceiverTask",
    2048,
    NULL,
    2,                    // Prioritas lebih tinggi
    NULL
  );
  
  xTaskCreate(
    ledControlTask,
    "LEDTask",
    2048,
    NULL,
    1,
    NULL
  );
  
  // Task untuk simulasi deadlock dan penyelesaiannya
  xTaskCreate(
    resourceATask,
    "ResourceATask",
    2048,
    NULL,
    1,
    NULL
  );
  
  xTaskCreate(
    resourceBTask,
    "ResourceBTask",
    2048,
    NULL,
    1,
    NULL
  );
  
  // Watchdog task untuk mendeteksi dan menyelesaikan deadlock
  xTaskCreate(
    watchdogTask,
    "WatchdogTask",
    2048,
    NULL,
    3,                    // Prioritas tinggi
    NULL
  );
  
  Serial.println("Semua task telah dibuat. Sistem berjalan!");
}

void loop() {
  // Periksa status tombol untuk memicu simulasi deadlock
  if (digitalRead(BUTTON_PIN) == LOW) {
    // Tombol ditekan
    delay(50);  // Debounce sederhana
    if (digitalRead(BUTTON_PIN) == LOW) {
      simulateDeadlock = true;
      Serial.println("TOMBOL DITEKAN: Simulasi deadlock akan dimulai!");
      while (digitalRead(BUTTON_PIN) == LOW) {
        delay(10); // Tunggu tombol dilepas
      }
    }
  }
  
  delay(100); // Tidak banyak yang perlu dilakukan di loop utama
}

// Task 1: Mengirim pesan ke queue
void senderTask1(void *parameter) {
  Message msg;
  msg.sender_id = 1;
  int counter = 0;
  
  while (true) {
    // Persiapkan pesan
    msg.value = counter++;
    msg.timestamp = millis();
    
    // Kirim pesan ke queue
    if (xQueueSend(messageQueue, &msg, pdMS_TO_TICKS(100)) == pdPASS) {
      Serial.print("Sender 1: Mengirim pesan nilai=");
      Serial.println(msg.value);
      
      // Gunakan LED semaphore untuk menunjukkan pengiriman berhasil
      if (xSemaphoreTake(ledSemaphore, pdMS_TO_TICKS(100)) == pdTRUE) {
        digitalWrite(LED_GREEN, HIGH);
        vTaskDelay(pdMS_TO_TICKS(50));
        digitalWrite(LED_GREEN, LOW);
        xSemaphoreGive(ledSemaphore);
      }
    } else {
      Serial.println("Sender 1: Queue penuh! Gagal mengirim.");
    }
    
    // Tunggu sebelum mengirim pesan berikutnya
    vTaskDelay(pdMS_TO_TICKS(1000));
  }
}

// Task 2: Mengirim pesan ke queue
void senderTask2(void *parameter) {
  Message msg;
  msg.sender_id = 2;
  int counter = 100; // Mulai dari nilai berbeda
  
  while (true) {
    // Persiapkan pesan
    msg.value = counter++;
    msg.timestamp = millis();
    
    // Kirim pesan ke queue
    if (xQueueSend(messageQueue, &msg, pdMS_TO_TICKS(100)) == pdPASS) {
      Serial.print("Sender 2: Mengirim pesan nilai=");
      Serial.println(msg.value);
      
      // Gunakan LED semaphore untuk menunjukkan pengiriman berhasil
      if (xSemaphoreTake(ledSemaphore, pdMS_TO_TICKS(100)) == pdTRUE) {
        digitalWrite(LED_RED, HIGH);
        vTaskDelay(pdMS_TO_TICKS(50));
        digitalWrite(LED_RED, LOW);
        xSemaphoreGive(ledSemaphore);
      }
    } else {
      Serial.println("Sender 2: Queue penuh! Gagal mengirim.");
    }
    
    // Tunggu sebelum mengirim pesan berikutnya
    vTaskDelay(pdMS_TO_TICKS(1500)); // Interval berbeda dari sender1
  }
}

// Task 3: Menerima dan memproses pesan dari queue
void receiverTask(void *parameter) {
  Message receivedMsg;
  
  while (true) {
    // Tunggu pesan dari queue
    if (xQueueReceive(messageQueue, &receivedMsg, pdMS_TO_TICKS(5000)) == pdPASS) {
      // Pesan diterima
      Serial.print("Receiver: Menerima pesan dari sender ");
      Serial.print(receivedMsg.sender_id);
      Serial.print(", nilai=");
      Serial.print(receivedMsg.value);
      Serial.print(", timestamp=");
      Serial.println(receivedMsg.timestamp);
      
      // Gunakan LED semaphore untuk menunjukkan penerimaan berhasil
      if (xSemaphoreTake(ledSemaphore, pdMS_TO_TICKS(100)) == pdTRUE) {
        digitalWrite(LED_BUILTIN, HIGH);
        vTaskDelay(pdMS_TO_TICKS(200));
        digitalWrite(LED_BUILTIN, LOW);
        xSemaphoreGive(ledSemaphore);
      }
    } else {
      // Timeout - tidak ada pesan diterima
      Serial.println("Receiver: Timeout - tidak ada pesan baru.");
    }
    
    // Jeda singkat
    vTaskDelay(pdMS_TO_TICKS(100));
  }
}

// Task 4: Kontrol LED dengan semaphore
void ledControlTask(void *parameter) {
  int flashPattern = 0;
  
  while (true) {
    // Pola flash untuk menunjukkan sistem berjalan
    if (xSemaphoreTake(ledSemaphore, pdMS_TO_TICKS(500)) == pdTRUE) {
      // Ada akses eksklusif ke LED
      switch(flashPattern) {
        case 0:
          digitalWrite(LED_BUILTIN, HIGH);
          digitalWrite(LED_RED, LOW);
          digitalWrite(LED_GREEN, LOW);
          break;
        case 1:
          digitalWrite(LED_BUILTIN, LOW);
          digitalWrite(LED_RED, HIGH);
          digitalWrite(LED_GREEN, LOW);
          break;
        case 2:
          digitalWrite(LED_BUILTIN, LOW);
          digitalWrite(LED_RED, LOW);
          digitalWrite(LED_GREEN, HIGH);
          break;
      }
      
      flashPattern = (flashPattern + 1) % 3;
      xSemaphoreGive(ledSemaphore);
    } else {
      Serial.println("LEDTask: Tidak bisa mendapat semaphore untuk LED!");
    }
    
    vTaskDelay(pdMS_TO_TICKS(3000)); // Jalankan pola setiap 3 detik
  }
}

// Task untuk mensimulasikan deadlock (Resource A)
void resourceATask(void *parameter) {
  while (true) {
    if (simulateDeadlock) {
      Serial.println("ResourceA: Mencoba mendapatkan ResourceA...");
      
      // Coba untuk mendapatkan resource A
      if (xSemaphoreTake(resourceASemaphore, pdMS_TO_TICKS(500)) == pdTRUE) {
        Serial.println("ResourceA: Berhasil mendapatkan ResourceA");
        
        // Perilaku normal: proses resource A
        sharedCounter += 1;
        Serial.print("ResourceA: Updated counter=");
        Serial.println(sharedCounter);
        
        // Untuk simulasi deadlock, tunggu sebentar lalu coba ambil resource B
        vTaskDelay(pdMS_TO_TICKS(1000));
        
        Serial.println("ResourceA: Mencoba mendapatkan ResourceB...");
        if (xSemaphoreTake(resourceBSemaphore, pdMS_TO_TICKS(5000)) == pdTRUE) {
          // Berhasil mendapatkan resource B
          sharedCounter += 10;
          Serial.print("ResourceA: Memiliki kedua resources. Counter=");
          Serial.println(sharedCounter);
          
          // Lepaskan resource B
          xSemaphoreGive(resourceBSemaphore);
          Serial.println("ResourceA: ResourceB dilepaskan");
        } else {
          // Timeout mendapatkan resource B - potensi deadlock
          Serial.println("ResourceA: TIMEOUT mendapatkan ResourceB - kemungkinan DEADLOCK!");
        }
        
        // Lepaskan resource A
        xSemaphoreGive(resourceASemaphore);
        Serial.println("ResourceA: ResourceA dilepaskan");
      }
      
      // Reset flag simulasi
      simulateDeadlock = false;
    }
    
    vTaskDelay(pdMS_TO_TICKS(500));
  }
}

// Task untuk mensimulasikan deadlock (Resource B)
void resourceBTask(void *parameter) {
  while (true) {
    if (simulateDeadlock) {
      Serial.println("ResourceB: Mencoba mendapatkan ResourceB...");
      
      // Coba untuk mendapatkan resource B
      if (xSemaphoreTake(resourceBSemaphore, pdMS_TO_TICKS(500)) == pdTRUE) {
        Serial.println("ResourceB: Berhasil mendapatkan ResourceB");
        
        // Perilaku normal: proses resource B
        sharedCounter += 5;
        Serial.print("ResourceB: Updated counter=");
        Serial.println(sharedCounter);
        
        // Untuk simulasi deadlock, tunggu sebentar lalu coba ambil resource A
        vTaskDelay(pdMS_TO_TICKS(1000));
        
        Serial.println("ResourceB: Mencoba mendapatkan ResourceA...");
        if (xSemaphoreTake(resourceASemaphore, pdMS_TO_TICKS(5000)) == pdTRUE) {
          // Berhasil mendapatkan resource A
          sharedCounter += 50;
          Serial.print("ResourceB: Memiliki kedua resources. Counter=");
          Serial.println(sharedCounter);
          
          // Lepaskan resource A
          xSemaphoreGive(resourceASemaphore);
          Serial.println("ResourceB: ResourceA dilepaskan");
        } else {
          // Timeout mendapatkan resource A - potensi deadlock
          Serial.println("ResourceB: TIMEOUT mendapatkan ResourceA - kemungkinan DEADLOCK!");
        }
        
        // Lepaskan resource B
        xSemaphoreGive(resourceBSemaphore);
        Serial.println("ResourceB: ResourceB dilepaskan");
      }
    }
    
    vTaskDelay(pdMS_TO_TICKS(500));
  }
}

// Watchdog task untuk mendeteksi dan memecahkan deadlock
void watchdogTask(void *parameter) {
  unsigned long lastCounterUpdate = 0;
  int lastCounterValue = sharedCounter;
  bool deadlockDetected = false;
  
  while (true) {
    // Periksa apakah counter telah diperbarui
    if (lastCounterValue != sharedCounter) {
      // Ada aktivitas, perbarui timestamp
      lastCounterUpdate = millis();
      lastCounterValue = sharedCounter;
      deadlockDetected = false;
    } else if (simulateDeadlock && millis() - lastCounterUpdate > 8000) {
      // Tidak ada aktivitas selama 8 detik saat simulasi deadlock aktif
      // Kemungkinan deadlock terdeteksi
      
      if (!deadlockDetected) {
        deadlockDetected = true;
        Serial.println("\n*** DEADLOCK TERDETEKSI! ***");
        Serial.println("Watchdog: Counter tidak berubah selama > 8 detik");
        
        // Tindakan untuk memecahkan deadlock
        Serial.println("Watchdog: Memaksa lepas semua semaphore untuk reset sistem...");
        
        // Reset semaphore dengan cara "paksa" lepaskan jika dipegang
        // Dalam produksi, ini harus dilakukan dengan lebih hati-hati
        xSemaphoreGive(resourceASemaphore);
        xSemaphoreGive(resourceBSemaphore);
        
        // Reset ulang semaphore (hapus dan buat ulang)
        vSemaphoreDelete(resourceASemaphore);
        vSemaphoreDelete(resourceBSemaphore);
        resourceASemaphore = xSemaphoreCreateMutex();
        resourceBSemaphore = xSemaphoreCreateMutex();
        
        Serial.println("Watchdog: Semaphore direset. Deadlock seharusnya dipecahkan.");
        Serial.println("*** SISTEM PULIH DARI DEADLOCK ***\n");
        
        // Kedipkan semua LED untuk menunjukkan recovery
        if (xSemaphoreTake(ledSemaphore, pdMS_TO_TICKS(500)) == pdTRUE) {
          for (int i = 0; i < 5; i++) {
            digitalWrite(LED_BUILTIN, HIGH);
            digitalWrite(LED_RED, HIGH);
            digitalWrite(LED_GREEN, HIGH);
            vTaskDelay(pdMS_TO_TICKS(100));
            digitalWrite(LED_BUILTIN, LOW);
            digitalWrite(LED_RED, LOW);
            digitalWrite(LED_GREEN, LOW);
            vTaskDelay(pdMS_TO_TICKS(100));
          }
          xSemaphoreGive(ledSemaphore);
        }
        
        // Reset variabel simulasi
        simulateDeadlock = false;
      }
    }
    
    vTaskDelay(pdMS_TO_TICKS(1000)); // Periksa setiap detik
  }
}