#include "vda5050_source_adapter.h"

#include <algorithm>
#include <cmath>
#include <string>
#include <utility>

#include <yaml-cpp/yaml.h>

#include <boost/dll/alias.hpp>

#include <robot/robot.h>

namespace mission_plugins
{
namespace
{
bool isNavigationProfile(const std::string& profile)
{
    return profile.empty() || profile == "position" || profile == "docking" ||
           profile == "go_straight" || profile == "rotate";
}

    using mission_adapters::Action;
    using mission_adapters::ActionType;
    using mission_adapters::ConversionResult;
    using mission_adapters::Mission;
    using mission_adapters::MissionType;
    using mission_adapters::SubmitMode;

    /// Gom action của edge và của node cuối vào mission, sắp theo sequenceId của VDA5050.
    void collectActions(const std::shared_ptr<Mission>& mission)
    {
        for (const auto& edge : mission->edges)
        {
            for (const auto& action : edge.actions)
            {
                Action ma;
                ma.sequenceId = edge.sequenceId;
                ma.type       = ActionType::EDGE_ACTION;
                ma.action     = action;
                mission->actions.push_back(std::move(ma));
            }
        }

        if (!mission->nodes.empty())
        {
            const auto& node = mission->nodes.back();
            for (const auto& action : node.actions)
            {
                Action ma;
                ma.sequenceId = node.sequenceId;
                ma.type       = ActionType::NODE_ACTION;
                ma.action     = action;
                mission->actions.push_back(std::move(ma));
            }
        }

        // stable_sort chứ không sort: `sequenceId` ở đây là của NODE/EDGE sở hữu action, nên mọi
        // action trên cùng một node có khoá BẰNG NHAU. Với khoá bằng nhau, std::sort không bảo đảm
        // giữ thứ tự — mà thứ tự đó chính là thứ tự trong mảng JSON, thứ VDA5050 quy định là thứ tự
        // thực hiện. Bản cũ chạy đúng chỉ nhờ libstdc++ dùng insertion sort cho dải nhỏ.
        std::stable_sort(mission->actions.begin(), mission->actions.end(),
            [](const Action& a, const Action& b) {
                return a.sequenceId < b.sequenceId;
            });
    }

}  // namespace

mission_adapters::MissionSourceAdapter::Ptr VDA5050SourceAdapter::create()
{
    return std::make_shared<VDA5050SourceAdapter>();
}

bool VDA5050SourceAdapter::configure(const std::string& name, robot::NodeHandle& nh)
{
    name_ = name;

    nh.getParam(name + "/global_frame", global_frame_, std::string("map"));
    if (global_frame_.empty())
    {
        robot::log_error("VDA5050SourceAdapter[%s]: global_frame is empty", name.c_str());
        return false;
    }

    if (!loadCompoundActions(name, nh))
        return false;

    last_order_id_.clear();
    last_order_update_id_ = 0;
    converted_node_count_ = 0;
    return true;
}

bool VDA5050SourceAdapter::loadCompoundActions(const std::string& name, robot::NodeHandle& nh)
{
    compound_actions_.clear();

    YAML::Node table;
    if (!nh.getParam(name + "/compound_actions", table) || !table.IsMap())
        return true;   // Không khai bảng là hợp lệ: adapter chạy y như trước.

    for (auto entry = table.begin(); entry != table.end(); ++entry)
    {
        const std::string action_type = entry->first.as<std::string>();
        const YAML::Node& steps_node = entry->second["steps"];

        if (action_type.empty() || !steps_node || !steps_node.IsSequence() || steps_node.size() == 0)
        {
            robot::log_error("VDA5050SourceAdapter[%s]: compound action '%s' has no 'steps' list",
                             name.c_str(), action_type.c_str());
            return false;
        }

        Steps steps;
        for (std::size_t i = 0; i < steps_node.size(); ++i)
        {
            const YAML::Node& n = steps_node[i];
            Step step;

            try
            {
                if (n["action"])         step.action        = n["action"].as<std::string>();
                if (n["move_to"])        step.move_to       = n["move_to"].as<std::string>();
                if (n["move_to_param"])  step.move_to_param = n["move_to_param"].as<std::string>();
                if (n["move"])           step.move          = n["move"].as<double>();
                if (n["profile"])        step.motion_hint   = n["profile"].as<std::string>();
                if (n["marker"])         step.marker        = n["marker"].as<std::string>();
            }
            catch (const YAML::Exception& ex)
            {
                robot::log_error("VDA5050SourceAdapter[%s]: '%s' step %zu is malformed: %s",
                                 name.c_str(), action_type.c_str(), i, ex.what());
                return false;
            }

            const int keys = (step.action.empty() ? 0 : 1) + (step.move_to.empty() ? 0 : 1) +
                             (step.move_to_param.empty() ? 0 : 1) +
                             (std::isfinite(step.move) ? 1 : 0);
            if (keys != 1)
            {
                // Không đúng một từ khoá thì không có cách diễn giải nào là hiển nhiên đúng. Chặn ở
                // boot thay vì đoán lúc order đầu tiên tới.
                robot::log_error("VDA5050SourceAdapter[%s]: '%s' step %zu must have exactly one of "
                                 "action / move_to / move_to_param / move (found %d)",
                                 name.c_str(), action_type.c_str(), i, keys);
                return false;
            }

            if (!isNavigationProfile(step.motion_hint))
            {
                robot::log_error("VDA5050SourceAdapter[%s]: '%s' step %zu has invalid profile '%s'",
                                 name.c_str(), action_type.c_str(), i, step.motion_hint.c_str());
                return false;
            }
            if (!step.action.empty() && !step.motion_hint.empty())
            {
                robot::log_error("VDA5050SourceAdapter[%s]: '%s' step %zu is action-only and must "
                                 "not set profile",
                                 name.c_str(), action_type.c_str(), i);
                return false;
            }
            if (!step.marker.empty() && step.motion_hint != "docking")
            {
                robot::log_error("VDA5050SourceAdapter[%s]: '%s' step %zu sets marker but is not "
                                 "a docking navigation step",
                                 name.c_str(), action_type.c_str(), i);
                return false;
            }

            steps.push_back(step);
        }

        compound_actions_[action_type] = std::move(steps);
    }

    // Chống đệ quy: một step sinh ra actionType mà chính nó cũng là compound thì expander sẽ mở rộng
    // output của mình — vòng lặp vô hạn lúc convert.
    for (const auto& entry : compound_actions_)
    {
        for (const Step& step : entry.second)
        {
            if (!step.action.empty() && compound_actions_.count(step.action) != 0)
            {
                robot::log_error("VDA5050SourceAdapter[%s]: compound '%s' emits '%s' which is itself "
                                 "a compound action — that would recurse",
                                 name.c_str(), entry.first.c_str(), step.action.c_str());
                return false;
            }
        }
    }

    robot::log_info("VDA5050SourceAdapter[%s]: %zu compound action(s) loaded", name.c_str(),
                    compound_actions_.size());
    return true;
}

const VDA5050SourceAdapter::Steps*
VDA5050SourceAdapter::findCompound(const std::string& action_type) const
{
    const auto it = compound_actions_.find(action_type);
    return it == compound_actions_.end() ? nullptr : &it->second;
}

std::string VDA5050SourceAdapter::schema() const
{
    return mission_adapters::schema::kVda5050Order;
}

size_t VDA5050SourceAdapter::countReleasedNodes(const robot_protocol_msgs::Order& order)
{
    // VDA5050: base là tiền tố của danh sách node — dừng ở node đầu tiên chưa release.
    size_t count = 0;
    while (count < order.nodes.size() && order.nodes[count].released)
        ++count;

    return count;
}

size_t VDA5050SourceAdapter::countReleasedEdges(const robot_protocol_msgs::Order& order,
                                                size_t released_node_count)
{
    if (released_node_count < 2)
        return 0;

    size_t count = 0;
    const size_t limit = std::min(order.edges.size(), released_node_count - 1);
    while (count < limit && order.edges[count].released)
        ++count;

    return count;
}

size_t VDA5050SourceAdapter::executableNodeCount(const robot_protocol_msgs::Order& order) const
{
    const size_t released = countReleasedNodes(order);
    if (released > 0)
        return released;

    // Không node nào released. Theo VDA5050 order phải có ít nhất một base node, nên trường hợp này
    // gần như luôn là host không điền `released`. Coi cả order là base — im lặng không chạy gì sẽ
    // khiến fleet manager chờ vô hạn mà không có dấu hiệu nào.
    robot::log_warning("VDA5050SourceAdapter[%s]: order '%s' has no released node — treating the "
                       "whole order as base (did the host leave the 'released' field out?)",
                       name_.c_str(), order.orderId.c_str());
    return order.nodes.size();
}

robot_geometry_msgs::PoseStamped
VDA5050SourceAdapter::toPose(const robot_protocol_msgs::Node& node) const
{
    robot_geometry_msgs::PoseStamped pose;
    pose.header.frame_id = global_frame_;
    pose.pose.position.x = node.nodePosition.x;   // [m]
    pose.pose.position.y = node.nodePosition.y;   // [m]
    pose.pose.position.z = 0.0;

    // theta [rad] quanh trục z -> quaternion.
    const double half_theta = 0.5 * node.nodePosition.theta;
    pose.pose.orientation.x = 0.0;
    pose.pose.orientation.y = 0.0;
    pose.pose.orientation.z = std::sin(half_theta);
    pose.pose.orientation.w = std::cos(half_theta);

    return pose;
}

bool VDA5050SourceAdapter::validate(const mission_adapters::MissionRequest& request,
                                    std::string& reason) const
{
    if (!request.order)
    {
        reason = "request is missing the order payload";
        return false;
    }

    const auto& order = *request.order;

    if (order.nodes.empty())
    {
        reason = "order has no node";
        return false;
    }

    // VDA5050: n node liên thông cần đúng n-1 edge. Thiếu edge nghĩa là đồ thị đứt đoạn, cắt chặng
    // theo chỉ số sẽ lấy nhầm edge của đoạn khác.
    if (order.nodes.size() > 1 && order.edges.size() < order.nodes.size() - 1)
    {
        reason = "fewer edges than nodes - 1";
        return false;
    }

    for (const auto& node : order.nodes)
    {
        if (!std::isfinite(node.nodePosition.x) || !std::isfinite(node.nodePosition.y) ||
            !std::isfinite(node.nodePosition.theta))
        {
            reason = "nodePosition contains NaN/Inf at node '" + node.nodeId + "'";
            return false;
        }
    }

    // Bản cập nhật cũ hơn (hoặc phát lại) của order đang chạy: từ chối thay vì chạy lại tuyến đường.
    if (!order.orderId.empty() && order.orderId == last_order_id_ &&
        order.orderUpdateId <= last_order_update_id_)
    {
        reason = "orderUpdateId " + std::to_string(order.orderUpdateId) +
                 " is not newer than the running one (" + std::to_string(last_order_update_id_) + ")";
        return false;
    }

    return true;
}


bool VDA5050SourceAdapter::expandCompound(
    const std::shared_ptr<mission_adapters::Mission>& leg,
    std::vector<std::shared_ptr<mission_adapters::Mission>>& out) const
{
    if (compound_actions_.empty() || leg->actions.empty())
        return true;   // Không có gì để mở rộng — đường đi thường.

    std::vector<Action> buffer;      // action thường đang chờ xả
    std::vector<Action> on_the_leg;  // bộ đệm ĐẦU TIÊN: ở lại trên chặng nav
    bool first_flush = true;
    bool expanded_any = false;

    // Chặng chỉ-action sinh ra từ bộ đệm sau lần xả đầu.
    auto flush = [&](void) {
        if (buffer.empty())
            return;
        if (first_flush)
        {
            on_the_leg = buffer;
        }
        else
        {
            auto extra = std::make_shared<Mission>();
            extra->type = MissionType::VDA5050_ORDER;
            extra->has_goal = false;
            extra->actions = buffer;
            out.push_back(std::move(extra));
        }
        buffer.clear();
    };

    for (const Action& entry : leg->actions)
    {
        // Compound chỉ áp cho NODE_ACTION: một chuỗi dò-rồi-tiến-vào không có nghĩa khi gắn vào một
        // cạnh mà robot đang đi trên đó.
        const Steps* steps = (entry.type == ActionType::NODE_ACTION)
                                 ? findCompound(entry.action.actionType)
                                 : nullptr;

        if (steps == nullptr)
        {
            if (entry.type == ActionType::EDGE_ACTION &&
                findCompound(entry.action.actionType) != nullptr)
            {
                robot::log_warning("VDA5050SourceAdapter[%s]: '%s' is a compound action but sits on "
                                   "an EDGE — running it as a plain action",
                                   name_.c_str(), entry.action.actionType.c_str());
            }
            buffer.push_back(entry);
            continue;
        }

        flush();
        first_flush = false;
        expanded_any = true;

        for (const Step& step : *steps)
        {
            auto sub = std::make_shared<Mission>();
            sub->type = MissionType::VDA5050_ORDER;

            if (!step.action.empty())
            {
                // Action nội bộ: mang NGUYÊN actionParameters của action gốc — handler cần biết dò
                // trạm nào, và nó là chỗ duy nhất hiểu ý nghĩa các tham số đó.
                Action generated;
                generated.type = ActionType::NODE_ACTION;
                generated.sequenceId = entry.sequenceId;
                generated.action = entry.action;
                generated.action.actionType = step.action;
                generated.action.actionId = entry.action.actionId + "-" + step.action;

                sub->has_goal = false;
                sub->actions.push_back(std::move(generated));
            }
            else
            {
                sub->has_goal = true;
                // Mission nav luôn tự mô tả profile hiệu lực. Action-only giữ chuỗi rỗng để không
                // bao giờ bị hiểu nhầm là một yêu cầu điều khiển.
                sub->motion_hint = step.motion_hint.empty() ? "position" : step.motion_hint;
                sub->marker = step.marker;
                sub->start = leg->start;
                sub->goal = leg->goal;   // chỗ dựa; đích thật đến muộn qua goal_frame/relative_distance

                if (!step.move_to.empty())
                {
                    sub->goal_frame = step.move_to;
                }
                else if (!step.move_to_param.empty())
                {
                    // Thay thế xảy ra tại CONVERT: runtime không bao giờ thấy placeholder, và thiếu
                    // tham số thì order bị từ chối trước khi robot nhúc nhích.
                    const auto it = std::find_if(
                        entry.action.actionParameters.begin(), entry.action.actionParameters.end(),
                        [&step](const robot_protocol_msgs::ActionParameter& p) {
                            return p.key == step.move_to_param;
                        });

                    if (it == entry.action.actionParameters.end() || it->value.empty())
                    {
                        robot::log_error("VDA5050SourceAdapter[%s]: compound action '%s' (actionId "
                                         "'%s') requires parameter '%s' but it is missing",
                                         name_.c_str(), entry.action.actionType.c_str(),
                                         entry.action.actionId.c_str(), step.move_to_param.c_str());
                        return false;
                    }
                    sub->goal_frame = it->value;
                }
                else
                {
                    sub->relative_distance = step.move;
                }
            }

            out.push_back(std::move(sub));
        }
    }

    flush();

    if (expanded_any)
        leg->actions = on_the_leg;

    return true;
}

ConversionResult VDA5050SourceAdapter::convert(const mission_adapters::MissionRequest& request)
{
    ConversionResult result;

    std::string reason;
    if (!validate(request, reason))
    {
        robot::log_warning("VDA5050SourceAdapter[%s]: dropping the order — %s", name_.c_str(),
                           reason.c_str());
        return result;
    }

    const auto& order = *request.order;

    const size_t node_count = executableNodeCount(order);
    const size_t edge_count = (countReleasedNodes(order) > 0)
                                  ? countReleasedEdges(order, node_count)
                                  : order.edges.size();

    // Order update của đúng order đang chạy: chỉ sinh phần vừa được release thêm.
    const bool is_update = !order.orderId.empty() && order.orderId == last_order_id_;

    size_t start_node_idx = 0;
    if (is_update)
    {
        if (converted_node_count_ >= node_count)
        {
            robot::log_info("VDA5050SourceAdapter[%s]: order '%s' update %u released no further "
                            "node — there is no new work",
                            name_.c_str(), order.orderId.c_str(),
                            static_cast<unsigned>(order.orderUpdateId));

            last_order_update_id_ = order.orderUpdateId;
            return result;
        }

        // Node cuối đã chuyển đổi trở thành node xuất phát của chặng tiếp theo.
        start_node_idx = converted_node_count_ - 1;
        result.mode = SubmitMode::kAppend;
    }

    // Chặng được cắt tại mỗi node có action: robot chạy tới node đó rồi mới thực hiện action.
    std::vector<size_t> action_node_indices;
    for (size_t i = start_node_idx + 1; i < node_count; ++i)
    {
        if (!order.nodes[i].actions.empty())
            action_node_indices.push_back(i);
    }

    // Node xuất phát có action (và chưa từng được chuyển đổi) cũng là một chặng — chặng chỉ-action.
    if (!is_update && !order.nodes[start_node_idx].actions.empty())
        action_node_indices.insert(action_node_indices.begin(), start_node_idx);

    auto makeMission = [&](size_t from, size_t to) {
        auto mission = std::make_shared<Mission>();
        mission->type = MissionType::VDA5050_ORDER;

        mission->nodes.assign(order.nodes.begin() + from, order.nodes.begin() + to + 1);

        if (to > from)
        {
            const size_t edge_to = std::min(to, edge_count);
            if (from < edge_to)
                mission->edges.assign(order.edges.begin() + from, order.edges.begin() + edge_to);
        }

        // D8: chặng một node là chặng chỉ-có-action (action nằm ngay tại node xuất phát) — không có
        // quãng đường nào để đi. Quy tắc thuần cấu trúc, adapter không cần biết robot đang ở đâu.
        mission->has_goal = (to > from);

        if (mission->has_goal)
        {
            // Mission self-contained: consumer không phải suy goal ra từ nodes (A4).
            mission->start = toPose(order.nodes[from]);
            mission->goal  = toPose(order.nodes[to]);
            mission->motion_hint = "position";
        }

        collectActions(mission);
        return mission;
    };

    // Mission không goal mà cũng không action là mission rỗng — không sinh ra nó.
    auto isMeaningful = [](const std::shared_ptr<Mission>& mission) {
        return mission->has_goal || !mission->actions.empty();
    };

    size_t segment_start = start_node_idx;

    for (const size_t action_node_idx : action_node_indices)
    {
        auto mission = makeMission(segment_start, action_node_idx);

        // Mở rộng TRƯỚC isMeaningful: hàm này gỡ action compound khỏi chặng, và một chặng chỉ-action
        // sau khi gỡ có thể trở thành rỗng.
        std::vector<std::shared_ptr<Mission>> expanded;
        if (!expandCompound(mission, expanded))
        {
            // Thiếu tham số cấu trúc: bỏ CẢ order. Một order chạy nửa vời — robot tới trạm sạc rồi
            // không sạc — nguy hiểm hơn là không chạy.
            result.missions.clear();
            return result;
        }

        if (isMeaningful(mission))
            result.missions.push_back(std::move(mission));

        for (auto& sub : expanded)
            result.missions.push_back(std::move(sub));

        segment_start = action_node_idx;
    }

    // Đoạn còn lại sau node có action cuối cùng.
    if (segment_start + 1 < node_count)
    {
        auto mission = makeMission(segment_start, node_count - 1);
        if (isMeaningful(mission))
            result.missions.push_back(std::move(mission));
    }

    if (result.missions.empty())
        return result;

    last_order_id_        = order.orderId;
    last_order_update_id_ = order.orderUpdateId;
    converted_node_count_ = node_count;

    return result;
}

}  // namespace mission_plugins

BOOST_DLL_ALIAS(mission_plugins::VDA5050SourceAdapter::create, VDA5050SourceAdapter)