void Serial::Task(void* arg)
{
    Serial* pThis = reinterpret_cast<Serial*>(arg);

    while (pThis->m_EnableWorkingThreads)
    {
        timeval timeout;
        timeout.tv_sec = 0;
        timeout.tv_usec = 1000 * 100;

        FD_ZERO(&pThis->fds_read);
        FD_SET(pThis->m_SerialPortFileDescriptor, &pThis->fds_read);

        int result = select(pThis->m_SerialPortFileDescriptor + 1, &pThis->fds_read, &pThis->fds_write, nullptr, &timeout);
        printf("select result: %d\n", result);
        if (result <= 0)
        {
            printf("Select failed or timeout occured: %d\n", result);
            continue;
        }

        if (FD_ISSET(pThis->m_SerialPortFileDescriptor, &pThis->fds_write))
        {
            printf("FD_ISSET(pThis->m_SerialPortFileDescriptor, &pThis->fds_write)\n");
            while (!pThis->m_WriteBuffer.empty())
            {
                // printf("Write\n");
                BytesVector message = pThis->m_WriteBuffer.pop_front();

                int bytesWritten = write(pThis->m_SerialPortFileDescriptor, message.data(), message.size());
                if (bytesWritten < 0 || bytesWritten != message.size())
                {
                    // TODO: Debug log
                    printf("========== WRITE FAILED ==========\n");
                }
            }
        }

        if (FD_ISSET(pThis->m_SerialPortFileDescriptor, &pThis->fds_read))
        {
            printf("FD_ISSET(pThis->m_SerialPortFileDescriptor, &pThis->fds_write)\n");
            int bytesRead = read(pThis->m_SerialPortFileDescriptor, pThis->m_BytesChunk.data(), pThis->m_BytesChunk.size());
            printf("BytesRead:%d\n", bytesRead);
            if (bytesRead <= 0)
            {
                printf("Read Failed: %d\n", result);
                // TODO: Read failed
                continue;
            }

            // If received less than MINIMAL_BYTES_CHUNK bytes, resize buffer.
            if (bytesRead < MINIMAL_BYTES_CHUNK && bytesRead > 0)
            {
                pThis->m_BytesChunk.resize(bytesRead);
            }

            std::cout << std::endl;
            for (auto x: pThis->m_BytesChunk)
            {
                printf("%02x ", x);
            }

            pThis->m_ReadBuffer.push_back(std::move(pThis->m_BytesChunk));
            pThis->m_BytesChunk.resize(MINIMAL_BYTES_CHUNK);
        }
    }
}

void Serial::Send(BytesVector&& payload)
{
    // bool isWriteBufferEmpty = m_WriteBuffer.empty();

    m_WriteBuffer.push_back(std::move(payload));

    // If write buffer was not empty that means asynchronous write process
    // is in progress and calling Write() method is unnecessary.
    // if (isWriteBufferEmpty)
    // {
        Write();
    // }
}

void Serial::Write()
{
    FD_ZERO(&fds_write);
    FD_SET(m_SerialPortFileDescriptor, &fds_write);
}