#include <gtest/gtest.h>

#include <cmath>
#include <memory>
#include <vector>

#include "obstacle_nav2/obstacle_array_layer.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "obstacle_scanner/msg/obstacle.hpp"
#include "obstacle_scanner/msg/obstacle_array.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/buffer.h"

using obstacle_nav2::ObstacleArrayLayer;

static constexpr double kResolution = 0.05;
static constexpr double kOriginX = -1.0;
static constexpr double kOriginY = -1.0;

// ============================================================================
// rasterizeCircle tests
// ============================================================================

TEST(ObstacleArrayLayerTest, RasterizeCircleMarksLethalCells)
{
  nav2_costmap_2d::Costmap2D grid(40, 40, kResolution, kOriginX, kOriginY);

  ObstacleArrayLayer::rasterizeCircle(
    grid, 0.0, 0.0, 0.1,
    kResolution, kOriginX, kOriginY);

  // center cell should be lethal
  unsigned int mx, my;
  grid.worldToMap(0.0, 0.0, mx, my);
  EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::LETHAL_OBSTACLE);

  // cell outside radius should be free
  grid.worldToMap(0.3, 0.3, mx, my);
  EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::FREE_SPACE);
}

TEST(ObstacleArrayLayerTest, RasterizeCircleDisplacedCenter)
{
  nav2_costmap_2d::Costmap2D grid(40, 40, kResolution, kOriginX, kOriginY);

  ObstacleArrayLayer::rasterizeCircle(
    grid, -0.5, -0.5, 0.15,
    kResolution, kOriginX, kOriginY);

  unsigned int mx, my;
  grid.worldToMap(-0.5, -0.5, mx, my);
  EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::LETHAL_OBSTACLE);

  grid.worldToMap(0.5, 0.5, mx, my);
  EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::FREE_SPACE);
}

TEST(ObstacleArrayLayerTest, RasterizeCircleAtCostmapEdge)
{
  nav2_costmap_2d::Costmap2D grid(40, 40, kResolution, kOriginX, kOriginY);

  // should not crash or access out of bounds
  ObstacleArrayLayer::rasterizeCircle(
    grid, 0.9, 0.9, 0.2,
    kResolution, kOriginX, kOriginY);

  unsigned int mx, my;
  grid.worldToMap(0.85, 0.85, mx, my);
  ASSERT_LT(mx, 40u);
  ASSERT_LT(my, 40u);
  EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::LETHAL_OBSTACLE);
}

TEST(ObstacleArrayLayerTest, RasterizeCircleEntirelyOutsideCostmap)
{
  nav2_costmap_2d::Costmap2D grid(40, 40, kResolution, kOriginX, kOriginY);

  EXPECT_NO_THROW(
    ObstacleArrayLayer::rasterizeCircle(
      grid, 5.0, 5.0, 0.1,
      kResolution, kOriginX, kOriginY));
}

TEST(ObstacleArrayLayerTest, RasterizeCircleZeroRadius)
{
  nav2_costmap_2d::Costmap2D grid(40, 40, kResolution, kOriginX, kOriginY);

  ObstacleArrayLayer::rasterizeCircle(
    grid, 0.0, 0.0, 0.0,
    kResolution, kOriginX, kOriginY);

  unsigned int mx, my;
  grid.worldToMap(0.0, 0.0, mx, my);
  EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::LETHAL_OBSTACLE);
}

TEST(ObstacleArrayLayerTest, RasterizeCircleAllCellsInRadiusAreLethal)
{
  nav2_costmap_2d::Costmap2D grid(40, 40, kResolution, kOriginX, kOriginY);

  ObstacleArrayLayer::rasterizeCircle(
    grid, 0.0, 0.0, 0.1,
    kResolution, kOriginX, kOriginY);

  // spot-check cells within radius are lethal
  unsigned int mx, my;
  grid.worldToMap(0.05, 0.0, mx, my);
  EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::LETHAL_OBSTACLE);

  grid.worldToMap(0.0, 0.05, mx, my);
  EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::LETHAL_OBSTACLE);
}

// ============================================================================
// clampRadius tests
// ============================================================================

TEST(ObstacleArrayLayerTest, ClampRadiusPassesThroughValid)
{
  EXPECT_DOUBLE_EQ(ObstacleArrayLayer::clampRadius(0.1, 0.02, 0.5, 0.05), 0.1);
  EXPECT_DOUBLE_EQ(ObstacleArrayLayer::clampRadius(0.02, 0.02, 0.5, 0.05), 0.02);
  EXPECT_DOUBLE_EQ(ObstacleArrayLayer::clampRadius(0.5, 0.02, 0.5, 0.05), 0.5);
}

TEST(ObstacleArrayLayerTest, ClampRadiusFallsBackForNonPositive)
{
  constexpr double kDefault = 0.05;
  EXPECT_DOUBLE_EQ(ObstacleArrayLayer::clampRadius(0.0, 0.02, 0.5, kDefault), kDefault);
  EXPECT_DOUBLE_EQ(ObstacleArrayLayer::clampRadius(-0.1, 0.02, 0.5, kDefault), kDefault);
}

TEST(ObstacleArrayLayerTest, ClampRadiusFallsBackForNonFinite)
{
  constexpr double kDefault = 0.05;
  EXPECT_DOUBLE_EQ(ObstacleArrayLayer::clampRadius(NAN, 0.02, 0.5, kDefault), kDefault);
  EXPECT_DOUBLE_EQ(
    ObstacleArrayLayer::clampRadius(INFINITY, 0.02, 0.5, kDefault), kDefault);
  EXPECT_DOUBLE_EQ(
    ObstacleArrayLayer::clampRadius(-INFINITY, 0.02, 0.5, kDefault), kDefault);
}

TEST(ObstacleArrayLayerTest, ClampRadiusClampsBelowMinimum)
{
  EXPECT_DOUBLE_EQ(ObstacleArrayLayer::clampRadius(0.01, 0.02, 0.5, 0.05), 0.02);
}

TEST(ObstacleArrayLayerTest, ClampRadiusClampsAboveMaximum)
{
  EXPECT_DOUBLE_EQ(ObstacleArrayLayer::clampRadius(0.6, 0.02, 0.5, 0.05), 0.5);
}

TEST(ObstacleArrayLayerTest, InitializesEnabledStateFromDefaultParameter)
{
  auto node = std::make_shared<nav2_util::LifecycleNode>("obstacle_layer_test");
  nav2_costmap_2d::LayeredCostmap layered_costmap("odom", true, false);
  layered_costmap.resizeMap(40, 40, kResolution, kOriginX, kOriginY);
  tf2_ros::Buffer tf_buffer(node->get_clock());
  ObstacleArrayLayer layer;

  layer.initialize(
    &layered_costmap, "obstacle_layer", &tf_buffer, node,
    rclcpp::CallbackGroup::SharedPtr());

  EXPECT_TRUE(layer.isEnabled());
}

TEST(ObstacleArrayLayerTest, EmptySnapshotClearsPreviousObstacle)
{
  nav2_costmap_2d::Costmap2D grid(40, 40, kResolution, kOriginX, kOriginY);
  const std::vector<ObstacleArrayLayer::CircleObstacle> occupied{{0.0, 0.0, 0.1}};

  const auto occupied_bounds = ObstacleArrayLayer::applySnapshot(
    grid, occupied, kResolution, kOriginX, kOriginY);

  unsigned int mx, my;
  ASSERT_TRUE(grid.worldToMap(0.0, 0.0, mx, my));
  EXPECT_TRUE(occupied_bounds.valid);
  EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::LETHAL_OBSTACLE);

  const auto cleared_bounds = ObstacleArrayLayer::applySnapshot(
    grid, {}, kResolution, kOriginX, kOriginY);

  EXPECT_FALSE(cleared_bounds.valid);
  EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::FREE_SPACE);
}

TEST(ObstacleArrayLayerTest, TransformSnapshotUsesMessageTimestamp)
{
  auto clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
  tf2_ros::Buffer buffer(clock);
  buffer.setUsingDedicatedThread(true);

  geometry_msgs::msg::TransformStamped old_transform;
  old_transform.header.frame_id = "odom";
  old_transform.child_frame_id = "laser_link";
  old_transform.header.stamp = rclcpp::Time(9, 800000000, RCL_SYSTEM_TIME);
  old_transform.transform.translation.x = 100.0;
  old_transform.transform.rotation.w = 1.0;
  ASSERT_TRUE(buffer.setTransform(old_transform, "test", false));

  geometry_msgs::msg::TransformStamped exact_transform = old_transform;
  exact_transform.header.stamp = rclcpp::Time(10, 0, RCL_SYSTEM_TIME);
  exact_transform.transform.translation.x = 2.0;
  ASSERT_TRUE(buffer.setTransform(exact_transform, "test", false));

  obstacle_scanner::msg::ObstacleArray snapshot;
  snapshot.header.frame_id = "laser_link";
  snapshot.header.stamp = exact_transform.header.stamp;
  obstacle_scanner::msg::Obstacle obstacle;
  obstacle.center_x = 1.0;
  obstacle.center_y = 0.5;
  obstacle.radius = 0.04;
  snapshot.obstacles.push_back(obstacle);

  const auto transformed = ObstacleArrayLayer::transformSnapshot(
    snapshot, "odom", buffer, rclcpp::Duration::from_seconds(0.0));

  ASSERT_EQ(transformed.size(), 1u);
  EXPECT_NEAR(transformed[0].center_x, 3.0, 1e-9);
  EXPECT_NEAR(transformed[0].center_y, 0.5, 1e-9);
  EXPECT_NEAR(transformed[0].radius, 0.04, 1e-9);
}

TEST(ObstacleArrayLayerTest, EmptySnapshotDoesNotRequireTransform)
{
  auto clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
  tf2_ros::Buffer buffer(clock);
  obstacle_scanner::msg::ObstacleArray snapshot;
  snapshot.header.frame_id = "missing_frame";
  snapshot.header.stamp = rclcpp::Time(10, 0, RCL_SYSTEM_TIME);

  const auto transformed = ObstacleArrayLayer::transformSnapshot(
    snapshot, "odom", buffer, rclcpp::Duration::from_seconds(0.0));

  EXPECT_TRUE(transformed.empty());
}

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  testing::InitGoogleTest(&argc, argv);
  const int result = RUN_ALL_TESTS();
  rclcpp::shutdown();
  return result;
}
