/*********************************************************************
 *
 * move_base2 — kiểm chỗ nối RecoveryPort <-> recovery_core.
 *
 * Đây là seam giữa hai gói: nếu nó đúng thì mọi behavior của recovery_core dùng được từ lõi mà lõi
 * không biết gì về recovery_core. Test nạp plugin qua đúng đường Boost.DLL mà runtime đi.
 *
 * Author: DuongTD
 *********************************************************************/
#include <gtest/gtest.h>

#include <cstdlib>
#include <string>

#include <robot/node_handle.h>

#include <move_base2/runners/recovery_runner.h>

#include "fake_ports.h"

namespace
{
using move_base2::RecoveryOutputKind;
using move_base2::RecoveryRunner;
using move_base2::RecoveryTick;
using move_base2::RecoveryTrigger;
using move_base2::testing::FakeClockPort;
using move_base2::testing::FakePosePort;

/// Bộ đồ nghề tối thiểu: đồng hồ giả + pose giả, không costmap (behavior họ kNone không cần).
struct Rig
{
  Rig()
  {
    pose.setPosition(0.0, 0.0);

    RecoveryRunner::Deps deps;
    deps.clock = &clock;
    deps.pose = &pose;
    runner.setDeps(deps);
  }

  bool load(const std::string& ns)
  {
    runner.setNamespace(ns);
    robot::NodeHandle nh;
    if (!runner.configure(nh))
    {
      return false;
    }
    std::string error;
    return runner.configureRoutes(nh, error);
  }

  FakeClockPort clock{1000.0};
  FakePosePort pose;
  RecoveryRunner runner;
};

TEST(RecoveryRunner, LoadsBehaviorsInDeclaredOrder)
{
  Rig rig;
  ASSERT_TRUE(rig.load("recovery"));

  ASSERT_EQ(rig.runner.behaviorCount(), 2u);
  EXPECT_EQ(rig.runner.behaviorName(0), "wait_short");
  EXPECT_EQ(rig.runner.behaviorName(1), "wait_long");
}

TEST(RecoveryRunner, ResolvesPerTriggerRoutesByBehaviorName)
{
  Rig rig;
  ASSERT_TRUE(rig.load("recovery"));

  const move_base2::RecoveryRoutes& routes = rig.runner.routes();
  ASSERT_EQ(routes.planning_failed.size(), 1u);
  EXPECT_EQ(routes.planning_failed[0], 0u);  // wait_short
  ASSERT_EQ(routes.controlling_failed.size(), 2u);
  EXPECT_EQ(routes.controlling_failed[0], 1u);  // wait_long
  EXPECT_EQ(routes.controlling_failed[1], 0u);  // wait_short
  ASSERT_EQ(routes.oscillation.size(), 1u);
  EXPECT_EQ(routes.oscillation[0], 1u);  // wait_long
}

TEST(RecoveryRunner, SkipsFuturePluginFromRoutesWithoutCreatingInvalidIndexes)
{
  Rig rig;
  rig.runner.setNamespace("recovery_missing_detour");
  robot::NodeHandle nh;

  // Registry báo false vì plugin chưa có, nhưng wait vẫn được nạp và phải dùng được.
  EXPECT_FALSE(rig.runner.configure(nh));
  ASSERT_EQ(rig.runner.behaviorCount(), 1u);

  std::string error;
  ASSERT_TRUE(rig.runner.configureRoutes(nh, error)) << error;
  const move_base2::RecoveryRoutes& routes = rig.runner.routes();
  ASSERT_EQ(routes.controlling_failed.size(), 1u);
  EXPECT_EQ(routes.controlling_failed[0], 0u);
  ASSERT_EQ(routes.oscillation.size(), 1u);
  EXPECT_EQ(routes.oscillation[0], 0u);
}

TEST(RecoveryRunner, ReportsOutputKindOfLoadedBehaviors)
{
  Rig rig;
  ASSERT_TRUE(rig.load("recovery"));

  EXPECT_EQ(rig.runner.outputKind(0), RecoveryOutputKind::kNone);
  EXPECT_EQ(rig.runner.outputKind(1), RecoveryOutputKind::kNone);
}

TEST(RecoveryRunner, OutOfRangeIndexIsSafeAndNeverClaimsVelocity)
{
  Rig rig;
  ASSERT_TRUE(rig.load("recovery"));

  // Giả định an toàn: không biết là gì thì không cấp quyền phát vận tốc.
  EXPECT_EQ(rig.runner.outputKind(99), RecoveryOutputKind::kNone);
  EXPECT_TRUE(rig.runner.behaviorName(99).empty());
  EXPECT_FALSE(rig.runner.start(99, RecoveryTrigger::kPlanningFailed));
}

TEST(RecoveryRunner, StartBeforeConfigureFails)
{
  Rig rig;
  EXPECT_FALSE(rig.runner.start(0, RecoveryTrigger::kPlanningFailed));
}

TEST(RecoveryRunner, UpdateWithoutActiveBehaviorFailsInsteadOfCrashing)
{
  Rig rig;
  ASSERT_TRUE(rig.load("recovery"));

  const RecoveryTick tick = rig.runner.update();

  EXPECT_EQ(tick.status, RecoveryTick::Status::kFailed);
  EXPECT_FALSE(tick.has_velocity);
  EXPECT_FALSE(tick.message.empty());
}

TEST(RecoveryRunner, RunsBehaviorToSuccessOnRealClock)
{
  Rig rig;
  ASSERT_TRUE(rig.load("recovery"));
  ASSERT_TRUE(rig.runner.start(0, RecoveryTrigger::kPlanningFailed));  // wait_duration: 1.0 s

  rig.clock.advance(0.5);
  EXPECT_EQ(rig.runner.update().status, RecoveryTick::Status::kRunning);

  rig.clock.advance(0.5);
  EXPECT_EQ(rig.runner.update().status, RecoveryTick::Status::kSucceeded);
}

TEST(RecoveryRunner, NoneFamilyNeverReportsVelocityToTheCore)
{
  Rig rig;
  ASSERT_TRUE(rig.load("recovery"));
  ASSERT_TRUE(rig.runner.start(0, RecoveryTrigger::kPlanningFailed));

  for (int i = 0; i < 5; ++i)
  {
    rig.clock.advance(0.3);
    const RecoveryTick tick = rig.runner.update();
    // Lõi dùng has_velocity để quyết định có lấy cmd hay không; behavior đứng yên không được bật.
    EXPECT_FALSE(tick.has_velocity);
    EXPECT_FALSE(tick.has_path);
    if (tick.status != RecoveryTick::Status::kRunning)
    {
      break;
    }
  }
}

TEST(RecoveryRunner, BehaviorTimeoutSurfacesAsFailed)
{
  Rig rig;
  ASSERT_TRUE(rig.load("recovery"));
  // wait_long: wait_duration 5 s nhưng timeout 3 s -> phải kết thúc bằng kFailed, không treo.
  ASSERT_TRUE(rig.runner.start(1, RecoveryTrigger::kControllingFailed));

  rig.clock.advance(2.0);
  ASSERT_EQ(rig.runner.update().status, RecoveryTick::Status::kRunning);

  rig.clock.advance(1.5);
  const RecoveryTick tick = rig.runner.update();

  EXPECT_EQ(tick.status, RecoveryTick::Status::kFailed);
  EXPECT_NE(tick.message.find("timeout"), std::string::npos);
}

TEST(RecoveryRunner, CancelIsSafeWithoutActiveBehavior)
{
  Rig rig;
  ASSERT_TRUE(rig.load("recovery"));

  rig.runner.cancel();  // không được crash
  SUCCEED();
}

TEST(RecoveryRunner, CancelledTickSurfacesAsFailedNotRunning)
{
  Rig rig;
  ASSERT_TRUE(rig.load("recovery"));
  ASSERT_TRUE(rig.runner.start(0, RecoveryTrigger::kPlanningFailed));

  rig.runner.cancel();
  rig.clock.advance(0.1);
  const RecoveryTick tick = rig.runner.update();

  // State machine hiện tại không tick sau cancel, nên nhánh này không đạt tới trong runtime thật.
  // Nhưng nếu ai đó nới điều kiện tick, kCancelled phải thành kFailed chứ không im lặng thành
  // kRunning — đó là lý do nhánh dịch được giữ lại.
  EXPECT_EQ(tick.status, RecoveryTick::Status::kFailed);
}

TEST(RecoveryRunner, RestartingSecondBehaviorWorks)
{
  Rig rig;
  ASSERT_TRUE(rig.load("recovery"));

  ASSERT_TRUE(rig.runner.start(0, RecoveryTrigger::kPlanningFailed));
  rig.clock.advance(1.0);
  ASSERT_EQ(rig.runner.update().status, RecoveryTick::Status::kSucceeded);

  // State machine chuyển sang behavior kế tiếp sau khi cái trước kết thúc.
  ASSERT_TRUE(rig.runner.start(1, RecoveryTrigger::kOscillation));
  rig.clock.advance(0.5);
  EXPECT_EQ(rig.runner.update().status, RecoveryTick::Status::kRunning);
}

TEST(RecoveryRunner, EmptyBehaviorListFailsConfigure)
{
  Rig rig;
  EXPECT_FALSE(rig.load("recovery_empty"));
  EXPECT_EQ(rig.runner.behaviorCount(), 0u);
}

TEST(RecoveryRunner, MissingLibraryPathFailsConfigure)
{
  Rig rig;
  EXPECT_FALSE(rig.load("recovery_missing_library"));
  EXPECT_EQ(rig.runner.behaviorCount(), 0u);
}

TEST(RecoveryRunner, ConfigureRequiresClockAndPose)
{
  RecoveryRunner runner;
  runner.setNamespace("recovery");

  robot::NodeHandle nh;
  EXPECT_FALSE(runner.configure(nh)) << "a missing ClockPort/PosePort must fail right away, not at "
                                        "tick time";
}

TEST(RecoveryRunner, ConfigureTwiceRejected)
{
  Rig rig;
  ASSERT_TRUE(rig.load("recovery"));

  robot::NodeHandle nh;
  EXPECT_FALSE(rig.runner.configure(nh));
}

}  // namespace

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
#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();
}