/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * Nguồn mission từ VDA5050 Order (schema "vda5050.order").
 *
 * Author: DuongTD
 *********************************************************************/
#ifndef MISSION_ADAPTERS_PLUGINS_VDA5050_SOURCE_ADAPTER_H_
#define MISSION_ADAPTERS_PLUGINS_VDA5050_SOURCE_ADAPTER_H_

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

#include <robot/node_handle.h>

#include <mission_adapters/adapter.h>

namespace mission_plugins
{

/**
 * @class VDA5050SourceAdapter
 * @brief Cắt một VDA5050 Order thành các chặng, mỗi chặng kết thúc tại một node có action.
 *
 * Ba điểm conformance mà adapter này chịu trách nhiệm:
 *
 * 1. **base / horizon** — chỉ phần `released == true` được thực thi. Horizon là dự định của fleet
 *    manager, chưa được phép chạy; robot đi vào đó là đi vào đoạn đường chưa ai cho phép.
 * 2. **orderId / orderUpdateId** — phân biệt yêu cầu MỚI (thay hàng đợi) với bản CẬP NHẬT của yêu
 *    cầu đang chạy (nối tiếp). Nhầm hai thứ này thì mỗi lần fleet manager release thêm horizon,
 *    robot lại huỷ và chạy lại chặng đang đi.
 * 3. **goal / start self-contained** — mission mang sẵn pose đích, consumer không phải tự đoán từ
 *    `nodes.back()`.
 *
 * State là member, không phải static local: nhiều instance trong cùng tiến trình không được nhìn
 * thấy order của nhau.
 */
class VDA5050SourceAdapter : public mission_adapters::MissionSourceAdapter
{
public:
    /// @brief Factory được PluginRegistry nạp qua boost::dll::import_alias.
    static mission_adapters::MissionSourceAdapter::Ptr create();

    /**
     * @brief Đọc param riêng của instance.
     *
     * Param (namespace `<name>`):
     *  - `global_frame` [string, mặc định "map"]: frame gán cho pose sinh ra từ nodePosition.
     *  - `compound_actions` [map, optional]: action "phải dò rồi mới biết đích" — xem @ref Step.
     */
    bool configure(const std::string& name, robot::NodeHandle& nh) override;

    std::string schema() const override;

    bool validate(const mission_adapters::MissionRequest& request,
                  std::string& reason) const override;

    mission_adapters::ConversionResult
    convert(const mission_adapters::MissionRequest& request) override;

private:
    /**
     * @struct Step
     * @brief Một bước trong chuỗi mở rộng của compound action.
     *
     * Engine hiểu đúng bốn từ khoá và **không biết** `charge`, `dock_target` hay `LiftFork` nghĩa là
     * gì — đó là điều kiện để cùng một engine dùng lại cho model robot khác: thêm model = thêm YAML
     * + một `.so` handler, không sửa dòng nào ở đây.
     *
     * Mỗi step phải có ĐÚNG MỘT trong bốn khoá đầu; sai cú pháp thì `configure()` từ chối lúc **boot**.
     */
    struct Step
    {
        std::string action;          ///< `action: <type>`      -> chặng chỉ-action
        std::string move_to;         ///< `move_to: <frame>`    -> chặng nav, frame cố định
        std::string move_to_param;   ///< `move_to_param: <key>`-> frame lấy từ actionParameters
        double      move = std::numeric_limits<double>::quiet_NaN();  ///< `move: <m>` tương đối

        std::string motion_hint;     ///< `profile:` -> position | docking | go_straight | rotate
        std::string marker;          ///< `marker:` -> khoá override planner, chỉ hợp lệ với docking
    };

    /// @brief Chuỗi chặng thay cho một action. Bảng chỉ giữ CẤU TRÚC, dữ liệu tới từ order.
    using Steps = std::vector<Step>;

    /// @brief Nạp bảng `compound_actions`. Trả false nếu bảng có mà khai sai — lỗi nổ lúc boot.
    bool loadCompoundActions(const std::string& name, robot::NodeHandle& nh);

    /// @brief Tra bảng theo actionType. nullptr nếu là action thường.
    const Steps* findCompound(const std::string& action_type) const;

    /**
     * @brief Mở rộng action của một chặng thành chuỗi chặng, giữ nguyên thứ tự mảng JSON.
     * @param leg Chặng nav tới node; action compound sẽ được GỠ khỏi nó.
     * @param out Nơi nối thêm chặng sinh ra.
     * @return false nếu thiếu tham số cấu trúc — bên gọi phải bỏ CẢ order.
     *
     * Action thường tích vào bộ đệm; gặp compound thì xả bộ đệm rồi phát chuỗi. Bộ đệm ĐẦU TIÊN nằm
     * lại trên chặng nav, các bộ đệm sau thành chặng chỉ-action. Nhờ vậy `[MutedOn, charge, MutedOff]`
     * cho ra `MutedOff` **sau** chuỗi charge — gom hết vào chặng nav sẽ bật lại cảm biến an toàn
     * trước khi robot lùi vào trạm.
     */
    bool expandCompound(const std::shared_ptr<mission_adapters::Mission>& leg,
                        std::vector<std::shared_ptr<mission_adapters::Mission>>& out) const;

    /// Số node đầu tiên có released == true. 0 nghĩa là không node nào được release.
    static size_t countReleasedNodes(const robot_protocol_msgs::Order& order);

    /// Số edge thuộc phần base, không vượt quá `released_node_count - 1`.
    static size_t countReleasedEdges(const robot_protocol_msgs::Order& order,
                                     size_t released_node_count);

    /**
     * @brief Phần order được phép thực thi.
     * @return số node base; 0 nếu order không dùng trường `released` (xem ghi chú trong .cpp).
     */
    size_t executableNodeCount(const robot_protocol_msgs::Order& order) const;

    /// Dựng PoseStamped từ nodePosition (theta [rad] -> quaternion quanh trục z).
    robot_geometry_msgs::PoseStamped toPose(const robot_protocol_msgs::Node& node) const;

    std::string name_;

    /// Frame gán cho goal/start. VDA5050 `mapId` là danh tính bản đồ, KHÔNG phải frame TF.
    std::string global_frame_ = "map";

    /// actionType -> chuỗi chặng. Rỗng = không action nào cần mở rộng, adapter chạy như trước.
    std::map<std::string, Steps> compound_actions_;

    /// Order đang chạy — dùng để nhận ra order update so với order mới.
    std::string last_order_id_;
    std::uint32_t last_order_update_id_ = 0;

    /// Số node base đã chuyển đổi của order đang chạy; điểm bắt đầu cho phần release thêm.
    size_t converted_node_count_ = 0;
};

}  // namespace mission_plugins

#endif  // MISSION_ADAPTERS_PLUGINS_VDA5050_SOURCE_ADAPTER_H_