mission_adapters/include/mission_adapters/mission_adapters.h

#ifndef MISSION_ADAPTERS_MISSION_ADAPTERS_H
#define MISSION_ADAPTERS_MISSION_ADAPTERS_H

#include <string>
#include <vector>
#include <queue>
#include <memory>
#include <thread>
#include <mutex>
#include <functional>
#include <atomic>

#include <robot/robot.h>
#include <robot_protocol_msgs/Order.h>
#include <robot_geometry_msgs/PoseStamped.h>

namespace mission_adapters
{
    // ── Priority constants ────────────────────────────────────────────────────
    // Số càng nhỏ = ưu tiên càng cao (min-heap)
    static constexpr int PRIORITY_EMERGENCY = 0;
    static constexpr int PRIORITY_CANCEL    = 1;
    static constexpr int PRIORITY_RESUME    = 2;
    static constexpr int PRIORITY_PAUSE     = 3;
    static constexpr int PRIORITY_NAV_DONE  = 4;
    // FIX #4: Dedicated constant — same value as NAV_DONE but semantically correct name.
    static constexpr int PRIORITY_NAV_FAILED = 4;
    static constexpr int PRIORITY_ORDER     = 5;

    enum class MissionState
    {
        IDLE,
        QUEUED,
        RUNNING,
        PAUSED,
        WAITING_ACTION,
        RECOVERY,
        COMPLETED,
        FAILED,
        CANCELLED,
        EMERGENCY
    };

    enum class EventType
    {
        SUBMIT_MISSIONS,
        NAV_DONE,
        NAV_FAILED,
        PAUSE,
        RESUME,
        CANCEL,
        EMERGENCY
    };

    enum class MissionType
    {
        SIMPLE_GOAL,
        VDA5050_ORDER
    };

    enum class ActionType
    {
        NODE_ACTION,
        EDGE_ACTION
    };

    class Action
    {
    public:
        int sequenceId;
        ActionType type;
        robot_protocol_msgs::Action action;
    };

    class Mission
    {
    public:
        int sequenceId;
        MissionType type;
        int priority;
        robot_geometry_msgs::PoseStamped start;
        robot_geometry_msgs::PoseStamped goal;
        std::vector<robot_protocol_msgs::Node> nodes;
        std::vector<robot_protocol_msgs::Edge> edges;
        std::vector<Action> actions;
    };

    struct Event
    {
        EventType type;
        int priority;
        std::vector<std::shared_ptr<Mission>> missions;
    };

    struct EventCompare
    {
        bool operator()(const Event& lhs, const Event& rhs) const
        {
            return lhs.priority > rhs.priority;
        }
    };

    class EventBus
    {
    private:
        mutable std::mutex mutex_;
        std::priority_queue<Event, std::vector<Event>, EventCompare> queue_;
        std::condition_variable cv_;
        bool stop_ = false;

    public:
        void push(const Event& event);
        bool pop(Event& event);
        void stop();
        void reset();

        inline bool empty() const
        {
            std::lock_guard<std::mutex> lock(mutex_);
            return queue_.empty();
        }

        inline void clear()
        {
            std::lock_guard<std::mutex> lock(mutex_);
            while (!queue_.empty()) queue_.pop();
        }
    };

    class VDA5050Adapter
    {
    public:
        std::vector<std::shared_ptr<Mission>> convert(const robot_protocol_msgs::Order& order);
    };

    class GoalAdapter
    {
    public:
        std::vector<std::shared_ptr<Mission>> convert(const robot_geometry_msgs::PoseStamped& goal);
    };

    class MissionManager
    {
    public:
        void submit(const std::vector<std::shared_ptr<Mission>>& missions);

        std::shared_ptr<Mission> nextMission();

        void onNavigationDone();
        void onNavigationFailed();
        void cancel();
        void pause();
        void resume();
        void emergency();

        MissionState state() const;

        // FIX #3: Returns true if there is any pending work — queue OR active mission.
        bool hasMission() const;

    private:
        mutable std::mutex mutex_;
        MissionState state_ = MissionState::IDLE;
        std::queue<std::shared_ptr<Mission>> mission_queue_;
        std::shared_ptr<Mission> current_mission_;
    };

    class EventProcessor
    {
    public:
        explicit EventProcessor(MissionManager& mission_manager);
        ~EventProcessor();

        void start();
        void stop();

        void goalEvent(const robot_geometry_msgs::PoseStamped& goal);
        void orderEvent(const robot_protocol_msgs::Order& order);
        void cancelEvent();
        void pauseEvent();
        void resumeEvent();
        void navDoneEvent();
        void navFailedEvent();
        void emergencyEvent();

    private:
        void spin();
        void process(const Event& event);

    private:
        EventBus event_bus_;
        GoalAdapter goal_adapter_;
        VDA5050Adapter vda5050_adapter_;
        MissionManager& mission_manager_;
        std::thread worker_;
        std::atomic<bool> running_{false};
    };

    class MissionExecutor
    {
    public:
        using MissionCallback = std::function<void(const std::shared_ptr<Mission>&)>;

        explicit MissionExecutor(MissionManager& manager);
        ~MissionExecutor();

        void start();
        void stop();

        // FIX #1: Must be called BEFORE start(), not inside the run loop.
        void setMissionCallback(MissionCallback cb)
        {
            std::lock_guard<std::mutex> lock(mutex_);
            mission_callback_ = std::move(cb);
        }

    private:
        std::shared_ptr<Mission> mission_execute_;

        // Last mission that was dispatched to the callback.
        std::shared_ptr<Mission> last_dispatched_mission_;

        MissionCallback mission_callback_;
        MissionManager& mission_manager_;
        mutable std::mutex mutex_;
        std::thread worker_;
        std::atomic<bool> running_{false};

        void spin();
    };

} // namespace mission_adapters

#endif  // MISSION_ADAPTERS_MISSION_ADAPTERS_H