move_base2/test/walking_skeleton_test.cpp

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * move_base2 — test bộ khung chạy end-to-end với thành phần giả.
 *
 * Đây là test chứng minh contract giữa các cổng đã khớp: nhận yêu cầu, đi qua đủ state, gọi đúng
 * cổng, phát đúng lệnh vận tốc, và báo kết quả đúng một lần.
 *
 * Author: DuongTD
 *********************************************************************/
#include <gtest/gtest.h>

#include <algorithm>
#include <string>
#include <vector>

#include <move_base2/control_loop.h>

#include "fake_ports.h"

using move_base2::ControlLoop;
using move_base2::ControlLoopConfig;
using move_base2::ControlLoopDeps;
using move_base2::MotionProfile;
using move_base2::NavigationRequest;
using move_base2::NavigationState;
using move_base2::RecoveryTrigger;
using move_base2::testing::ActionScript;
using move_base2::testing::ControllerScript;
using move_base2::testing::FakeActionPort;
using move_base2::testing::FakeClockPort;
using move_base2::testing::FakeControllerPort;
using move_base2::testing::FakeMissionPort;
using move_base2::testing::FakePlannerPort;
using move_base2::testing::FakePosePort;
using move_base2::testing::FakeRecoveryPort;
using move_base2::testing::PlannerScript;
using move_base2::testing::RecoveryScript;

namespace
{

constexpr double kControlPeriod = 0.05;  ///< [s]

ControlLoopConfig baseConfig()
{
  ControlLoopConfig config;

  config.state_machine.planner_patience = 0.5;      // [s]
  config.state_machine.controller_patience = 0.5;   // [s]
  config.state_machine.oscillation_timeout = 0.0;   // tắt
  config.state_machine.oscillation_distance = 0.5;  // [m]
  config.state_machine.max_planning_retries = -1;
  config.state_machine.recovery_behavior_count = 2;
  config.state_machine.recovery_enabled = true;

  config.velocity.max_vel_x = 0.5;          // [m/s]
  config.velocity.min_vel_x = -0.2;         // [m/s]
  config.velocity.max_vel_theta = 1.0;      // [rad/s]
  config.velocity.max_accel_x = 100.0;      // [m/s^2] lớn để test tập trung vào state, không vào ramp
  config.velocity.max_accel_theta = 100.0;  // [rad/s^2]

  config.nominal_control_period = kControlPeriod;

  config.position.global_planner_name = "FakeGlobalPlanner";
  config.position.local_planner_name = "FakeLocalPlanner";
  config.position.default_xy_tolerance = 0.15;   // [m]
  config.position.default_yaw_tolerance = 0.10;  // [rad]

  config.docking = config.position;
  config.docking.local_planner_name = "FakeDockPlanner";
  config.go_straight = config.position;
  config.go_straight.local_planner_name = "FakeStraightPlanner";
  config.rotate = config.position;
  config.rotate.local_planner_name = "FakeRotatePlanner";

  return config;
}

NavigationRequest makeRequest(double goal_x, std::uint64_t sequence_id = 0)
{
  NavigationRequest request;
  request.profile = MotionProfile::kPosition;
  request.goal.header.frame_id = "map";
  request.goal.pose.position.x = goal_x;
  request.goal.pose.orientation.w = 1.0;
  request.mission_sequence_id = sequence_id;
  return request;
}

/// @brief Gắn @p count action tên "act_<i>" vào yêu cầu — kiểm được thứ tự tới ActionPort (D8).
NavigationRequest withActions(NavigationRequest request, std::size_t count)
{
  for (std::size_t i = 0; i < count; ++i)
  {
    robot_protocol_msgs::Action action;
    action.actionType = "act_" + std::to_string(i);
    request.actions.push_back(action);
  }
  return request;
}

/// @brief Yêu cầu chỉ-có-action (D8): has_goal = false, goal để mặc định — không được validate.
NavigationRequest makeActionOnlyRequest(std::size_t count, std::uint64_t sequence_id = 0)
{
  NavigationRequest request;
  request.has_goal = false;
  request.mission_sequence_id = sequence_id;
  return withActions(request, count);
}

/**
 * @class Fixture
 * @brief Dựng sẵn control loop nối với toàn bộ cổng giả.
 */
class Fixture
{
public:
  explicit Fixture(const ControlLoopConfig& config = baseConfig()) : recovery_(2)
  {
    pose_.setPosition(0.0, 0.0);

    deps_.clock = &clock_;
    deps_.pose = &pose_;
    deps_.planner = &planner_;
    deps_.controller = &controller_;
    deps_.recovery = &recovery_;
    deps_.mission = &mission_;
    deps_.action = &action_;

    std::string error;
    EXPECT_TRUE(loop_.configure(config, deps_, error)) << error;
  }

  /// @brief Chạy tối đa @p max_cycles cycle, ghi lại chuỗi state (đã nén lặp liên tiếp).
  void run(std::size_t max_cycles = 200)
  {
    for (std::size_t i = 0; i < max_cycles; ++i)
    {
      record();
      if (!loop_.step())
      {
        record();
        return;
      }
      clock_.advance(kControlPeriod);
    }
    hit_limit_ = true;
  }

  /// @brief Chạy đúng một cycle và ghi lại state sau cycle đó.
  void stepOnce()
  {
    loop_.step();
    clock_.advance(kControlPeriod);
    record();
  }

  const std::vector<std::string>& states() const
  {
    return states_;
  }

  bool hitLimit() const
  {
    return hit_limit_;
  }

  ControlLoop loop_;
  FakeClockPort clock_;
  FakePosePort pose_;
  FakePlannerPort planner_;
  FakeControllerPort controller_;
  FakeRecoveryPort recovery_;
  FakeMissionPort mission_;
  FakeActionPort action_;
  ControlLoopDeps deps_;

private:
  void record()
  {
    const std::string name = move_base2::toString(loop_.state());
    if (states_.empty() || states_.back() != name)
    {
      states_.push_back(name);
    }
  }

  std::vector<std::string> states_;
  bool hit_limit_ = false;
};

std::string join(const std::vector<std::string>& items)
{
  std::string out = "[";
  for (std::size_t i = 0; i < items.size(); ++i)
  {
    out += items[i];
    if (i + 1 < items.size())
    {
      out += ", ";
    }
  }
  return out + "]";
}

}  // namespace

// ================================================================================================
// Cấu hình và cửa vào
// ================================================================================================

TEST(ControlLoop, RefusesToConfigureWithMissingPorts)
{
  ControlLoop loop;
  ControlLoopDeps deps;  // toàn null

  std::string error;
  EXPECT_FALSE(loop.configure(baseConfig(), deps, error));
  EXPECT_NE(error.find("cổng"), std::string::npos);
  EXPECT_FALSE(loop.initialized());
}

TEST(ControlLoop, RefusesToConfigureWithInvalidVelocityLimits)
{
  Fixture fixture;
  ControlLoopConfig bad = baseConfig();
  bad.velocity.max_vel_x = -1.0;

  ControlLoop loop;
  std::string error;
  EXPECT_FALSE(loop.configure(bad, fixture.deps_, error));
}

TEST(ControlLoop, RefusesToConfigureWithoutPositionPlanner)
{
  Fixture fixture;
  ControlLoopConfig bad = baseConfig();
  bad.position.local_planner_name.clear();

  ControlLoop loop;
  std::string error;
  EXPECT_FALSE(loop.configure(bad, fixture.deps_, error));
  EXPECT_NE(error.find("position"), std::string::npos);
}

TEST(ControlLoop, RejectsGoalWithInvalidQuaternion)
{
  Fixture fixture;
  NavigationRequest request = makeRequest(3.0);
  request.goal.pose.orientation.w = 0.0;  // norm = 0

  std::string reason;
  EXPECT_FALSE(fixture.loop_.submit(request, reason));
  EXPECT_NE(reason.find("quaternion"), std::string::npos);
}

TEST(ControlLoop, RejectsGoalWithNonFiniteCoordinates)
{
  Fixture fixture;
  NavigationRequest request = makeRequest(3.0);
  request.goal.pose.position.x = std::numeric_limits<double>::quiet_NaN();

  std::string reason;
  EXPECT_FALSE(fixture.loop_.submit(request, reason));
}

TEST(ControlLoop, RejectsRequestWhenLocalPlannerCannotBeLoaded)
{
  Fixture fixture;
  fixture.controller_.setSwapSucceeds(false);

  std::string reason;
  EXPECT_FALSE(fixture.loop_.submit(makeRequest(3.0), reason));
  EXPECT_NE(reason.find("local planner"), std::string::npos);
}

TEST(ControlLoop, RejectsRequestWhenGlobalPlannerCannotBeLoaded)
{
  Fixture fixture;
  fixture.planner_.setSwapSucceeds(false);

  std::string reason;
  EXPECT_FALSE(fixture.loop_.submit(makeRequest(3.0), reason));
  EXPECT_NE(reason.find("global planner"), std::string::npos);
}

TEST(ControlLoop, ProfileSelectsItsOwnLocalPlannerAndTolerances)
{
  Fixture fixture;

  NavigationRequest docking = makeRequest(1.0);
  docking.profile = MotionProfile::kDocking;

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(docking, reason)) << reason;
  EXPECT_EQ(fixture.controller_.activeController(), "FakeDockPlanner");
  EXPECT_NEAR(fixture.controller_.xyTolerance(), 0.15, 1e-9) << "tolerance 0 -> dùng default profile";

  NavigationRequest rotate = makeRequest(1.0);
  rotate.profile = MotionProfile::kRotate;
  rotate.tolerance.yaw = 0.02;  // [rad]
  ASSERT_TRUE(fixture.loop_.submit(rotate, reason)) << reason;
  EXPECT_EQ(fixture.controller_.activeController(), "FakeRotatePlanner");
  EXPECT_NEAR(fixture.controller_.yawTolerance(), 0.02, 1e-9);
}

// ================================================================================================
// Đường đi thuận lợi
// ================================================================================================

TEST(ControlLoop, HappyPathReachesSucceededAndReportsOnce)
{
  Fixture fixture;
  fixture.planner_.setScript({PlannerScript::kOk});
  fixture.controller_.setScript({ControllerScript::kOk, ControllerScript::kOk,
                                 ControllerScript::kOk, ControllerScript::kGoalReached});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0, 42), reason)) << reason;

  fixture.run();

  EXPECT_FALSE(fixture.hitLimit());
  EXPECT_EQ(fixture.states(), (std::vector<std::string>{"IDLE", "PLANNING", "CONTROLLING",
                                                        "SUCCEEDED"}))
      << join(fixture.states());
  EXPECT_STREQ(fixture.loop_.lastOutcome(), "SUCCEEDED");
  EXPECT_EQ(fixture.loop_.outcomeReportCount(), 1u);
  EXPECT_EQ(fixture.mission_.reportCountFor(42), 1u)
      << "mỗi chặng chỉ được báo kết quả đúng một lần";
}

TEST(ControlLoop, DirectGoalWithoutMissionIdDoesNotTouchMissionLayer)
{
  Fixture fixture;
  fixture.controller_.setScript({ControllerScript::kOk, ControllerScript::kGoalReached});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0, 0), reason)) << reason;
  fixture.run();

  EXPECT_EQ(fixture.loop_.outcomeReportCount(), 1u);
  EXPECT_TRUE(fixture.mission_.reports().empty())
      << "goal trực tiếp không thuộc mission nào thì không báo lên mission layer";
}

TEST(ControlLoop, ControllerCommandIsPublishedWhileControlling)
{
  Fixture fixture;
  fixture.controller_.setNominalSpeed(0.3);
  fixture.controller_.setScript({ControllerScript::kOk});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;

  fixture.loop_.step();  // IDLE -> PLANNING (lập plan trong cycle này)
  fixture.clock_.advance(kControlPeriod);
  fixture.loop_.step();  // PLANNING -> CONTROLLING, chạy controller ngay

  ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);
  EXPECT_NEAR(fixture.loop_.lastCommand().linear.x, 0.3, 1e-9);
}

TEST(ControlLoop, OrderIsForwardedToThePlanner)
{
  Fixture fixture;
  NavigationRequest request = makeRequest(3.0);
  request.order = std::make_shared<robot_protocol_msgs::Order>();

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(request, reason)) << reason;

  fixture.loop_.step();
  EXPECT_TRUE(fixture.planner_.sawOrder());
}

// ================================================================================================
// Bất biến an toàn
// ================================================================================================

TEST(ControlLoop, NoVelocityIsEmittedOutsideControllingAndRecovering)
{
  Fixture fixture;
  fixture.controller_.setScript({ControllerScript::kOk, ControllerScript::kOk,
                                 ControllerScript::kGoalReached});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;

  for (int i = 0; i < 50; ++i)
  {
    const bool running = fixture.loop_.step();
    const NavigationState state = fixture.loop_.state();

    if (move_base2::mustBeStopped(state))
    {
      ASSERT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0)
          << "state " << move_base2::toString(state) << " ở cycle " << i;
      ASSERT_DOUBLE_EQ(fixture.loop_.lastCommand().angular.z, 0.0)
          << "state " << move_base2::toString(state) << " ở cycle " << i;
    }

    if (!running)
    {
      break;
    }
    fixture.clock_.advance(kControlPeriod);
  }
}

TEST(ControlLoop, NaNFromControllerIsBlockedByArbiter)
{
  Fixture fixture;
  fixture.controller_.setScript({ControllerScript::kNaN});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;

  fixture.loop_.step();
  fixture.clock_.advance(kControlPeriod);
  fixture.loop_.step();  // CONTROLLING, controller trả NaN

  EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0);
  EXPECT_GT(fixture.loop_.arbiter().nonFiniteRejections(), 0u);
}

TEST(ControlLoop, OverspeedCommandIsClampedNotPassedThrough)
{
  Fixture fixture;
  fixture.controller_.setScript({ControllerScript::kTooFast});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;

  fixture.loop_.step();
  fixture.clock_.advance(kControlPeriod);
  fixture.loop_.step();

  EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.5);
  EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().angular.z, 1.0);
  EXPECT_GT(fixture.loop_.arbiter().velocityClamps(), 0u);
}

TEST(ControlLoop, EmptyPlanIsTreatedAsFailureNotAsAValidPlan)
{
  // Fake cố ý vi phạm contract (trả true kèm plan rỗng) để kiểm guard của bên gọi: plan rỗng lọt
  // xuống sẽ thành front()/back() trên vector rỗng ở tầng dưới.
  Fixture fixture;
  fixture.planner_.setScript({PlannerScript::kEmpty});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;
  fixture.run();

  EXPECT_EQ(fixture.controller_.setPlanCount(), 0u) << "không được đẩy plan rỗng xuống controller";
  EXPECT_STREQ(fixture.loop_.lastOutcome(), "FAILED");
}

TEST(ControlLoop, LostPoseStopsTheRobotImmediately)
{
  Fixture fixture;
  fixture.controller_.setScript({ControllerScript::kOk});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;

  fixture.loop_.step();
  fixture.clock_.advance(kControlPeriod);
  fixture.loop_.step();
  ASSERT_GT(fixture.loop_.lastCommand().linear.x, 0.0);

  fixture.pose_.setAvailable(false);
  fixture.clock_.advance(kControlPeriod);
  fixture.loop_.step();

  EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0)
      << "mất TF thì phải dừng ngay, không đi tiếp bằng pose cũ";
}

// ================================================================================================
// Recovery
// ================================================================================================

TEST(ControlLoop, PlannerFailureDrivesRecoveryThenSucceeds)
{
  // Kịch bản thật: planner bế tắc cho tới khi recovery gỡ được thế, sau đó lập plan bình thường.
  // Lật kịch bản planner ngay khi quan sát thấy RECOVERING, thay vì đếm trước số cycle — đếm trước
  // làm test phụ thuộc vào đúng thời điểm hết kiên nhẫn, thứ không phải là thứ đang được kiểm.
  Fixture fixture;
  fixture.planner_.setScript({PlannerScript::kFail});
  fixture.controller_.setScript({ControllerScript::kOk, ControllerScript::kGoalReached});
  fixture.recovery_.setScript({RecoveryScript::kRunning, RecoveryScript::kSucceeded});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0, 7), reason)) << reason;

  bool planner_unblocked = false;
  std::vector<std::string> states;

  for (int i = 0; i < 200; ++i)
  {
    const std::string name = move_base2::toString(fixture.loop_.state());
    if (states.empty() || states.back() != name)
    {
      states.push_back(name);
    }

    if (!planner_unblocked && fixture.loop_.state() == NavigationState::kRecovering)
    {
      fixture.planner_.setScript({PlannerScript::kOk});
      planner_unblocked = true;
    }

    if (!fixture.loop_.step())
    {
      states.push_back(move_base2::toString(fixture.loop_.state()));
      break;
    }
    fixture.clock_.advance(kControlPeriod);
  }

  EXPECT_TRUE(planner_unblocked) << "không bao giờ vào recovery";
  EXPECT_EQ(states, (std::vector<std::string>{"IDLE", "PLANNING", "RECOVERING", "PLANNING",
                                              "CONTROLLING", "SUCCEEDED"}))
      << join(states);
  EXPECT_EQ(fixture.recovery_.startCount(), 1u);
  EXPECT_EQ(fixture.recovery_.lastStartIndex(), 0u);
  EXPECT_EQ(fixture.recovery_.lastTrigger(), RecoveryTrigger::kPlanningFailed);
  EXPECT_EQ(fixture.mission_.reportCountFor(7), 1u);
}

TEST(ControlLoop, AllRecoveriesExhaustedEndsInAbortedWithSingleReport)
{
  Fixture fixture;
  fixture.planner_.setScript({PlannerScript::kFail});  // luôn hỏng
  fixture.recovery_.setScript({RecoveryScript::kSucceeded});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0, 9), reason)) << reason;
  fixture.run();

  EXPECT_FALSE(fixture.hitLimit());
  EXPECT_EQ(fixture.states(),
            (std::vector<std::string>{"IDLE", "PLANNING", "RECOVERING", "PLANNING", "RECOVERING",
                                      "PLANNING", "ABORTED"}))
      << join(fixture.states());

  EXPECT_EQ(fixture.recovery_.startedIndices(), (std::vector<std::size_t>{0u, 1u}))
      << "phải chạy lần lượt từng behavior, không lặp lại behavior đầu";
  EXPECT_STREQ(fixture.loop_.lastOutcome(), "FAILED");
  EXPECT_EQ(fixture.loop_.outcomeReportCount(), 1u);
  EXPECT_EQ(fixture.mission_.reportCountFor(9), 1u);
}

TEST(ControlLoop, RecoveryRefusingToStartMovesOnToTheNextBehavior)
{
  Fixture fixture;
  fixture.planner_.setScript({PlannerScript::kFail});
  fixture.recovery_.setStartSucceeds(false);

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;
  fixture.run();

  EXPECT_EQ(fixture.recovery_.startedIndices(), (std::vector<std::size_t>{0u, 1u}));
  EXPECT_EQ(fixture.recovery_.updateCount(), 0u)
      << "không được tick một behavior chưa start thành công";
  EXPECT_STREQ(fixture.loop_.lastOutcome(), "FAILED");
}

TEST(ControlLoop, RecoveryVelocityGoesThroughArbiterWithOneHandoverCycle)
{
  Fixture fixture;
  fixture.planner_.setScript({PlannerScript::kFail});
  fixture.recovery_.setScript({RecoveryScript::kRunning, RecoveryScript::kRunning,
                               RecoveryScript::kSucceeded});
  fixture.recovery_.setRecoveryVelocity(true, -0.15);  // [m/s], âm = lùi
  fixture.controller_.setScript({ControllerScript::kOk});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;

  bool saw_reverse = false;
  for (int i = 0; i < 60; ++i)
  {
    const bool running = fixture.loop_.step();
    if (fixture.loop_.state() == NavigationState::kRecovering &&
        fixture.loop_.lastCommand().linear.x < -1e-6)
    {
      saw_reverse = true;
      EXPECT_GE(fixture.loop_.lastCommand().linear.x, -0.2) << "lệnh lùi phải nằm trong trần";
    }
    if (!running)
    {
      break;
    }
    fixture.clock_.advance(kControlPeriod);
  }

  EXPECT_TRUE(saw_reverse) << "recovery phát vận tốc nhưng lệnh không tới được đầu ra";
}

TEST(ControlLoop, RecoveryDisabledAbortsOnFirstFailure)
{
  ControlLoopConfig config = baseConfig();
  config.state_machine.recovery_enabled = false;
  config.state_machine.recovery_behavior_count = 0;

  Fixture fixture(config);
  fixture.planner_.setScript({PlannerScript::kFail});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;
  fixture.run();

  EXPECT_EQ(fixture.recovery_.startCount(), 0u);
  EXPECT_EQ(fixture.states(), (std::vector<std::string>{"IDLE", "PLANNING", "ABORTED"}))
      << join(fixture.states());
}

// ================================================================================================
// Pause / cancel
// ================================================================================================

TEST(ControlLoop, CancelWhileControllingStopsAndReportsCancelled)
{
  Fixture fixture;
  fixture.controller_.setScript({ControllerScript::kOk});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0, 5), reason)) << reason;

  fixture.loop_.step();
  fixture.clock_.advance(kControlPeriod);
  fixture.loop_.step();
  ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);
  ASSERT_GT(fixture.loop_.lastCommand().linear.x, 0.0);

  fixture.loop_.requestCancel();

  int cycles_to_zero = 0;
  for (int i = 0; i < 10; ++i)
  {
    fixture.clock_.advance(kControlPeriod);
    const bool running = fixture.loop_.step();
    ++cycles_to_zero;
    if (!running)
    {
      break;
    }
  }

  EXPECT_LE(cycles_to_zero, 2) << "cmd_vel phải về 0 trong vòng 2 cycle sau khi huỷ";
  EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0);
  EXPECT_EQ(fixture.loop_.state(), NavigationState::kCancelled);
  EXPECT_STREQ(fixture.loop_.lastOutcome(), "CANCELLED");
  EXPECT_EQ(fixture.mission_.reportCountFor(5), 1u);
}

TEST(ControlLoop, CancelDuringRecoveryStopsTheBehavior)
{
  Fixture fixture;
  fixture.planner_.setScript({PlannerScript::kFail});
  fixture.recovery_.setScript({RecoveryScript::kRunning});
  fixture.recovery_.setRecoveryVelocity(true, -0.15);

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;

  for (int i = 0; i < 40 && fixture.loop_.state() != NavigationState::kRecovering; ++i)
  {
    fixture.loop_.step();
    fixture.clock_.advance(kControlPeriod);
  }
  ASSERT_EQ(fixture.loop_.state(), NavigationState::kRecovering);

  fixture.loop_.requestCancel();
  fixture.loop_.step();
  fixture.clock_.advance(kControlPeriod);

  EXPECT_EQ(fixture.recovery_.cancelCount(), 1u);
  EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0);

  fixture.loop_.step();
  EXPECT_EQ(fixture.loop_.state(), NavigationState::kCancelled);
}

TEST(ControlLoop, PauseAndResumeMidControlling)
{
  Fixture fixture;
  fixture.controller_.setScript({ControllerScript::kOk});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;

  fixture.loop_.step();
  fixture.clock_.advance(kControlPeriod);
  fixture.loop_.step();
  ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);

  fixture.loop_.requestPause();
  fixture.clock_.advance(kControlPeriod);
  fixture.loop_.step();

  EXPECT_EQ(fixture.loop_.state(), NavigationState::kPaused);
  EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0);

  // Dừng lâu rồi tiếp tục: không được vì thế mà rơi vào recovery.
  fixture.clock_.advance(30.0);
  fixture.loop_.requestResume();
  fixture.loop_.step();

  EXPECT_EQ(fixture.loop_.state(), NavigationState::kControlling);
  EXPECT_GT(fixture.loop_.lastCommand().linear.x, 0.0);
}

// ================================================================================================
// Nhiều chặng liên tiếp
// ================================================================================================

TEST(ControlLoop, ThreeSequentialMissionLegsEachReportedExactlyOnce)
{
  Fixture fixture;

  for (std::uint64_t leg = 1; leg <= 3; ++leg)
  {
    fixture.planner_.setScript({PlannerScript::kOk});
    fixture.controller_.setScript({ControllerScript::kOk, ControllerScript::kGoalReached});

    std::string reason;
    ASSERT_TRUE(fixture.loop_.submit(makeRequest(static_cast<double>(leg), leg), reason)) << reason;

    for (int i = 0; i < 50; ++i)
    {
      const bool running = fixture.loop_.step();
      fixture.clock_.advance(kControlPeriod);
      if (!running)
      {
        break;
      }
    }

    ASSERT_EQ(fixture.loop_.state(), NavigationState::kSucceeded) << "chặng " << leg;
    ASSERT_EQ(fixture.mission_.reportCountFor(leg), 1u) << "chặng " << leg;
  }

  EXPECT_EQ(fixture.loop_.outcomeReportCount(), 3u);
  EXPECT_EQ(fixture.mission_.reports().size(), 3u);
}

// ================================================================================================
// Action của mission (D8)
// ================================================================================================

TEST(ControlLoopActions, ActionOnlyMissionNeverTouchesPlannerOrController)
{
  Fixture fixture;
  fixture.action_.setScript({ActionScript::kRunning, ActionScript::kSucceeded,
                             ActionScript::kRunning, ActionScript::kSucceeded});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeActionOnlyRequest(2, 7), reason)) << reason;
  fixture.run();

  ASSERT_FALSE(fixture.hitLimit()) << join(fixture.states());
  EXPECT_EQ(fixture.states(),
            (std::vector<std::string>{"IDLE", "EXECUTING_ACTIONS", "SUCCEEDED"}))
      << join(fixture.states());

  // Không một lời gọi nào tới planner/controller — và vì thế không một lệnh vận tốc nào khác 0.
  EXPECT_EQ(fixture.planner_.makePlanCount(), 0u);
  EXPECT_EQ(fixture.controller_.computeCount(), 0u);
  EXPECT_EQ(fixture.loop_.lastCommand().linear.x, 0.0);
  EXPECT_EQ(fixture.loop_.lastCommand().angular.z, 0.0);

  // Hai action tới port nguyên vẹn, đúng thứ tự; kết quả báo đúng một lần sau action cuối.
  EXPECT_EQ(fixture.action_.startedActionTypes(),
            (std::vector<std::string>{"act_0", "act_1"}));
  EXPECT_EQ(fixture.mission_.reportCountFor(7), 1u);
}

TEST(ControlLoopActions, MissionWithGoalRunsActionsAfterGoalReached)
{
  Fixture fixture;
  fixture.planner_.setScript({PlannerScript::kOk});
  fixture.controller_.setScript({ControllerScript::kOk, ControllerScript::kGoalReached});
  fixture.action_.setScript({ActionScript::kRunning, ActionScript::kSucceeded});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(withActions(makeRequest(2.0, 11), 1), reason)) << reason;
  fixture.run();

  ASSERT_FALSE(fixture.hitLimit()) << join(fixture.states());
  EXPECT_EQ(fixture.states(),
            (std::vector<std::string>{"IDLE", "PLANNING", "CONTROLLING", "EXECUTING_ACTIONS",
                                      "SUCCEEDED"}))
      << join(fixture.states());
  EXPECT_EQ(fixture.action_.startedActionTypes(), (std::vector<std::string>{"act_0"}));
  EXPECT_EQ(fixture.mission_.reportCountFor(11), 1u);
}

TEST(ControlLoopActions, ActionFailureAbortsAndReportsOnce)
{
  Fixture fixture;
  fixture.action_.setScript({ActionScript::kRunning, ActionScript::kFailed});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeActionOnlyRequest(1, 13), reason)) << reason;
  fixture.run();

  ASSERT_FALSE(fixture.hitLimit()) << join(fixture.states());
  EXPECT_EQ(fixture.loop_.state(), NavigationState::kAborted);
  EXPECT_EQ(fixture.mission_.reportCountFor(13), 1u);
  EXPECT_EQ(fixture.recovery_.startCount(), 0u) << "action hỏng không được kéo recovery vào";
}

TEST(ControlLoopActions, SubmitRejectsActionRequestWhenActionPortMissing)
{
  Fixture fixture;

  ControlLoop bare_loop;
  ControlLoopDeps deps = fixture.deps_;
  deps.action = nullptr;

  std::string error;
  ASSERT_TRUE(bare_loop.configure(baseConfig(), deps, error)) << error;

  std::string reason;
  EXPECT_FALSE(bare_loop.submit(makeActionOnlyRequest(1), reason));
  EXPECT_NE(reason.find("action port"), std::string::npos) << reason;

  // Không goal lẫn action thì bị từ chối bất kể có port hay không.
  EXPECT_FALSE(fixture.loop_.submit(makeActionOnlyRequest(0), reason));
  EXPECT_NE(reason.find("goal lẫn action"), std::string::npos) << reason;
}

// ================================================================================================
// Lập plan bất đồng bộ (kBusy)
//
// Global planner nặng mất hàng trăm ms, control loop chạy 20 Hz và là thread duy nhất phát cmd_vel.
// Các test dưới đây khoá lại điều kiện để tách được hai nhịp đó mà state machine không phải đoán.
// ================================================================================================

TEST(ControlLoopAsyncPlanner, StaysInPlanningWhileThePlannerIsStillWorking)
{
  Fixture fixture;
  fixture.planner_.setLatencyCycles(3);
  fixture.controller_.setScript({ControllerScript::kOk});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;

  fixture.stepOnce();  // IDLE -> PLANNING, kick lượt lập plan
  ASSERT_EQ(fixture.loop_.state(), NavigationState::kPlanning);

  for (int i = 0; i < 3; ++i)
  {
    fixture.stepOnce();
    EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning)
        << "rời PLANNING khi planner còn đang tính, cycle " << i;
  }

  fixture.stepOnce();
  EXPECT_EQ(fixture.loop_.state(), NavigationState::kControlling);
}

TEST(ControlLoopAsyncPlanner, DoesNotRestartAPlanThatIsAlreadyRunning)
{
  // `start_planner` là tín hiệu MỨC, bật lại mỗi cycle chừng nào còn ở PLANNING. Kick lại một lượt
  // đang chạy sẽ làm mất toàn bộ thời gian đã bỏ ra cho nó.
  Fixture fixture;
  fixture.planner_.setLatencyCycles(4);

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;

  for (int i = 0; i < 4; ++i)
  {
    fixture.stepOnce();
  }

  EXPECT_EQ(fixture.planner_.makePlanCount(), 1u)
      << "lượt lập plan bị khởi động lại mỗi cycle";
}

TEST(ControlLoopAsyncPlanner, PlannerPatienceStillFiresWhileThePlannerIsBusy)
{
  // Đây là thứ DUY NHẤT phát hiện được thread planner treo. Ở chế độ đồng bộ, planner treo làm
  // treo luôn control loop nên còn dễ thấy; ở chế độ bất đồng bộ nó im lặng hoàn toàn.
  ControlLoopConfig config = baseConfig();
  config.state_machine.planner_patience = 0.2;  // [s] = 4 cycle
  config.state_machine.max_planning_retries = -1;

  Fixture fixture(config);
  fixture.planner_.setLatencyCycles(1000);  // không bao giờ xong

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;

  for (int i = 0; i < 20; ++i)
  {
    fixture.stepOnce();
  }

  // Planner treo vĩnh viễn thì đường đi đúng là: hết kiên nhẫn -> recovery -> thử lại -> hết
  // recovery -> ABORTED. Điều phải khoá lại là nó KHÔNG đứng im ở PLANNING.
  EXPECT_NE(std::find(fixture.states().begin(), fixture.states().end(), "RECOVERING"),
            fixture.states().end())
      << "planner treo mà không ai escalate — robot đứng ở PLANNING vĩnh viễn: "
      << join(fixture.states());
  EXPECT_EQ(fixture.loop_.state(), NavigationState::kAborted) << join(fixture.states());
}

TEST(ControlLoopAsyncPlanner, KeepsFollowingTheOldPlanWhileReplanningInBackground)
{
  // Chính là lý do tồn tại của cả mô hình: lập lại plan không được làm gián đoạn việc bám plan cũ.
  ControlLoopConfig config = baseConfig();
  config.state_machine.controller_patience = 100.0;  // [s] không cho controller hết kiên nhẫn
  Fixture fixture(config);

  fixture.controller_.setNominalSpeed(0.3);  // [m/s]
  fixture.controller_.setScript({ControllerScript::kOk});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;

  fixture.stepOnce();  // PLANNING
  fixture.stepOnce();  // -> CONTROLLING
  ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);

  for (int i = 0; i < 5; ++i)
  {
    fixture.stepOnce();
    EXPECT_EQ(fixture.loop_.state(), NavigationState::kControlling);
    EXPECT_NEAR(fixture.loop_.lastCommand().linear.x, 0.3, 1e-9)
        << "cmd_vel gián đoạn trong lúc lập lại plan, cycle " << i;
  }
}

TEST(ControlLoopAsyncPlanner, CancelWhilePlanningCancelsTheRunningPlan)
{
  // Không huỷ thì lượt đang bay vẫn về và chiếm chỗ hộp thư, rồi bị nhận nhầm cho yêu cầu kế tiếp.
  Fixture fixture;
  fixture.planner_.setLatencyCycles(100);

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;

  fixture.stepOnce();
  ASSERT_EQ(fixture.loop_.state(), NavigationState::kPlanning);
  ASSERT_TRUE(fixture.planner_.isPlanning());

  fixture.loop_.requestCancel();
  fixture.stepOnce();

  EXPECT_GE(fixture.planner_.cancelCount(), 1u);
  EXPECT_FALSE(fixture.planner_.isPlanning());
}

TEST(ControlLoopAsyncPlanner, AcceptingANewRequestCancelsAnInFlightPlan)
{
  Fixture fixture;
  fixture.planner_.setLatencyCycles(100);

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;
  fixture.stepOnce();
  ASSERT_TRUE(fixture.planner_.isPlanning());

  const std::size_t before = fixture.planner_.cancelCount();
  fixture.loop_.requestCancel();
  fixture.stepOnce();
  fixture.stepOnce();

  ASSERT_TRUE(fixture.loop_.submit(makeRequest(5.0), reason)) << reason;
  fixture.stepOnce();

  EXPECT_GT(fixture.planner_.cancelCount(), before)
      << "yêu cầu mới không huỷ lượt lập plan của goal cũ";
}

TEST(ControlLoopAsyncPlanner, FailureToStartAPlanIsTreatedAsAFailedAttempt)
{
  // Không khởi động được mà im lặng thì state machine đứng ở PLANNING vô hạn: không có kFailed để
  // đếm lượt, không có kPlanReady để đi tiếp.
  ControlLoopConfig config = baseConfig();
  config.state_machine.max_planning_retries = 2;
  Fixture fixture(config);
  fixture.planner_.setSwapSucceeds(true);
  fixture.planner_.setScript({PlannerScript::kFail});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;

  fixture.run();

  EXPECT_FALSE(fixture.hitLimit()) << join(fixture.states());
  EXPECT_NE(std::find(fixture.states().begin(), fixture.states().end(), "RECOVERING"),
            fixture.states().end())
      << join(fixture.states());
}

// ================================================================================================
// Preempt — goal mới thay goal cũ NGAY
//
// Bấm goal mới nghĩa là goal cũ không còn muốn nữa. Xếp hàng chờ robot đi hết chặng cũ là hành vi
// không ai mong đợi, và mission layer cũng đã chốt "preempt ngay" (Q1, Phase 2).
// ================================================================================================

TEST(ControlLoopPreempt, NewGoalWhileControllingReplansImmediately)
{
  Fixture fixture;
  fixture.controller_.setScript({ControllerScript::kOk});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;
  fixture.stepOnce();  // IDLE -> PLANNING
  fixture.stepOnce();  // -> CONTROLLING
  ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);

  const std::size_t plans_before = fixture.planner_.makePlanCount();

  ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0), reason)) << reason;
  fixture.stepOnce();

  EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning)
      << "goal mới nằm chờ thay vì thay goal cũ ngay";
  EXPECT_GT(fixture.planner_.makePlanCount(), plans_before) << "không lập plan lại cho goal mới";
}

TEST(ControlLoopPreempt, NewGoalWhilePlanningReplansImmediately)
{
  Fixture fixture;
  fixture.planner_.setLatencyCycles(50);  // plan cũ còn lâu mới xong

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;
  fixture.stepOnce();
  ASSERT_EQ(fixture.loop_.state(), NavigationState::kPlanning);

  ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0), reason)) << reason;
  fixture.stepOnce();

  EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning);
  EXPECT_GE(fixture.planner_.cancelCount(), 1u) << "lượt lập plan của goal cũ không bị huỷ";
}

TEST(ControlLoopPreempt, OldMissionIsReportedPreemptedExactlyOnceUnderItsOwnId)
{
  // Bất biến quan trọng nhất: báo kết quả ĐÚNG MỘT LẦN và ĐÚNG ID. Preempt báo kết quả chặng cũ và
  // nhận chặng mới trong cùng một cycle — dùng nhầm id thì mission layer mất dấu cả hai chặng.
  Fixture fixture;
  fixture.controller_.setScript({ControllerScript::kOk});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0, 11), reason)) << reason;
  fixture.stepOnce();
  fixture.stepOnce();
  ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);

  ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0, 22), reason)) << reason;
  fixture.stepOnce();

  EXPECT_EQ(fixture.mission_.reportCountFor(11), 1u) << "chặng bị thay không được báo đúng một lần";
  EXPECT_EQ(fixture.mission_.reportCountFor(22), 0u) << "chặng MỚI bị báo kết quả ngay khi nhận";
}

TEST(ControlLoopPreempt, PreemptedGoalStillFinishesTheNewOne)
{
  // Preempt không được để lại trạng thái nửa vời: chặng mới phải chạy tới cùng như bình thường.
  Fixture fixture;
  fixture.controller_.setScript({ControllerScript::kOk, ControllerScript::kOk,
                                 ControllerScript::kGoalReached});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0, 11), reason)) << reason;
  fixture.stepOnce();
  fixture.stepOnce();

  ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0, 22), reason)) << reason;
  fixture.run();

  EXPECT_FALSE(fixture.hitLimit()) << join(fixture.states());
  EXPECT_STREQ(fixture.loop_.lastOutcome(), "SUCCEEDED");
  EXPECT_EQ(fixture.mission_.reportCountFor(22), 1u);
}

TEST(ControlLoopPreempt, NewGoalDuringRecoveryCancelsTheRunningBehavior)
{
  ControlLoopConfig config = baseConfig();
  config.state_machine.planner_patience = 0.05;  // [s] vào recovery nhanh
  Fixture fixture(config);
  fixture.planner_.setScript({PlannerScript::kFail});
  fixture.recovery_.setScript({RecoveryScript::kRunning});

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;
  for (int i = 0; i < 6 && fixture.loop_.state() != NavigationState::kRecovering; ++i)
  {
    fixture.stepOnce();
  }
  ASSERT_EQ(fixture.loop_.state(), NavigationState::kRecovering) << join(fixture.states());

  fixture.planner_.setScript({PlannerScript::kOk});
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0), reason)) << reason;
  fixture.stepOnce();

  EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning);
  EXPECT_GE(fixture.recovery_.cancelCount(), 1u) << "behavior đang chạy không được bảo dừng";
}

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