#include <gtest/gtest.h>

#include <cstdlib>

#include <cmath>
#include <string>

#include <mission_adapters/mission_request.h>

#include "goal_source_adapter.h"
#include "mission_test_utils.h"
#include "vda5050_source_adapter.h"

namespace
{
using namespace mission_adapters;
using mission_plugins::GoalSourceAdapter;
using mission_plugins::VDA5050SourceAdapter;
using mission_test::makeAction;
using mission_test::makeGoal;
using mission_test::makeOrder;

class AdapterTest : public ::testing::Test
{
protected:
    void SetUp() override
    {
        robot::NodeHandle nh;
        ASSERT_TRUE(goal_adapter.configure("goal_src", nh));
        ASSERT_TRUE(order_adapter.configure("vda5050_src", nh));
    }

    /// Chuyển một order, trả về danh sách mission (bỏ phần mode).
    std::vector<std::shared_ptr<Mission>> convertOrder(const robot_protocol_msgs::Order& order)
    {
        return order_adapter.convert(MissionRequest::fromOrder(order)).missions;
    }

    /// Chuyển một order và giữ nguyên cả ConversionResult để kiểm mode.
    ConversionResult convertOrderFull(const robot_protocol_msgs::Order& order)
    {
        return order_adapter.convert(MissionRequest::fromOrder(order));
    }

    GoalSourceAdapter goal_adapter;
    VDA5050SourceAdapter order_adapter;
};

// ── Schema ──────────────────────────────────────────────────────────────────────────────────────

TEST_F(AdapterTest, AdaptersDeclareDistinctSchemas)
{
    EXPECT_EQ(goal_adapter.schema(), schema::kPoseStamped);
    EXPECT_EQ(order_adapter.schema(), schema::kVda5050Order);
    EXPECT_NE(goal_adapter.schema(), order_adapter.schema());
}

// ── GoalSourceAdapter ───────────────────────────────────────────────────────────────────────────

TEST_F(AdapterTest, GoalAdapterCreatesSingleMission)
{
    const auto missions = goal_adapter.convert(MissionRequest::fromPose(makeGoal(5.5, 9.1))).missions;

    ASSERT_EQ(missions.size(), 1u);
    EXPECT_EQ(missions.front()->type, MissionType::SIMPLE_GOAL);
    EXPECT_EQ(missions.front()->motion_hint, "position");
    EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.x, 5.5);
    EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.y, 9.1);
}

TEST_F(AdapterTest, GoalAdapterRejectsMissingPayload)
{
    MissionRequest request;
    request.schema = schema::kPoseStamped;

    std::string reason;
    EXPECT_FALSE(goal_adapter.validate(request, reason));
    EXPECT_FALSE(reason.empty());
}

TEST_F(AdapterTest, GoalAdapterRejectsNonFiniteGoal)
{
    auto goal = makeGoal(1.0, 1.0);
    goal.pose.position.x = std::numeric_limits<double>::quiet_NaN();
    goal.pose.orientation.w = 1.0;

    std::string reason;
    EXPECT_FALSE(goal_adapter.validate(MissionRequest::fromPose(goal), reason))
        << "a goal containing NaN slipped through the boundary into navigation";
}

TEST_F(AdapterTest, GoalAdapterRejectsZeroQuaternion)
{
    auto goal = makeGoal(1.0, 1.0);

    std::string reason;
    ASSERT_TRUE(goal_adapter.validate(MissionRequest::fromPose(goal), reason)) << reason;

    // Host quên set orientation: quaternion toàn 0 không phải "hướng bất kỳ", nó là dữ liệu hỏng.
    goal.pose.orientation.w = 0.0;
    EXPECT_FALSE(goal_adapter.validate(MissionRequest::fromPose(goal), reason));
}

// ── VDA5050SourceAdapter ────────────────────────────────────────────────────────────────────────

TEST_F(AdapterTest, EmptyOrderIsRejectedByValidate)
{
    robot_protocol_msgs::Order order;

    std::string reason;
    EXPECT_FALSE(order_adapter.validate(MissionRequest::fromOrder(order), reason));
    EXPECT_TRUE(convertOrder(order).empty());
}

TEST_F(AdapterTest, OrderWithoutActionsCreatesOneTailMission)
{
    const auto missions = convertOrder(makeOrder(4));

    ASSERT_EQ(missions.size(), 1u);
    EXPECT_EQ(missions.front()->nodes.size(), 4u);
    EXPECT_EQ(missions.front()->edges.size(), 3u);
}

TEST_F(AdapterTest, InvalidOrderWithMissingEdgesIsRejected)
{
    auto order = makeOrder(4);
    order.edges.pop_back();

    std::string reason;
    EXPECT_FALSE(order_adapter.validate(MissionRequest::fromOrder(order), reason));
    EXPECT_TRUE(convertOrder(order).empty());
}

TEST_F(AdapterTest, OrderSplitsAtNodeAction)
{
    auto order = makeOrder(5);
    order.nodes[2].actions.push_back(makeAction("dock"));

    const auto missions = convertOrder(order);

    ASSERT_EQ(missions.size(), 2u);
    EXPECT_EQ(missions[0]->nodes.size(), 3u);
    EXPECT_EQ(missions[1]->nodes.size(), 3u);
}

TEST_F(AdapterTest, OrderCollectsAndSortsActions)
{
    auto order = makeOrder(2);
    order.edges[0].actions.push_back(makeAction("edge"));
    order.nodes[1].actions.push_back(makeAction("node"));

    const auto missions = convertOrder(order);

    ASSERT_EQ(missions.size(), 1u);
    ASSERT_EQ(missions.front()->actions.size(), 2u);
    EXPECT_EQ(missions.front()->actions[0].type, ActionType::EDGE_ACTION);
    EXPECT_EQ(missions.front()->actions[1].type, ActionType::NODE_ACTION);
}

// ── A4: conformance VDA5050 ─────────────────────────────────────────────────────────────────────

TEST_F(AdapterTest, MissionCarriesGoalAndStartFromNodes)
{
    auto order = makeOrder(3);
    order.nodes[2].nodePosition.x = 7.0;
    order.nodes[2].nodePosition.y = 8.0;
    order.nodes[2].nodePosition.theta = 0.0;

    const auto missions = convertOrder(order);

    ASSERT_EQ(missions.size(), 1u);
    // Mission phải self-contained: consumer không phải tự đoán goal từ nodes.back().
    EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.x, 7.0);
    EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.y, 8.0);
    EXPECT_DOUBLE_EQ(missions.front()->goal.pose.orientation.w, 1.0);
    EXPECT_EQ(missions.front()->goal.header.frame_id, "map");

    EXPECT_DOUBLE_EQ(missions.front()->start.pose.position.x, 0.0);
}

TEST_F(AdapterTest, NodeThetaBecomesQuaternion)
{
    auto order = makeOrder(2);
    order.nodes[1].nodePosition.theta = M_PI;   // [rad]

    const auto missions = convertOrder(order);

    ASSERT_EQ(missions.size(), 1u);
    // theta = pi -> quay 180 độ quanh z: (z, w) = (1, 0).
    EXPECT_NEAR(missions.front()->goal.pose.orientation.z, 1.0, 1e-9);
    EXPECT_NEAR(missions.front()->goal.pose.orientation.w, 0.0, 1e-9);
}

TEST_F(AdapterTest, HorizonNodesAreNeverDispatched)
{
    auto order = makeOrder(5);
    // Chỉ 3 node đầu là base; hai node cuối là horizon (fleet manager chưa cho phép đi).
    order.nodes[3].released = false;
    order.nodes[4].released = false;
    order.edges[3].released = false;

    const auto missions = convertOrder(order);

    ASSERT_EQ(missions.size(), 1u);
    EXPECT_EQ(missions.front()->nodes.size(), 3u) << "the robot was handed the horizon part that "
                                                     "is not released yet";
    EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.x, 2.0);
}

TEST_F(AdapterTest, OrderWithoutReleasedFlagIsTreatedAsFullBase)
{
    auto order = makeOrder(3);
    for (auto& node : order.nodes) node.released = false;
    for (auto& edge : order.edges) edge.released = false;

    // Host không điền `released`: chạy cả order còn hơn đứng im không dấu hiệu.
    const auto missions = convertOrder(order);

    ASSERT_EQ(missions.size(), 1u);
    EXPECT_EQ(missions.front()->nodes.size(), 3u);
}

TEST_F(AdapterTest, StaleOrderUpdateIdIsRejected)
{
    ASSERT_FALSE(convertOrder(makeOrder(3, "order_A", 2)).empty());

    std::string reason;

    // Cùng orderId, orderUpdateId cũ hơn -> phát lại/đến trễ, không được chạy lại tuyến đường.
    EXPECT_FALSE(order_adapter.validate(MissionRequest::fromOrder(makeOrder(3, "order_A", 1)),
                                        reason));
    EXPECT_FALSE(order_adapter.validate(MissionRequest::fromOrder(makeOrder(3, "order_A", 2)),
                                        reason));

    // Cùng orderId, update mới hơn -> chấp nhận.
    EXPECT_TRUE(order_adapter.validate(MissionRequest::fromOrder(makeOrder(4, "order_A", 3)),
                                       reason)) << reason;

    // orderId khác -> order mới, không so orderUpdateId.
    EXPECT_TRUE(order_adapter.validate(MissionRequest::fromOrder(makeOrder(3, "order_B", 0)),
                                       reason)) << reason;
}

TEST_F(AdapterTest, OrderUpdateAppendsOnlyNewlyReleasedSegment)
{
    // Base ban đầu: 3 node released, 2 node horizon.
    auto order = makeOrder(5, "order_A", 0);
    order.nodes[3].released = false;
    order.nodes[4].released = false;
    order.edges[3].released = false;

    const auto first = convertOrderFull(order);
    ASSERT_EQ(first.missions.size(), 1u);
    EXPECT_EQ(first.mode, SubmitMode::kReplace);
    EXPECT_EQ(first.missions.front()->nodes.size(), 3u);

    // Fleet manager release nốt horizon.
    auto update = makeOrder(5, "order_A", 1);
    const auto second = convertOrderFull(update);

    ASSERT_EQ(second.missions.size(), 1u);
    EXPECT_EQ(second.mode, SubmitMode::kAppend)
        << "an order update replaced the whole queue -> the robot cancels and redoes the leg it is "
           "on";

    // Chặng mới bắt đầu từ node cuối của phần đã chạy, không lặp lại phần cũ.
    EXPECT_EQ(second.missions.front()->nodes.size(), 3u);
    EXPECT_DOUBLE_EQ(second.missions.front()->start.pose.position.x, 2.0);
    EXPECT_DOUBLE_EQ(second.missions.front()->goal.pose.position.x, 4.0);
}

TEST_F(AdapterTest, OrderUpdateWithoutNewNodesProducesNoWork)
{
    ASSERT_FALSE(convertOrder(makeOrder(3, "order_A", 0)).empty());

    // Update không release thêm node nào: không có việc mới, và cũng không được xoá gì.
    const auto result = convertOrderFull(makeOrder(3, "order_A", 1));
    EXPECT_TRUE(result.empty());
}

TEST_F(AdapterTest, NewOrderIdReplacesQueue)
{
    ASSERT_FALSE(convertOrder(makeOrder(3, "order_A", 0)).empty());

    const auto result = convertOrderFull(makeOrder(2, "order_B", 0));

    ASSERT_FALSE(result.empty());
    EXPECT_EQ(result.mode, SubmitMode::kReplace);
}

// ── D8: goal optional, action đi nguyên vẹn ─────────────────────────────────────────────────────

TEST_F(AdapterTest, ActionAtFirstNodeBecomesGoallessMission)
{
    auto order = makeOrder(3);
    order.nodes[0].actions.push_back(makeAction("pick_at_start"));

    const auto missions = convertOrder(order);

    ASSERT_EQ(missions.size(), 2u);

    // Chặng đầu: action ngay tại chỗ đứng, không có quãng đường nào để đi.
    EXPECT_FALSE(missions[0]->has_goal);
    ASSERT_EQ(missions[0]->actions.size(), 1u);
    EXPECT_EQ(missions[0]->actions.front().action.actionId, "pick_at_start");

    // Chặng sau: di chuyển bình thường.
    EXPECT_TRUE(missions[1]->has_goal);
    EXPECT_DOUBLE_EQ(missions[1]->goal.pose.position.x, 2.0);
}

TEST_F(AdapterTest, MovingMissionsAlwaysHaveGoal)
{
    const auto missions = convertOrder(makeOrder(3));

    ASSERT_EQ(missions.size(), 1u);
    EXPECT_TRUE(missions.front()->has_goal);
}

TEST_F(AdapterTest, ActionsPassThroughUnchangedAndInOrder)
{
    auto order = makeOrder(3);
    order.edges[0].sequenceId = 1;
    order.edges[0].actions.push_back(makeAction("beep"));
    order.edges[1].sequenceId = 3;
    order.edges[1].actions.push_back(makeAction("horn"));
    order.nodes[2].sequenceId = 4;
    order.nodes[2].actions.push_back(makeAction("lift"));

    const auto missions = convertOrder(order);

    ASSERT_EQ(missions.size(), 1u);
    const auto& actions = missions.front()->actions;

    ASSERT_EQ(actions.size(), 3u);
    EXPECT_EQ(actions[0].action.actionId, "beep");
    EXPECT_EQ(actions[1].action.actionId, "horn");
    EXPECT_EQ(actions[2].action.actionId, "lift");

    // Mission layer không diễn giải actionType — nó chỉ chuyển tiếp.
    EXPECT_EQ(actions[2].action.actionType, "TEST");
    EXPECT_EQ(actions[2].type, ActionType::NODE_ACTION);
}

TEST_F(AdapterTest, NonFiniteNodePositionIsRejected)
{
    auto order = makeOrder(3);
    order.nodes[1].nodePosition.x = std::numeric_limits<double>::infinity();

    std::string reason;
    EXPECT_FALSE(order_adapter.validate(MissionRequest::fromOrder(order), reason));
}

}  // namespace

// ── Compound action: mở rộng thành chuỗi chặng ──────────────────────────────────────────────────

namespace
{
/// @brief Gắn một action có tham số vào node.
void addAction(robot_protocol_msgs::Node& node, const std::string& type,
               const std::string& param_key = "", const std::string& param_value = "")
{
    robot_protocol_msgs::Action action;
    action.actionType = type;
    action.actionId = type + "_id";

    if (!param_key.empty())
    {
        robot_protocol_msgs::ActionParameter p;
        p.key = param_key;
        p.value = param_value;
        action.actionParameters.push_back(p);
    }
    node.actions.push_back(action);
}
}  // namespace

TEST_F(AdapterTest, CompoundActionExpandsIntoASequenceOfLegs)
{
    auto order = makeOrder(2);
    addAction(order.nodes[1], "charge", "goal_frame", "charger_02_goal");

    const auto missions = convertOrder(order);

    // nav -> n1 | DetectCharger | docking | startCharging
    ASSERT_EQ(missions.size(), 4u);

    EXPECT_TRUE(missions[0]->has_goal);
    EXPECT_TRUE(missions[0]->actions.empty()) << "action compound phải được GỠ khỏi chặng nav";
    EXPECT_EQ(missions[0]->motion_hint, "position");

    ASSERT_EQ(missions[1]->actions.size(), 1u);
    EXPECT_EQ(missions[1]->actions[0].action.actionType, "DetectCharger");
    EXPECT_FALSE(missions[1]->has_goal);

    EXPECT_TRUE(missions[2]->has_goal);
    EXPECT_EQ(missions[2]->motion_hint, "docking");
    EXPECT_EQ(missions[2]->goal_frame, "charger_02_goal") << "frame phải lấy từ actionParameters";
    EXPECT_EQ(missions[2]->marker, "charger") << "marker phải chọn override planner độc lập TF";

    ASSERT_EQ(missions[3]->actions.size(), 1u);
    EXPECT_EQ(missions[3]->actions[0].action.actionType, "startCharging");
    EXPECT_FALSE(missions[3]->has_goal);
}

TEST_F(AdapterTest, GeneratedActionsCarryTheOriginalParameters)
{
    auto order = makeOrder(2);
    addAction(order.nodes[1], "charge", "goal_frame", "charger_02_goal");

    const auto missions = convertOrder(order);
    ASSERT_EQ(missions.size(), 4u);

    // Handler dò cần biết dò trạm nào; nó là chỗ duy nhất hiểu ý nghĩa các tham số đó.
    const auto& detect = missions[1]->actions[0].action;
    ASSERT_EQ(detect.actionParameters.size(), 1u);
    EXPECT_EQ(detect.actionParameters[0].key, "goal_frame");
    EXPECT_EQ(detect.actionParameters[0].value, "charger_02_goal");

    // actionId suy từ id gốc để truy vết được trong log.
    EXPECT_NE(detect.actionId.find("charge_id"), std::string::npos);
}

TEST_F(AdapterTest, PlainActionsKeepTheirJsonOrderAroundACompound)
{
    auto order = makeOrder(2);
    addAction(order.nodes[1], "MutedOn");
    addAction(order.nodes[1], "charge", "goal_frame", "charger_goal");
    addAction(order.nodes[1], "MutedOff");

    const auto missions = convertOrder(order);

    // nav+MutedOn | Detect | docking | startCharging | MutedOff
    ASSERT_EQ(missions.size(), 5u);

    ASSERT_EQ(missions[0]->actions.size(), 1u);
    EXPECT_EQ(missions[0]->actions[0].action.actionType, "MutedOn");

    // MutedOff phải nằm SAU chuỗi charge. Gom hết action thường 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 — đúng thứ MutedOn sinh ra để tránh.
    ASSERT_EQ(missions[4]->actions.size(), 1u);
    EXPECT_EQ(missions[4]->actions[0].action.actionType, "MutedOff");
    EXPECT_FALSE(missions[4]->has_goal);
}

TEST_F(AdapterTest, ActionOutsideTheTableRunsAsAPlainAction)
{
    auto order = makeOrder(2);
    addAction(order.nodes[1], "MutedOn");

    const auto missions = convertOrder(order);

    ASSERT_EQ(missions.size(), 1u) << "action thường không được mở rộng";
    ASSERT_EQ(missions[0]->actions.size(), 1u);
    EXPECT_EQ(missions[0]->actions[0].action.actionType, "MutedOn");
}

TEST_F(AdapterTest, RelativeMoveStepBecomesALegWithADistance)
{
    auto order = makeOrder(2);
    addAction(order.nodes[1], "UnDockFromStation");

    const auto missions = convertOrder(order);
    ASSERT_EQ(missions.size(), 2u);

    EXPECT_TRUE(missions[1]->has_goal);
    EXPECT_EQ(missions[1]->motion_hint, "go_straight");
    EXPECT_DOUBLE_EQ(missions[1]->relative_distance, -1.0);
    EXPECT_TRUE(missions[1]->goal_frame.empty());
}

TEST_F(AdapterTest, FixedFrameStepDoesNotNeedAnyParameter)
{
    auto order = makeOrder(2);
    addAction(order.nodes[1], "FixedDock");

    const auto missions = convertOrder(order);
    ASSERT_EQ(missions.size(), 2u);
    EXPECT_EQ(missions[1]->goal_frame, "dock_target");
    EXPECT_EQ(missions[1]->marker, "trolley");
}

TEST_F(AdapterTest, MissingStructuralParameterDropsTheWholeOrder)
{
    auto order = makeOrder(2);
    addAction(order.nodes[1], "charge");   // thiếu goal_frame

    // 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. Và hàng đợi đang chạy không bị đụng tới (A1).
    EXPECT_TRUE(convertOrder(order).empty());
}

TEST(CompoundConfig, RejectsAStepWithTwoKeywords)
{
    robot::NodeHandle nh;
    VDA5050SourceAdapter adapter;
    EXPECT_FALSE(adapter.configure("vda5050_bad_step", nh))
        << "không đúng một từ khoá thì không có cách diễn giải nào hiển nhiên đúng";
}

TEST(CompoundConfig, RejectsARecursiveCompound)
{
    robot::NodeHandle nh;
    VDA5050SourceAdapter adapter;
    EXPECT_FALSE(adapter.configure("vda5050_recursive", nh))
        << "expander sẽ mở rộng chính output của mình";
}

TEST(CompoundConfig, RejectsInvalidOrActionOnlyProfiles)
{
    robot::NodeHandle nh;
    VDA5050SourceAdapter adapter;
    EXPECT_FALSE(adapter.configure("vda5050_bad_profile", nh));
}

int main(int argc, char** argv)
{
    // Bảng `compound_actions` nằm trong cây config test. `overwrite = 0` để shell vẫn override được
    // khi cần chạy tay với cây khác — cùng cách `plugin_registry_test` làm.
#ifdef MISSION_ADAPTERS_TEST_CONFIG_DIR
    setenv("PNKX_NAV_CORE_CONFIG_DIR", MISSION_ADAPTERS_TEST_CONFIG_DIR, 0);
#endif

    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}