/*********************************************************************
 *
 * move_base2 — kiểm việc đọc và validate cấu hình runtime.
 *
 * Bản cũ đọc param không kiểm miền giá trị: một `controller_frequency` bằng 0 hay một
 * `max_planning_retries` âm đi thẳng vào vòng điều khiển. Ở đây cấu hình sai phải chặn runtime khởi
 * động, chứ không phải hiện ra thành hành vi lạ lúc chạy.
 *
 * Author: DuongTD
 *********************************************************************/
#include <gtest/gtest.h>

#include <cstdlib>
#include <string>

#include <robot/node_handle.h>

#include <move_base2/config/move_base2_config.h>

namespace
{
using move_base2::MoveBase2Config;

MoveBase2Config loadFrom(const std::string& ns)
{
  robot::NodeHandle root;
  robot::NodeHandle nh(root, ns);

  MoveBase2Config config;
  config.fromNodeHandle(nh);
  return config;
}

/// Điền con số mà RecoveryRunner báo lại sau khi nạp behavior — bước bắt buộc trước validate().
MoveBase2Config withRecoveryCount(MoveBase2Config config, std::size_t count)
{
  config.state_machine.recovery_behavior_count = count;
  return config;
}

/// Cấu hình tối thiểu hợp lệ, dựng bằng tay (không qua YAML).
MoveBase2Config minimalValid()
{
  MoveBase2Config config;
  config.position.local_planner_name = "AnyLocalPlanner";
  config.state_machine.recovery_behavior_count = 1;
  return config;
}

TEST(MoveBase2Config, DefaultsAreValid)
{
  const MoveBase2Config config = minimalValid();

  std::string error;
  EXPECT_TRUE(config.validate(error)) << error;
}

TEST(MoveBase2Config, RejectsRecoveryEnabledWithNoBehaviorLoaded)
{
  // Ràng buộc thứ tự khởi tạo: RecoveryRunner không nạp được behavior nào mà
  // recovery_behavior_enabled vẫn true thì mọi lỗi dẫn thẳng tới ABORTED — chặn ngay lúc khởi động.
  MoveBase2Config config = minimalValid();
  config.state_machine.recovery_behavior_count = 0;
  config.state_machine.recovery_enabled = true;

  std::string error;
  EXPECT_FALSE(config.validate(error));
}

TEST(MoveBase2Config, ReadsSensorGatewayBlockFromYaml)
{
  const MoveBase2Config config = loadFrom("move_base2");

  EXPECT_TRUE(config.sensors.laser_sor_enabled);
  EXPECT_EQ(config.sensors.laser_sor_mean_k, 8);
  EXPECT_DOUBLE_EQ(config.sensors.laser_sor_stddev_mul, 1.5);
}

TEST(MoveBase2Config, LaserFilterIsOffWhenTheSensorsBlockIsAbsent)
{
  // Cây config gen-1 không có khoá nào cho bộ lọc. Thiếu khoá phải giữ hành vi host ROS đang chạy —
  // tức là KHÔNG lọc — chứ không phải âm thầm bật một bộ lọc lên.
  const MoveBase2Config config = loadFrom("move_base2_no_planner");

  EXPECT_FALSE(config.sensors.laser_sor_enabled);
}

TEST(MoveBase2Config, RejectsSensorFilterParametersOutOfRange)
{
  const MoveBase2Config config = withRecoveryCount(loadFrom("move_base2_bad_sensors"), 1);

  std::string error;
  EXPECT_FALSE(config.validate(error));
  EXPECT_NE(error.find("laser_sor_mean_k"), std::string::npos) << error;
}

TEST(MoveBase2Config, ReadsEveryGroupFromYaml)
{
  const MoveBase2Config config = loadFrom("move_base2");

  EXPECT_DOUBLE_EQ(config.controller_frequency, 20.0);
  EXPECT_DOUBLE_EQ(config.planner_timeout, 5.0);

  EXPECT_DOUBLE_EQ(config.state_machine.planner_patience, 5.0);
  EXPECT_DOUBLE_EQ(config.state_machine.controller_patience, 15.0);
  EXPECT_EQ(config.state_machine.max_planning_retries, -1);
  EXPECT_TRUE(config.state_machine.recovery_enabled);

  EXPECT_DOUBLE_EQ(config.velocity.max_vel_x, 0.5);
  EXPECT_DOUBLE_EQ(config.velocity.min_vel_x, -0.2);
  EXPECT_DOUBLE_EQ(config.velocity.max_accel_x, 1.0);

  EXPECT_EQ(config.global_frame, "map");
  EXPECT_EQ(config.robot_base_frame, "base_link");
  EXPECT_EQ(config.recovery_namespace, "recovery");
}

TEST(MoveBase2Config, ReadsProfileBindingsFromNestedNamespaces)
{
  const MoveBase2Config config = loadFrom("move_base2");

  EXPECT_EQ(config.position.global_planner_name, "TestGlobalPlanner");
  EXPECT_EQ(config.position.local_planner_name, "TestLocalPlanner");
  EXPECT_DOUBLE_EQ(config.position.default_xy_tolerance, 0.15);

  // Ghép nối cần sai số chặt hơn nhiều — đây chính là thứ sáu entry point cũ khác nhau ở.
  EXPECT_EQ(config.docking.global_planner_name, "TestDockPlanner");
  EXPECT_DOUBLE_EQ(config.docking.default_xy_tolerance, 0.02);
  EXPECT_DOUBLE_EQ(config.docking.default_yaw_tolerance, 0.02);
}

TEST(MoveBase2Config, LoadedConfigValidates)
{
  const MoveBase2Config config = withRecoveryCount(loadFrom("move_base2"), 2);

  std::string error;
  EXPECT_TRUE(config.validate(error)) << error;
}

TEST(MoveBase2Config, RejectsZeroControllerFrequency)
{
  const MoveBase2Config config = loadFrom("move_base2_bad_frequency");

  std::string error;
  EXPECT_FALSE(config.validate(error));
  EXPECT_NE(error.find("controller_frequency"), std::string::npos);
}

TEST(MoveBase2Config, RejectsAbsurdlyHighControllerFrequency)
{
  MoveBase2Config config = minimalValid();
  config.controller_frequency = 5000.0;

  std::string error;
  EXPECT_FALSE(config.validate(error));
}

TEST(MoveBase2Config, RejectsGlobalFrameEqualToBaseFrame)
{
  const MoveBase2Config config = withRecoveryCount(loadFrom("move_base2_bad_frames"), 1);

  std::string error;
  EXPECT_FALSE(config.validate(error));
  EXPECT_NE(error.find("global_frame"), std::string::npos);
}

TEST(MoveBase2Config, RejectsConfigWithNoLocalPlannerAtAll)
{
  const MoveBase2Config config = withRecoveryCount(loadFrom("move_base2_no_planner"), 1);

  std::string error;
  EXPECT_FALSE(config.validate(error))
      << "cấu hình này lúc chạy sẽ từ chối MỌI yêu cầu — phải chặn ngay lúc khởi động";
}

TEST(MoveBase2Config, RejectsNonPositiveToleranceOnConfiguredProfile)
{
  MoveBase2Config config = minimalValid();
  config.position.default_xy_tolerance = 0.0;

  std::string error;
  EXPECT_FALSE(config.validate(error));
  EXPECT_NE(error.find("xy_goal_tolerance"), std::string::npos);
}

TEST(MoveBase2Config, PropagatesStateMachineValidationFailure)
{
  MoveBase2Config config = minimalValid();
  config.state_machine.oscillation_timeout = 5.0;
  config.state_machine.oscillation_distance = 0.0;

  std::string error;
  EXPECT_FALSE(config.validate(error));
  EXPECT_NE(error.find("oscillation"), std::string::npos);
}

TEST(MoveBase2Config, PropagatesVelocityValidationFailure)
{
  MoveBase2Config config = minimalValid();
  config.velocity.max_vel_x = -1.0;

  std::string error;
  EXPECT_FALSE(config.validate(error));
}

TEST(MoveBase2Config, ControlLoopConfigDerivesPeriodFromFrequency)
{
  MoveBase2Config config = minimalValid();
  config.controller_frequency = 25.0;

  const auto loop_config = config.toControlLoopConfig();

  EXPECT_NEAR(loop_config.nominal_control_period, 0.04, 1e-9);  // [s]
  EXPECT_EQ(loop_config.position.local_planner_name, "AnyLocalPlanner");
}

TEST(MoveBase2Config, RecoveryBehaviorCountIsNotReadFromYaml)
{
  // Số behavior phải là số nạp được THẬT, do RecoveryRunner báo lại. Đọc từ YAML thì một behavior
  // hỏng vẫn khiến state machine tin là còn đường phục hồi.
  const MoveBase2Config config = loadFrom("move_base2");
  EXPECT_EQ(config.state_machine.recovery_behavior_count, 0u);
}

TEST(MoveBase2Config, DescribeMentionsEveryGroup)
{
  const std::string text = loadFrom("move_base2").describe();

  EXPECT_NE(text.find("controller_frequency"), std::string::npos);
  EXPECT_NE(text.find("position"), std::string::npos);
  EXPECT_NE(text.find("docking"), std::string::npos);
  EXPECT_NE(text.find("planner_patience"), std::string::npos);
  EXPECT_NE(text.find("max_vel_x"), std::string::npos);
}

}  // namespace

// ================================================================================================
// Schema gen-1 (legacy)
//
// Cây config production hiện tại là schema gen-1. Không đọc được nó thì move_base2 không chạy được
// trên cấu hình thật, và bước 11 (chạy song song bản cũ/mới) không thực hiện được.
// ================================================================================================

TEST(MoveBase2ConfigLegacy, MapsPlannerNamesToProfiles)
{
  robot::NodeHandle root;
  robot::NodeHandle nh(root, "legacy_move_base");

  MoveBase2Config config;
  config.fromLegacyNodeHandle(nh);

  EXPECT_EQ(config.position.local_planner_name, "HybridLocalPlanner");
  EXPECT_EQ(config.docking.local_planner_name, "DockLocalPlanner");

  // Profile không khai global planner riêng thì rơi về khoá ở root.
  EXPECT_EQ(config.position.global_planner_name, "SBPLLatticePlanner");
  // Profile có khai riêng thì dùng bản riêng.
  EXPECT_EQ(config.docking.global_planner_name, "DockPlanner");
}

TEST(MoveBase2ConfigLegacy, RootToleranceAppliesToEveryProfile)
{
  robot::NodeHandle root;
  robot::NodeHandle nh(root, "legacy_move_base");

  MoveBase2Config config;
  config.fromLegacyNodeHandle(nh);

  EXPECT_DOUBLE_EQ(config.position.default_xy_tolerance, 0.25);
  EXPECT_DOUBLE_EQ(config.docking.default_yaw_tolerance, 0.30);
  EXPECT_DOUBLE_EQ(config.rotate.default_xy_tolerance, 0.25);
}

TEST(MoveBase2ConfigLegacy, ZeroPatienceBecomesOneControlCycleNotDisabled)
{
  // Khác biệt NGỮ NGHĨA, không phải khác biệt giá trị. Gen-1: 0 = "fail -> recovery ngay". Gen-2:
  // 0 = "tắt đồng hồ". Dịch nhầm là biến một cấu hình phản ứng-ngay thành cấu hình không-bao-giờ.
  robot::NodeHandle root;
  robot::NodeHandle nh(root, "legacy_move_base");

  MoveBase2Config config;
  config.fromLegacyNodeHandle(nh);

  EXPECT_GT(config.state_machine.planner_patience, 0.0);
  EXPECT_NEAR(config.state_machine.planner_patience, 1.0 / 20.0, 1e-9);
}

TEST(MoveBase2ConfigLegacy, KeepsGen1DefaultForRobotBaseFrame)
{
  // Gen-1 mặc định "base_footprint", gen-2 "base_link". Chế độ legacy giữ default gen-1 để không
  // đổi hành vi của một hệ đang chạy chỉ vì đổi runtime.
  robot::NodeHandle root;
  robot::NodeHandle nh(root, "legacy_move_base");

  MoveBase2Config config;
  config.fromLegacyNodeHandle(nh);

  EXPECT_EQ(config.robot_base_frame, "base_footprint");
}

TEST(MoveBase2ConfigLegacy, LoadedLegacyConfigPassesValidation)
{
  robot::NodeHandle root;
  robot::NodeHandle nh(root, "legacy_move_base");

  MoveBase2Config config;
  config.fromLegacyNodeHandle(nh);
  config.state_machine.recovery_behavior_count = 2;

  std::string error;
  EXPECT_TRUE(config.validate(error)) << error;
}

TEST(MoveBase2ConfigLegacy, AutoDetectPrefersTheModernSchema)
{
  // Không trộn từng khoá giữa hai schema: hai nguồn cùng có hiệu lực cho một tham số là đúng kiểu
  // lỗi "sửa config mãi không ăn" mà hai cây config trùng tên của workspace đã gây ra một lần.
  robot::NodeHandle root;
  const MoveBase2Config config = MoveBase2Config::load(root);

  EXPECT_EQ(config.robot_base_frame, "base_link") << "chọn nhầm schema gen-1 dù có namespace mới";
  EXPECT_TRUE(config.sensors.laser_sor_enabled) << "khoá chỉ có ở schema mới không được đọc";
}

TEST(MoveBase2ConfigLegacy, AutoDetectFallsBackToLegacyWhenNoModernNamespace)
{
  robot::NodeHandle root;
  robot::NodeHandle nh(root, "legacy_move_base");

  const MoveBase2Config config = MoveBase2Config::load(nh);

  EXPECT_EQ(config.robot_base_frame, "base_footprint");
  EXPECT_EQ(config.position.local_planner_name, "HybridLocalPlanner");
}

int main(int argc, char** argv)
{
#ifdef MOVE_BASE2_TEST_CONFIG_DIR
  setenv("PNKX_NAV_CORE_CONFIG_DIR", MOVE_BASE2_TEST_CONFIG_DIR, 0);
#endif
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}