#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN  // in only one cpp file

#include "core/basic_obj_types.hpp"
#include "core/sophiar_pool.h"
#include "utility/debug_utility.hpp"
#include "utility/coro_worker.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);

    std::ifstream config_file("data/transform_tree_config.json");
    BOOST_TEST(config_file.is_open());
    auto config = nlohmann::json::parse(config_file);
    BOOST_TEST(initialize(config));

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

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

    global_context->run();
}