/**
 * @file vda5050_order_demo.cpp
 * @brief Chạy một VDA5050 Order qua VDA5050SourceAdapter và in ra các chặng thu được.
 *
 * Order dùng ở đây lấy nguyên toạ độ từ một order thật do fleet manager gửi xuống (4 node, 3 edge,
 * edge cuối có NURBS bậc 2), chỉ khác một điểm: gắn thêm một action vào **node thứ 2**. Mục đích là
 * thấy tận mắt adapter cắt order thành chặng như thế nào khi có action ở giữa đường.
 *
 * Chạy:  ./devel/lib/mission_adapters/vda5050_order_demo
 */
#include "mission_adapters/mission_request.h"
#include "mission_adapters/types.h"
#include "../plugins/vda5050_source_adapter.h"

#include <cstdio>
#include <string>

namespace
{

/// Một node released, kèm toạ độ thật lấy từ order của fleet.
robot_protocol_msgs::Node makeNode(int sequence_id, const std::string& id, double x, double y)
{
    robot_protocol_msgs::Node node;
    node.sequenceId = sequence_id;
    node.nodeId = id;
    node.released = true;
    node.nodePosition.x = x;
    node.nodePosition.y = y;
    node.nodePosition.mapId = "97b3bf93-10c5-4c79-2cc9-08deee20817a";
    return node;
}

robot_protocol_msgs::Edge makeEdge(int sequence_id, const std::string& id,
                                   const std::string& start_id, const std::string& end_id)
{
    robot_protocol_msgs::Edge edge;
    edge.sequenceId = sequence_id;
    edge.edgeId = id;
    edge.released = true;
    edge.startNodeId = start_id;
    edge.endNodeId = end_id;
    return edge;
}

robot_protocol_msgs::Action makeAction(const std::string& id, const std::string& type)
{
    robot_protocol_msgs::Action action;
    action.actionId = id;
    action.actionType = type;
    action.blockingType = "HARD";
    return action;
}

void printOrder(const robot_protocol_msgs::Order& order)
{
    std::printf("ORDER GUI XUONG  (orderId=%s, orderUpdateId=%d)\n",
                order.orderId.c_str(), (int)order.orderUpdateId);
    for (const auto& n : order.nodes)
        std::printf("   node seq=%-2d %-10s pos=(%7.3f, %7.3f)  actions=%zu%s\n",
                    (int)n.sequenceId, n.nodeId.c_str(),
                    n.nodePosition.x, n.nodePosition.y, n.actions.size(),
                    n.actions.empty() ? "" : ("  <-- " + n.actions.front().actionType).c_str());
    for (const auto& e : order.edges)
        std::printf("   edge seq=%-2d %-10s %s -> %s  actions=%zu\n",
                    (int)e.sequenceId, e.edgeId.c_str(),
                    e.startNodeId.c_str(), e.endNodeId.c_str(), e.actions.size());
}

void printMissions(const std::vector<std::shared_ptr<mission_adapters::Mission>>& missions)
{
    std::printf("\nKET QUA: adapter cat thanh %zu chang\n", missions.size());
    std::printf("%s\n", std::string(78, '-').c_str());

    int idx = 0;
    for (const auto& m : missions)
    {
        std::printf("  Chang #%d\n", ++idx);
        std::printf("      type      : %s\n",
                    m->type == mission_adapters::MissionType::VDA5050_ORDER ? "VDA5050_ORDER"
                                                                            : "SIMPLE_GOAL");
        std::printf("      has_goal  : %s\n", m->has_goal ? "true" : "false");
        if (m->has_goal)
            std::printf("      di chuyen : (%7.3f, %7.3f) -> (%7.3f, %7.3f)\n",
                        m->start.pose.position.x, m->start.pose.position.y,
                        m->goal.pose.position.x, m->goal.pose.position.y);
        else
            std::printf("      di chuyen : (khong - chang chi-co-action)\n");

        std::printf("      nodes     : %zu, edges: %zu\n", m->nodes.size(), m->edges.size());
        if (m->actions.empty())
        {
            std::printf("      actions   : (khong co)\n");
        }
        else
        {
            for (const auto& a : m->actions)
                std::printf("      actions   : seq=%d  %-12s  id=%s  (%s)\n",
                            a.sequenceId, a.action.actionType.c_str(), a.action.actionId.c_str(),
                            a.type == mission_adapters::ActionType::NODE_ACTION ? "NODE_ACTION"
                                                                                : "EDGE_ACTION");
        }
        std::printf("\n");
    }
}

}  // namespace

int main()
{
    // Toa do lay tu order that: 14:39:21, orderId c16cd9e3..., 4 node / 3 edge.
    robot_protocol_msgs::Order order;
    order.orderId = "demo-dua-tren-order-that";
    order.orderUpdateId = 0;

    order.nodes.push_back(makeNode(0, "node_A", 10.010, 10.000));
    order.nodes.push_back(makeNode(2, "node_B", 10.260, 10.192));
    order.nodes.push_back(makeNode(4, "node_C", 14.979, 7.599));
    order.nodes.push_back(makeNode(6, "node_D", 12.986, 2.851));

    // ĐIỂM KHÁC DUY NHẤT so với order thật: mot action gan vao NODE THU 2.
    order.nodes[1].actions.push_back(makeAction("act-tai-node-2", "PickUp"));

    order.edges.push_back(makeEdge(1, "edge_AB", "node_A", "node_B"));
    order.edges.push_back(makeEdge(3, "edge_BC", "node_B", "node_C"));
    order.edges.push_back(makeEdge(5, "edge_CD", "node_C", "node_D"));

    printOrder(order);

    mission_plugins::VDA5050SourceAdapter adapter;

    std::string reason;
    const auto request = mission_adapters::MissionRequest::fromOrder(order);
    if (!adapter.validate(request, reason))
    {
        std::printf("\nAdapter TU CHOI order: %s\n", reason.c_str());
        return 1;
    }

    printMissions(adapter.convert(request).missions);
    return 0;
}