/*********************************************************************
 *
 * Kiểm `elapsed` + `timeout` của base.
 *
 * Bản trước khai `RecoveryResult::elapsed` trong header nhưng KHÔNG nơi nào ghi vào nó, nên caller
 * không có cách phát hiện recovery treo. Test này khoá cả hai chiều: elapsed phải bám đồng hồ thật,
 * và quá timeout phải là kFailed kèm stop output đúng họ.
 *
 * Author: DuongTD
 *********************************************************************/
#include <gtest/gtest.h>

#include <cstdlib>
#include <string>

#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;

/// NodeHandle trỏ vào một namespace có sẵn khoá `timeout` trong config test.
robot::NodeHandle timeoutNodeHandle(const std::string& ns)
{
  robot::NodeHandle root;
  return robot::NodeHandle(root, ns);
}

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

  // Không khai `timeout` trong namespace này -> 0 = không giới hạn.
  EXPECT_DOUBLE_EQ(behavior.timeout(), 0.0);

  ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
  const auto result = behavior.update(robot::Time(1000.0 + 3600.0));
  EXPECT_EQ(result.status, RecoveryStatus::kRunning);
}

TEST(Timeout, ReadFromConfiguredNamespace)
{
  MockBehavior behavior(RecoveryOutputType::kNone);
  robot::NodeHandle nh = timeoutNodeHandle("recovery/rotate");
  recovery_core::RecoveryContext ctx;
  ASSERT_TRUE(behavior.configure("mock", ctx, nh));

  EXPECT_DOUBLE_EQ(behavior.timeout(), 20.0);
}

TEST(Timeout, ExceededYieldsFailedAndStopsTicking)
{
  MockBehavior behavior(RecoveryOutputType::kNone);
  robot::NodeHandle nh = timeoutNodeHandle("recovery/rotate");  // timeout: 20 s
  recovery_core::RecoveryContext ctx;
  ASSERT_TRUE(behavior.configure("mock", ctx, nh));
  ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));

  ASSERT_EQ(behavior.update(robot::Time(1019.0)).status, RecoveryStatus::kRunning);
  const int calls_before = behavior.update_calls;

  const auto timed_out = behavior.update(robot::Time(1020.5));

  EXPECT_EQ(timed_out.status, RecoveryStatus::kFailed);
  EXPECT_NEAR(timed_out.elapsed, 20.5, 1e-6);
  EXPECT_FALSE(timed_out.message.empty());
  // Quá hạn thì KHÔNG giao quyền cho plugin nữa.
  EXPECT_EQ(behavior.update_calls, calls_before);
}

TEST(Timeout, VelocityFamilyStopsWithExplicitZeroTwist)
{
  VelocityRig rig;
  MockBehavior behavior(RecoveryOutputType::kVelocity);
  robot::NodeHandle nh = timeoutNodeHandle("recovery/rotate");  // timeout: 20 s
  ASSERT_TRUE(behavior.configure("mock", rig.ctx, nh));
  ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));

  const auto timed_out = behavior.update(robot::Time(1021.0));

  ASSERT_EQ(timed_out.status, RecoveryStatus::kFailed);
  ASSERT_NE(timed_out.velocity(), nullptr);
  EXPECT_DOUBLE_EQ(timed_out.velocity()->linear.x, 0.0);
  EXPECT_DOUBLE_EQ(timed_out.velocity()->angular.z, 0.0);
}

TEST(Timeout, ElapsedIsSetOnEveryResult)
{
  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)));

  // Plugin không đặt elapsed; base phải điền vào.
  behavior.next_result = recovery_core::RecoveryResult::Running();
  EXPECT_NEAR(behavior.update(robot::Time(1002.0)).elapsed, 2.0, 1e-6);

  behavior.next_result = recovery_core::RecoveryResult::Succeeded();
  EXPECT_NEAR(behavior.update(robot::Time(1007.5)).elapsed, 7.5, 1e-6);
}

TEST(Timeout, OutOfRangeConfigFallsBackToDisabled)
{
  MockBehavior behavior(RecoveryOutputType::kNone);
  // `recovery/bad_timeout` khai timeout âm -> phải cảnh báo và về 0 chứ không nhận giá trị âm.
  robot::NodeHandle nh = timeoutNodeHandle("recovery_bad/bad_timeout");
  recovery_core::RecoveryContext ctx;
  ASSERT_TRUE(behavior.configure("mock", ctx, nh));

  EXPECT_DOUBLE_EQ(behavior.timeout(), 0.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();
}