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

#include "core/basic_obj_types.hpp"
#include "core/global_defs.h"
#include "core/sophiar_pool.h"
#include "utility/debug_utility.hpp"
#include "utility/variable_helper.hpp"

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

#include <spdlog/spdlog.h>

#include <fstream>
#include <numbers>

using namespace sophiar;
using namespace std::chrono_literals;

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

awaitable<void> test_joint_angle_control() {
    auto joint_angle = VARIABLE_AUTO_DELEGATE(array6_obj, "ur_joint_actual_q");
    auto var_index = REQUIRE_VARIABLE(bool_obj, "robot_is_moving");
    auto target_index = REQUIRE_VARIABLE(array6_obj, "ur_target_joint_q");
    co_await coro_wait_for_variable_value<bool_obj>(var_index, false);
    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
    auto next_joint_angle = array6_obj::new_instance();
    next_joint_angle->value = joint_angle->value;
    next_joint_angle->value[0] += std::numbers::pi;
    UPDATE_VARIABLE(array6_obj, target_index, next_joint_angle);
    co_await coro_wait_for_variable_value<bool_obj>(var_index, true);
    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == true);
    co_await coro_wait_for_variable_value<bool_obj>(var_index, false);
    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
    next_joint_angle = array6_obj::new_instance();
    next_joint_angle->value = joint_angle->value;
    next_joint_angle->value[0] -= std::numbers::pi;
    UPDATE_VARIABLE(array6_obj, target_index, next_joint_angle);
    co_await coro_wait_for_variable_value<bool_obj>(var_index, true);
    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == true);
    co_await coro_wait_for_variable_value<bool_obj>(var_index, false);
    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
    co_return;
}

awaitable<void> test_joint_speed_control() {
    auto var_index = REQUIRE_VARIABLE(bool_obj, "robot_is_moving");
    auto target_index = REQUIRE_VARIABLE(array6_obj, "ur_target_joint_qd");
    co_await coro_wait_for_variable_value<bool_obj>(var_index, false);
    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
    for (int i = 0; i < 40; ++i) {
        auto next_joint_speed = array6_obj::new_instance();
        next_joint_speed->value.fill({});
        next_joint_speed->value[0] = 0.2;
        UPDATE_VARIABLE(array6_obj, target_index, next_joint_speed);
        co_await coro_sleep(50ms);
    }
    co_await coro_wait_for_variable_value<bool_obj>(var_index, false);
    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
    for (int i = 0; i < 40; ++i) {
        auto next_joint_speed = array6_obj::new_instance();
        next_joint_speed->value.fill({});
        next_joint_speed->value[0] = -0.2;
        UPDATE_VARIABLE(array6_obj, target_index, next_joint_speed);
        co_await coro_sleep(50ms);
    }
    co_await coro_wait_for_variable_value<bool_obj>(var_index, false);
    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
    co_return;
}

awaitable<void> test_tcp_pose_control() {
    auto tcp_pose = VARIABLE_AUTO_DELEGATE(transform_obj, "robot_tcp_actual_pose");
    auto var_index = REQUIRE_VARIABLE(bool_obj, "robot_is_moving");
    auto target_index = REQUIRE_VARIABLE(transform_obj, "robot_target_tcp_pose");
    co_await coro_wait_for_variable_value<bool_obj>(var_index, false);
    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
    auto next_tcp_pose = transform_obj::new_instance();
    next_tcp_pose->value = tcp_pose->value;
    next_tcp_pose->value.translation().z() += 0.1;
    UPDATE_VARIABLE(transform_obj, target_index, next_tcp_pose);
    co_await coro_wait_for_variable_value<bool_obj>(var_index, true);
    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == true);
    co_await coro_wait_for_variable_value<bool_obj>(var_index, false);
    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
    next_tcp_pose = transform_obj::new_instance();
    next_tcp_pose->value = tcp_pose->value;
    next_tcp_pose->value.translation().z() -= 0.1;
    UPDATE_VARIABLE(transform_obj, target_index, next_tcp_pose);
    co_await coro_wait_for_variable_value<bool_obj>(var_index, true);
    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == true);
    co_await coro_wait_for_variable_value<bool_obj>(var_index, false);
    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
    co_return;
}

awaitable<void> test_tcp_speed_control() {
    // set custom tcp
    auto tcp_offset_index = REQUIRE_VARIABLE(transform_obj, "robot_tcp_offset_pose");
    UPDATE_VARIABLE_VAL(transform_obj, tcp_offset_index, Eigen::Translation3d(0, 0, 0.15));

    auto var_index = REQUIRE_VARIABLE(bool_obj, "robot_is_moving");
    auto target_index = REQUIRE_VARIABLE(array6_obj, "robot_target_tcp_speed");
    co_await coro_wait_for_variable_value<bool_obj>(var_index, false);
    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
    for (int i = 0; i < 40; ++i) {
        auto next_tcp_speed = array6_obj::new_instance();
        next_tcp_speed->value.fill({});
        next_tcp_speed->value[3] = 0.2;
        UPDATE_VARIABLE(array6_obj, target_index, next_tcp_speed);
        co_await coro_sleep(50ms);
    }
    co_await coro_wait_for_variable_value<bool_obj>(var_index, false);
    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
    for (int i = 0; i < 40; ++i) {
        auto next_tcp_speed = array6_obj::new_instance();
        next_tcp_speed->value.fill({});
        next_tcp_speed->value[3] = -0.2;
        UPDATE_VARIABLE(array6_obj, target_index, next_tcp_speed);
        co_await coro_sleep(50ms);
    }
    co_await coro_wait_for_variable_value<bool_obj>(var_index, false);
    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
    co_return;
}

BOOST_AUTO_TEST_CASE(test_ur_interface) {
    spdlog::set_level(spdlog::level::trace);
    std::ifstream config_file("data/ur_interface_config.json");
    BOOST_TEST(config_file.is_open());
    auto config = nlohmann::json::parse(config_file);
    BOOST_TEST(initialize(config));
    co_spawn(*global_context, test_tcp_pose_control(), detached);
    global_context->run();
}