#include <mission_adapters/mission_manager.h>

#include <chrono>

#include <robot/robot.h>

namespace mission_adapters
{
namespace
{
    /**
     * @brief Đánh thức thread đang chờ việc khi rời scope, kể cả khi hàm return sớm.
     *
     * Khai báo ngay sau std::lock_guard nên nó bị huỷ TRƯỚC lock_guard: notify xảy ra khi vẫn còn
     * giữ khoá — an toàn, và không có đường nào ra khỏi hàm mà quên đánh thức executor.
     */
    class NotifyOnExit
    {
    public:
        explicit NotifyOnExit(std::condition_variable& cv) : cv_(cv) {}
        ~NotifyOnExit() { cv_.notify_all(); }

        NotifyOnExit(const NotifyOnExit&) = delete;
        NotifyOnExit& operator=(const NotifyOnExit&) = delete;

    private:
        std::condition_variable& cv_;
    };

}  // namespace

    // ─────────────────────────────────────────────────────────────────────────
    // MissionManager
    // ─────────────────────────────────────────────────────────────────────────

    void MissionManager::setConfig(const MissionConfig& config)
    {
        std::lock_guard<std::mutex> lock(mutex_);
        NotifyOnExit notify(work_cv_);
        config_ = config;
    }

    bool MissionManager::hasWorkLocked() const
    {
        return pending_cancel_ != kInvalidMissionId ||
               (state_ == MissionState::QUEUED && !mission_queue_.empty());
    }

    bool MissionManager::timeoutArmedLocked() const
    {
        return config_.mission_timeout > 0.0 &&
               state_ == MissionState::RUNNING &&
               current_mission_ != nullptr;
    }

    MissionId MissionManager::currentIdLocked() const
    {
        return current_mission_ ? current_mission_->id : kInvalidMissionId;
    }

    void MissionManager::setStateLocked(MissionState next, const char* reason, MissionId id)
    {
        if (state_ == next)
            return;   // C11: chỉ log khi state đổi thật, không log lặp trong vòng lặp

        robot::log_info("MissionManager: %s -> %s (%s, mission %lu)",
                        toString(state_), toString(next), reason,
                        static_cast<unsigned long>(id));

        state_ = next;
    }

    void MissionManager::expireCurrentLocked()
    {
        if (!current_mission_)
            return;

        robot::log_warning("MissionManager: mission %lu exceeded %.3f [s] — cancelling the leg",
                           static_cast<unsigned long>(current_mission_->id),
                           config_.mission_timeout);

        // Khác NAV_FAILED: navigation vẫn đang chạy chặng này nên phải bảo nó dừng, nếu không robot
        // tiếp tục đi tới goal của một mission mà mission layer đã bỏ.
        const MissionId expired_id = currentIdLocked();
        requestCancelOfCurrentLocked();

        if (config_.clear_queue_on_failure)
            while (!mission_queue_.empty()) mission_queue_.pop();

        setStateLocked(mission_queue_.empty() ? MissionState::FAILED : MissionState::QUEUED,
                       "mission_timeout", expired_id);
    }

    void MissionManager::requestCancelOfCurrentLocked()
    {
        if (!current_mission_)
            return;

        pending_cancel_ = current_mission_->id;
        current_mission_.reset();
    }

    void MissionManager::submit(const std::vector<std::shared_ptr<Mission>>& missions)
    {
        // A1: rỗng = không có việc, KHÔNG phải lệnh xoá hàng đợi. Kiểm tra trước khi lấy khoá để
        // một order lỗi không bao giờ chạm được vào trạng thái.
        if (missions.empty())
            return;

        std::lock_guard<std::mutex> lock(mutex_);
        NotifyOnExit notify(work_cv_);

        if (state_ == MissionState::EMERGENCY)
        {
            robot::log_warning("MissionManager: rejecting submit while in EMERGENCY");
            return;
        }

        // Q1 — preempt tường minh: order mới thay order cũ thì chặng đang chạy phải được dừng,
        // không chỉ bị quên đi.
        requestCancelOfCurrentLocked();

        mission_queue_ = {};

        for (const auto& mission : missions)
        {
            if (!mission)
                continue;

            mission->id = next_mission_id_++;
            mission_queue_.push(mission);
        }

        if (mission_queue_.empty())
            return;

        // PAUSED được giữ nguyên: người vận hành đã chủ động dừng, order mới không được tự chạy.
        if (state_ != MissionState::PAUSED)
            setStateLocked(MissionState::QUEUED, "submit", mission_queue_.back()->id);
    }

    void MissionManager::append(const std::vector<std::shared_ptr<Mission>>& missions)
    {
        if (missions.empty())
            return;

        std::lock_guard<std::mutex> lock(mutex_);
        NotifyOnExit notify(work_cv_);

        if (state_ == MissionState::EMERGENCY)
        {
            robot::log_warning("MissionManager: rejecting append while in EMERGENCY");
            return;
        }

        for (const auto& mission : missions)
        {
            if (!mission)
                continue;

            mission->id = next_mission_id_++;
            mission_queue_.push(mission);
        }

        // RUNNING / PAUSED / QUEUED giữ nguyên: đây là phần nối tiếp, không phải yêu cầu mới.
        switch (state_)
        {
            case MissionState::IDLE:
            case MissionState::COMPLETED:
            case MissionState::FAILED:
            case MissionState::CANCELLED:
            case MissionState::CLEAR_EMERGENCY:
                setStateLocked(MissionState::QUEUED, "append", mission_queue_.back()->id);
                break;
            default:
                break;
        }
    }

    std::shared_ptr<const Mission> MissionManager::nextMission()
    {
        std::lock_guard<std::mutex> lock(mutex_);

        // M6: chỉ QUEUED mới sinh ra mission mới. RUNNING nghĩa là chặng hiện tại chưa xong nên
        // không có gì để giao thêm — dedup nằm ở đây, bên gọi không phải tự nhớ.
        if (state_ != MissionState::QUEUED)
            return nullptr;

        if (mission_queue_.empty())
        {
            setStateLocked(MissionState::IDLE, "queue is empty");
            return nullptr;
        }

        current_mission_ = mission_queue_.front();
        mission_queue_.pop();
        setStateLocked(MissionState::RUNNING, "dequeue", current_mission_->id);
        mission_start_time_ = robot::Time::now();   // mốc tính mission_timeout

        return current_mission_;
    }

    bool MissionManager::onNavigationDone(MissionId id)
    {
        std::lock_guard<std::mutex> lock(mutex_);
        NotifyOnExit notify(work_cv_);

        if (state_ != MissionState::RUNNING || !current_mission_)
            return false;

        // A2: outcome trễ của một mission đã bị thay thế không được tính cho mission đang chạy.
        if (id != current_mission_->id)
        {
            robot::log_warning("MissionManager: dropping NAV_DONE of mission %lu (currently "
                               "running %lu)",
                               static_cast<unsigned long>(id),
                               static_cast<unsigned long>(current_mission_->id));
            return false;
        }

        current_mission_.reset();
        setStateLocked(mission_queue_.empty() ? MissionState::COMPLETED : MissionState::QUEUED,
                       "nav_done", id);
        return true;
    }

    bool MissionManager::onNavigationFailed(MissionId id)
    {
        std::lock_guard<std::mutex> lock(mutex_);
        NotifyOnExit notify(work_cv_);

        if (state_ != MissionState::RUNNING || !current_mission_)
            return false;

        if (id != current_mission_->id)
        {
            robot::log_warning("MissionManager: dropping NAV_FAILED of mission %lu (currently "
                               "running %lu)",
                               static_cast<unsigned long>(id),
                               static_cast<unsigned long>(current_mission_->id));
            return false;
        }

        current_mission_.reset();

        // M8: mặc định xoá sạch hàng đợi. Với tuyến đường tuần tự, chặng n hỏng nghĩa là robot
        // không tới được node n — chạy tiếp chặng n+1 là cắt ngang đoạn chưa được cho phép đi.
        if (config_.clear_queue_on_failure)
            while (!mission_queue_.empty()) mission_queue_.pop();

        setStateLocked(mission_queue_.empty() ? MissionState::FAILED : MissionState::QUEUED,
                       "nav_failed", id);
        return true;
    }

    void MissionManager::cancel()
    {
        std::lock_guard<std::mutex> lock(mutex_);
        NotifyOnExit notify(work_cv_);

        // A3: huỷ phải dừng được robot, không chỉ xoá hàng đợi trong bộ nhớ.
        const MissionId cancelled_id = currentIdLocked();
        requestCancelOfCurrentLocked();
        while (!mission_queue_.empty()) mission_queue_.pop();

        if (state_ == MissionState::EMERGENCY) return;
        setStateLocked(MissionState::CANCELLED, "cancel", cancelled_id);
    }

    void MissionManager::emergency()
    {
        std::lock_guard<std::mutex> lock(mutex_);
        NotifyOnExit notify(work_cv_);

        const MissionId stopped_id = currentIdLocked();
        requestCancelOfCurrentLocked();
        while (!mission_queue_.empty()) mission_queue_.pop();
        setStateLocked(MissionState::EMERGENCY, "emergency", stopped_id);
    }

    void MissionManager::clearEmergency()
    {
        std::lock_guard<std::mutex> lock(mutex_);
        NotifyOnExit notify(work_cv_);

        if (state_ == MissionState::EMERGENCY)
            setStateLocked(MissionState::CLEAR_EMERGENCY, "clear_emergency");
    }

    void MissionManager::pause()
    {
        std::lock_guard<std::mutex> lock(mutex_);
        NotifyOnExit notify(work_cv_);

        if (state_ == MissionState::IDLE    ||
            state_ == MissionState::RUNNING ||
            state_ == MissionState::QUEUED)
            setStateLocked(MissionState::PAUSED, "pause", currentIdLocked());
    }

    void MissionManager::resume()
    {
        std::lock_guard<std::mutex> lock(mutex_);
        NotifyOnExit notify(work_cv_);

        if (state_ != MissionState::PAUSED) return;

        if (current_mission_)
            setStateLocked(MissionState::RUNNING, "resume", current_mission_->id);
        else if (!mission_queue_.empty())
            setStateLocked(MissionState::QUEUED, "resume");
        else
            setStateLocked(MissionState::IDLE, "resume");
    }

    MissionState MissionManager::state() const
    {
        std::lock_guard<std::mutex> lock(mutex_);
        return state_;
    }

    // FIX #3: hasMission now reflects ALL pending work, not just the queue.
    bool MissionManager::hasMission() const
    {
        std::lock_guard<std::mutex> lock(mutex_);
        return current_mission_ != nullptr || !mission_queue_.empty();
    }

    MissionId MissionManager::currentMissionId() const
    {
        std::lock_guard<std::mutex> lock(mutex_);
        return current_mission_ ? current_mission_->id : kInvalidMissionId;
    }

    MissionId MissionManager::takePendingCancel()
    {
        std::lock_guard<std::mutex> lock(mutex_);
        const MissionId id = pending_cancel_;
        pending_cancel_ = kInvalidMissionId;
        return id;
    }

    bool MissionManager::waitForWork()
    {
        std::unique_lock<std::mutex> lock(mutex_);

        while (true)
        {
            if (wake_requested_)
            {
                wake_requested_ = false;
                return false;
            }

            if (hasWorkLocked())
                return true;

            if (!timeoutArmedLocked())
            {
                work_cv_.wait(lock);
                continue;
            }

            // M7: đo bằng robot::Time, không đếm chu kỳ — số chu kỳ không nói gì về thời gian thật
            // khi vòng lặp bị nghẽn.
            const double remaining =
                (mission_start_time_ + robot::Duration(config_.mission_timeout) - robot::Time::now())
                    .toSec();

            if (remaining <= 0.0)
            {
                expireCurrentLocked();
                continue;
            }

            work_cv_.wait_for(lock, std::chrono::duration<double>(remaining));
        }
    }

    void MissionManager::wakeUp()
    {
        {
            std::lock_guard<std::mutex> lock(mutex_);
            wake_requested_ = true;
        }
        work_cv_.notify_all();
    }

} // namespace mission_adapters
