#include "control_app.h"

void ControlTask_Main(void) {
    uint32_t cmdMsg;

    // Tắt toàn bộ thiết bị khi khởi động (Tất cả đang nằm trên Port A theo sơ đồ)
    HAL_GPIO_WritePin(GPIOA, MOSFET1_Pin | MOSFET2_Pin | RELAY1_Pin | RELAY2_Pin | BUZZER_Pin, GPIO_PIN_RESET);

    for (;;) {
        EventBits_t bits = xEventGroupWaitBits(AlertEvent, SMOKE_ALERT_BIT, pdTRUE, pdFALSE, 0);

        if ((bits & SMOKE_ALERT_BIT) != 0) {
            HAL_GPIO_WritePin(BUZZER_GPIO_Port, BUZZER_Pin, GPIO_PIN_SET);
            uint32_t alertMsg = PACK_MSG(CMD_SMOKE_ALERT, 0, 1);
            xQueueSend((QueueHandle_t)TelemetryQueueHandle, &alertMsg, 0);
        }

        if (xQueueReceive((QueueHandle_t)CommandQueueHandle, &cmdMsg, pdMS_TO_TICKS(50)) == pdTRUE) {

            uint8_t cmd = UNPACK_CMD(cmdMsg);
            uint8_t id = UNPACK_ID(cmdMsg);

            if (cmd == CMD_TURN_ON_RELAY || cmd == CMD_TURN_OFF_RELAY) {
                GPIO_PinState state = (cmd == CMD_TURN_ON_RELAY) ? GPIO_PIN_SET : GPIO_PIN_RESET;
                if (id == 0) HAL_GPIO_WritePin(RELAY1_GPIO_Port, RELAY1_Pin, state);
                else if (id == 1) HAL_GPIO_WritePin(RELAY2_GPIO_Port, RELAY2_Pin, state);

                uint32_t syncMsg = PACK_MSG(CMD_SYNC_RELAY, id, (state == GPIO_PIN_SET) ? 1 : 0);
                xQueueSend((QueueHandle_t)TelemetryQueueHandle, &syncMsg, 0);
            }

            else if (cmd == CMD_TURN_ON_MOSFET || cmd == CMD_TURN_OFF_MOSFET) {
                GPIO_PinState state = (cmd == CMD_TURN_ON_MOSFET) ? GPIO_PIN_SET : GPIO_PIN_RESET;
                if (id == 0) HAL_GPIO_WritePin(MOSFET1_GPIO_Port, MOSFET1_Pin, state);
                else if (id == 1) HAL_GPIO_WritePin(MOSFET2_GPIO_Port, MOSFET2_Pin, state);

                uint32_t syncMsg = PACK_MSG(CMD_SYNC_MOSFET, id, (state == GPIO_PIN_SET) ? 1 : 0);
                xQueueSend((QueueHandle_t)TelemetryQueueHandle, &syncMsg, 0);
            }
        }
    }
}