/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * Kiểu dữ liệu nền của mission layer: trạng thái, mission, action.
 *
 * Author: DuongTD
 *********************************************************************/
#ifndef MISSION_ADAPTERS_TYPES_H_
#define MISSION_ADAPTERS_TYPES_H_

#include <cstdint>
#include <limits>
#include <memory>
#include <string>
#include <vector>

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

namespace mission_adapters
{
    /**
     * @brief Danh tính của một mission, do MissionManager cấp khi submit và đơn điệu tăng.
     *
     * Mọi kết quả từ phía navigation phải mang ID này. Không có nó thì outcome trễ của một mission
     * đã bị thay thế sẽ được ghi nhận cho mission đang chạy — mission mới "hoàn thành" mà robot
     * chưa hề đi.
     */
    using MissionId = std::uint64_t;

    /// @brief Mission chưa được submit (hoặc không tồn tại).
    static constexpr MissionId kInvalidMissionId = 0;

    /// @brief Trạng thái hàng đợi mission. Tại mỗi thời điểm robot chỉ ở đúng một trạng thái.
    enum class MissionState
    {
        IDLE,
        QUEUED,
        RUNNING,
        PAUSED,
        COMPLETED,
        FAILED,
        CANCELLED,
        EMERGENCY,
        CLEAR_EMERGENCY
    };

    /// @brief Tên trạng thái, dùng cho log và cho host hiển thị.
    const char* toString(MissionState state);

    /// @brief Nguồn gốc mission, quyết định cách consumer diễn giải nodes/edges.
    enum class MissionType
    {
        SIMPLE_GOAL,
        VDA5050_ORDER
    };

    /// @brief Action gắn với node (thực hiện tại chỗ) hay với edge (thực hiện dọc đường đi).
    enum class ActionType
    {
        NODE_ACTION,
        EDGE_ACTION
    };

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

    /**
     * @brief Một chặng việc self-contained mà navigation runtime có thể chạy trọn vẹn.
     *
     * @invariant Bất biến sau khi MissionManager::submit() gán @ref id: từ thời điểm đó mission chỉ
     *            được chia sẻ dưới dạng std::shared_ptr<const Mission>, nên ba thread (event, exec,
     *            navigation) đọc chung mà không cần khoá.
     */
    class Mission
    {
    public:
        MissionId id = kInvalidMissionId;   ///< 0 cho tới khi submit; sau đó không đổi
        MissionType type = MissionType::SIMPLE_GOAL;

        /**
         * false = mission chỉ-có-action: navigation runtime bỏ qua phần di chuyển và vào thẳng
         * thực thi action (D8). @ref start và @ref goal khi đó không mang ý nghĩa.
         *
         * @invariant has_goal == false thì @ref actions phải khác rỗng — mission không goal cũng
         *            không action là mission không có việc gì để làm.
         */
        bool has_goal = true;

        robot_geometry_msgs::PoseStamped start;   ///< chỉ hợp lệ khi has_goal
        robot_geometry_msgs::PoseStamped goal;    ///< chỉ hợp lệ khi has_goal

        std::vector<robot_protocol_msgs::Node> nodes;
        std::vector<robot_protocol_msgs::Edge> edges;

        /**
         * Action của chặng, đã sắp theo sequenceId.
         *
         * Mission layer KHÔNG diễn giải `actionType` và không lọc gì — đó là việc của navigation
         * runtime (D8). Ở đây chúng chỉ đi qua nguyên vẹn, đúng thứ tự.
         */
        std::vector<Action> actions;

        /**
         * Profile navigation hiệu lực của chặng có goal: `"position"`, `"docking"`,
         * `"go_straight"` hoặc `"rotate"`. Mission chỉ-action phải để rỗng. Adapter hiện tại
         * luôn ghi `"position"` rõ ràng cho chặng nav thường để trace/runtime biết chính xác
         * planner nào sẽ được chọn; bridge vẫn chấp nhận rỗng như `position` để tương thích output
         * cũ.
         *
         * Là **chuỗi** chứ không phải enum, và mission layer **không diễn giải** nó — cùng quy tắc
         * với @ref actions. Mission layer không được biết tới navigation nên không có kiểu nào để
         * diễn tả "profile"; lớp nối phía navigation mới dịch chuỗi này sang khái niệm của mình.
         */
        std::string motion_hint;

        /**
         * Marker chọn cặp planner docking. Rỗng hoặc không có override cấu hình thì navigation
         * dùng profile docking mặc định. Đây không phải goal_frame: marker là khoá cấu hình, còn
         * goal_frame là frame TF được resolve khi chặng bắt đầu.
         */
        std::string marker;

        /**
         * Đích lấy từ TF frame này thay vì từ @ref goal. Rỗng = dùng @ref goal.
         *
         * Chặng dò-rồi-tiến-vào không biết đích lúc được sinh ra: bước dò phía trước tạo ra frame
         * này, và navigation tra nó tại thời điểm chặng được nhận.
         */
        std::string goal_frame;

        /// Quãng đường tương đối [m] so với pose hiện tại; dương = tiến, âm = lùi. NaN = không dùng.
        double relative_distance = std::numeric_limits<double>::quiet_NaN();
    };

} // namespace mission_adapters

#endif  // MISSION_ADAPTERS_TYPES_H_
