#include <gtest/gtest.h>

#include <chrono>
#include <thread>

#include <mission_adapters/mission_adapters.h>
#include <mission_adapters/plugin_registry.h>

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

namespace
{
using namespace mission_adapters;
using mission_test::FakeNavigationClient;
using mission_test::makeGoal;
using mission_test::makeMission;
using mission_test::waitForState;

/// Chờ tới khi predicate đúng hoặc hết hạn. Trả giá trị cuối cùng của predicate.
template <typename Predicate>
bool waitFor(Predicate predicate, std::chrono::milliseconds timeout)
{
    const auto deadline = std::chrono::steady_clock::now() + timeout;
    while (std::chrono::steady_clock::now() < deadline)
    {
        if (predicate())
            return true;

        std::this_thread::sleep_for(std::chrono::milliseconds(5));
    }

    return predicate();
}

constexpr std::chrono::milliseconds kWaitTimeout{500};

class MissionLifecycleTest : public ::testing::Test
{
protected:
    void SetUp() override
    {
        // Đăng ký thẳng thay vì nạp .so: test này kiểm luồng mission, không kiểm đường Boost.DLL
        // (việc đó thuộc plugin_registry_test).
        ASSERT_TRUE(registry.registerAdapter(std::make_shared<mission_plugins::GoalSourceAdapter>()));
        ASSERT_TRUE(registry.registerAdapter(std::make_shared<mission_plugins::VDA5050SourceAdapter>()));
    }

    /// Một mission dựng thẳng, đóng gói trong vector để submit().
    std::vector<std::shared_ptr<Mission>> oneMission(double x, double y)
    {
        return {makeMission(x, y)};
    }

    PluginRegistry registry;
};

TEST_F(MissionLifecycleTest, EventProcessorProcessesGoalAndEmergency)
{
    MissionManager manager;
    EventProcessor processor(manager, registry);

    processor.start();
    processor.goalEvent(makeGoal(5.0, 6.0));
    EXPECT_TRUE(waitForState(manager, MissionState::QUEUED, kWaitTimeout));

    processor.emergencyEvent();
    EXPECT_TRUE(waitForState(manager, MissionState::EMERGENCY, kWaitTimeout));

    processor.goalEvent(makeGoal(7.0, 8.0));
    std::this_thread::sleep_for(std::chrono::milliseconds(50));
    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);

    processor.clearEmergencyEvent();
    EXPECT_TRUE(waitForState(manager, MissionState::CLEAR_EMERGENCY, kWaitTimeout));

    processor.stop();
}

TEST_F(MissionLifecycleTest, MissionExecutorDispatchesEachMissionOnce)
{
    MissionManager manager;
    MissionExecutor executor(manager);
    FakeNavigationClient client;

    executor.setNavigationClient(&client);
    manager.submit(oneMission(10.0, 10.0));
    executor.start();

    ASSERT_TRUE(waitFor([&client] { return client.dispatchCount() == 1u; }, kWaitTimeout));

    const auto first = client.dispatched().front();
    ASSERT_TRUE(manager.onNavigationDone(first->id));
    manager.submit(oneMission(11.0, 11.0));

    ASSERT_TRUE(waitFor([&client] { return client.dispatchCount() == 2u; }, kWaitTimeout));

    // Chặng thứ hai chưa xong -> không được giao thêm lần nào nữa.
    std::this_thread::sleep_for(std::chrono::milliseconds(120));
    executor.stop();

    EXPECT_EQ(client.dispatchCount(), 2u);
    EXPECT_TRUE(client.cancelled().empty());
}

// ─────────────────────────────────────────────────────────────────────────────
// A3 — cancel/emergency ở tầng mission phải tới được navigation.
// ─────────────────────────────────────────────────────────────────────────────

TEST_F(MissionLifecycleTest, CancelStopsNavigationExactlyOnce)
{
    MissionManager manager;
    MissionExecutor executor(manager);
    FakeNavigationClient client;

    executor.setNavigationClient(&client);
    manager.submit(oneMission(1.0, 1.0));
    executor.start();

    ASSERT_TRUE(waitFor([&client] { return client.dispatchCount() == 1u; }, kWaitTimeout));
    const MissionId running_id = client.dispatched().front()->id;

    manager.cancel();

    ASSERT_TRUE(waitFor([&client] { return !client.cancelled().empty(); }, kWaitTimeout));
    std::this_thread::sleep_for(std::chrono::milliseconds(120));
    executor.stop();

    ASSERT_EQ(client.cancelled().size(), 1u) << "cancelActive must be called exactly once";
    EXPECT_EQ(client.cancelled().front(), running_id);
    EXPECT_EQ(manager.state(), MissionState::CANCELLED);
}

TEST_F(MissionLifecycleTest, PreemptCancelsOldMissionBeforeDispatchingNewOne)
{
    MissionManager manager;
    MissionExecutor executor(manager);
    FakeNavigationClient client;

    executor.setNavigationClient(&client);
    manager.submit(oneMission(1.0, 1.0));
    executor.start();

    ASSERT_TRUE(waitFor([&client] { return client.dispatchCount() == 1u; }, kWaitTimeout));
    const MissionId old_id = client.dispatched().front()->id;

    // Order mới thay order cũ trong lúc chặng đầu đang chạy.
    manager.submit(oneMission(2.0, 2.0));

    ASSERT_TRUE(waitFor([&client] { return client.dispatchCount() == 2u; }, kWaitTimeout));
    executor.stop();

    ASSERT_EQ(client.cancelled().size(), 1u) << "the replaced leg was not told to stop";
    EXPECT_EQ(client.cancelled().front(), old_id);
    EXPECT_NE(client.dispatched().back()->id, old_id);
}

TEST_F(MissionLifecycleTest, RejectedDispatchFailsMissionInsteadOfHanging)
{
    MissionManager manager;
    MissionExecutor executor(manager);
    FakeNavigationClient client;

    client.setAcceptDispatch(false);
    executor.setNavigationClient(&client);
    manager.submit(oneMission(3.0, 3.0));
    executor.start();

    EXPECT_TRUE(waitForState(manager, MissionState::FAILED, kWaitTimeout))
        << "navigation rejected it yet the mission is still stuck in RUNNING";

    executor.stop();
    EXPECT_EQ(client.dispatchCount(), 1u) << "a rejected mission was handed over again on the next "
                                             "round";
}

TEST_F(MissionLifecycleTest, OrderProducingNoMissionLeavesQueueUntouched)
{
    MissionManager manager;
    EventProcessor processor(manager, registry);

    processor.start();
    processor.goalEvent(makeGoal(4.0, 4.0));
    ASSERT_TRUE(waitForState(manager, MissionState::QUEUED, kWaitTimeout));

    // Order thiếu edge -> convert() rỗng -> không được đụng vào hàng đợi hiện tại (A1).
    auto invalid_order = mission_test::makeOrder(4);
    invalid_order.edges.pop_back();
    processor.orderEvent(invalid_order);

    std::this_thread::sleep_for(std::chrono::milliseconds(80));
    processor.stop();

    EXPECT_EQ(manager.state(), MissionState::QUEUED);
    EXPECT_TRUE(manager.hasMission()) << "a failed order cleared the queue";
}

// ─────────────────────────────────────────────────────────────────────────────
// End-to-end: một Order đi hết đường qua cả ba thành phần.
// ─────────────────────────────────────────────────────────────────────────────

/// Dựng bộ ba thành phần đã nối dây sẵn, dùng chung cho các test end-to-end bên dưới.
struct MissionStack
{
    explicit MissionStack(PluginRegistry& registry)
        : processor(manager, registry)
        , executor(manager)
    {
        executor.setNavigationClient(&client);
        processor.start();
        executor.start();
    }

    ~MissionStack()
    {
        executor.stop();
        processor.stop();
    }

    MissionManager manager;
    EventProcessor processor;
    MissionExecutor executor;
    FakeNavigationClient client;
};

TEST_F(MissionLifecycleTest, OrderRunsThreeLegsThenCompletes)
{
    MissionStack stack(registry);

    // Order 4 node, action tại node 1 và node 2 -> cắt thành 3 chặng.
    auto order = mission_test::makeOrder(4);
    order.nodes[1].actions.push_back(mission_test::makeAction("lift"));
    order.nodes[2].actions.push_back(mission_test::makeAction("drop"));

    stack.processor.orderEvent(order);

    for (int leg = 0; leg < 3; ++leg)
    {
        const size_t expected = static_cast<size_t>(leg) + 1;
        ASSERT_TRUE(waitFor([&stack, expected] { return stack.client.dispatchCount() == expected; },
                            kWaitTimeout))
            << "leg number " << leg << " was not handed over";

        const auto mission = stack.client.dispatched().back();
        stack.processor.navDoneEvent(mission->id);
    }

    EXPECT_TRUE(waitForState(stack.manager, MissionState::COMPLETED, kWaitTimeout));
    EXPECT_EQ(stack.client.dispatchCount(), 3u);
    EXPECT_TRUE(stack.client.cancelled().empty());
}

TEST_F(MissionLifecycleTest, CancelMidOrderThenAcceptNewOrder)
{
    MissionStack stack(registry);

    auto order = mission_test::makeOrder(4);
    order.nodes[1].actions.push_back(mission_test::makeAction("lift"));
    order.nodes[2].actions.push_back(mission_test::makeAction("drop"));

    stack.processor.orderEvent(order);
    ASSERT_TRUE(waitFor([&stack] { return stack.client.dispatchCount() == 1u; }, kWaitTimeout));

    const MissionId running_id = stack.client.dispatched().front()->id;
    stack.processor.cancelEvent();

    ASSERT_TRUE(waitFor([&stack] { return !stack.client.cancelled().empty(); }, kWaitTimeout));
    EXPECT_EQ(stack.client.cancelled().front(), running_id);
    EXPECT_TRUE(waitForState(stack.manager, MissionState::CANCELLED, kWaitTimeout));

    // Sau khi huỷ vẫn nhận được order mới.
    stack.processor.orderEvent(mission_test::makeOrder(2, "order_moi"));

    ASSERT_TRUE(waitFor([&stack] { return stack.client.dispatchCount() == 2u; }, kWaitTimeout));
    EXPECT_EQ(stack.manager.state(), MissionState::RUNNING);
    EXPECT_NE(stack.client.dispatched().back()->id, running_id);
}

// A5 end-to-end: huỷ ngay sau khi phát order thì không mission nào được chạy.
TEST_F(MissionLifecycleTest, OrderThenImmediateCancelDispatchesNothing)
{
    MissionStack stack(registry);

    stack.processor.orderEvent(mission_test::makeOrder(3));
    stack.processor.cancelEvent();

    // Cho cả hai sự kiện chạy xong rồi mới kết luận.
    ASSERT_TRUE(waitForState(stack.manager, MissionState::CANCELLED, kWaitTimeout));
    std::this_thread::sleep_for(std::chrono::milliseconds(120));

    EXPECT_EQ(stack.client.dispatchCount(), 0u)
        << "the cancel was processed before the order so the order still runs — the robot drives "
           "what the user just cancelled";
    EXPECT_FALSE(stack.manager.hasMission());
}

TEST_F(MissionLifecycleTest, EmergencyOvertakesLongEventQueue)
{
    MissionStack stack(registry);

    // Nhồi hàng đợi bằng nhiều goal rồi mới bấm emergency.
    for (int i = 0; i < 50; ++i)
        stack.processor.goalEvent(makeGoal(i, i));

    stack.processor.emergencyEvent();

    EXPECT_TRUE(waitForState(stack.manager, MissionState::EMERGENCY, kWaitTimeout))
        << "emergency must queue-jump, not wait behind 50 events";
    EXPECT_FALSE(stack.manager.hasMission());
}

TEST_F(MissionLifecycleTest, ActionsReachNavigationIntactAndInOrder)
{
    MissionStack stack(registry);

    auto order = mission_test::makeOrder(3);
    order.edges[0].sequenceId = 1;
    order.edges[0].actions.push_back(mission_test::makeAction("beep"));
    order.nodes[1].sequenceId = 2;
    order.nodes[1].actions.push_back(mission_test::makeAction("lift"));

    stack.processor.orderEvent(order);
    ASSERT_TRUE(waitFor([&stack] { return stack.client.dispatchCount() == 1u; }, kWaitTimeout));

    const auto mission = stack.client.dispatched().front();
    ASSERT_EQ(mission->actions.size(), 2u) << "the action was lost on the way down to navigation";
    EXPECT_EQ(mission->actions[0].action.actionId, "beep");
    EXPECT_EQ(mission->actions[1].action.actionId, "lift");
    EXPECT_TRUE(mission->has_goal);
}

TEST_F(MissionLifecycleTest, GoallessMissionReachesNavigationWithItsActions)
{
    MissionStack stack(registry);

    // Action ngay tại node xuất phát -> chặng đầu không có quãng đường nào để đi.
    auto order = mission_test::makeOrder(3);
    order.nodes[0].actions.push_back(mission_test::makeAction("pick_at_start"));

    stack.processor.orderEvent(order);
    ASSERT_TRUE(waitFor([&stack] { return stack.client.dispatchCount() == 1u; }, kWaitTimeout));

    const auto mission = stack.client.dispatched().front();
    EXPECT_FALSE(mission->has_goal);
    ASSERT_EQ(mission->actions.size(), 1u);
    EXPECT_EQ(mission->actions.front().action.actionId, "pick_at_start");
}

}  // namespace

int main(int argc, char** argv)
{
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}