/*********************************************************************
 *
 * Kiểm quét cung và đo góc bằng pose thật của RotateRecovery.
 *
 * Bản trước không dùng `ctx()` một lần nào trong toàn file: quay mù, và đếm góc bằng
 * `angular_speed * control_period`.
 *
 * 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 kTwoPi = 2.0 * M_PI;

struct RotateFixture
{
  RotateFixture()
  {
    rig.pose.setPose(0.0, 0.0, 0.0);
    loaded = registry.loadFromConfig(nh, "recovery", rig.ctx);
    rotate = recovery_test::findBehavior(registry, "rotate");
  }

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

TEST(RotateSafety, RefusesToStartWhenArcIsBlocked)
{
  RotateFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.rotate, nullptr);

  // Footprint 0.6 x 0.4 quanh gốc: khi quay 90 độ, mũi robot quét tới y ~ +/-0.3.
  // Đặt vật cản ở đó -> cung quay bị chặn dù vị trí hiện tại vẫn trống.
  fixture.rig.costmap.setLethalCircle(0.0, 0.32, 0.06);

  RecoveryGoal goal;
  goal.angle = kTwoPi;

  EXPECT_FALSE(fixture.rotate->start(goal, robot::Time(1000.0)));
}

TEST(RotateSafety, StartsWhenArcIsClear)
{
  RotateFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.rotate, nullptr);

  RecoveryGoal goal;
  goal.angle = kTwoPi;

  EXPECT_TRUE(fixture.rotate->start(goal, robot::Time(1000.0)));
}

TEST(RotateSafety, PartialArcAvoidsBlockedSector)
{
  RotateFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.rotate, nullptr);

  // Vật cản chỉ chặn khi robot đã quay đáng kể; cung nhỏ vẫn phải đi được.
  fixture.rig.costmap.setLethalCircle(0.0, 0.32, 0.06);

  RecoveryGoal small_arc;
  small_arc.angle = 0.05;

  EXPECT_TRUE(fixture.rotate->start(small_arc, robot::Time(1000.0)));
}

TEST(RotateSafety, StopsWithZeroCommandWhenPoseIsLost)
{
  RotateFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.rotate, nullptr);

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

  fixture.rig.pose.setAvailable(false);
  const auto result = fixture.rotate->update(robot::Time(1000.2));

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

TEST(RotateSafety, RefusesToStartWhenPoseIsUnavailable)
{
  RotateFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.rotate, nullptr);

  fixture.rig.pose.setAvailable(false);

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

TEST(RotateSafety, FullRotationCountsPastPiCorrectly)
{
  RotateFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.rotate, nullptr);

  RecoveryGoal goal;
  goal.angle = kTwoPi;
  ASSERT_TRUE(fixture.rotate->start(goal, robot::Time(1000.0)));

  // Cộng dồn góc quay THẬT do test tự đo, độc lập với con số plugin báo.
  double swept = 0.0;
  double previous_yaw = fixture.rig.pose.rawPose().theta;

  robot::Time now(1000.0);
  bool finished = false;
  for (int i = 0; i < 2000; ++i)
  {
    now = robot::Time(now.toSec() + 0.1);
    const auto result = fixture.rotate->update(now);
    if (result.terminal())
    {
      EXPECT_EQ(result.status, RecoveryStatus::kSucceeded);
      finished = true;
      break;
    }
    fixture.rig.applyCommand(result.command, 0.1);

    const double yaw = fixture.rig.pose.rawPose().theta;
    swept += std::abs(recovery_core::normalizeAngle(yaw - previous_yaw));
    previous_yaw = yaw;
  }

  ASSERT_TRUE(finished);
  // Quay đủ vòng: phép chuẩn hoá từng bước phải đếm đúng qua mốc pi, không bị wrap về 0.
  EXPECT_NEAR(swept, kTwoPi, 0.05 * kTwoPi);
}

TEST(RotateSafety, SlowLoopDoesNotOvershoot)
{
  RotateFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.rotate, nullptr);

  RecoveryGoal goal;
  goal.angle = 1.0;
  ASSERT_TRUE(fixture.rotate->start(goal, robot::Time(1000.0)));

  double swept = 0.0;
  double previous_yaw = fixture.rig.pose.rawPose().theta;

  // dt gấp 5 lần nhịp thường.
  robot::Time now(1000.0);
  for (int i = 0; i < 200; ++i)
  {
    now = robot::Time(now.toSec() + 0.5);
    const auto result = fixture.rotate->update(now);
    if (result.terminal())
    {
      break;
    }
    fixture.rig.applyCommand(result.command, 0.5);

    const double yaw = fixture.rig.pose.rawPose().theta;
    swept += std::abs(recovery_core::normalizeAngle(yaw - previous_yaw));
    previous_yaw = yaw;
  }

  EXPECT_NEAR(swept, 1.0, 0.05);
}

TEST(RotateSafety, CancelEmitsZeroCommand)
{
  RotateFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  ASSERT_NE(fixture.rotate, nullptr);

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

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

  EXPECT_EQ(result.status, RecoveryStatus::kCancelled);
  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
#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();
}