mission_adapters/test/mission_test_utils.h

/*********************************************************************
 *
 * Helper dùng chung cho test của gói: dựng goal/order giả và chờ trạng thái.
 *
 *********************************************************************/
#ifndef MISSION_ADAPTERS_TEST_MISSION_TEST_UTILS_H_
#define MISSION_ADAPTERS_TEST_MISSION_TEST_UTILS_H_

#include <chrono>
#include <cstdint>
#include <mutex>
#include <string>
#include <thread>
#include <vector>

#include <mission_adapters/mission_adapters.h>

namespace mission_test
{

/**
 * @brief NavigationClient giả: ghi lại mọi lệnh xuống navigation để test kiểm thứ tự và số lần.
 *
 * Không mô phỏng chuyển động — test tự quyết định khi nào outcome quay về.
 */
class FakeNavigationClient : public mission_adapters::NavigationClient
{
public:
    bool dispatch(const std::shared_ptr<const mission_adapters::Mission>& mission) override
    {
        std::lock_guard<std::mutex> lock(mutex_);
        dispatched_.push_back(mission);
        return accept_dispatch_;
    }

    void cancelActive(mission_adapters::MissionId id) override
    {
        std::lock_guard<std::mutex> lock(mutex_);
        cancelled_.push_back(id);
    }

    /// Ép navigation từ chối mọi mission tiếp theo.
    void setAcceptDispatch(bool accept)
    {
        std::lock_guard<std::mutex> lock(mutex_);
        accept_dispatch_ = accept;
    }

    std::vector<std::shared_ptr<const mission_adapters::Mission>> dispatched() const
    {
        std::lock_guard<std::mutex> lock(mutex_);
        return dispatched_;
    }

    std::vector<mission_adapters::MissionId> cancelled() const
    {
        std::lock_guard<std::mutex> lock(mutex_);
        return cancelled_;
    }

    size_t dispatchCount() const
    {
        std::lock_guard<std::mutex> lock(mutex_);
        return dispatched_.size();
    }

private:
    mutable std::mutex mutex_;
    std::vector<std::shared_ptr<const mission_adapters::Mission>> dispatched_;
    std::vector<mission_adapters::MissionId> cancelled_;
    bool accept_dispatch_ = true;
};


/// @brief Goal hợp lệ: quaternion đã chuẩn hoá (w=1) để qua được validate() của adapter.
inline robot_geometry_msgs::PoseStamped makeGoal(double x, double y)
{
    robot_geometry_msgs::PoseStamped goal;
    goal.header.frame_id = "map";
    goal.pose.position.x = x;
    goal.pose.position.y = y;
    goal.pose.orientation.w = 1.0;
    return goal;
}

/// @brief Mission dựng thẳng, không qua adapter — dùng cho test của MissionManager/Executor.
inline std::shared_ptr<mission_adapters::Mission> makeMission(double x, double y)
{
    auto mission = std::make_shared<mission_adapters::Mission>();
    mission->type = mission_adapters::MissionType::SIMPLE_GOAL;
    mission->goal = makeGoal(x, y);
    return mission;
}

/// @brief n mission độc lập, goal khác nhau.
inline std::vector<std::shared_ptr<mission_adapters::Mission>> makeMissions(int count)
{
    std::vector<std::shared_ptr<mission_adapters::Mission>> missions;
    missions.reserve(static_cast<size_t>(count));
    for (int i = 0; i < count; ++i)
        missions.push_back(makeMission(i, i));

    return missions;
}

inline robot_protocol_msgs::Action makeAction(const std::string& id)
{
    robot_protocol_msgs::Action action;
    action.actionId = id;
    action.actionType = "TEST";
    return action;
}

/// @brief Node đã released (base). Order thật của fleet manager luôn có ít nhất phần base.
inline robot_protocol_msgs::Node makeNode(int sequence_id, bool add_action = false)
{
    robot_protocol_msgs::Node node;
    node.sequenceId = sequence_id;
    node.nodeId = "node_" + std::to_string(sequence_id);
    node.released = true;
    node.nodePosition.x = sequence_id;
    node.nodePosition.y = sequence_id;

    if (add_action)
        node.actions.push_back(makeAction("node_action_" + std::to_string(sequence_id)));

    return node;
}

inline robot_protocol_msgs::Edge makeEdge(int sequence_id, bool add_action = false)
{
    robot_protocol_msgs::Edge edge;
    edge.sequenceId = sequence_id;
    edge.edgeId = "edge_" + std::to_string(sequence_id);
    edge.released = true;

    if (add_action)
        edge.actions.push_back(makeAction("edge_action_" + std::to_string(sequence_id)));

    return edge;
}

inline robot_protocol_msgs::Order makeOrder(int node_count, const std::string& order_id = "order_1",
                                           std::uint32_t order_update_id = 0)
{
    robot_protocol_msgs::Order order;
    order.orderId = order_id;
    order.orderUpdateId = order_update_id;

    for (int i = 0; i < node_count; ++i)
        order.nodes.push_back(makeNode(i));

    for (int i = 0; i < node_count - 1; ++i)
        order.edges.push_back(makeEdge(i));

    return order;
}

inline bool waitForState(mission_adapters::MissionManager& manager,
                         mission_adapters::MissionState expected,
                         std::chrono::milliseconds timeout)
{
    const auto deadline = std::chrono::steady_clock::now() + timeout;
    while (std::chrono::steady_clock::now() < deadline)
    {
        if (manager.state() == expected)
            return true;

        std::this_thread::sleep_for(std::chrono::milliseconds(5));
    }

    return manager.state() == expected;
}

}  // namespace mission_test

#endif  // MISSION_ADAPTERS_TEST_MISSION_TEST_UTILS_H_