/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * move_base2 — test facade `NavigationServer`: đường lệnh vận tốc ra host, và đường dữ liệu cảm
 * biến từ host vào costmap.
 *
 * Author: DuongTD
 *********************************************************************/
#include <gtest/gtest.h>

#include <atomic>
#include <chrono>
#include <cmath>
#include <cstddef>
#include <limits>
#include <string>
#include <thread>
#include <vector>

#include <robot_costmap_2d/layered_costmap.h>

#include <move_base2/navigation_server.h>

#include "fake_ports.h"
#include "spy_layer.h"

using move_base2::ControlLoopConfig;
using move_base2::ControlLoopDeps;
using move_base2::MotionProfile;
using move_base2::NavigationRequest;
using move_base2::NavigationServer;
using move_base2::NavigationState;
using move_base2::SensorGatewayConfig;
using move_base2::testing::attachSpy;
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::SpyPtr;
using robot_costmap_2d::LayerType;

namespace
{

constexpr double kControlPeriod = 0.05;  ///< [s]
constexpr double kClockStart = 1000.0;   ///< [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 không vướng ramp
  config.velocity.max_accel_theta = 100.0;  // [rad/s^2]

  config.nominal_control_period = kControlPeriod;
  config.robot_base_frame = "base_link";

  config.position.global_planner_name = "FakeGlobalPlanner";
  config.position.local_planner_name = "FakeLocalPlanner";

  config.docking = config.position;
  config.go_straight = config.position;
  config.rotate = config.position;

  return config;
}

NavigationRequest makeRequest(double goal_x)
{
  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;
  return request;
}

robot_geometry_msgs::Vector3 makeVector(double x, double y = 0.0, double z = 0.0)
{
  robot_geometry_msgs::Vector3 v;
  v.x = x;
  v.y = y;
  v.z = z;
  return v;
}

robot_nav_msgs::Odometry makeOdometry(double vx, double wz)
{
  robot_nav_msgs::Odometry odom;
  odom.header.frame_id = "odom";
  odom.twist.twist.linear.x = vx;   // [m/s]
  odom.twist.twist.angular.z = wz;  // [rad/s]
  return odom;
}

robot_sensor_msgs::LaserScan makeScan(std::size_t rays = 40, float range = 1.0F)
{
  robot_sensor_msgs::LaserScan scan;
  scan.header.frame_id = "laser";
  scan.angle_min = -1.5F;                                  // [rad]
  scan.angle_max = 1.5F;                                   // [rad]
  scan.angle_increment = 3.0F / static_cast<float>(rays);  // [rad]
  scan.range_min = 0.05F;                                  // [m]
  scan.range_max = 10.0F;                                  // [m]
  scan.ranges.assign(rays, range);
  return scan;
}

/**
 * @class Fixture
 * @brief `NavigationServer` nối đủ cổng giả, cộng hai costmap thật để kiểm đường cảm biến.
 */
class Fixture
{
public:
  Fixture()
    : clock_(kClockStart)
    , recovery_(2)
    , global_("map", false, true)
    , local_("odom", true, false)
  {
    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_;
  }

  void configure(const ControlLoopConfig& config = baseConfig())
  {
    std::string error;
    ASSERT_TRUE(server_.configureLoop(config, deps_, error)) << error;
  }

  void configureSensors(const SensorGatewayConfig& config)
  {
    std::string error;
    ASSERT_TRUE(server_.configureSensors(config, error)) << error;
  }

  /// @brief Chạy @p cycles control cycle, mỗi cycle nhích đồng hồ giả một chu kỳ.
  void spin(std::size_t cycles)
  {
    for (std::size_t i = 0; i < cycles; ++i)
    {
      server_.spinOnce();
      clock_.advance(kControlPeriod);
    }
  }

  void attachCostmaps()
  {
    server_.attachCostmaps(&global_, &local_);
  }

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

  robot_costmap_2d::LayeredCostmap global_;
  robot_costmap_2d::LayeredCostmap local_;
};

}  // namespace

// ================================================================================================
// getTwist() — LỆNH vận tốc, không phải vận tốc đo được
//
// Host lấy getTwist() rồi publish thẳng ra /cmd_vel. Nếu giá trị đó đến từ odometry thì có một vòng
// lặp dương: robot chạy 0.5 m/s -> đọc odom 0.5 -> phát lệnh 0.5 -> mãi mãi. VelocityArbiter — toàn
// bộ hàng rào an toàn của gói — cũng bị bỏ qua hoàn toàn. Các test dưới đây khoá lại điều đó.
// ================================================================================================

TEST(NavigationServerTwist, ReturnsArbiterCommandNotOdometryVelocity)
{
  Fixture fixture;
  fixture.configure();
  fixture.controller_.setNominalSpeed(0.3);  // [m/s]
  fixture.controller_.setScript({ ControllerScript::kOk, ControllerScript::kOk });

  // Odometry báo robot đang chạy nhanh hơn hẳn lệnh mà controller muốn phát.
  fixture.server_.addOdometry("/odom", makeOdometry(1.7, 0.9));

  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
      << fixture.server_.lastRejectReason();

  fixture.spin(2);  // IDLE -> PLANNING -> CONTROLLING (controller chạy ngay ở cycle này)
  ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kControlling);

  const robot_nav_2d_msgs::Twist2DStamped twist = fixture.server_.getTwist();
  EXPECT_NEAR(twist.velocity.x, 0.3, 1e-9) << "getTwist returned the measured velocity instead of "
                                              "the command that was published";
  EXPECT_NEAR(twist.velocity.theta, 0.0, 1e-9);
}

TEST(NavigationServerTwist, OdometryAloneNeverProducesACommand)
{
  Fixture fixture;
  fixture.configure();

  fixture.server_.addOdometry("/odom", makeOdometry(1.7, 0.9));
  fixture.spin(1);  // IDLE, không có yêu cầu nào

  const robot_nav_2d_msgs::Twist2DStamped twist = fixture.server_.getTwist();
  EXPECT_NEAR(twist.velocity.x, 0.0, 1e-9);
  EXPECT_NEAR(twist.velocity.y, 0.0, 1e-9);
  EXPECT_NEAR(twist.velocity.theta, 0.0, 1e-9);
}

TEST(NavigationServerTwist, StampComesFromTheControlLoopClockNotWallClock)
{
  // Host loại lệnh quá hạn theo dấu này. Lấy giờ hệ thống lúc host hỏi sẽ làm một control loop đã
  // treo vẫn trông như đang phát lệnh tươi — đúng thứ dấu thời gian sinh ra để ngăn.
  // Stamp chỉ tiến khi đang có yêu cầu (xem test StampFreezesWhenIdle...), nên phải có goal chạy.
  Fixture fixture;
  fixture.configure();
  fixture.controller_.setNominalSpeed(0.3);  // [m/s]
  fixture.controller_.setScript({ ControllerScript::kOk, ControllerScript::kOk });
  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
      << fixture.server_.lastRejectReason();

  fixture.spin(1);
  EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), kClockStart, 1e-9);

  fixture.clock_.setTime(kClockStart + 12.0);
  fixture.spin(1);
  EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), kClockStart + 12.0, 1e-9);
}

TEST(NavigationServerTwist, StampFreezesWhenIdleSoTeleopOwnsCmdVel)
{
  // Không có yêu cầu nào thì stamp phải ĐỨNG YÊN dù control loop vẫn chạy: host publish /cmd_vel
  // qua cửa tươi 0.05 s (amr_publiser.cpp:361), stamp tươi mỗi cycle nghĩa là amr_node phát 0 ở
  // 20 Hz vĩnh viễn và đè chết teleop/joystick (rqt_robot_steering 10 Hz — robot chỉ nhích rồi
  // đứng im). Bản cũ chỉ đóng dấu trong executeCycle; đây là regression đã gặp trên sim.
  Fixture fixture;
  fixture.configure();

  fixture.spin(3);
  EXPECT_TRUE(fixture.server_.getTwist().header.stamp.isZero())
      << "no request has ever arrived yet the stamp is fresh — the host would publish 0 over "
         "teleop";
}

TEST(NavigationServerTwist, StampKeepsFreshBrieflyAfterGoalEndsThenFreezes)
{
  // Lệnh 0 cuối cùng phải qua được cửa 0.05 s của host — kết thúc mà đóng băng stamp ngay thì
  // robot giữ nguyên vận tốc chót. Cửa ân hạn 0.5 s; hết ân hạn stamp phải đứng yên trả /cmd_vel
  // cho teleop.
  Fixture fixture;
  fixture.configure();
  fixture.controller_.setNominalSpeed(0.3);  // [m/s]
  fixture.controller_.setScript({ ControllerScript::kOk, ControllerScript::kGoalReached });
  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
      << fixture.server_.lastRejectReason();

  fixture.spin(4);  // IDLE -> PLANNING -> CONTROLLING -> tới đích (terminal)
  ASSERT_FALSE(fixture.server_.loop().hasActiveRequest());

  // Ngay sau khi kết thúc: còn trong ân hạn, stamp vẫn tiến để host phát lệnh dừng.
  const double stamp_in_grace = fixture.server_.getTwist().header.stamp.toSec();
  fixture.spin(1);
  EXPECT_GT(fixture.server_.getTwist().header.stamp.toSec(), stamp_in_grace)
      << "the stamp freezes as soon as the leg ends — the final stop command would never be "
         "published";

  // Chạy qua hết cửa ân hạn (0.5 s = 10 cycle) rồi thêm vài cycle: stamp phải đứng yên.
  fixture.spin(12);
  const double stamp_frozen = fixture.server_.getTwist().header.stamp.toSec();
  fixture.spin(3);
  EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), stamp_frozen, 1e-9)
      << "the grace period is over yet the stamp is still fresh — teleop would never get /cmd_vel "
         "back";
}

TEST(NavigationServerTwist, StampStaysStillWhenTheControlLoopStopsRunning)
{
  Fixture fixture;
  fixture.configure();

  fixture.spin(1);
  const double stamp_after_first = fixture.server_.getTwist().header.stamp.toSec();

  // Đồng hồ chạy tiếp nhưng KHÔNG có cycle nào — mô phỏng control thread treo.
  fixture.clock_.setTime(kClockStart + 30.0);
  fixture.server_.addOdometry("/odom", makeOdometry(1.7, 0.0));

  EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), stamp_after_first, 1e-9)
      << "the timestamp refreshes itself although the control loop is not running — the host would "
         "think the command is still valid";
}

TEST(NavigationServerTwist, IsStampedWithTheConfiguredRobotBaseFrame)
{
  ControlLoopConfig config = baseConfig();
  config.robot_base_frame = "base_footprint";

  Fixture fixture;
  fixture.configure(config);
  fixture.spin(1);

  EXPECT_EQ(fixture.server_.getTwist().header.frame_id, "base_footprint");
}

TEST(NavigationServerTwist, ConfigureIsRefusedWhenRobotBaseFrameIsEmpty)
{
  ControlLoopConfig config = baseConfig();
  config.robot_base_frame.clear();

  Fixture fixture;
  std::string error;
  EXPECT_FALSE(fixture.server_.configureLoop(config, fixture.deps_, error));
  EXPECT_FALSE(error.empty());
}

// ================================================================================================
// Đường dữ liệu cảm biến từ host vào costmap
// ================================================================================================

TEST(NavigationServerSensors, SamplesReachTheCostmapLayersOnceAttached)
{
  Fixture fixture;
  fixture.configure();

  SpyPtr static_layer = attachSpy(fixture.global_, LayerType::STATIC_LAYER, "navigation_map");
  SpyPtr local_voxel = attachSpy(fixture.local_, LayerType::VOXEL_LAYER, "obstacles");
  fixture.attachCostmaps();

  fixture.server_.addStaticMap("/map", robot_nav_msgs::OccupancyGrid());
  fixture.server_.addLaserScan("/b_scan", makeScan());
  fixture.server_.addPointCloud2("/camera/depth/points_proc", robot_sensor_msgs::PointCloud2());

  EXPECT_EQ(static_layer->count(), 1U);
  EXPECT_EQ(local_voxel->count(), 2U) << "laser + pointcloud2 must both reach the VoxelLayer";
  EXPECT_EQ(local_voxel->records()[0].topic, "/b_scan");
  EXPECT_EQ(local_voxel->records()[1].topic, "/camera/depth/points_proc");
}

TEST(NavigationServerSensors, StoringStillWorksWhenNoCostmapIsAttachedYet)
{
  // Trạng thái bình thường lúc khởi động: host đã bắt đầu bơm dữ liệu trước khi costmap được dựng.
  // Dữ liệu vẫn phải đọc lại được qua getter của contract host, và số mẫu mất phải đếm được.
  Fixture fixture;
  fixture.configure();

  fixture.server_.addLaserScan("/b_scan", makeScan());

  EXPECT_EQ(fixture.server_.getLaserScan("/b_scan").ranges.size(), 40U);
  EXPECT_EQ(fixture.server_.sensors().stats().dropped_no_costmap, 1U);
  EXPECT_EQ(fixture.server_.sensors().stats().delivered, 0U);
}

TEST(NavigationServerSensors, StaticMapReceivedBeforeAttachIsReplayed)
{
  // Không có phần phát lại này thì thứ tự "map tới trước, costmap dựng sau" — thứ tự thường gặp
  // nhất khi khởi động — để global costmap trắng vĩnh viễn: /map là topic latched, host không gửi
  // lại. Bản cũ bù bằng cặp biến public map_save_/map_name_save_.
  Fixture fixture;
  fixture.configure();

  fixture.server_.addStaticMap("/map", robot_nav_msgs::OccupancyGrid());
  ASSERT_EQ(fixture.server_.sensors().stats().dropped_no_costmap, 1U);

  SpyPtr static_layer = attachSpy(fixture.global_, LayerType::STATIC_LAYER, "navigation_map");
  fixture.attachCostmaps();

  ASSERT_EQ(static_layer->count(), 1U) << "a static map received before a costmap was attached "
                                          "must not be replayed";
  EXPECT_EQ(static_layer->records()[0].topic, "/map");
}

TEST(NavigationServerSensors, LegacyMapSavePublicMemberIsAlsoReplayed)
{
  // `map_save_`/`map_name_save_` là biến PUBLIC của BaseNavigation mà host tự gán
  // (sensor_converter.cpp). Giữ đường này để host không phải sửa gì khi đổi sang move_base2.
  Fixture fixture;
  fixture.configure();

  fixture.server_.map_name_save_ = "/map";
  fixture.server_.map_save_ = robot_nav_msgs::OccupancyGrid();

  SpyPtr static_layer = attachSpy(fixture.global_, LayerType::STATIC_LAYER, "navigation_map");
  fixture.attachCostmaps();

  EXPECT_EQ(static_layer->count(), 1U);
  EXPECT_EQ(static_layer->records()[0].topic, "/map");
}

TEST(NavigationServerSensors, ReplayDoesNotDuplicateAMapAlreadyReceivedThroughTheApi)
{
  Fixture fixture;
  fixture.configure();

  fixture.server_.addStaticMap("/map", robot_nav_msgs::OccupancyGrid());
  fixture.server_.map_name_save_ = "/map";  // host gán cả hai đường, như bản cũ đang làm
  fixture.server_.map_save_ = robot_nav_msgs::OccupancyGrid();

  SpyPtr static_layer = attachSpy(fixture.global_, LayerType::STATIC_LAYER, "navigation_map");
  fixture.attachCostmaps();

  EXPECT_EQ(static_layer->count(), 1U) << "the same map was replayed twice";
}

TEST(NavigationServerSensors, StaleLaserScansAreNotReplayedOnAttach)
{
  // Cố ý: phát lại một scan cũ là dựng vật cản ở chỗ robot có thể đã rời khỏi từ lâu. Mẫu kế tiếp
  // chỉ cách vài chục ms — chờ nó an toàn hơn hẳn.
  Fixture fixture;
  fixture.configure();

  fixture.server_.addLaserScan("/b_scan", makeScan());

  SpyPtr voxel = attachSpy(fixture.local_, LayerType::VOXEL_LAYER, "obstacles");
  fixture.attachCostmaps();

  EXPECT_EQ(voxel->count(), 0U);
}

TEST(NavigationServerSensors, StoredLaserScanIsTheSameOneHandedToTheCostmap)
{
  // Bản cũ cất bản ĐÃ LỌC. Nếu getter trả bản thô còn costmap thấy bản lọc thì hai nguồn sự thật
  // lệch nhau, và mọi chẩn đoán dựa trên getter sẽ nói dối về thứ costmap thật sự dùng.
  SensorGatewayConfig sensors;
  sensors.laser_sor_enabled = true;
  sensors.laser_sor_mean_k = 5;
  sensors.laser_sor_stddev_mul = 1.0;

  Fixture fixture;
  fixture.configure();
  fixture.configureSensors(sensors);

  std::vector<float> seen_by_layer;
  SpyPtr voxel = attachSpy(fixture.local_, LayerType::VOXEL_LAYER, "obstacles");
  voxel->setObserver([&seen_by_layer](const void* data, const std::type_info& type,
                                      const std::string&) {
    if (type == typeid(robot_sensor_msgs::LaserScan))
    {
      seen_by_layer = static_cast<const robot_sensor_msgs::LaserScan*>(data)->ranges;
    }
  });
  fixture.attachCostmaps();

  fixture.server_.addLaserScan("/b_scan", makeScan());

  const std::vector<float> stored = fixture.server_.getLaserScan("/b_scan").ranges;
  ASSERT_FALSE(seen_by_layer.empty());
  ASSERT_EQ(stored.size(), seen_by_layer.size());

  // So từng phần tử chứ không so cả vector: bộ lọc biến outlier thành NaN để giữ nguyên cấu trúc
  // scan, mà NaN != NaN nên operator== của vector sẽ báo khác nhau dù nội dung giống hệt.
  for (std::size_t i = 0; i < stored.size(); ++i)
  {
    if (std::isnan(stored[i]))
    {
      EXPECT_TRUE(std::isnan(seen_by_layer[i])) << "mismatch at ray " << i;
    }
    else
    {
      EXPECT_FLOAT_EQ(stored[i], seen_by_layer[i]) << "mismatch at ray " << i;
    }
  }
}

TEST(NavigationServerSensors, DepthCameraDataIsStoredAndForwardedAsConstPtr)
{
  Fixture fixture;
  fixture.configure();

  SpyPtr voxel = attachSpy(fixture.local_, LayerType::VOXEL_LAYER, "obstacles");
  fixture.attachCostmaps();

  robot_sensor_msgs::DepthCameraData::Ptr data =
      boost::make_shared<robot_sensor_msgs::DepthCameraData>();
  data->header.frame_id = "camera_optical";
  fixture.server_.addDepthCameraData("/camera/depth/data", data);

  ASSERT_EQ(voxel->count(), 1U);
  EXPECT_TRUE(*voxel->records()[0].type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr));
  EXPECT_EQ(voxel->records()[0].topic, "/camera/depth/data");
}

TEST(NavigationServerSensors, NullDepthPointerIsRejectedAtTheDoor)
{
  Fixture fixture;
  fixture.configure();

  SpyPtr voxel = attachSpy(fixture.local_, LayerType::VOXEL_LAYER, "obstacles");
  fixture.attachCostmaps();

  fixture.server_.addDepthCameraData("/camera/depth/data",
                                     robot_sensor_msgs::DepthCameraData::ConstPtr());

  EXPECT_EQ(voxel->count(), 0U);
  EXPECT_EQ(fixture.server_.sensors().stats().dropped_no_costmap, 0U);
}

// ================================================================================================
// Trần vận tốc (bước 12) — đường tầng an toàn hạ tốc độ robot
//
// `setTwistLinear` không phải lệnh jog dù tên nghe như vậy: host gọi nó theo cặp +v/-v để đặt trần
// cho hai chiều, và giá trị truyền xuống mang theo tốc độ đã bị tầng an toàn hạ
// (amr_control.cpp:561, 671-680). Trước đây `NavigationServer` trả false và không làm gì.
// ================================================================================================

TEST(NavigationServerLimits, ForwardAndBackwardLimitsReachTheController)
{
  Fixture fixture;
  fixture.configure();

  EXPECT_TRUE(fixture.server_.setTwistLinear(makeVector(0.30)));   // [m/s] trần tiến
  EXPECT_TRUE(fixture.server_.setTwistLinear(makeVector(-0.15)));  // [m/s] trần lùi, ÂM

  fixture.spin(1);

  EXPECT_NEAR(fixture.controller_.limitForward(), 0.30, 1e-9);
  EXPECT_NEAR(fixture.controller_.limitBackward(), -0.15, 1e-9);
}

TEST(NavigationServerLimits, AngularLimitReachesTheController)
{
  Fixture fixture;
  fixture.configure();

  EXPECT_TRUE(fixture.server_.setTwistAngular(makeVector(0.0, 0.0, 0.45)));  // [rad/s]
  fixture.spin(1);

  EXPECT_NEAR(fixture.controller_.limitAngular(), 0.45, 1e-9);
}

TEST(NavigationServerLimits, LimitTakesEffectInTheSameCycleItIsPushed)
{
  // Chậm một cycle nghĩa là một chu kỳ nữa robot chạy quá tốc độ mà tầng an toàn vừa yêu cầu hạ.
  Fixture fixture;
  fixture.configure();

  ASSERT_TRUE(fixture.server_.setTwistLinear(makeVector(0.12)));
  fixture.spin(1);
  EXPECT_NEAR(fixture.controller_.limitForward(), 0.12, 1e-9);
}

TEST(NavigationServerLimits, NonFiniteLimitIsRejectedAtTheDoor)
{
  Fixture fixture;
  fixture.configure();

  const double nan = std::numeric_limits<double>::quiet_NaN();
  EXPECT_FALSE(fixture.server_.setTwistLinear(makeVector(nan)));
  EXPECT_FALSE(fixture.server_.setTwistAngular(makeVector(0.0, 0.0, nan)));

  fixture.spin(1);
  EXPECT_NEAR(fixture.controller_.limitForward(), 0.0, 1e-9);
}

TEST(NavigationServerLimits, LatestLimitWinsWhenSetSeveralTimesWithinOneCycle)
{
  // Host gọi từ thread của nó với nhịp riêng; nhiều lời gọi giữa hai cycle là bình thường. Thứ phải
  // có hiệu lực là giá trị MỚI NHẤT, không phải giá trị đầu tiên.
  Fixture fixture;
  fixture.configure();

  ASSERT_TRUE(fixture.server_.setTwistLinear(makeVector(0.40)));
  ASSERT_TRUE(fixture.server_.setTwistLinear(makeVector(0.10)));  // tầng an toàn vừa hạ tiếp
  fixture.spin(1);

  EXPECT_NEAR(fixture.controller_.limitForward(), 0.10, 1e-9);
}

TEST(NavigationServerLimits, OdometryReachesTheControllerAsMeasuredVelocity)
{
  // Bản cũ đưa vận tốc đo được vào controller bằng con trỏ tới bộ nhớ host ghi
  // (`tc_->setOdom(&odometry_)`) — data race không có gì bảo vệ. Ở đây truyền theo giá trị, qua
  // control thread.
  Fixture fixture;
  fixture.configure();

  fixture.server_.addOdometry("/odom", makeOdometry(0.42, -0.17));
  fixture.spin(1);

  EXPECT_NEAR(fixture.controller_.measuredVelocity().linear.x, 0.42, 1e-9);
  EXPECT_NEAR(fixture.controller_.measuredVelocity().angular.z, -0.17, 1e-9);
}

// ================================================================================================
// pause / resume / cancel — host gọi từ thread khác
//
// OPC-UA và VDA5050 chạy thread riêng (amr_control.cpp:159, 184) và gọi thẳng ba hàm này.
// `ControlLoop` tự khai là không thread-safe, và `requestPause()` còn ghi HAI cờ không nguyên tử —
// xen kẽ với `requestResume()` có thể để lại cả hai cùng false. Nên chúng chỉ được ghi nhận ở đây,
// rồi chuyển xuống lõi trên control thread.
// ================================================================================================

TEST(NavigationServerLifecycle, PauseTakesEffectOnTheNextCycleNotImmediately)
{
  Fixture fixture;
  fixture.configure();
  fixture.controller_.setScript({ ControllerScript::kOk, ControllerScript::kOk });

  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
      << fixture.server_.lastRejectReason();
  fixture.spin(2);
  ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kControlling);

  fixture.server_.pause();
  EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kControlling)
      << "pause() goes straight into the core from the host thread";

  fixture.spin(1);
  EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kPaused);
}

TEST(NavigationServerLifecycle, ResumeAfterPauseReturnsToControlling)
{
  Fixture fixture;
  fixture.configure();
  fixture.controller_.setScript({ ControllerScript::kOk });

  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
      << fixture.server_.lastRejectReason();
  fixture.spin(2);

  fixture.server_.pause();
  fixture.spin(1);
  ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kPaused);

  fixture.server_.resume();
  fixture.spin(1);
  EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kControlling);
}

TEST(NavigationServerLifecycle, PauseThenResumeWithinOneCycleEndsResumed)
{
  // Hai cờ đối nghịch được đặt dưới cùng một lock, nên lệnh sau luôn thắng lệnh trước — không có
  // trạng thái "cả hai cùng false" như bản ghi hai cờ rời rạc.
  Fixture fixture;
  fixture.configure();
  fixture.controller_.setScript({ ControllerScript::kOk });

  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
      << fixture.server_.lastRejectReason();
  fixture.spin(2);
  ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kControlling);

  fixture.server_.pause();
  fixture.server_.resume();
  fixture.spin(1);

  EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kControlling);
}

TEST(NavigationServerLifecycle, CancelWinsOverAPauseRequestedInTheSameCycle)
{
  // "Tạm dừng rồi huỷ" và "huỷ rồi tạm dừng" phải cho cùng kết quả: huỷ thắng.
  Fixture fixture;
  fixture.configure();
  fixture.controller_.setScript({ ControllerScript::kOk });

  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
      << fixture.server_.lastRejectReason();
  fixture.spin(2);
  ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kControlling);

  fixture.server_.pause();
  fixture.server_.cancel();
  fixture.spin(1);

  EXPECT_NE(fixture.server_.loop().state(), NavigationState::kPaused) << "pause won over cancel";
}

TEST(NavigationServerLifecycle, LifecycleRequestIsConsumedExactlyOnce)
{
  Fixture fixture;
  fixture.configure();
  fixture.controller_.setScript({ ControllerScript::kOk });

  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
      << fixture.server_.lastRejectReason();
  fixture.spin(2);

  fixture.server_.pause();
  fixture.spin(1);
  ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kPaused);

  // Không có lệnh mới: cờ đã bị tiêu thụ, các cycle sau không được tự tạm dừng lại lần nữa.
  fixture.server_.resume();
  fixture.spin(3);
  EXPECT_NE(fixture.server_.loop().state(), NavigationState::kPaused);
}

// ================================================================================================
// Control thread
//
// Contract `BaseNavigation` KHÔNG có hàm spin nào: host nạp plugin, gọi initialize(), rồi chỉ tương
// tác qua moveTo/getTwist/getFeedback. Runtime vì thế phải TỰ LÁI mình, đúng như bản cũ (thread
// planner + action server). Thiếu control thread thì goal nằm im trong chỗ chờ vĩnh viễn — không
// cycle nào chạy, không state nào đổi, và không log gì cả. Đó là lỗi đã thật sự xảy ra trên sim.
// ================================================================================================

TEST(NavigationServerControlThread, RunsCyclesWithoutAnyoneCallingSpinOnce)
{
  Fixture fixture;
  fixture.configure();
  fixture.controller_.setScript({ ControllerScript::kOk });

  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
      << fixture.server_.lastRejectReason();

  // Ở đây KHÔNG gọi spin() của fixture: chính control thread phải đẩy state đi.
  ASSERT_TRUE(fixture.server_.startControlThread(200.0));  // [Hz]

  bool left_idle = false;
  for (int i = 0; i < 500 && !left_idle; ++i)
  {
    std::this_thread::sleep_for(std::chrono::milliseconds(2));
    left_idle = fixture.server_.loop().state() != NavigationState::kIdle;
  }
  fixture.server_.stopControlThread();

  EXPECT_TRUE(left_idle) << "the goal was accepted but no cycle ran — the control thread is "
                            "missing";
}

TEST(NavigationServerControlThread, RefusesToStartBeforeTheLoopIsConfigured)
{
  NavigationServer server;
  EXPECT_FALSE(server.startControlThread(20.0));
  EXPECT_FALSE(server.controlThreadRunning());
}

TEST(NavigationServerControlThread, RefusesNonPositiveFrequency)
{
  Fixture fixture;
  fixture.configure();

  EXPECT_FALSE(fixture.server_.startControlThread(0.0));
  EXPECT_FALSE(fixture.server_.startControlThread(-5.0));
}

TEST(NavigationServerControlThread, SecondStartIsRefusedAndStopIsIdempotent)
{
  Fixture fixture;
  fixture.configure();

  ASSERT_TRUE(fixture.server_.startControlThread(100.0));
  EXPECT_FALSE(fixture.server_.startControlThread(100.0)) << "started a second thread";

  fixture.server_.stopControlThread();
  fixture.server_.stopControlThread();  // không được treo hay sập
  EXPECT_FALSE(fixture.server_.controlThreadRunning());
}

TEST(NavigationServerControlThread, DestructorStopsTheThread)
{
  // Thread chạm loop_, runtime_ và sensors_ mỗi cycle; huỷ chúng khi thread còn sống là hỏng ở chỗ
  // không truy được.
  Fixture fixture;
  fixture.configure();
  ASSERT_TRUE(fixture.server_.startControlThread(100.0));
  SUCCEED();  // destructor của fixture phải join, không treo
}

// ================================================================================================
// Dữ liệu hiển thị — host gọi từ BỐN ros::Timer khác nhau
// ================================================================================================

TEST(NavigationServerPlannerData, GettersDoNotShareMutableState)
{
  // `getGlobalData` và `getLocalData` mỗi cái được gọi từ HAI timer (costmap và plan). Ghi vào
  // `global_data_`/`local_data_` dùng chung đã gây `std::bad_alloc` rồi hỏng heap và giết tiến trình.
  Fixture fixture;
  fixture.configure();
  fixture.spin(1);

  robot::move_base_core::PlannerDataOutput a = fixture.server_.getGlobalData();
  robot::move_base_core::PlannerDataOutput b = fixture.server_.getGlobalData();

  a.plan.poses.clear();
  EXPECT_TRUE(b.plan.poses.empty() || !a.plan.poses.empty())
      << "two calls returned the same memory";
  EXPECT_NO_THROW({ (void)fixture.server_.getLocalData(); });
}

TEST(NavigationServerPlannerData, ConcurrentGettersDoNotCorruptEachOther)
{
  // Không chứng minh được không có race (cần ThreadSanitizer), nhưng chạy đúng hình dạng lời gọi
  // của host: bốn thread cùng đọc trong khi control thread cùng ghi bộ đệm.
  Fixture fixture;
  fixture.configure();
  fixture.controller_.setScript({ ControllerScript::kOk });
  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
      << fixture.server_.lastRejectReason();
  ASSERT_TRUE(fixture.server_.startControlThread(200.0));

  std::atomic<bool> stop{ false };
  std::vector<std::thread> readers;
  for (int i = 0; i < 4; ++i)
  {
    readers.emplace_back([&fixture, &stop, i]() {
      while (!stop.load())
      {
        if (i % 2 == 0)
        {
          (void)fixture.server_.getGlobalData();
        }
        else
        {
          (void)fixture.server_.getLocalData();
        }
      }
    });
  }

  std::this_thread::sleep_for(std::chrono::milliseconds(150));
  stop.store(true);
  for (auto& t : readers)
  {
    t.join();
  }
  fixture.server_.stopControlThread();

  SUCCEED();
}

TEST(NavigationServerPlannerData, PlanIsStampedWithTheControlLoopClock)
{
  Fixture fixture;
  fixture.configure();

  fixture.clock_.setTime(kClockStart + 7.0);
  fixture.spin(1);

  EXPECT_NEAR(fixture.server_.getGlobalData().plan.header.stamp.toSec(), kClockStart + 7.0, 1e-9)
      << "the plan carries a timestamp from another clock than the control loop — the host would "
         "treat it as stale and drop it";
}

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