/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * Thread lấy mission kế tiếp từ MissionManager và đẩy xuống phía navigation.
 *
 * Author: DuongTD
 *********************************************************************/
#ifndef MISSION_ADAPTERS_MISSION_EXECUTOR_H_
#define MISSION_ADAPTERS_MISSION_EXECUTOR_H_

#include <atomic>
#include <memory>
#include <mutex>
#include <thread>

#include <mission_adapters/mission_manager.h>
#include <mission_adapters/navigation_client.h>
#include <mission_adapters/types.h>

namespace mission_adapters
{
    /**
     * @class MissionExecutor
     * @brief Thread duy nhất phát lệnh xuống navigation.
     *
     * Mọi lời gọi ra ngoài (dispatch, cancelActive) đi ra từ đây, nên MissionManager không bao giờ
     * gọi ra ngoài khi đang giữ khoá. Thứ tự trong một vòng là bất biến an toàn: **cancel trước,
     * dispatch sau** — chặng cũ phải được bảo dừng trước khi chặng mới bắt đầu.
     */
    class MissionExecutor
    {
    public:
        explicit MissionExecutor(MissionManager& manager);
        ~MissionExecutor();

        /**
         * @brief Gắn cổng navigation. Con trỏ non-owning, phải sống lâu hơn executor.
         *
         * FIX #1: gọi TRƯỚC start(), không gọi trong vòng lặp.
         */
        void setNavigationClient(NavigationClient* client);

        void start();
        void stop();

    private:
        void spin();

        /// Một vòng công việc. Tách riêng để test gọi trực tiếp, không cần thread.
        void step();

        NavigationClient* navigation_client_ = nullptr;   ///< non-owning, có thể null
        MissionManager& mission_manager_;
        mutable std::mutex mutex_;
        std::thread worker_;
        std::atomic<bool> running_{false};
    };

} // namespace mission_adapters

#endif  // MISSION_ADAPTERS_MISSION_EXECUTOR_H_
