#include "uart_app.h"

static QueueHandle_t UartRxByteQueue = NULL;
static uint8_t rxByte;

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
    if (huart->Instance == USART3) {
        BaseType_t xHigherPriorityTaskWoken = pdFALSE;

        if (UartRxByteQueue != NULL) {
            xQueueSendFromISR(UartRxByteQueue, &rxByte, &xHigherPriorityTaskWoken);
        }

        HAL_UART_Receive_IT(&huart3, &rxByte, 1);
        portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
    }
}

void UartRxTask_Main(void) {
    UartRxByteQueue = xQueueCreate(32, sizeof(uint8_t));
    uint8_t rxBuffer[5];
    uint8_t rxIndex = 0;
    uint8_t incomingByte;

    HAL_UART_Receive_IT(&huart3, &rxByte, 1);

    for (;;) {
        if (xQueueReceive(UartRxByteQueue, &incomingByte, portMAX_DELAY) == pdTRUE) {
            if (rxIndex == 0 && incomingByte != 0xAA) continue;
            if (rxIndex == 1 && incomingByte != 0xBB) { rxIndex = 0; continue; }

            rxBuffer[rxIndex++] = incomingByte;

            if (rxIndex == 5) {
                uint8_t receivedChecksum = rxBuffer[4];
                uint8_t calculatedChecksum = 0xAA ^ 0xBB ^ rxBuffer[2] ^ rxBuffer[3];

                if (receivedChecksum == calculatedChecksum) {
                    uint32_t msg = PACK_MSG(rxBuffer[2], rxBuffer[3], 0);
                    xQueueSend((QueueHandle_t)CommandQueueHandle, &msg, 0);
                }
                rxIndex = 0;
            }
        }
    }
}

void UartTxTask_Main(void) {
    uint32_t telemetryMsg;
    uint8_t txFrame[6];

    for (;;) {
        if (xQueueReceive((QueueHandle_t)TelemetryQueueHandle, &telemetryMsg, portMAX_DELAY) == pdTRUE) {

            uint8_t cmd = UNPACK_CMD(telemetryMsg);
            uint8_t id = UNPACK_ID(telemetryMsg);
            uint16_t payload = UNPACK_PAYLOAD(telemetryMsg);
            uint16_t data16 = 0;

            if (cmd == CMD_UPDATE_TEMP || cmd == CMD_UPDATE_HUMID || cmd == CMD_MQ2_ANALOG || cmd == CMD_SMOKE_ALERT) {
                data16 = payload;
            }
            else if (cmd == CMD_SYNC_RELAY || cmd == CMD_SYNC_MOSFET) {
                data16 = (id << 4) | (payload & 0x0F);
            }

            txFrame[0] = 0xAA;
            txFrame[1] = 0xBB;
            txFrame[2] = cmd;
            txFrame[3] = (uint8_t)(data16 >> 8);
            txFrame[4] = (uint8_t)(data16 & 0xFF);
            txFrame[5] = txFrame[0] ^ txFrame[1] ^ txFrame[2] ^ txFrame[3] ^ txFrame[4];

            HAL_UART_Transmit(&huart3, txFrame, 6, 100);
        }
    }
}