增加了一些功能

CMakeLists.txt

@@ -10,8 +10,26 @@ include_directories(./src)
 find_package(Boost REQUIRED)
 include_directories(${Boost_INCLUDE_DIRS})
 
+find_package(fmt REQUIRED)
+list(APPEND EXTRA_LIBS fmt::fmt)
+
+find_package(spdlog REQUIRED)
+list(APPEND EXTRA_LIBS spdlog::spdlog)
+
+find_package(Eigen3 REQUIRED)
+list(APPEND EXTRA_LIBS Eigen3::Eigen)
+
 file(GLOB_RECURSE SRC_FILES ./src/*.cpp)
 add_executable(${PROJECT_NAME} ${SRC_FILES})
 
+IF (WIN32)
+    list(APPEND EXTRA_LIBS ws2_32 winmm)
+ENDIF()
+
+target_link_libraries(${PROJECT_NAME} ${EXTRA_LIBS})
+
+file(GLOB_RECURSE EXTERN_DEF_FILES ./src/extern_defs/*.cpp)
+file(GLOB_RECURSE CORE_IMPL_FILES ./src/core/*.cpp)
+
 add_subdirectory(benchmark)
 add_subdirectory(tests)

benchmark/CMakeLists.txt

@@ -1,4 +1,7 @@
 find_package(benchmark REQUIRED)
 
-add_executable(benchmark_small_obj small_obj/main.cpp)
-target_link_libraries(benchmark_small_obj benchmark::benchmark)
+add_executable(benchmark_small_obj core/small_obj.cpp ${EXTERN_DEF_FILES})
+target_link_libraries(benchmark_small_obj benchmark::benchmark ${EXTRA_LIBS})
+
+add_executable(benchmark_datanode_base core/datanode_base.cpp ${EXTERN_DEF_FILES} ${CORE_IMPL_FILES})
+target_link_libraries(benchmark_datanode_base benchmark::benchmark ${EXTRA_LIBS})

benchmark/core/datanode_base.cpp

@@ -0,0 +1,68 @@
+#include "core/datanode_base.h"
+
+#include <boost/asio/awaitable.hpp>
+#include <boost/asio/co_spawn.hpp>
+#include <boost/asio/detached.hpp>
+
+#include <cassert>
+
+#include <benchmark/benchmark.h>
+
+using boost::asio::awaitable;
+using boost::asio::co_spawn;
+using boost::asio::detached;
+
+using namespace sophiar;
+
+static void BM_timestamp(benchmark::State &state) {
+    for (auto _: state) {
+        benchmark::DoNotOptimize(sophiar::current_timestamp());
+    }
+}
+
+BENCHMARK(BM_timestamp);
+
+static void BM_datanode_chain(benchmark::State &state) {
+
+    struct test_type : public datanode_base<high_freq_tag> {
+        uint64_t cnt = 0;
+
+        awaitable<bool> exec() {
+            auto input_data = get_input_data(0);
+            auto output_data = data_packet::new_instance();
+            output_data->copy_content(*input_data);
+            (*output_data)[0] += 1.0;
+            output_data->timestamp = current_timestamp();
+            cnt = (*output_data)[0];
+            set_output_data(0, output_data);
+            co_return true;
+        }
+    };
+
+    co_spawn(high_freq_context, [&]() -> awaitable<void> {
+        auto total = state.range(0);
+        auto pool = new test_type[total];
+        for (size_t i = 0; i < total; ++i) {
+            auto &node = pool[i];
+            co_await node.init();
+            node.set_trigger_mode(test_type::trigger_mode_type::INPUT);
+            node.set_trigger_input_mask(0b1);
+            if (i != 0) {
+                test_type::connect(pool[i - 1], 0, node, 0);
+            }
+            co_await node.start();
+        }
+
+        auto test_data = test_type::data_packet::new_instance();
+        test_data->refresh();
+        for (auto _: state) {
+            pool[0].update_input_data(0, test_data);
+            assert(pool[total - 1].cnt == total);
+        }
+    }, detached);
+    high_freq_context.run();
+}
+
+BENCHMARK(BM_datanode_chain)->RangeMultiplier(10)->Range(1, 1e6);
+
+BENCHMARK_MAIN();

src/core/datanode_base.cpp

@@ -0,0 +1,362 @@
+#include "datanode_base.h"
+
+#include "utility/bit_operations.hpp"
+#include "utility/coro_signal.hpp"
+#include "utility/statistic_timer.hpp"
+
+#include <boost/asio/co_spawn.hpp>
+#include <boost/asio/detached.hpp>
+#include <boost/asio/experimental/awaitable_operators.hpp>
+#include <boost/asio/high_resolution_timer.hpp>
+#include <boost/asio/this_coro.hpp>
+#include <boost/asio/use_awaitable.hpp>
+#include <boost/intrusive/list.hpp>
+
+#include <fmt/format.h>
+
+#include <cassert>
+#include <functional>
+#include <string>
+
+namespace sophiar {
+
+    using boost::asio::awaitable;
+    using boost::asio::co_spawn;
+    using boost::asio::detached;
+    using boost::asio::use_awaitable;
+
+    using namespace boost::asio::experimental::awaitable_operators;
+
+    template<typename FreqTag>
+    struct datanode_base<FreqTag>::impl {
+
+        datanode_base *q_this = nullptr;
+
+        using connection_hook = boost::intrusive::list_base_hook<
+                boost::intrusive::link_mode<
+                        boost::intrusive::auto_unlink> >;
+        struct connection_node_type : public connection_hook {
+            datanode_base *datanode;
+            uint8_t channel_index;
+        };
+
+        using connection_list_type = boost::intrusive::list<
+                connection_node_type, boost::intrusive::constant_time_size<false> >;
+
+        using state_type = typename tristate_obj<FreqTag>::state_type;
+
+        typename data_packet::pointer input_data[MAX_CHANNEL_CNT];
+        typename data_packet::pointer output_data[MAX_CHANNEL_CNT];
+
+        connection_node_type channel_input_nodes[MAX_CHANNEL_CNT];
+        connection_list_type channel_output_list[MAX_CHANNEL_CNT];
+        channel_mask_type input_update_mask = 0;
+        channel_mask_type output_updated_mask = 0;
+
+        trigger_mode_type trigger_mode = trigger_mode_type::MANUAL;
+        channel_mask_type trigger_input_mask = 0;
+        std::chrono::microseconds trigger_interval{0};
+        std::chrono::microseconds minimal_exec_interval{0};
+        boost::asio::high_resolution_timer trigger_timer;
+
+        exec_state_type exec_state = exec_state_type::IDLE;
+        bool last_exec_success = false;
+        timestamp_type last_exec_ts = 0;
+
+        bool enable_stimer = false; // 是否启用统计时钟
+
+        using trigger_stimer_type = statistic_timer<statistic_timer_single_shot_tag>;
+        using exec_stimer_type = statistic_timer<statistic_timer_start_stop_tag>;
+        trigger_stimer_type trigger_stimer;
+        exec_stimer_type exec_stimer;
+
+        coro_signal exec_cancel_signal;
+        coro_signal run_finished_signal;
+        bool exec_block = false;
+
+        impl()
+                : trigger_timer(datanode_base::get_context()),
+                  exec_cancel_signal(datanode_base::get_context()),
+                  run_finished_signal(datanode_base::get_context()) {}
+
+        bool check_is_running() const {
+            return q_this->get_state() == state_type::RUNNING;
+        }
+
+        bool check_input_trigger_condition() const {
+            return (input_update_mask & trigger_input_mask) == trigger_input_mask;
+        }
+
+        void commit_output() {
+            for (uint8_t i = 0; i < MAX_CHANNEL_CNT; ++i) {
+                if (test_bit(output_updated_mask, i)) {
+                    for (const auto &node: channel_output_list[i]) {
+                        node.datanode->update_input_data(node.channel_index, output_data[i]);
+                    }
+                }
+            }
+            output_updated_mask = 0;
+        }
+
+        awaitable<bool> exec_impl(timestamp_type current_ts = current_timestamp()) {
+            exec_state = exec_state_type::RUNNING;
+            last_exec_ts = current_ts;
+
+            if (enable_stimer) {
+                trigger_stimer.start(current_ts);
+                exec_stimer.start(current_ts);
+            }
+            auto ret_val = co_await q_this->exec();
+            if (enable_stimer) {
+                exec_stimer.stop();
+            }
+            last_exec_success = ret_val;
+
+            input_update_mask = 0;
+            commit_output();
+            exec_state = exec_state_type::IDLE;
+
+            co_return ret_val;
+        }
+
+        awaitable<void> periodic_run() {
+            auto exec_cancel_token = exec_cancel_signal.new_token();
+            for (;;) {
+                co_await exec_impl();
+                assert(exec_state == exec_state_type::IDLE);
+
+                // check if cancelled
+                if (exec_cancel_signal.try_wait(exec_cancel_token)) break;
+
+                // schedule next run
+                auto current_ts = current_timestamp();
+                auto time_left = last_exec_ts + trigger_interval.count() - current_ts;
+                if (time_left <= 0) {
+                    // TODO show warning, requested frequency too high
+                } else {
+                    exec_state = exec_state_type::PENDING;
+                    trigger_timer.expires_at(get_time_from_timestamp(last_exec_ts) + trigger_interval);
+                    auto await_result = co_await (trigger_timer.async_wait(use_awaitable) ||
+                                                  exec_cancel_signal.coro_wait(exec_cancel_token));
+                    if (await_result.index() == 1) { // cancelled
+                        exec_state = exec_state_type::IDLE;
+                        break;
+                    }
+                }
+            }
+
+            run_finished_signal.try_notify_all();
+            co_return;
+        }
+
+        awaitable<bool> on_start() {
+            trigger_stimer.reset();
+            exec_stimer.reset();
+            exec_block = false;
+            if (trigger_mode == trigger_mode_type::PERIODIC) {
+                if (trigger_interval.count() < 0) {
+                    co_return false;
+                } else if (trigger_interval.count() == 0) {
+                    // TODO show warning, running without interval
+                }
+                co_spawn(q_this->get_context(), periodic_run(), detached);
+            }
+            co_return true;
+        }
+
+        awaitable<void> on_stop() {
+            exec_block = true;
+            if (trigger_mode == trigger_mode_type::PERIODIC ||
+                exec_state == exec_state_type::PENDING) {
+                exec_cancel_signal.try_notify_all();
+            }
+            if (exec_state == exec_state_type::RUNNING) {
+                co_await run_finished_signal.coro_wait();
+            }
+            co_return;
+        }
+
+        void update_input_data(uint8_t channel_index,
+                               const typename data_packet::pointer &data) {
+            assert(channel_index < MAX_CHANNEL_CNT);
+            input_data[channel_index] = data;
+            set_bit(input_update_mask, channel_index);
+            if (check_is_running() && trigger_mode == trigger_mode_type::INPUT) {
+                if (check_input_trigger_condition()) {
+                    co_spawn(q_this->get_context(), trigger(), detached);
+                }
+            }
+        }
+
+        boost::asio::awaitable<bool> trigger() {
+            if (!check_is_running()) co_return false; // only trigger when running
+            if (exec_block || exec_state != exec_state_type::IDLE) co_return false;
+            assert(trigger_mode != trigger_mode_type::PERIODIC);
+            auto current_ts = current_timestamp();
+            auto time_left = last_exec_ts + minimal_exec_interval.count() - current_ts;
+            if (time_left > 0) {
+                exec_state = exec_state_type::PENDING;
+                trigger_timer.expires_at(get_time_from_timestamp(last_exec_ts) + minimal_exec_interval);
+                auto await_result = co_await (trigger_timer.async_wait(use_awaitable) ||
+                                              exec_cancel_signal.coro_wait());
+                if (await_result.index() == 1) { // cancelled
+                    exec_state = exec_state_type::IDLE;
+                    run_finished_signal.try_notify_all();
+                    co_return false;
+                } else {
+                    current_ts = current_timestamp();
+                }
+            }
+
+            auto ret_val = co_await exec_impl(current_ts);
+            run_finished_signal.try_notify_all();
+            co_return ret_val;
+        }
+
+        bool cancel_pending() {
+            if (exec_state != exec_state_type::PENDING) return false;
+            if (trigger_mode == trigger_mode_type::PERIODIC) return false;
+            exec_cancel_signal.try_notify_all();
+            return true;
+        }
+
+    };
+
+    template<typename FreqTag>
+    awaitable<bool> datanode_base<FreqTag>::on_start() {
+        return pimpl->on_start();
+    }
+
+    template<typename FreqTag>
+    awaitable<void> datanode_base<FreqTag>::on_stop() {
+        return pimpl->on_stop();
+    }
+
+    template<typename FreqTag>
+    datanode_base<FreqTag>::datanode_base()
+            : pimpl(std::make_unique<impl>()) {
+        pimpl->q_this = this;
+        for (uint8_t i = 0; i < MAX_CHANNEL_CNT; ++i) {
+            pimpl->channel_input_nodes[i].datanode = this;
+            pimpl->channel_input_nodes[i].channel_index = i;
+        }
+    }
+
+    template<typename FreqTag>
+    void datanode_base<FreqTag>::update_input_data(uint8_t channel_index,
+                                                   const typename data_packet::pointer &data) {
+        pimpl->update_input_data(channel_index, data);
+    }
+
+    template<typename FreqTag>
+    boost::asio::awaitable<bool> datanode_base<FreqTag>::trigger() {
+        return pimpl->trigger();
+    }
+
+    template<typename FreqTag>
+    bool datanode_base<FreqTag>::cancel_pending() {
+        return pimpl->cancel_pending();
+    }
+
+    template<typename FreqTag>
+    bool datanode_base<FreqTag>::set_trigger_mode(trigger_mode_type mode) {
+        if (pimpl->check_is_running()) return false;
+        pimpl->trigger_mode = mode;
+        return true;
+    }
+
+    template<typename FreqTag>
+    typename datanode_base<FreqTag>::data_packet::pointer
+    datanode_base<FreqTag>::get_input_data(uint8_t channel_index) {
+        assert(channel_index < MAX_CHANNEL_CNT);
+        return pimpl->input_data[channel_index];
+    }
+
+    template<typename FreqTag>
+    void datanode_base<FreqTag>::set_output_data(uint8_t channel_index,
+                                                 const typename data_packet::pointer &data) {
+        assert(channel_index < MAX_CHANNEL_CNT);
+        pimpl->output_data[channel_index] = data;
+        set_bit(pimpl->output_updated_mask, channel_index);
+    }
+
+    template<typename FreqTag>
+    typename datanode_base<FreqTag>::exec_state_type
+    datanode_base<FreqTag>::get_exec_state() const {
+        return pimpl->exec_state;
+    }
+
+    template<typename FreqTag>
+    timestamp_type datanode_base<FreqTag>::get_last_exec_ts() const {
+        return pimpl->last_exec_ts;
+    }
+
+    template<typename FreqTag>
+    bool datanode_base<FreqTag>::is_last_exec_success() const {
+        return pimpl->last_exec_success;
+    }
+
+    template<typename FreqTag>
+    std::string datanode_base<FreqTag>::get_statistic_info_as_string() const {
+        return fmt::format("\nTrigger timer: {}\nExec timer: {}",
+                           pimpl->trigger_stimer.to_string(), pimpl->exec_stimer.to_string());
+    }
+
+    template<typename FreqTag>
+    void datanode_base<FreqTag>::connect(datanode_base<FreqTag> &sender, uint8_t sender_channel_index,
+                                         datanode_base<FreqTag> &receiver, uint8_t receiver_channel_index) {
+        assert(sender_channel_index < MAX_CHANNEL_CNT);
+        assert(receiver_channel_index < MAX_CHANNEL_CNT);
+        auto &receiver_channel_node = receiver.pimpl->channel_input_nodes[receiver_channel_index];
+        if (receiver_channel_node.is_linked()) {
+            receiver_channel_node.unlink();
+        }
+        sender.pimpl->channel_output_list[sender_channel_index].push_back(receiver_channel_node);
+    }
+
+    template<typename FreqTag>
+    void datanode_base<FreqTag>::disconnect(uint8_t input_channel_index) {
+        assert(input_channel_index < MAX_CHANNEL_CNT);
+        auto &input_node = pimpl->channel_input_nodes[input_channel_index];
+        if (input_node.is_linked()) {
+            input_node.unlink();
+        }
+    }
+
+    template<typename FreqTag>
+    void datanode_base<FreqTag>::set_trigger_input_mask(channel_mask_type mask) {
+        pimpl->trigger_input_mask = mask;
+    }
+
+    template<typename FreqTag>
+    void datanode_base<FreqTag>::set_trigger_interval(std::chrono::milliseconds interval) {
+        pimpl->trigger_interval = interval;
+    }
+
+    template<typename FreqTag>
+    void datanode_base<FreqTag>::set_minimal_exec_interval(std::chrono::milliseconds interval) {
+        pimpl->minimal_exec_interval = interval;
+    }
+
+    template<typename FreqTag>
+    void datanode_base<FreqTag>::enable_statistic_timer() {
+        pimpl->enable_stimer = true;
+    }
+
+    template<typename FreqTag>
+    void datanode_base<FreqTag>::disable_statistic_timer() {
+        pimpl->enable_stimer = false;
+    }
+
+    template<typename FreqTag>
+    datanode_base<FreqTag>::~datanode_base() = default;
+
+    // 显式实例化
+
+    template
+    class datanode_base<high_freq_tag>;
+
+    template
+    class datanode_base<low_freq_tag>;
+
+}

src/core/datanode_base.h

@@ -0,0 +1,128 @@
+#ifndef SOPHIAR2_DATANODE_BASE_H
+#define SOPHIAR2_DATANODE_BASE_H
+
+#include "core/small_obj.hpp"
+#include "core/timestamp_helper.hpp"
+#include "core/tristate_obj.h"
+
+#include <chrono>
+#include <cstring>
+#include <memory>
+
+namespace sophiar {
+
+    // TODO 修改内容
+    struct data_packet_content {
+        static constexpr size_t DATA_PACKET_LENGTH = 14;
+
+        uint32_t user_data;
+        timestamp_type timestamp;
+        double data[DATA_PACKET_LENGTH];
+
+        auto &operator[](size_t index) {
+            assert(index < DATA_PACKET_LENGTH);
+            return data[index];
+        }
+
+        const auto &operator[](size_t index) const {
+            assert(index < DATA_PACKET_LENGTH);
+            return data[index];
+        }
+
+        void refresh() {
+            user_data = 0;
+            timestamp = current_timestamp();
+            std::memset(data, 0x00, sizeof(double) * DATA_PACKET_LENGTH);
+        }
+    };
+
+    template<typename FreqTag>
+    class datanode_base : public tristate_obj<FreqTag> {
+    public:
+
+        struct data_packet : public data_packet_content,
+                             public small_obj<data_packet> {
+
+            void copy_content(const data_packet_content &content) {
+                std::memcpy(dynamic_cast<data_packet_content *>(this),
+                            &content,
+                            sizeof(data_packet_content));
+            }
+
+        };
+
+        enum class exec_state_type {
+            IDLE,
+            PENDING,
+            RUNNING
+        };
+
+        enum class trigger_mode_type {
+            MANUAL,
+            INPUT,
+            PERIODIC
+        };
+
+        static constexpr uint8_t MAX_CHANNEL_CNT = 8;
+        using channel_mask_type = uint8_t;
+        static_assert(sizeof(channel_mask_type) * CHAR_BIT >= MAX_CHANNEL_CNT);
+
+        datanode_base();
+
+        ~datanode_base() override;
+
+        bool set_trigger_mode(trigger_mode_type mode);
+
+        void set_trigger_input_mask(channel_mask_type mask);
+
+        void set_trigger_interval(std::chrono::milliseconds interval);
+
+        void set_minimal_exec_interval(std::chrono::milliseconds interval);
+
+        exec_state_type get_exec_state() const;
+
+        timestamp_type get_last_exec_ts() const;
+
+        bool is_last_exec_success() const;
+
+        void enable_statistic_timer();
+
+        void disable_statistic_timer();
+
+        std::string get_statistic_info_as_string() const;
+
+        void update_input_data(uint8_t channel_index,
+                               const typename data_packet::pointer &data);
+
+        boost::asio::awaitable<bool> trigger();
+
+        bool cancel_pending();
+
+        static void connect(datanode_base &sender, uint8_t sender_channel_index,
+                            datanode_base &receiver, uint8_t receiver_channel_index);
+
+        void disconnect(uint8_t input_channel_index);
+
+    protected:
+
+        typename data_packet::pointer get_input_data(uint8_t channel_index);
+
+        void set_output_data(uint8_t channel_index,
+                             const typename data_packet::pointer &data);
+
+        virtual boost::asio::awaitable<bool> exec() = 0;
+
+        boost::asio::awaitable<bool> on_start() override;
+
+        boost::asio::awaitable<void> on_stop() override;
+
+    private:
+
+        struct impl;
+        std::unique_ptr<impl> pimpl;
+
+    };
+
+}
+
+#endif //SOPHIAR2_DATANODE_BASE_H

src/core/datanode_base.hpp

@@ -1,32 +0,0 @@
-#ifndef SOPHIAR2_DATANODE_BASE_HPP
-#define SOPHIAR2_DATANODE_BASE_HPP
-
-#include "core/small_obj.hpp"
-#include "core/timestamp_helper.hpp"
-
-#include <boost/logic/tribool.hpp>
-
-namespace sophiar {
-
-//    template<typename T>
-//    concept DataNode_Impl = requires(T a) {
-//        { a.exec() } -> std::convertible_to<boost::tribool>;
-//    };
-
-    struct data_packet : small_obj<data_packet> {
-
-        static const size_t DATA_PACKET_LENGTH = 14;
-
-        uint32_t user_data;
-        uint64_t timestamp;
-        double data[DATA_PACKET_LENGTH];
-
-        void make_timestamp_current() {
-            timestamp = current_timestamp();
-        }
-
-    };
-
-}
-
-#endif //SOPHIAR2_DATANODE_BASE_HPP

src/core/global_io_context.hpp

@@ -0,0 +1,31 @@
+#ifndef SOPHIAR2_GLOBAL_IO_CONTEXT_HPP
+#define SOPHIAR2_GLOBAL_IO_CONTEXT_HPP
+
+#include <boost/asio/io_context.hpp>
+
+namespace sophiar {
+
+    extern boost::asio::io_context high_freq_context;
+    extern boost::asio::io_context low_freq_context;
+
+    struct high_freq_tag;
+    struct low_freq_tag;
+
+    template<typename FreqTag>
+    struct use_context {
+        static auto &get_context();
+    };
+
+    template<>
+    inline auto &use_context<high_freq_tag>::get_context() {
+        return high_freq_context;
+    }
+
+    template<>
+    inline auto &use_context<low_freq_tag>::get_context() {
+        return low_freq_context;
+    }
+
+}
+
+#endif //SOPHIAR2_GLOBAL_IO_CONTEXT_HPP

src/core/small_obj.hpp

@@ -1,6 +1,7 @@
 #ifndef SOPHIAR2_SMALL_OBJ_H
 #define SOPHIAR2_SMALL_OBJ_H
 
+#include <boost/core/noncopyable.hpp>
 #include <boost/pool/object_pool.hpp>
 #include <boost/smart_ptr/intrusive_ptr.hpp>
 
@@ -10,7 +11,7 @@
 namespace sophiar {
 
     template<typename DeriveT>
-    struct small_obj_ref_counter {
+    struct small_obj_ref_counter : private boost::noncopyable {
         uint32_t ref_count = 0;
     };
 

src/core/timestamp_helper.hpp

@@ -1,19 +1,27 @@
 #ifndef SOPHIAR2_TIMESTAMP_HELPER_HPP
 #define SOPHIAR2_TIMESTAMP_HELPER_HPP
 
-#include <boost/date_time/posix_time/posix_time_types.hpp>
+#include <chrono>
 
 namespace sophiar {
 
-    auto get_local_time() {
-        return boost::posix_time::microsec_clock::local_time();
+    inline auto get_local_time() {
+        return std::chrono::high_resolution_clock::now();
     }
 
-    static const auto program_start_time = get_local_time();
+    using local_time_type = decltype(get_local_time());
 
-    auto current_timestamp() {
+    extern local_time_type program_start_time;
+
+    inline auto current_timestamp() {
         auto time_diff = get_local_time() - program_start_time;
-        return time_diff.total_microseconds();
+        return std::chrono::duration_cast<std::chrono::microseconds>(time_diff).count();
+    }
+
+    using timestamp_type = decltype(current_timestamp());
+
+    inline auto get_time_from_timestamp(timestamp_type ts) {
+        return program_start_time + std::chrono::microseconds(ts);
     }
 
 }

src/core/tristate_obj.cpp

@@ -0,0 +1,169 @@
+#include "tristate_obj.h"
+
+#include "utility/coro_signal.hpp"
+
+#include <boost/asio/experimental/awaitable_operators.hpp>
+
+#include <cassert>
+
+namespace sophiar {
+
+    using boost::asio::awaitable;
+
+    using namespace boost::asio::experimental::awaitable_operators;
+
+    template<typename FreqTag>
+    struct tristate_obj<FreqTag>::impl {
+
+        tristate_obj *q_this = nullptr;
+
+        state_type state = state_type::INITIAL;
+        coro_signal init_cancel_signal;
+        coro_signal start_cancel_signal;
+        coro_signal init_finished_signal;
+        coro_signal start_finished_signal;
+        coro_signal stop_finished_signal;
+        coro_signal reset_finished_signal;
+
+        impl()
+                : init_cancel_signal(tristate_obj::get_context()),
+                  start_cancel_signal(tristate_obj::get_context()),
+                  init_finished_signal(tristate_obj::get_context()),
+                  start_finished_signal(tristate_obj::get_context()),
+                  stop_finished_signal(tristate_obj::get_context()),
+                  reset_finished_signal(tristate_obj::get_context()) {}
+
+        awaitable<bool> init() {
+            if (state == state_type::INITIALIZING) {
+                auto result = co_await (init_finished_signal.coro_wait() ||
+                                        reset_finished_signal.coro_wait());
+                co_return result.index() == 0;
+            }
+            if (state == state_type::RESETTING) {
+                co_await reset_finished_signal.coro_wait();
+                co_return false;
+            }
+            if (state != state_type::INITIAL) co_return true; // >= PENDING
+            state = state_type::INITIALIZING;
+            auto result = co_await (q_this->on_init() || init_cancel_signal.coro_wait());
+            if (result.index() == 0 && std::get<0>(result) == true) { // succeeded
+                state = state_type::PENDING;
+                init_finished_signal.try_notify_all();
+                co_return true;
+            } else { // failed
+                co_await q_this->on_reset();
+                state = state_type::INITIAL;
+                reset_finished_signal.try_notify_all();
+                co_return false;
+            }
+        }
+
+        awaitable<bool> start() {
+            if (state == state_type::STARTING) {
+                auto result = co_await (start_finished_signal.coro_wait() ||
+                                        stop_finished_signal.coro_wait());
+                co_return result.index() == 0;
+            }
+            if (state == state_type::STOPPING) {
+                co_await stop_finished_signal.coro_wait();
+                co_return false;
+            }
+            if (state == state_type::RUNNING) co_return true;
+            if (state != state_type::PENDING) co_return false; // INITIAL, INITIALIZING, RESETTING
+            state = state_type::STARTING;
+            auto result = co_await (q_this->on_start() || start_cancel_signal.coro_wait());
+            if (result.index() == 0 && std::get<0>(result) == true) { // succeeded
+                state = state_type::RUNNING;
+                start_finished_signal.try_notify_all();
+                co_return true;
+            } else { // failed
+                co_await q_this->on_stop();
+                state = state_type::PENDING;
+                stop_finished_signal.try_notify_all();
+                co_return false;
+            }
+        }
+
+        awaitable<void> stop() {
+            if (state == state_type::RUNNING) {
+                state = state_type::STOPPING;
+                co_await q_this->on_stop();
+                state = state_type::PENDING;
+                stop_finished_signal.try_notify_all();
+            } else if (state == state_type::STOPPING) {
+                co_await stop_finished_signal.coro_wait();
+            } else if (state == state_type::STARTING) {
+                start_cancel_signal.try_notify_all();
+                co_await stop_finished_signal.coro_wait();
+            }
+            co_return;
+        }
+
+        awaitable<void> reset() {
+            // force reset
+            if (state == state_type::RUNNING ||
+                state == state_type::STOPPING ||
+                state == state_type::STARTING) {
+                co_await stop();
+                assert(state == state_type::PENDING);
+            }
+
+            if (state == state_type::PENDING) {
+                state = state_type::RESETTING;
+                co_await q_this->on_reset();
+                state = state_type::INITIAL;
+                reset_finished_signal.try_notify_all();
+            } else if (state == state_type::RESETTING) {
+                co_await reset_finished_signal.coro_wait();
+            } else if (state == state_type::INITIALIZING) {
+                init_cancel_signal.try_notify_all();
+                co_await reset_finished_signal.coro_wait();
+            }
+            co_return;
+        }
+
+    };
+
+    template<typename FreqTag>
+    typename tristate_obj<FreqTag>::state_type tristate_obj<FreqTag>::get_state() const {
+        return pimpl->state;
+    }
+
+    template<typename FreqTag>
+    awaitable<bool> tristate_obj<FreqTag>::init() {
+        return pimpl->init();
+    }
+
+    template<typename FreqTag>
+    awaitable<bool> tristate_obj<FreqTag>::start() {
+        return pimpl->start();;
+    }
+
+    template<typename FreqTag>
+    awaitable<void> tristate_obj<FreqTag>::stop() {
+        return pimpl->stop();
+    }
+
+    template<typename FreqTag>
+    awaitable<void> tristate_obj<FreqTag>::reset() {
+        return pimpl->reset();
+    }
+
+    template<typename FreqTag>
+    tristate_obj<FreqTag>::tristate_obj()
+            : pimpl(std::make_unique<impl>()) {
+        pimpl->q_this = this;
+    }
+
+    template<typename FreqTag>
+    tristate_obj<FreqTag>::~tristate_obj() = default;
+
+    // 显式实例化
+
+    template
+    class tristate_obj<high_freq_tag>;
+
+    template
+    class tristate_obj<low_freq_tag>;
+
+}

src/core/tristate_obj.h

@@ -0,0 +1,61 @@
+#ifndef SOPHIAR2_TRISTATE_OBJ_H
+#define SOPHIAR2_TRISTATE_OBJ_H
+
+#include "core/global_io_context.hpp"
+
+#include <coroutine>
+#include <memory>
+
+#include <boost/asio/awaitable.hpp>
+#include <boost/core/noncopyable.hpp>
+
+namespace sophiar {
+
+    template<typename FreqTag>
+    class tristate_obj : public use_context<FreqTag>, private boost::noncopyable {
+    public:
+
+        enum class state_type {
+            INITIAL,
+            INITIALIZING,
+            RESETTING,
+            PENDING,
+            STARTING,
+            STOPPING,
+            RUNNING,
+        };
+
+        tristate_obj();
+
+        virtual ~tristate_obj();
+
+        boost::asio::awaitable<bool> init();
+
+        boost::asio::awaitable<bool> start();
+
+        boost::asio::awaitable<void> stop();
+
+        boost::asio::awaitable<void> reset();
+
+        state_type get_state() const;
+
+    protected:
+
+        virtual boost::asio::awaitable<bool> on_init() { co_return true; } // TODO add no_throw
+
+        virtual boost::asio::awaitable<bool> on_start() { co_return true; }
+
+        virtual boost::asio::awaitable<void> on_stop() { co_return; }
+
+        virtual boost::asio::awaitable<void> on_reset() { co_return; }
+
+    private:
+
+        struct impl;
+        std::unique_ptr<impl> pimpl;
+
+    };
+
+}
+
+#endif //SOPHIAR2_TRISTATE_OBJ_H

src/core/tristate_obj.hpp

@@ -1,186 +0,0 @@
-#ifndef SOPHIAR2_TRISTATE_OBJ_HPP
-#define SOPHIAR2_TRISTATE_OBJ_HPP
-
-#include <boost/logic/tribool.hpp>
-
-namespace sophiar {
-
-    using state_type = uint8_t;
-    static const state_type ST_INITIAL = 0;
-    static const state_type ST_INITIALIZING = 1;
-    static const state_type ST_PENDING = 2;
-    static const state_type ST_STARING = 3;
-    static const state_type ST_RUNNING = 4;
-
-//    template<typename T>
-//    concept Tristate_Impl = requires(T a) {
-//        { a.on_init() } -> std::convertible_to<boost::tribool>;
-//        { a.on_start() } -> std::convertible_to<boost::tribool>;
-//        a.on_stop();
-//        a.on_reset();
-//        a.after_init();
-//        a.after_start();
-//        a.after_stop();
-//        a.after_reset();
-//        a.after_state_change(state_type{});
-//    };
-
-    template<typename DeriveT>
-    class tristate_obj {
-    public:
-
-        state_type state = ST_INITIAL;
-
-        boost::tribool init() {
-            if (state == ST_INITIAL) {
-                auto ret_val = call_on_init();
-                if (ret_val == false) {
-                    call_on_reset();
-                    return false;
-                }
-                if (ret_val == true) {
-                    state = ST_PENDING;
-                    call_after_init();
-                } else {
-                    state = ST_INITIALIZING;
-                }
-                return ret_val;
-            }
-            if (state == ST_INITIALIZING) {
-                return boost::indeterminate;
-            }
-            return true;
-        }
-
-        boost::tribool start() {
-            if (state < ST_PENDING) {
-                return false;
-            }
-            if (state == ST_PENDING) {
-                auto ret_val = call_on_start();
-                if (ret_val == false) {
-                    call_on_stop();
-                    return false;
-                }
-                if (ret_val == true) {
-                    state = ST_RUNNING;
-                    call_after_start();
-                } else {
-                    state = ST_STARING;
-                }
-                return ret_val;
-            }
-            if (state == ST_STARING) {
-                return boost::indeterminate;
-            }
-            return true;
-        }
-
-        void stop() {
-            if (state < ST_STARING) return;
-            call_on_stop();
-            auto old_state = state;
-            state = ST_PENDING;
-            if (old_state == ST_RUNNING) {
-                call_after_stop();
-            }
-        }
-
-        void reset() {
-            if (state == ST_INITIAL) return;
-            if (state > ST_PENDING) {
-                stop();
-            }
-            call_on_reset();
-            auto old_state = state;
-            state = ST_INITIAL;
-            if (old_state == ST_PENDING) {
-                call_after_reset();
-            }
-        }
-
-    protected:
-
-        void init_finished(bool flag = true) {
-            if (state != ST_INITIALIZING) return;
-            if (flag) {
-                state = ST_PENDING;
-                call_after_init();
-            } else {
-                call_on_reset();
-                state = ST_INITIAL;
-            }
-        }
-
-        void start_finished(bool flag = true) {
-            if (state != ST_STARING) return;
-            if (flag) {
-                state = ST_RUNNING;
-                call_after_start();
-            } else {
-                call_on_stop();
-                state = ST_PENDING;
-            }
-        }
-
-    private:
-
-        auto get_real_ptr() {
-            return static_cast<DeriveT *>(this);
-        }
-
-#define CHECK_AND_CALL_ON_BOOL(cmd) \
-        boost::tribool call_on_##cmd() { \
-            if constexpr(requires(DeriveT a) { \
-                a.on_##cmd(); \
-            }) { \
-                return get_real_ptr()->on_##cmd(); \
-            } \
-            return true; \
-        }
-
-        CHECK_AND_CALL_ON_BOOL(init)
-        CHECK_AND_CALL_ON_BOOL(start)
-
-#undef CHECK_AND_CALL_ON_BOOL
-
-#define CHECK_AND_CALL_ON_VOID(cmd) \
-        void call_on_##cmd() { \
-            if constexpr(requires(DeriveT a) { \
-                a.on_##cmd(); \
-            }) { \
-                get_real_ptr()->on_##cmd(); \
-            } \
-        }
-
-        CHECK_AND_CALL_ON_VOID(stop)
-        CHECK_AND_CALL_ON_VOID(reset)
-
-#undef CHECK_AND_CALL_ON_VOID
-
-#define CHECK_AND_CALL_AFTER(cmd, old_state) \
-        void call_after_##cmd() { \
-            if constexpr(requires(DeriveT a) { \
-                a.after_##cmd(); \
-            }) { \
-                get_real_ptr()->after_##cmd(); \
-            } \
-            if constexpr(requires(DeriveT a) { \
-                a.after_state_change(state_type{}); \
-            }) { \
-                get_real_ptr()->after_state_change(old_state); \
-            } \
-        }
-
-        CHECK_AND_CALL_AFTER(init, ST_INITIAL)
-        CHECK_AND_CALL_AFTER(start, ST_PENDING)
-        CHECK_AND_CALL_AFTER(stop, ST_RUNNING)
-        CHECK_AND_CALL_AFTER(reset, ST_PENDING)
-
-#undef CHECK_AND_CALL_AFTER
-
-    };
-
-}
-
-#endif //SOPHIAR2_TRISTATE_OBJ_HPP

src/core/types/vectorx.hpp

@@ -0,0 +1,410 @@
+#ifndef SOPHIAR2_VECTORX_HTTP
+#define SOPHIAR2_VECTORX_HTTP
+
+#include "core/small_obj.hpp"
+#include "utility/versatile_buffer.hpp"
+
+#include <Eigen/Core>
+
+#include <cassert>
+#include <type_traits>
+#include <utility>
+
+namespace sophiar {
+
+    template<typename FloatType, size_t Length>
+    class vectorx;
+
+    template<typename FreqTag, typename FloatType, size_t Length>
+    class vectorx_obj;
+
+    template<typename FloatPointerType, size_t Length>
+    class vectorx_view {
+    public:
+
+        using FloatType = std::remove_pointer_t<FloatPointerType>;
+        using RealFloatType = std::remove_const_t<FloatType>;
+
+        static_assert(std::is_pointer_v<FloatPointerType>);
+        static_assert(std::is_arithmetic_v<FloatType>);
+
+        using eigen_row_vector_type = Eigen::Map<Eigen::Matrix<RealFloatType, 1, Length>>;
+        using eigen_const_row_vector_type = Eigen::Map<const Eigen::Matrix<RealFloatType, 1, Length>>;
+        using eigen_column_vector_type = Eigen::Map<Eigen::Matrix<RealFloatType, Length, 1>>;
+        using eigen_const_column_vector_type = Eigen::Map<const Eigen::Matrix<RealFloatType, Length, 1>>;
+
+        explicit vectorx_view(FloatPointerType other_data)
+                : data(other_data) {}
+
+        vectorx_view(vectorx<RealFloatType, Length> &other)
+                : data(other.data) {}
+
+        template<typename ThisFloatType = FloatType,
+                typename = std::enable_if_t<std::is_const_v<ThisFloatType>>>
+        vectorx_view(const vectorx<RealFloatType, Length> &other)
+                : data(other.data) {}
+
+        vectorx_view(const vectorx_view<RealFloatType *, Length> &other)
+                : data(other.data) {}
+
+        template<typename ThisFloatType = FloatType,
+                typename = std::enable_if_t<std::is_const_v<ThisFloatType>>>
+        vectorx_view(const vectorx_view<const RealFloatType *, Length> &other)
+                : data(other.data) {}
+
+        auto to_vectorx() const {
+            return vectorx<RealFloatType, Length>(*this);
+        }
+
+        template<typename FreqTag>
+        auto to_vectorx_obj() const {
+            return vectorx_obj<FreqTag, RealFloatType, Length>::new_instance(*this);
+        };
+
+        template<size_t Pos,
+                typename ThisFloatType = FloatType,
+                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>>
+        FloatType &at() {
+            static_assert(Pos < Length);
+            return data[Pos];
+        }
+
+        template<size_t Pos>
+        const FloatType &at() const {
+            static_assert(Pos < Length);
+            return data[Pos];
+        }
+
+        template<typename ThisFloatType = FloatType,
+                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>>
+        FloatType &at(size_t pos) {
+            assert(pos < Length);
+            return data[pos];
+        }
+
+        const FloatType &at(size_t pos) const {
+            assert(pos < Length);
+            return data[pos];
+        }
+
+        template<typename ThisFloatType = FloatType,
+                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>>
+        FloatType &operator[](size_t pos) {
+            return at(pos);
+        }
+
+        const FloatType &operator[](size_t pos) const {
+            return at(pos);
+        }
+
+        template<typename OtherFloatPointerType, size_t OtherLength,
+                typename = std::enable_if_t<OtherLength < Length>>
+        auto fill(const vectorx_view<OtherFloatPointerType, OtherLength> &other) {
+            copy_from_vector_view<OtherLength>(other);
+            return vectorx_view<FloatPointerType, Length - OtherLength>(data + OtherLength);
+        };
+
+        template<typename OtherFloatType, size_t OtherLength,
+                typename = std::enable_if_t<OtherLength < Length>>
+        auto fill(const vectorx<OtherFloatType, OtherLength> &other) {
+            return fill(other.get_view());
+        };
+
+        template<typename OtherFloatType, int OtherRows, int OtherColumns,
+                size_t CopyLength = OtherRows * OtherColumns,
+                typename = std::enable_if_t<OtherRows * OtherColumns < Length>>
+        auto fill(const Eigen::Matrix<OtherFloatType, OtherRows, OtherColumns> &other) {
+            copy_from_eigen_matrix<CopyLength>(other);
+            return vectorx_view<FloatPointerType, Length - CopyLength>(data + CopyLength);
+        }
+
+        template<typename OtherFloatType, size_t OtherLength,
+                typename = std::enable_if_t<OtherLength < Length>>
+        auto fill(const OtherFloatType(&list)[OtherLength]) {
+            copy_from_list<OtherLength>(list);
+            return vectorx_view<FloatPointerType, Length - OtherLength>(data + OtherLength);
+        };
+
+        template<typename OtherFloatPointerType, size_t OtherLength,
+                typename = std::enable_if_t<OtherLength == Length>>
+        void fill(const vectorx_view<OtherFloatPointerType, OtherLength> &other) {
+            copy_from_vector_view<OtherLength>(other);
+        };
+
+        template<typename OtherFloatType, size_t OtherLength,
+                typename = std::enable_if_t<OtherLength == Length>>
+        void fill(const vectorx<OtherFloatType, OtherLength> &other) {
+            fill(other.get_view());
+        };
+
+        template<typename OtherFloatType, int OtherRows, int OtherColumns,
+                typename = std::enable_if_t<OtherRows * OtherColumns == Length>>
+        void fill(const Eigen::Matrix<OtherFloatType, OtherRows, OtherColumns> &other) {
+            copy_from_eigen_matrix<OtherRows * OtherColumns>(other);
+        }
+
+        template<typename OtherFloatType, size_t OtherLength,
+                typename = std::enable_if_t<OtherLength == Length>>
+        void fill(const OtherFloatType(&list)[OtherLength]) {
+            copy_from_list<OtherLength>(list);
+        };
+
+        static constexpr uint16_t length() {
+            return sizeof(FloatType) * Length;
+        }
+
+        void write_to_buffer(versatile_buffer &buffer) const {
+            for (size_t i = 0; i < Length; ++i) {
+                buffer << data[i];
+            }
+        }
+
+        template<typename ThisFloatType = FloatType,
+                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>>
+        void fill_from_buffer(versatile_buffer &buffer) {
+            for (size_t i = 0; i < Length; ++i) {
+                buffer >> data[i];
+            }
+        }
+
+        template<typename ThisFloatType = FloatType,
+                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>>
+        auto as_eigen_row_view() {
+            return eigen_row_vector_type(data);
+        }
+
+        auto as_eigen_const_row_view() const {
+            return eigen_const_row_vector_type(data);
+        }
+
+        template<typename ThisFloatType = FloatType,
+                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>>
+        auto as_eigen_column_view() {
+            return eigen_column_vector_type(data);
+        }
+
+        auto as_eigen_const_column_view() const {
+            return eigen_const_column_vector_type(data);
+        }
+
+    private:
+        FloatPointerType data;
+
+        template<typename, size_t> friend
+        class vectorx_view;
+
+        template<typename, size_t> friend
+        class vectorx;
+
+        template<size_t CopyLength, typename OtherFloatPointerType, size_t OtherLength,
+                typename ThisFloatType = FloatType,
+                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>,
+                typename = std::enable_if_t<std::is_convertible_v<
+                        std::remove_pointer_t<OtherFloatPointerType>, RealFloatType>>>
+        void copy_from_vector_view(const vectorx_view<OtherFloatPointerType, OtherLength> &other) {
+            static_assert(CopyLength <= Length);
+            for (size_t i = 0; i < CopyLength; ++i) {
+                data[i] = other.data[i];
+            }
+        }
+
+        template<size_t CopyLength, typename OtherFloatType, size_t OtherLength,
+                typename ThisFloatType = FloatType,
+                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>,
+                typename = std::enable_if_t<std::is_convertible_v<OtherFloatType, RealFloatType>>>
+        void copy_from_list(const OtherFloatType(&list)[OtherLength]) {
+            static_assert(CopyLength <= Length);
+            for (size_t i = 0; i < CopyLength; ++i) {
+                data[i] = list[i];
+            }
+        }
+
+        template<size_t CopyLength, typename OtherFloatType, int OtherRows, int OtherColumns,
+                typename ThisFloatType = FloatType,
+                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>,
+                typename = std::enable_if_t<std::is_convertible_v<OtherFloatType, RealFloatType>>>
+        void copy_from_eigen_matrix(const Eigen::Matrix<OtherFloatType, OtherRows, OtherColumns> &other) {
+            static_assert(OtherRows == 1 || OtherColumns == 1);
+            static_assert(OtherRows * OtherColumns == Length);
+            static_assert(CopyLength <= Length);
+            for (size_t i = 0; i < CopyLength; ++i) {
+                data[i] = other(i);
+            }
+        }
+    };
+
+    template<typename FloatType, size_t Length>
+    class vectorx {
+    public:
+
+        static_assert(std::is_arithmetic_v<FloatType>);
+
+        vectorx() = default;
+
+        template<typename OtherFloatType>
+        explicit vectorx(const OtherFloatType(&list)[Length]) {
+            copy_from_list(list);
+        }
+
+        template<typename OtherFloatPointerType>
+        explicit vectorx(const vectorx_view<OtherFloatPointerType, Length> &other) {
+            copy_from_vector_view(other);
+        }
+
+        template<typename OtherFloatType>
+        explicit vectorx(const vectorx<OtherFloatType, Length> &other)
+                :vectorx(other.get_view()) {}
+
+        template<typename OtherFloatType, size_t OtherRows, size_t OtherColumns>
+        explicit vectorx(const Eigen::Matrix<OtherFloatType, OtherRows, OtherColumns> &other) {
+            copy_from_eigen_matrix(other);
+        }
+
+        template<typename OtherFloatType>
+        vectorx &operator=(const OtherFloatType(&list)[Length]) {
+            copy_from_list(list);
+            return *this;
+        }
+
+        template<typename OtherFloatType>
+        vectorx &operator=(const vectorx_view<OtherFloatType, Length> &other) {
+            copy_from_vector_view(other);
+            return *this;
+        }
+
+        template<typename OtherFloatType, int OtherRows, int OtherColumns>
+        vectorx &operator=(const Eigen::Matrix<OtherFloatType, OtherRows, OtherColumns> &other) {
+            copy_from_eigen_matrix(other);
+            return *this;
+        }
+
+        template<size_t Pos>
+        FloatType &at() {
+            static_assert(Pos < Length);
+            return data[Pos];
+        }
+
+        template<size_t Pos>
+        const FloatType &at() const {
+            static_assert(Pos < Length);
+            return data[Pos];
+        }
+
+        FloatType &at(size_t pos) {
+            assert(pos < Length);
+            return data[pos];
+        }
+
+        const FloatType &at(size_t pos) const {
+            assert(pos < Length);
+            return data[pos];
+        }
+
+        FloatType &operator[](size_t pos) {
+            return at(pos);
+        }
+
+        const FloatType &operator[](size_t pos) const {
+            return at(pos);
+        }
+
+        template<size_t StartPos = 0, size_t EndPos = Length>
+        auto get_view() {
+            static_assert(StartPos < EndPos && EndPos <= Length);
+            return vectorx_view<FloatType *, EndPos - StartPos>(data + StartPos);
+        };
+
+        template<size_t StartPos = 0, size_t EndPos = Length>
+        auto get_view() const {
+            static_assert(StartPos < EndPos && EndPos <= Length);
+            return vectorx_view<const FloatType *, EndPos - StartPos>(data + StartPos);
+        };
+
+        static constexpr uint16_t length() {
+            return sizeof(FloatType) * Length;
+        }
+
+        void write_to_buffer(versatile_buffer &buffer) const {
+            for (const auto &item: data) {
+                buffer << item;
+            }
+        }
+
+        void fill_from_buffer(versatile_buffer &buffer) {
+            for (auto &item: data) {
+                buffer >> item;
+            }
+        }
+
+    private:
+        FloatType data[Length];
+
+        template<typename, size_t> friend
+        class vectorx_view;
+
+        template<typename OtherFloatType,
+                typename = std::enable_if_t<std::is_convertible_v<OtherFloatType, FloatType>>>
+        void copy_from_list(const OtherFloatType(&list)[Length]) {
+            for (size_t i = 0; i < Length; ++i) {
+                data[i] = list[i];
+            }
+        }
+
+        template<typename OtherFloatType, int OtherRows, int OtherColumns,
+                typename = std::enable_if_t<std::is_convertible_v<OtherFloatType, FloatType>>>
+        void copy_from_eigen_matrix(const Eigen::Matrix<OtherFloatType, OtherRows, OtherColumns> &other) {
+            static_assert(OtherRows == 1 || OtherColumns == 1);
+            static_assert(OtherRows * OtherColumns == Length);
+            for (size_t i = 0; i < Length; ++i) {
+                data[i] = other(i);
+            }
+        }
+
+        template<typename OtherFloatPointerType,
+                typename = std::enable_if_t<std::is_convertible_v<
+                        std::remove_pointer_t<OtherFloatPointerType>, FloatType>>>
+        void copy_from_vector_view(const vectorx_view<OtherFloatPointerType, Length> &other) {
+            for (size_t i = 0; i < Length; ++i) {
+                data[i] = other.data[i];
+            }
+        }
+
+    };
+
+    template<typename FreqTag, typename FloatType, size_t Length>
+    class vectorx_obj : public vectorx<FloatType, Length>,
+                        public small_obj<vectorx_obj<FreqTag, FloatType, Length>> {
+    public:
+        vectorx_obj() = default;
+
+        template<typename OtherFloatType>
+        explicit vectorx_obj(const OtherFloatType(&list)[Length])
+                :vectorx<FloatType, Length>(list) {}
+
+        template<typename OtherFloatPointerType>
+        explicit vectorx_obj(const vectorx_view<OtherFloatPointerType, Length> &other)
+                :vectorx<FloatType, Length>(other) {}
+
+        template<typename OtherFloatType>
+        explicit vectorx_obj(const vectorx<OtherFloatType, Length> &other)
+                :vectorx<FloatType, Length>(other) {}
+
+        template<typename OtherFloatType, size_t OtherRows, size_t OtherColumns>
+        explicit vectorx_obj(const Eigen::Matrix<OtherFloatType, OtherRows, OtherColumns> &other)
+                :vectorx<FloatType, Length>(other) {}
+    };
+
+    using vector3d = vectorx<double, 3>;
+    using vector6d = vectorx<double, 6>;
+
+    using vector3d_cview = vectorx_view<const double *, 3>;
+    using vector6d_cview = vectorx_view<const double *, 6>;
+
+    template<typename FreqTag>
+    using vector3d_obj = vectorx_obj<FreqTag, double, 3>;
+    template<typename FreqTag>
+    using vector6d_obj = vectorx_obj<FreqTag, double, 6>;
+
+}
+
+#endif //SOPHIAR2_VECTORX_HTTP

src/extern_defs/core/global_io_context.cpp

@@ -0,0 +1,27 @@
+#include "core/global_io_context.hpp"
+#include "third_party/static_block.hpp"
+
+#include <boost/predef.h>
+
+#ifdef BOOST_OS_WINDOWS_AVAILABLE
+
+#include <timeapi.h>
+#include <cstdlib>
+
+#endif // BOOST_OS_WINDOWS_AVAILABLE
+
+namespace sophiar {
+
+    boost::asio::io_context high_freq_context;
+    boost::asio::io_context low_freq_context;
+
+#ifdef BOOST_OS_WINDOWS_AVAILABLE
+
+    static_block { // make windows timer more precise
+        timeBeginPeriod(1);
+        std::atexit([]() { timeEndPeriod(1); });
+    };
+
+#endif // BOOST_OS_WINDOWS_AVAILABLE
+
+}

src/extern_defs/core/timestamp_helper.cpp

@@ -0,0 +1,7 @@
+#include "core/timestamp_helper.hpp"
+
+namespace sophiar {
+
+    local_time_type program_start_time = get_local_time();
+
+}

src/main.cpp

@@ -1,18 +1,45 @@
+#include <coroutine>
 #include <iostream>
+#include <thread>
 
-#include "core/small_obj.hpp"
-#include "core/tristate_obj.hpp"
-#include "core/datanode_base.hpp"
+#include "robot/ur/ur_interface.h"
+#include "utility/debug_utility.hpp"
 
-struct test_impl : public sophiar::tristate_obj<test_impl> {
-};
+#include <boost/asio/co_spawn.hpp>
+#include <boost/asio/detached.hpp>
 
-void func(test_impl &obj) {
-    obj.start();
-}
+using boost::asio::co_spawn;
+using boost::asio::detached;
+
+using namespace sophiar;
+
+struct fake_state_listener : public ur_status_listener {
+    fake_state_listener()
+            : node(this) {}
+
+    void on_new_ur_status(const ur_status::pointer &status) override {
+        ++cnt;
+    }
+
+    size_t cnt = 0;
+    ur_status_signal_type::node_type node;
+};
 
 int main() {
-    test_impl a;
-    func(a);
-    std::cout << (int) a.state << std::endl;
+    ur_interface ur;
+    fake_state_listener listener;
+    ur.set_ur_ip(boost::asio::ip::make_address_v4("192.168.38.141"));
+    ur.add_status_listener(listener.node);
+    ur.set_report_frequency(125);
+    auto func = [&]() -> boost::asio::awaitable<void> {
+        co_await ur.init();
+        co_await ur.start();
+        co_await coro_sleep(std::chrono::seconds(1));
+        co_await ur.reset();
+        co_return;
+    };
+    co_spawn(ur.get_context(), func(), detached);
+    ur.get_context().run();
+    std::cout << listener.cnt << std::endl;
+    return 0;
 }

src/robot/ur/ur_interface.cpp

@@ -0,0 +1,565 @@
+#include "ur_interface.h"
+
+#include "utility/debug_utility.hpp"
+#include "utility/versatile_buffer.hpp"
+
+#include <boost/algorithm/string/join.hpp>
+#include <boost/asio/ip/tcp.hpp>
+#include <boost/asio/co_spawn.hpp>
+#include <boost/asio/detached.hpp>
+#include <boost/asio/use_awaitable.hpp>
+#include <boost/smart_ptr/scoped_ptr.hpp>
+
+#include <spdlog/spdlog.h>
+
+#include <algorithm>
+#include <cassert>
+#include <exception>
+#include <string>
+#include <vector>
+
+namespace sophiar {
+
+    using boost::asio::awaitable;
+    using boost::asio::co_spawn;
+    using boost::asio::detached;
+    using boost::asio::use_awaitable;
+    using boost::scoped_ptr;
+
+    using namespace boost::asio::ip;
+
+    struct ur_interface::impl {
+
+        // Header types
+        static constexpr uint8_t RTDE_REQUEST_PROTOCOL_VERSION = 86;
+        static constexpr uint8_t RTDE_GET_URCONTROL_VERSION = 118;
+        static constexpr uint8_t RTDE_TEXT_MESSAGE = 77;
+        static constexpr uint8_t RTDE_DATA_PACKAGE = 85;
+        static constexpr uint8_t RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS = 79;
+        static constexpr uint8_t RTDE_CONTROL_PACKAGE_SETUP_INPUTS = 73;
+        static constexpr uint8_t RTDE_CONTROL_PACKAGE_START = 83;
+        static constexpr uint8_t RTDE_CONTROL_PACKAGE_PAUSE = 80;
+
+        struct rtde_packet_header {
+            uint16_t packet_size;
+            uint8_t packet_type;
+
+            static constexpr uint16_t length() {
+                return sizeof(uint16_t) + sizeof(uint8_t);
+            }
+
+            void write_to_buffer(versatile_buffer &buffer) const {
+                buffer << packet_size << packet_type;
+            }
+        };
+
+        struct rtde_request_protocol_version_content {
+            uint16_t protocol_version;
+
+            static constexpr uint16_t length() {
+                return sizeof(uint16_t);
+            }
+
+            void write_to_buffer(versatile_buffer &buffer) const {
+                buffer << protocol_version;
+            }
+        };
+
+        struct rtde_request_protocol_version_result_content {
+            uint8_t accepted;
+
+            void fill_from_buffer(versatile_buffer &buffer) {
+                buffer >> accepted;
+            }
+        };
+
+        struct rtde_get_urcontrol_version_result_content {
+            uint32_t major, minor, bugfix, build;
+
+            void fill_from_buffer(versatile_buffer &buffer) {
+                buffer >> major >> minor >> bugfix >> build;
+            }
+        };
+
+        struct rtde_text_message_content {
+            uint8_t message_length, source_length, warning_level;
+            std::string message, source;
+
+            uint16_t length() const {
+                auto ret = sizeof(uint8_t) * 3;
+                ret += message_length;
+                ret += source_length;
+                return ret;
+            }
+
+            void fill_from_buffer(versatile_buffer &buffer) {
+                buffer >> message_length;
+                buffer.read_string(message, message_length);
+                buffer >> source_length;
+                buffer.read_string(source, source_length);
+                buffer >> warning_level;
+            }
+
+            void write_to_buffer(versatile_buffer &buffer) const {
+                assert(message.length() == message_length);
+                assert(source.length() == source_length);
+                buffer << message_length << message;
+                buffer << source_length << source;
+                buffer << warning_level;
+            }
+        };
+
+        template<typename RtdeDataContent>
+        struct rtde_data_package_content {
+            uint8_t recipe_id;
+            RtdeDataContent content;
+
+            uint16_t length() const {
+                return sizeof(uint8_t) + content.length();
+            }
+
+            void fill_from_buffer(versatile_buffer &buffer) {
+                buffer >> recipe_id >> content;
+            }
+
+            void write_to_buffer(versatile_buffer &buffer) const {
+                buffer << recipe_id << content;
+            }
+        };
+
+        struct rtde_setup_inputs_content {
+            std::vector<std::string> variable_names;
+
+            uint16_t length() const {
+                uint16_t ret = 0;
+                for (const auto &item: variable_names) {
+                    ret += item.length();
+                }
+                ret += variable_names.size() - 1;
+                return ret;
+            }
+
+            void write_to_buffer(versatile_buffer &buffer) const {
+                for (auto iter = variable_names.cbegin(), eiter = variable_names.cend(); iter != eiter; ++iter) {
+                    buffer << *iter;
+                    if (iter + 1 != eiter) {
+                        buffer << ',';
+                    }
+                }
+            }
+        };
+
+        struct rtde_setup_inputs_result_content {
+            uint8_t output_recipe_id;
+            std::vector<std::string> variable_types;
+
+            void fill_from_buffer(versatile_buffer &buffer) {
+                buffer >> output_recipe_id;
+                while (buffer) {
+                    variable_types.push_back(buffer.read_string_until(','));
+                }
+            }
+        };
+
+        struct rtde_setup_outputs_content
+                : public rtde_setup_inputs_content {
+            double output_frequency;
+
+            uint16_t length() const {
+                return sizeof(double) + rtde_setup_inputs_content::length();
+            }
+
+            void write_to_buffer(versatile_buffer &buffer) const {
+                buffer << output_frequency;
+                rtde_setup_inputs_content::write_to_buffer(buffer);
+            }
+        };
+
+        using rtde_setup_outputs_result_content = rtde_setup_inputs_result_content;
+        using rtde_start_result_content = rtde_request_protocol_version_result_content;
+        using rtde_pause_result_content = rtde_request_protocol_version_result_content;
+
+        struct ur_status_wrapper : public ur_status_content {
+
+            ur_status_wrapper() {
+                static_assert(sizeof(ur_status_wrapper) == sizeof(ur_status_content));
+                target_q = ur_vector6d_obj::new_instance();
+                target_qd = ur_vector6d_obj::new_instance();
+                target_qdd = ur_vector6d_obj::new_instance();
+                actual_q = ur_vector6d_obj::new_instance();
+                actual_qd = ur_vector6d_obj::new_instance();
+                actual_TCP_pose = ur_vector6d_obj::new_instance();
+                actual_TCP_speed = ur_vector6d_obj::new_instance();
+                target_TCP_pose = ur_vector6d_obj::new_instance();
+                target_TCP_speed = ur_vector6d_obj::new_instance();
+            }
+
+            void fill_from_buffer(versatile_buffer &buffer) {
+                buffer >> *target_q >> *target_qd >> *target_qdd;
+                buffer >> *actual_q >> *actual_qd;
+                buffer >> *actual_TCP_pose >> *actual_TCP_speed;
+                buffer >> *target_TCP_pose >> *target_TCP_speed;
+                buffer >> timestamp;
+                buffer >> robot_mode;
+                buffer >> runtime_state;
+                buffer >> safety_mode;
+                buffer >> last_cmd_id;
+                buffer >> is_moving;
+            }
+        };
+
+        struct ur_command_content {
+            int32_t move_type, cmd_id;
+            ur_vector6d_pointer tcp_pose, target;
+            double param_a, param_v;
+
+            ur_command_content() {
+                static int32_t cur_cmd_id = 0;
+                cmd_id = cur_cmd_id++;
+            }
+
+            uint16_t length() const {
+                return sizeof(int32_t) * 2 +
+                       ur_vector6d_obj::length() * 2 +
+                       sizeof(double) * 2;
+            }
+
+            void write_to_buffer(versatile_buffer &buffer) const {
+                buffer << move_type << cmd_id;
+                buffer << *tcp_pose << *target;
+                buffer << param_a << param_v;
+            }
+        };
+
+        struct ur_command_obj : public ur_command_content,
+                                public small_obj<ur_command_obj> {
+        };
+
+        std::vector<std::string> input_variable_list = {
+                "input_int_register_24", // Move Type
+                "input_int_register_25", // Cmd Id
+                "input_double_register_24", // 6 个参数表示 TCP Pose
+                "input_double_register_25",
+                "input_double_register_26",
+                "input_double_register_27",
+                "input_double_register_28",
+                "input_double_register_29",
+                "input_double_register_30", // 6 个参数表示 Target
+                "input_double_register_31",
+                "input_double_register_32",
+                "input_double_register_33",
+                "input_double_register_34",
+                "input_double_register_35",
+                "input_double_register_36", // 加速度参数
+                "input_double_register_37", // 速度参数
+        };
+        std::vector<std::string> output_variable_list = {
+                "target_q", // VECTOR6D
+                "target_qd", // VECTOR6D
+                "target_qdd", // VECTOR6D
+                "actual_q", // VECTOR6D
+                "actual_qd", // VECTOR6D
+                "actual_TCP_pose", // VECTOR6D
+                "actual_TCP_speed", // VECTOR6D
+                "target_TCP_pose", // VECTOR6D
+                "target_TCP_speed", // VECTOR6D
+                "timestamp", // DOUBLE
+                "robot_mode", // INT32
+                "runtime_state", // UINT32
+                "safety_mode", // INT32
+                "output_int_register_24", // INT32, last cmd Id
+                "output_bit_register_64", // BOOL, is moving
+        };
+
+        static constexpr uint16_t rtde_port = 30004;
+        static constexpr uint16_t rtde_protocol_version = 2;
+
+        ur_interface *q_this = nullptr;
+
+        double report_frequency = 125;
+        uint8_t inputs_recipe_id = 0, outputs_recipe_id = 0;
+
+        ip_address_type ur_ip;
+        scoped_ptr<tcp::socket> ur_socket; // https://stackoverflow.com/questions/3062803/
+        ur_vector6d_pointer ur_tcp;
+
+        versatile_buffer rtde_buffer;
+
+        ur_status_signal_type ur_status_signal;
+
+        // send packet with no content
+        awaitable<void> send_rtde_packet(uint8_t packet_type) {
+            rtde_packet_header header{
+                    .packet_size = rtde_packet_header::length(),
+                    .packet_type = packet_type,
+            };
+            assert(rtde_buffer.empty());
+            rtde_buffer.reset();
+            rtde_buffer << header;
+            assert(ur_socket->is_open());
+            co_await rtde_buffer.async_write_to(*ur_socket);
+            co_return;
+        }
+
+        template<typename RtdeContent>
+        awaitable<void> send_rtde_packet(uint8_t packet_type, const RtdeContent &content) {
+            rtde_packet_header header{
+                    .packet_size = rtde_packet_header::length() + content.length(),
+                    .packet_type = packet_type,
+            };
+            assert(rtde_buffer.empty());
+            rtde_buffer.reset();
+            rtde_buffer << header << content;
+            assert(ur_socket->is_open());
+            co_await rtde_buffer.async_write_to(*ur_socket);
+            co_return;
+        }
+
+        // contents will be read into rtde_buffer
+        awaitable<rtde_packet_header> receive_rtde_packet() {
+            rtde_packet_header header;
+            assert(ur_socket->is_open());
+            co_await async_read_value(*ur_socket, header.packet_size);
+            assert(rtde_buffer.empty());
+            co_await rtde_buffer.async_read_from(*ur_socket,
+                                                 header.packet_size - sizeof(header.packet_size));
+            rtde_buffer >> header.packet_type;
+            co_return header;
+        }
+
+        // return packet type
+        template<typename RtdeContent>
+        awaitable<uint8_t> receive_rtde_packet(RtdeContent &content) {
+            auto header = co_await receive_rtde_packet();
+            rtde_buffer >> content;
+            co_return header.packet_type;
+        }
+
+        awaitable<bool> negotiate_protocol() {
+            co_await send_rtde_packet(RTDE_REQUEST_PROTOCOL_VERSION,
+                                      rtde_request_protocol_version_content{
+                                              .protocol_version=rtde_protocol_version,
+                                      });
+            rtde_request_protocol_version_result_content protocol_result;
+            auto ret_packet_type = co_await receive_rtde_packet(protocol_result);
+            assert(ret_packet_type == RTDE_REQUEST_PROTOCOL_VERSION);
+            if (protocol_result.accepted != 1) {
+                // TODO show log
+                co_return false;
+            }
+            co_return true;
+        }
+
+        awaitable<void> print_ur_control_version() {
+            co_await send_rtde_packet(RTDE_GET_URCONTROL_VERSION);
+            rtde_get_urcontrol_version_result_content control_version_result;
+            auto ret_packet_type = co_await receive_rtde_packet(control_version_result);
+            assert(ret_packet_type == RTDE_GET_URCONTROL_VERSION);
+            SPDLOG_INFO("Get URControl version: V{}.{}.{}.{}",
+                        control_version_result.major, control_version_result.minor,
+                        control_version_result.bugfix, control_version_result.build);
+            co_return;
+        }
+
+        awaitable<bool> setup_inputs() {
+            rtde_setup_inputs_content setup_inputs_content;
+            std::copy(input_variable_list.cbegin(),
+                      input_variable_list.cend(),
+                      std::back_inserter(setup_inputs_content.variable_names));
+            co_await send_rtde_packet(RTDE_CONTROL_PACKAGE_SETUP_INPUTS,
+                                      setup_inputs_content);
+            rtde_setup_inputs_result_content setup_inputs_result;
+            auto ret_packet_type = co_await receive_rtde_packet(setup_inputs_result);
+            assert(ret_packet_type == RTDE_CONTROL_PACKAGE_SETUP_INPUTS);
+            inputs_recipe_id = setup_inputs_result.output_recipe_id;
+            if (inputs_recipe_id == 0) {
+                // TODO show log
+                co_return false;
+            }
+            co_return true;
+        }
+
+        awaitable<bool> setup_outputs() {
+            rtde_setup_outputs_content setup_outputs_content;
+            setup_outputs_content.output_frequency = report_frequency;
+            std::copy(output_variable_list.cbegin(),
+                      output_variable_list.cend(),
+                      std::back_inserter(setup_outputs_content.variable_names));
+            co_await send_rtde_packet(RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS,
+                                      setup_outputs_content);
+            rtde_setup_outputs_result_content setup_outputs_result;
+            auto ret_packet_type = co_await receive_rtde_packet(setup_outputs_result);
+            assert(ret_packet_type == RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS);
+            outputs_recipe_id = setup_outputs_result.output_recipe_id;
+            if (outputs_recipe_id == 0) {
+                // TODO show log
+                co_return false;
+            }
+            co_return true;
+        }
+
+        awaitable<bool> start_report() {
+            co_await send_rtde_packet(RTDE_CONTROL_PACKAGE_START);
+            rtde_start_result_content start_result;
+            auto ret_packet_type = co_await receive_rtde_packet(start_result);
+            assert(ret_packet_type == RTDE_CONTROL_PACKAGE_START);
+            if (start_result.accepted != 1) {
+                // TODO show log
+                co_return false;
+            }
+            co_return true;
+        }
+
+        awaitable<void> pause_report() {
+            co_await send_rtde_packet(RTDE_CONTROL_PACKAGE_PAUSE);
+            // let handle_packages() read the result
+            co_return;
+        }
+
+        void handle_data_package() {
+            auto status = ur_status::new_instance();
+            auto status_content_ptr = static_cast<ur_status_content *>(status.get());
+            auto status_wrapper_ptr = reinterpret_cast<ur_status_wrapper *>(status_content_ptr);
+            std::construct_at(status_wrapper_ptr);
+            rtde_data_package_content<ur_status_wrapper &> data_packet{
+                    .content = *status_wrapper_ptr,
+            };
+            rtde_buffer >> data_packet;
+            assert(data_packet.recipe_id == outputs_recipe_id);
+            ur_status_signal.emit_signal([=](ur_status_listener *listener) {
+                listener->on_new_ur_status(status);
+            });
+        }
+
+        void handle_text_message() {
+            rtde_text_message_content text_message;
+            rtde_buffer >> text_message;
+            // TODO show log
+        }
+
+        awaitable<void> handle_packages() {
+            for (;;) {
+                // receive package
+                rtde_packet_header header;
+                try {
+                    header = co_await receive_rtde_packet();
+                } catch (std::exception &e) {
+                    // TODO show log
+                    // TODO try auto re-connect
+                    co_spawn(q_this->get_context(), q_this->reset(), detached);
+                    co_return;
+                }
+
+                switch (header.packet_type) {
+                    case RTDE_CONTROL_PACKAGE_PAUSE: {
+                        rtde_pause_result_content pause_result;
+                        rtde_buffer >> pause_result;
+                        assert(pause_result.accepted == 1);
+                        co_return;
+                    }
+                    case RTDE_DATA_PACKAGE:
+                        handle_data_package();
+                        break;
+                    case RTDE_TEXT_MESSAGE:
+                        handle_text_message();
+                        break;
+                    default:
+                        rtde_buffer.reset();
+                        // TODO show log
+                        assert(false);
+                        break;
+                }
+            }
+        }
+
+        awaitable<bool> on_init_impl() {
+            ur_socket.reset(new tcp::socket(q_this->get_context()));
+            co_await ur_socket->async_connect({ur_ip, rtde_port}, use_awaitable);
+            // decrease delay
+            tcp::no_delay no_delay_option(true);
+            ur_socket->set_option(no_delay_option);
+            // setup steps
+            CO_ENSURE(negotiate_protocol())
+            co_await print_ur_control_version();
+            CO_ENSURE(setup_inputs())
+            CO_ENSURE(setup_outputs())
+            co_return true;
+        }
+
+        awaitable<bool> on_start_impl() {
+            CO_ENSURE(start_report())
+            co_spawn(q_this->get_context(), handle_packages(), detached);
+            co_return true;
+        }
+
+        impl() {
+            ur_tcp = ur_vector6d_obj::new_instance();
+            ur_tcp->get_view().fill({0, 0, 0, 0, 0, 0});
+        }
+
+    };
+
+    void ur_interface::set_ur_ip(const ip_address_type &ip) {
+        assert(get_state() == state_type::INITIAL);
+        pimpl->ur_ip = ip;
+    }
+
+    void ur_interface::set_report_frequency(double freq) {
+        assert(get_state() != state_type::RUNNING);
+        pimpl->report_frequency = freq;
+    }
+
+    void ur_interface::set_tcp(ur_vector6d_pointer tcp) {
+        pimpl->ur_tcp = std::move(tcp);
+    }
+
+    void ur_interface::add_status_listener(ur_status_signal_type::node_type &node) {
+        pimpl->ur_status_signal.add_listener(node);
+    }
+
+    awaitable<bool> ur_interface::on_init() {
+        try {
+            CO_ENSURE(pimpl->on_init_impl())
+        } catch (std::exception &e) {
+            SPDLOG_ERROR("Failed to init UR interface: {}", e.what());
+            co_return false;
+        }
+
+        co_return co_await tristate_obj::on_init();
+    }
+
+    awaitable<bool> ur_interface::on_start() {
+        try {
+            CO_ENSURE(pimpl->on_start_impl())
+        } catch (std::exception &e) {
+            SPDLOG_ERROR("Failed to start UR interface: {}", e.what());
+            co_return false;
+        }
+
+        co_return co_await tristate_obj::on_start();
+    }
+
+    boost::asio::awaitable<void> ur_interface::on_stop() {
+        try {
+            co_await pimpl->pause_report();
+        } catch (std::exception &e) {
+            SPDLOG_ERROR("Failed to start UR interface: {}", e.what());
+        }
+        co_await tristate_obj::on_stop();
+        co_return;
+    }
+
+    boost::asio::awaitable<void> ur_interface::on_reset() {
+        pimpl->ur_socket.reset();
+        co_await tristate_obj::on_reset();
+        co_return;
+    }
+
+    ur_interface::ur_interface()
+            : pimpl(std::make_unique<impl>()) {
+        pimpl->q_this = this;
+    }
+
+    ur_interface::~ur_interface() = default;
+
+}

src/robot/ur/ur_interface.h

@@ -0,0 +1,125 @@
+#ifndef SOPHIAR2_UR_INTERFACE_H
+#define SOPHIAR2_UR_INTERFACE_H
+
+#include "core/global_io_context.hpp"
+#include "core/small_obj.hpp"
+#include "core/tristate_obj.h"
+#include "core/types/vectorx.hpp"
+#include "utility/tiny_signal.hpp"
+
+#include <boost/asio/ip/address.hpp>
+
+#include <memory>
+
+namespace sophiar {
+
+    using ur_freq_tag = high_freq_tag;
+
+    namespace ur_runtime_status {
+        using ur_runtime_status_type = uint32_t;
+        static constexpr ur_runtime_status_type STOPPING = 0;
+        static constexpr ur_runtime_status_type STOPPED = 1;
+        static constexpr ur_runtime_status_type PLAYING = 2;
+        static constexpr ur_runtime_status_type PAUSING = 3;
+        static constexpr ur_runtime_status_type PAUSED = 4;
+        static constexpr ur_runtime_status_type RESUMING = 5;
+    }
+
+    namespace ur_robot_mode {
+        using ur_robot_mode_type = int32_t;
+        static constexpr ur_robot_mode_type NO_CONTROLLER = -1;
+        static constexpr ur_robot_mode_type DISCONNECTED = 0;
+        static constexpr ur_robot_mode_type CONFIRM_SAFETY = 1;
+        static constexpr ur_robot_mode_type BOOTING = 2;
+        static constexpr ur_robot_mode_type POWER_OFF = 3;
+        static constexpr ur_robot_mode_type POWER_ON = 4;
+        static constexpr ur_robot_mode_type IDLE = 5;
+        static constexpr ur_robot_mode_type BACKDRIVE = 6;
+        static constexpr ur_robot_mode_type RUNNING = 7;
+        static constexpr ur_robot_mode_type UPDATING_FIRMWARE = 8;
+    }
+
+    namespace ur_safety_mode {
+        using ur_safety_mode_type = int32_t;
+        static constexpr ur_safety_mode_type NORMAL = 1;
+        static constexpr ur_safety_mode_type REDUCED = 2;
+        static constexpr ur_safety_mode_type PROTECTIVE_STOP = 3;
+        static constexpr ur_safety_mode_type RECOVERY = 4;
+        static constexpr ur_safety_mode_type SAFEGUARD_STOP = 5;
+        static constexpr ur_safety_mode_type SYSTEM_EMERGENCY_STOP = 6;
+        static constexpr ur_safety_mode_type ROBOT_EMERGENCY_STOP = 7;
+        static constexpr ur_safety_mode_type VIOLATION = 8;
+        static constexpr ur_safety_mode_type FAULT = 9;
+        static constexpr ur_safety_mode_type VALIDATE_JOINT_ID = 10;
+        static constexpr ur_safety_mode_type UNDEFINED_SAFETY_MODE = 11;
+    }
+
+    using ur_vector6d_obj = vector6d_obj<ur_freq_tag>;
+    using ur_vector6d_pointer = ur_vector6d_obj::pointer;
+
+    struct ur_status_content {
+        ur_vector6d_pointer target_q, target_qd, target_qdd;
+        ur_vector6d_pointer actual_q, actual_qd;
+        ur_vector6d_pointer actual_TCP_pose, actual_TCP_speed;
+        ur_vector6d_pointer target_TCP_pose, target_TCP_speed;
+        double timestamp;
+        int32_t robot_mode;
+        uint32_t runtime_state;
+        int32_t safety_mode;
+        int32_t last_cmd_id;
+        bool is_moving;
+    };
+
+    struct ur_status : public ur_status_content,
+                       public small_obj<ur_status> {
+    };
+
+    struct ur_status_listener {
+        virtual void on_new_ur_status(const ur_status::pointer &status) = 0;
+    };
+
+    using ur_status_signal_type = tiny_signal<ur_status_listener>;
+
+    class ur_interface : public tristate_obj<ur_freq_tag> {
+    public:
+
+        static constexpr double a_joint_default = 1.4; // rad/s^2
+        static constexpr double a_tool_default = 1.2; // m/s^2
+        static constexpr double v_joint_default = 1.05; // rad/s
+        static constexpr double v_tool_default = 0.25; // m/s
+
+        using ip_address_type = boost::asio::ip::address;
+
+        ur_interface();
+
+        ~ur_interface() override;
+
+        void set_ur_ip(const ip_address_type &ip);
+
+        // default 125Hz
+        void set_report_frequency(double freq);
+
+        void add_status_listener(ur_status_signal_type::node_type &node);
+
+        void set_tcp(ur_vector6d_pointer tcp);
+
+    protected:
+
+        boost::asio::awaitable<bool> on_init() override;
+
+        boost::asio::awaitable<bool> on_start() override;
+
+        boost::asio::awaitable<void> on_stop() override;
+
+        boost::asio::awaitable<void> on_reset() override;
+
+    private:
+        struct impl;
+        std::unique_ptr<impl> pimpl;
+
+    };
+
+}
+
+
+#endif //SOPHIAR2_UR_INTERFACE_H

src/third_party/bitops.hpp

@@ -1,602 +0,0 @@
-#ifndef BITOPS_HH
-#define BITOPS_HH
-
-#if defined(__cpp_constexpr) && __cpp_constexpr >= 201304
-#define constexpr14 constexpr
-#else
-#define constexpr14
-#endif
-
-#include <cstdint>
-#include <cstddef>
-#include <climits>
-#include <limits>
-#include <type_traits>
-#include <algorithm>
-
-namespace std {
-
-//This implementation makes the following platform assumptions:
-//* signed right shift is an arithmetic shift
-//* CHAR_BIT == 8
-//* Native integer types are exactly 8, 16, 32, and 64 bits wide. No support is added for larger types.
-//* Signed numbers are implemented using 2's compliment
-//
-//The implementation is not designed to be efficient. The purpose is only to prove that each proposed function is implementable.
-//A real implementation may use techniques such as SFINAE, static_assert, overloading, and/or = delete to limit the set of overloads.
-//These have been omitted here to improve readability.
-
-////////////////////////////////////
-//Explicit shifts
-////////////////////////////////////
-
-//Logical left shift, undefined if s < 0 or x > sizeof(x) * CHAR_BIT
-//Just about every processor in existance has this
-//Included for symmetry
-template <typename Integral>
-  constexpr Integral shll(Integral x, int s) noexcept {
-    return Integral(typename std::make_unsigned<Integral>::type(x) << s);
-  }
-
-//Logical right shift, undefined if s < 0 or x > sizeof(x) * CHAR_BIT
-//Just about every processor in existance has this
-//Included for symmetry, also can right shift a signed number easily without a cast to unsigned
-template <typename Integral>
-  constexpr Integral shlr(Integral x, int s) noexcept {
-    return Integral(typename std::make_unsigned<Integral>::type(x) >> s);
-  }
-
-//Arithmetic left shift, undefined if s < 0 or x > sizeof(x) * CHAR_BIT
-//Just about every processor in existance has this
-//Included for symmetry
-template <typename Integral>
-  constexpr Integral shal(Integral x, int s) noexcept {
-    return shll(x, s);
-  }
-
-//Arithmetic right shift, undefined if s < 0 or x > sizeof(x) * CHAR_BIT
-//Just about every processor in existance has this, signed right shift is implementation defined. There is no standards compliant alternative to shar().
-template <typename Integral>
-  constexpr Integral shar(Integral x, int s) noexcept {
-    //Assumes signed right shift is arithmetic. If it is not the platform will need to implement this another way.
-    return Integral(typename std::make_signed<Integral>::type(x) >> s);
-  }
-
-//Circular left shift (rotate), undefined if s < 0 or x > sizeof(x) * CHAR_BIT
-//Just about every processor in existance has this, including the PDP-11 (1969) and yet C or C++ never included a way to get at this instruction.
-template <typename Integral>
-  constexpr Integral rotl(Integral x, int s) noexcept {
-    return (x << s) | shlr(x, (sizeof(x)*CHAR_BIT-s));
-  }
-
-//Circular right shift (rotate), undefined if s < 0 or x > sizeof(x) * CHAR_BIT
-//Just about every processor in existance has this, including the PDP-11 (1969) and yet C or C++ never included a way to get at this instruction.
-template <typename Integral>
-  constexpr Integral rotr(Integral x, int s) noexcept {
-    return shlr(x, s) | ( x << (sizeof(x)*CHAR_BIT-s));
-  }
-
-////////////////////////////////////
-//Zero and One Counting algorithms
-////////////////////////////////////
-
-//Returns the number of trailing zeros in x, or sizeof(x) * CHAR_BIT if x is 0
-//i386 bsf, cmov
-//x86_64 w/ BMI1: tzcnt
-//Alpha: cttz
-//MIPS: CLZ
-//gcc: x == 0 ? sizeof(x) * CHAR_BIT :__builtin_ctz(x)
-//Applications: SSE2 strlen, Howard Hinnant's gcd example
-template <typename Integral>
-  constexpr14 int cntt0(Integral x) noexcept {
-    constexpr int nbits = int(sizeof(x) * CHAR_BIT);
-    if(x == 0) { return nbits; }
-    Integral n = 0;
-    if(sizeof(x) > 1) {
-      if(sizeof(x) > 2) {
-        if(sizeof(x) > 4) {
-          if((x & Integral(0xFFFFFFFFUL)) == 0) { n = n + 32; x = shlr(x, 32); }
-        }
-        if((x & Integral(0xFFFFUL)) == 0) { n = n + 16; x = shlr(x, 16); }
-      }
-      if((x & Integral(0xFFUL)) == 0) { n = n + 8; x = shlr(x, 8); }
-    }
-    if((x & Integral(0xFUL)) == 0) { n = n + 4; x = shlr(x, 4); }
-    if((x & Integral(0x3UL)) == 0) { n = n + 2; x = shlr(x, 2); }
-    return n - (x & 1);
-  }
-
-//Returns the number of leading zeroes in x, or sizeof(x) * CHAR_BIT if x is 0
-//i386 bsr, cmov
-//x86_64 w/ SSE4: lzcnt
-//ARMv5: CLZ
-//IA64: clz
-//Alpha: CTLZ
-//PowerPC: cntlz[dw]
-//gcc: x == 0 ? sizeof(x) * CHAR_BIT :__builtin_clz(x)
-template <typename Integral>
-  constexpr14 int cntl0(Integral x) noexcept {
-    constexpr int nbits = int(sizeof(x) * CHAR_BIT);
-    if(x == 0) { return nbits; }
-    Integral n = 1;
-    if(sizeof(x) > 1) {
-      if(sizeof(x) > 2) {
-        if(sizeof(x) > 4) {
-          if((shlr(x, nbits-32)) == 0) { n = n + 32; x = x << 32; }
-        }
-        if((shlr(x, nbits-16)) == 0) { n = n + 16; x = x << 16; }
-      }
-      if((shlr(x, nbits-8)) == 0) { n = n + 8; x = x << 8; }
-    }
-    if((shlr(x, nbits-4)) == 0) { n = n + 4; x = x << 4; }
-    if((shlr(x, nbits-2)) == 0) { n = n + 2; x = x << 2; }
-    n = n - (shlr(x, 31));
-    return n;
-  }
-
-//Returns the number of leading 1 bits in x.
-//ARMv8: CLS
-//Blackfin: SIGNBITS
-//C6X: NORM
-//Picochip: SBC
-//MIPS: CTO
-//gcc: __builtin_clrsb(x)
-template <typename Integral>
-  constexpr14 int cntl1(Integral x) noexcept {
-    return cntl0(~x);
-  }
-
-//Returns the number of trailing 1 bits in x.
-template <typename Integral>
-  constexpr14 int cntt1(Integral x) noexcept {
-    return cntt0(~x);
-  }
-
-//Returns the number of 1 bits in x.
-//x86_64 SSE4: popcnt
-//IA64: popcnt
-//Alpha: CTPOP
-//PowerPC: popcntb
-//SparcV9: POPC
-//Blackfin: ONES
-//gcc: __builtin_popcount(x)
-template <typename Integral>
-  constexpr14 int popcount(Integral x) noexcept {
-    x = (x & Integral(0x5555555555555555UL)) + (shlr(x, 1) & Integral(0x5555555555555555UL));
-    x = (x & Integral(0x3333333333333333UL)) + (shlr(x, 2) & Integral(0x3333333333333333UL));
-    x = (x & Integral(0x0F0F0F0F0F0F0F0FUL)) + (shlr(x, 4) & Integral(0x0F0F0F0F0F0F0F0FUL));
-    if(sizeof(x) > 1) {
-      x = (x & Integral(0x00FF00FF00FF00FFUL)) + (shlr(x, 8) & Integral(0x00FF00FF00FF00FFUL));
-      if(sizeof(x) > 2) {
-        x = (x & Integral(0x0000FFFF0000FFFFUL)) + (shlr(x, 16) & Integral(0x0000FFFF0000FFFFUL));
-        if(sizeof(x) > 4) {
-          x = (x & Integral(0x00000000FFFFFFFFUL)) + (shlr(x, 32) & Integral(0x00000000FFFFFFFFUL));
-        }
-      }
-    }
-    return x;
-  }
-
-//Returns the number of 1 bits in x mod 2
-//gcc: __builtin_parity(x)
-template <typename Integral>
-  constexpr14 int parity(Integral x) noexcept {
-    x = x ^ shlr(x, 1);
-    x = x ^ shlr(x, 2);
-    x = x ^ shlr(x, 4);
-    if(sizeof(x) > 1) {
-      x = x ^ shlr(x, 8);
-      if(sizeof(x) > 2) {
-        x = x ^ shlr(x, 16);
-        if(sizeof(x) > 4) {
-          x = x ^ shlr(x, 32);
-        }
-      }
-    }
-    return x;
-  }
-
-////////////////////////////////////
-//Rightmost bit manipulation
-////////////////////////////////////
-
-//Reset least siginificant 1 bit
-//Resets the least siginificant 1 bit of x. Returns 0 if x is 0.
-//x86_64 BMI1: BLSR
-template <typename Integral>
-  constexpr Integral rstls1b(Integral x) {
-    return x & (x-1);
-  }
-
-//Set the least significant 0 bit
-//x86_64 AMD TBM: BLCS
-template <typename Integral>
-  constexpr Integral setls0b(Integral x) {
-    return x | (x + 1);
-  }
-
-//Isolate least siginificant 1 bit
-//Isolates the least significant 1 bit of x and returns it. Returns 0 if x is 0.
-//x86_64 BMI1: BLSI
-//x86_64 AMD TBM: BLSIC, NOT
-template <typename Integral>
-  constexpr Integral isols1b(Integral x) {
-    return x & -x;
-  }
-
-//Set the least significant zero bit to 1 and all of the rest to 0.
-template <typename Integral>
-  constexpr Integral isols0b(Integral x) {
-    return (~x) & (x + 1);
-  }
-
-//Reset the trailing 1's in x
-//x86_64 AMD TBM: BLCFILL
-template <typename Integral>
-  constexpr Integral rstt1(Integral x) {
-    return x & (x + 1);
-  }
-
-//Set all of the trailing 0's in x
-//x86_64 AMD TBM: BLSFILL
-template <typename Integral>
-  constexpr Integral sett0(Integral x) {
-    return x | (x - 1);
-  }
-
-//Returns a mask with all of the trailing 0's set.
-template <typename Integral>
-  constexpr Integral maskt0(Integral x) {
-    return (~x) & (x-1);
-  }
-
-//Returns a mask with all of the trailing 1's set.
-template <typename Integral>
-  constexpr Integral maskt1(Integral x) {
-    return ~((~x) | (x + 1));
-  }
-
-//Returns a mask with all of the trailing 0's  and the least significant 1 bit set.
-//x86_64 BMI1: BLSMSK
-//x86_64 AMD TBM: TZMSK
-template <typename Integral>
-  constexpr Integral maskt0ls1b(Integral x) {
-    return (x-1) ^ x;
-  }
-
-//Returns a mask with all of the trailing 1's and the least significant 0 bit set.
-template <typename Integral>
-  constexpr Integral maskt1ls0b(Integral x) {
-    return x ^ (x + 1);
-  }
-
-////////////////////////////////////
-//Bit and Byte reversal algorithms
-////////////////////////////////////
-
-//Reverse each group of blocks of bits in x.
-//
-//bits_per_block == 1: reverses the bits of x
-//ARMv7: RBIT
-//EPIPHANY: BITR
-//bits_per_block == 2: reverses each pair of bits in x
-//bits_per_block == 4: reverses the nibbles in x
-//AVR: SWAP
-//bits_per_block == 8: reverses the bytes in x (assuming CHAR_BIT == 8). This is the traditional byte swap.
-//i386: bswap
-//ARMv5: REV
-//PDP-11: SWAB
-//gcc: __builtin_bswap[16|32|64](x)
-//(blocks_per_group == 2) ARMv6: REV16
-//(blocks_per_group == 4) ARMv8: REV32
-//bits_per_block == 16,32,etc.. reverses the words in x.
-//(bits_per_block == 16) MC68020: SWAP
-template <typename Integral>
-  constexpr14 auto reverse_bits(Integral x,
-      int subword_bits = 1,
-      int group_subwords = 1)
-  noexcept -> typename std::enable_if<std::is_unsigned<Integral>::value, Integral>::type {
-    int group_sz = int(sizeof(Integral) * CHAR_BIT) / group_subwords;
-    int k = group_sz - subword_bits;
-    if(k & 1) x = shll(x & Integral(0x5555555555555555UL), 1) | shlr(x & Integral(0xAAAAAAAAAAAAAAAAUL), 1);
-    if(k & 2) x = shll(x & Integral(0x3333333333333333UL), 2) | shlr(x & Integral(0xCCCCCCCCCCCCCCCCUL), 2);
-    if(k & 4) x = shll(x & Integral(0x0F0F0F0F0F0F0F0FUL), 4) | shlr(x & Integral(0xF0F0F0F0F0F0F0F0UL), 4);
-    //sizeof comparisons added to help compiler remove these checks for small integers
-    if(sizeof(x) > 1 && k & 8) x = shll(x & Integral(0x00FF00FF00FF00FFUL), 8) | shlr(x & Integral(0xFF00FF00FF00FF00UL), 8);
-    if(sizeof(x) > 2 && k & 16) x = shll(x & Integral(0x0000FFFF0000FFFFUL), 16) | shlr(x & Integral(0xFFFF0000FFFF0000UL), 16);
-    if(sizeof(x) > 4 && k & 32) x = shll(x & Integral(0x00000000FFFFFFFFUL), 32) | shlr(x & Integral(0xFFFFFFFF00000000UL), 32);
-    return x;
-  }
-
-//Signed version calls unsigned to avoid sign extension issues
-template <typename Integral>
-  constexpr14 auto reverse_bits(Integral x,
-      int subword_bits = 1,
-      int group_subwords = 1)
-  noexcept -> typename std::enable_if<std::is_signed<Integral>::value, Integral>::type {
-    return Integral(reverse_bits(typename std::make_unsigned<Integral>::type(x), subword_bits, group_subwords));
-  }
-
-
-//Byte reversal, simple wrapper around reverse_bits
-template <typename Integral>
-  constexpr14 Integral reverse_bytes(Integral x,
-      int bytes_per_block=1,
-      int blocks_per_group = sizeof(Integral)) noexcept {
-    return reverse_bits(x, CHAR_BIT * bytes_per_block, blocks_per_group);
-  }
-
-////////////////////////////////////
-//Single bit manipulation
-////////////////////////////////////
-
-//Sets bit b in x, undefined behavior if b < 0 or b >= sizeof(x) * CHAR_BIT
-template <typename Integral>
-  constexpr Integral setbit(Integral x, int b) noexcept {
-    return x | (Integral(1) << b);
-  }
-
-//Resets bit b in x, undefined behavior if b < 0 or b >= sizeof(x) * CHAR_BIT
-template <typename Integral>
-  constexpr Integral rstbit(Integral x, int b) noexcept {
-    return x & ~(Integral(1) << b);
-  }
-
-//Flips bit b in x, undefined behavior if b < 0 or b >= sizeof(x) * CHAR_BIT
-template <typename Integral>
-  constexpr Integral flipbit(Integral x, int b) noexcept {
-    return x ^ (Integral(1) << b);
-  }
-
-//Return whether or not bit b is set in x, undefined behavior if b < 0 or b >= sizeof(x) * CHAR_BIT
-template <typename Integral>
-  constexpr bool testbit(Integral x, int b) noexcept {
-    return x & (Integral(1) << b);
-  }
-
-////////////////////////////////////
-//Range of bits manipulation
-////////////////////////////////////
-
-//Resets all bits >= position b, nop if b > sizeof(x) * CHAR_BIT
-//x86_64 w/ BMI2: BZHI
-template <typename Integral>
-  constexpr Integral rstbitsge(Integral x, int b) noexcept {
-    return x & ((Integral(1) << b)-1);
-  }
-
-//Resets all bits < position b, nop if b > sizeof(x) * CHAR_BIT
-template <typename Integral>
-  constexpr Integral rstbitsle(Integral x, int b) noexcept {
-    return x & ~((Integral(1) << (b+1))-1);
-  }
-
-//Set all bits >= position b, nop if b > sizeof(x) * CHAR_BIT
-template <typename Integral>
-  constexpr Integral setbitsge(Integral x, int b) noexcept {
-    return x | ~((Integral(1) << b)-1);
-  }
-
-//Sets all bits < position b, nop if b > sizeof(x) * CHAR_BIT
-template <typename Integral>
-  constexpr Integral setbitsle(Integral x, int b) noexcept {
-    return x | ((Integral(1) << (b+1))-1);
-  }
-
-//Flip all bits >= position b, nop if b > sizeof(x) * CHAR_BIT
-template <typename Integral>
-  constexpr Integral flipbitsge(Integral x, int b) noexcept {
-    return x ^ ~((Integral(1) << b)-1);
-  }
-
-//Flip all bits < position b, nop if b > sizeof(x) * CHAR_BIT
-template <typename Integral>
-  constexpr Integral flipbitsle(Integral x, int b) noexcept {
-    return x ^ ((Integral(1) << (b+1))-1);
-  }
-
-////////////////////////////////////
-//Power of 2 manipulation
-////////////////////////////////////
-
-//Returns true if x is a power of 2
-//Application: Integral template arguments, add optimizations for power of 2
-template <typename Integral>
-  constexpr bool ispow2(Integral x) noexcept {
-    return x > 0 && (x & (x-1)) == 0;
-    //return popcount(x) == 1;
-  }
-
-//Round up to the next power of 2
-//Application: Growable containers whose size must be a power of 2
-//Application: Extending a 2d image size to a power of 2 for 3d graphics libraries (OpenGL/DirectX)
-template <typename Integral>
-constexpr14 Integral ceilp2(Integral x) noexcept {
-  x = x-1;
-  x |= shlr(x, 1);
-  x |= shlr(x, 2);
-  x |= shlr(x, 4);
-  if(sizeof(x) > 1) {
-    x |= shlr(x, 8);
-    if(sizeof(x) > 2) {
-      x |= shlr(x, 16);
-      if(sizeof(x) > 4) {
-        x |= shlr(x, 32);
-      }
-    }
-  }
-  return x + 1;
-}
-
-//Round down to the previous power of 2
-//Application: See ceilp2
-template <typename Integral>
-constexpr14 Integral floorp2(Integral x) noexcept {
-  x |= shlr(x, 1);
-  x |= shlr(x, 2);
-  x |= shlr(x, 4);
-  if(sizeof(x) > 1) {
-    x |= shlr(x, 8);
-    if(sizeof(x) > 2) {
-      x |= shlr(x, 16);
-      if(sizeof(x) > 4) {
-        x |= shlr(x, 32);
-      }
-    }
-  }
-  return x - shlr(x, 1);
-}
-
-////////////////////////////////////
-//Saturated Arithmetic
-////////////////////////////////////
-
-//Perform saturated addition on l and r.
-//ARMv7 DSP extensions: QADD
-template <typename IntegralL, typename IntegralR>
-  constexpr auto satadd(IntegralL l, IntegralR r) noexcept -> decltype(l + r) {
-    typedef decltype(l + r) LR;
-    return LR(l) > std::numeric_limits<LR>::max() - LR(r) ? std::numeric_limits<LR>::max() : l + r;
-  }
-
-//Perform saturated subtraction on l and r.
-//ARMv7 DSP extensions: QSUB
-template <typename IntegralL, typename IntegralR>
-  constexpr auto satsub(IntegralL l, IntegralR r) noexcept -> decltype(l - r) {
-    typedef decltype(l + r) LR;
-    return LR(l) < std::numeric_limits<LR>::min() + LR(r) ? std::numeric_limits<LR>::min() : l - r;
-  }
-
-
-
-////////////////////////////////////
-//Pointer and size alignment helpers
-////////////////////////////////////
-
-//Returns true if t is aligned to a
-template <typename Integral>
-constexpr bool is_aligned(Integral t, size_t a) noexcept {
-  return ((t & (a-1)) == 0);
-}
-bool is_aligned(void* t, size_t a) noexcept {
-  return is_aligned(uintptr_t(t), a);
-}
-
-//Returns the smallest number n when n >= val && is_aligned(n, align). align must be a power of 2!
-template <typename Integral>
-constexpr Integral align_up(Integral val, size_t a) noexcept {
-  return ((val + (a -1)) & -a);
-}
-void* align_up(void* val, size_t a) noexcept {
-  return (void*)align_up(uintptr_t(val), a);
-}
-
-//Returns the largest number n when n <= val && is_aligned(n, align). align must be a power of 2!
-template <typename Integral>
-constexpr Integral align_down(Integral val, size_t a) noexcept {
-  return val & -a;
-}
-void* align_down(void* val, size_t a) noexcept {
-  return (void*)align_down(uintptr_t(val), a);
-}
-
-///////////////////////////////////
-//Bit Shuffling
-///////////////////////////////////
-
-//Outer Perfect Shuffle
-template <typename Integral>
-constexpr14 Integral outer_pshuffle(Integral x) noexcept {
-  Integral t = 0;
-  if(sizeof(x) > 4) {
-    t = (x ^ shlr(x, 16)) & Integral(0x00000000FFFF0000UL);
-    x = x ^ t ^ shll(t, 16);
-  }
-  if(sizeof(x) > 2) {
-    t = (x ^ shlr(x, 8)) & Integral(0x0000FF000000FF00UL);
-    x = x ^ t ^ shll(t, 8);
-  }
-  if(sizeof(x) > 1) {
-    t = (x ^ shlr(x, 4)) & Integral(0x00F000F000F000F0UL);
-    x = x ^ t ^ shll(t, 4);
-  }
-  t = (x ^ shlr(x, 2)) & Integral(0x0C0C0C0C0C0C0C0CUL);
-  x = x ^ t ^ shll(t, 2);
-  t = (x ^ shlr(x, 1)) & Integral(0x2222222222222222UL);
-  x = x ^ t ^ shll(t, 1);
-  return x;
-}
-
-template <typename Integral>
-constexpr14 Integral outer_punshuffle(Integral x) noexcept {
-  Integral t = 0;
-  t = (x ^ shlr(x, 1)) & Integral(0x2222222222222222UL);
-  x = x ^ t ^ shll(t, 1);
-  t = (x ^ shlr(x, 2)) & Integral(0x0C0C0C0C0C0C0C0CUL);
-  x = x ^ t ^ shll(t, 2);
-  if(sizeof(x) > 1) {
-    t = (x ^ shlr(x, 4)) & Integral(0x00F000F000F000F0UL);
-    x = x ^ t ^ shll(t, 4);
-  }
-  if(sizeof(x) > 2) {
-    t = (x ^ shlr(x, 8)) & Integral(0x0000FF000000FF00UL);
-    x = x ^ t ^ shll(t, 8);
-  }
-  if(sizeof(x) > 4) {
-    t = (x ^ shlr(x, 16)) & Integral(0x00000000FFFF0000UL);
-    x = x ^ t ^ shll(t, 16);
-  }
-  return x;
-}
-
-template <typename Integral>
-constexpr14 Integral inner_pshuffle(Integral x) noexcept {
-  return outer_pshuffle(reverse_bits(x, sizeof(x)*CHAR_BIT/2, 2));
-}
-
-template <typename Integral>
-constexpr14 Integral inner_punshuffle(Integral x) noexcept {
-  return reverse_bits(outer_punshuffle(x), sizeof(x)*CHAR_BIT/2, 2);
-}
-
-///////////////////////////////////
-//Bits Deposit and Extract
-///////////////////////////////////
-
-//Parallel Bits Deposit
-//x    HGFEDCBA
-//mask 01100100
-//res  0CB00A00
-//x86_64 BMI2: PDEP
-template <typename Integral>
-constexpr14 Integral deposit_bits(Integral x, Integral mask) {
-  Integral res = 0;
-  for(Integral bb = 1; mask != 0; bb += bb) {
-    if(x & bb) {
-      res |= mask & (-mask);
-    }
-    mask &= (mask - 1);
-  }
-  return res;
-}
-
-//Parallel Bits Extract
-//x    HGFEDCBA
-//mask 01100100
-//res  00000GFC
-//x86_64 BMI2: PEXT
-template <typename Integral>
-constexpr14 Integral extract_bits(Integral x, Integral mask) {
-  Integral res = 0;
-  for(Integral bb = 1; mask != 0; bb += bb) {
-    if(x & mask & -mask) {
-      res |= bb;
-    }
-    mask &= (mask - 1);
-  }
-  return res;
-}
-
-} //namespace std
-
-#endif

src/third_party/static_block.hpp

@@ -0,0 +1,80 @@
+/**
+ * @file static_block.hpp
+ *
+ * @brief An implementation of a Java-style static block, in C++ (and potentially a
+ * GCC/clang extension to avoid warnings).
+ *
+ * @note Partially inspired by Andrei Alexandrescu's Scope Guard and
+ * discussions on stackoverflow.com
+ *
+ * By Eyal Rozenberg <eyalroz1@gmx.com>
+ *
+ * Licensed under the Apache License v2.0:
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ */
+#pragma once
+#ifndef STATIC_BLOCK_HPP_
+#define STATIC_BLOCK_HPP_
+
+#ifndef CONCATENATE
+#define CONCATENATE(s1, s2)     s1##s2
+#define EXPAND_THEN_CONCATENATE(s1, s2) CONCATENATE(s1, s2)
+#endif /* CONCATENATE */
+
+#ifndef UNIQUE_IDENTIFIER
+/**
+ * This macro expands into a different identifier in every expansion.
+ * Note that you _can_ clash with an invocation of UNIQUE_IDENTIFIER
+ * by manually using the same identifier elsewhere; or by carefully
+ * choosing another prefix etc.
+ */
+#ifdef __COUNTER__
+#define UNIQUE_IDENTIFIER(prefix) EXPAND_THEN_CONCATENATE(prefix, __COUNTER__)
+#else
+#define UNIQUE_IDENTIFIER(prefix) EXPAND_THEN_CONCATENATE(prefix, __LINE__)
+#endif /* COUNTER */
+#else
+#endif /* UNIQUE_IDENTIFIER */
+
+/**
+ * Following is a mechanism for executing code statically.
+ *
+ * @note Caveats:
+ * - Your static block must be surround by curly braces.
+ * - No need for a semicolon after the block (but it won't hurt).
+ * - Do not put static blocks in files, as it might get compiled multiple
+ *   times ane execute multiple times.
+ * - A static_block can only be used in file scope - not within any other block etc.
+ * - Templated static blocks will probably not work. Avoid them.
+ * - No other funny business, this is fragile.
+ * - This does not having any threading issues (AFAICT) - as it has no static
+ *   initialization order issue. Of course, you have to _keep_ it safe with
+ *   your static code.
+ * - Execution of the code is guaranteed to occur before main() executes,
+ *   but the relative order of statics being initialized is unknown/unclear. So,
+ *   do not call any method of an instance of a class which you expect to have been
+ *   constructed; it may not have been. Instead, you can use a static getInstance() method
+ *   (look this idiom up on the web, it's safe).
+ * - Variables defined within the static block are not global; they will
+ *   go out of scope as soon as its execution concludes.
+ *
+ * Usage example:
+ *
+ *   static_block {
+ *         do_stuff();
+ *         std::cout << "in the static block!\n";
+ *   }
+ *
+ */
+#define static_block STATIC_BLOCK_IMPL1(UNIQUE_IDENTIFIER(_static_block_))
+
+#define STATIC_BLOCK_IMPL1(prefix) \
+	STATIC_BLOCK_IMPL2(CONCATENATE(prefix,_fn),CONCATENATE(prefix,_var))
+
+#define STATIC_BLOCK_IMPL2(function_name,var_name) \
+static void function_name(); \
+static int var_name __attribute((unused)) = (function_name(), 0) ; \
+static void function_name()
+
+#endif // STATIC_BLOCK_HPP_

src/utility/bit_operations.hpp

@@ -0,0 +1,49 @@
+#ifndef SOPHIAR2_BIT_OPERATIONS_HPP
+#define SOPHIAR2_BIT_OPERATIONS_HPP
+
+#include <boost/endian/conversion.hpp>
+
+#include <bit>
+#include <climits>
+#include <cstdint>
+#include <limits>
+#include <type_traits>
+
+namespace sophiar {
+
+    using bit_pos_type = std::uint8_t;
+
+    template<typename T>
+    std::enable_if_t<std::is_integral_v<T>>
+    inline set_bit(T &val, bit_pos_type pos) {
+        static_assert(sizeof(T) * CHAR_BIT - 1 <= std::numeric_limits<bit_pos_type>::max());
+        val |= (T(1) << pos);
+    }
+
+    template<typename T>
+    std::enable_if_t<std::is_integral_v<T>, bool>
+    inline test_bit(const T &val, bit_pos_type pos) {
+        static_assert(sizeof(T) * CHAR_BIT - 1 <= std::numeric_limits<bit_pos_type>::max());
+        return val & (T(1) << pos);
+    }
+
+    template<typename T>
+    std::enable_if_t<std::is_integral_v<T>, bit_pos_type>
+    inline popcount(const T &val) {
+        static_assert(sizeof(T) * CHAR_BIT <= std::numeric_limits<bit_pos_type>::max());
+        return std::popcount(std::make_unsigned_t<T>(val));
+    }
+
+    template<typename T>
+    std::enable_if_t<std::is_arithmetic_v<T>>
+    inline swap_net_loc_endian(T &val) {
+        if constexpr(boost::endian::order::native == boost::endian::order::big) {
+            return;
+        } else {
+            boost::endian::endian_reverse_inplace(val);
+        }
+    }
+
+}
+
+#endif //SOPHIAR2_BIT_OPERATIONS_HPP

src/utility/coro_signal.hpp

@@ -0,0 +1,109 @@
+#ifndef SOPHIAR2_CORO_SIGNAL_HPP
+#define SOPHIAR2_CORO_SIGNAL_HPP
+
+#include <boost/asio/awaitable.hpp>
+#include <boost/asio/experimental/channel.hpp>
+#include <boost/asio/io_context.hpp>
+#include <boost/asio/use_awaitable.hpp>
+#include <boost/core/noncopyable.hpp>
+#include <boost/system/error_code.hpp>
+
+#include <cassert>
+
+namespace sophiar {
+
+    class coro_signal : private boost::noncopyable {
+    public:
+
+        class signal_token : private boost::noncopyable {
+        public:
+
+            explicit signal_token(coro_signal &_s)
+                    : signal(_s) {
+                ++signal.waiting_count;
+            }
+
+            ~signal_token() {
+                if (!is_fulfilled) {
+                    --signal.waiting_count;
+                }
+                // handle leaking signal
+                if (signal.waiting_count == 0 &&
+                    signal.channel.ready()) {
+                    signal.channel.reset();
+                    signal.is_notifying = false;
+                }
+            }
+
+            void fulfill() {
+                is_fulfilled = true;
+                --signal.waiting_count;
+            }
+
+        private:
+
+            coro_signal &signal;
+            bool is_fulfilled = false;
+
+        };
+
+        coro_signal(boost::asio::io_context &context)
+                : channel(context, 1) {}
+
+        auto new_token() {
+            return signal_token{*this};
+        }
+
+        boost::asio::awaitable<void> coro_wait(signal_token &token) {
+            co_await channel.async_receive(boost::asio::use_awaitable);
+            token.fulfill();
+            try_notify_more();
+            co_return;
+        }
+
+        boost::asio::awaitable<void> coro_wait() {
+            signal_token token(*this);
+            co_await coro_wait(token);
+            co_return;
+        }
+
+        bool try_wait(signal_token &token) {
+            if (!channel.ready()) return false;
+            token.fulfill();
+            try_notify_more();
+            return true;
+        }
+
+        void try_notify_all() {
+            if (is_notifying || waiting_count == 0) return;
+            is_notifying = true;
+            send_signal();
+        }
+
+    private:
+
+        using channel_type = boost::asio::experimental::channel<
+                void(boost::system::error_code, bool)>;
+
+        channel_type channel;
+        size_t waiting_count = 0;
+        bool is_notifying = false;
+
+        void send_signal() {
+            assert(!channel.ready());
+            channel.try_send(boost::system::error_code{}, true);
+        }
+
+        void try_notify_more() {
+            if (waiting_count > 0) {
+                send_signal();
+            } else {
+                is_notifying = false;
+            }
+        }
+
+    };
+
+}
+
+#endif //SOPHIAR2_CORO_SIGNAL_HPP

src/utility/debug_utility.hpp

@@ -0,0 +1,39 @@
+#ifndef SOPHIAR2_DEBUG_UTILITY_HPP
+#define SOPHIAR2_DEBUG_UTILITY_HPP
+
+#include "core/timestamp_helper.hpp"
+
+#include <boost/asio/awaitable.hpp>
+#include <boost/asio/high_resolution_timer.hpp>
+#include <boost/asio/this_coro.hpp>
+#include <boost/asio/use_awaitable.hpp>
+
+#include <fmt/format.h>
+
+#include <chrono>
+#include <coroutine>
+#include <cstring>
+#include <iostream>
+
+using boost::asio::awaitable;
+using boost::asio::use_awaitable;
+
+#define FILE_LINE_TRACE {\
+    std::cout << fmt::format("F:{} L:{} T:{}", \
+                             __FILE_NAME__, __LINE__, sophiar::current_timestamp()) \
+              << std::endl; }\
+
+#define CO_ENSURE(func) \
+    { \
+        bool ok = co_await (func); \
+        if (!ok) co_return false; \
+    }
+
+inline awaitable<void> coro_sleep(std::chrono::milliseconds t) {
+    boost::asio::high_resolution_timer timer(co_await boost::asio::this_coro::executor);
+    timer.expires_from_now(t);
+    co_await timer.async_wait(use_awaitable);
+    co_return;
+}
+
+#endif //SOPHIAR2_DEBUG_UTILITY_HPP

src/utility/statistic_timer.hpp

@@ -0,0 +1,112 @@
+#ifndef SOPHIAR2_STATISTIC_TIMER_HPP
+#define SOPHIAR2_STATISTIC_TIMER_HPP
+
+#include "core/timestamp_helper.hpp"
+
+#include <boost/accumulators/accumulators.hpp>
+#include <boost/accumulators/statistics/max.hpp>
+#include <boost/accumulators/statistics/min.hpp>
+#include <boost/accumulators/statistics/stats.hpp>
+#include <boost/accumulators/statistics/variance.hpp>
+
+#include <fmt/format.h>
+
+#include <cassert>
+#include <cmath>
+#include <string>
+#include <type_traits>
+
+namespace sophiar {
+
+    struct statistic_timer_single_shot_tag;
+    struct statistic_timer_start_stop_tag;
+
+    template<typename ModeTag>
+    class statistic_timer {
+    public:
+
+        void start(timestamp_type current_ts = current_timestamp());
+
+        template<typename CurrentModeTag = ModeTag>
+        std::enable_if_t<std::is_same_v<CurrentModeTag, statistic_timer_start_stop_tag>>
+        stop(timestamp_type current_ts = current_timestamp()) {
+            assert(is_running);
+            is_running = false;
+            accumulator(current_ts - last_ts);
+        }
+
+        void reset() {
+            accumulator = {};
+        }
+
+        auto get_count() const {
+            return boost::accumulators::count(accumulator);
+        }
+
+        auto get_min() const {
+            return boost::accumulators::min(accumulator);
+        }
+
+        auto get_max() const {
+            return boost::accumulators::max(accumulator);
+        }
+
+        auto get_mean() const {
+            return boost::accumulators::mean(accumulator);
+        }
+
+        auto get_std() const {
+            return std::sqrt(boost::accumulators::variance(accumulator));
+        }
+
+        std::string to_string() const;
+
+    private:
+
+        using float_type = double;
+        using accumulator_type = boost::accumulators::accumulator_set<
+                float_type, boost::accumulators::stats<
+                        boost::accumulators::tag::min,
+                        boost::accumulators::tag::max,
+                        boost::accumulators::tag::variance> >;
+
+        accumulator_type accumulator;
+        bool is_running = false;
+        timestamp_type last_ts = 0;
+    };
+
+    template<>
+    void statistic_timer<statistic_timer_single_shot_tag>::start(timestamp_type current_ts) {
+        if (last_ts != 0) {
+            accumulator(current_ts - last_ts);
+        }
+        last_ts = current_ts;
+    }
+
+    template<>
+    void statistic_timer<statistic_timer_start_stop_tag>::start(timestamp_type current_ts) {
+        assert(!is_running);
+        is_running = true;
+        last_ts = current_ts;
+    }
+
+    template<>
+    std::string statistic_timer<statistic_timer_single_shot_tag>::to_string() const {
+        return fmt::format("Cnt: {}, "
+                           "Min: {:.3f}ms({:.3f}Hz), Max: {:.3f}ms({:.3f}Hz), "
+                           "Mean: {:.3f}({:.3f})ms({:.3f}Hz)",
+                           get_count(),
+                           get_min() / 1e3, 1e6 / get_min(),
+                           get_max() / 1e3, 1e6 / get_max(),
+                           get_mean() / 1e3, get_std() / 1e3, 1e6 / get_mean());
+    }
+
+    template<>
+    std::string statistic_timer<statistic_timer_start_stop_tag>::to_string() const {
+        return fmt::format("Cnt: {}, Min: {:.3f}ms, Max: {:.3f}ms, Mean: {:.3f}({:.3f})ms",
+                           get_count(), get_min() / 1e3, get_max() / 1e3,
+                           get_mean() / 1e3, get_std() / 1e3);
+    }
+}
+
+#endif //SOPHIAR2_STATISTIC_TIMER_HPP

src/utility/tiny_signal.hpp

@@ -0,0 +1,46 @@
+#ifndef SOPHIAR2_TINY_SIGNAL_HPP
+#define SOPHIAR2_TINY_SIGNAL_HPP
+
+#include <boost/intrusive/list.hpp>
+
+#include <type_traits>
+
+template<typename ListenerType>
+class tiny_signal {
+public:
+
+    using hook_type = boost::intrusive::list_base_hook<
+            boost::intrusive::link_mode<
+                    boost::intrusive::auto_unlink>>;
+
+    struct node_type : public hook_type {
+        ListenerType *listener;
+
+        explicit node_type(ListenerType *other = nullptr)
+                : listener(other) {}
+    };
+
+    using list_type = boost::intrusive::list<
+            node_type, boost::intrusive::constant_time_size<false>>;
+
+    void add_listener(node_type &node) {
+        if (node.is_linked()) {
+            node.unlink();
+        }
+        node_list.push_back(node);
+    }
+
+    template<typename FuncType>
+    std::enable_if_t<std::is_void_v<std::invoke_result_t<FuncType, ListenerType *>>>
+    emit_signal(FuncType &&func) const {
+        for (const auto &node: node_list) {
+            func(node.listener);
+        }
+    }
+
+private:
+    list_type node_list;
+
+};
+
+#endif //SOPHIAR2_TINY_SIGNAL_HPP

src/utility/versatile_buffer.hpp

@@ -0,0 +1,192 @@
+#ifndef SOPHIAR2_VERSATILE_BUFFER_HPP
+#define SOPHIAR2_VERSATILE_BUFFER_HPP
+
+#include "utility/bit_operations.hpp"
+
+#include <boost/asio/awaitable.hpp>
+#include <boost/asio/read.hpp>
+#include <boost/asio/use_awaitable.hpp>
+#include <boost/asio/write.hpp>
+
+#include <cassert>
+#include <string>
+#include <type_traits>
+
+namespace sophiar {
+
+    static constexpr size_t VERSATILE_BUFFER_MAX_SIZE = 1024;
+
+    // handle endian properly
+    class versatile_buffer {
+    public:
+
+        template<typename T>
+        std::enable_if_t<std::is_arithmetic_v<T>>
+        write_value(T val) {
+            assert(end_pos + sizeof(T) <= VERSATILE_BUFFER_MAX_SIZE);
+
+            swap_net_loc_endian(val);
+            std::memcpy(buffer + end_pos, &val, sizeof(T));
+
+            end_pos += sizeof(T);
+        }
+
+        template<typename T>
+        std::enable_if_t<std::is_void_v<
+                decltype(std::declval<const T>().write_to_buffer(
+                        std::declval<versatile_buffer &>()))>>
+        write_value(const T &val) {
+            val.write_to_buffer(*this);
+        }
+
+        void write_value(const std::string &str) {
+            assert(end_pos + str.length() <= VERSATILE_BUFFER_MAX_SIZE);
+            std::memcpy(buffer + end_pos, str.data(), str.length());
+            end_pos += str.length();
+        }
+
+        template<typename T>
+        versatile_buffer &operator<<(const T &val) {
+            write_value(val);
+            return *this;
+        }
+
+        template<typename T>
+        std::enable_if_t<std::is_arithmetic_v<T>, T>
+        read_value() {
+            assert(start_pos + sizeof(T) <= end_pos);
+
+            T tmp_val;
+            std::memcpy(&tmp_val, buffer + start_pos, sizeof(T));
+            swap_net_loc_endian(tmp_val);
+
+            start_pos += sizeof(T);
+            return tmp_val;
+        }
+
+        template<typename T>
+        std::enable_if_t<std::is_arithmetic_v<T>>
+        read_value(T &val) {
+            val = read_value<T>();
+        }
+
+        template<typename T>
+        std::enable_if_t<std::is_void_v<
+                decltype(std::declval<T>().fill_from_buffer(
+                        std::declval<versatile_buffer &>()))>>
+        read_value(T &val) {
+            val.fill_from_buffer(*this);
+        }
+
+        std::string read_string(size_t length) {
+            assert(start_pos + length <= end_pos);
+            auto ret = std::string{buffer + start_pos, buffer + start_pos + length};
+            start_pos += length;
+            return ret;
+        }
+
+        void read_string(std::string &str, size_t length) {
+            str = read_string(length);
+        }
+
+        std::string read_string_until(char sep) {
+            auto cur_pos = start_pos;
+            while (cur_pos != end_pos && buffer[cur_pos] != sep) {
+                ++cur_pos;
+            }
+            auto ret = std::string{buffer + start_pos, buffer + cur_pos};
+            if (cur_pos != end_pos) {
+                start_pos = cur_pos + 1;
+            } else {
+                start_pos = cur_pos;
+            }
+            return ret;
+        }
+
+        void read_string_until(std::string &str, char sep) {
+            str = read_string_until(sep);
+        }
+
+        template<typename T>
+        versatile_buffer &operator>>(T &val) {
+            read_value(val);
+            return *this;
+        }
+
+        // will cause reset
+        template<typename AsyncReadStream>
+        boost::asio::awaitable<void> async_read_from(AsyncReadStream &s, size_t length) {
+            assert(empty());
+            reset();
+            assert(length <= VERSATILE_BUFFER_MAX_SIZE);
+            co_await boost::asio::async_read(s,
+                                             boost::asio::buffer(buffer, length),
+                                             boost::asio::use_awaitable);
+            end_pos = length;
+            co_return;
+        }
+
+        // will cause reset
+        template<typename AsyncWriteStream>
+        boost::asio::awaitable<void> async_write_to(AsyncWriteStream &s) {
+            co_await boost::asio::async_write(s,
+                                              boost::asio::buffer(buffer + start_pos, end_pos - start_pos),
+                                              boost::asio::use_awaitable);
+            reset();
+            co_return;
+        }
+
+        bool empty() const {
+            return start_pos == end_pos;
+        }
+
+        explicit operator bool() const {
+            return !empty();
+        }
+
+        void reset() {
+            start_pos = 0;
+            end_pos = 0;
+        }
+
+        size_t size() const {
+            return end_pos - start_pos;
+        }
+
+    private:
+        char buffer[VERSATILE_BUFFER_MAX_SIZE];
+        size_t start_pos = 0, end_pos = 0;
+
+    };
+
+    template<typename AsyncWriteStream, typename T>
+    std::enable_if_t<std::is_arithmetic_v<T>, boost::asio::awaitable<void>>
+    inline async_write_value(AsyncWriteStream &s, T val) {
+        swap_net_loc_endian(val);
+        co_await boost::asio::async_write(s,
+                                          boost::asio::buffer(&val, sizeof(T)),
+                                          boost::asio::use_awaitable);
+        co_return;
+    }
+
+    template<typename T, typename AsyncReadStream>
+    std::enable_if_t<std::is_arithmetic_v<T>, boost::asio::awaitable<T>>
+    inline async_read_value(AsyncReadStream &s) {
+        T tmp_val;
+        co_await boost::asio::async_read(s,
+                                         boost::asio::buffer(&tmp_val, sizeof(T)),
+                                         boost::asio::use_awaitable);
+        swap_net_loc_endian(tmp_val);
+        co_return tmp_val;
+    }
+
+    template<typename T, typename AsyncReadStream>
+    std::enable_if_t<std::is_arithmetic_v<T>, boost::asio::awaitable<void>>
+    inline async_read_value(AsyncReadStream &s, T &val) {
+        val = co_await async_read_value<T>(s);
+        co_return;
+    }
+
+}
+
+#endif //SOPHIAR2_VERSATILE_BUFFER_HPP

tests/CMakeLists.txt

@@ -2,5 +2,11 @@ set (Boost_USE_STATIC_LIBS OFF)
 find_package (Boost REQUIRED COMPONENTS unit_test_framework)
 include_directories (${Boost_INCLUDE_DIRS})
 
-add_executable (test_core core/main.cpp)
-target_link_libraries (test_core ${Boost_LIBRARIES})
+add_executable (test_core
+        core/datanode_base.cpp
+        core/small_obj.cpp
+        core/tristate_obj.cpp
+        core/vectorx.cpp
+        ${EXTERN_DEF_FILES}
+        ${CORE_IMPL_FILES})
+target_link_libraries (test_core ${Boost_LIBRARIES} ${EXTRA_LIBS})

tests/core/datanode_base.cpp

@@ -0,0 +1,189 @@
+#define BOOST_TEST_DYN_LINK
+
+#include "core/datanode_base.h"
+#include "utility/bit_operations.hpp"
+#include "utility/debug_utility.hpp"
+
+#include <boost/asio/co_spawn.hpp>
+#include <boost/asio/detached.hpp>
+#include <boost/asio/high_resolution_timer.hpp>
+#include <boost/asio/this_coro.hpp>
+#include <boost/asio/use_awaitable.hpp>
+#include <boost/test/unit_test.hpp>
+
+#include <fmt/format.h>
+
+#include <algorithm>
+#include <chrono>
+#include <iostream>
+
+using boost::asio::awaitable;
+using boost::asio::co_spawn;
+using boost::asio::detached;
+using boost::asio::use_awaitable;
+
+using namespace sophiar;
+using namespace std::chrono_literals;
+
+awaitable<void> test_datanode_base_1() {
+
+    struct type_a : public datanode_base<high_freq_tag> {
+        unsigned int cnt = 0;
+
+        awaitable<bool> exec() {
+            ++cnt;
+            co_return true;
+        }
+    } node_a;
+
+    BOOST_TEST(co_await node_a.init());
+    BOOST_TEST(node_a.set_trigger_mode(type_a::trigger_mode_type::MANUAL));
+    BOOST_TEST(co_await node_a.start());
+    BOOST_TEST(co_await node_a.trigger());
+    BOOST_TEST(node_a.cnt == 1);
+
+    co_await coro_sleep(50ms);
+    node_a.set_minimal_exec_interval(10ms);
+    for (int i = 0; i < 5; ++i) {
+        co_spawn(co_await boost::asio::this_coro::executor, node_a.trigger(), detached);
+    }
+    BOOST_TEST(node_a.cnt == 2);
+    co_await coro_sleep(50ms);
+    BOOST_TEST(node_a.cnt == 3);
+
+    node_a.cnt = 0;
+    co_await coro_sleep(50ms);
+    node_a.set_minimal_exec_interval(10ms);
+    for (int i = 0; i < 5; ++i) {
+        co_spawn(co_await boost::asio::this_coro::executor, node_a.trigger(), detached);
+    }
+    BOOST_TEST(node_a.cancel_pending());
+    BOOST_TEST(node_a.cnt == 1);
+    co_await coro_sleep(50ms);
+    BOOST_TEST(node_a.cnt == 1);
+    co_await node_a.stop();
+
+    node_a.cnt = 0;
+    BOOST_TEST(node_a.set_trigger_mode(type_a::trigger_mode_type::INPUT));
+    node_a.set_trigger_input_mask(1);
+    BOOST_TEST(co_await node_a.start());
+    node_a.update_input_data(0, type_a::data_packet::new_instance());
+    BOOST_TEST(node_a.cnt == 1);
+
+    co_await coro_sleep(50ms);
+    for (int i = 0; i < 5; ++i) {
+        node_a.update_input_data(0, type_a::data_packet::new_instance());
+    }
+    BOOST_TEST(node_a.cnt == 2);
+    co_await coro_sleep(50ms);
+    BOOST_TEST(node_a.cnt == 3);
+    co_await node_a.stop();
+
+    node_a.cnt = 0;
+    BOOST_TEST(node_a.set_trigger_mode(type_a::trigger_mode_type::PERIODIC));
+    node_a.set_trigger_interval(50ms);
+    co_spawn(co_await boost::asio::this_coro::executor, [&]() -> awaitable<void> {
+        co_await coro_sleep(275ms);
+        co_await node_a.stop();
+    }, detached);
+    co_await node_a.start();
+    co_await coro_sleep(350ms);
+    BOOST_TEST(node_a.cnt == 6);
+    BOOST_TEST((node_a.get_state() == type_a::state_type::PENDING));
+
+}
+
+awaitable<void> test_datanode_base_2() {
+    struct test_type : public datanode_base<high_freq_tag> {
+        unsigned int cnt = 0;
+
+        awaitable<bool> exec() {
+            auto input_data = get_input_data(0);
+            cnt = (*input_data)[0];
+            auto output_data = data_packet::new_instance();
+            output_data->copy_content(*input_data);
+            (*output_data)[0] += 1.0;
+            set_output_data(0, output_data);
+            co_return true;
+        }
+    };
+
+    test_type node_a, node_b;
+    test_type::connect(node_a, 0, node_b, 0);
+    co_await node_a.init();
+    co_await node_b.init();
+    node_a.set_trigger_mode(test_type::trigger_mode_type::INPUT);
+    node_b.set_trigger_mode(test_type::trigger_mode_type::INPUT);
+    node_a.set_trigger_input_mask(0b1);
+    node_b.set_trigger_input_mask(0b1);
+    co_await node_a.start();
+    co_await node_b.start();
+    auto packet = test_type::data_packet::new_instance();
+    packet->refresh();
+    (*packet)[0] = 101;
+    node_a.update_input_data(0, packet);
+    BOOST_TEST(node_a.cnt == 101);
+    BOOST_TEST(node_b.cnt == 102);
+}
+
+awaitable<void> test_datanode_base_speed(size_t length, size_t repeat) {
+    struct test_type : public datanode_base<high_freq_tag> {
+        uint64_t cnt = 0;
+
+        awaitable<bool> exec() {
+            auto input_data = get_input_data(0);
+            auto output_data = data_packet::new_instance();
+            output_data->copy_content(*input_data);
+            for (size_t i = 0; i < test_type::data_packet::DATA_PACKET_LENGTH; ++i) {
+                (*output_data)[i] += i;
+            }
+            std::reverse(output_data->data, output_data->data + test_type::data_packet::DATA_PACKET_LENGTH);
+            output_data->timestamp = current_timestamp();
+            cnt = (*output_data)[0];
+            set_output_data(0, output_data);
+            co_return true;
+        }
+    };
+
+    auto pool = new test_type[length];
+    for (size_t i = 0; i < length; ++i) {
+        auto &node = pool[i];
+        co_await node.init();
+        node.set_trigger_mode(test_type::trigger_mode_type::INPUT);
+        node.set_trigger_input_mask(0b1);
+        if (i != 0) {
+            test_type::connect(pool[i - 1], 0, node, 0);
+        }
+        co_await node.start();
+    }
+
+    auto test_data = test_type::data_packet::new_instance();
+    test_data->refresh();
+
+    auto start_ts = current_timestamp();
+    for (int i = 0; i < repeat; ++i) {
+        pool[0].update_input_data(0, test_data);
+//        assert(pool[length - 1].cnt == length);
+    }
+
+    auto time_used = (current_timestamp() - start_ts) / 1000.0;
+    auto time_left = 1000.0 / repeat - time_used / repeat;
+    std::cout << fmt::format("Length = {}, Repeat = {}, "
+                             "Time used = {:.3f}ms ({:.3f}ms, {:.2f}% left)",
+                             length, repeat, time_used,
+                             time_left, time_left / (10.0 / repeat))
+              << std::endl;
+
+    BOOST_TEST(true);
+}
+
+BOOST_AUTO_TEST_CASE(test_datanode_base) {
+//    co_spawn(high_freq_context, test_datanode_base_1(), detached);
+//    co_spawn(high_freq_context, test_datanode_base_2(), detached);
+//    high_freq_context.run();
+    for (auto length: {1, 4, 16, 64, 128})
+        for (auto repeat: {1, 50, 125, 500, 1000, 2000, 5000, 10000})
+            co_spawn(high_freq_context, test_datanode_base_speed(length, repeat), detached);
+    high_freq_context.run();
+}
+

tests/core/main.cpp

@@ -1,218 +0,0 @@
-#define BOOST_TEST_DYN_LINK
-#define BOOST_TEST_MAIN  // in only one cpp file
-
-#include <boost/test/unit_test.hpp>
-
-#include "core/small_obj.hpp"
-#include "core/tristate_obj.hpp"
-#include "third_party/bitops.hpp"
-
-using namespace sophiar;
-
-BOOST_AUTO_TEST_CASE(test_small_obj) {
-
-    struct test_obj : public small_obj<test_obj> {
-        explicit test_obj(int &_p) : probe(_p) {
-            probe = 1;
-        }
-
-        ~test_obj() {
-            probe = 2;
-        }
-
-        int &probe;
-    };
-
-    int my_probe = 0;
-    {
-        auto obj = test_obj::new_instance(my_probe);
-        BOOST_TEST(my_probe == 1);
-    }
-    BOOST_TEST(my_probe == 2);
-
-    my_probe = 0;
-    {
-        test_obj::pointer obj_2;
-        {
-            auto obj = test_obj::new_instance(my_probe);
-            obj_2 = obj;
-            BOOST_TEST(my_probe == 1);
-            BOOST_TEST(obj_2->ref_count == 2);
-        }
-        BOOST_TEST(my_probe == 1);
-        BOOST_TEST(obj_2->ref_count == 1);
-    }
-    BOOST_TEST(my_probe == 2);
-
-}
-
-BOOST_AUTO_TEST_CASE(test_tristate_obj) {
-
-    struct test_type : public tristate_obj<test_type> {
-        bool on_init() {
-            probe = std::setbit(probe, 0);
-            return true;
-        };
-
-        bool on_start() {
-            probe = std::setbit(probe, 1);
-            return true;
-        };
-
-        void on_stop() {
-            probe = std::setbit(probe, 2);
-        };
-
-        void on_reset() {
-            probe = std::setbit(probe, 3);
-        };
-
-        explicit test_type(int &_p) : probe(_p) {}
-
-        int &probe;
-    };
-
-    int probe = 0;
-    test_type test_obj(probe);
-    BOOST_TEST(test_obj.state == ST_INITIAL);
-    test_obj.init();
-    BOOST_TEST(test_obj.state == ST_PENDING);
-    BOOST_TEST(std::testbit(probe, 0));
-    test_obj.start();
-    BOOST_TEST(test_obj.state == ST_RUNNING);
-    BOOST_TEST(std::testbit(probe, 1));
-    test_obj.stop();
-    BOOST_TEST(test_obj.state == ST_PENDING);
-    BOOST_TEST(std::testbit(probe, 2));
-    test_obj.reset();
-    BOOST_TEST(test_obj.state == ST_INITIAL);
-    BOOST_TEST(std::testbit(probe, 3));
-
-    probe = 0;
-    test_obj.init();
-    test_obj.start();
-    test_obj.reset();
-    BOOST_TEST(test_obj.state == ST_INITIAL);
-    BOOST_TEST(std::popcount(probe) == 4);
-
-    struct test_type_2 : public tristate_obj<test_type_2> {
-        void after_init() {
-            probe = std::setbit(probe, 0);
-        }
-
-        void after_start() {
-            probe = std::setbit(probe, 1);
-        }
-
-        void after_stop() {
-            probe = std::setbit(probe, 2);
-        }
-
-        void after_reset() {
-            probe = std::setbit(probe, 3);
-        }
-
-        explicit test_type_2(int &_p) : probe(_p) {}
-
-        int &probe;
-    };
-
-    probe = 0;
-    test_type_2 obj_2(probe);
-    obj_2.init();
-    BOOST_TEST(std::testbit(probe, 0));
-    obj_2.start();
-    BOOST_TEST(std::testbit(probe, 1));
-    obj_2.stop();
-    BOOST_TEST(std::testbit(probe, 2));
-    obj_2.reset();
-    BOOST_TEST(std::testbit(probe, 3));
-
-    struct test_type_3 : public tristate_obj<test_type_3> {
-        void after_state_change(state_type old_state) {
-            switch (old_state) {
-                case ST_INITIAL:
-                    probe = std::setbit(probe, 0);
-                    break;
-                case ST_PENDING:
-                    probe = std::setbit(probe, 1);
-                    break;
-                case ST_RUNNING:
-                    probe = std::setbit(probe, 2);
-                    break;
-                default:
-                    return;
-            }
-        }
-
-        explicit test_type_3(int &_p) : probe(_p) {}
-
-        int &probe;
-    };
-
-    probe = 0;
-    test_type_3 obj_3(probe);
-    obj_3.init();
-    BOOST_TEST(std::testbit(probe, 0));
-    obj_3.start();
-    BOOST_TEST(std::testbit(probe, 1));
-    obj_3.stop();
-    BOOST_TEST(std::testbit(probe, 2));
-    probe = 0;
-    obj_3.reset();
-    BOOST_TEST(std::testbit(probe, 1));
-
-    struct test_type_4 : public tristate_obj<test_type_4> {
-        auto on_init() {
-            return boost::indeterminate;
-        }
-
-        auto on_start() {
-            return boost::indeterminate;
-        }
-
-        void _init_finished(bool flag) {
-            init_finished(flag);
-        }
-
-        void _start_finished(bool flag) {
-            start_finished(flag);
-        }
-
-        void after_init() {
-            probe = std::setbit(probe, 0);
-        }
-
-        void after_start() {
-            probe = std::setbit(probe, 1);
-        }
-
-        explicit test_type_4(int &_p) : probe(_p) {}
-
-        int &probe;
-    };
-
-    probe = 0;
-    test_type_4 obj_4(probe);
-    obj_4.init();
-    BOOST_TEST(obj_4.state == ST_INITIALIZING);
-    BOOST_TEST(!std::testbit(probe, 0));
-    obj_4._init_finished(false);
-    BOOST_TEST(obj_4.state == ST_INITIAL);
-    BOOST_TEST(!std::testbit(probe, 0));
-    obj_4.init();
-    obj_4._init_finished(true);
-    BOOST_TEST(obj_4.state == ST_PENDING);
-    BOOST_TEST(std::testbit(probe, 0));
-    obj_4.start();
-    BOOST_TEST(obj_4.state == ST_STARING);
-    BOOST_TEST(!std::testbit(probe, 1));
-    obj_4._start_finished(false);
-    BOOST_TEST(obj_4.state == ST_PENDING);
-    BOOST_TEST(!std::testbit(probe, 1));
-    obj_4.start();
-    obj_4._start_finished(true);
-    BOOST_TEST(obj_4.state == ST_RUNNING);
-    BOOST_TEST(std::testbit(probe, 1));
-
-}

tests/core/small_obj.cpp

@@ -0,0 +1,45 @@
+#define BOOST_TEST_DYN_LINK
+#define BOOST_TEST_MAIN  // in only one cpp file
+
+#include <boost/test/unit_test.hpp>
+
+#include "core/small_obj.hpp"
+
+using namespace sophiar;
+
+BOOST_AUTO_TEST_CASE(test_small_obj) {
+
+    struct test_obj : public small_obj<test_obj> {
+        explicit test_obj(int &_p) : probe(_p) {
+            probe = 1;
+        }
+
+        ~test_obj() {
+            probe = 2;
+        }
+
+        int &probe;
+    };
+
+    int my_probe = 0;
+    {
+        auto obj = test_obj::new_instance(my_probe);
+        BOOST_TEST(my_probe == 1);
+    }
+    BOOST_TEST(my_probe == 2);
+
+    my_probe = 0;
+    {
+        test_obj::pointer obj_2;
+        {
+            auto obj = test_obj::new_instance(my_probe);
+            obj_2 = obj;
+            BOOST_TEST(my_probe == 1);
+            BOOST_TEST(obj_2->ref_count == 2);
+        }
+        BOOST_TEST(my_probe == 1);
+        BOOST_TEST(obj_2->ref_count == 1);
+    }
+    BOOST_TEST(my_probe == 2);
+
+}

tests/core/tristate_obj.cpp

@@ -0,0 +1,260 @@
+#define BOOST_TEST_DYN_LINK
+
+#include "core/tristate_obj.h"
+#include "utility/bit_operations.hpp"
+#include "utility/debug_utility.hpp"
+
+#include <boost/asio/co_spawn.hpp>
+#include <boost/asio/detached.hpp>
+#include <boost/asio/high_resolution_timer.hpp>
+#include <boost/asio/this_coro.hpp>
+#include <boost/asio/use_awaitable.hpp>
+#include <boost/test/unit_test.hpp>
+
+#include <chrono>
+
+using boost::asio::awaitable;
+using boost::asio::co_spawn;
+using boost::asio::detached;
+using boost::asio::use_awaitable;
+
+using namespace sophiar;
+using namespace std::chrono_literals;
+
+awaitable<void> test_tristate_obj_1() {
+    struct type_a : public tristate_obj<high_freq_tag> {
+        unsigned int cnt = 0;
+
+        awaitable<bool> on_init() {
+            set_bit(cnt, 0);
+            co_return true;
+        }
+
+        awaitable<bool> on_start() {
+            set_bit(cnt, 1);
+            co_return true;
+        }
+
+        awaitable<void> on_stop() {
+            set_bit(cnt, 2);
+            co_return;
+        }
+
+        awaitable<void> on_reset() {
+            set_bit(cnt, 3);
+            co_return;
+        }
+    } node_a;
+
+    co_await node_a.init();
+    BOOST_TEST(node_a.cnt == 0b0001);
+    BOOST_TEST((node_a.get_state() == type_a::state_type::PENDING));
+    co_await node_a.start();
+    BOOST_TEST(node_a.cnt == 0b0011);
+    BOOST_TEST((node_a.get_state() == type_a::state_type::RUNNING));
+    co_await node_a.stop();
+    BOOST_TEST(node_a.cnt == 0b0111);
+    BOOST_TEST((node_a.get_state() == type_a::state_type::PENDING));
+    co_await node_a.reset();
+    BOOST_TEST(node_a.cnt == 0b1111);
+    BOOST_TEST((node_a.get_state() == type_a::state_type::INITIAL));
+
+    node_a.cnt = 0;
+    co_await node_a.start(); // test start before init
+    BOOST_TEST(node_a.cnt == 0b0000);
+    BOOST_TEST((node_a.get_state() == type_a::state_type::INITIAL));
+    co_await node_a.init();
+    co_await node_a.start();
+    BOOST_TEST(node_a.cnt == 0b0011);
+    BOOST_TEST((node_a.get_state() == type_a::state_type::RUNNING));
+    co_await node_a.reset(); // test force reset
+    BOOST_TEST(node_a.cnt == 0b1111);
+    BOOST_TEST((node_a.get_state() == type_a::state_type::INITIAL));
+
+    co_return;
+}
+
+awaitable<void> test_tristate_obj_2() {
+    struct type_a : public tristate_obj<high_freq_tag> {
+        int cnt = 0;
+        boost::asio::high_resolution_timer timer;
+
+        type_a()
+                : timer(get_context()) {}
+
+        awaitable<bool> on_init() {
+            timer.expires_from_now(100ms);
+            co_await timer.async_wait(use_awaitable);
+            set_bit(cnt, 0);
+            co_return true;
+        }
+
+        awaitable<bool> on_start() {
+            timer.expires_from_now(100ms);
+            co_await timer.async_wait(use_awaitable);
+            set_bit(cnt, 1);
+            co_return true;
+        }
+
+        awaitable<void> on_stop() {
+            timer.expires_from_now(100ms);
+            co_await timer.async_wait(use_awaitable);
+            set_bit(cnt, 2);
+            co_return;
+        }
+
+        awaitable<void> on_reset() {
+            timer.expires_from_now(100ms);
+            co_await timer.async_wait(use_awaitable);
+            set_bit(cnt, 3);
+            co_return;
+        }
+    } node_a;
+
+    co_spawn(co_await boost::asio::this_coro::executor, [&]() -> awaitable<void> {
+        co_await coro_sleep(50ms);
+        co_await node_a.reset();
+        co_return;
+    }, detached);
+    BOOST_TEST((co_await node_a.init() == false));
+    BOOST_TEST(node_a.cnt == 0b1000);
+    BOOST_TEST((node_a.get_state() == type_a::state_type::INITIAL));
+
+    node_a.cnt = 0;
+    BOOST_TEST((co_await node_a.init() == true));
+    BOOST_TEST((node_a.get_state() == type_a::state_type::PENDING));
+    co_spawn(co_await boost::asio::this_coro::executor, [&]() -> awaitable<void> {
+        co_await coro_sleep(50ms);
+        co_await node_a.stop();
+        co_return;
+    }, detached);
+    BOOST_TEST((co_await node_a.start() == false));
+    BOOST_TEST(node_a.cnt == 0b0101);
+    BOOST_TEST((node_a.get_state() == type_a::state_type::PENDING));
+
+    node_a.cnt = 0b0001;
+    co_spawn(co_await boost::asio::this_coro::executor, [&]() -> awaitable<void> {
+        co_await coro_sleep(50ms);
+        co_await node_a.reset();
+        co_return;
+    }, detached);
+    BOOST_TEST((co_await node_a.start() == false));
+    BOOST_TEST(node_a.cnt = 0b0101);
+    BOOST_TEST((node_a.get_state() == type_a::state_type::PENDING));
+
+    co_await coro_sleep(150ms);
+    BOOST_TEST(node_a.cnt == 0b1101);
+    BOOST_TEST((node_a.get_state() == type_a::state_type::INITIAL));
+
+    co_return;
+}
+
+awaitable<void> test_tristate_obj_3() {
+    struct type_a : public tristate_obj<high_freq_tag> {
+        int cnt = 0;
+        boost::asio::high_resolution_timer timer;
+
+        type_a()
+                : timer(get_context()) {}
+
+        awaitable<bool> on_init() {
+            timer.expires_from_now(100ms);
+            co_await timer.async_wait(use_awaitable);
+            ++cnt;
+            co_return true;
+        }
+
+        awaitable<bool> on_start() {
+            timer.expires_from_now(100ms);
+            co_await timer.async_wait(use_awaitable);
+            ++cnt;
+            co_return true;
+        }
+
+        awaitable<void> on_stop() {
+            timer.expires_from_now(100ms);
+            co_await timer.async_wait(use_awaitable);
+            ++cnt;
+            co_return;
+        }
+
+        awaitable<void> on_reset() {
+            timer.expires_from_now(100ms);
+            co_await timer.async_wait(use_awaitable);
+            ++cnt;
+            co_return;
+        }
+    } node_a;
+
+    node_a.cnt = 0;
+    co_spawn(co_await boost::asio::this_coro::executor, [&]() -> awaitable<void> {
+        BOOST_TEST(co_await node_a.init() == true);
+        BOOST_TEST(node_a.cnt == 1);
+    }, detached);
+    BOOST_TEST(co_await node_a.init() == true);
+    BOOST_TEST((node_a.get_state() == type_a::state_type::PENDING));
+    BOOST_TEST(node_a.cnt == 1);
+
+    node_a.cnt = 0;
+    co_spawn(co_await boost::asio::this_coro::executor, [&]() -> awaitable<void> {
+        BOOST_TEST(co_await node_a.start() == true);
+        BOOST_TEST(node_a.cnt == 1);
+    }, detached);
+    BOOST_TEST(co_await node_a.start() == true);
+    BOOST_TEST((node_a.get_state() == type_a::state_type::RUNNING));
+    BOOST_TEST(node_a.cnt == 1);
+
+    node_a.cnt = 0;
+    co_spawn(co_await boost::asio::this_coro::executor, [&]() -> awaitable<void> {
+        co_await node_a.stop();
+        BOOST_TEST(node_a.cnt == 1);
+    }, detached);
+    co_await node_a.stop();
+    BOOST_TEST((node_a.get_state() == type_a::state_type::PENDING));
+    BOOST_TEST(node_a.cnt == 1);
+
+    node_a.cnt = 0;
+    co_spawn(co_await boost::asio::this_coro::executor, [&]() -> awaitable<void> {
+        co_await node_a.reset();
+        BOOST_TEST(node_a.cnt == 1);
+    }, detached);
+    co_await node_a.reset();
+    BOOST_TEST((node_a.get_state() == type_a::state_type::INITIAL));
+    BOOST_TEST(node_a.cnt == 1);
+
+    node_a.cnt = 0;
+    co_spawn(co_await boost::asio::this_coro::executor, [&]() -> awaitable<void> {
+        co_await coro_sleep(50ms);
+        co_await node_a.reset();
+        co_return;
+    }, detached);
+    co_spawn(co_await boost::asio::this_coro::executor, [&]() -> awaitable<void> {
+        BOOST_TEST(co_await node_a.init() == false);
+        BOOST_TEST(node_a.cnt == 1);
+    }, detached);
+    BOOST_TEST(co_await node_a.init() == false);
+    BOOST_TEST((node_a.get_state() == type_a::state_type::INITIAL));
+    BOOST_TEST(node_a.cnt == 1);
+
+    co_await node_a.init();
+    node_a.cnt = 0;
+    co_spawn(co_await boost::asio::this_coro::executor, [&]() -> awaitable<void> {
+        co_await coro_sleep(50ms);
+        co_await node_a.stop();
+        co_return;
+    }, detached);
+    co_spawn(co_await boost::asio::this_coro::executor, [&]() -> awaitable<void> {
+        BOOST_TEST(co_await node_a.start() == false);
+        BOOST_TEST(node_a.cnt == 1);
+    }, detached);
+    BOOST_TEST(co_await node_a.start() == false);
+    BOOST_TEST((node_a.get_state() == type_a::state_type::PENDING));
+    BOOST_TEST(node_a.cnt == 1);
+}
+
+BOOST_AUTO_TEST_CASE(test_tristate_obj) {
+    co_spawn(high_freq_context, test_tristate_obj_1(), detached);
+    co_spawn(high_freq_context, test_tristate_obj_2(), detached);
+    co_spawn(high_freq_context, test_tristate_obj_3(), detached);
+    high_freq_context.run();
+}

tests/core/vectorx.cpp

@@ -0,0 +1,149 @@
+#define BOOST_TEST_DYN_LINK
+
+#include <boost/test/unit_test.hpp>
+
+#include "core/global_io_context.hpp"
+#include "core/types/vectorx.hpp"
+
+using namespace sophiar;
+
+void test_vectorx_view_func_1(vectorx_view<double *, 2> data) {}
+
+void test_vectorx_view_func_2(vectorx_view<const double *, 2> data) {}
+
+BOOST_AUTO_TEST_CASE(test_vectorx) {
+
+    auto test_a = vector3d({3, 2, 1});
+    BOOST_TEST(test_a[0] == 3);
+    BOOST_TEST(test_a[1] == 2);
+    BOOST_TEST(test_a[2] == 1);
+    BOOST_TEST(test_a.at<0>() == 3);
+    BOOST_TEST(test_a.at<1>() == 2);
+    BOOST_TEST(test_a.at<2>() == 1);
+
+    test_a = {4, 5, 6};
+    BOOST_TEST(test_a[0] == 4);
+    BOOST_TEST(test_a[1] == 5);
+    BOOST_TEST(test_a[2] == 6);
+
+    BOOST_TEST(test_a.length() == sizeof(double) * 3);
+
+    auto view_a = test_a.get_view<0, 2>();
+    BOOST_TEST(view_a[0] == 4);
+    BOOST_TEST(view_a[1] == 5);
+    BOOST_TEST(view_a.at<0>() == 4);
+    BOOST_TEST(view_a.at<1>() == 5);
+
+    auto view_b = test_a.get_view<1, 3>();
+    BOOST_TEST(view_b[0] == 5);
+    BOOST_TEST(view_b[1] == 6);
+    BOOST_TEST(view_b.at<0>() == 5);
+    BOOST_TEST(view_b.at<1>() == 6);
+
+    view_b.at<1>() = 7;
+    BOOST_TEST(test_a.at<2>() == 7);
+
+    auto test_b = view_b.to_vectorx();
+    static_assert(std::is_same_v<decltype(test_b), vectorx<double, 2>>);
+    test_b.at<1>() = 8;
+    BOOST_TEST(view_b.at<1>() == 7);
+
+    const vector3d &const_test_a = test_a;
+    auto view_c = const_test_a.get_view<0, 2>();
+    BOOST_TEST(view_c[0] == 4);
+    BOOST_TEST(view_c[1] == 5);
+    BOOST_TEST(view_c.at<0>() == 4);
+    BOOST_TEST(view_c.at<1>() == 5);
+
+    auto test_c = view_c.to_vectorx();
+    static_assert(std::is_same_v<decltype(test_c), vectorx<double, 2>>);
+    test_c.at<1>() = 6;
+    BOOST_TEST(view_c.at<1>() == 5);
+
+    auto test_c_obj = view_c.to_vectorx_obj<high_freq_tag>();
+    BOOST_TEST(test_c_obj->at<0>() == 4);
+    BOOST_TEST(test_c_obj->at<1>() == 5);
+    test_c.at<1>() = 7;
+    BOOST_TEST(test_c_obj->at<0>() == 4);
+    BOOST_TEST(test_c_obj->at<1>() == 5);
+
+    const auto &test_d = test_c;
+
+    test_vectorx_view_func_1(view_b);
+//    test_vectorx_view_func_1(view_c);
+    test_vectorx_view_func_2(view_b);
+    test_vectorx_view_func_2(view_c);
+
+    test_vectorx_view_func_1(test_c);
+//    test_vectorx_view_func_1(test_d);
+    test_vectorx_view_func_2(test_c);
+    test_vectorx_view_func_2(test_d);
+
+    test_vectorx_view_func_1(test_c.get_view());
+//    test_vectorx_view_func_1(test_d.get_view());
+    test_vectorx_view_func_2(test_c.get_view());
+    test_vectorx_view_func_2(test_d.get_view());
+
+    test_a = {1, 2, 3};
+    auto eigen_column = test_a.get_view().as_eigen_column_view();
+    BOOST_TEST(eigen_column(0) == 1);
+    BOOST_TEST(eigen_column(1) == 2);
+    BOOST_TEST(eigen_column(2) == 3);
+
+    auto transposed = eigen_column.transpose();
+    transposed(1) = 3; // will affect origin
+    BOOST_TEST(eigen_column(1) == 3);
+    BOOST_TEST(test_a.at<1>() == 3);
+
+    Eigen::Vector3d real_vector = eigen_column;
+    real_vector(0) = 2; // will not affect origin
+    BOOST_TEST(eigen_column(0) == 1);
+    BOOST_TEST(test_a.at<0>() == 1);
+
+    auto eigen_const_column = test_a.get_view().as_eigen_const_column_view();
+    test_a = {1, 2, 3};
+    BOOST_TEST(eigen_const_column(0) == 1);
+    BOOST_TEST(eigen_const_column(1) == 2);
+    BOOST_TEST(eigen_const_column(2) == 3);
+
+    Eigen::Vector3d real_vector_2 = eigen_const_column;
+    real_vector(0) = 2; // will not affect origin
+    BOOST_TEST(eigen_const_column(0) == 1);
+    BOOST_TEST(test_a.at<0>() == 1);
+
+    test_a.get_view().fill({10}).fill({11}).fill({12});
+    BOOST_TEST(test_a[0] == 10);
+    BOOST_TEST(test_a[1] == 11);
+    BOOST_TEST(test_a[2] == 12);
+
+    test_a.get_view().fill({13, 14}).fill({15});
+    BOOST_TEST(test_a[0] == 13);
+    BOOST_TEST(test_a[1] == 14);
+    BOOST_TEST(test_a[2] == 15);
+
+    auto test_e = vectorx<double, 1>({16});
+    auto test_f = vectorx<double, 2>({17, 18});
+    test_a.get_view().fill(test_e).fill(test_f);
+    BOOST_TEST(test_a[0] == 16);
+    BOOST_TEST(test_a[1] == 17);
+    BOOST_TEST(test_a[2] == 18);
+
+    test_a = {1, 2, 3};
+    test_a.get_view().fill(test_e.get_view()).fill(test_f.get_view());
+    BOOST_TEST(test_a[0] == 16);
+    BOOST_TEST(test_a[1] == 17);
+    BOOST_TEST(test_a[2] == 18);
+
+    auto eigen_vec3 = Eigen::Vector3d(20, 21, 22);
+    test_a = eigen_vec3;
+    BOOST_TEST(test_a[0] == 20);
+    BOOST_TEST(test_a[1] == 21);
+    BOOST_TEST(test_a[2] == 22);
+
+    test_a = {1, 2, 3};
+    test_a.get_view().fill(eigen_vec3);
+    BOOST_TEST(test_a[0] == 20);
+    BOOST_TEST(test_a[1] == 21);
+    BOOST_TEST(test_a[2] == 22);
+
+}