#define BOOST_TEST_DYN_LINK

#include "core/transform_tree.h"

#include <boost/test/unit_test.hpp>

#include <nlohmann/json.hpp>

#include <fstream>

using namespace nlohmann;
using namespace sophiar;

BOOST_AUTO_TEST_CASE(test_transform_tree) {

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

    transform_tree tree;
    tree.load_construct_config(json::parse(config_file));

    auto index_c = tree.get_index_by_name("C");
    auto index_d = tree.get_index_by_name("D");
    auto query_result = tree.query_transform(index_d, index_c);
    BOOST_TEST(std::get<0>(query_result) == true);
    BOOST_TEST(std::get<1>(query_result).isApprox(
            (Eigen::Isometry3d) Eigen::Translation3d(3, 5, 7)));

    auto index_e = tree.get_index_by_name("E");
    query_result = tree.query_transform(index_e, index_c);
    BOOST_TEST(std::get<0>(query_result) == false);
    BOOST_TEST(std::get<1>(query_result).isApprox(
            (Eigen::Isometry3d) Eigen::Translation3d(4, 6, 8)));

}