/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * move_base2 — test bộ trọng tài vận tốc.
 *
 * Ba quy tắc phải được chứng minh chứ không chỉ được ghi trong comment: kNone phát 0, mọi lệnh đi
 * qua sanitize, và đổi nguồn luôn chèn một cycle 0.
 *
 * Author: DuongTD
 *********************************************************************/
#include <gtest/gtest.h>

#include <cmath>
#include <limits>
#include <string>

#include <move_base2/core/velocity_arbiter.h>

using move_base2::VelocityArbiter;
using move_base2::VelocityLimits;
using move_base2::VelocitySource;

namespace
{

constexpr double kDt = 0.05;  ///< [s] chu kỳ dùng trong test

VelocityLimits baseLimits()
{
  VelocityLimits limits;
  limits.max_vel_x = 0.5;         // [m/s]
  limits.min_vel_x = -0.2;        // [m/s]
  limits.max_vel_theta = 1.0;     // [rad/s]
  limits.max_accel_x = 100.0;     // [m/s^2] rất lớn: mặc định tắt ảnh hưởng của giới hạn gia tốc
  limits.max_accel_theta = 100.0; // [rad/s^2]
  limits.zero_velocity_epsilon = 1e-3;
  return limits;
}

robot_geometry_msgs::Twist twist(double linear_x, double angular_z)
{
  robot_geometry_msgs::Twist cmd;
  cmd.linear.x = linear_x;
  cmd.angular.z = angular_z;
  return cmd;
}

VelocityArbiter makeArbiter(const VelocityLimits& limits = baseLimits())
{
  VelocityArbiter arbiter;
  std::string error;
  EXPECT_TRUE(arbiter.configure(limits, error)) << error;
  return arbiter;
}

}  // namespace

// ================================================================================================
// Cấu hình
// ================================================================================================

TEST(VelocityLimits, RejectsNonPositiveMaxVelX)
{
  VelocityLimits limits = baseLimits();
  limits.max_vel_x = 0.0;

  std::string error;
  EXPECT_FALSE(limits.validate(error));
  EXPECT_NE(error.find("max_vel_x"), std::string::npos);
}

TEST(VelocityLimits, RejectsPositiveMinVelXBecauseItIsTheReverseLimit)
{
  VelocityLimits limits = baseLimits();
  limits.min_vel_x = 0.3;

  std::string error;
  EXPECT_FALSE(limits.validate(error));
  EXPECT_NE(error.find("min_vel_x"), std::string::npos);
}

TEST(VelocityLimits, RejectsNonPositiveAccelerations)
{
  std::string error;

  VelocityLimits linear = baseLimits();
  linear.max_accel_x = 0.0;
  EXPECT_FALSE(linear.validate(error));

  VelocityLimits angular = baseLimits();
  angular.max_accel_theta = -1.0;
  EXPECT_FALSE(angular.validate(error));
}

TEST(VelocityLimits, DescribeMarksReverseAsDisabledWhenZero)
{
  VelocityLimits limits = baseLimits();
  limits.min_vel_x = 0.0;
  EXPECT_NE(limits.describe().find("reversing forbidden"), std::string::npos);
}

TEST(VelocityArbiter, RefusesToEmitBeforeConfigure)
{
  VelocityArbiter arbiter;
  EXPECT_FALSE(arbiter.initialized());

  const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(0.4, 0.0), kDt);
  EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0);
  EXPECT_DOUBLE_EQ(cmd.angular.z, 0.0);
}

TEST(VelocityArbiter, ConfigureFailsLoudlyOnBadLimits)
{
  VelocityLimits limits = baseLimits();
  limits.max_vel_theta = -1.0;

  VelocityArbiter arbiter;
  std::string error;
  EXPECT_FALSE(arbiter.configure(limits, error));
  EXPECT_FALSE(arbiter.initialized());
}

// ================================================================================================
// Quy tắc 1 — nguồn kNone phát 0
// ================================================================================================

TEST(VelocityArbiter, NoneSourceEmitsExactZeroImmediately)
{
  VelocityArbiter arbiter = makeArbiter();

  arbiter.arbitrate(VelocitySource::kController, twist(0.5, 0.8), kDt);
  ASSERT_GT(arbiter.lastCommand().linear.x, 0.0);

  const auto cmd = arbiter.arbitrate(VelocitySource::kNone, twist(0.5, 0.8), kDt);

  EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0) << "a zero command must be immediate, not ramped down";
  EXPECT_DOUBLE_EQ(cmd.angular.z, 0.0);
  EXPECT_TRUE(arbiter.stopped());
}

TEST(VelocityArbiter, NoneSourceIgnoresCandidateEntirely)
{
  VelocityArbiter arbiter = makeArbiter();

  const auto cmd = arbiter.arbitrate(VelocitySource::kNone, twist(99.0, 99.0), kDt);
  EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0);
  EXPECT_DOUBLE_EQ(cmd.angular.z, 0.0);
  EXPECT_EQ(arbiter.activeSource(), VelocitySource::kNone);
}

// ================================================================================================
// Quy tắc 2 — sanitize
// ================================================================================================

TEST(VelocityArbiter, NaNIsBlockedAndCounted)
{
  VelocityArbiter arbiter = makeArbiter();
  const double nan_value = std::numeric_limits<double>::quiet_NaN();

  const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(nan_value, 0.3), kDt);

  EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0);
  EXPECT_DOUBLE_EQ(cmd.angular.z, 0.0) << "one broken axis breaks the whole command, it is not "
                                          "patched per axis";
  EXPECT_EQ(arbiter.nonFiniteRejections(), 1u);
}

TEST(VelocityArbiter, InfinityIsBlockedToo)
{
  VelocityArbiter arbiter = makeArbiter();
  const double inf_value = std::numeric_limits<double>::infinity();

  const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(0.2, inf_value), kDt);

  EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0);
  EXPECT_DOUBLE_EQ(cmd.angular.z, 0.0);
  EXPECT_EQ(arbiter.nonFiniteRejections(), 1u);
}

TEST(VelocityArbiter, ForwardVelocityIsClampedToMax)
{
  VelocityArbiter arbiter = makeArbiter();

  const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(9.0, 0.0), kDt);

  EXPECT_DOUBLE_EQ(cmd.linear.x, 0.5);
  EXPECT_EQ(arbiter.velocityClamps(), 1u);
}

TEST(VelocityArbiter, ReverseVelocityIsClampedToMinNotToZero)
{
  VelocityArbiter arbiter = makeArbiter();

  const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(-9.0, 0.0), kDt);

  EXPECT_DOUBLE_EQ(cmd.linear.x, -0.2) << "min_vel_x is the REVERSE limit, not a lower bound of "
                                          "zero";
}

TEST(VelocityArbiter, ReverseIsForbiddenWhenMinVelXIsZero)
{
  VelocityLimits limits = baseLimits();
  limits.min_vel_x = 0.0;
  VelocityArbiter arbiter = makeArbiter(limits);

  const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(-0.5, 0.0), kDt);
  EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0);
}

TEST(VelocityArbiter, YawRateIsClampedBothDirections)
{
  VelocityArbiter arbiter = makeArbiter();

  EXPECT_DOUBLE_EQ(arbiter.arbitrate(VelocitySource::kController, twist(0.0, 5.0), kDt).angular.z,
                   1.0);
  EXPECT_DOUBLE_EQ(arbiter.arbitrate(VelocitySource::kController, twist(0.0, -5.0), kDt).angular.z,
                   -1.0);
}

TEST(VelocityArbiter, LateralAndUnusedAxesAreDropped)
{
  VelocityArbiter arbiter = makeArbiter();

  robot_geometry_msgs::Twist candidate = twist(0.2, 0.1);
  candidate.linear.y = 0.7;
  candidate.linear.z = 0.7;
  candidate.angular.x = 0.7;
  candidate.angular.y = 0.7;

  const auto cmd = arbiter.arbitrate(VelocitySource::kController, candidate, kDt);

  EXPECT_DOUBLE_EQ(cmd.linear.y, 0.0);
  EXPECT_DOUBLE_EQ(cmd.linear.z, 0.0);
  EXPECT_DOUBLE_EQ(cmd.angular.x, 0.0);
  EXPECT_DOUBLE_EQ(cmd.angular.y, 0.0);
}

TEST(VelocityArbiter, AccelerationIsLimitedByRealDtNotNominalPeriod)
{
  VelocityLimits limits = baseLimits();
  limits.max_accel_x = 1.0;  // [m/s^2]
  VelocityArbiter arbiter = makeArbiter(limits);

  // dt = 0.05 s -> bước nhảy tối đa 0.05 m/s.
  const auto small_step = arbiter.arbitrate(VelocitySource::kController, twist(0.5, 0.0), 0.05);
  EXPECT_NEAR(small_step.linear.x, 0.05, 1e-9);
  EXPECT_EQ(arbiter.accelerationClamps(), 1u);

  // Cycle chậm gấp 10: dt = 0.5 s -> bước nhảy tối đa 0.5 m/s, nên đạt luôn trần vận tốc.
  const auto big_step = arbiter.arbitrate(VelocitySource::kController, twist(0.5, 0.0), 0.5);
  EXPECT_NEAR(big_step.linear.x, 0.5, 1e-9);
}

TEST(VelocityArbiter, NonPositiveDtSkipsAccelerationLimitInsteadOfInventingOne)
{
  VelocityLimits limits = baseLimits();
  limits.max_accel_x = 1.0;
  VelocityArbiter arbiter = makeArbiter(limits);

  const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(0.5, 0.0), 0.0);
  EXPECT_NEAR(cmd.linear.x, 0.5, 1e-9);
}

TEST(VelocityArbiter, DecelerationIsAlsoLimited)
{
  VelocityLimits limits = baseLimits();
  limits.max_accel_x = 1.0;
  VelocityArbiter arbiter = makeArbiter(limits);

  // Tăng dần tới 0.3 m/s.
  for (int i = 0; i < 20; ++i)
  {
    arbiter.arbitrate(VelocitySource::kController, twist(0.3, 0.0), 0.05);
  }
  ASSERT_NEAR(arbiter.lastCommand().linear.x, 0.3, 1e-6);

  // Yêu cầu về 0 ngay: vẫn cùng nguồn nên bị giới hạn gia tốc chặn lại.
  const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(0.0, 0.0), 0.05);
  EXPECT_NEAR(cmd.linear.x, 0.25, 1e-6);
}

// ================================================================================================
// Quy tắc 3 — đổi nguồn chèn một cycle 0
// ================================================================================================

TEST(VelocityArbiter, SourceHandoverInsertsExactlyOneZeroCycle)
{
  VelocityArbiter arbiter = makeArbiter();

  const auto controlling = arbiter.arbitrate(VelocitySource::kController, twist(0.4, 0.0), kDt);
  ASSERT_NEAR(controlling.linear.x, 0.4, 1e-9);

  const auto handover = arbiter.arbitrate(VelocitySource::kRecovery, twist(-0.15, 0.0), kDt);
  EXPECT_DOUBLE_EQ(handover.linear.x, 0.0) << "the handover cycle must be 0";
  EXPECT_EQ(arbiter.handoverCycles(), 1u);

  const auto recovering = arbiter.arbitrate(VelocitySource::kRecovery, twist(-0.15, 0.0), kDt);
  EXPECT_NEAR(recovering.linear.x, -0.15, 1e-9) << "exactly ONE zero cycle, no more";
}

TEST(VelocityArbiter, HandoverWorksInBothDirections)
{
  VelocityArbiter arbiter = makeArbiter();

  arbiter.arbitrate(VelocitySource::kRecovery, twist(-0.1, 0.0), kDt);
  arbiter.arbitrate(VelocitySource::kRecovery, twist(-0.1, 0.0), kDt);
  ASSERT_NEAR(arbiter.lastCommand().linear.x, -0.1, 1e-9);

  EXPECT_DOUBLE_EQ(arbiter.arbitrate(VelocitySource::kController, twist(0.3, 0.0), kDt).linear.x,
                   0.0);
  EXPECT_NEAR(arbiter.arbitrate(VelocitySource::kController, twist(0.3, 0.0), kDt).linear.x, 0.3,
              1e-9);
  EXPECT_EQ(arbiter.handoverCycles(), 1u);
}

TEST(VelocityArbiter, GoingThroughNoneDoesNotCountAsHandover)
{
  // kController -> kNone -> kController: cycle kNone đã ép về 0 rồi, không cần chèn thêm.
  VelocityArbiter arbiter = makeArbiter();

  arbiter.arbitrate(VelocitySource::kController, twist(0.4, 0.0), kDt);
  arbiter.arbitrate(VelocitySource::kNone, twist(0.0, 0.0), kDt);

  const auto resumed = arbiter.arbitrate(VelocitySource::kController, twist(0.4, 0.0), kDt);
  EXPECT_NEAR(resumed.linear.x, 0.4, 1e-9);
  EXPECT_EQ(arbiter.handoverCycles(), 0u);
}

TEST(VelocityArbiter, FirstCommandAfterConfigureNeedsNoHandover)
{
  VelocityArbiter arbiter = makeArbiter();

  const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(0.3, 0.0), kDt);
  EXPECT_NEAR(cmd.linear.x, 0.3, 1e-9);
  EXPECT_EQ(arbiter.handoverCycles(), 0u);
}

// ================================================================================================
// Dừng khẩn và reset
// ================================================================================================

TEST(VelocityArbiter, EmergencyStopIgnoresAccelerationLimit)
{
  VelocityLimits limits = baseLimits();
  limits.max_accel_x = 0.01;  // giảm tốc bình thường sẽ mất rất nhiều cycle
  VelocityArbiter arbiter = makeArbiter(limits);

  for (int i = 0; i < 100; ++i)
  {
    arbiter.arbitrate(VelocitySource::kController, twist(0.5, 0.0), kDt);
  }
  ASSERT_GT(arbiter.lastCommand().linear.x, 0.0);

  const auto cmd = arbiter.emergencyStop();
  EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0);
  EXPECT_TRUE(arbiter.stopped());
  EXPECT_EQ(arbiter.activeSource(), VelocitySource::kNone);
}

TEST(VelocityArbiter, ResetClearsCountersAndHistory)
{
  VelocityArbiter arbiter = makeArbiter();

  arbiter.arbitrate(VelocitySource::kController,
                    twist(std::numeric_limits<double>::quiet_NaN(), 0.0), kDt);
  arbiter.arbitrate(VelocitySource::kController, twist(9.0, 0.0), kDt);
  arbiter.arbitrate(VelocitySource::kRecovery, twist(0.1, 0.0), kDt);
  ASSERT_GT(arbiter.nonFiniteRejections(), 0u);
  ASSERT_GT(arbiter.velocityClamps(), 0u);
  ASSERT_GT(arbiter.handoverCycles(), 0u);

  arbiter.reset();

  EXPECT_EQ(arbiter.nonFiniteRejections(), 0u);
  EXPECT_EQ(arbiter.velocityClamps(), 0u);
  EXPECT_EQ(arbiter.accelerationClamps(), 0u);
  EXPECT_EQ(arbiter.handoverCycles(), 0u);
  EXPECT_EQ(arbiter.activeSource(), VelocitySource::kNone);
  EXPECT_TRUE(arbiter.stopped());
}

TEST(VelocityArbiter, StoppedUsesEpsilonNotExactZero)
{
  VelocityLimits limits = baseLimits();
  limits.zero_velocity_epsilon = 0.01;
  VelocityArbiter arbiter = makeArbiter(limits);

  arbiter.arbitrate(VelocitySource::kController, twist(0.005, 0.0), kDt);
  EXPECT_TRUE(arbiter.stopped());

  arbiter.arbitrate(VelocitySource::kController, twist(0.05, 0.0), kDt);
  EXPECT_FALSE(arbiter.stopped());
}

int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}