uint32_t start = xTaskGetTickCount();
startReading();

while ((xTaskGetTickCount() - start) < pdMS_TO_TICKS(1000))
{
    // Step 1: Receive 3-byte header
    usart_transfer_t rxHeader = {
        .data = msg,
        .dataSize = 3
    };
    if (USART_TransferReceiveNonBlocking(UHF_USART, &g_uartHandle, &rxHeader, NULL) != kStatus_Success ||
        xSemaphoreTake(uhf_rx_done_sema, pdMS_TO_TICKS(200)) != pdTRUE)
    {
        continue;
    }

    uint8_t len = msg[1];
    uint8_t opcode = msg[2];

    if (len + 7 > MAX_MSG_SIZE)
    {
        PRINTF("Invalid length %d\r\n", len);
        continue;
    }

    // Step 2: Receive remaining payload + 2 status + 2 CRC
    usart_transfer_t rxPayload = {
        .data = &msg[3],
        .dataSize = len + 4
    };
    if (USART_TransferReceiveNonBlocking(UHF_USART, &g_uartHandle, &rxPayload, NULL) != kStatus_Success ||
        xSemaphoreTake(uhf_rx_done_sema, pdMS_TO_TICKS(200)) != pdTRUE)
    {
        continue;
    }

    uint8_t fullLen = len + 7;

    // Step 3: CRC check
    uint16_t crc_calc = calculateCRC(&msg[1], fullLen - 3);
    uint16_t crc_recv = (msg[fullLen - 2] << 8) | msg[fullLen - 1];
    if (crc_calc != crc_recv)
    {
        PRINTF("Bad CRC (calc=0x%04X, recv=0x%04X)\r\n", crc_calc, crc_recv);
        continue;
    }

    // Step 4: Process tag
    if (opcode == TMR_SR_OPCODE_READ_TAG_ID_MULTIPLE)
    {
        if (fullLen < 32) // Minimum size for valid tag packet
        {
            PRINTF("Too short for EPC\n");
            continue;
        }


        uint8_t epcLength = getTagEPCBytes();
        uint8_t epcBuf[epcLength];

        PRINTF("EPC [ ");
        for (uint8_t i = 0; i < epcLength; i++)
        {
            epcBuf[i] = msg[31 + i];
              if (epcBuf[i] < 0x10) PRINTF("0");
            PRINTF("%X ", epcBuf[i]);
        }
        PRINTF("]\r\n");
        add_to_list_if_not_present(epcBuf);
    }
    else if (opcode == RESPONSE_IS_KEEPALIVE)
    {
        PRINTF("[Keepalive]\r\n");
    }
    else
    {
        PRINTF("Unknown opcode: 0x%02X\r\n", opcode);
    }
}

stopReading();
PRINTF("[UHF] Multi-tag read complete.\r\n");