#include <mission_adapters/mission_executor.h>

#include <robot/robot.h>

namespace mission_adapters
{
    // ─────────────────────────────────────────────────────────────────────────
    // MissionExecutor
    // ─────────────────────────────────────────────────────────────────────────

    MissionExecutor::MissionExecutor(MissionManager& manager)
        : mission_manager_(manager)
    {}

    MissionExecutor::~MissionExecutor() { stop(); }

    void MissionExecutor::setNavigationClient(NavigationClient* client)
    {
        std::lock_guard<std::mutex> lock(mutex_);
        navigation_client_ = client;
    }

    void MissionExecutor::start()
    {
        if (running_) return;
        running_ = true;
        worker_  = std::thread(&MissionExecutor::spin, this);
    }

    void MissionExecutor::stop()
    {
        running_ = false;
        mission_manager_.wakeUp();   // gỡ thread ra khỏi waitForWork() để join() không treo
        if (worker_.joinable()) worker_.join();
    }

    void MissionExecutor::step()
    {
        NavigationClient* client = nullptr;
        {
            std::lock_guard<std::mutex> lock(mutex_);
            client = navigation_client_;
        }

        if (!client)
            return;

        // Cancel trước dispatch: nếu order mới vừa thay order cũ, robot phải được bảo dừng chặng cũ
        // trước khi nhận chặng mới. Đảo thứ tự sẽ có lúc hai lệnh cùng hiệu lực.
        const MissionId cancel_id = mission_manager_.takePendingCancel();
        if (cancel_id != kInvalidMissionId)
            client->cancelActive(cancel_id);

        const auto mission = mission_manager_.nextMission();
        if (!mission)
            return;

        if (!client->dispatch(mission))
        {
            // Navigation từ chối: coi như chặng này hỏng, đừng để mission kẹt RUNNING vĩnh viễn.
            robot::log_error("MissionExecutor: navigation rejected mission %lu",
                             static_cast<unsigned long>(mission->id));
            mission_manager_.onNavigationFailed(mission->id);
        }
    }

    void MissionExecutor::spin()
    {
        while (running_)
        {
            // M10: chờ đúng lúc có việc thay vì poll. Poll 20 Hz nghĩa là mỗi lệnh cancel phải
            // đợi trung bình 25 ms trước khi tới được navigation, không vì lý do gì cả.
            if (!mission_manager_.waitForWork())
                continue;

            step();
        }
    }

} // namespace mission_adapters
