/*********************************************************************
 *
 * Kiểm WaitRecovery — behavior mới của bộ default.
 *
 * Đây là recovery an toàn nhất (robot không di chuyển) và hữu dụng nhất cho AMR trong kho, nơi phần
 * lớn tình huống chặn đường là vật cản động. Nó cũng là chỗ rẻ nhất để chứng minh đường `elapsed`
 * của base chạy đúng theo đồng hồ thật.
 *
 * Author: DuongTD
 *********************************************************************/
#include <gtest/gtest.h>

#include <cstdlib>

#include <robot/node_handle.h>

#include <recovery_core/recovery_registry.h>

#include "recovery_test_utils.h"

namespace
{
using recovery_core::RecoveryGoal;
using recovery_core::RecoveryOutputType;
using recovery_core::RecoveryStatus;
using recovery_test::VelocityRig;

constexpr double kConfiguredWait = 3.0;  // [s] khớp `recovery/wait/wait_duration`

struct WaitFixture
{
  WaitFixture()
  {
    loaded = registry.loadFromConfig(nh, "recovery", rig.ctx);
    wait = recovery_test::findBehavior(registry, "wait");
  }

  VelocityRig rig;
  robot::NodeHandle nh;
  recovery_core::RecoveryRegistry registry;
  bool loaded = false;
  recovery_core::RecoveryBehavior* wait = nullptr;
};

TEST(WaitRecovery, DeclaresNoOutputFamily)
{
  WaitFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.wait, nullptr);

  EXPECT_EQ(fixture.wait->outputKind(), RecoveryOutputType::kNone);
}

TEST(WaitRecovery, NeverEmitsVelocity)
{
  WaitFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.wait, nullptr);

  ASSERT_TRUE(fixture.wait->start(RecoveryGoal(), robot::Time(1000.0)));

  robot::Time now(1000.0);
  for (int i = 0; i < 60; ++i)
  {
    now = robot::Time(now.toSec() + 0.1);
    const auto result = fixture.wait->update(now);
    // Behavior đứng yên tuyệt đối không được làm caller tưởng nó đang lái robot.
    EXPECT_EQ(result.velocity(), nullptr);
    EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
    if (result.terminal())
    {
      break;
    }
  }
}

TEST(WaitRecovery, SucceedsAfterConfiguredDuration)
{
  WaitFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.wait, nullptr);

  ASSERT_TRUE(fixture.wait->start(RecoveryGoal(), robot::Time(1000.0)));

  EXPECT_EQ(fixture.wait->update(robot::Time(1001.0)).status, RecoveryStatus::kRunning);
  EXPECT_EQ(fixture.wait->update(robot::Time(1002.9)).status, RecoveryStatus::kRunning);
  EXPECT_EQ(fixture.wait->update(robot::Time(1003.0)).status, RecoveryStatus::kSucceeded);
}

TEST(WaitRecovery, CountsByClockNotByTickCount)
{
  WaitFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.wait, nullptr);

  ASSERT_TRUE(fixture.wait->start(RecoveryGoal(), robot::Time(1000.0)));

  // Một tick duy nhất nhưng nhảy qua trọn thời lượng: phải xong ngay, không cần đủ số nhịp.
  const auto result = fixture.wait->update(robot::Time(1000.0 + kConfiguredWait));

  EXPECT_EQ(result.status, RecoveryStatus::kSucceeded);
  EXPECT_NEAR(result.elapsed, kConfiguredWait, 1e-6);
}

TEST(WaitRecovery, ProgressAdvancesMonotonically)
{
  WaitFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.wait, nullptr);

  ASSERT_TRUE(fixture.wait->start(RecoveryGoal(), robot::Time(1000.0)));

  double last = -1.0;
  for (int i = 1; i <= 5; ++i)
  {
    const auto result = fixture.wait->update(robot::Time(1000.0 + 0.5 * i));
    EXPECT_GE(result.progress, last);
    EXPECT_GE(result.remaining, 0.0);
    last = result.progress;
  }
}

TEST(WaitRecovery, PerRunDurationOverride)
{
  WaitFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.wait, nullptr);

  RecoveryGoal goal;
  goal.params["wait_duration"] = 1.0;

  ASSERT_TRUE(fixture.wait->start(goal, robot::Time(1000.0)));

  EXPECT_EQ(fixture.wait->update(robot::Time(1000.5)).status, RecoveryStatus::kRunning);
  EXPECT_EQ(fixture.wait->update(robot::Time(1001.0)).status, RecoveryStatus::kSucceeded);
}

TEST(WaitRecovery, InvalidOverrideFallsBackToConfiguredDuration)
{
  WaitFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.wait, nullptr);

  RecoveryGoal goal;
  goal.params["wait_duration"] = -5.0;  // vô lý -> phải cảnh báo và dùng default

  ASSERT_TRUE(fixture.wait->start(goal, robot::Time(1000.0)));

  EXPECT_EQ(fixture.wait->update(robot::Time(1002.0)).status, RecoveryStatus::kRunning);
  EXPECT_EQ(fixture.wait->update(robot::Time(1003.0)).status, RecoveryStatus::kSucceeded);
}

TEST(WaitRecovery, NeedsNoPoseOrCollisionPorts)
{
  // Điểm mạnh của WaitRecovery: chạy được cả khi TF hỏng, nên nó là đường phục hồi cuối cùng còn
  // dùng được khi mọi thứ khác đã mất pose.
  WaitFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.wait, nullptr);

  fixture.rig.pose.setAvailable(false);

  ASSERT_TRUE(fixture.wait->start(RecoveryGoal(), robot::Time(1000.0)));
  EXPECT_EQ(fixture.wait->update(robot::Time(1003.0)).status, RecoveryStatus::kSucceeded);
}

}  // namespace

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