#define BOOST_TEST_DYN_LINK

#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_fracture_robot_direct_control() {
    auto move_index = REQUIRE_VARIABLE(bool_obj, "robot_is_moving");
    auto target_index = REQUIRE_VARIABLE(array6_obj, "motor_velocity");
    co_await coro_wait_for_not_empty<bool_obj>(move_index);
    BOOST_TEST(QUERY_VARIABLE(bool_obj, move_index)->value == false);
    for (int i = 0; i < 100; ++i) {
        auto motor_speed = array6_obj::new_instance();
        motor_speed->value.fill(5);
        UPDATE_VARIABLE(array6_obj, target_index, motor_speed);
        co_await coro_sleep(20ms);
    }
    co_return;
}

BOOST_AUTO_TEST_CASE(test_fracture_robot_interface) {
    spdlog::set_level(spdlog::level::trace);
    std::ifstream config_file("data/fracture_robot_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_fracture_robot_direct_control(), detached);
    global_context->run();
}