#include "main.h" // Includes STM32 HAL headers
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"
#include "timers.h" // For FreeRTOS software timers

// --- Global Handles for RTOS Objects ---
// Task Handles
TaskHandle_t xServoControlTaskHandle = NULL;
TaskHandle_t xLEDStatusTaskHandle = NULL;
TaskHandle_t xSerialCommandTaskHandle = NULL;

// Queue Handles
QueueHandle_t xQueueServoCommand = NULL;
QueueHandle_t xQueueLEDCommand = NULL;
QueueHandle_t xQueueSerialRx = NULL; // For receiving characters from UART ISR

// Semaphore Handles
SemaphoreHandle_t xSemaphoreServoUpdate = NULL; // Binary semaphore for servo update trigger
SemaphoreHandle_t xMutexSerialOutput = NULL;    // Mutex for protecting serial output

// Timer Handle (FreeRTOS software timer)
TimerHandle_t xLEDModeChangeTimer = NULL;

// --- Global Variables ---
UART_HandleTypeDef huart1;
TIM_HandleTypeDef htim2; // Timer for servo PWM and interrupt
TIM_HandleTypeDef htim3; // Timer for periodic interrupt (to signal servo task)

// --- Enums and Structs for Inter-Task Communication ---
typedef enum
{
    LED_MODE_OFF,
    LED_MODE_RED_SLOW,
    LED_MODE_GREEN_FAST,
    LED_MODE_YELLOW_SOLID
} LED_Mode_t;

typedef struct
{
    int angle; // Target angle for the servo
} ServoCommand_t;

typedef struct
{
    LED_Mode_t mode;
} LEDCommand_t;

// --- Function Prototypes ---
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_USART1_UART_Init(void);
static void MX_TIM2_Init(void); // For servo PWM
static void MX_TIM3_Init(void); // For periodic interrupt

void vServoControlTask(void *pvParameters);
void vLEDStatusTask(void *pvParameters);
void vSerialCommandTask(void *pvParameters);

void print_serial_protected(const char *format, ...);
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim); // Timer interrupt callback
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart);     // UART receive callback

// FreeRTOS software timer callback
void vLEDModeChangeTimerCallback(TimerHandle_t xTimer);

// --- Main Function ---
int main(void)
{
    // Reset of all peripherals, Initializes the Flash interface and the Systick.
    HAL_Init();

    // Configure the system clock
    SystemClock_Config();

    // Initialize all configured peripherals
    MX_GPIO_Init();
    MX_USART1_UART_Init();
    MX_TIM2_Init(); // Initialize TIM2 for servo PWM
    MX_TIM3_Init(); // Initialize TIM3 for periodic interrupt

    // Start PWM on channel 1 of TIM2 for the servo (connected to PA0)
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);

    // Start TIM3 in interrupt mode (for periodic servo updates)
    HAL_TIM_Base_Start_IT(&htim3);

    // --- Create RTOS Objects ---
    // Create queues
    xQueueServoCommand = xQueueCreate(5, sizeof(ServoCommand_t));
    xQueueLEDCommand = xQueueCreate(5, sizeof(LEDCommand_t));
    xQueueSerialRx = xQueueCreate(10, sizeof(char)); // Queue for individual characters from UART ISR

    // Create semaphores/mutexes
    xSemaphoreServoUpdate = xSemaphoreCreateBinary();
    xMutexSerialOutput = xSemaphoreCreateMutex();

    // Check if RTOS objects were created successfully
    if (xQueueServoCommand == NULL || xQueueLEDCommand == NULL || xQueueSerialRx == NULL ||
        xSemaphoreServoUpdate == NULL || xMutexSerialOutput == NULL)
    {
        // Error handling: Could not create RTOS objects.
        // In a real application, you might blink an error LED or halt.
        while (1)
        {
            // Infinite loop indicating an error
            HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13); // On-board LED (PC13)
            HAL_Delay(100);
        }
    }

    // Create FreeRTOS software timer for LED mode changes
    // Period: 10 seconds (10000 ms)
    // Auto-reload: pdTRUE (repeats automatically)
    // ID: 0 (not used in this example, but good practice to assign)
    xLEDModeChangeTimer = xTimerCreate("LEDModeTimer", pdMS_TO_TICKS(10000), pdTRUE, (void *)0, vLEDModeChangeTimerCallback);
    if (xLEDModeChangeTimer != NULL)
    {
        xTimerStart(xLEDModeChangeTimer, 0); // Start the timer immediately
    }

    // --- Create Tasks ---
    // Servo Control Task: High priority as precise movement might be critical
    xTaskCreate(vServoControlTask, "ServoControl", configMINIMAL_STACK_SIZE * 2, NULL, tskIDLE_PRIORITY + 3, &xServoControlTaskHandle);

    // LED Status Task: Medium priority
    xTaskCreate(vLEDStatusTask, "LEDStatus", configMINIMAL_STACK_SIZE * 2, NULL, tskIDLE_PRIORITY + 2, &xLEDStatusTaskHandle);

    // Serial Command Task: Medium priority, as it handles user input
    xTaskCreate(vSerialCommandTask, "SerialCommand", configMINIMAL_STACK_SIZE * 4, NULL, tskIDLE_PRIORITY + 2, &xSerialCommandTaskHandle);

    // Start the FreeRTOS scheduler
    vTaskStartScheduler();

    // Should never reach here unless there is not enough heap memory.
    while (1)
    {
        // Infinite loop if scheduler fails to start
    }
}

// --- Task Definitions ---

/**
 * @brief Task to control the servo motor.
 * Receives target angles via a queue and updates the servo position.
 * Triggered by a semaphore given from a timer ISR.
 * @param pvParameters Not used.
 */
void vServoControlTask(void *pvParameters)
{
    (void)pvParameters; // Cast to void to suppress unused parameter warning

    ServoCommand_t currentServoCommand = {90}; // Default to 90 degrees
    uint16_t pwm_pulse = 0;

    // Function to set servo PWM pulse width (adjust for your servo)
    // A typical servo expects 50Hz PWM (20ms period).
    // Pulse width from ~500us (0 deg) to ~2500us (180 deg).
    // With TIM2 configured for 1MHz counter clock (Prescaler 71, Period 19999),
    // 1us corresponds to 1 count.
    // 500us = 500 counts
    // 2500us = 2500 counts
    auto set_servo_angle = [&](int angle) {
        if (angle < 0) angle = 0;
        if (angle > 180) angle = 180;

        // Linear mapping: 0 deg -> 500, 180 deg -> 2500
        pwm_pulse = 500 + (angle * (2000 / 180));
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, pwm_pulse);
        print_serial_protected("Servo set to %d degrees (PWM: %d)\r\n", angle, pwm_pulse);
    };

    set_servo_angle(currentServoCommand.angle); // Set initial position

    for (;;)
    {
        // Wait indefinitely for the semaphore to be given by the Timer ISR
        if (xSemaphoreTake(xSemaphoreServoUpdate, portMAX_DELAY) == pdTRUE)
        {
            // Check if a new command is available in the queue
            if (xQueueReceive(xQueueServoCommand, &currentServoCommand, 0) == pdPASS)
            {
                // New command received, update servo angle
                set_servo_angle(currentServoCommand.angle);
            }
            else
            {
                // No new command, just re-apply the current angle (useful if servo drifts or for periodic updates)
                set_servo_angle(currentServoCommand.angle);
            }
        }
    }
}

/**
 * @brief Task to control the status LEDs based on received commands.
 * @param pvParameters Not used.
 */
void vLEDStatusTask(void *pvParameters)
{
    (void)pvParameters;

    LEDCommand_t currentLEDCommand = {LED_MODE_GREEN_FAST}; // Default mode
    TickType_t xLastWakeTime;
    const TickType_t xFrequency = pdMS_TO_TICKS(50); // Base frequency for LED updates

    xLastWakeTime = xTaskGetTickCount();

    for (;;)
    {
        // Check for new LED commands without blocking
        if (xQueueReceive(xQueueLEDCommand, &currentLEDCommand, 0) == pdPASS)
        {
            print_serial_protected("LED mode changed to: ");
            switch (currentLEDCommand.mode)
            {
            case LED_MODE_OFF:
                print_serial_protected("OFF\r\n");
                break;
            case LED_MODE_RED_SLOW:
                print_serial_protected("RED_SLOW\r\n");
                break;
            case LED_MODE_GREEN_FAST:
                print_serial_protected("GREEN_FAST\r\n");
                break;
            case LED_MODE_YELLOW_SOLID:
                print_serial_protected("YELLOW_SOLID\r\n");
                break;
            }
        }

        // Implement LED logic based on current mode
        switch (currentLEDCommand.mode)
        {
        case LED_MODE_OFF:
            HAL_GPIO_WritePin(GPIOA, GPIO_PIN_1, GPIO_PIN_RESET); // Red
            HAL_GPIO_WritePin(GPIOA, GPIO_PIN_2, GPIO_PIN_RESET); // Green
            HAL_GPIO_WritePin(GPIOA, GPIO_PIN_3, GPIO_PIN_RESET); // Yellow
            break;
        case LED_MODE_RED_SLOW:
            // Blink Red LED slowly (e.g., 500ms on, 500ms off)
            if ((xTaskGetTickCount() / pdMS_TO_TICKS(500)) % 2 == 0)
            {
                HAL_GPIO_WritePin(GPIOA, GPIO_PIN_1, GPIO_PIN_SET);
            }
            else
            {
                HAL_GPIO_WritePin(GPIOA, GPIO_PIN_1, GPIO_PIN_RESET);
            }
            HAL_GPIO_WritePin(GPIOA, GPIO_PIN_2, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(GPIOA, GPIO_PIN_3, GPIO_PIN_RESET);
            break;
        case LED_MODE_GREEN_FAST:
            // Blink Green LED quickly (e.g., 100ms on, 100ms off)
            if ((xTaskGetTickCount() / pdMS_TO_TICKS(100)) % 2 == 0)
            {
                HAL_GPIO_WritePin(GPIOA, GPIO_PIN_2, GPIO_PIN_SET);
            }
            else
            {
                HAL_GPIO_WritePin(GPIOA, GPIO_PIN_2, GPIO_PIN_RESET);
            }
            HAL_GPIO_WritePin(GPIOA, GPIO_PIN_1, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(GPIOA, GPIO_PIN_3, GPIO_PIN_RESET);
            break;
        case LED_MODE_YELLOW_SOLID:
            HAL_GPIO_WritePin(GPIOA, GPIO_PIN_1, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(GPIOA, GPIO_PIN_2, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(GPIOA, GPIO_PIN_3, GPIO_PIN_SET); // Yellow solid
            break;
        }

        // Delay for a short period to allow other tasks to run and control blink rate
        vTaskDelayUntil(&xLastWakeTime, xFrequency);
    }
}

/**
 * @brief Task to receive and process commands from the serial monitor.
 * Sends parsed commands to other tasks via queues.
 * Uses a mutex to protect serial output.
 * @param pvParameters Not used.
 */
void vSerialCommandTask(void *pvParameters)
{
    (void)pvParameters;

    char rx_char;
    char rx_buffer[64]; // Buffer to store incoming serial data
    int buffer_idx = 0;

    // Start receiving characters via UART interrupt
    HAL_UART_Receive_IT(&huart1, (uint8_t *)&rx_char, 1);

    print_serial_protected("\r\n--- Smart Actuator and Indicator System ---\r\n");
    print_serial_protected("Available commands:\r\n");
    print_serial_protected("  servo <angle> (e.g., servo 90)\r\n");
    print_serial_protected("  led <mode> (modes: off, red_slow, green_fast, yellow_solid)\r\n");
    print_serial_protected("-------------------------------------------\r\n");

    for (;;)
    {
        // Wait for a character to be received from the UART ISR
        if (xQueueReceive(xQueueSerialRx, &rx_char, portMAX_DELAY) == pdPASS)
        {
            // Echo character back
            HAL_UART_Transmit(&huart1, (uint8_t *)&rx_char, 1, HAL_MAX_DELAY);

            if (rx_char == '\r' || rx_char == '\n')
            {
                if (buffer_idx > 0)
                {
                    rx_buffer[buffer_idx] = '\0'; // Null-terminate the string
                    print_serial_protected("\r\nReceived command: '%s'\r\n", rx_buffer);

                    // --- Command Parsing ---
                    if (strncmp(rx_buffer, "servo ", 6) == 0)
                    {
                        int angle = atoi(&rx_buffer[6]);
                        ServoCommand_t cmd = {angle};
                        if (xQueueSend(xQueueServoCommand, &cmd, 0) != pdPASS)
                        {
                            print_serial_protected("Error: Servo command queue full.\r\n");
                        }
                        else
                        {
                            print_serial_protected("Servo command sent: %d\r\n", angle);
                        }
                    }
                    else if (strncmp(rx_buffer, "led ", 4) == 0)
                    {
                        LEDCommand_t cmd;
                        char *mode_str = &rx_buffer[4];
                        if (strcmp(mode_str, "off") == 0)
                        {
                            cmd.mode = LED_MODE_OFF;
                        }
                        else if (strcmp(mode_str, "red_slow") == 0)
                        {
                            cmd.mode = LED_MODE_RED_SLOW;
                        }
                        else if (strcmp(mode_str, "green_fast") == 0)
                        {
                            cmd.mode = LED_MODE_GREEN_FAST;
                        }
                        else if (strcmp(mode_str, "yellow_solid") == 0)
                        {
                            cmd.mode = LED_MODE_YELLOW_SOLID;
                        }
                        else
                        {
                            print_serial_protected("Unknown LED mode: %s\r\n", mode_str);
                            buffer_idx = 0; // Reset buffer on unknown command
                            continue;       // Skip sending to queue
                        }

                        if (xQueueSend(xQueueLEDCommand, &cmd, 0) != pdPASS)
                        {
                            print_serial_protected("Error: LED command queue full.\r\n");
                        }
                        else
                        {
                            print_serial_protected("LED command sent.\r\n");
                        }
                    }
                    else
                    {
                        print_serial_protected("Unknown command.\r\n");
                    }
                    buffer_idx = 0; // Reset buffer after processing
                }
            }
            else
            {
                // Store character in buffer, handle buffer overflow
                if (buffer_idx < sizeof(rx_buffer) - 1)
                {
                    rx_buffer[buffer_idx++] = rx_char;
                }
                else
                {
                    print_serial_protected("\r\nError: Serial buffer overflow.\r\n");
                    buffer_idx = 0; // Reset buffer on overflow
                }
            }
            // Re-enable UART receive interrupt for the next character
            HAL_UART_Receive_IT(&huart1, (uint8_t *)&rx_char, 1);
        }
    }
}

// --- Helper Functions ---

/**
 * @brief Prints a formatted string to the serial monitor, protected by a mutex.
 * @param format Format string.
 * @param ... Variable arguments.
 */
void print_serial_protected(const char *format, ...)
{
    if (xSemaphoreTake(xMutexSerialOutput, portMAX_DELAY) == pdTRUE)
    {
        char buffer[128]; // Small buffer for formatted string
        va_list args;
        va_start(args, format);
        vsnprintf(buffer, sizeof(buffer), format, args);
        va_end(args);

        HAL_UART_Transmit(&huart1, (uint8_t *)buffer, strlen(buffer), HAL_MAX_DELAY);

        xSemaphoreGive(xMutexSerialOutput);
    }
}

// --- Interrupt Service Routines (ISRs) and Callbacks ---

/**
 * @brief Callback for TIM3 period elapsed interrupt.
 * Gives a semaphore to unblock the servo control task.
 * @param htim Pointer to the TIM handle.
 */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    if (htim->Instance == TIM3->Instance)
    {
        BaseType_t xHigherPriorityTaskWoken = pdFALSE;
        // Give the semaphore from ISR context
        xSemaphoreGiveFromISR(xSemaphoreServoUpdate, &xHigherPriorityTaskWoken);
        portYIELD_FROM_ISR(xHigherPriorityTaskWoken); // Request context switch if higher priority task unblocked
    }
}

/**
 * @brief Callback for UART receive complete interrupt.
 * Sends the received character to the serial receive queue.
 * @param huart Pointer to the UART handle.
 */
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
    if (huart->Instance == USART1)
    {
        BaseType_t xHigherPriorityTaskWoken = pdFALSE;
        char rx_char;
        // Read the received character (assuming it's already in huart->Instance->DR)
        // For HAL, the character is often passed implicitly or needs to be read from DR.
        // In this setup, the character is passed to the RxCpltCallback via the `huart` handle's Rx buffer.
        // We need to re-enable receive IT after processing.
        rx_char = huart->pRxBuffPtr[0]; // Assuming pRxBuffPtr points to the single character buffer

        // Send character to the queue from ISR context
        xQueueSendFromISR(xQueueSerialRx, &rx_char, &xHigherPriorityTaskWoken);

        // Re-enable UART receive interrupt for the next character
        // Note: This is crucial! Without it, only one character will be received.
        HAL_UART_Receive_IT(&huart1, (uint8_t *)&rx_char, 1);

        portYIELD_FROM_ISR(xHigherPriorityTaskWoken); // Request context switch if higher priority task unblocked
    }
}

/**
 * @brief FreeRTOS software timer callback.
 * Changes the LED mode periodically.
 * @param xTimer Handle of the timer that expired.
 */
void vLEDModeChangeTimerCallback(TimerHandle_t xTimer)
{
    static LED_Mode_t currentMode = LED_MODE_GREEN_FAST;
    LEDCommand_t cmd;

    // Cycle through LED modes
    switch (currentMode)
    {
    case LED_MODE_GREEN_FAST:
        currentMode = LED_MODE_RED_SLOW;
        break;
    case LED_MODE_RED_SLOW:
        currentMode = LED_MODE_YELLOW_SOLID;
        break;
    case LED_MODE_YELLOW_SOLID:
        currentMode = LED_MODE_GREEN_FAST;
        break;
    default:
        currentMode = LED_MODE_GREEN_FAST; // Fallback
        break;
    }
    cmd.mode = currentMode;

    // Send the new LED command to the LED task (non-blocking)
    if (xQueueSend(xQueueLEDCommand, &cmd, 0) != pdPASS)
    {
        // This might happen if the LED task is not processing fast enough
        // or if the queue is very small. For a software timer, it's good to handle.
        // print_serial_protected("Warning: LED command not sent from timer callback (queue full).\r\n");
    }
}

// --- STM32 HAL Peripheral Initialization Functions ---

/**
 * @brief System Clock Configuration
 * @retval None
 */
void SystemClock_Config(void)
{
    RCC_OscInitTypeDef RCC_OscInitStruct = {0};
    RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

    /** Initializes the RCC Oscillators according to the specified parameters
     * in the RCC_OscInitTypeDef structure.
     */
    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
    RCC_OscInitStruct.HSEState = RCC_HSE_ON;
    RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
    RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
    RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9; // 8MHz * 9 = 72MHz
    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
    {
        Error_Handler();
    }

    /** Initializes the CPU, AHB and APB buses clocks
     */
    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
    RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2; // Max 36MHz
    RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; // Max 72MHz

    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
    {
        Error_Handler();
    }
}

/**
 * @brief GPIO Initialization Function
 * @param None
 * @retval None
 */
static void MX_GPIO_Init(void)
{
    GPIO_InitTypeDef GPIO_InitStruct = {0};

    /* GPIO Ports Clock Enable */
    __HAL_RCC_GPIOC_CLK_ENABLE(); // For PC13 (on-board LED)
    __HAL_RCC_GPIOA_CLK_ENABLE();
    __HAL_RCC_GPIOB_CLK_ENABLE();

    /*Configure GPIO pin Output Level */
    HAL_GPIO_WritePin(GPIOC, GPIO_PIN_13, GPIO_PIN_RESET); // On-board LED off
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_1 | GPIO_PIN_2 | GPIO_PIN_3, GPIO_PIN_RESET); // LEDs off

    /*Configure GPIO pin : PC13 (on-board LED) */
    GPIO_InitStruct.Pin = GPIO_PIN_13;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);

    /*Configure GPIO pins : PA1 PA2 PA3 (Red, Green, Yellow LEDs) */
    GPIO_InitStruct.Pin = GPIO_PIN_1 | GPIO_PIN_2 | GPIO_PIN_3;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
}

/**
 * @brief USART1 Initialization Function (for Serial Monitor)
 * PA9: TX, PA10: RX
 * @param None
 * @retval None
 */
static void MX_USART1_UART_Init(void)
{
    huart1.Instance = USART1;
    huart1.Init.BaudRate = 115200;
    huart1.Init.WordLength = UART_WORDLENGTH_8B;
    huart1.Init.StopBits = UART_STOPBITS_1;
    huart1.Init.Parity = UART_PARITY_NONE;
    huart1.Init.Mode = UART_MODE_TX_RX;
    huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
    huart1.Init.OverSampling = UART_OVERSAMPLING_16;
    if (HAL_UART_Init(&huart1) != HAL_OK)
    {
        Error_Handler();
    }
}

/**
 * @brief TIM2 Initialization Function (for Servo PWM)
 * PA0: TIM2_CH1
 * @param None
 * @retval None
 */
static void MX_TIM2_Init(void)
{
    TIM_MasterConfigTypeDef sMasterConfig = {0};
    TIM_OC_InitTypeDef sConfigOC = {0};

    htim2.Instance = TIM2;
    htim2.Init.Prescaler = 71; // 72MHz / (71 + 1) = 1MHz counter clock
    htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
    htim2.Init.Period = 19999; // 1MHz / (19999 + 1) = 50Hz PWM frequency (20ms period)
    htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
    if (HAL_TIM_PWM_Init(&htim2) != HAL_OK)
    {
        Error_Handler();
    }

    sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
    sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
    if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
    {
        Error_Handler();
    }

    sConfigOC.OCMode = TIM_OCMODE_PWM1;
    sConfigOC.Pulse = 1500; // Initial pulse width (1.5ms for 90 degrees)
    sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
    sConfigOC.OCFastOutput = TIM_OCFAST_DISABLE;
    if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
    {
        Error_Handler();
    }

    /* Configure PA0 as alternate function for TIM2_CH1 */
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    GPIO_InitStruct.Pin = GPIO_PIN_0;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; // Alternate function push-pull
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
}

/**
 * @brief TIM3 Initialization Function (for Periodic Interrupt)
 * @param None
 * @retval None
 */
static void MX_TIM3_Init(void)
{
    TIM_ClockConfigTypeDef sClockSourceConfig = {0};
    TIM_MasterConfigTypeDef sMasterConfig = {0};

    htim3.Instance = TIM3;
    htim3.Init.Prescaler = 7199; // 72MHz / (7199 + 1) = 10kHz counter clock
    htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
    htim3.Init.Period = 999; // 10kHz / (999 + 1) = 10Hz interrupt frequency (100ms period)
    htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
    if (HAL_TIM_Base_Init(&htim3) != HAL_OK)
    {
        Error_Handler();
    }

    sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
    if (HAL_TIM_ConfigClockSource(&htim3, &sClockSourceConfig) != HAL_OK)
    {
        Error_Handler();
    }
    sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
    sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
    if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
    {
        Error_Handler();
    }

    /* TIM3 interrupt Init */
    HAL_NVIC_SetPriority(TIM3_IRQn, 5, 0); // Set interrupt priority (adjust as needed for FreeRTOS)
    HAL_NVIC_EnableIRQ(TIM3_IRQn);
}

/**
 * @brief This function is executed in case of error occurrence.
 * @retval None
 */
void Error_Handler(void)
{
    /* User can add his own implementation to report the HAL error return state */
    __disable_irq();
    while (1)
    {
        HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13); // Blink on-board LED (PC13)
        HAL_Delay(500);
    }
}

#ifdef USE_FULL_ASSERT
/**
 * @brief Reports the name of the source file and the source line number
 * where the assert_param error has occurred.
 * @param file: pointer to the source file name
 * @param line: assert_param error line source number
 * @retval None
 */
void assert_failed(uint8_t *file, uint32_t line)
{
    /* User can add his own implementation to report the file name and line number,
     e.g. printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
}
#endif /* USE_FULL_ASSERT */

// --- FreeRTOS Hook Functions (Optional but good practice) ---

// vApplicationMallocFailedHook() will only be called if
// configUSE_MALLOC_FAILED_HOOK is set to 1 in FreeRTOSConfig.h.
void vApplicationMallocFailedHook(void)
{
    /* Called if a call to pvPortMalloc() fails because there is insufficient
    free memory available in the FreeRTOS heap.  pvPortMalloc() is called
    internally by FreeRTOS API functions that create tasks, queues, software
    timers, and semaphores.  The size of the FreeRTOS heap is defined by
    configTOTAL_HEAP_SIZE in FreeRTOSConfig.h. */
    taskDISABLE_INTERRUPTS();
    for (;;)
    {
        HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);
        HAL_Delay(200);
    }
}

// vApplicationIdleHook() will only be called if configUSE_IDLE_HOOK is set
// to 1 in FreeRTOSConfig.h.
void vApplicationIdleHook(void)
{
    /* Called on each iteration of the idle task.  At this point which specifies
    that the system is idle, and can be used to power down the processor,
    or enter a low power state. */
    // Example: Put the CPU into a low power state if supported
    // __WFI(); // Wait For Interrupt
}

// vApplicationStackOverflowHook() will only be called if
// configCHECK_FOR_STACK_OVERFLOW is set to 1 or 2.
void vApplicationStackOverflowHook(TaskHandle_t pxTask, char *pcTaskName)
{
    /* Run time stack overflow checking is performed if
    configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2.  This hook
    function is called if a stack overflow is detected. */
    (void)pxTask;
    (void)pcTaskName;
    taskDISABLE_INTERRUPTS();
    for (;;)
    {
        HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);
        HAL_Delay(50);
    }
}

// vApplicationTickHook() will only be called if configUSE_TICK_HOOK is set
// to 1 in FreeRTOSConfig.h.
void vApplicationTickHook(void)
{
    /* This function will be called by each tick interrupt if
    configUSE_TICK_HOOK is set to 1 in FreeRTOSConfig.h.  User code can be
    added here, but the tick hook is normally only used to increment a
    counter to keep track of the number of ticks that have occurred since
    the scheduler was started. */
}