//#define CONFIG_FREERTOS_USE_STATIC_ALLOCATION 1
//#define configSUPPORT_STATIC_ALLOCATION 1

#include <Arduino.h>  
#include <FreeRTOS.h> 
#include <task.h>     


#define GREEN_LED_PIN 4
#define BLUE_LED_PIN 2
#define RED_LED_PIN 8


#define STACK_SIZE 256

TaskHandle_t greenTaskHandle = NULL;
TaskHandle_t blueTaskHandle = NULL;
TaskHandle_t redTaskHandle = NULL;

void GreenTask(void *pvParameters) {
  Serial.println("Greentask jalan");
  while (1) {
    digitalWrite(GREEN_LED_PIN, HIGH);
    vTaskDelay(pdMS_TO_TICKS(500)); 
    digitalWrite(GREEN_LED_PIN, LOW);
    vTaskDelay(pdMS_TO_TICKS(500)); 
  }
}

void BlueTask(void *pvParameters) {
  Serial.println("Bluetask jalan");
  while (1) {
    digitalWrite(BLUE_LED_PIN, HIGH);
    vTaskDelay(pdMS_TO_TICKS(500)); 
    digitalWrite(BLUE_LED_PIN, LOW);
    vTaskDelay(pdMS_TO_TICKS(500)); 
  }
}

void RedTask(void *pvParameters) {
  Serial.println("Redtask jalan");
  while (1) {
    digitalWrite(RED_LED_PIN, HIGH);
    vTaskDelay(pdMS_TO_TICKS(500)); 
    digitalWrite(RED_LED_PIN, LOW);
    vTaskDelay(pdMS_TO_TICKS(500)); 
  }
}

void setup() {
  Serial.begin(115200);
  delay(100);
  Serial.println("Program Dimulai!");

  pinMode(GREEN_LED_PIN, OUTPUT);
  pinMode(BLUE_LED_PIN, OUTPUT);
  pinMode(RED_LED_PIN, OUTPUT);

  if (xTaskCreate(GreenTask, "GreenTask", STACK_SIZE, NULL, tskIDLE_PRIORITY + 2, NULL) != pdPASS) {
    Serial.println("Gagal membuat GreenTask!");
    while(1);
  } else {
    Serial.println("GreenTask dibuat.");
  }

  if (xTaskCreate(BlueTask, "BlueTask", STACK_SIZE, NULL, tskIDLE_PRIORITY + 1, &blueTaskHandle) != pdPASS) {
    Serial.println("Gagal membuat BlueTask!");
    while(1);
  } else {
    Serial.println("BlueTask dibuat.");
  }

  if (xTaskCreate(RedTask, "RedTask", STACK_SIZE, NULL, tskIDLE_PRIORITY + 1, NULL) != pdPASS) {
    Serial.println("Gagal membuat RedTask!");
    while(1);
  } else {
    Serial.println("RedTask dibuat.");
  }

  Serial.println("Semua task telah mencoba dibuat. Memulai Scheduler...");
  
  vTaskStartScheduler();

  Serial.println("ERROR = Scheduler tidak dapat dimulai atau ada masalah fatal!");
  while(1);
}

void loop() {
}