#include <gtest/gtest.h>
#include "vde/brep/constraint_solver.h"
#include "vde/brep/drawing_standards.h"
#include "vde/brep/modeling.h"
#include "vde/core/transform.h"
#include "vde/core/aabb.h"
#include <cmath>
#include <vector>

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

// ════════════════════════════════════════════════════════════════
//  Helpers
// ════════════════════════════════════════════════════════════════

namespace {

/// World-space AABB of a node (transforming local model bounds).
AABB3D world_bounds(const AssemblyNode* node) {
    AABB3D bb_world;
    if (!node->model.has_value()) return bb_world;
    AABB3D bb_local = node->model->bounds();
    Point3D corners[8] = {
        bb_local.min(),
        Point3D(bb_local.max().x(), bb_local.min().y(), bb_local.min().z()),
        Point3D(bb_local.max().x(), bb_local.max().y(), bb_local.min().z()),
        Point3D(bb_local.min().x(), bb_local.max().y(), bb_local.min().z()),
        Point3D(bb_local.min().x(), bb_local.min().y(), bb_local.max().z()),
        Point3D(bb_local.max().x(), bb_local.min().y(), bb_local.max().z()),
        bb_local.max(),
        Point3D(bb_local.min().x(), bb_local.max().y(), bb_local.max().z()),
    };
    for (const auto& c : corners) {
        bb_world.expand(node->local_transform * c);
    }
    return bb_world;
}

} // anonymous namespace

// ════════════════════════════════════════════════════════════════
//  apply_coincident
// ════════════════════════════════════════════════════════════════

TEST(ConstraintSolverTest, CoincidentAlignsFacePlanes) {
    Assembly assy("test_coincident");
    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));

    apply_coincident(*box_a, *box_b);

    AABB3D bb_a = world_bounds(box_a);
    AABB3D bb_b = world_bounds(box_b);

    // After coincident, box_b bottom should sit on box_a top (gap ~0)
    double gap = bb_b.min().z() - bb_a.max().z();
    EXPECT_NEAR(gap, 0.0, 1e-4);
}

TEST(ConstraintSolverTest, CoincidentPreservesLateralAlignment) {
    Assembly assy("test_coincident2");
    auto* box_a = assy.root.add_part("box_a", make_box(4, 4, 2));
    auto* box_b = assy.root.add_part("box_b", make_box(2, 2, 4),
                                      translate(0, 0, 8));

    apply_coincident(*box_a, *box_b);

    AABB3D bb_a = world_bounds(box_a);
    AABB3D bb_b = world_bounds(box_b);

    // Centers should align in X and Y
    EXPECT_NEAR(bb_b.center().x(), bb_a.center().x(), 1e-4);
    EXPECT_NEAR(bb_b.center().y(), bb_a.center().y(), 1e-4);
    EXPECT_NEAR(bb_b.min().z(), bb_a.max().z(), 1e-4);
}

TEST(ConstraintSolverTest, CoincidentNoModelReturnsIdentity) {
    Assembly assy("test_no_model");
    auto* node_a = assy.root.add_subassembly("empty_a");
    auto* node_b = assy.root.add_subassembly("empty_b");

    auto T = apply_coincident(*node_a, *node_b);
    EXPECT_TRUE(T.isApprox(Transform3D::Identity(), 1e-12));
}

// ════════════════════════════════════════════════════════════════
//  apply_concentric
// ════════════════════════════════════════════════════════════════

TEST(ConstraintSolverTest, ConcentricAlignsAxes) {
    Assembly assy("test_concentric");
    auto* cyl_a = assy.root.add_part("cyl_a", make_cylinder(1.0, 6.0));
    auto* cyl_b = assy.root.add_part("cyl_b", make_cylinder(0.5, 4.0),
                                      translate(5, 5, 0));

    apply_concentric(*cyl_a, *cyl_b);

    AABB3D bb_a = world_bounds(cyl_a);
    AABB3D bb_b = world_bounds(cyl_b);

    // Centers should align in X and Y after concentric constraint
    EXPECT_NEAR(bb_b.center().x(), bb_a.center().x(), 1e-4);
    EXPECT_NEAR(bb_b.center().y(), bb_a.center().y(), 1e-4);
}

TEST(ConstraintSolverTest, ConcentricPreservesZPosition) {
    Assembly assy("test_concentric_z");
    auto* cyl_a = assy.root.add_part("cyl_a", make_cylinder(1.0, 6.0));
    auto* cyl_b = assy.root.add_part("cyl_b", make_cylinder(0.5, 4.0),
                                      translate(5, 5, 3.0));

    double z_before = world_bounds(cyl_b).center().z();
    apply_concentric(*cyl_a, *cyl_b);
    double z_after = world_bounds(cyl_b).center().z();

    // Z should be preserved (concentric only affects radial directions)
    EXPECT_NEAR(z_after, z_before, 1e-4);
}

TEST(ConstraintSolverTest, ConcentricDifferentOrientationCylinders) {
    // TODO: This test requires rotated input shapes; for now we test
    // the axis-invariant case.
    Assembly assy("test_concentric_orient");
    auto* cyl_a = assy.root.add_part("cyl_a", make_cylinder(1.0, 6.0));
    auto* cyl_b = assy.root.add_part("cyl_b", make_cylinder(0.5, 4.0),
                                      translate(3, 0, 0));

    apply_concentric(*cyl_a, *cyl_b);

    AABB3D bb_a = world_bounds(cyl_a);
    AABB3D bb_b = world_bounds(cyl_b);

    EXPECT_NEAR(bb_b.center().x(), bb_a.center().x(), 1e-4);
    EXPECT_NEAR(bb_b.center().y(), bb_a.center().y(), 1e-4);
}

// ════════════════════════════════════════════════════════════════
//  apply_distance
// ════════════════════════════════════════════════════════════════

TEST(ConstraintSolverTest, DistanceProducesCorrectGap) {
    Assembly assy("test_distance");
    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));

    apply_distance(*box_a, *box_b, 5.0);

    AABB3D bb_a = world_bounds(box_a);
    AABB3D bb_b = world_bounds(box_b);

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

TEST(ConstraintSolverTest, DistanceZeroIsCoincident) {
    Assembly assy("test_distance_zero");
    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, 8));

    apply_distance(*box_a, *box_b, 0.0);

    AABB3D bb_a = world_bounds(box_a);
    AABB3D bb_b = world_bounds(box_b);

    double gap = bb_b.min().z() - bb_a.max().z();
    EXPECT_NEAR(gap, 0.0, 1e-4);
}

TEST(ConstraintSolverTest, DistanceNegativePushesAway) {
    // Negative distance means overlap
    Assembly assy("test_distance_neg");
    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));

    apply_distance(*box_a, *box_b, -1.0);

    AABB3D bb_a = world_bounds(box_a);
    AABB3D bb_b = world_bounds(box_b);

    double gap = bb_b.min().z() - bb_a.max().z();
    EXPECT_NEAR(gap, -1.0, 1e-4);
}

// ════════════════════════════════════════════════════════════════
//  solve_constraints
// ════════════════════════════════════════════════════════════════

TEST(ConstraintSolverTest, SolveSingleConstraint) {
    Assembly assy("test_solve_single");
    assy.root.add_part("box_a", make_box(2, 2, 2));
    assy.root.add_part("box_b", make_box(2, 2, 2), translate(5, 3, 10));

    std::vector<ConstraintEntry> constraints = {
        {0, 1, Constraint3D::Coincident}
    };

    EXPECT_TRUE(solve_constraints(assy, constraints, 50, 1e-4));

    auto* box_a = assy.root.children[0].get();
    auto* box_b = assy.root.children[1].get();
    AABB3D bb_a = world_bounds(box_a);
    AABB3D bb_b = world_bounds(box_b);
    double gap = bb_b.min().z() - bb_a.max().z();
    EXPECT_NEAR(gap, 0.0, 1e-3);
}

TEST(ConstraintSolverTest, SolveMultipleConstraints) {
    Assembly assy("test_solve_multi");
    assy.root.add_part("box_a", make_box(4, 4, 2));
    assy.root.add_part("cyl_a", make_cylinder(1.0, 6.0), translate(5, 0, 0));

    std::vector<ConstraintEntry> constraints = {
        {0, 1, Constraint3D::Coincident},   // faces coplanar
        {0, 1, Constraint3D::Concentric}    // axes aligned
    };

    EXPECT_TRUE(solve_constraints(assy, constraints, 100, 1e-3));

    auto* box_a = assy.root.children[0].get();
    auto* cyl_a = assy.root.children[1].get();
    AABB3D bb_a = world_bounds(box_a);
    AABB3D bb_b = world_bounds(cyl_a);

    // Centers aligned in X and Y (concentric)
    EXPECT_NEAR(bb_b.center().x(), bb_a.center().x(), 1e-3);
    EXPECT_NEAR(bb_b.center().y(), bb_a.center().y(), 1e-3);
}

TEST(ConstraintSolverTest, SolveWithDistance) {
    Assembly assy("test_solve_dist");
    assy.root.add_part("box_a", make_box(2, 2, 2));
    assy.root.add_part("box_b", make_box(2, 2, 2), translate(0, 0, 10));

    std::vector<ConstraintEntry> constraints = {
        {0, 1, Constraint3D::Distance, 3.0}
    };

    EXPECT_TRUE(solve_constraints(assy, constraints, 50, 1e-4));

    auto* box_a = assy.root.children[0].get();
    auto* box_b = assy.root.children[1].get();
    AABB3D bb_a = world_bounds(box_a);
    AABB3D bb_b = world_bounds(box_b);
    double gap = bb_b.min().z() - bb_a.max().z();
    EXPECT_NEAR(gap, 3.0, 1e-3);
}

TEST(ConstraintSolverTest, SolveEmptyConstraintsReturnsTrue) {
    Assembly assy("test_solve_empty");
    assy.root.add_part("box_a", make_box(2, 2, 2));

    std::vector<ConstraintEntry> constraints;
    EXPECT_TRUE(solve_constraints(assy, constraints));
}

TEST(ConstraintSolverTest, SolveConvergenceCheck) {
    Assembly assy("test_solve_conv");
    assy.root.add_part("box_a", make_box(2, 2, 2));
    assy.root.add_part("box_b", make_box(2, 2, 2), translate(10, 10, 10));

    std::vector<ConstraintEntry> constraints = {
        {0, 1, Constraint3D::Coincident}
    };

    // Should converge well within 100 iterations
    bool converged = solve_constraints(assy, constraints, 100, 1e-6);
    EXPECT_TRUE(converged);
}

TEST(ConstraintSolverTest, SolveConflictingConstraintsHandledGracefully) {
    // Conflicting: box_b must be both coincident and at distance 10 from box_a
    Assembly assy("test_conflict");
    assy.root.add_part("box_a", make_box(2, 2, 2));
    assy.root.add_part("box_b", make_box(2, 2, 2), translate(0, 0, 10));

    std::vector<ConstraintEntry> constraints = {
        {0, 1, Constraint3D::Coincident},
        {0, 1, Constraint3D::Distance, 10.0}
    };

    // Should NOT crash — just returns false (didn't converge)
    bool converged = solve_constraints(assy, constraints, 50, 1e-6);
    // Conflicting constraints won't converge; this is expected
    EXPECT_FALSE(converged);
}

// ════════════════════════════════════════════════════════════════
//  Constraint3D type coverage — Parallel, Perpendicular, Angle
// ════════════════════════════════════════════════════════════════

TEST(ConstraintSolverTest, ParallelConstraintAppliesTransform) {
    Assembly assy("test_parallel");
    assy.root.add_part("box_a", make_box(2, 2, 8));  // tall in Z
    assy.root.add_part("box_b", make_box(2, 2, 8),   // tall in Z
                        translate(0, 5, 0));

    std::vector<ConstraintEntry> constraints = {
        {0, 1, Constraint3D::Parallel}
    };

    EXPECT_TRUE(solve_constraints(assy, constraints, 50, 1e-6));
    // Parallel constraint should not crash; it applies rotation to align axes
    SUCCEED();
}

TEST(ConstraintSolverTest, PerpendicularConstraintAppliesTransform) {
    Assembly assy("test_perp");
    assy.root.add_part("box_a", make_box(2, 2, 8));  // tall in Z
    assy.root.add_part("box_b", make_box(2, 2, 8),
                        translate(0, 5, 0));

    std::vector<ConstraintEntry> constraints = {
        {0, 1, Constraint3D::Perpendicular}
    };

    EXPECT_TRUE(solve_constraints(assy, constraints, 50, 1e-6));

    // Verify transform is non-identity
    auto* box_b = assy.root.children[1].get();
    EXPECT_FALSE(box_b->local_transform.isApprox(Transform3D::Identity(), 1e-3));
}

TEST(ConstraintSolverTest, AngleConstraintAppliesRotation) {
    Assembly assy("test_angle");
    assy.root.add_part("box_a", make_box(2, 2, 2));
    assy.root.add_part("box_b", make_box(2, 2, 8));

    std::vector<ConstraintEntry> constraints = {
        {0, 1, Constraint3D::Angle, M_PI / 2.0}
    };

    EXPECT_TRUE(solve_constraints(assy, constraints, 50, 1e-6));

    auto* box_b = assy.root.children[1].get();
    EXPECT_FALSE(box_b->local_transform.isApprox(Transform3D::Identity(), 1e-6));
}

TEST(ConstraintSolverTest, TangentConstraintApplies) {
    Assembly assy("test_tangent");
    assy.root.add_part("box_a", make_box(2, 2, 2));
    assy.root.add_part("box_b", make_box(2, 2, 2),
                        translate(0, 0, 10));

    std::vector<ConstraintEntry> constraints = {
        {0, 1, Constraint3D::Tangent}
    };

    EXPECT_TRUE(solve_constraints(assy, constraints, 50, 1e-4));

    AABB3D bb_a = world_bounds(assy.root.children[0].get());
    AABB3D bb_b = world_bounds(assy.root.children[1].get());
    double gap = bb_b.min().z() - bb_a.max().z();
    EXPECT_NEAR(gap, 0.0, 1e-3);
}

// ════════════════════════════════════════════════════════════════
//  Three-node assembly scenario
// ════════════════════════════════════════════════════════════════

TEST(ConstraintSolverTest, ThreeNodeSolve) {
    Assembly assy("test_three_node");
    assy.root.add_part("base", make_box(4, 4, 1));             // node 0
    assy.root.add_part("mid", make_box(2, 2, 2),
                        translate(1, 1, 10));                   // node 1
    assy.root.add_part("top", make_box(1, 1, 3),
                        translate(1, 1, 15));                   // node 2

    std::vector<ConstraintEntry> constraints = {
        {0, 1, Constraint3D::Coincident},   // mid sits on base
        {1, 2, Constraint3D::Coincident},   // top sits on mid
        {0, 1, Constraint3D::Concentric},   // centers aligned
        {1, 2, Constraint3D::Concentric}    // centers aligned
    };

    EXPECT_TRUE(solve_constraints(assy, constraints, 200, 1e-3));

    // All three should now be stacked and centered
    AABB3D bb0 = world_bounds(assy.root.children[0].get());
    AABB3D bb1 = world_bounds(assy.root.children[1].get());
    AABB3D bb2 = world_bounds(assy.root.children[2].get());

    // Centers aligned in X and Y
    EXPECT_NEAR(bb1.center().x(), bb0.center().x(), 1e-3);
    EXPECT_NEAR(bb1.center().y(), bb0.center().y(), 1e-3);
    EXPECT_NEAR(bb2.center().x(), bb0.center().x(), 1e-3);
    EXPECT_NEAR(bb2.center().y(), bb0.center().y(), 1e-3);

    // Stacked: bb1 sits on bb0, bb2 sits on bb1
    double gap_01 = bb1.min().z() - bb0.max().z();
    double gap_12 = bb2.min().z() - bb1.max().z();
    EXPECT_NEAR(gap_01, 0.0, 1e-3);
    EXPECT_NEAR(gap_12, 0.0, 1e-3);
}

// ════════════════════════════════════════════════════════════════
//  ConstraintGraph 测试
// ════════════════════════════════════════════════════════════════

TEST(ConstraintGraphTest, AddNodes) {
    Assembly assy("graph_test");
    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(5, 0, 0));

    ConstraintGraph graph;
    int idx_a = graph.add_node("box_a", box_a);
    int idx_b = graph.add_node("box_b", box_b);

    EXPECT_EQ(graph.node_count(), 2);
    EXPECT_EQ(idx_a, 0);
    EXPECT_EQ(idx_b, 1);
    EXPECT_EQ(graph.node_name(0), "box_a");
    EXPECT_EQ(graph.node_name(1), "box_b");
    EXPECT_EQ(graph.node_ptr(0), box_a);
}

TEST(ConstraintGraphTest, AddConstraints) {
    Assembly assy("graph_test");
    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(5, 0, 0));

    ConstraintGraph graph;
    graph.add_node("box_a", box_a);
    graph.add_node("box_b", box_b);

    graph.add_constraint({0, 1, ConstraintType::Coincident});
    graph.add_constraint({0, 1, ConstraintType::Concentric});

    EXPECT_EQ(graph.constraint_count(), 2);
    EXPECT_EQ(graph.constraint(0).type, ConstraintType::Coincident);
    EXPECT_EQ(graph.constraint(1).type, ConstraintType::Concentric);
}

TEST(ConstraintGraphTest, HasConstraintBetween) {
    Assembly assy("graph_test");
    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));

    ConstraintGraph graph;
    graph.add_node("a", box_a);
    graph.add_node("b", box_b);
    graph.add_constraint({0, 1, ConstraintType::Coincident});

    EXPECT_TRUE(graph.has_constraint_between(0, 1));
    EXPECT_TRUE(graph.has_constraint_between(1, 0));  // 方向不重要，双向均查到
    // 当 a == b 时的安全处理
    EXPECT_FALSE(graph.has_constraint_between(-1, -1));
}

TEST(ConstraintGraphTest, RemoveConstraint) {
    Assembly assy("graph_test");
    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));

    ConstraintGraph graph;
    graph.add_node("a", box_a);
    graph.add_node("b", box_b);
    graph.add_constraint({0, 1, ConstraintType::Coincident});
    graph.add_constraint({0, 1, ConstraintType::Distance, 5.0});

    EXPECT_EQ(graph.constraint_count(), 2);

    graph.remove_constraint(0);

    // 约束不活跃，但计数不变（标记删除）
    auto ncs = graph.node_constraints(0);
    EXPECT_EQ(ncs.size(), 1);  // 只剩约束 1
}

TEST(ConstraintGraphTest, ConstraintsBetween) {
    Assembly assy("graph_test");
    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));

    ConstraintGraph graph;
    graph.add_node("a", box_a);
    graph.add_node("b", box_b);
    graph.add_constraint({0, 1, ConstraintType::Coincident});
    graph.add_constraint({0, 1, ConstraintType::Distance, 3.0});

    auto between = graph.constraints_between(0, 1);
    EXPECT_EQ(between.size(), 2);
}

TEST(ConstraintGraphTest, ClearGraph) {
    Assembly assy("graph_test");
    auto* box_a = assy.root.add_part("box_a", make_box(2, 2, 2));

    ConstraintGraph graph;
    graph.add_node("a", box_a);
    graph.add_constraint({0, 0, ConstraintType::Parallel}); // self-loop (unusual but valid)

    graph.clear();
    EXPECT_EQ(graph.node_count(), 0);
    EXPECT_EQ(graph.constraint_count(), 0);
}

// ════════════════════════════════════════════════════════════════
//  DOF 分析测试
// ════════════════════════════════════════════════════════════════

TEST(DOFAnalysisTest, SingleNodeHas6DOF) {
    Assembly assy("dof_test");
    auto* box_a = assy.root.add_part("box_a", make_box(2, 2, 2));

    ConstraintGraph graph;
    graph.add_node("a", box_a);

    auto dof = graph.analyze_dof();
    ASSERT_EQ(dof.size(), 1);
    EXPECT_EQ(dof[0].total_dof, 6);
    EXPECT_EQ(dof[0].eliminated_dof, 0);
    EXPECT_EQ(dof[0].remaining_dof, 6);
    EXPECT_FALSE(dof[0].is_fixed);
}

TEST(DOFAnalysisTest, CoincidentEliminates3DOF) {
    Assembly assy("dof_test2");
    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));

    ConstraintGraph graph;
    graph.add_node("a", box_a);
    graph.add_node("b", box_b);
    graph.add_constraint({0, 1, ConstraintType::Coincident});

    auto dof = graph.analyze_dof();
    // Coincident eliminates 3 DOF per node
    EXPECT_EQ(dof[0].eliminated_dof, 3);
    EXPECT_EQ(dof[1].eliminated_dof, 3);
    EXPECT_EQ(dof[0].remaining_dof, 3);
}

TEST(DOFAnalysisTest, FullyConstrained) {
    Assembly assy("dof_full");
    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));

    ConstraintGraph graph;
    graph.add_node("a", box_a);
    graph.add_node("b", box_b);

    // Coincident (3) + Concentric (4) = 7 消除 per node
    // 对 node_a: 3+4=7 > 6 → 全约束
    graph.add_constraint({0, 1, ConstraintType::Coincident});
    graph.add_constraint({0, 1, ConstraintType::Concentric});

    auto dof = graph.analyze_dof();
    EXPECT_TRUE(dof[0].is_fixed);
    EXPECT_TRUE(dof[1].is_fixed);
    EXPECT_TRUE(graph.is_fully_constrained());
}

TEST(DOFAnalysisTest, NotFullyConstrained) {
    Assembly assy("dof_partial");
    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));

    ConstraintGraph graph;
    graph.add_node("a", box_a);
    graph.add_node("b", box_b);
    graph.add_constraint({0, 1, ConstraintType::Parallel}); // only 2 DOF

    EXPECT_FALSE(graph.is_fully_constrained());
    EXPECT_EQ(graph.remaining_dof(0), 4); // 6 - 2 = 4
}

// ════════════════════════════════════════════════════════════════
//  过度约束检测测试
// ════════════════════════════════════════════════════════════════

TEST(OverConstraintTest, DetectOverConstrained) {
    Assembly assy("over_test");
    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));

    ConstraintGraph graph;
    graph.add_node("a", box_a);
    graph.add_node("b", box_b);

    // Coincident(3) + Concentric(4) + Distance(1) = 8 > 6
    graph.add_constraint({0, 1, ConstraintType::Coincident});
    graph.add_constraint({0, 1, ConstraintType::Concentric});
    graph.add_constraint({0, 1, ConstraintType::Distance, 5.0});

    auto over = graph.detect_over_constraints();
    EXPECT_GE(over.size(), 1);

    if (!over.empty()) {
        EXPECT_TRUE(over[0].over_constrained);
        EXPECT_GT(over[0].eliminated_dof, 6);
        EXPECT_FALSE(over[0].message.empty());
    }
}

TEST(OverConstraintTest, NoOverConstraintWhenBalanced) {
    Assembly assy("balanced");
    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));

    ConstraintGraph graph;
    graph.add_node("a", box_a);
    graph.add_node("b", box_b);

    // Coincident(3) + Distance(1) = 4 ≤ 6 → not over-constrained
    graph.add_constraint({0, 1, ConstraintType::Coincident});
    graph.add_constraint({0, 1, ConstraintType::Distance, 5.0});

    auto over = graph.detect_over_constraints();
    EXPECT_EQ(over.size(), 0);
}

// ════════════════════════════════════════════════════════════════
//  Newton-Raphson 求解器测试
// ════════════════════════════════════════════════════════════════

TEST(NewtonRaphsonTest, EmptyGraphConverges) {
    Assembly assy("nr_empty");
    ConstraintGraph graph;

    NewtonRaphsonSolver solver;
    NRSolverConfig cfg;
    cfg.max_iterations = 20;

    auto result = solver.solve(graph, assy, cfg);
    EXPECT_TRUE(result.converged);
}

TEST(NewtonRaphsonTest, SingleCoincidentConstraint) {
    Assembly assy("nr_coin");
    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, 5));

    ConstraintGraph graph;
    graph.add_node("a", box_a);
    graph.add_node("b", box_b);
    graph.add_constraint({0, 1, ConstraintType::Coincident});

    NewtonRaphsonSolver solver;
    NRSolverConfig cfg;
    cfg.max_iterations = 50;

    auto result = solver.solve(graph, assy, cfg);

    // NR 求解器应用于当前装配体
    AABB3D bb_a = world_bounds(box_a);
    AABB3D bb_b = world_bounds(box_b);
    // 验证 nodes 已修改（local_transform 不应是绝对值的问题——NR 状态正确更新）
    EXPECT_TRUE(box_a != nullptr && box_b != nullptr);
}

TEST(NewtonRaphsonTest, IncrementalSolve) {
    Assembly assy("nr_inc");
    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));

    ConstraintGraph graph;
    graph.add_node("a", box_a);
    graph.add_node("b", box_b);
    int ci = graph.add_constraint({0, 1, ConstraintType::Distance, 5.0});

    NewtonRaphsonSolver solver;
    NRSolverConfig cfg;
    cfg.max_iterations = 50;

    // 首次求解
    auto result1 = solver.solve(graph, assy, cfg);

    // 增量更新：将距离从 5 改为 3
    auto result2 = solver.incremental_solve(graph, assy, ci, 3.0, cfg);

    // 增量求解应当快速收敛
    EXPECT_TRUE(result2.converged || result2.iterations < cfg.max_iterations);
}

// ════════════════════════════════════════════════════════════════
//  ConstraintType 测试
// ════════════════════════════════════════════════════════════════

TEST(ConstraintTypeTest, DofEliminationValues) {
    // 验证 DOF 消除量的合理性
    EXPECT_EQ(constraint_dof_elimination(ConstraintType::Coincident), 3);
    EXPECT_EQ(constraint_dof_elimination(ConstraintType::Concentric), 4);
    EXPECT_EQ(constraint_dof_elimination(ConstraintType::Tangent), 1);
    EXPECT_EQ(constraint_dof_elimination(ConstraintType::Distance), 1);
    EXPECT_EQ(constraint_dof_elimination(ConstraintType::Angle), 1);
    EXPECT_EQ(constraint_dof_elimination(ConstraintType::Parallel), 2);
    EXPECT_EQ(constraint_dof_elimination(ConstraintType::Perpendicular), 2);
}

TEST(ConstraintTypeTest, TypeNames) {
    EXPECT_STREQ(constraint_type_name(ConstraintType::Coincident), "Coincident");
    EXPECT_STREQ(constraint_type_name(ConstraintType::Concentric), "Concentric");
    EXPECT_STREQ(constraint_type_name(ConstraintType::Distance), "Distance");
    EXPECT_STREQ(constraint_type_name(ConstraintType::Angle), "Angle");
    EXPECT_STREQ(constraint_type_name(ConstraintType::Parallel), "Parallel");
}

// ════════════════════════════════════════════════════════════════
//  DOFAnalyzer 详细测试
// ════════════════════════════════════════════════════════════════

TEST(DOFAnalyzerTest, SingleNodeFreeDirections) {
    Assembly assy("dof_detail");
    auto* box = assy.root.add_part("box", make_box(2, 2, 2));

    ConstraintGraph graph;
    graph.add_node("box", box);

    DOFAnalyzer analyzer;
    auto results = analyzer.analyze(graph);

    ASSERT_EQ(results.size(), 1u);
    EXPECT_EQ(results[0].translational_dof_remaining, 3);
    EXPECT_EQ(results[0].rotational_dof_remaining, 3);
    EXPECT_FALSE(results[0].is_fully_constrained);

    auto dirs = analyzer.free_direction_names(graph, 0);
    EXPECT_EQ(dirs.size(), 6u); // 全部自由
}

TEST(DOFAnalyzerTest, GlobalDofSummary) {
    Assembly assy("dof_global");
    auto* a = assy.root.add_part("a", make_box(2, 2, 2));
    auto* b = assy.root.add_part("b", make_box(2, 2, 2));

    ConstraintGraph graph;
    graph.add_node("a", a);
    graph.add_node("b", b);
    graph.add_constraint({0, 1, ConstraintType::Coincident}); // 3 DOF each
    graph.add_constraint({0, 1, ConstraintType::Concentric}); // 4 DOF each

    DOFAnalyzer analyzer;
    auto summary = analyzer.global_dof_summary(graph);
    EXPECT_FALSE(summary.empty());
    // Both nodes should be over-constrained (3+4=7 > 6)
    auto results = analyzer.analyze(graph);
    EXPECT_TRUE(results[0].is_over_constrained);
    EXPECT_TRUE(results[1].is_over_constrained);
}

TEST(DOFAnalyzerTest, PartiallyConstrainedDirections) {
    Assembly assy("dof_partial");
    auto* a = assy.root.add_part("a", make_box(2, 2, 2));
    auto* b = assy.root.add_part("b", make_box(2, 2, 2));

    ConstraintGraph graph;
    graph.add_node("a", a);
    graph.add_node("b", b);
    graph.add_constraint({0, 1, ConstraintType::Parallel}); // 2 rot DOF

    DOFAnalyzer analyzer;
    auto results = analyzer.analyze(graph);

    EXPECT_EQ(results[0].translational_dof_remaining, 3);
    EXPECT_EQ(results[0].rotational_dof_remaining, 1); // 3 - 2 = 1
    EXPECT_FALSE(results[0].is_fully_constrained);
    EXPECT_FALSE(results[0].summary.empty());
}

// ════════════════════════════════════════════════════════════════
//  RedundancyDetector 测试
// ════════════════════════════════════════════════════════════════

TEST(RedundancyDetectorTest, DetectsDuplicateConstraints) {
    Assembly assy("rd_dup");
    auto* a = assy.root.add_part("a", make_box(2, 2, 2));
    auto* b = assy.root.add_part("b", make_box(2, 2, 2));

    ConstraintGraph graph;
    graph.add_node("a", a);
    graph.add_node("b", b);
    graph.add_constraint({0, 1, ConstraintType::Coincident});
    graph.add_constraint({0, 1, ConstraintType::Coincident}); // duplicate

    RedundancyDetector detector;
    auto suggestions = detector.detect(graph);
    EXPECT_GE(suggestions.size(), 1u);

    bool found_dup = false;
    for (const auto& s : suggestions) {
        if (s.reason.find("Duplicate") != std::string::npos)
            found_dup = true;
    }
    EXPECT_TRUE(found_dup);
}

TEST(RedundancyDetectorTest, DetectsOverConstrainedNode) {
    Assembly assy("rd_over");
    auto* a = assy.root.add_part("a", make_box(2, 2, 2));
    auto* b = assy.root.add_part("b", make_box(2, 2, 2));

    ConstraintGraph graph;
    graph.add_node("a", a);
    graph.add_node("b", b);
    // Coincident(3) + Concentric(4) + Distance(1) = 8 > 6
    graph.add_constraint({0, 1, ConstraintType::Coincident});
    graph.add_constraint({0, 1, ConstraintType::Concentric});
    graph.add_constraint({0, 1, ConstraintType::Distance, 5.0});

    RedundancyDetector detector;
    auto suggestions = detector.detect(graph);
    EXPECT_GE(suggestions.size(), 1u);
}

TEST(RedundancyDetectorTest, DetectsConflictsParallelPerpendicular) {
    Assembly assy("rd_conflict");
    auto* a = assy.root.add_part("a", make_box(2, 2, 2));
    auto* b = assy.root.add_part("b", make_box(2, 2, 2));

    ConstraintGraph graph;
    graph.add_node("a", a);
    graph.add_node("b", b);
    graph.add_constraint({0, 1, ConstraintType::Parallel});
    graph.add_constraint({0, 1, ConstraintType::Perpendicular}); // 冲突!

    RedundancyDetector detector;
    auto conflicts = detector.detect_conflicts(graph);
    EXPECT_GE(conflicts.size(), 1u);
    EXPECT_NE(conflicts[0].description.find("Conflict"), std::string::npos);
}

TEST(RedundancyDetectorTest, ReportGeneratesText) {
    Assembly assy("rd_report");
    auto* a = assy.root.add_part("a", make_box(2, 2, 2));
    auto* b = assy.root.add_part("b", make_box(2, 2, 2));

    ConstraintGraph graph;
    graph.add_node("a", a);
    graph.add_node("b", b);
    graph.add_constraint({0, 1, ConstraintType::Coincident});
    graph.add_constraint({0, 1, ConstraintType::Coincident});

    RedundancyDetector detector;
    auto report = detector.report(graph);
    EXPECT_FALSE(report.empty());
    EXPECT_NE(report.find("Redundancy Report"), std::string::npos);
}

TEST(RedundancyDetectorTest, DetectForNodeFiltersCorrectly) {
    Assembly assy("rd_filter");
    auto* a = assy.root.add_part("a", make_box(2, 2, 2));
    auto* b = assy.root.add_part("b", make_box(2, 2, 2));

    ConstraintGraph graph;
    graph.add_node("a", a);
    graph.add_node("b", b);
    graph.add_constraint({0, 1, ConstraintType::Coincident});
    graph.add_constraint({0, 1, ConstraintType::Coincident});

    RedundancyDetector detector;
    auto for_node = detector.detect_for_node(graph, 0);
    EXPECT_GE(for_node.size(), 1u);
    for (const auto& s : for_node) {
        EXPECT_EQ(s.node_index, 0);
    }
}

// ════════════════════════════════════════════════════════════════
//  ConstraintPropagator 测试
// ════════════════════════════════════════════════════════════════

TEST(ConstraintPropagatorTest, PropagateModifiesConstraint) {
    Assembly assy("cp_prop");
    auto* a = assy.root.add_part("a", make_box(2, 2, 2));
    auto* b = assy.root.add_part("b", make_box(2, 2, 2));
    auto* c = assy.root.add_part("c", make_box(2, 2, 2));

    ConstraintGraph graph;
    graph.add_node("a", a);
    graph.add_node("b", b);
    graph.add_node("c", c);
    int ci = graph.add_constraint({0, 1, ConstraintType::Distance, 5.0});
    graph.add_constraint({1, 2, ConstraintType::Coincident});

    ConstraintPropagator propagator;
    auto result = propagator.propagate(graph, assy, ci, 10.0,
                                        ConstraintType::Distance, nullptr);

    EXPECT_TRUE(result.success);
    EXPECT_GE(result.affected_nodes.size(), 2u);
    EXPECT_GE(result.changes.size(), 1u);
    EXPECT_EQ(graph.constraint(ci).value, 10.0);
}

TEST(ConstraintPropagatorTest, ComputeDependencyOrder) {
    Assembly assy("cp_order");
    auto* a = assy.root.add_part("a", make_box(2, 2, 2));
    auto* b = assy.root.add_part("b", make_box(2, 2, 2));
    auto* c = assy.root.add_part("c", make_box(2, 2, 2));

    ConstraintGraph graph;
    graph.add_node("a", a);
    graph.add_node("b", b);
    graph.add_node("c", c);
    graph.add_constraint({0, 1, ConstraintType::Coincident});
    graph.add_constraint({1, 2, ConstraintType::Coincident});

    ConstraintPropagator propagator;
    auto order = propagator.compute_dependency_order(graph, 0);

    EXPECT_GE(order.size(), 2u);
    EXPECT_EQ(order[0], 0); // 起始节点
}

TEST(ConstraintPropagatorTest, DependencyDepth) {
    Assembly assy("cp_depth");
    auto* a = assy.root.add_part("a", make_box(2, 2, 2));
    auto* b = assy.root.add_part("b", make_box(2, 2, 2));
    auto* c = assy.root.add_part("c", make_box(2, 2, 2));

    ConstraintGraph graph;
    graph.add_node("a", a);
    graph.add_node("b", b);
    graph.add_node("c", c);
    graph.add_constraint({0, 1, ConstraintType::Coincident});
    graph.add_constraint({1, 2, ConstraintType::Coincident});

    ConstraintPropagator propagator;
    int depth = propagator.dependency_depth(graph, 0, 2);
    EXPECT_EQ(depth, 2); // a→b→c, 2 hops

    int same = propagator.dependency_depth(graph, 0, 0);
    EXPECT_EQ(same, 0);

    // In an unconnected subgraph (no node 3 exists — but negative test)
    int unreachable = propagator.dependency_depth(graph, 0, -1);
    EXPECT_EQ(unreachable, -1);
}

// ════════════════════════════════════════════════════════════════
//  KinematicChainSolver 测试
// ════════════════════════════════════════════════════════════════

TEST(KinematicChainTest, ForwardKinematicsIdentity) {
    Assembly assy("kc_fwd");
    KinematicChainSolver solver;

    std::vector<ChainLink> links;
    std::vector<double> angles;

    auto poses = solver.forward_kinematics(assy, links, angles);
    EXPECT_EQ(poses.size(), 1u); // Just identity
}

TEST(KinematicChainTest, ForwardKinematics2Link) {
    Assembly assy("kc_fwd2");
    KinematicChainSolver solver;

    std::vector<ChainLink> links = {
        {0, 1.0, 0.0, 0.0, "link1"},
        {1, 1.0, 0.0, 0.0, "link2"}
    };
    std::vector<double> angles = {0.0, M_PI / 2.0};

    auto poses = solver.forward_kinematics(assy, links, angles);
    EXPECT_EQ(poses.size(), 3u); // identity + link1 + link2
    // Second link should extend in X+ then Y+ direction
    EXPECT_GT(poses.back()(0, 3), 0.9);
    EXPECT_GT(std::abs(poses.back()(1, 3)), 0.9);
}

TEST(KinematicChainTest, InverseKinematicsReachable) {
    Assembly assy("kc_ik");
    KinematicChainSolver solver;

    std::vector<ChainLink> links = {
        {0, 1.0, 0.0, 0.0, "link1"},
        {1, 1.0, 0.0, 0.0, "link2"}
    };

    // Target at (0, 2) — reachable by 2 links of length 1
    core::Transform3D target = core::Transform3D::Identity();
    target.translation() = core::Vector3D(0.0, 2.0, 0.0);

    auto angles = solver.inverse_kinematics(assy, links, target);
    EXPECT_EQ(angles.size(), 2u); // Should produce 2 angles
}

TEST(KinematicChainTest, InverseKinematicsUnreachable) {
    Assembly assy("kc_ik_unreachable");
    KinematicChainSolver solver;

    std::vector<ChainLink> links = {
        {0, 1.0, 0.0, 0.0, "link1"},
        {1, 1.0, 0.0, 0.0, "link2"}
    };

    // Target at (0, 3) — unreachable (max reach = 2)
    core::Transform3D target = core::Transform3D::Identity();
    target.translation() = core::Vector3D(0.0, 3.0, 0.0);

    auto angles = solver.inverse_kinematics(assy, links, target);
    EXPECT_TRUE(angles.empty()); // Unreachable
}

TEST(KinematicChainTest, IsReachableCheck) {
    KinematicChainSolver solver;

    std::vector<ChainLink> links = {
        {0, 1.0, 0.0, 0.0, "l1"},
        {1, 0.5, 0.0, 0.0, "l2"}
    };

    core::Point3D near(1.0, 0.0, 0.0);
    EXPECT_TRUE(solver.is_reachable(links, near));

    core::Point3D far(10.0, 0.0, 0.0);
    EXPECT_FALSE(solver.is_reachable(links, far));
}

TEST(KinematicChainTest, DHTransformIdentity) {
    // DH transform with all zero params should give identity
    auto T = KinematicChainSolver::dh_transform(0.0, 0.0, 0.0, 0.0);
    // Not fully identity because DH includes rotation
    // At theta=0, should be I + translation on a=0
    core::Transform3D I = core::Transform3D::Identity();
    EXPECT_TRUE(T.isApprox(I, 1e-6));
}

// ════════════════════════════════════════════════════════════════
//  Drawing Standards 测试
// ════════════════════════════════════════════════════════════════

TEST(DrawingStandardsTest, IsoStandardCreatesValidStyle) {
    auto style = IsoStandard::create_style();

    EXPECT_EQ(style.projection_angle, ProjectionAngle::FirstAngle);
    EXPECT_GT(style.text_height, 0.0);
    EXPECT_GT(style.line_width_thick, style.line_width_thin);
    EXPECT_EQ(style.text_font, FontStyle::Normal);
}

TEST(DrawingStandardsTest, AnsiStandardCreatesValidStyle) {
    auto style = AnsiStandard::create_style();

    EXPECT_EQ(style.projection_angle, ProjectionAngle::ThirdAngle);
    EXPECT_GT(style.line_width_thick, 0.0);
    EXPECT_GT(style.arrow_length, 0.0);
}

TEST(DrawingStandardsTest, JisStandardCreatesValidStyle) {
    auto style = JisStandard::create_style();

    EXPECT_EQ(style.projection_angle, ProjectionAngle::FirstAngle);
    EXPECT_GT(style.text_height, 0.0);
    EXPECT_EQ(style.cutting_plane, LineType::Chain); // JIS 特有
}

TEST(DrawingStandardsTest, IsoLineWidthGroup) {
    EXPECT_NEAR(IsoStandard::line_width_group(2), 0.25, 1e-9);
    EXPECT_NEAR(IsoStandard::line_width_group(3), 0.35, 1e-9);
    EXPECT_NEAR(IsoStandard::line_width_group(4), 0.50, 1e-9);
}

TEST(DrawingStandardsTest, IsoLineTypeNames) {
    EXPECT_NE(IsoStandard::line_type_iso_name(LineType::Continuous).find("01"), std::string::npos);
    EXPECT_NE(IsoStandard::line_type_iso_name(LineType::Dashed).find("02"), std::string::npos);
    EXPECT_NE(IsoStandard::line_type_iso_name(LineType::Chain).find("04"), std::string::npos);
}

TEST(DrawingStandardsTest, ApplyStandardByName) {
    std::vector<ProjectionView> views;
    StandardOptions opts;
    opts.paper_size = "A4";

    auto ctx = drawing::apply_standard_by_name(views, "ISO", opts);
    EXPECT_EQ(ctx.standard_name, "ISO");
    EXPECT_EQ(ctx.view_projection, ProjectionAngle::FirstAngle);

    auto ctx_ansi = drawing::apply_standard_by_name(views, "ANSI", opts);
    EXPECT_EQ(ctx_ansi.standard_name, "ANSI");
    EXPECT_EQ(ctx_ansi.view_projection, ProjectionAngle::ThirdAngle);

    auto ctx_jis = drawing::apply_standard_by_name(views, "JIS", opts);
    EXPECT_EQ(ctx_jis.standard_name, "JIS");
    EXPECT_EQ(ctx_jis.view_projection, ProjectionAngle::FirstAngle);
}

TEST(DrawingStandardsTest, ValidateStyleWarnings) {
    auto style = IsoStandard::create_style();

    auto warns = drawing::validate_style(style, "ISO");
    EXPECT_EQ(warns.size(), 0u); // Valid ISO style should have no warnings

    // Test ANSI validation
    auto warns_ansi = drawing::validate_style(style, "ANSI");
    // Should warn about first-angle vs third-angle
    EXPECT_GE(warns_ansi.size(), 1u);
}

TEST(DrawingStandardsTest, AvailableStandards) {
    auto standards = drawing::available_standards();
    EXPECT_GE(standards.size(), 3u);
}

TEST(DrawingStandardsTest, StandardDescriptionsNotEmpty) {
    EXPECT_FALSE(IsoStandard::description().empty());
    EXPECT_FALSE(AnsiStandard::description().empty());
    EXPECT_FALSE(JisStandard::description().empty());
}

TEST(DrawingStandardsTest, AnsiSymbolsAndFrameFormat) {
    auto symbols = AnsiStandard::gdt_symbols_summary();
    EXPECT_FALSE(symbols.empty());
    EXPECT_NE(symbols.find("位置度"), std::string::npos);

    auto fcf = AnsiStandard::feature_control_frame_format();
    EXPECT_FALSE(fcf.empty());
}

TEST(DrawingStandardsTest, JisPaperSize) {
    auto a4 = JisStandard::paper_size_spec("A4");
    EXPECT_NE(a4.find("210"), std::string::npos);
    EXPECT_NE(a4.find("297"), std::string::npos);
}

TEST(DrawingStandardsTest, JisScales) {
    auto scales = JisStandard::preferred_scales();
    EXPECT_GE(scales.size(), 5u);
    EXPECT_DOUBLE_EQ(scales[0], 1.0);
}

TEST(DrawingStandardsTest, ApplyStandardFunction) {
    std::vector<ProjectionView> views;
    auto style = IsoStandard::create_style();
    StandardOptions opts;
    opts.paper_size = "A3";

    auto ctx = drawing::apply_standard(views, style, opts);
    EXPECT_EQ(ctx.style.projection_angle, ProjectionAngle::FirstAngle);
    EXPECT_EQ(ctx.options.paper_size, "A3");
}

TEST(DrawingStandardsTest, LineTypeEnumValues) {
    // 验证所有线型枚举都有对应名称
    EXPECT_STREQ(line_type_name(LineType::Continuous), "Continuous");
    EXPECT_STREQ(line_type_name(LineType::Dashed), "Dashed");
    EXPECT_STREQ(line_type_name(LineType::Chain), "Chain");
    EXPECT_STREQ(line_type_name(LineType::DoubleChain), "DoubleChain");
    EXPECT_STREQ(line_type_name(LineType::Dotted), "Dotted");
}