/*********************************************************************
 *
 * Kiểm base CƯỠNG CHẾ bất biến họ output.
 *
 * Bản trước để plugin tự đặt `output_type` mỗi tick, nên nó không dùng được để route: một plugin họ
 * path trả `kVelocity` ở tick đầu, còn base thì tự sinh `Velocity(zero)` cho mọi họ ở nhánh
 * terminal. Test này khoá lại hành vi đúng.
 *
 * Author: DuongTD
 *********************************************************************/
#include <gtest/gtest.h>

#include <cmath>
#include <cstdlib>
#include <limits>

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

/// Dựng một MockBehavior đã configure + start, sẵn sàng nhận update().
struct Fixture
{
  explicit Fixture(RecoveryOutputType kind) : behavior(kind)
  {
    ctx = rig.ctx;
    EXPECT_TRUE(behavior.configure("mock", ctx, nh));
    EXPECT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
  }

  VelocityRig rig;
  robot::NodeHandle nh;
  recovery_core::RecoveryContext ctx;
  MockBehavior behavior;
};

TEST(OutputKind, NoneFamilyNeverReportsVelocity)
{
  Fixture fixture(RecoveryOutputType::kNone);

  // Plugin cố tình trả output vận tốc dù nó khai họ kNone.
  robot_geometry_msgs::Twist rogue;
  rogue.linear.x = 0.5;
  fixture.behavior.next_result =
      recovery_core::RecoveryResult::Velocity(rogue, RecoveryStatus::kRunning);

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

  EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
  EXPECT_EQ(result.velocity(), nullptr);
  // Quan trọng nhất: giá trị rogue không được rò ra ngoài dưới bất kỳ dạng nào.
  EXPECT_DOUBLE_EQ(result.command.linear.x, 0.0);
}

TEST(OutputKind, PathFamilyNeverReportsVelocity)
{
  Fixture fixture(RecoveryOutputType::kPath);

  robot_geometry_msgs::Twist rogue;
  rogue.angular.z = 1.0;
  fixture.behavior.next_result =
      recovery_core::RecoveryResult::Velocity(rogue, RecoveryStatus::kRunning);

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

  EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
  EXPECT_EQ(result.velocity(), nullptr);
  EXPECT_EQ(result.pathOut(), nullptr);
}

TEST(OutputKind, VelocityFamilyNeverReportsPath)
{
  Fixture fixture(RecoveryOutputType::kVelocity);

  robot_nav_msgs::Path rogue;
  rogue.poses.resize(2);
  fixture.behavior.next_result =
      recovery_core::RecoveryResult::PathOut(rogue, RecoveryStatus::kRunning);

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

  EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
  EXPECT_EQ(result.pathOut(), nullptr);
  EXPECT_TRUE(result.path.poses.empty());
}

TEST(OutputKind, MatchingKindPassesThrough)
{
  Fixture fixture(RecoveryOutputType::kVelocity);

  robot_geometry_msgs::Twist cmd;
  cmd.linear.x = -0.1;
  fixture.behavior.next_result =
      recovery_core::RecoveryResult::Velocity(cmd, RecoveryStatus::kRunning);

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

  ASSERT_NE(result.velocity(), nullptr);
  EXPECT_DOUBLE_EQ(result.velocity()->linear.x, -0.1);
}

TEST(OutputKind, NoneIsAlwaysAllowed)
{
  // Một behavior họ velocity vẫn được phép nói "tick này không có output".
  Fixture fixture(RecoveryOutputType::kVelocity);
  fixture.behavior.next_result = recovery_core::RecoveryResult::Running();

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

  EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
  EXPECT_EQ(result.status, RecoveryStatus::kRunning);
}

TEST(OutputKind, TerminalStopOutputMatchesFamily)
{
  {
    Fixture none_family(RecoveryOutputType::kNone);
    none_family.behavior.next_result = recovery_core::RecoveryResult::Succeeded();
    ASSERT_EQ(none_family.behavior.update(robot::Time(1000.1)).status, RecoveryStatus::kSucceeded);

    // Tick sau khi đã kết thúc: họ kNone KHÔNG được báo cáo mình phát vận tốc.
    const auto after = none_family.behavior.update(robot::Time(1000.2));
    EXPECT_EQ(after.output_type, RecoveryOutputType::kNone);
  }
  {
    Fixture velocity_family(RecoveryOutputType::kVelocity);
    velocity_family.behavior.next_result = recovery_core::RecoveryResult::Succeeded();
    ASSERT_EQ(velocity_family.behavior.update(robot::Time(1000.1)).status,
              RecoveryStatus::kSucceeded);

    // Họ velocity thì ngược lại: phải có lệnh dừng tường minh để caller publish.
    const auto after = velocity_family.behavior.update(robot::Time(1000.2));
    ASSERT_NE(after.velocity(), nullptr);
    EXPECT_DOUBLE_EQ(after.velocity()->linear.x, 0.0);
    EXPECT_DOUBLE_EQ(after.velocity()->angular.z, 0.0);
  }
}

TEST(OutputKind, NonFiniteVelocityIsForcedToStop)
{
  Fixture fixture(RecoveryOutputType::kVelocity);

  robot_geometry_msgs::Twist bad;
  bad.linear.x = std::numeric_limits<double>::quiet_NaN();
  fixture.behavior.next_result =
      recovery_core::RecoveryResult::Velocity(bad, RecoveryStatus::kRunning);

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

  // NaN lọt ra cmd_vel là không được phép đi tiếp.
  EXPECT_EQ(result.status, RecoveryStatus::kFailed);
  ASSERT_NE(result.velocity(), nullptr);
  EXPECT_TRUE(std::isfinite(result.velocity()->linear.x));
  EXPECT_DOUBLE_EQ(result.velocity()->linear.x, 0.0);
}

TEST(OutputKind, InfiniteAngularVelocityIsForcedToStop)
{
  Fixture fixture(RecoveryOutputType::kVelocity);

  robot_geometry_msgs::Twist bad;
  bad.angular.z = std::numeric_limits<double>::infinity();
  fixture.behavior.next_result =
      recovery_core::RecoveryResult::Velocity(bad, RecoveryStatus::kRunning);

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

  EXPECT_EQ(result.status, RecoveryStatus::kFailed);
  ASSERT_NE(result.velocity(), nullptr);
  EXPECT_DOUBLE_EQ(result.velocity()->angular.z, 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();
}