#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 "utility/versatile_buffer2.hpp"

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

#include <spdlog/spdlog.h>

#include <fstream>
#include <iostream>
#include <numbers>

#include "utility/assert_utility.h"

using namespace Eigen;
using namespace sophiar;
using namespace std::chrono_literals;

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

awaitable<void> collect_data() {
    static constexpr auto resolution = 32;
    static constexpr auto move_angle = 0.3;
    static constexpr auto move_distance = 75;

    static constexpr auto pi = std::numbers::pi;
    static constexpr auto step_angle = 2 * pi / resolution;

    auto flange_index = REQUIRE_VARIABLE(transform_obj, "flange_in_base");
    auto ref_index = REQUIRE_VARIABLE(transform_obj, "ref_in_tracker");
    auto is_moving_index = REQUIRE_VARIABLE(bool_obj, "robot_is_moving");
    auto target_index = REQUIRE_VARIABLE(transform_obj, "target_flange_pose");

    auto print_data = [=, ss = string_writer{}]() mutable {
        QUERY_VARIABLE(transform_obj, flange_index)->write_to(ss);
        std::cout << ss.get_string_and_reset() << std::endl;
        QUERY_VARIABLE(transform_obj, ref_index)->write_to(ss);
        std::cout << ss.get_string_and_reset() << std::endl;
    };

    // wait for robot ready
    co_await coro_wait_for_not_empty<transform_obj>(flange_index);
    co_await coro_wait_for_not_empty<transform_obj>(ref_index);

    // print initial states
    print_data();

    // start moving
    auto start_pose = QUERY_VARIABLE(transform_obj, flange_index);
    for (int i = 0; i < resolution; ++i) {
        auto angle = i * step_angle;
        auto rot_axis = Vector3d{cos(angle), sin(angle), 0};
        auto next_pose = transform_obj::new_instance();
        next_pose->value = start_pose->value
                           * Translation3d{move_distance * rot_axis}
                           * AngleAxisd{move_angle, rot_axis};
        UPDATE_VARIABLE(transform_obj, target_index, next_pose);
        // wait for movement to start and then finish
        co_await coro_wait_for_variable_value<bool_obj>(is_moving_index, true);
        co_await coro_wait_for_variable_value<bool_obj>(is_moving_index, false);
        // wait for robot to be stable
        co_await coro_sleep(1s);
        print_data();
    }

    // move back to start pose
    UPDATE_VARIABLE(transform_obj, target_index, start_pose);
    co_return;
}

int main() {
    spdlog::set_level(spdlog::level::trace);
    std::ifstream config_file("data/kuka_calibration_config.json");
    assert(config_file.is_open());
    auto config = nlohmann::json::parse(config_file);
    auto ok = initialize(config);
    assert(ok);
    co_spawn(*global_context, collect_data(), detached);
    global_context->run();
}