/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * move_base2 — chạy toàn bộ kịch bản khai báo trong nav_test_harness/scenarios.
 *
 * Test này **tự tìm** file trong thư mục kịch bản và sinh một ca gtest cho mỗi file. Đó là điều kiện
 * để giữ đúng mục tiêu của Phase 5: thêm một ca test = thêm một file YAML, không sửa C++.
 *
 * Author: DuongTD
 *********************************************************************/
#include <gtest/gtest.h>

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

#include <nav_test_harness/scenario.h>
#include <nav_test_harness/scenario_runner.h>

#include "move_base2_scenario_driver.h"
#include "recovery_scenario_driver.h"

namespace
{
using move_base2::testing::MoveBase2ScenarioDriver;
using move_base2::testing::RecoveryScenarioDriver;
using nav_test_harness::Scenario;
using nav_test_harness::ScenarioReport;
using nav_test_harness::ScenarioRunner;

/// @brief Thư mục kịch bản, do CMake truyền vào — test không được đoán đường dẫn.
std::string scenarioDir()
{
  return MOVE_BASE2_SCENARIO_DIR;
}

std::vector<std::string> scenarioFiles()
{
  std::vector<std::string> files = nav_test_harness::listScenarioFiles(scenarioDir());
  // Sắp xếp để thứ tự chạy ổn định giữa các máy — thứ tự đọc thư mục không được đảm bảo.
  std::sort(files.begin(), files.end());
  return files;
}

/// @brief Nạp, chạy, và khẳng định một kịch bản.
void runScenarioFile(const std::string& path)
{
  Scenario scenario;
  std::string error;
  ASSERT_TRUE(nav_test_harness::loadScenarioFile(path, scenario, error))
      << "could not load " << path << ": " << error;

  // Chọn driver theo DỮ LIỆU của kịch bản, không theo một khoá cấu hình riêng: kịch bản khai vật
  // cản nghĩa là nó nói về va chạm thật, và chỉ driver nạp recovery_core thật mới trả lời được. Đây
  // cũng là lý do MoveBase2ScenarioDriver báo lỗi setup khi thấy `obstacles` thay vì chạy lặng lẽ.
  ScenarioRunner runner;
  ScenarioReport report;

  // KHÔNG gọi setup() ở đây: `ScenarioRunner::run` đã tự gọi. Gọi hai lần từng làm driver nạp
  // plugin THẬT dựng registry hai lượt, và lượt cũ giữ con trỏ tới cầu nối vừa bị huỷ — use after
  // free, biểu hiện ra ngoài chỉ là một dòng "không lấy được pose" trông như lỗi TF.
  (void)error;

  if (scenario.obstacles.empty())
  {
    MoveBase2ScenarioDriver driver;
    report = runner.run(scenario, driver);
  }
  else
  {
    RecoveryScenarioDriver driver;
    report = runner.run(scenario, driver);

    EXPECT_GT(driver.startRejections(), 0u)
        << scenario.name
        << ": no behavior REFUSED to start — a test case with obstacles that never reaches a "
           "safety branch is checking something other than what it describes";
  }

  EXPECT_TRUE(report.passed) << nav_test_harness::formatReport(report);
}

/**
 * @class ScenarioFixture
 * @brief Một ca gtest cho mỗi file kịch bản, tên ca lấy từ tên file.
 */
class ScenarioFixture : public ::testing::TestWithParam<std::string>
{
};

TEST_P(ScenarioFixture, Passes)
{
  runScenarioFile(GetParam());
}

/// @brief Tên ca test lấy từ tên file, bỏ đuôi và ký tự không hợp lệ.
std::string caseName(const ::testing::TestParamInfo<std::string>& info)
{
  std::string name = info.param;
  const std::size_t slash = name.find_last_of('/');
  if (slash != std::string::npos)
  {
    name = name.substr(slash + 1);
  }
  const std::size_t dot = name.find_last_of('.');
  if (dot != std::string::npos)
  {
    name = name.substr(0, dot);
  }
  for (char& c : name)
  {
    if (!std::isalnum(static_cast<unsigned char>(c)))
    {
      c = '_';
    }
  }
  return name;
}

INSTANTIATE_TEST_SUITE_P(Scenarios, ScenarioFixture, ::testing::ValuesIn(scenarioFiles()),
                         caseName);

/// @brief Thư mục kịch bản không được rỗng — rỗng thì mọi ca ở trên biến mất mà suite vẫn xanh.
TEST(ScenarioSuite, ScenarioDirectoryIsNotEmpty)
{
  const std::vector<std::string> files = scenarioFiles();
  EXPECT_FALSE(files.empty()) << "no scenario found in " << scenarioDir()
                              << " — the suite would go green without checking anything";
}

}  // namespace

int main(int argc, char** argv)
{
  // Kịch bản có vật cản nạp plugin recovery THẬT qua Boost.DLL; ctest không mang theo biến môi
  // trường của shell nên binary phải tự trỏ, giống recovery_runner_test.
#ifdef MOVE_BASE2_TEST_CONFIG_DIR
  setenv("PNKX_NAV_CORE_CONFIG_DIR", MOVE_BASE2_TEST_CONFIG_DIR, 0);
#endif
#ifdef MOVE_BASE2_TEST_LIBRARY_DIR
  setenv("PNKX_NAV_CORE_LIBRARY_PATH", MOVE_BASE2_TEST_LIBRARY_DIR, 0);
#endif
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}