/* ================================================
   DEMO RTOS CONCEPT - STM32 Bluepill
   Tanpa library tambahan - pakai millis()
   Konsep: Cooperative Multitasking
   Mirip FreeRTOS tapi tanpa overhead kernel
   ================================================ */
 
// Pin definitions
#define LED_TASK1  PA0    // LED Merah - Task 1 (500ms)
#define LED_TASK2  PA1    // LED Biru  - Task 2 (1000ms)
#define LED_BOARD  PC13   // LED Onboard (2000ms)
#define LED_TASK4  PA2    // LED Tambahan - Task 4 (250ms)
 
// Task state variables
unsigned long task1_lastTime = 0;
unsigned long task2_lastTime = 0;
unsigned long task3_lastTime = 0;
unsigned long task4_lastTime = 0; // Tambahan state Task 4
unsigned long monitor_lastTime = 0;

bool task1_state = false;
bool task2_state = false;
bool task3_state = false;
bool task4_state = false; // Tambahan state Task 4

uint32_t task1_count = 0;
uint32_t task2_count = 0;
uint32_t task3_count = 0;
uint32_t task4_count = 0; // Tambahan counter Task 4
 
void vTask1_LED() {
  unsigned long now = millis();
  if (now - task1_lastTime >= 500) {
    task1_lastTime = now;
    task1_state = !task1_state;
    digitalWrite(LED_TASK1, task1_state);
    task1_count++;
  }
}
 
void vTask2_LED() {
  unsigned long now = millis();
  if (now - task2_lastTime >= 1000) {
    task2_lastTime = now;
    task2_state = !task2_state;
    digitalWrite(LED_TASK2, task2_state);
    task2_count++;
  }
}
 
void vTask3_LED() {
  unsigned long now = millis();
  if (now - task3_lastTime >= 2000) {
    task3_lastTime = now;
    task3_state = !task3_state;
    digitalWrite(LED_BOARD, task3_state);
    task3_count++;
  }
}

// ==== FUNGSI TASK 4 (Periode 250ms) ====
void vTask4_LED() {
  unsigned long now = millis();
  if (now - task4_lastTime >= 250) {   // periode 250ms
    task4_lastTime = now;
    task4_state = !task4_state;
    digitalWrite(LED_TASK4, task4_state);   
    task4_count++;
  }
}
 
void vTaskMonitor() {
  unsigned long now = millis();
  if (now - monitor_lastTime >= 2000) {
    monitor_lastTime = now;
    Serial.println("================================");
    Serial.print("[");
    Serial.print(now);
    Serial.println(" ms] Scheduler Running");
    Serial.print("Task4 (250ms)  cycles: "); // Tambahan Monitor Task 4
    Serial.println(task4_count);
    Serial.print("Task1 (500ms)  cycles: ");
    Serial.println(task1_count);
    Serial.print("Task2 (1000ms) cycles: ");
    Serial.println(task2_count);
    Serial.print("Task3 (2000ms) cycles: ");
    Serial.println(task3_count);
    
    Serial.flush();
  }
}
 
void setup() {
  Serial.begin(115200);
  delay(500);
  pinMode(LED_TASK1, OUTPUT);
  pinMode(LED_TASK2, OUTPUT);
  pinMode(LED_BOARD, OUTPUT);
  pinMode(LED_TASK4, OUTPUT); // Inisialisasi pin Task 4
  
  Serial.println("================================");
  Serial.println("  DEMO RTOS CONCEPT - BLUEPILL");
  Serial.println("================================");
  Serial.println("Task4: LED PA2  - period 250ms");
  Serial.println("Task1: LED PA0  - period 500ms");
  Serial.println("Task2: LED PA1  - period 1000ms");
  Serial.println("Task3: LED PC13 - period 2000ms");
  Serial.println("Scheduler started!");
  Serial.println("================================");
}
 
// loop() = RTOS SCHEDULER (Round Robin)
void loop() {
  vTask4_LED();      // Task 4 dipanggil SEBELUM Task 1
  vTask1_LED();      // Task 1: cek apakah sudah 500ms
  vTask2_LED();      // Task 2: cek apakah sudah 1000ms
  vTask3_LED();      // Task 3: cek apakah sudah 2000ms
  vTaskMonitor();    // Monitor: print status setiap 2s
}