Sophiar2/src/sensor/ndi/ndi_interface.cpp

#include "core/basic_obj_types.hpp"
#include "core/small_obj.hpp"
#include "utility/config_utility.hpp"
#include "utility/coro_worker.hpp"
#include "utility/coro_worker_helper_func.hpp"
#include "utility/debug_utility.hpp"
#include "utility/name_translator.hpp"
#include "utility/variable_helper.hpp"
#include "utility/versatile_buffer2.hpp"
#include "utility/string_map.hpp"
#include "third_party/static_block.hpp"

#include <boost/algorithm/hex.hpp>
#include <boost/asio/experimental/awaitable_operators.hpp>
#include <boost/asio/experimental/channel.hpp>
#include <boost/asio/ip/address.hpp>
#include <boost/asio/ip/tcp.hpp>
#include <boost/asio/buffers_iterator.hpp>
#include <boost/asio/co_spawn.hpp>
#include <boost/asio/detached.hpp>
#include <boost/asio/read_until.hpp>
#include <boost/asio/serial_port.hpp>
#include <boost/asio/streambuf.hpp>
#include <boost/asio/use_awaitable.hpp>
#include <boost/crc.hpp>
#include <boost/iostreams/device/mapped_file.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/smart_ptr/scoped_ptr.hpp>
#include <boost/system/error_code.hpp>

#ifdef BOOST_OS_WINDOWS_AVAILABLE

#include <winbase.h> // for sending serial break

#endif // BOOST_OS_WINDOWS_AVAILABLE

#include <fmt/format.h>
#include <spdlog/spdlog.h>

#include <chrono>
#include <functional>
#include <vector>
#include <unordered_map>

DEFAULT_TRISTATE_OBJ_DEF(ndi_interface)

namespace sophiar {

    using boost::asio::async_read;
    using boost::asio::async_read_until;
    using boost::asio::async_write;
    using boost::asio::awaitable;
    using boost::asio::basic_streambuf;
    using boost::asio::buffer;
    using boost::asio::buffers_begin;
    using boost::asio::buffers_end;
    using boost::asio::co_spawn;
    using boost::asio::detached;
    using boost::asio::experimental::channel;
    using boost::algorithm::hex;
    using boost::algorithm::unhex;
    using boost::asio::serial_port;
    using boost::asio::use_awaitable;
    using boost::iostreams::mapped_file;
    using boost::lexical_cast;
    using boost::scoped_ptr;
    using boost::system::error_code;

    using namespace boost::asio::experimental::awaitable_operators;
    using namespace boost::asio::ip;
    using namespace std::string_view_literals;

    enum class ndi_address_type {
        ethernet,
        serial,
    };

    name_translator<ndi_address_type> ndi_address_type_translator;

    static_block {
        ndi_address_type_translator.register_item("ethernet", ndi_address_type::ethernet);
        ndi_address_type_translator.register_item("serial", ndi_address_type::serial);
    }

    struct ndi_interface::impl {

        using tool_info_index_type = uint8_t;
        using port_handle_type = uint8_t;
        static constexpr port_handle_type invalid_port_handle = ~(port_handle_type) 0;

        struct tool_info {
            port_handle_type port_handle = invalid_port_handle; // assigned by NDI
            variable_index_type transform_var_index = invalid_variable_index;
            variable_index_type rms_var_index = invalid_variable_index;
            uint32_t last_frame_number = 0;
            std::string rom_path;
        };

        using tool_info_pool_type = std::vector<tool_info>;
        using port_handle_map_type = std::unordered_map<port_handle_type, tool_info_index_type>;

        tool_info_pool_type tool_info_pool;
        port_handle_map_type port_handle_map;

        static constexpr auto default_tracking_interval = std::chrono::milliseconds(17); // 60Hz
        static constexpr uint16_t default_ndi_port = 8765;
        static constexpr auto ndi_endian = boost::endian::order::little;

        static constexpr uint16_t binary_reply_header_magic = 0xA5C4;
        static constexpr uint16_t extended_binary_reply_header_magic = 0xA5C8;
        static constexpr uint16_t streaming_header_magic = 0xB5D4;

        using crc_checker_type = boost::crc_16_type;

        ndi_interface *q_this = nullptr;

        using reply_queue_type = channel<void(error_code, dynamic_memory::pointer)>;
        using reply_queue_ptr_type = scoped_ptr<reply_queue_type>;
        reply_queue_ptr_type command_reply_queue;

        using reply_callback_type = std::function<void(dynamic_memory::pointer &&)>;
        string_map<reply_callback_type> stream_reply_callback_pool;

        ndi_address_type address_type;

        // If address type is ethernet
        using ip_address_type = boost::asio::ip::address;
        ip_address_type ndi_ip;
        uint16_t ndi_port = default_ndi_port;
        scoped_ptr<tcp::socket> ndi_tcp_socket;

        // If address type is serial
        std::string com_port_name;
        scoped_ptr<serial_port> ndi_com_socket;

        int ndi_api_major_version = 0;
        bool accept_unreliable_transform = true;
        bool prefer_stream_tracking = true;
        bool using_stream_tracking = false;

        coro_worker::pointer receive_reply_worker;
        coro_worker::pointer tracking_tools_worker;

        void load_init_config(const nlohmann::json &config) {
            // load connection info
            address_type = ndi_address_type_translator.translate(LOAD_STRING_ITEM("address_type"));
            if (address_type == ndi_address_type::ethernet) {
                ndi_ip = make_address(LOAD_STRING_ITEM("ip"));
                if (config.contains("tcp_port")) {
                    ndi_port = LOAD_UINT_ITEM("tcp_port");
                } else { // use default value
                    ndi_port = default_ndi_port;
                }
            } else if (address_type == ndi_address_type::serial) {
                com_port_name = LOAD_STRING_ITEM("com_port");
            }

            // load tool info
            assert(tool_info_pool.empty());
            ENSURE_ARRAY("tool_list")
            for (auto &tool_config: config["tool_list"]) {
                auto rom_path = LOAD_STRING_ITEM2(tool_config, "rom_path");
                assert(tool_config.contains("outputs"));
                auto &output_config = tool_config["outputs"];
                auto transform_var_index = TRY_LOAD_VARIABLE_INDEX2(output_config, transform_obj, "transform");
                auto rms_var_index = TRY_LOAD_VARIABLE_INDEX2(output_config, double_obj, "marker_uncertainty");
                tool_info_pool.push_back(
                        {
                                .port_handle = invalid_port_handle,
                                .transform_var_index = transform_var_index,
                                .rms_var_index = rms_var_index,
                                .rom_path = std::move(rom_path)
                        });
            }
        }

        void load_start_config(const nlohmann::json &config) {
            // P.55 Polaris Spectra-Vicra API Guide
            accept_unreliable_transform = TRY_LOAD_BOOL_ITEM("allow_unreliable", false);
            prefer_stream_tracking = TRY_LOAD_BOOL_ITEM("prefer_stream_tracking", true);
        }

        template<typename T>
        std::enable_if_t<std::is_arithmetic_v<T>, awaitable<T>>
        ndi_async_read_value() {
            if (address_type == ndi_address_type::serial) {
                return async_read_value<ndi_endian, T>(*ndi_com_socket);
            } else if (address_type == ndi_address_type::ethernet) {
                return async_read_value<ndi_endian, T>(*ndi_tcp_socket);
            }
            assert(false);
            __builtin_unreachable();
        }

        template<WriteableMemory MemoryType>
        auto ndi_async_fill_memory(MemoryType &mem) {
            if (address_type == ndi_address_type::serial) {
                return async_fill_memory_from(*ndi_com_socket, mem);
            } else if (address_type == ndi_address_type::ethernet) {
                return async_fill_memory_from(*ndi_tcp_socket, mem);
            }
            assert(false);
            __builtin_unreachable();
        }

        awaitable<dynamic_memory::pointer> read_binary_reply(uint16_t header_magic) {
            // read length and crc
            auto header_buf = static_memory<4>{};
            co_await ndi_async_fill_memory(header_buf);
            auto header_reader = versatile_reader<ndi_endian>(header_buf);
            uint16_t reply_length, header_crc16;
            header_reader >> reply_length >> header_crc16;

            // check header crc
            crc_checker_type crc_checker;
            uint16_t header_magic_ndi = header_magic;
            swap_net_loc_endian<ndi_endian>(header_magic_ndi);
            crc_checker.process_bytes(&header_magic_ndi, 2); // header magic
            crc_checker.process_bytes(header_buf.data(), 2); // reply size
            if (crc_checker.checksum() != header_crc16) {
                // TODO show log
            }

            // read reply content
            auto reply_buf = dynamic_memory::new_instance(reply_length + 2);
            co_await ndi_async_fill_memory(*reply_buf);

            // check reply crc
            crc_checker.reset();
            crc_checker.process_bytes(reply_buf->data(), reply_length);
            auto reply_crc = read_binary_value<ndi_endian, uint16_t>(reply_buf->data() + reply_length);
            if (crc_checker.checksum() != reply_crc) {
                // TODO show log
            }

            reply_buf->increase_size(-2); // strip the crc
            co_return std::move(reply_buf);
        }

        awaitable<dynamic_memory::pointer> read_extended_binary_reply() {
            auto reply_length = co_await ndi_async_read_value<uint32_t>();
            auto reply_buf = dynamic_memory::new_instance(reply_length);
            co_await ndi_async_fill_memory(*reply_buf);
            co_return std::move(reply_buf);
        }

        awaitable<void> read_and_handle_streaming_reply(uint16_t header_magic) {
            // read stream id
            auto stream_id_length = co_await ndi_async_read_value<uint16_t>();
            auto stream_id_buf = dynamic_memory::new_instance(stream_id_length + 2);
            co_await ndi_async_fill_memory(*stream_id_buf);

            // check header crc
            crc_checker_type crc_checker;
            uint16_t header_magic_ndi = header_magic;
            swap_net_loc_endian<ndi_endian>(header_magic_ndi);
            crc_checker.process_bytes(&header_magic_ndi, 2); // header magic
            uint16_t stream_id_length_ndi = stream_id_length;
            swap_net_loc_endian<ndi_endian>(stream_id_length_ndi);
            crc_checker.process_bytes(&stream_id_length_ndi, 2); // stream id length
            crc_checker.process_bytes(stream_id_buf->data(), stream_id_length); // stream id
            auto header_crc16 = read_binary_value<ndi_endian, uint16_t>(stream_id_buf->data() + stream_id_length);
            if (crc_checker.checksum() != header_crc16) {
                // TODO show log
            }

            // read the real content
            header_magic = co_await ndi_async_read_value<uint16_t>();
            dynamic_memory::pointer reply_buf;
            switch (header_magic) {
                case binary_reply_header_magic: {
                    reply_buf = co_await read_binary_reply(header_magic);
                    break;
                }
                case extended_binary_reply_header_magic: {
                    reply_buf = co_await read_extended_binary_reply();
                    break;
                }
                default: { // ascii reply
                    reply_buf = co_await read_ascii_reply(header_magic);
                    break;
                }
            }

            auto stream_id = std::string_view{stream_id_buf->data(), stream_id_length};
            assert(stream_reply_callback_pool.contains(stream_id));
            auto &callback = stream_reply_callback_pool.query(stream_id);
            callback(std::move(reply_buf));
            co_return;
        }

        // header_magic is also part of the reply
        awaitable<dynamic_memory::pointer> read_ascii_reply(uint16_t header_magic) {
            static constexpr char cr_ascii = '\r';

            using streambuf_type = basic_streambuf<DYNAMIC_ALLOCATOR(char) >;
            streambuf_type ascii_buf;

            // read the rest ascii reply
            if (address_type == ndi_address_type::serial) {
                co_await async_read_until(*ndi_com_socket, ascii_buf, cr_ascii, use_awaitable);
            } else if (address_type == ndi_address_type::ethernet) {
                co_await async_read_until(*ndi_tcp_socket, ascii_buf, cr_ascii, use_awaitable);
            }

            auto start_iter = buffers_begin(ascii_buf.data());
            auto end_iter = buffers_end(ascii_buf.data());

            // locate the cr character
            auto cr_iter = start_iter;
            while (cr_iter != end_iter && *cr_iter != cr_ascii)
                ++cr_iter;
            assert(cr_iter != end_iter);

            // get the crc value
            auto crc_iter = cr_iter - 4;
            uint16_t reply_crc16;
            try {
                unhex(crc_iter, cr_iter, &reply_crc16);
            } catch (boost::algorithm::non_hex_input &e) {
                // TODO show log
                co_return nullptr;
            }

            // create a string as reply
            auto reply_content_length = crc_iter - start_iter + 2;
            auto reply_buf = dynamic_memory::new_instance(reply_content_length);

            // insert the header_magic back to the head of the ascii reply
            write_binary_value<ndi_endian>(reply_buf->data(), header_magic);

            // insert the rest reply part
            std::copy(start_iter, crc_iter, reply_buf->data() + 2);

            // check crc value
            crc_checker_type crc_checker;
            crc_checker.process_bytes(reply_buf->data(), reply_content_length);
            if (crc_checker.checksum() != reply_crc16) {
                // TODO show log
            }

            co_return std::move(reply_buf);
        }

        // 读取并分发 "一份" 来自 NDI 的回复消息
        awaitable<bool> read_and_dispatch_reply() {
            auto header_magic = co_await ndi_async_read_value<uint16_t>();
            if (header_magic == streaming_header_magic) {
                co_await read_and_handle_streaming_reply(header_magic);
                co_return true;
            }

            dynamic_memory::pointer reply_buf;
            switch (header_magic) {
                case binary_reply_header_magic: {
                    reply_buf = co_await read_binary_reply(header_magic);
                    break;
                }
                case extended_binary_reply_header_magic: {
                    reply_buf = co_await read_extended_binary_reply();
                    break;
                }
                default: { // ascii reply
                    reply_buf = co_await read_ascii_reply(header_magic);
                    break;
                }
            }

            assert(reply_buf != nullptr);
            co_await command_reply_queue->async_send(error_code{}, std::move(reply_buf), use_awaitable);
            co_return true;
        }

        void start_receive_reply() {
            assert(receive_reply_worker == nullptr);
            auto worker_func = [this]() -> awaitable<bool> {
                co_await read_and_dispatch_reply();
                co_return true;
            };
            auto error_handler = [](std::exception &e) {
                // TODO show error
            };
            auto noexcept_worker_func = make_noexcept_func(
                    [this]() { return read_and_dispatch_reply(); },
                    std::move(error_handler));
            auto exit_func = reset_on_exit_func(q_this);
            receive_reply_worker = make_infinite_coro_worker(std::move(noexcept_worker_func),
                                                             std::move(exit_func));
            receive_reply_worker->run();
        }

        auto send_command(std::string_view cmd) {
            if (address_type == ndi_address_type::serial) {
                assert(ndi_com_socket->is_open());
                return async_write(*ndi_com_socket, buffer(cmd.data(), cmd.size()), use_awaitable);
            } else if (address_type == ndi_address_type::ethernet) {
                assert(ndi_tcp_socket->is_open());
                return async_write(*ndi_tcp_socket, buffer(cmd.data(), cmd.size()), use_awaitable);
            }
            assert(false);
            __builtin_unreachable();
        }

        template<typename T, typename OutputIterator>
        std::enable_if_t<std::is_integral_v<T>, OutputIterator>
        static write_number_as_hex(T val, OutputIterator out) {
            swap_net_loc_endian<boost::endian::order::big>(val);
            auto val_ptr = reinterpret_cast<uint8_t *>(&val);
            auto old_out = out;
            out = hex(val_ptr, val_ptr + sizeof(T), out);
            assert(out - old_out == sizeof(T) << 1);
            return out;
        }

        template<typename T, typename InputIterator>
        std::enable_if_t<std::is_integral_v<T>, T>
        static read_number_as_hex(InputIterator in) {
            T val;
            auto ret_ptr = unhex(in, in + (sizeof(T) << 1), &val);
            assert(ret_ptr - &val == 1);
            return val;
        }

        static int get_error_warning_code(std::string_view reply_str) {
            static constexpr auto error_prefix = "ERROR"sv;
            static constexpr auto warning_prefix = "WARNING"sv;

            try {
                if (reply_str.starts_with(error_prefix)) { // error
                    auto error_code = read_number_as_hex<uint8_t>(reply_str.data() + error_prefix.size());
                    return -error_code;
                } else if (reply_str.starts_with(warning_prefix)) { // warning
                    auto warning_code = read_number_as_hex<uint8_t>(reply_str.data() + warning_prefix.size());
                    return warning_code;
                }
            } catch (boost::bad_lexical_cast &e) {
                // TODO show log
                auto fatal_error_code = std::numeric_limits<int16_t>::min();
                return fatal_error_code;
            }

            return 0; // no error
        }

        awaitable<bool> simple_cmd_helper(std::string_view cmd) {
            co_await send_command(cmd);
            auto reply = co_await command_reply_queue->async_receive(use_awaitable);
            auto reply_str = reply->as_string_view();
            auto reply_code = get_error_warning_code(reply_str);
            if (reply_code < 0) { // error
                // TODO show error
                co_return false;
            } else if (reply_code > 0) { // warning
                // TODO show warning
            } else { // normal
                assert(reply_str == "OKAY");
            }
            co_return true;
        }

        // 对串口进行配置
        awaitable<bool> setup_com_socket() {
            assert(address_type == ndi_address_type::serial);
            assert(ndi_com_socket->is_open());

            // load default config
            ndi_com_socket->set_option(serial_port::baud_rate(9600));
            ndi_com_socket->set_option(serial_port::character_size(8));
            ndi_com_socket->set_option(serial_port::parity(serial_port::parity::none));
            ndi_com_socket->set_option(serial_port::stop_bits(serial_port::stop_bits::one));
            ndi_com_socket->set_option(serial_port::flow_control(serial_port::flow_control::none));

#ifdef BOOST_OS_WINDOWS_AVAILABLE // boost asio cannot send serial break under windows

            using namespace std::chrono_literals;
            auto handle = ndi_com_socket->native_handle();
            ENSURE_CO(SetCommBreak(handle));
            co_await coro_sleep(250ms); // hold serial break for 250ms
            ENSURE_CO(ClearCommBreak(handle));

#else // BOOST_OS_WINDOWS_AVAILABLE

            ndi_com_socket->send_break(); // cause the system to reset

#endif // BOOST_OS_WINDOWS_AVAILABLE

            // wait the RESET to be sent from ndi
            auto reset_reply = co_await command_reply_queue->async_receive(use_awaitable);
            auto reset_reply_str = reset_reply->as_string_view();
            auto reply_code = get_error_warning_code(reset_reply_str);
            if (reply_code < 0) {
                // TODO show log
                co_return false;
            }
            assert(reset_reply_str == "RESET");

            // change the serial config of NDI
            CO_ENSURE(simple_cmd_helper("COMM 70001\r"sv))

            // load advanced config
            ndi_com_socket->set_option(serial_port::baud_rate(19200));
            ndi_com_socket->set_option(serial_port::flow_control(serial_port::flow_control::hardware));

            co_return true;
        }

        awaitable<int> get_api_major_version() {
            co_await send_command("APIREV \r"sv);
            auto reply = co_await command_reply_queue->async_receive(use_awaitable);
            auto reply_code = get_error_warning_code(reply->as_string_view());
            if (reply_code < 0) {
                assert(false);
                co_return 0;
            }
            SPDLOG_INFO("NDI api version is {}.", reply->as_string_view());
            auto major_version = lexical_cast<int>(reply->data() + 2, 3); // Like G.001.004A0C0
            co_return major_version;
        }

        awaitable<bool> initialize_ndi() {
            return simple_cmd_helper("INIT \r"sv);
        }

        awaitable<port_handle_type> request_port_handle() {
            co_await send_command("PHRQ *********100**\r"sv);
            auto reply = co_await command_reply_queue->async_receive(use_awaitable);
            auto reply_code = get_error_warning_code(reply->as_string_view());
            if (reply_code < 0) {
                // TODO show log
                co_return -1;
            }
            auto port_handle = read_number_as_hex<port_handle_type>(reply->data());
            co_return port_handle;
        }

        awaitable<bool> upload_rom(port_handle_type port_handle, const std::string &rom_path) {
            static constexpr auto cmd_prefix = "PVWR "sv;
            static constexpr auto chunk_size = 64;
            static constexpr auto cmd_length = cmd_prefix.size()
                                               + 2 // port handle
                                               + 4 // start address
                                               + chunk_size * 2 // tool definition
                                               + 1; // cr

            // setup cmd prefix
            auto cmd_buf = static_memory<cmd_length>{};
            cmd_prefix.copy(cmd_buf.data(), cmd_prefix.size());
            auto cur_buf = cmd_buf.data() + cmd_prefix.size();

            // setup port handle
            assert(is_valid_id(port_handle));
            cur_buf = write_number_as_hex(port_handle, cur_buf);

            auto address_buf = cur_buf;
            auto data_buf = address_buf + 4;
            auto cr_buf = data_buf + chunk_size * 2;
            *cr_buf = '\r';
            assert(cr_buf + 1 == cmd_buf.data() + cmd_length);

            auto rom_file = mapped_file();
            try {
                rom_file.open(rom_path, boost::iostreams::mapped_file::readonly);
                assert(rom_file.is_open());
            } catch (std::exception &e) {
                // TODO show log
                co_return false;
            }
            auto file_data = rom_file.const_data();
            size_t cur_pos = 0, end_pos = rom_file.size();
            while (cur_pos != end_pos) {
                // write the address
                using real_address_type = uint16_t;
                assert(cur_pos < std::numeric_limits<real_address_type>::max());
                auto cur_address = static_cast<real_address_type >(cur_pos);
                write_number_as_hex(cur_address, address_buf);

                // write cur chunk
                auto chunk_end_pos = std::min(cur_pos + chunk_size, end_pos);
                auto ret_buf = hex(file_data + cur_pos, file_data + chunk_end_pos, data_buf);

                // padding
                if (ret_buf != cr_buf) {
                    assert(chunk_end_pos - cur_pos < chunk_size);
                    while (ret_buf != cr_buf) {
                        *ret_buf++ = '0';
                    }
                }

                // send command
                CO_ENSURE(simple_cmd_helper(cmd_buf.as_string_view()))

                cur_pos = chunk_end_pos;
            }

            co_return true;
        }

        awaitable<bool> initialize_port_handle(port_handle_type port_handle) {
            static constexpr auto pinit_cmd_template = "PINIT 00\r"sv;
            static constexpr auto pinit_cmd_length = pinit_cmd_template.size();
            static constexpr ptrdiff_t port_handle_offset = 6;
            auto pinit_cmd_buf = static_memory<pinit_cmd_length>();

            // fill the template
            assert(is_valid_id(port_handle));
            pinit_cmd_template.copy(pinit_cmd_buf.data(), pinit_cmd_length);
            write_number_as_hex(port_handle, pinit_cmd_buf.data() + port_handle_offset);

            auto ok = co_await simple_cmd_helper(pinit_cmd_buf.as_string_view());
            co_return ok;
        }

        awaitable<bool> enable_port_handle(port_handle_type port_handle) {
            static constexpr auto pena_cmd_template = "PENA 00D\r"sv;
            static constexpr auto pena_cmd_length = pena_cmd_template.size();
            static constexpr ptrdiff_t port_handle_offset = 5;
            auto pena_cmd_buf = static_memory<pena_cmd_length>();

            // fill the template
            assert(is_valid_id(port_handle));
            pena_cmd_template.copy(pena_cmd_buf.data(), pena_cmd_length);
            write_number_as_hex(port_handle, pena_cmd_buf.data() + port_handle_offset);

            auto ok = co_await simple_cmd_helper(pena_cmd_buf.as_string_view());
            co_return ok;
        }

        awaitable<bool> config_tools() {
            assert(port_handle_map.empty());
            for (size_t index = 0; index < tool_info_pool.size(); ++index) {
                auto &info = tool_info_pool[index];
                auto port_handle = co_await request_port_handle();
                if (!is_valid_id(port_handle)) {
                    // TODO show log
                    co_return false;
                }
                info.port_handle = port_handle;
                port_handle_map[port_handle] = index;

                // config port handle
                assert(!info.rom_path.empty());
                CO_ENSURE(upload_rom(port_handle, info.rom_path))
                CO_ENSURE(initialize_port_handle(port_handle))
                CO_ENSURE(enable_port_handle(port_handle))
            }
            co_return true;
        }

        awaitable<bool> start_ndi_tracking() {
            return simple_cmd_helper("TSTART 80\r"sv);
        }

        awaitable<bool> stop_ndi_tracking() {
            return simple_cmd_helper("TSTOP \r"sv);
        }

        void handle_bx_reply(dynamic_memory::pointer &&reply_buf) {
            auto reader = versatile_reader<ndi_endian>(*reply_buf);
            auto num_handle = reader.read_value<uint8_t>();
            auto ts = current_timestamp();
            for (uint8_t i = 0; i < num_handle; ++i) {
                auto port_handle = reader.read_value<port_handle_type>();
                assert(port_handle_map.contains(port_handle));
                auto tool_index = port_handle_map[port_handle];
                assert(tool_index < tool_info_pool.size());
                auto &info = tool_info_pool[tool_index];

                auto handle_status = reader.read_value<uint8_t>();
                uint32_t port_status, frame_number;
                if (handle_status != 0x01) { // not invalid
                    reader >> port_status >> frame_number;
                    try_update_variable<transform_obj>(info.transform_var_index, nullptr, ts);
                    try_update_variable<double_obj>(info.rms_var_index, nullptr, ts);
                    continue;
                }

                // normal reply
                float q0, qx, qy, qz, tx, ty, tz, rms;
                static_assert(sizeof(float) == 4);
                reader >> q0 >> qx >> qy >> qz;
                reader >> tx >> ty >> tz >> rms;
                reader >> port_status >> frame_number;

                // check frame number
                if (frame_number == info.last_frame_number) continue;
                info.last_frame_number = frame_number;

                auto new_trans = transform_obj::new_instance();
                new_trans->value = Eigen::Translation3d(tx, ty, tz) * Eigen::Quaterniond(q0, qx, qy, qz);
                try_update_variable<transform_obj>(info.transform_var_index, std::move(new_trans), ts);
                try_update_variable_value<double_obj>(info.rms_var_index, rms, ts);

                // TODO show log for abnormal value in port_status
            }

            auto system_status = reader.read_value<uint16_t>();
            // TODO show log for abnormal value in system_status
        }

        // consider unreliable transform
        std::string_view get_suitable_bx() const {
            static constexpr auto bx_cmd_reliable = "BX 0001\r"sv;
            static constexpr auto bx_cmd_unreliable = "BX 0801\r"sv;
            return accept_unreliable_transform ? bx_cmd_unreliable : bx_cmd_reliable;
        }

        // 通过 BX 指令追踪并更新 transform
        awaitable<bool> track_once_bx() {
            co_await send_command(get_suitable_bx());
            auto reply = co_await command_reply_queue->async_receive(use_awaitable);
            auto error_code = get_error_warning_code(reply->as_string_view());
            if (error_code != 0) {
                assert(error_code < 0);
                // TODO show error log
                co_return false; // ascii reply indicate error
            }

            handle_bx_reply(std::move(reply));
            co_return true;
        }

        awaitable<bool> start_stream(std::string_view cmd) {
            auto stream_cmd = fmt::format("STREAM {}\r", cmd);
            auto ok = co_await simple_cmd_helper(stream_cmd);
            co_return ok;
        }

        awaitable<bool> stop_stream(std::string_view cmd) {
            auto ustream_cmd = fmt::format("USTREAM {}\r", cmd);
            auto ok = co_await simple_cmd_helper(ustream_cmd);
            co_return ok;
        }

        awaitable<bool> on_init_impl() {
            // establish connection
            if (address_type == ndi_address_type::ethernet) {
                ndi_tcp_socket.reset(new tcp::socket(*global_context));
                co_await ndi_tcp_socket->async_connect({ndi_ip, ndi_port}, use_awaitable);
                // decrease delay
                tcp::no_delay no_delay_option(true);
                ndi_tcp_socket->set_option(no_delay_option);
            } else if (address_type == ndi_address_type::serial) {
                ndi_com_socket.reset(new serial_port(*global_context));
                ndi_com_socket->open(com_port_name); // will throw error if failed
                assert(ndi_com_socket->is_open());
            }

            // start the receiving reply coroutine
            assert(command_reply_queue == nullptr);
            command_reply_queue.reset(new reply_queue_type{*global_context});
            start_receive_reply();

            // extra config is needed for serial port
            if (address_type == ndi_address_type::serial) {
                CO_ENSURE(setup_com_socket())
            }

            ndi_api_major_version = co_await get_api_major_version();

            // TODO increase Param.Tracking.IlluminatorRate to 60 for Polaris Spectra device

            CO_ENSURE(initialize_ndi())
            CO_ENSURE(config_tools())

            co_return true;
        }

        awaitable<bool> on_start_impl() {
            CO_ENSURE(start_ndi_tracking())
            if (prefer_stream_tracking && ndi_api_major_version >= 3) { // use streaming
                using_stream_tracking = true;
                auto stream_cmd = get_suitable_bx();
                reply_callback_type callback = [this](dynamic_memory::pointer &&mem) {
                    handle_bx_reply(std::move(mem));
                };
                auto stream_cmd_id = stream_cmd.substr(0, stream_cmd.size() - 1); // strip the '\r'
                stream_reply_callback_pool.insert(stream_cmd_id, std::move(callback));
                CO_ENSURE(start_stream(stream_cmd))
            } else { // use interval streaming
                assert(tracking_tools_worker == nullptr);
                using_stream_tracking = false;
                auto error_handler = [](std::exception &e) {
                    // TODO show error
                };
                auto noexcept_worker_func = make_noexcept_func(
                        [this]() { return track_once_bx(); },
                        std::move(error_handler));
                tracking_tools_worker = make_interval_coro_worker(default_tracking_interval,
                                                                  std::move(noexcept_worker_func));
                tracking_tools_worker->run();
            }
            co_return true;
        }

        awaitable<void> on_stop_impl() {
            // stop tracking and streaming
            try {
                if (using_stream_tracking) {
                    co_await stop_stream(get_suitable_bx());
                    stream_reply_callback_pool.clear();
                } else {
                    SAFE_RESET_WORKER(tracking_tools_worker)
                }
                co_await stop_ndi_tracking();
            } catch (std::exception &e) {
                // do nothing
            }
            co_return;
        }

        awaitable<void> on_reset_impl() {
            // stop receiving reply
            SAFE_RESET_WORKER(receive_reply_worker)

            // delete connections
            ndi_tcp_socket.reset(nullptr);
            ndi_com_socket.reset(nullptr);

            // clear tool info
            tool_info_pool.clear();
            port_handle_map.clear();
            co_return;
        }

    };

    ndi_interface::ndi_interface()
            : pimpl(std::make_unique<impl>()) {
        pimpl->q_this = this;
    }

    boost::asio::awaitable<bool> ndi_interface::on_init(const nlohmann::json &config) noexcept {
        pimpl->load_init_config(config);
        try {
            CO_ENSURE(pimpl->on_init_impl())
        } catch (std::exception &e) {
            SPDLOG_ERROR("Failed to initialize ndi: {}", e.what());
            co_return false;
        }
        CO_ENSURE(tristate_obj::on_init(config))
        co_return true;
    }

    boost::asio::awaitable<bool> ndi_interface::on_start(const nlohmann::json &config) noexcept {
        pimpl->load_start_config(config);
        try {
            CO_ENSURE(pimpl->on_start_impl())
        } catch (std::exception &e) {
            // TODO show log
            co_return false;
        }
        CO_ENSURE(tristate_obj::on_start(config))
        co_return true;
    }

    boost::asio::awaitable<void> ndi_interface::on_stop() noexcept {
        co_await pimpl->on_stop_impl();
        co_await tristate_obj::on_stop();
        co_return;
    }

    boost::asio::awaitable<void> ndi_interface::on_reset() noexcept {
        co_await pimpl->on_reset_impl();
        co_await tristate_obj::on_reset();
        co_return;
    }

    ndi_interface::~ndi_interface() = default;

}