/*********************************************************************
 *
 * Kiểm tiến độ đo bằng POSE THẬT, không dead-reckon theo chu kỳ cấu hình.
 *
 * Đây là test cho lỗi nặng nhất của bản trước: quãng đi được tính bằng
 * `|cmd.linear.x| * control_period` với `control_period` lấy từ YAML. Control loop chạy chậm gấp N
 * lần là robot đi quá quãng gấp N lần, còn bánh trượt thì vẫn báo hoàn thành.
 *
 * 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;

constexpr double kConfiguredDistance = 0.28;  // [m] khớp `recovery/back_up/backup_distance`

struct BackUpFixture
{
  BackUpFixture()
  {
    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;
};

/**
 * @brief Chạy trọn một lượt lùi với chu kỳ @p dt, mô phỏng robot đi đúng lệnh phát ra.
 * @return quãng đường thực tế robot đã lùi [m].
 */
double runBackup(BackUpFixture& fixture, double dt, int max_ticks = 2000)
{
  EXPECT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));

  robot::Time now(1000.0);
  for (int i = 0; i < max_ticks; ++i)
  {
    now = robot::Time(now.toSec() + dt);
    const auto result = fixture.back_up->update(now);
    if (result.terminal())
    {
      EXPECT_EQ(result.status, RecoveryStatus::kSucceeded);
      break;
    }
    fixture.rig.applyCommand(result.command, dt);
  }

  return -fixture.rig.pose.rawPose().x;  // lùi theo -x
}

TEST(PoseProgress, NominalRateStopsAtRequestedDistance)
{
  BackUpFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.back_up, nullptr);

  const double traveled = runBackup(fixture, 0.1);

  EXPECT_NEAR(traveled, kConfiguredDistance, 0.05 * kConfiguredDistance);
}

TEST(PoseProgress, FiveTimesSlowerLoopStillStopsAtRequestedDistance)
{
  BackUpFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.back_up, nullptr);

  // Control loop chạy chậm gấp 5. Bản cũ sai ~400% ở đây vì nhân với hằng số config.
  const double traveled = runBackup(fixture, 0.5);

  EXPECT_NEAR(traveled, kConfiguredDistance, 0.05 * kConfiguredDistance);
}

TEST(PoseProgress, TwentyTimesSlowerLoopStillStopsAtRequestedDistance)
{
  BackUpFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.back_up, nullptr);

  const double traveled = runBackup(fixture, 2.0);

  EXPECT_NEAR(traveled, kConfiguredDistance, 0.05 * kConfiguredDistance);
}

TEST(PoseProgress, FasterLoopStopsAtRequestedDistance)
{
  BackUpFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.back_up, nullptr);

  const double traveled = runBackup(fixture, 0.02);

  EXPECT_NEAR(traveled, kConfiguredDistance, 0.05 * kConfiguredDistance);
}

TEST(PoseProgress, StalledRobotNeverReportsSuccess)
{
  BackUpFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.back_up, nullptr);

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

  // Bánh trượt hoàn toàn: lệnh vẫn phát nhưng pose không đổi. Bản cũ tích phân vận tốc LỆNH nên vẫn
  // báo kSucceeded; bản này phải chạy tới khi timeout chứ không được nói dối.
  robot::Time now(1000.0);
  bool reported_success = false;
  for (int i = 0; i < 200; ++i)
  {
    now = robot::Time(now.toSec() + 0.1);
    const auto result = fixture.back_up->update(now);
    if (result.status == RecoveryStatus::kSucceeded)
    {
      reported_success = true;
      break;
    }
    if (result.terminal())
    {
      break;  // timeout -> kFailed, đúng như mong đợi
    }
    // KHÔNG applyCommand: robot không nhúc nhích.
  }

  EXPECT_FALSE(reported_success);
}

TEST(PoseProgress, ProgressAndRemainingTrackRealPose)
{
  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);
  double last_progress = -1.0;
  for (int i = 0; i < 10; ++i)
  {
    now = robot::Time(now.toSec() + 0.1);
    const auto result = fixture.back_up->update(now);
    if (result.terminal())
    {
      break;
    }

    const double traveled = -fixture.rig.pose.rawPose().x;
    EXPECT_NEAR(result.remaining, kConfiguredDistance - traveled, 1e-6);
    EXPECT_GE(result.progress, last_progress);
    EXPECT_GE(result.progress, 0.0);
    EXPECT_LE(result.progress, 1.0);
    last_progress = result.progress;

    fixture.rig.applyCommand(result.command, 0.1);
  }
}

}  // 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();
}