#include <gtest/gtest.h>
#include "vde/brep/assembly_constraints.h"
#include "vde/brep/feature_tree.h"
#include "vde/brep/modeling.h"
#include "vde/core/transform.h"
#include <cmath>

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

// ════════════════════════════════════════════════════════════════
//  Assembly Constraint Tests
// ════════════════════════════════════════════════════════════════

TEST(AssemblyConstraintTest, CoincidentConstraint) {
    Assembly assy("test_assy");
    auto* box_a = assy.root.add_part("box_a", make_box(2, 2, 2));
    auto* box_b = assy.root.add_part("box_b", make_box(2, 2, 2),
                                      translate(0, 0, 10));

    EXPECT_TRUE(apply_constraint(assy, box_a, box_b, ConstraintType::Coincident));

    // After coincident constraint, box_b should be sitting directly on box_a
    // Verify by checking that the bounding boxes share a face plane
    AABB3D bb_a = box_a->model->bounds();
    // Compute box_b's world-space bounding box
    AABB3D bb_b_local = box_b->model->bounds();
    AABB3D bb_b_world;
    Point3D corners[8] = {
        bb_b_local.min(),
        Point3D(bb_b_local.max().x(), bb_b_local.min().y(), bb_b_local.min().z()),
        Point3D(bb_b_local.max().x(), bb_b_local.max().y(), bb_b_local.min().z()),
        Point3D(bb_b_local.min().x(), bb_b_local.max().y(), bb_b_local.min().z()),
        Point3D(bb_b_local.min().x(), bb_b_local.min().y(), bb_b_local.max().z()),
        Point3D(bb_b_local.max().x(), bb_b_local.min().y(), bb_b_local.max().z()),
        bb_b_local.max(),
        Point3D(bb_b_local.min().x(), bb_b_local.max().y(), bb_b_local.max().z()),
    };
    for (const auto& c : corners) {
        bb_b_world.expand(box_b->local_transform * c);
    }
    // box_a top = 1.0, box_b bottom should be very close to 1.0
    EXPECT_NEAR(bb_b_world.min().z(), bb_a.max().z(), 1e-4);
}

TEST(AssemblyConstraintTest, ConcentricConstraint) {
    Assembly assy("test_assy");
    auto* cyl_a = assy.root.add_part("cyl_a", make_cylinder(1.0, 4.0));
    auto* cyl_b = assy.root.add_part("cyl_b", make_cylinder(0.5, 2.0),
                                      translate(5, 0, 0));

    EXPECT_TRUE(apply_constraint(assy, cyl_a, cyl_b, ConstraintType::Concentric));

    // Both cylinders should now be concentric (same center in X,Y)
    AABB3D bb_a = cyl_a->model->bounds();
    Point3D center_a = bb_a.center();

    AABB3D bb_b_local = cyl_b->model->bounds();
    Point3D center_b_local = bb_b_local.center();
    Point3D center_b_world = cyl_b->local_transform * center_b_local;

    EXPECT_NEAR(center_b_world.x(), center_a.x(), 1e-4);
    EXPECT_NEAR(center_b_world.y(), center_a.y(), 1e-4);
}

TEST(AssemblyConstraintTest, DistanceConstraint) {
    Assembly assy("test_assy");
    auto* box_a = assy.root.add_part("box_a", make_box(2, 2, 2));
    auto* box_b = assy.root.add_part("box_b", make_box(2, 2, 2),
                                      translate(0, 0, 10));

    EXPECT_TRUE(apply_constraint(assy, box_a, box_b, ConstraintType::Distance, 5.0));

    // After distance constraint of 5.0, box_b bottom should be 5.0 above box_a top
    AABB3D bb_a = box_a->model->bounds();
    AABB3D bb_b_local = box_b->model->bounds();

    Point3D bb_b_min_local = bb_b_local.min();
    Point3D bb_b_min_world = box_b->local_transform * bb_b_min_local;

    double gap = bb_b_min_world.z() - bb_a.max().z();
    EXPECT_NEAR(gap, 5.0, 1e-4);
}

TEST(AssemblyConstraintTest, AngleConstraint) {
    Assembly assy("test_assy");
    auto* box_a = assy.root.add_part("box_a", make_box(2, 2, 2));
    auto* box_b = assy.root.add_part("box_b", make_box(1, 1, 4));

    // Apply 90-degree angle constraint (rotate node_b)
    EXPECT_TRUE(apply_constraint(assy, box_a, box_b, ConstraintType::Angle, M_PI / 2.0));

    // Verify the transform was applied (non-identity rotation)
    EXPECT_FALSE(box_b->local_transform.isApprox(Transform3D::Identity(), 1e-6));
}

TEST(AssemblyConstraintTest, PerpendicularConstraint) {
    Assembly assy("test_assy");
    auto* box_a = assy.root.add_part("box_a", make_box(2, 2, 2));
    auto* box_b = assy.root.add_part("box_b", make_box(1, 1, 4));

    EXPECT_TRUE(apply_constraint(assy, box_a, box_b, ConstraintType::Perpendicular));

    // Verify the transform is not identity (rotation was applied)
    EXPECT_FALSE(box_b->local_transform.isApprox(Transform3D::Identity(), 1e-6));
}

TEST(AssemblyConstraintTest, ParallelConstraint) {
    Assembly assy("test_assy");
    auto* box_a = assy.root.add_part("box_a", make_box(2, 2, 2));
    auto* box_b = assy.root.add_part("box_b", make_box(2, 2, 2),
                                      translate(0, 5, 0));

    EXPECT_TRUE(apply_constraint(assy, box_a, box_b, ConstraintType::Parallel));

    // Parallel constraint preserves the offset (no transform change for axis-aligned)
    AABB3D bb_b_local = box_b->model->bounds();
    Point3D center_b_local = bb_b_local.center();
    Point3D center_b_world = box_b->local_transform * center_b_local;
    EXPECT_NEAR(center_b_world.y(), 5.0, 1e-4);
}

// ════════════════════════════════════════════════════════════════
//  Feature Tree Tests
// ════════════════════════════════════════════════════════════════

TEST(FeatureTreeTest, PrimitiveBox) {
    FeatureNode node;
    node.type = FeatureType::PrimitiveBox;
    node.params.values = {2.0, 3.0, 4.0};

    BrepModel result = node.evaluate();
    EXPECT_GT(result.num_vertices(), 0u);
    EXPECT_GT(result.num_faces(), 0u);

    AABB3D bb = result.bounds();
    EXPECT_NEAR(bb.extent().x(), 2.0, 1e-4);
    EXPECT_NEAR(bb.extent().y(), 3.0, 1e-4);
    EXPECT_NEAR(bb.extent().z(), 4.0, 1e-4);
}

TEST(FeatureTreeTest, PrimitiveCylinder) {
    FeatureNode node;
    node.type = FeatureType::PrimitiveCylinder;
    node.params.values = {1.5, 6.0};

    BrepModel result = node.evaluate();
    EXPECT_GT(result.num_vertices(), 0u);
    EXPECT_GT(result.num_faces(), 0u);

    AABB3D bb = result.bounds();
    EXPECT_NEAR(bb.extent().x(), 3.0, 0.1);  // diameter
    EXPECT_NEAR(bb.extent().z(), 6.0, 1e-4); // height (cylinder is along Z axis)
}

TEST(FeatureTreeTest, PrimitiveSphere) {
    FeatureNode node;
    node.type = FeatureType::PrimitiveSphere;
    node.params.values = {2.5};

    BrepModel result = node.evaluate();
    EXPECT_GT(result.num_vertices(), 0u);

    AABB3D bb = result.bounds();
    EXPECT_NEAR(bb.extent().x(), 5.0, 0.1);  // diameter
    EXPECT_NEAR(bb.extent().y(), 5.0, 0.1);
    EXPECT_NEAR(bb.extent().z(), 5.0, 0.1);
}

TEST(FeatureTreeTest, NestedFeatureTree) {
    // Box → Fillet
    auto box_node = std::make_unique<FeatureNode>();
    box_node->type = FeatureType::PrimitiveBox;
    box_node->params.values = {10.0, 5.0, 3.0};

    auto fillet_node = std::make_unique<FeatureNode>();
    fillet_node->type = FeatureType::Fillet;
    fillet_node->params.values = {1.0};
    fillet_node->params.int_values = {0};
    fillet_node->children.push_back(std::move(box_node));

    BrepModel result = fillet_node->evaluate();
    EXPECT_TRUE(result.is_valid());
    EXPECT_GT(result.num_faces(), 0u);
}

TEST(FeatureTreeTest, BooleanUnionFeature) {
    // Box ∪ Cylinder
    auto box = std::make_unique<FeatureNode>();
    box->type = FeatureType::PrimitiveBox;
    box->params.values = {3.0, 3.0, 3.0};

    auto cyl = std::make_unique<FeatureNode>();
    cyl->type = FeatureType::PrimitiveCylinder;
    cyl->params.values = {1.0, 5.0};

    auto union_node = std::make_unique<FeatureNode>();
    union_node->type = FeatureType::BooleanUnion;
    union_node->children.push_back(std::move(box));
    union_node->children.push_back(std::move(cyl));

    BrepModel result = union_node->evaluate();
    EXPECT_GT(result.num_vertices(), 0u);
    EXPECT_GT(result.num_faces(), 0u);
}

// ════════════════════════════════════════════════════════════════
//  FeatureHistory Undo/Redo Tests
// ════════════════════════════════════════════════════════════════

TEST(FeatureHistoryTest, ApplyOperations) {
    FeatureHistory history;

    // Apply 3 operations
    BrepModel box1 = make_box(2, 2, 2);
    history.apply(box1, FeatureType::PrimitiveBox, FeatureParams{});
    EXPECT_EQ(history.size(), 1u);

    BrepModel box2 = make_box(3, 3, 3);
    history.apply(box2, FeatureType::PrimitiveBox, FeatureParams{});
    EXPECT_EQ(history.size(), 2u);

    BrepModel cyl = make_cylinder(1, 5);
    history.apply(cyl, FeatureType::PrimitiveCylinder, FeatureParams{});
    EXPECT_EQ(history.size(), 3u);
}

TEST(FeatureHistoryTest, UndoRedo) {
    FeatureHistory history;

    // Create 3 distinct models as initial states
    BrepModel box_small = make_box(2, 2, 2);
    BrepModel box_large = make_box(5, 5, 5);
    BrepModel cylinder = make_cylinder(2, 10);

    history.apply(box_small, FeatureType::PrimitiveBox, FeatureParams{});
    EXPECT_EQ(history.size(), 1u);

    history.apply(box_large, FeatureType::PrimitiveBox, FeatureParams{});
    EXPECT_EQ(history.size(), 2u);

    history.apply(cylinder, FeatureType::PrimitiveCylinder, FeatureParams{});
    EXPECT_EQ(history.size(), 3u);

    // Undo twice
    EXPECT_TRUE(history.undo());
    EXPECT_EQ(history.size(), 3u); // size doesn't change with undo

    EXPECT_TRUE(history.undo());
    EXPECT_EQ(history.size(), 3u);

    // Should not be able to undo past start
    EXPECT_TRUE(history.undo());
    EXPECT_FALSE(history.undo()); // no more to undo

    // Redo all
    EXPECT_TRUE(history.redo());
    EXPECT_TRUE(history.redo());
    EXPECT_TRUE(history.redo());
    EXPECT_FALSE(history.redo()); // no more to redo
}

TEST(FeatureHistoryTest, UndoRedoVerifyStates) {
    FeatureHistory history;

    BrepModel initial = make_box(2, 2, 2);
    history.apply(initial, FeatureType::PrimitiveBox, FeatureParams{});

    BrepModel second = make_box(5, 5, 5);
    history.apply(second, FeatureType::PrimitiveBox, FeatureParams{});

    BrepModel third = make_cylinder(2, 10);
    history.apply(third, FeatureType::PrimitiveCylinder, FeatureParams{});

    // Current should be the third operation's state
    AABB3D bb_current = history.current().bounds();
    EXPECT_NEAR(bb_current.extent().x(), 4.0, 0.5);  // cylinder diameter

    // Undo → should get second state
    EXPECT_TRUE(history.undo());
    AABB3D bb_undo = history.current().bounds();
    EXPECT_NEAR(bb_undo.extent().x(), 5.0, 1e-4);

    // Redo → should get third state back
    EXPECT_TRUE(history.redo());
    AABB3D bb_redo = history.current().bounds();
    EXPECT_NEAR(bb_redo.extent().x(), 4.0, 0.5);
}

TEST(FeatureHistoryTest, ApplyAfterUndoTruncatesRedo) {
    FeatureHistory history;

    history.apply(make_box(2, 2, 2), FeatureType::PrimitiveBox, FeatureParams{});
    history.apply(make_box(5, 5, 5), FeatureType::PrimitiveBox, FeatureParams{});
    history.apply(make_cylinder(2, 10), FeatureType::PrimitiveCylinder, FeatureParams{});

    // Undo one
    EXPECT_TRUE(history.undo());
    EXPECT_EQ(history.size(), 3u);

    // Apply a new operation → should truncate redo stack
    history.apply(make_box(10, 10, 10), FeatureType::PrimitiveBox, FeatureParams{});
    EXPECT_EQ(history.size(), 3u); // still 3 → old "third" was replaced

    // Redo should be exhausted (old third was deleted)
    EXPECT_FALSE(history.redo());
}

TEST(FeatureHistoryTest, ThreeOperationsUndoRedoVerify) {
    FeatureHistory history;

    // Apply op1: small box
    history.apply(make_box(2, 2, 2), FeatureType::PrimitiveBox, FeatureParams{});

    // Apply op2: large box
    history.apply(make_box(5, 5, 5), FeatureType::PrimitiveBox, FeatureParams{});

    // Apply op3: cylinder
    history.apply(make_cylinder(2, 10), FeatureType::PrimitiveCylinder, FeatureParams{});

    EXPECT_EQ(history.size(), 3u);

    // Verify current is cylinder
    {
        AABB3D bb = history.current().bounds();
        EXPECT_NEAR(bb.extent().x(), 4.0, 0.5);  // cylinder diameter ≈ 4
    }

    // Undo back to large box
    EXPECT_TRUE(history.undo());
    {
        AABB3D bb = history.current().bounds();
        EXPECT_NEAR(bb.extent().x(), 5.0, 1e-4);
    }

    // Undo back to small box
    EXPECT_TRUE(history.undo());
    {
        AABB3D bb = history.current().bounds();
        EXPECT_NEAR(bb.extent().x(), 2.0, 1e-4);
    }

    // Undo back to empty
    EXPECT_TRUE(history.undo());
    EXPECT_FALSE(history.undo());

    // Redo: empty → small box → large box → cylinder
    EXPECT_TRUE(history.redo());
    EXPECT_TRUE(history.redo());
    EXPECT_TRUE(history.redo());
    EXPECT_FALSE(history.redo());

    // Verify we're back at cylinder
    {
        AABB3D bb = history.current().bounds();
        EXPECT_NEAR(bb.extent().x(), 4.0, 0.5);
    }
}