#include <gtest/gtest.h>
#include "vde/brep/brep.h"
#include "vde/brep/modeling.h"
#include "vde/brep/ffd_deformation.h"
#include "vde/brep/brep_validate.h"
#include <cmath>

using namespace vde::brep;
using namespace vde::core;
using namespace vde::mesh;

// ═══════════════════════════════════════════════════════════
//  Lattice Creation Tests
// ═══════════════════════════════════════════════════════════

TEST(FFDLatticeTest, CreateLattice_Dimensions) {
    AABB3D bbox;
    bbox.expand(Point3D(0, 0, 0));
    bbox.expand(Point3D(10, 10, 10));

    auto lattice = create_lattice(bbox, 3, 4, 5);
    EXPECT_EQ(lattice.nx, 3);
    EXPECT_EQ(lattice.ny, 4);
    EXPECT_EQ(lattice.nz, 5);
    EXPECT_EQ(lattice.num_points(), 60u);
    EXPECT_TRUE(lattice.is_valid());
}

TEST(FFDLatticeTest, CreateLattice_ControlPointsAtCorners) {
    AABB3D bbox;
    bbox.expand(Point3D(0, 0, 0));
    bbox.expand(Point3D(10, 5, 3));

    auto lattice = create_lattice(bbox, 2, 2, 2);

    // First corner (0,0,0)
    auto& cp000 = lattice.control_point(0, 0, 0);
    EXPECT_NEAR(cp000.x(), 0.0, 1e-9);
    EXPECT_NEAR(cp000.y(), 0.0, 1e-9);
    EXPECT_NEAR(cp000.z(), 0.0, 1e-9);

    // Last corner (1,1,1)
    auto& cp111 = lattice.control_point(1, 1, 1);
    EXPECT_NEAR(cp111.x(), 10.0, 1e-9);
    EXPECT_NEAR(cp111.y(), 5.0, 1e-9);
    EXPECT_NEAR(cp111.z(), 3.0, 1e-9);
}

TEST(FFDLatticeTest, CreateCageLattice_FromVertices) {
    // Octahedron-like cage vertices
    std::vector<Point3D> cage = {
        Point3D(0, 0, 5), Point3D(5, 0, 0), Point3D(0, 5, 0),
        Point3D(-5, 0, 0), Point3D(0, -5, 0), Point3D(0, 0, -5)
    };

    auto lattice = create_cage_lattice(cage, 4, 4, 4);
    EXPECT_EQ(lattice.nx, 4);
    EXPECT_EQ(lattice.ny, 4);
    EXPECT_EQ(lattice.nz, 4);
    EXPECT_EQ(lattice.num_points(), 64u);
    EXPECT_TRUE(lattice.is_valid());

    // Bounding box should encompass all cage vertices
    EXPECT_LE(lattice.bbox.min().x(), -5.0);
    EXPECT_GE(lattice.bbox.max().x(), 5.0);
}

TEST(FFDLatticeTest, CreateLattice_MinimumSize) {
    AABB3D bbox;
    bbox.expand(Point3D(1, 1, 1));
    bbox.expand(Point3D(2, 2, 2));

    auto lattice = create_lattice(bbox, 2, 2, 2);
    EXPECT_TRUE(lattice.is_valid());
    EXPECT_EQ(lattice.num_points(), 8u);
}

TEST(FFDLatticeTest, CreateLattice_InvalidDimensionsThrows) {
    AABB3D bbox;
    bbox.expand(Point3D(0, 0, 0));
    bbox.expand(Point3D(1, 1, 1));

    EXPECT_THROW(create_lattice(bbox, 1, 2, 2), std::invalid_argument);
    EXPECT_THROW(create_lattice(bbox, 2, 1, 2), std::invalid_argument);
    EXPECT_THROW(create_lattice(bbox, 2, 2, 1), std::invalid_argument);
}

// ═══════════════════════════════════════════════════════════
//  Bernstein Polynomial Tests
// ═══════════════════════════════════════════════════════════

TEST(BernsteinTest, BasicValues) {
    // B_{0,1}(0) = 1
    EXPECT_NEAR(bernstein(0, 1, 0.0), 1.0, 1e-9);
    // B_{1,1}(1) = 1
    EXPECT_NEAR(bernstein(1, 1, 1.0), 1.0, 1e-9);
    // B_{0,1}(0.5) = 0.5
    EXPECT_NEAR(bernstein(0, 1, 0.5), 0.5, 1e-9);
}

TEST(BernsteinTest, PartitionOfUnity) {
    // Sum of all Bernstein basis functions of order n should be 1
    for (double t = 0.0; t <= 1.0; t += 0.1) {
        double sum = 0.0;
        for (int i = 0; i <= 3; ++i) {
            sum += bernstein(i, 3, t);
        }
        EXPECT_NEAR(sum, 1.0, 1e-9) << " at t=" << t;
    }
}

TEST(BernsteinTest, OutOfBoundsClamped) {
    EXPECT_NEAR(bernstein(0, 2, -0.5), 1.0, 1e-9);  // clamped to 0
    EXPECT_NEAR(bernstein(2, 2, 1.5), 1.0, 1e-9);   // clamped to 1
}

// ═══════════════════════════════════════════════════════════
//  B-Rep Deformation Tests
// ═══════════════════════════════════════════════════════════

TEST(FFDBrepTest, IdentityDeformation_Box) {
    auto box = make_box(4.0, 4.0, 4.0);
    ASSERT_TRUE(box.is_valid());

    auto bbox = box.bounds();
    auto lattice = create_lattice(bbox, 3, 3, 3);

    // Zero displacements = identity deformation
    std::vector<Vector3D> displacements(lattice.num_points(), Vector3D::Zero());

    auto deformed = deform_brep(box, lattice, displacements);
    EXPECT_TRUE(deformed.is_valid());
    EXPECT_EQ(deformed.num_vertices(), box.num_vertices());
    EXPECT_EQ(deformed.num_faces(), box.num_faces());
    EXPECT_EQ(deformed.num_edges(), box.num_edges());

    // Structural validity should be preserved
    EXPECT_TRUE(deformed.is_valid());
    // Validate (watertightness may be a pre-existing modeling concern)
    auto vr = validate(deformed);
    (void)vr; // pre-existing: make_box may not be perfectly watertight
}

TEST(FFDBrepTest, StretchBox_XDirection) {
    auto box = make_box(4.0, 4.0, 4.0);
    ASSERT_TRUE(box.is_valid());

    auto bbox = box.bounds();
    auto lattice = create_lattice(bbox, 3, 3, 3);

    // Stretch: move the +X control points
    std::vector<Vector3D> displacements(lattice.num_points(), Vector3D::Zero());
    // Move all control points at i=2 (max X) by 3 in X direction
    for (int k = 0; k < 3; ++k)
        for (int j = 0; j < 3; ++j)
            displacements[(k * 3 + j) * 3 + 2] = Vector3D(3.0, 0, 0);

    auto deformed = deform_brep(box, lattice, displacements);
    EXPECT_TRUE(deformed.is_valid());

    // Bounding box should be stretched
    auto def_bbox = deformed.bounds();
    EXPECT_GT(def_bbox.max().x(), bbox.max().x() + 1.0);
    EXPECT_NEAR(def_bbox.max().y(), bbox.max().y(), 1.0);
    EXPECT_NEAR(def_bbox.max().z(), bbox.max().z(), 1.0);

    // Validation should pass
    EXPECT_TRUE(deformed.is_valid());
    auto vr = validate(deformed);
    (void)vr;
}

TEST(FFDBrepTest, CompressBox_AllDirections) {
    auto box = make_box(4.0, 4.0, 4.0);
    ASSERT_TRUE(box.is_valid());

    auto bbox = box.bounds();
    auto lattice = create_lattice(bbox, 3, 3, 3);

    // Compress: move outer control points inward
    std::vector<Vector3D> displacements(lattice.num_points(), Vector3D::Zero());
    for (int k = 0; k < 3; ++k) {
        for (int j = 0; j < 3; ++j) {
            for (int i = 0; i < 3; ++i) {
                double dx = (i == 0) ? 1.0 : (i == 2) ? -1.0 : 0.0;
                double dy = (j == 0) ? 1.0 : (j == 2) ? -1.0 : 0.0;
                double dz = (k == 0) ? 1.0 : (k == 2) ? -1.0 : 0.0;
                displacements[(k * 3 + j) * 3 + i] = Vector3D(dx, dy, dz);
            }
        }
    }

    auto deformed = deform_brep(box, lattice, displacements);
    EXPECT_TRUE(deformed.is_valid());

    auto def_bbox = deformed.bounds();
    EXPECT_LT(def_bbox.max().x() - def_bbox.min().x(),
              bbox.max().x() - bbox.min().x());

    EXPECT_TRUE(deformed.is_valid());
    auto vr2 = validate(deformed);
    (void)vr2;
}

TEST(FFDBrepTest, TwistBox_AroundZ) {
    auto box = make_box(4.0, 4.0, 4.0);
    ASSERT_TRUE(box.is_valid());

    auto bbox = box.bounds();
    auto lattice = create_lattice(bbox, 3, 3, 3);

    // Twist: move top layer (+Z) in X direction, bottom layer (-Z) in -X
    std::vector<Vector3D> displacements(lattice.num_points(), Vector3D::Zero());
    double twist_amount = 2.0;
    for (int j = 0; j < 3; ++j) {
        for (int i = 0; i < 3; ++i) {
            // Top layer (k=2): move +X
            displacements[(2 * 3 + j) * 3 + i] = Vector3D(twist_amount, 0, 0);
            // Bottom layer (k=0): move -X
            displacements[(0 * 3 + j) * 3 + i] = Vector3D(-twist_amount, 0, 0);
        }
    }

    auto deformed = deform_brep(box, lattice, displacements);
    EXPECT_TRUE(deformed.is_valid());

    // Top should be wider (shifted in X)
    auto def_bbox = deformed.bounds();
    EXPECT_GT(def_bbox.max().x(), bbox.max().x() + 0.5);

    EXPECT_TRUE(deformed.is_valid());
    auto vr2b = validate(deformed);
    (void)vr2b;
}

TEST(FFDBrepTest, DeformSphere) {
    auto sphere = make_sphere(3.0, 16, 8);
    ASSERT_TRUE(sphere.is_valid());

    auto bbox = sphere.bounds();
    auto lattice = create_lattice(bbox, 3, 3, 3);

    // Squash in Z: compress top and bottom
    std::vector<Vector3D> displacements(lattice.num_points(), Vector3D::Zero());
    for (int j = 0; j < 3; ++j) {
        for (int i = 0; i < 3; ++i) {
            // Top layer: move down
            displacements[(2 * 3 + j) * 3 + i] = Vector3D(0, 0, -1.0);
            // Bottom layer: move up
            displacements[(0 * 3 + j) * 3 + i] = Vector3D(0, 0, 1.0);
        }
    }

    auto deformed = deform_brep(sphere, lattice, displacements);
    EXPECT_TRUE(deformed.is_valid());

    auto def_bbox = deformed.bounds();
    EXPECT_LT(def_bbox.max().z() - def_bbox.min().z(),
              bbox.max().z() - bbox.min().z());

    EXPECT_TRUE(deformed.is_valid());
    auto vr2c = validate(deformed);
    (void)vr2c;
}

TEST(FFDBrepTest, DeformBox_ToMesh) {
    auto box = make_box(4.0, 4.0, 4.0);
    ASSERT_TRUE(box.is_valid());

    auto bbox = box.bounds();
    auto lattice = create_lattice(bbox, 3, 3, 3);

    std::vector<Vector3D> displacements(lattice.num_points(), Vector3D::Zero());
    // Stretch middle in Y
    for (int k = 0; k < 3; ++k)
        for (int i = 0; i < 3; ++i)
            displacements[(k * 3 + 1) * 3 + i] = Vector3D(0, 2.0, 0);

    auto deformed = deform_brep(box, lattice, displacements);
    auto mesh = deformed.to_mesh();
    EXPECT_GT(mesh.num_faces(), 0u);
    EXPECT_GT(mesh.num_vertices(), 0u);
}

// ═══════════════════════════════════════════════════════════
//  Mesh Deformation Tests
// ═══════════════════════════════════════════════════════════

TEST(FFDMeshTest, IdentityDeformation_Mesh) {
    auto box = make_box(3.0, 3.0, 3.0);
    auto mesh = box.to_mesh();
    ASSERT_GT(mesh.num_vertices(), 0u);

    auto bbox = mesh.bounds();
    auto lattice = create_lattice(bbox, 3, 3, 3);

    std::vector<Vector3D> displacements(lattice.num_points(), Vector3D::Zero());

    auto deformed = deform_mesh(mesh, lattice, displacements);
    EXPECT_EQ(deformed.num_vertices(), mesh.num_vertices());
    EXPECT_EQ(deformed.num_faces(), mesh.num_faces());
}

TEST(FFDMeshTest, StretchMesh) {
    auto box = make_box(4.0, 4.0, 4.0);
    auto mesh = box.to_mesh();
    ASSERT_GT(mesh.num_vertices(), 0u);

    auto bbox = mesh.bounds();
    auto lattice = create_lattice(bbox, 3, 3, 3);

    // Stretch in Z
    std::vector<Vector3D> displacements(lattice.num_points(), Vector3D::Zero());
    for (int j = 0; j < 3; ++j)
        for (int i = 0; i < 3; ++i)
            displacements[(2 * 3 + j) * 3 + i] = Vector3D(0, 0, 5.0);

    auto deformed = deform_mesh(mesh, lattice, displacements);
    EXPECT_GT(deformed.num_faces(), 0u);
    EXPECT_GT(deformed.num_vertices(), 0u);

    auto def_bbox = deformed.bounds();
    EXPECT_GT(def_bbox.max().z(), bbox.max().z() + 2.0);
}

TEST(FFDMeshTest, EmptyMesh) {
    HalfedgeMesh empty_mesh;

    AABB3D bbox;
    bbox.expand(Point3D(0, 0, 0));
    bbox.expand(Point3D(1, 1, 1));
    auto lattice = create_lattice(bbox, 2, 2, 2);

    std::vector<Vector3D> displacements(lattice.num_points(), Vector3D::Zero());
    auto deformed = deform_mesh(empty_mesh, lattice, displacements);
    EXPECT_EQ(deformed.num_vertices(), 0u);
}

// ═══════════════════════════════════════════════════════════
//  Edge Cases and Error Handling
// ═══════════════════════════════════════════════════════════

TEST(FFDErrorTest, InvalidLatticeThrows) {
    auto box = make_box(2.0, 2.0, 2.0);
    FFDLattice bad_lattice;  // nx=ny=nz=0, not valid
    std::vector<Vector3D> empty_disp;

    EXPECT_THROW(deform_brep(box, bad_lattice, empty_disp), std::invalid_argument);
}

TEST(FFDErrorTest, MismatchedDisplacementsThrows) {
    auto box = make_box(2.0, 2.0, 2.0);
    auto bbox = box.bounds();
    auto lattice = create_lattice(bbox, 3, 3, 3);

    // Wrong size displacements
    std::vector<Vector3D> bad_disp(10, Vector3D::Zero());
    EXPECT_THROW(deform_brep(box, lattice, bad_disp), std::invalid_argument);
}

TEST(FFDErrorTest, DeformPointOutsideLattice_Clamped) {
    AABB3D bbox;
    bbox.expand(Point3D(0, 0, 0));
    bbox.expand(Point3D(10, 10, 10));
    auto lattice = create_lattice(bbox, 3, 3, 3);

    // Point well outside the lattice
    Point3D far_point(100, 100, 100);
    std::vector<Vector3D> displacements(lattice.num_points(), Vector3D::Zero());

    // Should not throw — points outside are clamped to [0,1]
    Point3D result = deform_point(far_point, lattice, displacements);
    // Result should be at the corner of the lattice (clamped to s=t=u=1)
    EXPECT_NEAR(result.x(), lattice.control_point(2, 2, 2).x(), 1e-6);
    EXPECT_NEAR(result.y(), lattice.control_point(2, 2, 2).y(), 1e-6);
    EXPECT_NEAR(result.z(), lattice.control_point(2, 2, 2).z(), 1e-6);
}