#include <STM32FreeRTOS.h>

void Task_CANReceiver(void *pvParameters);
void Task_MotorControl(void *pvParameters);
void Task_LEDHandler(void *pvParameters);
void Task_LightControl(void *pvParameters);
void Task_ServoCop(void *pvParameters);
void Task_ServoGatMua(void *pvParameters);

void setup() {
  Serial.begin(115200);
  delay(1000);

  Serial.println("Starting all tasks...");

  xTaskCreate(Task_CANReceiver, "CAN", 128, NULL, 1, NULL);
  xTaskCreate(Task_MotorControl, "Motor", 128, NULL, 1, NULL);
  xTaskCreate(Task_LEDHandler, "LED", 128, NULL, 1, NULL);
  xTaskCreate(Task_LightControl, "Light", 128, NULL, 1, NULL);
  // xTaskCreate(Task_ServoCop, "ServoCop", 128, NULL, 1, NULL);
  // xTaskCreate(Task_ServoGatMua, "ServoGat", 128, NULL, 1, NULL);

  vTaskStartScheduler();
}

void loop() {}

void Task_CANReceiver(void *pvParameters) {
  while (1) {
    Serial.println("[CANReceiver] Checking for CAN messages...");
    vTaskDelay(800);
  }
}

void Task_MotorControl(void *pvParameters) {
  while (1) {
    Serial.println("[MotorControl] Processing motor control logic...");
    vTaskDelay(1000);
  }
}

void Task_LEDHandler(void *pvParameters) {
  while (1) {
    Serial.println("[LEDHandler] Blinking LEDs...");
    vTaskDelay(500);
  }
}

void Task_LightControl(void *pvParameters) {
  while (1) {
    Serial.println("[LightControl] Managing light system...");
    vTaskDelay(700);
  }
}

void Task_ServoCop(void *pvParameters) {
  while (1) {
    Serial.println("[ServoCop] Monitoring sensor and moving servo...");
    vTaskDelay(900);
  }
}

void Task_ServoGatMua(void *pvParameters) {
  while (1) {
    Serial.println("[ServoGatMua] Simulating servo movement based on pot...");
    vTaskDelay(600);
  }
}