/*********************************************************************
 *
 * Ví dụ tối thiểu: nối mission layer vào một navigation runtime.
 *
 * Điểm chính của ví dụ là hai chiều dữ liệu, không phải thuật toán:
 *
 *   host  --goalEvent/orderEvent-->  EventProcessor  -->  MissionManager
 *                                                              |
 *                                                     MissionExecutor
 *                                                              |
 *                                            NavigationClient::dispatch/cancelActive
 *                                                              |
 *   host  <--navDoneEvent(id)/navFailedEvent(id)--------  navigation runtime
 *
 * Outcome quay về BẮT BUỘC mang MissionId. Bản trước của ví dụ này poll trường trạng thái của
 * navigation rồi suy ra "chặng nào vừa xong" — cách đó gán nhầm kết quả ngay khi có một order thay
 * thế order đang chạy.
 *
 *********************************************************************/
#include <cstddef>
#include <memory>

#include <robot/robot.h>

#include <mission_adapters/mission_adapters.h>
#include <mission_adapters/plugin_registry.h>

namespace
{

/**
 * @brief NavigationClient chỉ ghi log — chỗ để thay bằng runtime thật.
 *
 * Trong hệ thống thật, dispatch() dựng yêu cầu di chuyển từ mission rồi giao cho navigation runtime,
 * và runtime báo ngược kết quả qua navDoneEvent(mission->id) / navFailedEvent(mission->id).
 */
class LoggingNavigationClient : public mission_adapters::NavigationClient
{
public:
    bool dispatch(const std::shared_ptr<const mission_adapters::Mission>& mission) override
    {
        if (!mission)
            return false;

        robot::log_info("dispatch mission %lu — %zu action",
                        static_cast<unsigned long>(mission->id),
                        mission->actions.size());
        return true;
    }

    void cancelActive(mission_adapters::MissionId id) override
    {
        robot::log_info("cancel mission %lu", static_cast<unsigned long>(id));
    }
};

}  // namespace

int main(int argc, char** argv)
{
    robot::init(argc, argv, "mission_adapters_example");

    // Nguồn mission được nạp từ YAML: thêm loại nguồn mới không phải sửa file này.
    mission_adapters::PluginRegistry registry;
    robot::NodeHandle nh;
    if (!registry.loadFromConfig(nh))
    {
        robot::log_error("could not load enough mission sources — check "
                         "mission_adapters_params.yaml");
        return 1;
    }

    mission_adapters::MissionManager  mission_manager;
    mission_adapters::EventProcessor  event_processor(mission_manager, registry);
    mission_adapters::MissionExecutor mission_executor(mission_manager);

    LoggingNavigationClient navigation_client;
    mission_executor.setNavigationClient(&navigation_client);

    event_processor.start();
    mission_executor.start();

    // Nguồn mission thật (MQTT/OPC-UA/REST) gọi các hàm *Event() này từ thread của nó.
    robot_geometry_msgs::PoseStamped goal;
    goal.header.frame_id = "map";
    goal.pose.position.x = 2.0;   // [m]
    goal.pose.position.y = 1.0;   // [m]
    event_processor.goalEvent(goal);

    robot::Rate rate(20);   // [Hz]
    while (robot::ok())
    {
        // Runtime thật gọi navDoneEvent(id) / navFailedEvent(id) ở đây, với id lấy từ chính mission
        // mà dispatch() đã nhận — không suy đoán từ trạng thái navigation.
        rate.sleep();
    }

    mission_executor.stop();
    event_processor.stop();
    return 0;
}