/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * move_base2 — test facade `NavigationServer`: đường lệnh vận tốc ra host, và đường dữ liệu cảm
 * biến từ host vào costmap.
 *
 * Author: DuongTD
 *********************************************************************/
#include <gtest/gtest.h>

#include <cmath>
#include <cstddef>
#include <limits>
#include <string>
#include <vector>

#include <robot_costmap_2d/layered_costmap.h>

#include <move_base2/navigation_server.h>

#include "fake_ports.h"
#include "spy_layer.h"

using move_base2::ControlLoopConfig;
using move_base2::ControlLoopDeps;
using move_base2::MotionProfile;
using move_base2::NavigationRequest;
using move_base2::NavigationServer;
using move_base2::NavigationState;
using move_base2::SensorGatewayConfig;
using move_base2::testing::attachSpy;
using move_base2::testing::ControllerScript;
using move_base2::testing::FakeActionPort;
using move_base2::testing::FakeClockPort;
using move_base2::testing::FakeControllerPort;
using move_base2::testing::FakeMissionPort;
using move_base2::testing::FakePlannerPort;
using move_base2::testing::FakePosePort;
using move_base2::testing::FakeRecoveryPort;
using move_base2::testing::PlannerScript;
using move_base2::testing::SpyPtr;
using robot_costmap_2d::LayerType;

namespace
{

constexpr double kControlPeriod = 0.05;  ///< [s]
constexpr double kClockStart = 1000.0;   ///< [s]

ControlLoopConfig baseConfig()
{
  ControlLoopConfig config;

  config.state_machine.planner_patience = 0.5;      // [s]
  config.state_machine.controller_patience = 0.5;   // [s]
  config.state_machine.oscillation_timeout = 0.0;   // tắt
  config.state_machine.oscillation_distance = 0.5;  // [m]
  config.state_machine.max_planning_retries = -1;
  config.state_machine.recovery_behavior_count = 2;
  config.state_machine.recovery_enabled = true;

  config.velocity.max_vel_x = 0.5;          // [m/s]
  config.velocity.min_vel_x = -0.2;         // [m/s]
  config.velocity.max_vel_theta = 1.0;      // [rad/s]
  config.velocity.max_accel_x = 100.0;      // [m/s^2] lớn để test không vướng ramp
  config.velocity.max_accel_theta = 100.0;  // [rad/s^2]

  config.nominal_control_period = kControlPeriod;
  config.robot_base_frame = "base_link";

  config.position.global_planner_name = "FakeGlobalPlanner";
  config.position.local_planner_name = "FakeLocalPlanner";
  config.position.default_xy_tolerance = 0.15;   // [m]
  config.position.default_yaw_tolerance = 0.10;  // [rad]

  config.docking = config.position;
  config.go_straight = config.position;
  config.rotate = config.position;

  return config;
}

NavigationRequest makeRequest(double goal_x)
{
  NavigationRequest request;
  request.profile = MotionProfile::kPosition;
  request.goal.header.frame_id = "map";
  request.goal.pose.position.x = goal_x;
  request.goal.pose.orientation.w = 1.0;
  return request;
}

robot_geometry_msgs::Vector3 makeVector(double x, double y = 0.0, double z = 0.0)
{
  robot_geometry_msgs::Vector3 v;
  v.x = x;
  v.y = y;
  v.z = z;
  return v;
}

robot_nav_msgs::Odometry makeOdometry(double vx, double wz)
{
  robot_nav_msgs::Odometry odom;
  odom.header.frame_id = "odom";
  odom.twist.twist.linear.x = vx;   // [m/s]
  odom.twist.twist.angular.z = wz;  // [rad/s]
  return odom;
}

robot_sensor_msgs::LaserScan makeScan(std::size_t rays = 40, float range = 1.0F)
{
  robot_sensor_msgs::LaserScan scan;
  scan.header.frame_id = "laser";
  scan.angle_min = -1.5F;                                  // [rad]
  scan.angle_max = 1.5F;                                   // [rad]
  scan.angle_increment = 3.0F / static_cast<float>(rays);  // [rad]
  scan.range_min = 0.05F;                                  // [m]
  scan.range_max = 10.0F;                                  // [m]
  scan.ranges.assign(rays, range);
  return scan;
}

/**
 * @class Fixture
 * @brief `NavigationServer` nối đủ cổng giả, cộng hai costmap thật để kiểm đường cảm biến.
 */
class Fixture
{
public:
  Fixture()
    : clock_(kClockStart)
    , recovery_(2)
    , global_("map", false, true)
    , local_("odom", true, false)
  {
    pose_.setPosition(0.0, 0.0);

    deps_.clock = &clock_;
    deps_.pose = &pose_;
    deps_.planner = &planner_;
    deps_.controller = &controller_;
    deps_.recovery = &recovery_;
    deps_.mission = &mission_;
    deps_.action = &action_;
  }

  void configure(const ControlLoopConfig& config = baseConfig())
  {
    std::string error;
    ASSERT_TRUE(server_.configureLoop(config, deps_, error)) << error;
  }

  void configureSensors(const SensorGatewayConfig& config)
  {
    std::string error;
    ASSERT_TRUE(server_.configureSensors(config, error)) << error;
  }

  /// @brief Chạy @p cycles control cycle, mỗi cycle nhích đồng hồ giả một chu kỳ.
  void spin(std::size_t cycles)
  {
    for (std::size_t i = 0; i < cycles; ++i)
    {
      server_.spinOnce();
      clock_.advance(kControlPeriod);
    }
  }

  void attachCostmaps()
  {
    server_.attachCostmaps(&global_, &local_);
  }

  NavigationServer server_;
  FakeClockPort clock_;
  FakePosePort pose_;
  FakePlannerPort planner_;
  FakeControllerPort controller_;
  FakeRecoveryPort recovery_;
  FakeMissionPort mission_;
  FakeActionPort action_;
  ControlLoopDeps deps_;

  robot_costmap_2d::LayeredCostmap global_;
  robot_costmap_2d::LayeredCostmap local_;
};

}  // namespace

// ================================================================================================
// getTwist() — LỆNH vận tốc, không phải vận tốc đo được
//
// Host lấy getTwist() rồi publish thẳng ra /cmd_vel. Nếu giá trị đó đến từ odometry thì có một vòng
// lặp dương: robot chạy 0.5 m/s -> đọc odom 0.5 -> phát lệnh 0.5 -> mãi mãi. VelocityArbiter — toàn
// bộ hàng rào an toàn của gói — cũng bị bỏ qua hoàn toàn. Các test dưới đây khoá lại điều đó.
// ================================================================================================

TEST(NavigationServerTwist, ReturnsArbiterCommandNotOdometryVelocity)
{
  Fixture fixture;
  fixture.configure();
  fixture.controller_.setNominalSpeed(0.3);  // [m/s]
  fixture.controller_.setScript({ ControllerScript::kOk, ControllerScript::kOk });

  // Odometry báo robot đang chạy nhanh hơn hẳn lệnh mà controller muốn phát.
  fixture.server_.addOdometry("/odom", makeOdometry(1.7, 0.9));

  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
      << fixture.server_.lastRejectReason();

  fixture.spin(2);  // IDLE -> PLANNING -> CONTROLLING (controller chạy ngay ở cycle này)
  ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kControlling);

  const robot_nav_2d_msgs::Twist2DStamped twist = fixture.server_.getTwist();
  EXPECT_NEAR(twist.velocity.x, 0.3, 1e-9) << "getTwist trả vận tốc đo được thay vì lệnh đã phát";
  EXPECT_NEAR(twist.velocity.theta, 0.0, 1e-9);
}

TEST(NavigationServerTwist, OdometryAloneNeverProducesACommand)
{
  Fixture fixture;
  fixture.configure();

  fixture.server_.addOdometry("/odom", makeOdometry(1.7, 0.9));
  fixture.spin(1);  // IDLE, không có yêu cầu nào

  const robot_nav_2d_msgs::Twist2DStamped twist = fixture.server_.getTwist();
  EXPECT_NEAR(twist.velocity.x, 0.0, 1e-9);
  EXPECT_NEAR(twist.velocity.y, 0.0, 1e-9);
  EXPECT_NEAR(twist.velocity.theta, 0.0, 1e-9);
}

TEST(NavigationServerTwist, StampComesFromTheControlLoopClockNotWallClock)
{
  // Host loại lệnh quá hạn theo dấu này. Lấy giờ hệ thống lúc host hỏi sẽ làm một control loop đã
  // treo vẫn trông như đang phát lệnh tươi — đúng thứ dấu thời gian sinh ra để ngăn.
  Fixture fixture;
  fixture.configure();

  fixture.spin(1);
  EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), kClockStart, 1e-9);

  fixture.clock_.setTime(kClockStart + 12.0);
  fixture.spin(1);
  EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), kClockStart + 12.0, 1e-9);
}

TEST(NavigationServerTwist, StampStaysStillWhenTheControlLoopStopsRunning)
{
  Fixture fixture;
  fixture.configure();

  fixture.spin(1);
  const double stamp_after_first = fixture.server_.getTwist().header.stamp.toSec();

  // Đồng hồ chạy tiếp nhưng KHÔNG có cycle nào — mô phỏng control thread treo.
  fixture.clock_.setTime(kClockStart + 30.0);
  fixture.server_.addOdometry("/odom", makeOdometry(1.7, 0.0));

  EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), stamp_after_first, 1e-9)
      << "dấu thời gian tự tươi lại dù control loop không chạy — host sẽ tưởng lệnh còn hiệu lực";
}

TEST(NavigationServerTwist, IsStampedWithTheConfiguredRobotBaseFrame)
{
  ControlLoopConfig config = baseConfig();
  config.robot_base_frame = "base_footprint";

  Fixture fixture;
  fixture.configure(config);
  fixture.spin(1);

  EXPECT_EQ(fixture.server_.getTwist().header.frame_id, "base_footprint");
}

TEST(NavigationServerTwist, ConfigureIsRefusedWhenRobotBaseFrameIsEmpty)
{
  ControlLoopConfig config = baseConfig();
  config.robot_base_frame.clear();

  Fixture fixture;
  std::string error;
  EXPECT_FALSE(fixture.server_.configureLoop(config, fixture.deps_, error));
  EXPECT_FALSE(error.empty());
}

// ================================================================================================
// Đường dữ liệu cảm biến từ host vào costmap
// ================================================================================================

TEST(NavigationServerSensors, SamplesReachTheCostmapLayersOnceAttached)
{
  Fixture fixture;
  fixture.configure();

  SpyPtr static_layer = attachSpy(fixture.global_, LayerType::STATIC_LAYER, "navigation_map");
  SpyPtr local_voxel = attachSpy(fixture.local_, LayerType::VOXEL_LAYER, "obstacles");
  fixture.attachCostmaps();

  fixture.server_.addStaticMap("/map", robot_nav_msgs::OccupancyGrid());
  fixture.server_.addLaserScan("/b_scan", makeScan());
  fixture.server_.addPointCloud2("/camera/depth/points_proc", robot_sensor_msgs::PointCloud2());

  EXPECT_EQ(static_layer->count(), 1U);
  EXPECT_EQ(local_voxel->count(), 2U) << "laser + pointcloud2 phải cùng tới VoxelLayer";
  EXPECT_EQ(local_voxel->records()[0].topic, "/b_scan");
  EXPECT_EQ(local_voxel->records()[1].topic, "/camera/depth/points_proc");
}

TEST(NavigationServerSensors, StoringStillWorksWhenNoCostmapIsAttachedYet)
{
  // Trạng thái bình thường lúc khởi động: host đã bắt đầu bơm dữ liệu trước khi costmap được dựng.
  // Dữ liệu vẫn phải đọc lại được qua getter của contract host, và số mẫu mất phải đếm được.
  Fixture fixture;
  fixture.configure();

  fixture.server_.addLaserScan("/b_scan", makeScan());

  EXPECT_EQ(fixture.server_.getLaserScan("/b_scan").ranges.size(), 40U);
  EXPECT_EQ(fixture.server_.sensors().stats().dropped_no_costmap, 1U);
  EXPECT_EQ(fixture.server_.sensors().stats().delivered, 0U);
}

TEST(NavigationServerSensors, StaticMapReceivedBeforeAttachIsReplayed)
{
  // Không có phần phát lại này thì thứ tự "map tới trước, costmap dựng sau" — thứ tự thường gặp
  // nhất khi khởi động — để global costmap trắng vĩnh viễn: /map là topic latched, host không gửi
  // lại. Bản cũ bù bằng cặp biến public map_save_/map_name_save_.
  Fixture fixture;
  fixture.configure();

  fixture.server_.addStaticMap("/map", robot_nav_msgs::OccupancyGrid());
  ASSERT_EQ(fixture.server_.sensors().stats().dropped_no_costmap, 1U);

  SpyPtr static_layer = attachSpy(fixture.global_, LayerType::STATIC_LAYER, "navigation_map");
  fixture.attachCostmaps();

  ASSERT_EQ(static_layer->count(), 1U) << "static map nhận trước khi gắn costmap không được phát lại";
  EXPECT_EQ(static_layer->records()[0].topic, "/map");
}

TEST(NavigationServerSensors, LegacyMapSavePublicMemberIsAlsoReplayed)
{
  // `map_save_`/`map_name_save_` là biến PUBLIC của BaseNavigation mà host tự gán
  // (sensor_converter.cpp). Giữ đường này để host không phải sửa gì khi đổi sang move_base2.
  Fixture fixture;
  fixture.configure();

  fixture.server_.map_name_save_ = "/map";
  fixture.server_.map_save_ = robot_nav_msgs::OccupancyGrid();

  SpyPtr static_layer = attachSpy(fixture.global_, LayerType::STATIC_LAYER, "navigation_map");
  fixture.attachCostmaps();

  EXPECT_EQ(static_layer->count(), 1U);
  EXPECT_EQ(static_layer->records()[0].topic, "/map");
}

TEST(NavigationServerSensors, ReplayDoesNotDuplicateAMapAlreadyReceivedThroughTheApi)
{
  Fixture fixture;
  fixture.configure();

  fixture.server_.addStaticMap("/map", robot_nav_msgs::OccupancyGrid());
  fixture.server_.map_name_save_ = "/map";  // host gán cả hai đường, như bản cũ đang làm
  fixture.server_.map_save_ = robot_nav_msgs::OccupancyGrid();

  SpyPtr static_layer = attachSpy(fixture.global_, LayerType::STATIC_LAYER, "navigation_map");
  fixture.attachCostmaps();

  EXPECT_EQ(static_layer->count(), 1U) << "cùng một map bị phát lại hai lần";
}

TEST(NavigationServerSensors, StaleLaserScansAreNotReplayedOnAttach)
{
  // Cố ý: phát lại một scan cũ là dựng vật cản ở chỗ robot có thể đã rời khỏi từ lâu. Mẫu kế tiếp
  // chỉ cách vài chục ms — chờ nó an toàn hơn hẳn.
  Fixture fixture;
  fixture.configure();

  fixture.server_.addLaserScan("/b_scan", makeScan());

  SpyPtr voxel = attachSpy(fixture.local_, LayerType::VOXEL_LAYER, "obstacles");
  fixture.attachCostmaps();

  EXPECT_EQ(voxel->count(), 0U);
}

TEST(NavigationServerSensors, StoredLaserScanIsTheSameOneHandedToTheCostmap)
{
  // Bản cũ cất bản ĐÃ LỌC. Nếu getter trả bản thô còn costmap thấy bản lọc thì hai nguồn sự thật
  // lệch nhau, và mọi chẩn đoán dựa trên getter sẽ nói dối về thứ costmap thật sự dùng.
  SensorGatewayConfig sensors;
  sensors.laser_sor_enabled = true;
  sensors.laser_sor_mean_k = 5;
  sensors.laser_sor_stddev_mul = 1.0;

  Fixture fixture;
  fixture.configure();
  fixture.configureSensors(sensors);

  std::vector<float> seen_by_layer;
  SpyPtr voxel = attachSpy(fixture.local_, LayerType::VOXEL_LAYER, "obstacles");
  voxel->setObserver([&seen_by_layer](const void* data, const std::type_info& type,
                                      const std::string&) {
    if (type == typeid(robot_sensor_msgs::LaserScan))
    {
      seen_by_layer = static_cast<const robot_sensor_msgs::LaserScan*>(data)->ranges;
    }
  });
  fixture.attachCostmaps();

  fixture.server_.addLaserScan("/b_scan", makeScan());

  const std::vector<float> stored = fixture.server_.getLaserScan("/b_scan").ranges;
  ASSERT_FALSE(seen_by_layer.empty());
  ASSERT_EQ(stored.size(), seen_by_layer.size());

  // So từng phần tử chứ không so cả vector: bộ lọc biến outlier thành NaN để giữ nguyên cấu trúc
  // scan, mà NaN != NaN nên operator== của vector sẽ báo khác nhau dù nội dung giống hệt.
  for (std::size_t i = 0; i < stored.size(); ++i)
  {
    if (std::isnan(stored[i]))
    {
      EXPECT_TRUE(std::isnan(seen_by_layer[i])) << "lệch tại tia " << i;
    }
    else
    {
      EXPECT_FLOAT_EQ(stored[i], seen_by_layer[i]) << "lệch tại tia " << i;
    }
  }
}

TEST(NavigationServerSensors, DepthCameraDataIsStoredAndForwardedAsConstPtr)
{
  Fixture fixture;
  fixture.configure();

  SpyPtr voxel = attachSpy(fixture.local_, LayerType::VOXEL_LAYER, "obstacles");
  fixture.attachCostmaps();

  robot_sensor_msgs::DepthCameraData::Ptr data =
      boost::make_shared<robot_sensor_msgs::DepthCameraData>();
  data->header.frame_id = "camera_optical";
  fixture.server_.addDepthCameraData("/camera/depth/data", data);

  ASSERT_EQ(voxel->count(), 1U);
  EXPECT_TRUE(*voxel->records()[0].type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr));
  EXPECT_EQ(voxel->records()[0].topic, "/camera/depth/data");
}

TEST(NavigationServerSensors, NullDepthPointerIsRejectedAtTheDoor)
{
  Fixture fixture;
  fixture.configure();

  SpyPtr voxel = attachSpy(fixture.local_, LayerType::VOXEL_LAYER, "obstacles");
  fixture.attachCostmaps();

  fixture.server_.addDepthCameraData("/camera/depth/data",
                                     robot_sensor_msgs::DepthCameraData::ConstPtr());

  EXPECT_EQ(voxel->count(), 0U);
  EXPECT_EQ(fixture.server_.sensors().stats().dropped_no_costmap, 0U);
}

// ================================================================================================
// Trần vận tốc (bước 12) — đường tầng an toàn hạ tốc độ robot
//
// `setTwistLinear` không phải lệnh jog dù tên nghe như vậy: host gọi nó theo cặp +v/-v để đặt trần
// cho hai chiều, và giá trị truyền xuống mang theo tốc độ đã bị tầng an toàn hạ
// (amr_control.cpp:561, 671-680). Trước đây `NavigationServer` trả false và không làm gì.
// ================================================================================================

TEST(NavigationServerLimits, ForwardAndBackwardLimitsReachTheController)
{
  Fixture fixture;
  fixture.configure();

  EXPECT_TRUE(fixture.server_.setTwistLinear(makeVector(0.30)));   // [m/s] trần tiến
  EXPECT_TRUE(fixture.server_.setTwistLinear(makeVector(-0.15)));  // [m/s] trần lùi, ÂM

  fixture.spin(1);

  EXPECT_NEAR(fixture.controller_.limitForward(), 0.30, 1e-9);
  EXPECT_NEAR(fixture.controller_.limitBackward(), -0.15, 1e-9);
}

TEST(NavigationServerLimits, AngularLimitReachesTheController)
{
  Fixture fixture;
  fixture.configure();

  EXPECT_TRUE(fixture.server_.setTwistAngular(makeVector(0.0, 0.0, 0.45)));  // [rad/s]
  fixture.spin(1);

  EXPECT_NEAR(fixture.controller_.limitAngular(), 0.45, 1e-9);
}

TEST(NavigationServerLimits, LimitTakesEffectInTheSameCycleItIsPushed)
{
  // Chậm một cycle nghĩa là một chu kỳ nữa robot chạy quá tốc độ mà tầng an toàn vừa yêu cầu hạ.
  Fixture fixture;
  fixture.configure();

  ASSERT_TRUE(fixture.server_.setTwistLinear(makeVector(0.12)));
  fixture.spin(1);
  EXPECT_NEAR(fixture.controller_.limitForward(), 0.12, 1e-9);
}

TEST(NavigationServerLimits, NonFiniteLimitIsRejectedAtTheDoor)
{
  Fixture fixture;
  fixture.configure();

  const double nan = std::numeric_limits<double>::quiet_NaN();
  EXPECT_FALSE(fixture.server_.setTwistLinear(makeVector(nan)));
  EXPECT_FALSE(fixture.server_.setTwistAngular(makeVector(0.0, 0.0, nan)));

  fixture.spin(1);
  EXPECT_NEAR(fixture.controller_.limitForward(), 0.0, 1e-9);
}

TEST(NavigationServerLimits, LatestLimitWinsWhenSetSeveralTimesWithinOneCycle)
{
  // Host gọi từ thread của nó với nhịp riêng; nhiều lời gọi giữa hai cycle là bình thường. Thứ phải
  // có hiệu lực là giá trị MỚI NHẤT, không phải giá trị đầu tiên.
  Fixture fixture;
  fixture.configure();

  ASSERT_TRUE(fixture.server_.setTwistLinear(makeVector(0.40)));
  ASSERT_TRUE(fixture.server_.setTwistLinear(makeVector(0.10)));  // tầng an toàn vừa hạ tiếp
  fixture.spin(1);

  EXPECT_NEAR(fixture.controller_.limitForward(), 0.10, 1e-9);
}

TEST(NavigationServerLimits, OdometryReachesTheControllerAsMeasuredVelocity)
{
  // Bản cũ đưa vận tốc đo được vào controller bằng con trỏ tới bộ nhớ host ghi
  // (`tc_->setOdom(&odometry_)`) — data race không có gì bảo vệ. Ở đây truyền theo giá trị, qua
  // control thread.
  Fixture fixture;
  fixture.configure();

  fixture.server_.addOdometry("/odom", makeOdometry(0.42, -0.17));
  fixture.spin(1);

  EXPECT_NEAR(fixture.controller_.measuredVelocity().linear.x, 0.42, 1e-9);
  EXPECT_NEAR(fixture.controller_.measuredVelocity().angular.z, -0.17, 1e-9);
}

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