/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * move_base2 — test ControllerRunner: nạp plugin thật qua Boost.DLL, trần vận tốc phải tới được
 * plugin, và mọi đường lỗi phải trả về "không có lệnh" chứ không để dữ liệu hỏng đi tiếp.
 *
 * Author: DuongTD
 *********************************************************************/
#include <gtest/gtest.h>

#include <cmath>
#include <cstdint>
#include <cstdlib>
#include <limits>
#include <string>
#include <vector>

#include <robot/node_handle.h>

#include <move_base2/runners/controller_runner.h>

namespace
{
using move_base2::ControllerRunner;

/// [m/s] Lệnh nền của plugin test khi chưa đặt trần và vận tốc đo được bằng 0.
constexpr double kBaseSpeed = 0.25;
/// [rad/s]
constexpr double kBaseYawRate = 0.40;

/**
 * @brief Con trỏ costmap giả.
 *
 * `Costmap2DROBOT` không dựng được trong unit test (cần `tf3::BufferCore` thật và cây config đầy
 * đủ). An toàn ở đây vì `test_local_planner.cpp` không alias nào chạm vào con trỏ này — nó chỉ đi
 * qua `initialize()` rồi bị bỏ. Đường có costmap thật thuộc test tích hợp (Phase 5).
 */
robot_costmap_2d::Costmap2DROBOT* dummyCostmap()
{
  static std::uintptr_t placeholder = 0;
  return reinterpret_cast<robot_costmap_2d::Costmap2DROBOT*>(&placeholder);
}

std::vector<robot_geometry_msgs::PoseStamped> makePlan(std::size_t poses = 3)
{
  std::vector<robot_geometry_msgs::PoseStamped> plan;
  for (std::size_t i = 0; i < poses; ++i)
  {
    robot_geometry_msgs::PoseStamped pose;
    pose.header.frame_id = "map";
    pose.pose.position.x = static_cast<double>(i);  // [m]
    pose.pose.orientation.w = 1.0;
    plan.push_back(pose);
  }
  return plan;
}

robot_geometry_msgs::Vector3 vec(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_geometry_msgs::Twist twist(double vx, double wz = 0.0)
{
  robot_geometry_msgs::Twist t;
  t.linear.x = vx;   // [m/s]
  t.angular.z = wz;  // [rad/s]
  return t;
}

class Fixture
{
public:
  explicit Fixture(const std::string& name = "TestControllerOk")
  {
    robot::NodeHandle nh;
    std::string error;
    ok_ = runner_.configure(nh, nullptr, dummyCostmap(), name, error);
    error_ = error;
  }

  bool ok() const
  {
    return ok_;
  }

  const std::string& error() const
  {
    return error_;
  }

  ControllerRunner runner_;

private:
  bool ok_ = false;
  std::string error_;
};

}  // namespace

// ================================================================================================
// Cấu hình và nạp plugin
// ================================================================================================

TEST(ControllerRunner, RefusesNullCostmap)
{
  robot::NodeHandle nh;
  ControllerRunner runner;
  std::string error;

  EXPECT_FALSE(runner.configure(nh, nullptr, nullptr, "TestControllerOk", error));
  EXPECT_FALSE(error.empty());
  EXPECT_FALSE(runner.configured());
}

TEST(ControllerRunner, ConfigureFailsWhenTheInitialControllerCannotBeLoaded)
{
  robot::NodeHandle nh;
  ControllerRunner runner;
  std::string error;

  EXPECT_FALSE(runner.configure(nh, nullptr, dummyCostmap(), "TestControllerMissing", error));
  EXPECT_FALSE(runner.configured()) << "configure thất bại nhưng vẫn tự coi là đã cấu hình";
}

TEST(ControllerRunner, LoadsTheInitialControllerAndReportsItAsActive)
{
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  EXPECT_EQ(fixture.runner_.activeController(), "TestControllerOk");
  EXPECT_EQ(fixture.runner_.loadedCount(), 1U);
}

TEST(ControllerRunner, SwapsBetweenControllersAndReusesLoadedLibraries)
{
  // swapPlanner chạy ở cửa vào mỗi yêu cầu (profile position/docking/...). Đổi qua lại không được
  // dlopen lại.
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  ASSERT_TRUE(fixture.runner_.swapPlanner("TestControllerSecondary"));
  EXPECT_EQ(fixture.runner_.loadedCount(), 2U);

  ASSERT_TRUE(fixture.runner_.swapPlanner("TestControllerOk"));
  EXPECT_EQ(fixture.runner_.activeController(), "TestControllerOk");
  EXPECT_EQ(fixture.runner_.loadedCount(), 2U) << "quay lại controller cũ mà vẫn nạp lại thư viện";
}

TEST(ControllerRunner, FailedSwapKeepsThePreviousControllerActive)
{
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  EXPECT_FALSE(fixture.runner_.swapPlanner("TestControllerMissing"));
  EXPECT_EQ(fixture.runner_.activeController(), "TestControllerOk");
}

TEST(ControllerRunner, SwapBeforeConfigureIsRefused)
{
  ControllerRunner runner;
  EXPECT_FALSE(runner.swapPlanner("TestControllerOk"));
}

// ================================================================================================
// Trần vận tốc — đường tầng an toàn hạ tốc độ robot (bước 12)
// ================================================================================================

TEST(ControllerRunner, ForwardVelocityLimitReachesThePlugin)
{
  // Nếu lời gọi này không tới được plugin thì tầng an toàn yêu cầu giảm tốc mà robot vẫn chạy
  // nguyên tốc độ planner — và không có dấu hiệu nào cả.
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();
  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));

  robot_geometry_msgs::Twist cmd;
  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
  ASSERT_NEAR(cmd.linear.x, kBaseSpeed, 1e-9);

  ASSERT_TRUE(fixture.runner_.setTwistLinear(vec(0.10)));  // [m/s]
  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
  EXPECT_NEAR(cmd.linear.x, 0.10, 1e-9) << "trần vận tốc không tới được plugin";
}

TEST(ControllerRunner, AngularVelocityLimitReachesThePlugin)
{
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();
  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));

  robot_geometry_msgs::Twist cmd;
  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
  ASSERT_NEAR(cmd.angular.z, kBaseYawRate, 1e-9);

  ASSERT_TRUE(fixture.runner_.setTwistAngular(vec(0.0, 0.0, 0.15)));  // [rad/s]
  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
  EXPECT_NEAR(cmd.angular.z, 0.15, 1e-9);
}

TEST(ControllerRunner, LimitSetBeforeAControllerExistsIsAppliedOnceItIsLoaded)
{
  // Thứ tự khởi tạo không do move_base2 quyết: host có thể đặt trần trước khi controller được nạp.
  robot::NodeHandle nh;
  ControllerRunner runner;
  std::string error;
  ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), "", error)) << error;

  EXPECT_TRUE(runner.setTwistLinear(vec(0.08)));  // [m/s], chưa có controller nào
  ASSERT_TRUE(runner.swapPlanner("TestControllerOk"));
  ASSERT_TRUE(runner.setPlan(makePlan()));

  robot_geometry_msgs::Twist cmd;
  ASSERT_TRUE(runner.computeVelocityCommands(cmd));
  EXPECT_NEAR(cmd.linear.x, 0.08, 1e-9) << "trần đặt trước khi nạp controller bị mất";
}

TEST(ControllerRunner, LimitIsReappliedAfterSwappingController)
{
  // Trần thuộc về YÊU CẦU chứ không thuộc về instance planner. Instance mới không biết gì về trần
  // đã đặt — không áp lại là robot lặng lẽ chạy nhanh hơn mức tầng an toàn cho phép.
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  ASSERT_TRUE(fixture.runner_.setTwistLinear(vec(0.07)));  // [m/s]
  ASSERT_TRUE(fixture.runner_.swapPlanner("TestControllerSecondary"));
  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));

  robot_geometry_msgs::Twist cmd;
  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
  EXPECT_NEAR(cmd.linear.x, 0.07, 1e-9) << "đổi controller làm mất trần vận tốc đang có hiệu lực";
}

TEST(ControllerRunner, ControllerRefusingLimitsReportsFalse)
{
  // Host phải biết trần của nó không có hiệu lực, thay vì tưởng đã đặt được.
  Fixture fixture("TestControllerRefusesLimits");
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  EXPECT_FALSE(fixture.runner_.setTwistLinear(vec(0.10)));
  EXPECT_FALSE(fixture.runner_.setTwistAngular(vec(0.0, 0.0, 0.10)));
}

TEST(ControllerRunner, NonFiniteLimitIsRejected)
{
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();

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

// ================================================================================================
// Vận tốc đo được
// ================================================================================================

TEST(ControllerRunner, MeasuredVelocityReachesThePlugin)
{
  // Interface gen-1 nhận vận tốc hiện tại làm tham số của computeVelocityCommands. Bản cũ đưa nó
  // vào bằng con trỏ tới bộ nhớ host ghi (`setOdom(&odometry_)`) — một data race không có gì bảo
  // vệ. Ở đây truyền theo giá trị.
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();
  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));

  fixture.runner_.setMeasuredVelocity(twist(0.30));  // [m/s]

  robot_geometry_msgs::Twist cmd;
  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
  EXPECT_NEAR(cmd.linear.x, kBaseSpeed + 0.30, 1e-9) << "vận tốc đo được không tới được plugin";
}

TEST(ControllerRunner, NonFiniteMeasuredVelocityIsDroppedAndTheOldValueKept)
{
  // Nhiều local planner dùng vận tốc hiện tại làm mốc giới hạn gia tốc; NaN ở đó lan ra toàn bộ
  // cost function.
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();
  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));

  fixture.runner_.setMeasuredVelocity(twist(0.20));
  fixture.runner_.setMeasuredVelocity(twist(std::numeric_limits<double>::quiet_NaN()));

  robot_geometry_msgs::Twist cmd;
  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
  EXPECT_NEAR(cmd.linear.x, kBaseSpeed + 0.20, 1e-9);
}

// ================================================================================================
// Đường lỗi
// ================================================================================================

TEST(ControllerRunner, EmptyPlanIsRefused)
{
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  EXPECT_FALSE(fixture.runner_.setPlan({}));
}

TEST(ControllerRunner, SetPlanWithoutAControllerFails)
{
  robot::NodeHandle nh;
  ControllerRunner runner;
  std::string error;
  ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), "", error)) << error;

  EXPECT_FALSE(runner.setPlan(makePlan()));
}

TEST(ControllerRunner, ComputeWithoutAControllerYieldsNoCommand)
{
  robot::NodeHandle nh;
  ControllerRunner runner;
  std::string error;
  ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), "", error)) << error;

  robot_geometry_msgs::Twist cmd;
  EXPECT_FALSE(runner.computeVelocityCommands(cmd));
}

TEST(ControllerRunner, PluginReturningNoCommandIsPassedThroughAsFalse)
{
  Fixture fixture("TestControllerNoCommand");
  ASSERT_TRUE(fixture.ok()) << fixture.error();
  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));

  robot_geometry_msgs::Twist cmd;
  EXPECT_FALSE(fixture.runner_.computeVelocityCommands(cmd));
}

TEST(ControllerRunner, NaNCommandIsBlockedAtTheBoundary)
{
  // VelocityArbiter cũng chặn NaN, nhưng chặn tại nguồn cho biết ĐÚNG plugin nào đang trả dữ liệu
  // hỏng — arbiter chỉ thấy một con số vô nghĩa không rõ từ đâu.
  Fixture fixture("TestControllerNaN");
  ASSERT_TRUE(fixture.ok()) << fixture.error();
  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));

  robot_geometry_msgs::Twist cmd;
  EXPECT_FALSE(fixture.runner_.computeVelocityCommands(cmd));
  EXPECT_TRUE(std::isfinite(cmd.linear.x)) << "lệnh chứa NaN vẫn được ghi ra ngoài";
}

TEST(ControllerRunner, ExceptionFromThePluginIsContained)
{
  Fixture fixture("TestControllerThrowing");
  ASSERT_TRUE(fixture.ok()) << fixture.error();
  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));

  robot_geometry_msgs::Twist cmd;
  EXPECT_NO_THROW({ EXPECT_FALSE(fixture.runner_.computeVelocityCommands(cmd)); });
}

TEST(ControllerRunner, CommandIsClearedBeforeEveryAttempt)
{
  // Bên gọi dùng lại cùng một biến qua nhiều cycle. Trả false mà để nguyên lệnh cũ trong đó là mời
  // tầng trên phát lại một lệnh đã hết hạn.
  Fixture fixture("TestControllerNoCommand");
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  robot_geometry_msgs::Twist cmd = twist(9.0, 9.0);
  EXPECT_FALSE(fixture.runner_.computeVelocityCommands(cmd));
  EXPECT_NEAR(cmd.linear.x, 0.0, 1e-9);
  EXPECT_NEAR(cmd.angular.z, 0.0, 1e-9);
}

int main(int argc, char** argv)
{
  setenv("PNKX_NAV_CORE_CONFIG_DIR", MOVE_BASE2_TEST_CONFIG_DIR, 0);
  setenv("PNKX_NAV_CORE_LIBRARY_PATH", MOVE_BASE2_TEST_LIBRARY_DIR, 0);

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