recovery_core/test/recovery_lifecycle_test.cpp

/*********************************************************************
 *
 * Kiểm bất biến vòng đời của RecoveryBehavior: guard configure/start/update, kiểm ngữ cảnh bắt
 * buộc theo họ output, dt đo bằng đồng hồ thật, và đường cancel.
 *
 * Author: DuongTD
 *********************************************************************/
#include <gtest/gtest.h>

#include <cstdlib>

#include <robot/node_handle.h>

#include "recovery_test_utils.h"

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

TEST(RecoveryLifecycle, StartBeforeConfigureFails)
{
  MockBehavior behavior(RecoveryOutputType::kNone);
  const robot::Time now(1000.0);

  EXPECT_FALSE(behavior.start(RecoveryGoal(), now));
  EXPECT_EQ(behavior.start_calls, 0);
}

TEST(RecoveryLifecycle, UpdateBeforeStartReturnsFailedStopOutput)
{
  MockBehavior behavior(RecoveryOutputType::kNone);
  const robot::Time now(1000.0);

  const auto result = behavior.update(now);

  EXPECT_EQ(result.status, RecoveryStatus::kFailed);
  EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
  EXPECT_EQ(behavior.update_calls, 0);
}

TEST(RecoveryLifecycle, ConfigureTwiceRejected)
{
  MockBehavior behavior(RecoveryOutputType::kNone);
  robot::NodeHandle nh;
  recovery_core::RecoveryContext ctx;

  EXPECT_TRUE(behavior.configure("mock", ctx, nh));
  // Gọi lần hai với ctx khác là lỗi lập trình của caller — phải báo, không được nuốt.
  EXPECT_FALSE(behavior.configure("mock", ctx, nh));
  EXPECT_EQ(behavior.configure_calls, 1);
}

TEST(RecoveryLifecycle, EmptyInstanceNameRejected)
{
  MockBehavior behavior(RecoveryOutputType::kNone);
  robot::NodeHandle nh;
  recovery_core::RecoveryContext ctx;

  EXPECT_FALSE(behavior.configure("", ctx, nh));
  EXPECT_EQ(behavior.configure_calls, 0);
}

TEST(RecoveryLifecycle, VelocityFamilyRequiresPoseProvider)
{
  VelocityRig rig;
  MockBehavior behavior(RecoveryOutputType::kVelocity);
  robot::NodeHandle nh;

  recovery_core::RecoveryContext ctx = rig.ctx;
  ctx.pose = nullptr;

  // Không có pose thì tiến độ chỉ có thể dead-reckon — đúng lớp lỗi Phase 3 phải diệt.
  EXPECT_FALSE(behavior.configure("mock", ctx, nh));
}

TEST(RecoveryLifecycle, VelocityFamilyRequiresCollisionChecker)
{
  VelocityRig rig;
  MockBehavior behavior(RecoveryOutputType::kVelocity);
  robot::NodeHandle nh;

  recovery_core::RecoveryContext ctx = rig.ctx;
  ctx.collision = nullptr;

  EXPECT_FALSE(behavior.configure("mock", ctx, nh));
}

TEST(RecoveryLifecycle, PathFamilyRequiresPlanProvider)
{
  MockBehavior behavior(RecoveryOutputType::kPath);
  robot::NodeHandle nh;
  recovery_core::RecoveryContext ctx;

  EXPECT_FALSE(behavior.configure("mock", ctx, nh));
}

TEST(RecoveryLifecycle, NoneFamilyNeedsNoPorts)
{
  MockBehavior behavior(RecoveryOutputType::kNone);
  robot::NodeHandle nh;
  recovery_core::RecoveryContext ctx;

  EXPECT_TRUE(behavior.configure("mock", ctx, nh));
}

TEST(RecoveryLifecycle, ConfigureFailsWhenPluginRejects)
{
  MockBehavior behavior(RecoveryOutputType::kNone);
  behavior.configure_ok = false;
  robot::NodeHandle nh;
  recovery_core::RecoveryContext ctx;

  EXPECT_FALSE(behavior.configure("mock", ctx, nh));
  // Không được coi là đã cấu hình: start() sau đó phải hỏng.
  EXPECT_FALSE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
}

TEST(RecoveryLifecycle, StartRejectedWhenPluginRefuses)
{
  MockBehavior behavior(RecoveryOutputType::kNone);
  behavior.start_ok = false;
  robot::NodeHandle nh;
  recovery_core::RecoveryContext ctx;
  ASSERT_TRUE(behavior.configure("mock", ctx, nh));

  EXPECT_FALSE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
  EXPECT_EQ(behavior.status(), RecoveryStatus::kFailed);
}

TEST(RecoveryLifecycle, TerminalStateDoesNotTickAgain)
{
  MockBehavior behavior(RecoveryOutputType::kNone);
  robot::NodeHandle nh;
  recovery_core::RecoveryContext ctx;
  ASSERT_TRUE(behavior.configure("mock", ctx, nh));
  ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));

  behavior.next_result = recovery_core::RecoveryResult::Succeeded();
  ASSERT_EQ(behavior.update(robot::Time(1000.1)).status, RecoveryStatus::kSucceeded);
  const int calls_after_success = behavior.update_calls;

  const auto again = behavior.update(robot::Time(1000.2));
  EXPECT_EQ(again.status, RecoveryStatus::kSucceeded);
  EXPECT_EQ(behavior.update_calls, calls_after_success);  // không gọi thêm onUpdate
}

TEST(RecoveryLifecycle, CancelYieldsCancelledStopOutput)
{
  MockBehavior behavior(RecoveryOutputType::kNone);
  robot::NodeHandle nh;
  recovery_core::RecoveryContext ctx;
  ASSERT_TRUE(behavior.configure("mock", ctx, nh));
  ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));

  behavior.cancel();
  const auto result = behavior.update(robot::Time(1000.1));

  EXPECT_EQ(result.status, RecoveryStatus::kCancelled);
  EXPECT_EQ(behavior.cancel_calls, 1);
  EXPECT_EQ(behavior.update_calls, 0);  // cancel thay thế tick, không chạy logic plugin
}

TEST(RecoveryLifecycle, CancelOnVelocityFamilyEmitsExplicitZeroTwist)
{
  VelocityRig rig;
  MockBehavior behavior(RecoveryOutputType::kVelocity);
  robot::NodeHandle nh;
  ASSERT_TRUE(behavior.configure("mock", rig.ctx, nh));
  ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));

  behavior.cancel();
  const auto result = behavior.update(robot::Time(1000.1));

  // Họ velocity phải nhận lệnh dừng TƯỜNG MINH: caller đang lấy cmd_vel từ đây.
  ASSERT_NE(result.velocity(), nullptr);
  EXPECT_DOUBLE_EQ(result.velocity()->linear.x, 0.0);
  EXPECT_DOUBLE_EQ(result.velocity()->angular.z, 0.0);
}

TEST(RecoveryLifecycle, DtMeasuredFromRealClockNotConfiguredPeriod)
{
  MockBehavior behavior(RecoveryOutputType::kNone);
  robot::NodeHandle nh;
  recovery_core::RecoveryContext ctx;
  ASSERT_TRUE(behavior.configure("mock", ctx, nh));
  ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));

  behavior.update(robot::Time(1000.0));
  EXPECT_NEAR(behavior.last_dt, 0.0, 1e-9);  // tick đầu ngay sau start

  behavior.update(robot::Time(1000.5));
  EXPECT_NEAR(behavior.last_dt, 0.5, 1e-6);

  behavior.update(robot::Time(1002.5));
  EXPECT_NEAR(behavior.last_dt, 2.0, 1e-6);  // loop chạy chậm -> dt lớn, không phải hằng số config
}

TEST(RecoveryLifecycle, BackwardClockYieldsZeroDt)
{
  MockBehavior behavior(RecoveryOutputType::kNone);
  robot::NodeHandle nh;
  recovery_core::RecoveryContext ctx;
  ASSERT_TRUE(behavior.configure("mock", ctx, nh));
  ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));

  behavior.update(robot::Time(1001.0));
  behavior.update(robot::Time(1000.5));  // đồng hồ đi lùi

  EXPECT_NEAR(behavior.last_dt, 0.0, 1e-9);
}

TEST(RecoveryLifecycle, ElapsedTracksClockFromStart)
{
  MockBehavior behavior(RecoveryOutputType::kNone);
  robot::NodeHandle nh;
  recovery_core::RecoveryContext ctx;
  ASSERT_TRUE(behavior.configure("mock", ctx, nh));
  ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));

  const auto first = behavior.update(robot::Time(1001.25));
  EXPECT_NEAR(first.elapsed, 1.25, 1e-6);
  EXPECT_NEAR(behavior.elapsed(), 1.25, 1e-6);

  const auto second = behavior.update(robot::Time(1004.0));
  EXPECT_NEAR(second.elapsed, 4.0, 1e-6);
}

TEST(RecoveryLifecycle, RestartResetsElapsed)
{
  MockBehavior behavior(RecoveryOutputType::kNone);
  robot::NodeHandle nh;
  recovery_core::RecoveryContext ctx;
  ASSERT_TRUE(behavior.configure("mock", ctx, nh));

  ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
  behavior.update(robot::Time(1005.0));
  ASSERT_NEAR(behavior.elapsed(), 5.0, 1e-6);

  behavior.next_result = recovery_core::RecoveryResult::Running();
  ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(2000.0)));
  EXPECT_NEAR(behavior.elapsed(), 0.0, 1e-9);

  const auto result = behavior.update(robot::Time(2000.5));
  EXPECT_NEAR(result.elapsed, 0.5, 1e-6);
}

TEST(RecoveryLifecycle, GoalIsHandedToPluginVerbatim)
{
  MockBehavior behavior(RecoveryOutputType::kNone);
  robot::NodeHandle nh;
  recovery_core::RecoveryContext ctx;
  ASSERT_TRUE(behavior.configure("mock", ctx, nh));

  RecoveryGoal goal;
  goal.trigger = recovery_core::RecoveryTrigger::kOscillation;
  goal.angle = 1.5;
  goal.params["custom"] = 7.0;

  ASSERT_TRUE(behavior.start(goal, robot::Time(1000.0)));

  EXPECT_EQ(behavior.last_goal.trigger, recovery_core::RecoveryTrigger::kOscillation);
  ASSERT_TRUE(behavior.last_goal.angle.has_value());
  EXPECT_DOUBLE_EQ(*behavior.last_goal.angle, 1.5);
  EXPECT_DOUBLE_EQ(behavior.last_goal.param("custom", 0.0), 7.0);
}

}  // 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
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}