#include <gtest/gtest.h>
#include "vde/sdf/sdf_primitives.h"
#include <cmath>

using namespace vde::sdf;
using vde::core::Point3D;
using vde::core::Vector3D;

constexpr double EPS = 1e-9;
constexpr double SQRT2 = 1.4142135623730951;
constexpr double SQRT3 = 1.7320508075688772;

// ═══════════════════════════════════════════════════
//  sphere
// ═══════════════════════════════════════════════════

TEST(SdfSphere, Outside) {
    EXPECT_NEAR(sphere(Point3D(5, 0, 0), 3.0), 2.0, EPS);
    EXPECT_NEAR(sphere(Point3D(0, 0, 0), 3.0), -3.0, EPS);
}

TEST(SdfSphere, OnSurface) {
    EXPECT_NEAR(sphere(Point3D(3, 0, 0), 3.0), 0.0, EPS);
    EXPECT_NEAR(sphere(Point3D(0, -3, 0), 3.0), 0.0, EPS);
}

TEST(SdfSphere, Inside) {
    // Center of a radius-5 sphere: distance should be -5
    EXPECT_NEAR(sphere(Point3D(0, 0, 0), 5.0), -5.0, EPS);
    // Partway in
    EXPECT_NEAR(sphere(Point3D(2, 0, 0), 5.0), -3.0, EPS);
}

TEST(SdfSphere, Degenerate) {
    // Zero radius
    EXPECT_NEAR(sphere(Point3D(1, 0, 0), 0.0), 1.0, EPS);
    EXPECT_NEAR(sphere(Point3D(0, 0, 0), 0.0), 0.0, EPS);
}

// ═══════════════════════════════════════════════════
//  box
// ═══════════════════════════════════════════════════

TEST(SdfBox, Outside) {
    Point3D b(1, 1, 1);
    // Outside corner
    double d = box(Point3D(2, 2, 2), b);
    EXPECT_GT(d, 0.0);
    EXPECT_NEAR(d, std::sqrt(3.0), EPS);
}

TEST(SdfBox, OnSurface) {
    Point3D b(1, 1, 1);
    EXPECT_NEAR(box(Point3D(1, 0, 0), b), 0.0, EPS);
    EXPECT_NEAR(box(Point3D(0, 1, 0), b), 0.0, EPS);
    EXPECT_NEAR(box(Point3D(0, 0, 1), b), 0.0, EPS);
    EXPECT_NEAR(box(Point3D(-1, 0, 0), b), 0.0, EPS);
}

TEST(SdfBox, Inside) {
    Point3D b(3, 3, 3);
    // Center
    EXPECT_NEAR(box(Point3D(0, 0, 0), b), -3.0, EPS);
    // Mid-edge
    double d = box(Point3D(2, 0, 0), b);
    EXPECT_NEAR(d, -1.0, EPS);
}

TEST(SdfBox, Degenerate) {
    Point3D zero(0, 0, 0);
    double d = box(Point3D(1, 1, 1), zero);
    EXPECT_NEAR(d, std::sqrt(3.0), EPS);
}

// ═══════════════════════════════════════════════════
//  round_box
// ═══════════════════════════════════════════════════

TEST(SdfRoundBox, MatchesBoxAtZeroRadius) {
    Point3D b(2, 3, 4);
    for (double x = -5; x <= 5; x += 2.5) {
        for (double y = -5; y <= 5; y += 2.5) {
            for (double z = -5; z <= 5; z += 2.5) {
                Point3D pt(x, y, z);
                EXPECT_NEAR(round_box(pt, b, 0.0), box(pt, b), EPS);
            }
        }
    }
}

TEST(SdfRoundBox, RoundedShiftsBoundary) {
    Point3D b(1, 1, 1);
    // On surface of round box with r=0.5: outside moves in by 0.5
    double d_box = box(Point3D(1.5, 0, 0), b);
    double d_rbox = round_box(Point3D(1.5, 0, 0), b, 0.5);
    EXPECT_NEAR(d_rbox, d_box - 0.5, EPS);
}

// ═══════════════════════════════════════════════════
//  torus
// ═══════════════════════════════════════════════════

TEST(SdfTorus, OnSurfaceMajor) {
    // Point on major circle
    double d = torus(Point3D(5, 0, 0), 5.0, 1.0);
    EXPECT_NEAR(d, -1.0, EPS); // inside tube, at center of tube cross-section
}

TEST(SdfTorus, RingCenter_Inside) {
    // Center of torus (inside the hole)
    double d = torus(Point3D(0, 0, 0), 5.0, 1.0);
    EXPECT_NEAR(d, 4.0, EPS); // distance from origin to tube center = 5, minus tube radius = 4
}

TEST(SdfTorus, OutsideTube) {
    // Outside the whole shape
    double d = torus(Point3D(0, 3, 0), 5.0, 1.0);
    // Point (0,3,0): distance from Y axis = 0, so tube center at (5,0,0)
    // Distance to tube center: sqrt(25 + 9) = sqrt(34) ≈ 5.83, minus 1 = 4.83
    EXPECT_NEAR(d, std::sqrt(34.0) - 1.0, EPS);
}

TEST(SdfTorus, Degenerate) {
    double d = torus(Point3D(3, 0, 0), 3.0, 0.0);
    EXPECT_NEAR(d, 0.0, EPS);
}

// ═══════════════════════════════════════════════════
//  capsule
// ═══════════════════════════════════════════════════

TEST(SdfCapsule, OnAxis) {
    Point3D a(0, 0, 0), b(0, 4, 0);
    double r = 1.0;
    EXPECT_NEAR(capsule(Point3D(0, 0, 0), a, b, r), -r, EPS);
    EXPECT_NEAR(capsule(Point3D(0, 0, 1.0), a, b, r), 0.0, EPS);
    EXPECT_NEAR(capsule(Point3D(0, 0, 0), a, b, 0.0), 0.0, EPS);
}

TEST(SdfCapsule, OutsideCylinder) {
    Point3D a(0, 0, 0), b(0, 4, 0);
    double r = 1.0;
    double d = capsule(Point3D(3, 2, 0), a, b, r);
    EXPECT_NEAR(d, 2.0, EPS);
}

TEST(SdfCapsule, Midpoint) {
    Point3D a(0, 0, 0), b(2, 0, 0);
    double r = 0.5;
    EXPECT_NEAR(capsule(Point3D(1, 0, 0), a, b, r), -0.5, EPS);
    EXPECT_NEAR(capsule(Point3D(1, 0.5, 0), a, b, r), 0.0, EPS);
}

TEST(SdfCapsule, Degenerate) {
    Point3D pt(0, 0, 0);
    // Zero-length capsule = sphere
    double d = capsule(Point3D(2, 0, 0), pt, pt, 3.0);
    EXPECT_NEAR(d, -1.0, EPS);
}

// ═══════════════════════════════════════════════════
//  cylinder
// ═══════════════════════════════════════════════════

TEST(SdfCylinder, OnSurface) {
    double d = cylinder(Point3D(2, 0, 0), 2.0, 4.0);
    EXPECT_NEAR(d, 0.0, EPS);
}

TEST(SdfCylinder, Inside) {
    double d = cylinder(Point3D(0, 0, 0), 2.0, 4.0);
    EXPECT_NEAR(d, -2.0, EPS);
}

TEST(SdfCylinder, OutsideCap) {
    // Above top cap
    double d = cylinder(Point3D(0, 3, 0), 1.0, 4.0);
    EXPECT_NEAR(d, 1.0, EPS);
}

TEST(SdfCylinder, OutsideSide) {
    // Outside the side
    double d = cylinder(Point3D(3, 0, 0), 1.0, 4.0);
    EXPECT_NEAR(d, 2.0, EPS);
}

TEST(SdfCylinder, Degenerate) {
    // Zero radius = line segment
    double d = cylinder(Point3D(0, 0, 0), 0.0, 4.0);
    EXPECT_NEAR(d, 0.0, EPS);
    // Zero height
    double d2 = cylinder(Point3D(0, 0, 0), 2.0, 0.0);
    EXPECT_NEAR(d2, 0.0, EPS);
}

// ═══════════════════════════════════════════════════
//  cone
// ═══════════════════════════════════════════════════

TEST(SdfCone, Apex) {
    // Apex at y = height/2 = 2
    double d = cone(Point3D(0, 2, 0), std::atan(0.5), 4.0);
    EXPECT_NEAR(d, 0.0, EPS);
}

TEST(SdfCone, OnBaseEdge) {
    // Base at y = -height/2 = -2, base_radius = 4 * tan(atan(0.5)) = 2
    double d = cone(Point3D(2, -2, 0), std::atan(0.5), 4.0);
    EXPECT_NEAR(d, 0.0, 1e-6);
}

TEST(SdfCone, Inside) {
    // Inside the cone body
    double d = cone(Point3D(0, 0, 0), std::atan(0.5), 4.0);
    EXPECT_LT(d, 0.0);
}

TEST(SdfCone, Outside) {
    // Outside near base
    double d = cone(Point3D(3, -2, 0), std::atan(0.5), 4.0);
    EXPECT_GT(d, 0.0);
}

TEST(SdfCone, BelowBase) {
    // Below the base plane
    double d = cone(Point3D(0, -3, 0), std::atan(0.5), 4.0);
    EXPECT_GT(d, 0.0);
}

// ═══════════════════════════════════════════════════
//  plane
// ═══════════════════════════════════════════════════

TEST(SdfPlane, Above) {
    Vector3D n(0, 1, 0);
    EXPECT_NEAR(plane(Point3D(0, 5, 0), n, 0.0), 5.0, EPS);
}

TEST(SdfPlane, Below) {
    Vector3D n(0, 1, 0);
    EXPECT_NEAR(plane(Point3D(0, -3, 0), n, 0.0), -3.0, EPS);
}

TEST(SdfPlane, WithOffset) {
    Vector3D n(0, 1, 0);
    EXPECT_NEAR(plane(Point3D(0, 5, 0), n, 2.0), 3.0, EPS);
    EXPECT_NEAR(plane(Point3D(0, 1, 0), n, 2.0), -1.0, EPS);
}

// ═══════════════════════════════════════════════════
//  ellipsoid
// ═══════════════════════════════════════════════════

TEST(SdfEllipsoid, SphereCase) {
    // Ellipsoid with equal radii = sphere
    Point3D radii(3, 3, 3);
    double d = ellipsoid(Point3D(3, 0, 0), radii);
    EXPECT_NEAR(d, 0.0, 1e-6);
}

TEST(SdfEllipsoid, Inside) {
    Point3D radii(3, 2, 1);
    double d = ellipsoid(Point3D(0, 0, 0), radii);
    EXPECT_NEAR(d, -1.0, 1e-6);
}

TEST(SdfEllipsoid, OnSurface) {
    // At (3,0,0): scaled = (1,0,0), |scaled|=1 → on surface
    Point3D radii(3, 2, 1);
    double d = ellipsoid(Point3D(3, 0, 0), radii);
    EXPECT_NEAR(d, 0.0, 1e-6);
    d = ellipsoid(Point3D(0, 2, 0), radii);
    EXPECT_NEAR(d, 0.0, 1e-6);
    d = ellipsoid(Point3D(0, 0, 1), radii);
    EXPECT_NEAR(d, 0.0, 1e-6);
}

// ═══════════════════════════════════════════════════
//  triangular_prism
// ═══════════════════════════════════════════════════

TEST(SdfTriangularPrism, OnVertex) {
    Point3D a(0, 0, 0), b(4, 0, 0), c(2, 3, 0);
    double d = triangular_prism(Point3D(0, 0, 0), a, b, c, 2.0);
    EXPECT_NEAR(d, 0.0, 1e-6);  // vertex on boundary → SDF = 0
}

TEST(SdfTriangularPrism, Inside) {
    Point3D a(0, 0, 0), b(4, 0, 0), c(2, 3, 0);
    // Centroid of triangle, mid-Z
    double d = triangular_prism(Point3D(2, 1, 0), a, b, c, 4.0);
    EXPECT_LT(d, 0.0);
}

TEST(SdfTriangularPrism, OutsideXY) {
    Point3D a(0, 0, 0), b(4, 0, 0), c(2, 3, 0);
    // Point far outside the triangle but within Z range
    double d = triangular_prism(Point3D(10, 10, 0), a, b, c, 2.0);
    EXPECT_GT(d, 0.0);
}

TEST(SdfTriangularPrism, OutsideZ) {
    Point3D a(0, 0, 0), b(4, 0, 0), c(2, 3, 0);
    // Inside triangle but far above top cap
    double d = triangular_prism(Point3D(2, 1, 5), a, b, c, 2.0);
    EXPECT_NEAR(d, 4.0, 1e-6);
}

// ═══════════════════════════════════════════════════
//  hex_prism
// ═══════════════════════════════════════════════════

TEST(SdfHexPrism, Center) {
    double d = hex_prism(Point3D(0, 0, 0), 2.0, 4.0);
    EXPECT_LT(d, 0.0);
}

TEST(SdfHexPrism, OnVertex) {
    // Vertex at distance r along X
    double d = hex_prism(Point3D(2, 0, 0), 2.0, 2.0);
    EXPECT_NEAR(d, 0.0, 2e-5);
}

TEST(SdfHexPrism, OutsideCap) {
    double d = hex_prism(Point3D(0, 3, 0), 1.0, 4.0);
    EXPECT_NEAR(d, 1.0, 1e-6);
}

TEST(SdfHexPrism, Degenerate) {
    double d = hex_prism(Point3D(0, 0, 0), 0.0, 0.0);
    EXPECT_NEAR(d, 0.0, 1e-6);
}

// ═══════════════════════════════════════════════════
//  link
// ═══════════════════════════════════════════════════

TEST(SdfLink, AtCenterOfFirstTorus) {
    // Length 4, so torus centers at x=±2
    // Center of first torus tube: x=2+3=5? No, torus major_r=3
    // Tube center of first torus at (2+3, 0, 0) = (5, 0, 0) in XZ plane
    double d = link(Point3D(5, 0, 0), 4.0, 3.0, 1.0);
    EXPECT_NEAR(d, -1.0, EPS);
}

TEST(SdfLink, BetweenToruses) {
    // Point between the two toruses — this is in the torus hole, outside the shape
    double d = link(Point3D(0, 0, 0), 2.0, 2.0, 0.5);
    // Point (0,0,0) is in the hole region between the toruses
    // With length=2 (centers at ±1), major_r=2, minor_r=0.5:
    // distance to each torus tube ≈ 0.5, so the point is outside
    EXPECT_GT(d, 0.0);
}

TEST(SdfLink, FarOutside) {
    double d = link(Point3D(100, 0, 0), 4.0, 3.0, 1.0);
    EXPECT_GT(d, 90.0);
}

// ═══════════════════════════════════════════════════
//  infinite_cylinder
// ═══════════════════════════════════════════════════

TEST(SdfInfiniteCylinder, OnSurface) {
    Vector3D axis(0, 1, 0);
    double d = infinite_cylinder(Point3D(2, 5, 0), axis, 2.0);
    EXPECT_NEAR(d, 0.0, EPS);
    // Any Y should work (infinite)
    d = infinite_cylinder(Point3D(2, 100, 0), axis, 2.0);
    EXPECT_NEAR(d, 0.0, EPS);
}

TEST(SdfInfiniteCylinder, Inside) {
    Vector3D axis(0, 1, 0);
    double d = infinite_cylinder(Point3D(0, 0, 0), axis, 3.0);
    EXPECT_NEAR(d, -3.0, EPS);
}

TEST(SdfInfiniteCylinder, Outside) {
    Vector3D axis(0, 1, 0);
    double d = infinite_cylinder(Point3D(5, 0, 0), axis, 1.0);
    EXPECT_NEAR(d, 4.0, EPS);
}

TEST(SdfInfiniteCylinder, SkewAxis) {
    Vector3D axis(1, 0, 0);
    // Along X axis: distance = sqrt(y²+z²) - r
    double d = infinite_cylinder(Point3D(0, 3, 4), axis, 5.0);
    EXPECT_NEAR(d, 0.0, EPS);
}

// ═══════════════════════════════════════════════════
//  infinite_cone
// ═══════════════════════════════════════════════════

TEST(SdfInfiniteCone, AtApex) {
    Point3D apex(0, 0, 0);
    Vector3D axis(0, 1, 0);
    double d = infinite_cone(apex, apex, axis, 0.5);
    EXPECT_NEAR(d, 0.0, EPS);
}

TEST(SdfInfiniteCone, OnSurface) {
    Point3D apex(0, 0, 0);
    Vector3D axis(0, 1, 0);
    double angle = std::atan(1.0); // 45° cone
    // At y=2, radius should be 2 (tan45° = 1)
    double d = infinite_cone(Point3D(2, 2, 0), apex, axis, angle);
    EXPECT_NEAR(d, 0.0, 1e-6);
}

TEST(SdfInfiniteCone, Inside) {
    Point3D apex(0, 0, 0);
    Vector3D axis(0, 1, 0);
    double angle = std::atan(2.0); // wide cone
    // Point close to axis, should be inside
    double d = infinite_cone(Point3D(1, 2, 0), apex, axis, angle);
    EXPECT_LT(d, 0.0);
}

TEST(SdfInfiniteCone, Outside) {
    Point3D apex(0, 0, 0);
    Vector3D axis(0, 1, 0);
    double angle = std::atan(0.5); // narrow cone
    // Far from axis
    double d = infinite_cone(Point3D(10, 2, 0), apex, axis, angle);
    EXPECT_GT(d, 0.0);
}

TEST(SdfInfiniteCone, BehindApex) {
    Point3D apex(0, 0, 0);
    Vector3D axis(0, 1, 0);
    double angle = std::atan(1.0);
    // Behind apex (negative Y) → outside
    double d = infinite_cone(Point3D(0, -1, 0), apex, axis, angle);
    EXPECT_GT(d, 0.0);
}

// ═══════════════════════════════════════════════════
//  wedge
// ═══════════════════════════════════════════════════

TEST(SdfWedge, CenterOfBoxHalf) {
    // Wedge: box [-w/2,w/2]×[-h/2,h/2]×[-d/2,d/2] ∩ {z ≥ x}
    // Point inside the wedge region
    double d = wedge(Point3D(-0.5, 0.0, 1.0), 4.0, 4.0, 4.0);
    EXPECT_LT(d, 0.0);
}

TEST(SdfWedge, OutsideDiagonal) {
    // Point in box but on wrong side of diagonal (x > z)
    double d = wedge(Point3D(1.5, 0.0, 0.0), 4.0, 4.0, 4.0);
    EXPECT_GT(d, 0.0);
}

TEST(SdfWedge, OnDiagonalPlane) {
    // On the diagonal plane z = x
    double d = wedge(Point3D(1, 0, 1), 4.0, 4.0, 4.0);
    EXPECT_NEAR(d, 0.0, 1e-6);
}

TEST(SdfWedge, OutsideBox) {
    double d = wedge(Point3D(0, 10, 10), 4.0, 4.0, 4.0);
    EXPECT_GT(d, 0.0);
}

// ═══════════════════════════════════════════════════
//  extrusion
// ═══════════════════════════════════════════════════

TEST(SdfExtrusion, PassThrough) {
    double sd2 = 3.5;
    EXPECT_NEAR(extrusion(Point3D(1, 2, 100), sd2), 3.5, EPS);
    EXPECT_NEAR(extrusion(Point3D(-5, 7, -3), -2.0), -2.0, EPS);
}

TEST(SdfExtrusion, Zero) {
    EXPECT_NEAR(extrusion(Point3D(0, 0, 0), 0.0), 0.0, EPS);
}

// ═══════════════════════════════════════════════════
//  extrusion_bounded
// ═══════════════════════════════════════════════════

TEST(SdfExtrusionBounded, InsideSlabAndShape) {
    // 2D SDF says outside (positive), Z within bounds → d = max(pos, inside_Z)
    double d = extrusion_bounded(Point3D(0, 0, 0), -1.0, 2.0);
    EXPECT_NEAR(d, -1.0, EPS);
}

TEST(SdfExtrusionBounded, OutsideSlab) {
    // 2D SDF inside, but Z outside slab
    double d = extrusion_bounded(Point3D(0, 0, 5), -1.0, 2.0);
    EXPECT_NEAR(d, 3.0, EPS);
}

TEST(SdfExtrusionBounded, Zero) {
    double d = extrusion_bounded(Point3D(0, 0, 0), 0.0, 0.0);
    EXPECT_NEAR(d, 0.0, EPS);
}

// ═══════════════════════════════════════════════════
//  revolution
// ═══════════════════════════════════════════════════

TEST(SdfRevolution, PassThrough) {
    double sd2 = -1.5;
    EXPECT_NEAR(revolution(Point3D(3, 2, 4), sd2, 1.0), -1.5, EPS);
}

TEST(SdfRevolution, Zero) {
    EXPECT_NEAR(revolution(Point3D(0, 0, 0), 0.0, 0.0), 0.0, EPS);
}