#include <gtest/gtest.h>
#include "vde/core/distance.h"
#include "vde/core/line.h"
#include "vde/core/plane.h"
#include "vde/core/triangle.h"

using namespace vde::core;

// ── Point to Point ──

TEST(DistanceTest, PointToPoint) {
    EXPECT_DOUBLE_EQ(distance(Point3D(0,0,0), Point3D(1,0,0)), 1.0);
    EXPECT_DOUBLE_EQ(distance(Point3D(0,0,0), Point3D(3,4,0)), 5.0);
    EXPECT_DOUBLE_EQ(distance(Point3D(1,2,3), Point3D(1,2,3)), 0.0);
}

// ── Point to Plane ──

TEST(DistanceTest, PointToPlane) {
    Plane3D plane(Point3D(0,0,0), Vector3D(0,0,1));
    EXPECT_DOUBLE_EQ(distance(Point3D(0,0,5), plane), 5.0);
    EXPECT_DOUBLE_EQ(distance(Point3D(0,0,-5), plane), 5.0);
    EXPECT_DOUBLE_EQ(distance(Point3D(0,0,0), plane), 0.0);
}

TEST(DistanceTest, PointToPlane_OffsetOrigin) {
    Plane3D plane(Point3D(0,0,10), Vector3D(0,0,1));
    EXPECT_DOUBLE_EQ(distance(Point3D(0,0,0), plane), 10.0);
    EXPECT_DOUBLE_EQ(distance(Point3D(0,0,10), plane), 0.0);
}

// ── Point to Line ──

TEST(DistanceTest, PointToLine_OnLine) {
    Line3Dd line(Point3D(0,0,0), Vector3D(1,0,0));
    EXPECT_DOUBLE_EQ(distance(Point3D(5,0,0), line), 0.0);
}

TEST(DistanceTest, PointToLine_Perpendicular) {
    Line3Dd line(Point3D(0,0,0), Vector3D(1,0,0));
    // Point at (0, 3, 0), line along X-axis → distance = 3
    EXPECT_DOUBLE_EQ(distance(Point3D(0,3,0), line), 3.0);
}

TEST(DistanceTest, PointToLine_OffsetOrigin) {
    Line3Dd line(Point3D(1,2,0), Vector3D(0,1,0));
    // Line through (1,2,0) along Y-axis
    // Point (1,0,0) → perpendicular distance = 2
    EXPECT_DOUBLE_EQ(distance(Point3D(3,0,0), line), 2.0);
}

// ── Point to Segment ──

TEST(DistanceTest, PointToSegment_PointOnSegment) {
    Segment3Dd seg(Point3D(0,0,0), Point3D(2,0,0));
    EXPECT_DOUBLE_EQ(distance(Point3D(1,0,0), seg), 0.0);
}

TEST(DistanceTest, PointToSegment_EndpointClosest) {
    Segment3Dd seg(Point3D(0,0,0), Point3D(2,0,0));
    // Point beyond P1 → closest is P1
    EXPECT_DOUBLE_EQ(distance(Point3D(3,0,0), seg), 1.0);
    // Point before P0 → closest is P0
    EXPECT_DOUBLE_EQ(distance(Point3D(-1,0,0), seg), 1.0);
}

TEST(DistanceTest, PointToSegment_PerpendicularProjection) {
    Segment3Dd seg(Point3D(0,0,0), Point3D(4,0,0));
    // Point directly above middle → perpendicular projection
    EXPECT_DOUBLE_EQ(distance(Point3D(2,3,0), seg), 3.0);
}

TEST(DistanceTest, PointToSegment_ZeroLength) {
    Segment3Dd seg(Point3D(1,1,1), Point3D(1,1,1));
    EXPECT_DOUBLE_EQ(distance(Point3D(1,1,0), seg), 1.0);
}

// ── Point to Triangle ──

TEST(DistanceTest, PointToTriangle_InteriorProjection) {
    Triangle3D tri(Point3D(0,0,0), Point3D(2,0,0), Point3D(0,2,0));
    // Point directly above centroid → interior projection
    EXPECT_DOUBLE_EQ(distance(Point3D(0.5, 0.5, 3), tri), 3.0);
}

TEST(DistanceTest, PointToTriangle_OnPlaneInside) {
    Triangle3D tri(Point3D(0,0,0), Point3D(2,0,0), Point3D(0,2,0));
    // Point on the triangle plane, inside the triangle
    EXPECT_DOUBLE_EQ(distance(Point3D(0.5, 0.5, 0), tri), 0.0);
}

TEST(DistanceTest, PointToTriangle_OnPlaneOutside) {
    Triangle3D tri(Point3D(0,0,0), Point3D(2,0,0), Point3D(0,2,0));
    // Point on the triangle plane but outside → closest to edge or vertex
    double d = distance(Point3D(3, 3, 0), tri);
    EXPECT_GT(d, 0.0);
}

// ── closest_point ──

TEST(DistanceTest, ClosestPoint_Triangle) {
    Triangle3D tri(Point3D(0,0,0), Point3D(2,0,0), Point3D(0,2,0));
    Point3D c = closest_point(Point3D(1, 1, 5), tri);
    // Closest point should be on triangle plane, within bounds
    EXPECT_NEAR(c.z(), 0.0, 1e-9);
    EXPECT_GE(c.x(), 0.0);
    EXPECT_GE(c.y(), 0.0);
}

TEST(DistanceTest, ClosestPoint_Segment) {
    Segment3Dd seg(Point3D(0,0,0), Point3D(4,0,0));
    Point3D c = closest_point(Point3D(2, 5, 0), seg);
    EXPECT_NEAR(c.x(), 2.0, 1e-9);
    EXPECT_NEAR(c.y(), 0.0, 1e-9);
    EXPECT_NEAR(c.z(), 0.0, 1e-9);
}

TEST(DistanceTest, ClosestPoint_SegmentAtEndpoint) {
    Segment3Dd seg(Point3D(0,0,0), Point3D(4,0,0));
    Point3D c = closest_point(Point3D(5, 0, 3), seg);
    // Should clamp to P1
    EXPECT_NEAR(c.x(), 4.0, 1e-9);
    EXPECT_NEAR(c.y(), 0.0, 1e-9);
}