/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * Trạng thái và hàng đợi mission — nơi duy nhất giữ MissionState.
 *
 * Author: DuongTD
 *********************************************************************/
#ifndef MISSION_ADAPTERS_MISSION_MANAGER_H_
#define MISSION_ADAPTERS_MISSION_MANAGER_H_

#include <condition_variable>
#include <memory>
#include <mutex>
#include <queue>
#include <vector>

#include <robot/time.h>

#include <mission_adapters/mission_config.h>
#include <mission_adapters/types.h>

namespace mission_adapters
{
    /**
     * @class MissionManager
     * @brief Hàng đợi mission và trạng thái của nó. Thread-safe, không tự gọi ra ngoài.
     *
     * Manager cố ý KHÔNG giữ con trỏ tới navigation: nó chỉ ghi nhận "mission này cần được dừng"
     * (@ref takePendingCancel) và để MissionExecutor thực hiện lời gọi đó. Nhờ vậy không có lời gọi
     * ra ngoài nào xảy ra khi đang giữ khoá, và mọi lệnh xuống navigation đi ra từ đúng một thread.
     */
    class MissionManager
    {
    public:
        MissionManager() = default;
        explicit MissionManager(const MissionConfig& config) : config_(config) {}

        /// @brief Đổi tham số vận hành. Gọi trước khi có mission nào đang chạy.
        void setConfig(const MissionConfig& config);

        /**
         * @brief Nhận một loạt mission mới và cấp @ref MissionId cho từng cái.
         *
         * Semantics (A1): danh sách rỗng là **no-op tuyệt đối** — không đụng hàng đợi, không đổi
         * trạng thái. Một order lỗi không phải là lệnh huỷ việc đang chạy.
         *
         * Submit khi đang RUNNING là **preempt tường minh**: mission đang chạy được ghi vào ô
         * pending-cancel để navigation được bảo dừng, rồi hàng đợi mới thay hàng đợi cũ.
         */
        void submit(const std::vector<std::shared_ptr<Mission>>& missions);

        /**
         * @brief Nối thêm mission vào cuối hàng đợi, KHÔNG đụng chặng đang chạy.
         *
         * Dùng cho phần nối tiếp của một yêu cầu đang chạy (VDA5050 order update release thêm
         * horizon). Khác @ref submit ở chỗ không preempt và không yêu cầu dừng navigation — tuyến
         * đường không đổi thì robot không có lý do gì phải dừng lại.
         */
        void append(const std::vector<std::shared_ptr<Mission>>& missions);

        /**
         * @brief Lấy mission kế tiếp cần chạy.
         * @return Mission MỚI, hoặc nullptr nếu chưa tới lượt.
         *
         * Chỉ trả khác null đúng một lần cho mỗi mission, tại đúng bước chuyển QUEUED -> RUNNING
         * (M6). Bên gọi vì thế không cần tự khử trùng lặp.
         */
        std::shared_ptr<const Mission> nextMission();

        /**
         * @brief Ghi nhận một chặng hoàn tất (cả navigation lẫn action — D8).
         * @param id Mission vừa xong.
         * @return false nếu outcome không thuộc mission đang chạy (đến trễ sau khi bị thay thế).
         */
        bool onNavigationDone(MissionId id);

        /**
         * @brief Ghi nhận một chặng thất bại.
         * @return false nếu outcome không thuộc mission đang chạy.
         */
        bool onNavigationFailed(MissionId id);

        void cancel();
        void pause();
        void resume();
        void emergency();
        void clearEmergency();

        MissionState state() const;

        /// @brief Có việc đang treo hay không — hàng đợi HOẶC mission đang chạy.
        bool hasMission() const;

        /// @brief Mission đang chạy, kInvalidMissionId nếu không có.
        MissionId currentMissionId() const;

        /**
         * @brief Lấy và xoá yêu cầu dừng navigation đang treo.
         * @return kInvalidMissionId nếu không có yêu cầu nào.
         *
         * Chỉ MissionExecutor gọi hàm này; nó là cách manager nhờ executor gọi
         * NavigationClient::cancelActive() mà bản thân không cần biết navigation là gì.
         */
        MissionId takePendingCancel();

        /**
         * @brief Chờ tới khi có việc cho executor: mission mới cần giao, hoặc yêu cầu dừng đang treo.
         * @return false nếu bị đánh thức bởi @ref wakeUp (đang dừng), true nếu thật sự có việc.
         *
         * Thay cho vòng poll: mission layer không có deadline định kỳ nào, poll chỉ thêm độ trễ
         * trung bình nửa chu kỳ vào đường phản ứng cancel/emergency.
         */
        bool waitForWork();

        /// @brief Đánh thức mọi thread đang chờ trong @ref waitForWork (dùng khi dừng executor).
        void wakeUp();

    private:
        /// Gọi khi đã giữ mutex_. Ghi mission đang chạy vào ô pending-cancel rồi bỏ nó ra.
        void requestCancelOfCurrentLocked();

        /// Gọi khi đã giữ mutex_. Id mission đang chạy, kInvalidMissionId nếu không có.
        MissionId currentIdLocked() const;

        /**
         * Gọi khi đã giữ mutex_. Đổi trạng thái và ghi log ĐÚNG MỘT LẦN tại mỗi lần đổi thật.
         *
         * Đây là nơi duy nhất được gán state_: nếu không, mission biến mất khỏi hàng đợi mà không
         * có dòng log nào cho biết ai đã làm điều đó và vì sao.
         *
         * @param reason Lý do ngắn gọn, cho người đọc log lúc 2 giờ sáng.
         */
        void setStateLocked(MissionState next, const char* reason, MissionId id = kInvalidMissionId);

        /// Gọi khi đã giữ mutex_. true nếu executor có việc để làm ngay.
        bool hasWorkLocked() const;

        /// Gọi khi đã giữ mutex_. true nếu đang có chặng chạy và mission_timeout được bật.
        bool timeoutArmedLocked() const;

        /**
         * Gọi khi đã giữ mutex_. Đánh dấu chặng đang chạy là thất bại do quá hạn, và ghi yêu cầu
         * dừng navigation — khác NAV_FAILED ở chỗ navigation vẫn đang chạy nên phải bảo nó dừng.
         */
        void expireCurrentLocked();

        mutable std::mutex mutex_;
        std::condition_variable work_cv_;
        bool wake_requested_ = false;
        MissionState state_ = MissionState::IDLE;
        std::queue<std::shared_ptr<const Mission>> mission_queue_;
        std::shared_ptr<const Mission> current_mission_;
        MissionId pending_cancel_ = kInvalidMissionId;

        /// Thời điểm chặng hiện tại được giao — mốc tính mission_timeout.
        robot::Time mission_start_time_;

        MissionConfig config_;
        MissionId next_mission_id_ = 1;   ///< 0 dành riêng cho kInvalidMissionId
    };

} // namespace mission_adapters

#endif  // MISSION_ADAPTERS_MISSION_MANAGER_H_
