#include <gtest/gtest.h>
#include "vde/mesh/reverse_engineering.h"
#include "vde/mesh/point_cloud.h"
#include "vde/mesh/halfedge_mesh.h"
#include "vde/curves/nurbs_surface.h"

using namespace vde::mesh;
using namespace vde::curves;

// ═══════════════════════════════════════════════════════════
// Helper: generate a simple point cloud (e.g., a noisy plane)
// ═══════════════════════════════════════════════════════════

static PointCloud make_plane_cloud(int n = 100) {
    std::vector<Point3D> pts;
    for (int i = 0; i < n; ++i) {
        double u = static_cast<double>(i % 10) / 10.0;
        double v = static_cast<double>(i / 10) / 10.0;
        pts.emplace_back(u, v, 0.01 * std::sin(u * 10) * std::cos(v * 10));
    }
    return PointCloud(pts);
}

static PointCloud make_sphere_cloud(int n = 200) {
    std::vector<Point3D> pts;
    for (int i = 0; i < n; ++i) {
        double theta = 2.0 * M_PI * static_cast<double>(i) / n;
        double phi = M_PI * static_cast<double>((i * 7) % n) / n;
        double x = std::sin(phi) * std::cos(theta);
        double y = std::sin(phi) * std::sin(theta);
        double z = std::cos(phi);
        pts.emplace_back(x, y, z);
    }
    return PointCloud(pts);
}

// ═══════════════════════════════════════════════════════════
// PointCloud 基础测试
// ═══════════════════════════════════════════════════════════

TEST(PointCloudTest, Construction) {
    PointCloud empty;
    EXPECT_EQ(empty.size(), 0u);
    EXPECT_FALSE(empty.has_normals());
    EXPECT_FALSE(empty.has_colors());

    PointCloud cloud(std::vector<Point3D>{{0,0,0}, {1,0,0}, {0,1,0}});
    EXPECT_EQ(cloud.size(), 3u);
}

TEST(PointCloudTest, Bounds) {
    PointCloud cloud(std::vector<Point3D>{{0,0,0}, {2,0,0}, {0,3,0}, {2,3,0}});
    auto bb = cloud.bounds();
    EXPECT_NEAR(bb.min().x(), 0.0, 1e-9);
    EXPECT_NEAR(bb.min().y(), 0.0, 1e-9);
    EXPECT_NEAR(bb.max().x(), 2.0, 1e-9);
    EXPECT_NEAR(bb.max().y(), 3.0, 1e-9);
}

TEST(PointCloudTest, VoxelDownsample) {
    std::vector<Point3D> pts;
    for (int i = 0; i < 50; ++i) pts.emplace_back(i * 0.01, 0, 0);
    PointCloud cloud(pts);
    auto down = cloud.voxel_downsample(0.1);
    EXPECT_GT(down.size(), 0u);
    EXPECT_LT(down.size(), pts.size());
}

TEST(PointCloudTest, StatisticalOutlierRemoval) {
    std::vector<Point3D> pts;
    // Dense cluster
    for (int i = 0; i < 100; ++i)
        pts.emplace_back(0.01 * i, 0, 0);
    // Outlier far away
    pts.emplace_back(100, 100, 100);
    PointCloud cloud(pts);
    auto clean = cloud.statistical_outlier_removal(6, 1.0);
    EXPECT_LT(clean.size(), pts.size());
    // The outlier should be removed
    for (const auto& p : clean.vertices) {
        EXPECT_LT((p - Point3D(100, 100, 100)).norm(), 50.0);
    }
}

TEST(PointCloudTest, NormalEstimation) {
    auto cloud = make_plane_cloud(100);
    auto with_normals = cloud.normal_estimation(10);
    EXPECT_EQ(with_normals.size(), cloud.size());
    EXPECT_TRUE(with_normals.has_normals());
    // Plane normals should be mostly Z-aligned
    for (size_t i = 0; i < with_normals.size(); ++i) {
        double dot_z = std::abs(with_normals.normals[i].z());
        EXPECT_GT(dot_z, 0.7);  // Should be mostly pointing in Z
    }
}

TEST(PointCloudTest, SmoothingFilter) {
    std::vector<Point3D> pts;
    for (int i = 0; i < 50; ++i)
        pts.emplace_back(i * 0.02, 0, 0.1 * std::sin(i * 0.5));
    PointCloud cloud(pts);
    auto smoothed = cloud.smoothing_filter(0.5, 0.3, 3);
    EXPECT_EQ(smoothed.size(), cloud.size());
}

TEST(PointCloudTest, CurvatureAwareSampling) {
    auto cloud = make_sphere_cloud(200);
    auto indices = cloud.curvature_aware_sampling(50);
    EXPECT_LE(indices.size(), 50u);
    EXPECT_GT(indices.size(), 0u);
}

// ═══════════════════════════════════════════════════════════
// Plane fitting
// ═══════════════════════════════════════════════════════════

TEST(ReverseEngineeringTest, FitPlane_XY_Plane) {
    std::vector<Point3D> pts;
    for (int i = 0; i < 100; ++i)
        pts.emplace_back(static_cast<double>(i % 10), static_cast<double>(i / 10), 0.0);

    auto result = fit_plane(pts);
    EXPECT_TRUE(result.valid);
    EXPECT_NEAR(result.rms_error, 0.0, 1e-6);
    EXPECT_NEAR(std::abs(result.normal.z()), 1.0, 1e-6);
}

TEST(ReverseEngineeringTest, FitPlane_Slanted) {
    std::vector<Point3D> pts;
    // z = x + y
    for (int i = 0; i < 100; ++i) {
        double x = static_cast<double>(i % 10);
        double y = static_cast<double>(i / 10);
        pts.emplace_back(x, y, x + y);
    }

    auto result = fit_plane(pts);
    EXPECT_TRUE(result.valid);
    EXPECT_NEAR(result.rms_error, 0.0, 1e-6);
    // Normal should be perpendicular to (1,1,-1) → normalize
    double dot = result.normal.x() + result.normal.y() - result.normal.z();
    EXPECT_NEAR(std::abs(dot), std::sqrt(3.0), 1e-6);
}

TEST(ReverseEngineeringTest, FitPlane_InsufficientPoints) {
    std::vector<Point3D> pts = {{0,0,0}, {1,0,0}};
    auto result = fit_plane(pts);
    EXPECT_FALSE(result.valid);
}

// ═══════════════════════════════════════════════════════════
// Patch / NURBS fitting
// ═══════════════════════════════════════════════════════════

TEST(ReverseEngineeringTest, FitPatch_PlaneCloud) {
    auto cloud = make_plane_cloud(100);
    auto result = fit_patch(cloud.vertices, 2, 2, 5, 5);
    EXPECT_TRUE(result.valid);
    EXPECT_GT(result.rms_error, 0.0);
    // Surface should be evaluable
    auto pt = result.surface.evaluate(0.5, 0.5);
    EXPECT_TRUE(std::isfinite(pt.x()));
}

TEST(ReverseEngineeringTest, PointCloudToMesh) {
    auto cloud = make_plane_cloud(200);
    PointCloudToMeshParams p;
    p.knn = 8;
    p.poisson.max_depth = 5;
    auto mesh = point_cloud_to_mesh(cloud, p);
    EXPECT_GT(mesh.num_vertices(), 0u);
    EXPECT_GT(mesh.num_faces(), 0u);
}

TEST(ReverseEngineeringTest, MeshToNurbsSurface) {
    // Build simple mesh from plane cloud
    auto cloud = make_plane_cloud(100);
    PointCloudToMeshParams pm;
    pm.knn = 8;
    pm.poisson.max_depth = 5;
    auto mesh = point_cloud_to_mesh(cloud, pm);

    ASSERT_GT(mesh.num_vertices(), 0u);

    MeshToNURBSParams mp;
    mp.nurbs.num_cp_u = 5;
    mp.nurbs.num_cp_v = 5;
    mp.nurbs.degree_u = 2;
    mp.nurbs.degree_v = 2;
    mp.nurbs.max_iterations = 10;
    auto surf = mesh_to_nurbs_surface(mesh, mp);
    // Surface should be valid
    auto pt = surf.evaluate(0.5, 0.5);
    EXPECT_TRUE(std::isfinite(pt.x()));
}

// ═══════════════════════════════════════════════════════════
// Curvature-aware sampling (free function)
// ═══════════════════════════════════════════════════════════

TEST(ReverseEngineeringTest, CurvatureAwareSampling_Basic) {
    auto cloud = make_sphere_cloud(200);
    auto indices = curvature_aware_sampling(cloud.vertices, 50);
    EXPECT_LE(indices.size(), 50u);
    EXPECT_GT(indices.size(), 0u);
    // Indices should be within range
    for (auto idx : indices) EXPECT_LT(idx, cloud.vertices.size());
}

TEST(ReverseEngineeringTest, CurvatureAwareSampling_AllPoints) {
    auto cloud = make_plane_cloud(30);
    auto indices = curvature_aware_sampling(cloud.vertices, 100);
    EXPECT_EQ(indices.size(), cloud.vertices.size());
}

// ═══════════════════════════════════════════════════════════
// Hole filling
// ═══════════════════════════════════════════════════════════

TEST(ReverseEngineeringTest, HoleFilling_NoHoles) {
    // Create a closed mesh (tetrahedron-like)
    HalfedgeMesh mesh;
    mesh.build_from_triangles(
        {{0,0,0}, {1,0,0}, {0,1,0}, {0,0,1}},
        {{0,1,2}, {0,1,3}, {1,2,3}, {0,2,3}}
    );
    auto filled = hole_filling(mesh);
    // Should be same size or larger
    EXPECT_GE(filled.num_faces(), mesh.num_faces());
}

// ═══════════════════════════════════════════════════════════
// Smoothing filter (free function)
// ═══════════════════════════════════════════════════════════

TEST(ReverseEngineeringTest, SmoothingFilter_Basic) {
    auto cloud = make_plane_cloud(100);
    auto smoothed = smoothing_filter(cloud.vertices, 0.5, 3);
    EXPECT_EQ(smoothed.size(), cloud.vertices.size());
}

TEST(ReverseEngineeringTest, SmoothingFilter_EmptyPoints) {
    std::vector<Point3D> empty;
    auto smoothed = smoothing_filter(empty, 0.5, 3);
    EXPECT_TRUE(smoothed.empty());
}

// ═══════════════════════════════════════════════════════════
// Estimate normals (free function)
// ═══════════════════════════════════════════════════════════

TEST(ReverseEngineeringTest, EstimateNormals_Plane) {
    std::vector<Point3D> pts;
    for (int i = 0; i < 50; ++i)
        pts.emplace_back(i * 0.02, 0.0, 0.0);
    pts.emplace_back(0.0, 0.02, 0.0);
    for (int i = 0; i < 50; ++i)
        pts.emplace_back(0.0, i * 0.02, 0.0);

    auto normals = estimate_normals(pts, 10);
    EXPECT_EQ(normals.size(), pts.size());
    // Most normals should be Z-aligned
    int z_count = 0;
    for (const auto& n : normals) {
        if (std::abs(n.z()) > 0.7) ++z_count;
    }
    EXPECT_GT(z_count, static_cast<int>(pts.size() * 0.8));
}