/*********************************************************************
 *
 * Kiểm ba lớp an toàn của BackUpRecovery.
 *
 * Bản trước không dùng collision checker (chỉ null-check con trỏ, và chỉ khi `require_costmap` bật
 * — mặc định TẮT), nên mặc định robot lùi mù. Lùi là hướng robot thường không có sensor, nên đây là
 * bộ test quan trọng nhất của Phase 3.
 *
 * Author: DuongTD
 *********************************************************************/
#include <gtest/gtest.h>

#include <cmath>
#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::RecoveryStatus;
using recovery_test::VelocityRig;

struct BackUpFixture
{
  BackUpFixture()
  {
    // Robot nhìn theo +x tại gốc; lùi nghĩa là đi về phía -x.
    rig.pose.setPose(0.0, 0.0, 0.0);
    loaded = registry.loadFromConfig(nh, "recovery", rig.ctx);
    back_up = recovery_test::findBehavior(registry, "back_up");
  }

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

TEST(BackupSafety, RefusesToStartWhenObstacleIsBehind)
{
  BackUpFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.back_up, nullptr);

  // Vật cản chạm biên sau của footprint (footprint 0.6 x 0.4 -> biên sau ở x = -0.3).
  fixture.rig.costmap.setLethalCircle(-0.35, 0.0, 0.10);

  EXPECT_FALSE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
}

TEST(BackupSafety, StopsWithZeroCommandWhenObstacleAppearsMidRun)
{
  BackUpFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.back_up, nullptr);

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

  // Vài tick đầu chạy bình thường.
  robot::Time now(1000.0);
  for (int i = 0; i < 3; ++i)
  {
    now = robot::Time(now.toSec() + 0.1);
    const auto result = fixture.back_up->update(now);
    ASSERT_EQ(result.status, RecoveryStatus::kRunning);
    ASSERT_NE(result.velocity(), nullptr);
    EXPECT_LT(result.velocity()->linear.x, 0.0);  // âm = lùi
    fixture.rig.applyCommand(result.command, 0.1);
  }

  // Có người bước vào phía sau robot.
  const double robot_x = fixture.rig.pose.rawPose().x;
  fixture.rig.costmap.setLethalCircle(robot_x - 0.36, 0.0, 0.10);

  now = robot::Time(now.toSec() + 0.1);
  const auto blocked = fixture.back_up->update(now);

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

TEST(BackupSafety, StopsWhenPoseIsLost)
{
  BackUpFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.back_up, nullptr);

  ASSERT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
  ASSERT_EQ(fixture.back_up->update(robot::Time(1000.1)).status, RecoveryStatus::kRunning);

  // TF quá hạn / thiếu frame.
  fixture.rig.pose.setAvailable(false);
  const auto result = fixture.back_up->update(robot::Time(1000.2));

  EXPECT_EQ(result.status, RecoveryStatus::kFailed);
  ASSERT_NE(result.velocity(), nullptr);
  EXPECT_DOUBLE_EQ(result.velocity()->linear.x, 0.0);
}

TEST(BackupSafety, RefusesToStartWhenPoseIsUnavailable)
{
  BackUpFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.back_up, nullptr);

  fixture.rig.pose.setAvailable(false);

  EXPECT_FALSE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
}

TEST(BackupSafety, CommandNeverExceedsConfiguredSpeed)
{
  BackUpFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.back_up, nullptr);

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

  robot::Time now(1000.0);
  for (int i = 0; i < 40; ++i)
  {
    now = robot::Time(now.toSec() + 0.1);
    const auto result = fixture.back_up->update(now);
    if (result.terminal())
    {
      break;
    }
    ASSERT_NE(result.velocity(), nullptr);
    // linear_speed: 0.1 m/s trong config test.
    EXPECT_LE(std::abs(result.velocity()->linear.x), 0.1 + 1e-9);
    fixture.rig.applyCommand(result.command, 0.1);
  }
}

TEST(BackupSafety, RampsUpInsteadOfSteppingToFullSpeed)
{
  BackUpFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.back_up, nullptr);

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

  // acc_lim_x: 0.3 m/s^2 -> sau 0.05 s không thể vượt 0.015 m/s.
  const auto first = fixture.back_up->update(robot::Time(1000.05));
  ASSERT_NE(first.velocity(), nullptr);
  EXPECT_LE(std::abs(first.velocity()->linear.x), 0.015 + 1e-9);
}

TEST(BackupSafety, CancelEmitsZeroCommand)
{
  BackUpFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.back_up, nullptr);

  ASSERT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
  ASSERT_EQ(fixture.back_up->update(robot::Time(1000.1)).status, RecoveryStatus::kRunning);

  fixture.back_up->cancel();
  const auto result = fixture.back_up->update(robot::Time(1000.2));

  EXPECT_EQ(result.status, RecoveryStatus::kCancelled);
  ASSERT_NE(result.velocity(), nullptr);
  EXPECT_DOUBLE_EQ(result.velocity()->linear.x, 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
#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();
}