#include <gtest/gtest.h>
#include "vde/spatial/r_tree.h"
#include "vde/core/aabb.h"

using namespace vde::spatial;
using namespace vde::core;

// A simple wrapper to satisfy the RTree template
namespace {
    struct Point3DWithId {
        Point3D p;
        int id;
    };
} // namespace

// Instantiate the RTree for Point3D (items are point-based)
// RTree stores items and computes their AABBs
// For testing we use Point3D items directly

TEST(RTreeTest, BuildEmpty) {
    RTree<Point3D> tree;
    tree.build({});
    EXPECT_EQ(tree.size(), 0u);
}

TEST(RTreeTest, BuildSinglePoint) {
    RTree<Point3D> tree;
    tree.build({Point3D(1, 2, 3)});
    EXPECT_EQ(tree.size(), 1u);
}

TEST(RTreeTest, BuildAndRangeQuery) {
    RTree<Point3D> tree;
    std::vector<Point3D> points = {
        {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {1, 1, 0},
        {5, 5, 0}, {6, 5, 0}, {5, 6, 0}, {6, 6, 0},
    };
    tree.build(points);

    // Query: range covering the first cluster (0,0)-(2,2)
    AABB3D range(Point3D(-0.5, -0.5, -0.5), Point3D(2.5, 2.5, 0.5));
    auto results = tree.query_range(range);
    EXPECT_EQ(results.size(), 4u);  // 4 points in first cluster
}

TEST(RTreeTest, RangeQueryEmpty) {
    RTree<Point3D> tree;
    tree.build({Point3D(0, 0, 0), Point3D(1, 1, 1)});

    AABB3D far(Point3D(10, 10, 10), Point3D(20, 20, 20));
    auto results = tree.query_range(far);
    EXPECT_TRUE(results.empty());
}

TEST(RTreeTest, KNNQuery) {
    RTree<Point3D> tree;
    tree.build({
        {0, 0, 0}, {10, 0, 0}, {0, 10, 0}, {10, 10, 0},
    });

    auto nearest = tree.query_knn(Point3D(1, 0, 0), 1);
    ASSERT_EQ(nearest.size(), 1u);
    EXPECT_NEAR(nearest[0].x(), 0.0, 1e-6);
    EXPECT_NEAR(nearest[0].y(), 0.0, 1e-6);

    auto two_nearest = tree.query_knn(Point3D(0, 0, 0), 2);
    EXPECT_EQ(two_nearest.size(), 2u);
}

TEST(RTreeTest, KNNQuery_MoreThanStored) {
    RTree<Point3D> tree;
    tree.build({Point3D(1, 2, 3), Point3D(4, 5, 6)});

    auto results = tree.query_knn(Point3D(0, 0, 0), 10);
    EXPECT_EQ(results.size(), 2u);  // Only 2 items stored
}

TEST(RTreeTest, RayQuery) {
    RTree<Point3D> tree;
    std::vector<Point3D> points;
    for (int x = 0; x < 5; ++x)
        for (int y = 0; y < 5; ++y)
            points.push_back(Point3D(x, y, 0));
    tree.build(points);

    // Ray along X-axis at y=0, z=0 should hit points with y≈0
    Ray3Dd ray(Point3D(-1, 0, 0), Vector3D(1, 0, 0));
    auto hits = tree.query_ray(ray);
    EXPECT_GT(hits.size(), 0u);
}

TEST(RTreeTest, InsertAndRemove) {
    RTree<Point3D> tree;
    tree.insert(Point3D(0, 0, 0));
    EXPECT_EQ(tree.size(), 1u);

    tree.insert(Point3D(1, 1, 1));
    EXPECT_EQ(tree.size(), 2u);

    bool removed = tree.remove(Point3D(1, 1, 1));
    EXPECT_TRUE(removed);
    EXPECT_EQ(tree.size(), 1u);
}

TEST(RTreeTest, RemoveNonExistent) {
    RTree<Point3D> tree;
    tree.insert(Point3D(0, 0, 0));
    bool removed = tree.remove(Point3D(9, 9, 9));
    EXPECT_FALSE(removed);
    EXPECT_EQ(tree.size(), 1u);
}

TEST(RTreeTest, Clear) {
    RTree<Point3D> tree;
    tree.build({
        {0, 0, 0}, {1, 0, 0}, {0, 1, 0},
    });
    EXPECT_EQ(tree.size(), 3u);

    tree.clear();
    EXPECT_EQ(tree.size(), 0u);

    // After clear, queries should return empty
    AABB3D all(Point3D(-1, -1, -1), Point3D(10, 10, 10));
    EXPECT_TRUE(tree.query_range(all).empty());
}