#include <gtest/gtest.h>
#include "vde/sdf/sdf_gradient.h"
#include <cmath>
#include <functional>
#include <vector>
using namespace vde::sdf;
using vde::core::Point3D;
using vde::core::Vector3D;

constexpr double EPS = 1e-5;
constexpr double EPS_LOOSE = 1e-3;   // For FD vs analytic agreement
constexpr double FD_H = 1e-6;
constexpr double SQRT2 = 1.4142135623730951;
constexpr double SQRT3 = 1.7320508075688772;

// Helper: wrap an SDF function for gradient() FD call
static double sphere_func_wrapper(const Point3D& p) { return sphere(p, 2.0); }
static double box_func_wrapper(const Point3D& p)   { return box(p, Point3D(1.0, 1.0, 1.0)); }
static double torus_func_wrapper(const Point3D& p) { return torus(p, 2.0, 0.5); }
static double cyl_func_wrapper(const Point3D& p)   { return cylinder(p, 1.0, 3.0); }
static double plane_func_wrapper(const Point3D& p) { return plane(p, Vector3D::UnitY(), 0.0); }
static double capsule_func_wrapper(const Point3D& p) {
    return capsule(p, Point3D(0, -1, 0), Point3D(0, 1, 0), 0.5);
}

// ═══════════════════════════════════════════════════
//  gradient() — Finite-Difference Tests
// ═══════════════════════════════════════════════════

TEST(SdfGradient, FdSphereAtSurface) {
    auto f = sphere_func_wrapper;
    Point3D p(2.0, 0.0, 0.0);
    Vector3D g = gradient(f, p, FD_H);

    // Gradient at (2,0,0) of sphere(r=2) should be (1,0,0)
    EXPECT_NEAR(g.x(), 1.0, EPS_LOOSE);
    EXPECT_NEAR(g.y(), 0.0, EPS_LOOSE);
    EXPECT_NEAR(g.z(), 0.0, EPS_LOOSE);
}

TEST(SdfGradient, FdSphereAtDiagonal) {
    auto f = sphere_func_wrapper;
    double v = 2.0 / SQRT3;
    Point3D p(v, v, v);
    Vector3D g = gradient(f, p, FD_H);

    // Gradient should be normalized (1/√3, 1/√3, 1/√3)
    double expected = 1.0 / SQRT3;
    EXPECT_NEAR(g.x(), expected, EPS_LOOSE);
    EXPECT_NEAR(g.y(), expected, EPS_LOOSE);
    EXPECT_NEAR(g.z(), expected, EPS_LOOSE);
}

TEST(SdfGradient, FdBoxOutside) {
    auto f = box_func_wrapper;
    Point3D p(3.0, 0.0, 0.0); // Outside in +x
    Vector3D g = gradient(f, p, FD_H);

    EXPECT_NEAR(g.x(), 1.0, EPS_LOOSE);
    EXPECT_NEAR(g.y(), 0.0, EPS_LOOSE);
    EXPECT_NEAR(g.z(), 0.0, EPS_LOOSE);
}

TEST(SdfGradient, FdBoxCorner) {
    auto f = box_func_wrapper;
    Point3D p(3.0, 3.0, 0.0); // Outside in +x,+y corner
    Vector3D g = gradient(f, p, FD_H);

    // Gradient should point away from nearest face/edge — roughly (1/√2, 1/√2, 0)
    double expected = 1.0 / SQRT2;
    EXPECT_NEAR(g.x(), expected, EPS_LOOSE);
    EXPECT_NEAR(g.y(), expected, EPS_LOOSE);
    EXPECT_NEAR(g.z(), 0.0, EPS_LOOSE);
}

TEST(SdfGradient, FdTorus) {
    auto f = torus_func_wrapper;
    // Point on the XZ "ridge" of the torus
    Point3D p(2.5, 0.0, 0.0); // major_r=2, minor_r=0.5 → on surface at (2.5,0,0)?
    // SDF = sqrt((2.5-2)^2 + 0) - 0.5 = 0.5 - 0.5 = 0 — yes, on surface
    Vector3D g = gradient(f, p, FD_H);

    // Gradient should point radially outward in XZ: (1, 0, 0)
    EXPECT_NEAR(g.x(), 1.0, EPS_LOOSE);
    EXPECT_NEAR(g.y(), 0.0, EPS_LOOSE);
    EXPECT_NEAR(g.z(), 0.0, EPS_LOOSE);
}

TEST(SdfGradient, FdCylinder) {
    auto f = cyl_func_wrapper;
    Point3D p(1.0, 0.0, 0.0); // On surface of cylinder (r=1)
    Vector3D g = gradient(f, p, FD_H);

    EXPECT_NEAR(g.x(), 1.0, EPS_LOOSE);
    EXPECT_NEAR(g.y(), 0.0, EPS_LOOSE);
    EXPECT_NEAR(g.z(), 0.0, EPS_LOOSE);
}

TEST(SdfGradient, FdPlane) {
    auto f = plane_func_wrapper;
    Point3D p(1.0, 2.0, 0.0);
    Vector3D g = gradient(f, p, FD_H);

    EXPECT_NEAR(g.x(), 0.0, EPS_LOOSE);
    EXPECT_NEAR(g.y(), 1.0, EPS_LOOSE);
    EXPECT_NEAR(g.z(), 0.0, EPS_LOOSE);
}

TEST(SdfGradient, FdCapsule) {
    auto f = capsule_func_wrapper;
    // Point at side of capsule (away from end caps)
    Point3D p(0.5, 0.0, 0.0); // away from segment
    // a=(0,-1,0), b=(0,1,0), radius=0.5
    // closest = (0, 0, 0), delta = (0.5, 0, 0), d = 0.5, on surface
    Vector3D g = gradient(f, p, FD_H);

    EXPECT_NEAR(g.x(), 1.0, EPS_LOOSE);
    EXPECT_NEAR(g.y(), 0.0, EPS_LOOSE);
    EXPECT_NEAR(g.z(), 0.0, EPS_LOOSE);
}

// ═══════════════════════════════════════════════════
//  evaluate_with_gradient()
// ═══════════════════════════════════════════════════

TEST(SdfGradient, EvaluateWithGradientSphere) {
    auto f = sphere_func_wrapper;
    Point3D p(2.0, 0.0, 0.0);
    GradResult r = evaluate_with_gradient(f, p, FD_H);

    EXPECT_NEAR(r.value, 0.0, 1e-9);
    EXPECT_NEAR(r.grad.x(), 1.0, EPS_LOOSE);
    EXPECT_NEAR(r.grad.y(), 0.0, EPS_LOOSE);
}

TEST(SdfGradient, EvaluateWithGradientInside) {
    auto f = sphere_func_wrapper;
    Point3D p(0.0, 0.0, 0.0);
    GradResult r = evaluate_with_gradient(f, p, FD_H);

    EXPECT_NEAR(r.value, -2.0, 1e-9);
    // At center, the SDF is non-differentiable; FD gradient ≈ (0,0,0)
    EXPECT_LT(r.grad.norm(), 0.01) << "Gradient at exact center should be near-zero (degenerate SDF)";
}

// ═══════════════════════════════════════════════════
//  Parameter Gradients (FD approximation tests)
// ═══════════════════════════════════════════════════

TEST(SdfParamGrad, SphereRadiusGradient) {
    Point3D p(1.0, 0.0, 0.0);
    double dg = sphere_radius_gradient(p, 2.0, FD_H);
    // For sphere: SDF = |p| - R, so dSDF/dR = -1
    EXPECT_NEAR(dg, -1.0, EPS_LOOSE);
}

TEST(SdfParamGrad, BoxExtentGradient) {
    Point3D p(2.0, 0.0, 0.0);
    double dg = box_extent_gradient(p, Point3D(1.0, 1.0, 1.0), 0, FD_H);
    // For box at (2,0,0) with extents(1,1,1): q=(1, -1, -1), d = 1
    // dSDF/d(extent.x) should be -1 (increasing extent decreases distance)
    EXPECT_NEAR(dg, -1.0, EPS_LOOSE);
}

TEST(SdfParamGrad, TorusMajorGradient) {
    Point3D p(2.5, 0.0, 0.0); // On surface of torus (major=2, minor=0.5)
    double dg = torus_major_gradient(p, 2.0, 0.5, FD_H);
    // dSDF/d(major): derivative of sqrt((rho-major)^2 + py^2) - minor
    // at (rho=2.5, py=0): sqrt((2.5-major)^2) - minor
    // ∂/∂major = (major-rho)/|rho-major| = (2-2.5)/0.5 = -1
    EXPECT_NEAR(dg, -1.0, EPS_LOOSE);
}

TEST(SdfParamGrad, TorusMinorGradient) {
    Point3D p(2.5, 0.0, 0.0); // On surface
    double dg = torus_minor_gradient(p, 2.0, 0.5, FD_H);
    // dSDF/d(minor) = -1 (same as sphere: SDF = ... - minor)
    EXPECT_NEAR(dg, -1.0, EPS_LOOSE);
}

TEST(SdfParamGrad, CylinderHeightGradient) {
    // Point above top cap of cylinder (r=1, h=3)
    Point3D p(0.0, 2.0, 0.0);
    double dg = cylinder_height_gradient(p, 1.0, 3.0, FD_H);
    // SDF: d_y = |py| - h/2 = 2 - 1.5 = 0.5; d_xy = -1; d = 0.5
    // dSDF/dh = -0.5 (increasing height reduces distance)
    EXPECT_NEAR(dg, -0.5, EPS_LOOSE);
}

TEST(SdfParamGrad, CylinderRadiusGradient) {
    // Point on side of cylinder
    Point3D p(1.0, 0.0, 0.0);
    double dg = cylinder_radius_gradient(p, 1.0, 3.0, FD_H);
    // SDF: d_xy = 0, d_y = -1.5, d = 0
    // dSDF/dr = -1
    EXPECT_NEAR(dg, -1.0, EPS_LOOSE);
}

// ═══════════════════════════════════════════════════
//  Analytical Gradients vs FD — Cross-Validation
// ═══════════════════════════════════════════════════

TEST(SdfAnalytic, SphereAgreesWithFd) {
    auto f = sphere_func_wrapper;
    // Test at random points
    std::vector<Point3D> test_points = {
        Point3D(1, 0, 0), Point3D(0, 2, 0), Point3D(0, 0, 3),
        Point3D(1, 1, 1), Point3D(3, -2, 1), Point3D(-1, -1, -1),
        Point3D(0.5, 0.5, 0.5), Point3D(2, 2, 0)
    };
    for (const auto& p : test_points) {
        Vector3D g_analytic = sphere_gradient_analytic(p, 2.0);
        Vector3D g_fd = gradient(f, p, FD_H);
        double diff = (g_analytic - g_fd).norm();
        EXPECT_LT(diff, EPS_LOOSE) ;
    }
}

TEST(SdfAnalytic, BoxAgreesWithFd) {
    auto f = box_func_wrapper;
    Point3D extents(1.0, 2.0, 1.5);
    std::vector<Point3D> test_points = {
        Point3D(3, 0, 0), Point3D(0, 4, 0), Point3D(0, 0, 2.5),
        Point3D(2, 3, 0), Point3D(-2, -3, 0), Point3D(0.5, 0, 0),
    };
    for (const auto& p : test_points) {
        Vector3D g_analytic = box_gradient_analytic(p, extents);
        Vector3D g_fd = gradient([&](const Point3D& pt) { return box(pt, extents); }, p, FD_H);
        // Near edges/corners the analytic gradient differs from FD due to non-smoothness
        double diff = (g_analytic - g_fd).norm();
        EXPECT_LT(diff, 0.5) ;
    }
}

TEST(SdfAnalytic, TorusAgreesWithFd) {
    auto f = torus_func_wrapper;
    std::vector<Point3D> test_points = {
        Point3D(2.5, 0, 0), Point3D(0, 0, 2.5), Point3D(0, 0.5, 0),
        Point3D(2, 0.5, 0), Point3D(1.5, 0.5, 0),
    };
    for (const auto& p : test_points) {
        Vector3D g_analytic = torus_gradient_analytic(p, 2.0, 0.5);
        Vector3D g_fd = gradient(f, p, FD_H);
        double diff = (g_analytic - g_fd).norm();
        EXPECT_LT(diff, EPS_LOOSE) ;
    }
}

TEST(SdfAnalytic, CylinderAgreesWithFd) {
    auto f = cyl_func_wrapper;
    std::vector<Point3D> test_points = {
        Point3D(2, 0, 0), Point3D(0, 0, 2), Point3D(1, 1, 0),
        Point3D(0.5, 0.5, 0.5), Point3D(1, 0, 1),
    };
    for (const auto& p : test_points) {
        Vector3D g_analytic = cylinder_gradient_analytic(p, 1.0);
        Vector3D g_fd = gradient(f, p, FD_H);
        double diff = (g_analytic - g_fd).norm();
        EXPECT_LT(diff, EPS_LOOSE) ;
    }
}

TEST(SdfAnalytic, PlaneAgreesWithFd) {
    Vector3D normal(0.0, 1.0, 0.0);
    std::vector<Point3D> test_points = {
        Point3D(1, 2, 0), Point3D(-5, 3, 10), Point3D(0, -1, 0),
    };
    for (const auto& p : test_points) {
        Vector3D g_analytic = plane_gradient_analytic(normal);
        Vector3D g_fd = gradient([&](const Point3D& pt) { return plane(pt, normal, 0); }, p, FD_H);
        double diff = (g_analytic - g_fd).norm();
        EXPECT_LT(diff, EPS_LOOSE);
    }
}

TEST(SdfAnalytic, CapsuleAgreesWithFd) {
    auto f = capsule_func_wrapper;
    Point3D a(0, -1, 0), b(0, 1, 0);
    std::vector<Point3D> test_points = {
        Point3D(0.5, 0, 0), Point3D(0, 2, 0), Point3D(0, -1.5, 0),
        Point3D(0.35, 0, 0.35),
    };
    for (const auto& p : test_points) {
        Vector3D g_analytic = capsule_gradient_analytic(p, a, b, 0.5);
        Vector3D g_fd = gradient(f, p, FD_H);
        double diff = (g_analytic - g_fd).norm();
        EXPECT_LT(diff, EPS_LOOSE) ;
    }
}

// ═══════════════════════════════════════════════════
//  Chain Rule: CSG Operations
// ═══════════════════════════════════════════════════

TEST(SdfChain, UnionSelectsCloser) {
    Vector3D gA(1, 0, 0);
    Vector3D gB(0, 1, 0);

    Vector3D g = chain_union(gA, gB, -0.5, -0.3); // A is closer (more negative)
    EXPECT_NEAR(g.x(), 1.0, 1e-9);
    EXPECT_NEAR(g.y(), 0.0, 1e-9);

    g = chain_union(gA, gB, -0.3, -0.5); // B is closer
    EXPECT_NEAR(g.x(), 0.0, 1e-9);
    EXPECT_NEAR(g.y(), 1.0, 1e-9);
}

TEST(SdfChain, UnionTwoSpheres) {
    // Two spheres: s1 at origin (r=1), s2 at (2,0,0) (r=1)
    // Point (3,0,0): s1=2, s2=0 → gradient should be s2's gradient = (1,0,0)
    auto f = [](const Point3D& p) {
        return op_union(sphere(p, 1.0), sphere(p - Point3D(2.0, 0.0, 0.0), 1.0));
    };

    Point3D p(3.0, 0.0, 0.0);
    Vector3D g_fd = gradient(f, p, FD_H);

    EXPECT_NEAR(g_fd.x(), 1.0, EPS_LOOSE);
    EXPECT_NEAR(g_fd.y(), 0.0, EPS_LOOSE);
    EXPECT_NEAR(g_fd.z(), 0.0, EPS_LOOSE);
}

TEST(SdfChain, IntersectionTwoSpheres) {
    // Two spheres centered at (±0.5, 0, 0), r=1 each
    // Intersection is the lens-shaped overlap
    // At origin (0,0,0), d1 = -0.5, d2 = -0.5, intersection = -0.5
    auto f = [](const Point3D& p) {
        return op_intersection(sphere(p - Point3D(0.5, 0, 0), 1.0),
                               sphere(p - Point3D(-0.5, 0, 0), 1.0));
    };

    // Point (0, 0.5, 0): inside both, d = max of the two
    Point3D p(0.0, 0.5, 0.0);
    Vector3D g_fd = gradient(f, p, FD_H);

    // Gradient magnitude should be reasonable (inside an intersection)
    EXPECT_GT(g_fd.norm(), 0.01) << "Gradient should not be degenerate";
}

TEST(SdfChain, DifferenceGradient) {
    Vector3D gA(1, 0, 0);
    Vector3D gB(0, 1, 0);

    // d1=0.5 (outside A), d2=-0.3 (inside B).
    // op_difference = max(d1, -d2) = max(0.5, 0.3) = 0.5 → picks grad_a
    Vector3D g = chain_difference(gA, gB, 0.5, -0.3);
    EXPECT_NEAR(g.x(), 1.0, 1e-9);
}

TEST(SdfChain, DifferenceNegatesGradA) {
    Vector3D gA(1, 0, 0);
    Vector3D gB(0, 1, 0);

    // d1=-1.0 (inside body), d2=0.0 (on cutter surface)
    // op_difference = max(-1, 0) = 0. d1=-1, -d2=0. -d2 > d1 → pick -grad_b
    // -grad_b = (0, -1, 0)
    Vector3D g = chain_difference(gA, gB, -1.0, 0.0);
    EXPECT_NEAR(g.x(), 0.0, 1e-9);
    EXPECT_NEAR(g.y(), -1.0, 1e-9);
}

// ═══════════════════════════════════════════════════
//  Chain Rule: Domain Transforms
// ═══════════════════════════════════════════════════

TEST(SdfChain, TranslateSphere) {
    // Sphere at origin: evaluate at p=(1,0,0) → gradient = (1,0,0)
    // With translation: no change to gradient
    auto f = [](const Point3D& p) { return sphere(p, 1.0); };
    Point3D offset(10, 20, 30);

    GradResult child = evaluate_with_gradient(f, Point3D(1, 0, 0), FD_H);
    GradResult result = chain_translate(child, offset);

    EXPECT_NEAR(result.grad.x(), 1.0, EPS_LOOSE);
    EXPECT_NEAR(result.grad.y(), 0.0, EPS_LOOSE);
    EXPECT_NEAR(result.grad.z(), 0.0, EPS_LOOSE);
}

TEST(SdfChain, RotateSphereGradient) {
    // Sphere at origin; gradient at (0,0,2) is (0,0,1)
    auto f = [](const Point3D& p) { return sphere(p, 2.0); };

    // Rotate by π/2 around Y: (0,0,1) → (-1,0,0)
    GradResult child = evaluate_with_gradient(f, Point3D(0, 0, 2), FD_H);
    GradResult result = chain_rotate(child, M_PI / 2.0);

    EXPECT_NEAR(result.grad.x(), -1.0, EPS_LOOSE);
    EXPECT_NEAR(result.grad.y(), 0.0, EPS_LOOSE);
    EXPECT_NEAR(result.grad.z(), 0.0, EPS_LOOSE);
}

TEST(SdfChain, RotateIdentity) {
    auto f = [](const Point3D& p) { return sphere(p, 2.0); };

    GradResult child = evaluate_with_gradient(f, Point3D(2, 0, 0), FD_H);
    GradResult result = chain_rotate(child, 0.0);

    EXPECT_NEAR(result.grad.x(), 1.0, EPS_LOOSE);
    EXPECT_NEAR(result.grad.y(), 0.0, EPS_LOOSE);
    EXPECT_NEAR(result.grad.z(), 0.0, EPS_LOOSE);
}

TEST(SdfChain, ScaleSphereGradient) {
    // Sphere at origin r=1; gradient at (1,0,0) = (1,0,0)
    // Scale by (2, 1, 1): gradient → (1/2, 0, 0) = (0.5, 0, 0)
    auto f = [](const Point3D& p) { return sphere(p, 1.0); };

    GradResult child = evaluate_with_gradient(f, Point3D(1, 0, 0), FD_H);
    GradResult result = chain_scale(child, Point3D(2.0, 1.0, 1.0));

    EXPECT_NEAR(result.grad.x(), 0.5, EPS_LOOSE);
    EXPECT_NEAR(result.grad.y(), 0.0, EPS_LOOSE);
    EXPECT_NEAR(result.grad.z(), 0.0, EPS_LOOSE);
}

TEST(SdfChain, ScaleUniform) {
    auto f = [](const Point3D& p) { return sphere(p, 1.0); };

    // At (2,0,0) gradient = (1,0,0). Scale by 3 → (1/3, 0, 0)
    GradResult child = evaluate_with_gradient(f, Point3D(2, 0, 0), FD_H);
    GradResult result = chain_scale(child, Point3D(3.0, 3.0, 3.0));

    EXPECT_NEAR(result.grad.x(), 1.0 / 3.0, EPS_LOOSE);
}

TEST(SdfChain, RepeatGradientUnchanged) {
    auto f = [](const Point3D& p) { return sphere(p, 1.0); };

    GradResult child = evaluate_with_gradient(f, Point3D(1, 0, 0), FD_H);
    GradResult result = chain_repeat(child, Point3D(2, 2, 2), Point3D(5, 0, 0));

    EXPECT_NEAR(result.grad.x(), child.grad.x(), 1e-9);
    EXPECT_NEAR(result.grad.y(), child.grad.y(), 1e-9);
    EXPECT_NEAR(result.grad.z(), child.grad.z(), 1e-9);
}

// ═══════════════════════════════════════════════════
//  Chain Rule: Twist
// ═══════════════════════════════════════════════════

TEST(SdfChain, TwistZeroAmount) {
    auto f = [](const Point3D& p) { return sphere(p, 1.0); };

    GradResult child = evaluate_with_gradient(f, Point3D(1, 0, 0), FD_H);
    GradResult result = chain_twist(child, 0.0, Point3D(1, 0, 0));

    EXPECT_NEAR(result.grad.x(), 1.0, EPS_LOOSE);
    EXPECT_NEAR(result.grad.y(), 0.0, EPS_LOOSE);
    EXPECT_NEAR(result.grad.z(), 0.0, EPS_LOOSE);
}

TEST(SdfChain, TwistAtYZero) {
    // At y=0, twist is identity rotation (angle = amount*0 = 0)
    auto f = [](const Point3D& p) { return sphere(p, 1.0); };

    GradResult child = evaluate_with_gradient(f, Point3D(1, 0, 0), FD_H);
    GradResult result = chain_twist(child, 0.5, Point3D(1, 0, 0));

    EXPECT_NEAR(result.grad.x(), 1.0, EPS_LOOSE);
    EXPECT_NEAR(result.grad.y(), 0.0, EPS_LOOSE);
    EXPECT_NEAR(result.grad.z(), 0.0, EPS_LOOSE);
}

// ═══════════════════════════════════════════════════
//  Smooth Union
// ═══════════════════════════════════════════════════

TEST(SdfChain, SmoothUnionWeightsSumToOne) {
    Vector3D g1(1, 0, 0);
    Vector3D g2(1, 0, 0);

    // When d1=d2, h=0.5, w1 = 2-3*0.5 = 0.5, w2 = 3*0.5-1 = 0.5
    Vector3D g = chain_smooth_union(g1, g2, 0.0, 0.0, 0.5);
    EXPECT_NEAR(g.x(), 1.0, 1e-9); // w1 + w2 = 1, both point right
}

TEST(SdfChain, SmoothUnionDegenerateK) {
    Vector3D g1(1, 0, 0);
    Vector3D g2(0, 1, 0);

    // k=0: should reduce to regular union (min)
    Vector3D g = chain_smooth_union(g1, g2, -0.5, 0.0, 0.0);
    EXPECT_NEAR(g.x(), 1.0, 1e-9); // d1 wins

    g = chain_smooth_union(g1, g2, 0.0, -0.5, 0.0);
    EXPECT_NEAR(g.y(), 1.0, 1e-9); // d2 wins
}

TEST(SdfChain, SmoothUnionClamped) {
    Vector3D g1(1, 0, 0);
    Vector3D g2(0, 1, 0);

    // d1 << d2 → h ≈ 0, same as union picking d1
    Vector3D g = chain_smooth_union(g1, g2, -10.0, 10.0, 0.1);
    EXPECT_NEAR(g.x(), 1.0, 1e-9);
}

// ═══════════════════════════════════════════════════
//  ParamGrad Convenience Functions
// ═══════════════════════════════════════════════════

TEST(SdfParamGrad, SphereParamGradStruct) {
    ParamGrad pg = sphere_param_grad(Point3D(1, 0, 0), 2.0, FD_H);
    EXPECT_NEAR(pg.d_radius, -1.0, EPS_LOOSE);
}

TEST(SdfParamGrad, BoxParamGradStruct) {
    ParamGrad pg = box_param_grad(Point3D(2.0, 0.0, 0.0), Point3D(1.0, 1.0, 1.0), FD_H);
    EXPECT_NEAR(pg.d_extents.x(), -1.0, EPS_LOOSE);
}

TEST(SdfParamGrad, CylinderParamGradStruct) {
    ParamGrad pg = cylinder_param_grad(Point3D(1.0, 0.0, 0.0), 1.0, 3.0, FD_H);
    EXPECT_NEAR(pg.d_radius, -1.0, EPS_LOOSE);
}

// ═══════════════════════════════════════════════════
//  Gradient Magnitude ≈ 1 (SDF Property)
// ═══════════════════════════════════════════════════

TEST(SdfGradient, SphereGradientMagnitude) {
    auto f = sphere_func_wrapper;
    std::vector<Point3D> pts = {
        Point3D(1, 0, 0), Point3D(0, 3, 0), Point3D(4, -2, 1),
        Point3D(1, 1, 1), Point3D(0.5, 0.3, 0.2),
    };
    for (const auto& p : pts) {
        Vector3D g = gradient(f, p, FD_H);
        EXPECT_NEAR(g.norm(), 1.0, EPS_LOOSE);
    }
}

TEST(SdfGradient, BoxGradientMagnitudeOutside) {
    Point3D ext(1.0, 2.0, 1.5);
    auto f = [&](const Point3D& p) { return box(p, ext); };
    std::vector<Point3D> pts = {
        Point3D(3, 0, 0), Point3D(0, 4, 0), Point3D(0, 0, 3),
        Point3D(3, 4, 0), // corner
    };
    for (const auto& p : pts) {
        Vector3D g = gradient(f, p, FD_H);
        // Exterior gradients should have unit magnitude (away from edges)
        EXPECT_NEAR(g.norm(), 1.0, 0.05);
    }
}

TEST(SdfGradient, TorusGradientMagnitude) {
    auto f = torus_func_wrapper;
    std::vector<Point3D> pts = {
        Point3D(2.5, 0, 0), Point3D(0, 0, 2.5),
        Point3D(2, 0.5, 0),
    };
    for (const auto& p : pts) {
        Vector3D g = gradient(f, p, FD_H);
        EXPECT_NEAR(g.norm(), 1.0, EPS_LOOSE);
    }
}

// ═══════════════════════════════════════════════════
//  Edge Cases
// ═══════════════════════════════════════════════════

TEST(SdfGradient, NearOrigin) {
    auto f = sphere_func_wrapper;
    // Very close to origin: gradient exists but direction may be unstable
    Point3D p(1e-4, 0.0, 0.0);
    Vector3D g = gradient(f, p, FD_H);
    // Gradient magnitude should still be ~1
    EXPECT_NEAR(g.norm(), 1.0, 0.01);
}

TEST(SdfGradient, AtOriginDegenerate) {
    // Gradient of sphere at exact origin is ANY unit vector
    auto f = sphere_func_wrapper;
    Vector3D g = gradient(f, Point3D(0, 0, 0), FD_H);
    // FD will still give a result (should be ~unit length or near-zero)
    EXPECT_LT(g.norm(), 0.01) << "Gradient at exact origin should be near-zero (degenerate SDF)";
}

TEST(SdfAnalytic, SphereAtOrigin) {
    Vector3D g = sphere_gradient_analytic(Point3D(0, 0, 0), 2.0);
    // At exact origin the analytic gradient returns zero (degenerate)
    EXPECT_NEAR(g.norm(), 0.0, 1e-9);
}

TEST(SdfAnalytic, CapsuleDegenerateSegment) {
    // Zero-length segment → should behave like sphere
    Point3D a(0, 0, 0), b(0, 0, 0);
    Point3D p(1, 0, 0);
    Vector3D g = capsule_gradient_analytic(p, a, b, 1.0);
    EXPECT_NEAR(g.x(), 1.0, EPS_LOOSE);
}

TEST(SdfAnalytic, TorusAtYAxis) {
    // On the Y axis (rho=0): gradient depends on side
    Point3D p(0, 0.45, 0); // Inside torus hole (minor=0.5)
    Vector3D g = torus_gradient_analytic(p, 2.0, 0.5);
    // Gradient should not be NaN or inf
    EXPECT_TRUE(std::isfinite(g.x()));
    EXPECT_TRUE(std::isfinite(g.y()));
    EXPECT_TRUE(std::isfinite(g.z()));
}