mission_adapters/test/mission_manager_test.cpp

#include <gtest/gtest.h>

#include <chrono>
#include <thread>
#include <vector>

#include <mission_adapters/mission_manager.h>

#include "mission_test_utils.h"

namespace
{
using namespace mission_adapters;
using mission_test::makeMission;
using mission_test::makeMissions;

TEST(MissionManagerTest, ManagerRunsMissionLifecycle)
{
    MissionManager manager;

    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(1.0, 2.0)});
    EXPECT_EQ(manager.state(), MissionState::QUEUED);
    EXPECT_TRUE(manager.hasMission());

    auto mission = manager.nextMission();
    ASSERT_NE(mission, nullptr);
    EXPECT_NE(mission->id, kInvalidMissionId) << "submit must return a MissionId";
    EXPECT_EQ(manager.state(), MissionState::RUNNING);

    EXPECT_TRUE(manager.onNavigationDone(mission->id));
    EXPECT_EQ(manager.state(), MissionState::COMPLETED);
    EXPECT_FALSE(manager.hasMission());
}

TEST(MissionManagerTest, ManagerHandlesPauseResumeCancelAndFailure)
{
    MissionManager manager;

    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(1.0, 1.0)});
    manager.pause();
    EXPECT_EQ(manager.state(), MissionState::PAUSED);

    manager.resume();
    EXPECT_EQ(manager.state(), MissionState::QUEUED);

    manager.cancel();
    EXPECT_EQ(manager.state(), MissionState::CANCELLED);
    EXPECT_FALSE(manager.hasMission());

    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(2.0, 2.0)});
    const auto mission = manager.nextMission();
    ASSERT_NE(mission, nullptr);
    EXPECT_TRUE(manager.onNavigationFailed(mission->id));
    EXPECT_EQ(manager.state(), MissionState::FAILED);
    EXPECT_FALSE(manager.hasMission());
}

TEST(MissionManagerTest, NavigationResultIsIgnoredOutsideRunningState)
{
    MissionManager manager;

    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(1.0, 1.0)});
    const MissionId queued_id = manager.nextMission()->id;

    manager.emergency();
    EXPECT_FALSE(manager.onNavigationDone(queued_id));
    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);

    EXPECT_FALSE(manager.onNavigationFailed(queued_id));
    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);

    manager.clearEmergency();
    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(2.0, 2.0)});
    manager.cancel();
    EXPECT_FALSE(manager.onNavigationDone(queued_id));
    EXPECT_EQ(manager.state(), MissionState::CANCELLED);
}

TEST(MissionManagerTest, EmergencyClearsAndBlocksNewMissionsUntilCleared)
{
    MissionManager manager;

    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(1.0, 1.0)});
    manager.emergency();
    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
    EXPECT_FALSE(manager.hasMission());

    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(2.0, 2.0)});
    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
    EXPECT_FALSE(manager.hasMission());

    manager.clearEmergency();
    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(3.0, 3.0)});
    EXPECT_EQ(manager.state(), MissionState::QUEUED);
    EXPECT_TRUE(manager.hasMission());
}

// ─────────────────────────────────────────────────────────────────────────────
// A1 — submit rỗng không được đụng vào hàng đợi đang chạy.
//
// Một order lỗi từ fleet manager (convert() trả rỗng) không phải là lệnh "huỷ mọi thứ". Nếu nó xoá
// queue thì chặng đang chạy mất chủ: robot vẫn đi tới goal cũ vì không ai bảo navigation dừng, còn
// mission layer thì kẹt ở RUNNING mà không còn mission nào để hoàn tất.
// ─────────────────────────────────────────────────────────────────────────────

TEST(MissionManagerTest, EmptySubmitLeavesRunningMissionUntouched)
{
    MissionManager manager;

    manager.submit(makeMissions(2));

    const auto running = manager.nextMission();
    ASSERT_NE(running, nullptr);
    ASSERT_EQ(manager.state(), MissionState::RUNNING);

    // Order lỗi -> convert() rỗng -> submit rỗng.
    manager.submit({});

    EXPECT_EQ(manager.state(), MissionState::RUNNING);
    EXPECT_TRUE(manager.hasMission()) << "an empty submit wiped the running mission and the queue";
    EXPECT_EQ(manager.currentMissionId(), running->id) << "the running mission was replaced";
    EXPECT_EQ(manager.takePendingCancel(), kInvalidMissionId)
        << "an empty submit must not ask navigation to stop";

    // Chặng thứ hai vẫn phải còn trong hàng đợi.
    EXPECT_TRUE(manager.onNavigationDone(running->id));
    EXPECT_EQ(manager.state(), MissionState::QUEUED);
}

// ─────────────────────────────────────────────────────────────────────────────
// A2 — outcome phải mang MissionId.
//
// Không có ID thì kết quả của chặng cũ (đến trễ, sau khi order đã bị thay) được ghi nhận cho chặng
// mới: mission mới "hoàn thành" trong khi robot chưa hề chạy nó.
// ─────────────────────────────────────────────────────────────────────────────

TEST(MissionManagerTest, StaleOutcomeOfPreemptedMissionIsRejected)
{
    MissionManager manager;

    manager.submit(makeMissions(1));
    const auto first = manager.nextMission();
    ASSERT_NE(first, nullptr);

    // Order mới thay order cũ trong lúc chặng đầu đang chạy.
    manager.submit(makeMissions(1));
    const auto second = manager.nextMission();
    ASSERT_NE(second, nullptr);
    ASSERT_NE(first->id, second->id);

    // Outcome của chặng ĐẦU về muộn, sau khi chặng hai đã bắt đầu.
    EXPECT_FALSE(manager.onNavigationDone(first->id));
    EXPECT_EQ(manager.state(), MissionState::RUNNING) << "a late outcome completed the wrong "
                                                         "mission";
    EXPECT_EQ(manager.currentMissionId(), second->id);

    // Outcome đúng của chặng hai vẫn được nhận bình thường.
    EXPECT_TRUE(manager.onNavigationDone(second->id));
    EXPECT_EQ(manager.state(), MissionState::COMPLETED);
}

TEST(MissionManagerTest, MissionIdsAreUniqueAndMonotonic)
{
    MissionManager manager;

    manager.submit(makeMissions(3));

    MissionId previous = kInvalidMissionId;
    for (int i = 0; i < 3; ++i)
    {
        const auto mission = manager.nextMission();
        ASSERT_NE(mission, nullptr) << "leg number " << i;
        EXPECT_GT(mission->id, previous);
        previous = mission->id;

        ASSERT_TRUE(manager.onNavigationDone(mission->id));
    }

    EXPECT_EQ(manager.state(), MissionState::COMPLETED);
}

TEST(MissionManagerTest, RunningMissionIsDispatchedOnlyOnce)
{
    MissionManager manager;

    manager.submit(makeMissions(2));

    const auto first = manager.nextMission();
    ASSERT_NE(first, nullptr);

    // M6: trong lúc RUNNING không có mission mới nào để giao.
    EXPECT_EQ(manager.nextMission(), nullptr);
    EXPECT_EQ(manager.nextMission(), nullptr);

    ASSERT_TRUE(manager.onNavigationDone(first->id));
    const auto second = manager.nextMission();
    ASSERT_NE(second, nullptr);
    EXPECT_NE(second->id, first->id);
}

// ─────────────────────────────────────────────────────────────────────────────
// A3 — mission layer phải có đường bảo navigation dừng.
// ─────────────────────────────────────────────────────────────────────────────

TEST(MissionManagerTest, CancelRequestsNavigationStopOfRunningMission)
{
    MissionManager manager;

    manager.submit(makeMissions(2));
    const auto running = manager.nextMission();
    ASSERT_NE(running, nullptr);

    manager.cancel();

    EXPECT_EQ(manager.takePendingCancel(), running->id);
    EXPECT_EQ(manager.takePendingCancel(), kInvalidMissionId) << "the stop request was taken twice";
    EXPECT_EQ(manager.state(), MissionState::CANCELLED);
    EXPECT_FALSE(manager.hasMission());
}

TEST(MissionManagerTest, EmergencyRequestsNavigationStopOfRunningMission)
{
    MissionManager manager;

    manager.submit(makeMissions(1));
    const auto running = manager.nextMission();
    ASSERT_NE(running, nullptr);

    manager.emergency();

    EXPECT_EQ(manager.takePendingCancel(), running->id);
    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
}

TEST(MissionManagerTest, PreemptRequestsNavigationStopOfReplacedMission)
{
    MissionManager manager;

    manager.submit(makeMissions(1));
    const auto first = manager.nextMission();
    ASSERT_NE(first, nullptr);

    manager.submit(makeMissions(1));

    EXPECT_EQ(manager.takePendingCancel(), first->id)
        << "a new order replaced the old one without telling navigation to stop the old leg";
    EXPECT_EQ(manager.state(), MissionState::QUEUED);
}

TEST(MissionManagerTest, SubmitWhilePausedKeepsRobotStopped)
{
    MissionManager manager;

    manager.submit(makeMissions(1));
    manager.pause();

    manager.submit(makeMissions(1));

    EXPECT_EQ(manager.state(), MissionState::PAUSED) << "a new order started running on its own "
                                                        "while PAUSED";
    EXPECT_EQ(manager.nextMission(), nullptr);

    manager.resume();
    EXPECT_EQ(manager.state(), MissionState::QUEUED);
    EXPECT_NE(manager.nextMission(), nullptr);
}

// ─────────────────────────────────────────────────────────────────────────────
// M8 — một chặng hỏng thì phần còn lại của hàng đợi đi đâu.
// ─────────────────────────────────────────────────────────────────────────────

TEST(MissionManagerTest, FailureClearsRemainingQueueByDefault)
{
    MissionManager manager;   // clear_queue_on_failure mặc định = true

    manager.submit(makeMissions(3));
    const auto first = manager.nextMission();
    ASSERT_NE(first, nullptr);

    ASSERT_TRUE(manager.onNavigationFailed(first->id));

    // Tuyến tuần tự: không tới được chặng 1 thì chặng 2 nằm sau nó cũng không còn ý nghĩa.
    EXPECT_EQ(manager.state(), MissionState::FAILED);
    EXPECT_FALSE(manager.hasMission());
}

TEST(MissionManagerTest, FailureKeepsQueueWhenConfigured)
{
    MissionConfig config;
    config.clear_queue_on_failure = false;   // các mission trong hàng đợi độc lập với nhau
    MissionManager manager(config);

    manager.submit(makeMissions(3));
    const auto first = manager.nextMission();
    ASSERT_NE(first, nullptr);

    ASSERT_TRUE(manager.onNavigationFailed(first->id));

    EXPECT_EQ(manager.state(), MissionState::QUEUED);
    EXPECT_TRUE(manager.hasMission());

    const auto second = manager.nextMission();
    ASSERT_NE(second, nullptr);
    EXPECT_NE(second->id, first->id);
}

TEST(MissionManagerTest, LastMissionFailureEndsInFailedEvenWhenQueueKept)
{
    MissionConfig config;
    config.clear_queue_on_failure = false;
    MissionManager manager(config);

    manager.submit(makeMissions(1));
    const auto only = manager.nextMission();
    ASSERT_NE(only, nullptr);

    ASSERT_TRUE(manager.onNavigationFailed(only->id));
    EXPECT_EQ(manager.state(), MissionState::FAILED);
}

// ─────────────────────────────────────────────────────────────────────────────
// M7 — mission_timeout là lưới cuối khi navigation không bao giờ báo kết quả về.
// ─────────────────────────────────────────────────────────────────────────────

TEST(MissionManagerTest, TimeoutDisabledByDefaultLetsMissionRunOn)
{
    MissionManager manager;

    manager.submit(makeMissions(1));
    ASSERT_NE(manager.nextMission(), nullptr);

    // waitForWork phải chặn (không có việc); wakeUp là đường ra duy nhất.
    std::thread waker([&manager] {
        std::this_thread::sleep_for(std::chrono::milliseconds(80));
        manager.wakeUp();
    });

    EXPECT_FALSE(manager.waitForWork());
    waker.join();

    EXPECT_EQ(manager.state(), MissionState::RUNNING) << "the mission was cancelled although the "
                                                         "timeout is off";
}

TEST(MissionManagerTest, TimeoutFailsMissionAndRequestsNavigationStop)
{
    MissionConfig config;
    config.mission_timeout = 0.15;   // [s]
    MissionManager manager(config);

    manager.submit(makeMissions(1));
    const auto running = manager.nextMission();
    ASSERT_NE(running, nullptr);

    const auto started = std::chrono::steady_clock::now();

    // waitForWork tự thức dậy đúng lúc hết hạn và biến quá hạn thành việc cần làm.
    ASSERT_TRUE(manager.waitForWork());

    const double elapsed =
        std::chrono::duration<double>(std::chrono::steady_clock::now() - started).count();
    EXPECT_GE(elapsed, 0.15) << "the leg was cancelled before it expired";

    EXPECT_EQ(manager.state(), MissionState::FAILED);
    EXPECT_EQ(manager.takePendingCancel(), running->id)
        << "it expired but navigation was not told to stop — the robot keeps driving to the old "
           "goal";
}

TEST(MissionManagerTest, TimeoutIsMeasuredPerMissionNotPerQueue)
{
    MissionConfig config;
    config.mission_timeout = 0.2;   // [s]
    config.clear_queue_on_failure = false;
    MissionManager manager(config);

    manager.submit(makeMissions(2));

    const auto first = manager.nextMission();
    ASSERT_NE(first, nullptr);

    // Chặng đầu xong sớm; mốc thời gian phải được gieo lại cho chặng hai.
    std::this_thread::sleep_for(std::chrono::milliseconds(120));
    ASSERT_TRUE(manager.onNavigationDone(first->id));

    const auto second = manager.nextMission();
    ASSERT_NE(second, nullptr);

    // Nếu mốc không được gieo lại, chặng hai sẽ hết hạn gần như tức thì.
    const auto started = std::chrono::steady_clock::now();
    ASSERT_TRUE(manager.waitForWork());
    const double elapsed =
        std::chrono::duration<double>(std::chrono::steady_clock::now() - started).count();

    EXPECT_GE(elapsed, 0.19) << "the second leg inherited the clock of the first one";
    EXPECT_EQ(manager.takePendingCancel(), second->id);
}

}  // namespace

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