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

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

// ═══════════════════════════════════════════════════════════
//  check_pair — basic scenarios
// ═══════════════════════════════════════════════════════════

TEST(InterferenceCheckTest, Pair_SeparatedBoxes_NoInterference) {
    auto box_a = make_box(2, 2, 2);
    auto box_b = make_box(2, 2, 2);
    Transform3D ta = translate(0, 0, 5);   // box_a at z=5
    Transform3D tb = translate(0, 0, -5);   // box_b at z=-5

    auto result = check_pair(box_a, box_b, ta, tb);
    EXPECT_FALSE(result.interfering);
    EXPECT_EQ(result.penetration, 0.0);
}

TEST(InterferenceCheckTest, Pair_OverlappingBoxes_HasInterference) {
    auto box_a = make_box(2, 2, 2);        // [-1,1]³
    auto box_b = make_box(2, 2, 2);
    Transform3D ta = translate(0.5, 0, 0); // shifted +0.5 in X
    Transform3D tb = Transform3D::Identity();

    auto result = check_pair(box_a, box_b, ta, tb);
    EXPECT_TRUE(result.interfering);
    EXPECT_GT(result.penetration, 0.0);
}

TEST(InterferenceCheckTest, Pair_TouchingBoxes_Interference) {
    auto box_a = make_box(2, 2, 2);
    auto box_b = make_box(2, 2, 2);
    // Box A from [-1,1], Box B from [1,3] — touching at x=1
    Transform3D ta = Transform3D::Identity();
    Transform3D tb = translate(2, 0, 0);

    auto result = check_pair(box_a, box_b, ta, tb);
    // Touching faces — GJK may or may not detect this as interference
    // depending on numerical precision
    SUCCEED();  // just verify no crash
}

TEST(InterferenceCheckTest, Pair_ContainedSphere_Interference) {
    auto box = make_box(4, 4, 4);
    auto sphere = make_sphere(1.0);
    Transform3D t_box = Transform3D::Identity();
    Transform3D t_sphere = Transform3D::Identity(); // sphere at origin inside box

    auto result = check_pair(box, sphere, t_box, t_sphere);
    EXPECT_TRUE(result.interfering);
    EXPECT_GT(result.overlap_volume, 0.0);
}

TEST(InterferenceCheckTest, Pair_DisjointCylinders_NoInterference) {
    auto cyl_a = make_cylinder(1.0, 4.0);
    auto cyl_b = make_cylinder(1.0, 4.0);
    Transform3D ta = translate(-5, 0, 0);
    Transform3D tb = translate(5, 0, 0);

    auto result = check_pair(cyl_a, cyl_b, ta, tb);
    EXPECT_FALSE(result.interfering);
}

// ═══════════════════════════════════════════════════════════
//  check_interference — assembly-level
// ═══════════════════════════════════════════════════════════

TEST(InterferenceCheckTest, Assembly_EmptyAssembly_NoPairs) {
    Assembly assy("empty");
    auto result = check_interference(assy);
    EXPECT_FALSE(result.has_interference);
    EXPECT_EQ(result.total_pairs_checked, 0);
}

TEST(InterferenceCheckTest, Assembly_SinglePart_NoPairs) {
    Assembly assy("single");
    assy.root.add_part("box", make_box(2, 2, 2));
    auto result = check_interference(assy);
    EXPECT_FALSE(result.has_interference);
    EXPECT_EQ(result.total_pairs_checked, 0);
}

TEST(InterferenceCheckTest, Assembly_SeparatedParts_NoInterference) {
    Assembly assy("separated");
    assy.root.add_part("left", make_box(2, 2, 2), translate(-5, 0, 0));
    assy.root.add_part("right", make_box(2, 2, 2), translate(5, 0, 0));

    auto result = check_interference(assy);
    EXPECT_FALSE(result.has_interference);
    EXPECT_EQ(result.total_pairs_checked, 1);
    EXPECT_EQ(result.interfering_pairs, 0);
}

TEST(InterferenceCheckTest, Assembly_OverlappingParts_HasInterference) {
    Assembly assy("overlap");
    assy.root.add_part("bottom", make_box(4, 4, 2));
    assy.root.add_part("top", make_box(2, 2, 2), translate(0, 0, 1));

    auto result = check_interference(assy);
    EXPECT_TRUE(result.has_interference);
    EXPECT_GT(result.conflicts.size(), 0u);
}

TEST(InterferenceCheckTest, Assembly_ThreeParts_CheckAllPairs) {
    Assembly assy("three");
    // Three boxes spread out — no interference
    assy.root.add_part("A", make_box(2, 2, 2), translate(-5, 0, 0));
    assy.root.add_part("B", make_box(2, 2, 2), translate(0, -5, 0));
    assy.root.add_part("C", make_box(2, 2, 2), translate(0, 5, 0));

    auto result = check_interference(assy);
    // 3 parts → 3 pairs (A-B, A-C, B-C)
    EXPECT_EQ(result.total_pairs_checked, 3);
    EXPECT_FALSE(result.has_interference);
}

TEST(InterferenceCheckTest, Assembly_CoarseOnly_FasterCheck) {
    Assembly assy("coarse");
    assy.root.add_part("box1", make_box(2, 2, 2), translate(0, 0, -2));
    assy.root.add_part("box2", make_box(2, 2, 2), translate(0, 0, 2));

    auto result = check_interference(assy, true);  // coarse only
    EXPECT_EQ(result.total_pairs_checked, 1);
    // AABB check: both boxes at z=±2 with size 2 → bounds [-1,1]×[-1,1]×[-3,-1] and [-1,1]×[-1,1]×[1,3]
    // These don't overlap → no interference
    EXPECT_FALSE(result.has_interference);
}

TEST(InterferenceCheckTest, Assembly_SubassemblyTraversal) {
    Assembly assy("root");
    auto* sub = assy.root.add_subassembly("group", translate(0, 5, 0));
    sub->add_part("sub_box", make_box(2, 2, 2));
    assy.root.add_part("main_box", make_box(4, 4, 4));

    // sub is at y=5, sub_box inside it at identity → sub_box world = (0,5,0)
    // main_box is at origin
    // 2×2 box at y=5 vs 4×4 box at origin → separated by at least 1 unit
    auto result = check_interference(assy);
    EXPECT_EQ(result.total_pairs_checked, 1);
    EXPECT_FALSE(result.has_interference);
}

TEST(InterferenceCheckTest, Assembly_InterferenceSummary) {
    Assembly assy("summary_test");
    assy.root.add_part("part_a", make_box(2, 2, 2));
    assy.root.add_part("part_b", make_box(2, 2, 2), translate(0.5, 0, 0));

    auto result = check_interference(assy);
    auto summary = result.summary();
    EXPECT_FALSE(summary.empty());
    EXPECT_NE(summary.find("interference"), std::string::npos);
}

// ═══════════════════════════════════════════════════════════
//  penetration_depth
// ═══════════════════════════════════════════════════════════

TEST(InterferenceCheckTest, Penetration_Separated_Zero) {
    auto box_a = make_box(2, 2, 2);
    auto box_b = make_box(2, 2, 2);
    double pen = penetration_depth(box_a, box_b,
        translate(-5, 0, 0), translate(5, 0, 0));
    EXPECT_EQ(pen, 0.0);
}

TEST(InterferenceCheckTest, Penetration_Overlapping_Positive) {
    auto box_a = make_box(2, 2, 2);
    auto box_b = make_box(2, 2, 2);
    // box_a: origin, box_b: shifted 0.5 in X → overlapping
    double pen = penetration_depth(box_a, box_b,
        Transform3D::Identity(), translate(0.5, 0, 0));
    EXPECT_GE(pen, 0.0);  // penetration should be ≥ 0
}

TEST(InterferenceCheckTest, LargeAssembly_PerformanceAcceptable) {
    Assembly assy("large");
    // 10×10 grid of small boxes → 100 parts, 4950 pairs
    for (int i = 0; i < 10; ++i) {
        for (int j = 0; j < 10; ++j) {
            assy.root.add_part(
                "box_" + std::to_string(i) + "_" + std::to_string(j),
                make_box(1, 1, 1),
                translate(3.0 * i, 3.0 * j, 0));
        }
    }

    auto result = check_interference(assy);
    // All boxes spaced 3 units apart (size 1) → no interference
    EXPECT_EQ(result.total_pairs_checked, 4950);
    EXPECT_FALSE(result.has_interference);
}