#ifndef SOPHIAR2_FIVE_DOF_OFFSET_CALCULATOR_HPP
#define SOPHIAR2_FIVE_DOF_OFFSET_CALCULATOR_HPP

#include "core/basic_obj_types.hpp"
#include "utility/config_utility.hpp"
#include "utility/coro_signal_group.hpp"
#include "utility/coro_worker.hpp"
#include "utility/simple_tristate_obj.hpp"
#include "utility/variable_helper.hpp"

namespace sophiar {

    // Fix origin and Z-axis only
    inline coro_worker::pointer make_5dof_offset_calculator_func(const nlohmann::json &config) {
        auto cur_index = LOAD_VARIABLE_INDEX(transform_obj, "current_transform_var");
        auto tar_index = LOAD_VARIABLE_INDEX(transform_obj, "target_transform_var");
        auto offset_index = LOAD_VARIABLE_INDEX(transform_obj, "offset_transform_var");

        auto signal_group = coro_signal_any_group::new_instance();
        signal_group->add_watcher(REQUIRE_VARIABLE_WATCHER(cur_index));
        signal_group->add_watcher(REQUIRE_VARIABLE_WATCHER(tar_index));
        auto signal_watcher = signal_group->new_watcher();
        signal_group->start();
        auto exit_func = SIGNAL_GROUP_AUTO_CLOSER(signal_group);

        auto worker = make_infinite_coro_worker(
                [=,
                        watcher = std::move(signal_watcher)]() mutable
                        -> boost::asio::awaitable<bool> {
                    co_await watcher.coro_wait(false);
                    auto ts = watcher.get_last_update_ts();
                    auto cur_trans = QUERY_VARIABLE(transform_obj, cur_index);
                    auto tar_trans = QUERY_VARIABLE(transform_obj, tar_index);
                    if (cur_trans == nullptr || tar_trans == nullptr) [[unlikely]] {
                        UPDATE_VARIABLE_WITH_TS(transform_obj, offset_index, nullptr, ts);
                    } else [[likely]] {
                        auto dof6_trans = cur_trans->value.inverse() * tar_trans->value;
                        auto z_axis = Eigen::Vector3d::UnitZ();
                        auto new_z_axis = dof6_trans.linear() * z_axis;
                        auto dof5_rot = Eigen::Quaterniond::FromTwoVectors(z_axis, new_z_axis);
                        auto dof5_trans = Eigen::Translation3d{dof6_trans.translation()};
                        auto offset = transform_obj::new_instance();
                        offset->value = dof5_trans * dof5_rot;
                        UPDATE_VARIABLE_WITH_TS(transform_obj, offset_index, offset, ts);
                    }
                    co_return true;
                }, std::move(exit_func));

        return std::move(worker);
    }

}

#endif //SOPHIAR2_FIVE_DOF_OFFSET_CALCULATOR_HPP