#include "goal_source_adapter.h"

#include <cmath>

#include <boost/dll/alias.hpp>

namespace mission_plugins
{

mission_adapters::MissionSourceAdapter::Ptr GoalSourceAdapter::create()
{
    return std::make_shared<GoalSourceAdapter>();
}

bool GoalSourceAdapter::configure(const std::string& name, robot::NodeHandle& nh)
{
    (void)nh;   // nguồn này chưa có param riêng
    name_ = name;
    return true;
}

std::string GoalSourceAdapter::schema() const
{
    return mission_adapters::schema::kPoseStamped;
}

bool GoalSourceAdapter::validate(const mission_adapters::MissionRequest& request,
                                 std::string& reason) const
{
    if (!request.pose)
    {
        reason = "request is missing the pose payload";
        return false;
    }

    const auto& position = request.pose->pose.position;
    const auto& orientation = request.pose->pose.orientation;

    // NaN/Inf từ host phải bị chặn ngay tại biên: lọt xuống dưới thì mọi phép so khoảng cách tới
    // goal đều trả false và robot chạy tới khi có người bấm dừng.
    if (!std::isfinite(position.x) || !std::isfinite(position.y) || !std::isfinite(position.z))
    {
        reason = "goal contains NaN/Inf in position";
        return false;
    }

    if (!std::isfinite(orientation.x) || !std::isfinite(orientation.y) ||
        !std::isfinite(orientation.z) || !std::isfinite(orientation.w))
    {
        reason = "goal contains NaN/Inf in orientation";
        return false;
    }

    // Quaternion toàn 0 là lỗi hay gặp khi host quên set orientation — nó không phải "hướng bất kỳ",
    // nó là dữ liệu hỏng.
    const double norm_squared = orientation.x * orientation.x +
                                orientation.y * orientation.y +
                                orientation.z * orientation.z +
                                orientation.w * orientation.w;

    if (std::sqrt(norm_squared) < kMinQuaternionNorm)
    {
        reason = "goal has a quaternion that cannot be normalized (norm ~ 0)";
        return false;
    }

    return true;
}

mission_adapters::ConversionResult
GoalSourceAdapter::convert(const mission_adapters::MissionRequest& request)
{
    mission_adapters::ConversionResult result;

    if (!request.pose)
        return result;

    auto mission = std::make_shared<mission_adapters::Mission>();
    mission->type = mission_adapters::MissionType::SIMPLE_GOAL;
    mission->goal = *request.pose;
    mission->motion_hint = "position";

    // Goal đơn lẻ luôn thay việc đang chạy: người dùng bấm một đích mới nghĩa là bỏ đích cũ.
    result.mode = mission_adapters::SubmitMode::kReplace;
    result.missions.push_back(mission);
    return result;
}

}  // namespace mission_plugins

BOOST_DLL_ALIAS(mission_plugins::GoalSourceAdapter::create, GoalSourceAdapter)