#define BOOST_TEST_DYN_LINK

#include "core/transform_tree.h"
#include "core/basic_obj_types.hpp"
#include "utility/debug_utility.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/asio/awaitable.hpp>
#include <boost/asio/co_spawn.hpp>
#include <boost/asio/detached.hpp>

#include <nlohmann/json.hpp>

#include <fstream>

using namespace nlohmann;
using namespace sophiar;

using boost::asio::awaitable;
using boost::asio::co_spawn;
using boost::asio::detached;

BOOST_AUTO_TEST_CASE(test_transform_tree) {

    spdlog::set_level(spdlog::level::trace);

    REGISTER_TYPE(transform_obj_watcher);
    REGISTER_TYPE(transform_tree);

    std::ifstream config_file("data/transform_tree_config.json");
    BOOST_TEST(config_file.is_open());

    using namespace std::chrono_literals;
    auto c_in_b_index = global_sophiar_manager.register_global_obj<transform_obj>("C_in_B");
    auto worker_a = make_interval_coro_worker(global_context, 1s, [=, cur_y = int(0)]() mutable -> awaitable<bool> {
        auto new_trans = Eigen::Isometry3d(Eigen::Translation3d(0, ++cur_y, 0));
        UPDATE_GLOBAL_OBJ_VALUE(transform_obj, c_in_b_index, std::move(new_trans));
        co_return true;
    });
    worker_a->run();

    auto d_in_root_index = global_sophiar_manager.register_global_obj<transform_obj>("D_in_Root");
    auto worker_b = make_interval_coro_worker(global_context, 2s, [=, cur_z = int(0)]() mutable -> awaitable<bool> {
        auto new_trans = Eigen::Isometry3d(Eigen::Translation3d(0, 0, --cur_z));
        UPDATE_GLOBAL_OBJ_VALUE(transform_obj, d_in_root_index, std::move(new_trans));
        co_return true;
    });
    worker_b->run();

    global_sophiar_manager.load_config_and_start(nlohmann::json::parse(config_file));

    global_context.run();
}