#include <gtest/gtest.h>
#include "vde/core/cam_mesh.h"
#include "vde/core/cam_strategies.h"
#include "vde/curves/nurbs_curve.h"
#include "vde/mesh/halfedge_mesh.h"
#include "vde/core/point.h"
#include <cmath>
#include <vector>

using namespace vde::core;
using namespace vde::mesh;
using namespace vde::curves;

// ===========================================================================
//  Test helpers
// ===========================================================================

/// Build a unit cube halfedge-mesh centred at (0,0,0) with given half-size
static HalfedgeMesh make_cube_mesh(double half = 1.0) {
    HalfedgeMesh m;
    // 8 vertices
    double h = half;
    m.add_vertex(Point3D(-h, -h, -h));  // 0
    m.add_vertex(Point3D( h, -h, -h));  // 1
    m.add_vertex(Point3D( h,  h, -h));  // 2
    m.add_vertex(Point3D(-h,  h, -h));  // 3
    m.add_vertex(Point3D(-h, -h,  h));  // 4
    m.add_vertex(Point3D( h, -h,  h));  // 5
    m.add_vertex(Point3D( h,  h,  h));  // 6
    m.add_vertex(Point3D(-h,  h,  h));  // 7

    // 12 triangles (CCW from outside)
    // Bottom: z = -h
    m.add_face({0, 2, 1});    m.add_face({0, 3, 2});
    // Top: z = +h
    m.add_face({4, 5, 6});    m.add_face({4, 6, 7});
    // Front: y = -h
    m.add_face({0, 1, 5});    m.add_face({0, 5, 4});
    // Back: y = +h
    m.add_face({2, 3, 7});    m.add_face({2, 7, 6});
    // Left: x = -h
    m.add_face({0, 4, 7});    m.add_face({0, 7, 3});
    // Right: x = +h
    m.add_face({1, 2, 6});    m.add_face({1, 6, 5});

    return m;
}

/// Build a simple pyramid mesh: square base at z=-1, apex at z=+1
static HalfedgeMesh make_pyramid_mesh() {
    HalfedgeMesh m;
    m.add_vertex(Point3D(-1, -1, -1));  // 0
    m.add_vertex(Point3D( 1, -1, -1));  // 1
    m.add_vertex(Point3D( 1,  1, -1));  // 2
    m.add_vertex(Point3D(-1,  1, -1));  // 3
    m.add_vertex(Point3D( 0,  0,  1));  // 4 — apex

    // Base (two triangles)
    m.add_face({0, 2, 1});
    m.add_face({0, 3, 2});
    // Sides
    m.add_face({0, 1, 4});
    m.add_face({1, 2, 4});
    m.add_face({2, 3, 4});
    m.add_face({3, 0, 4});

    return m;
}

/// Count linear cutting segments in a toolpath
static int count_linear_cuts(const Toolpath& tp) {
    int count = 0;
    for (auto& s : tp.segments) {
        if (s.type == PathSegmentType::Linear && s.z_depth <= 0) {
            count++;
        }
    }
    return count;
}

/// Sample a NURBS curve at `count` evenly-spaced parameter values
static std::vector<Point3D> sample_curve(const NurbsCurve& curve, int count) {
    std::vector<Point3D> pts;
    pts.reserve(count);
    auto [t0, t1] = curve.domain();
    for (int i = 0; i < count; ++i) {
        double t = t0 + (t1 - t0) * static_cast<double>(i) / static_cast<double>(count - 1);
        pts.push_back(curve.evaluate(t));
    }
    return pts;
}

// ===========================================================================
//  Test 1: extract_contour_from_mesh — cube at z=0
// ===========================================================================

TEST(CamMeshTest, ExtractContour_CubeAtZero) {
    auto cube = make_cube_mesh(2.0);   // half-size 2, spans [-2, +2]
    auto contours = extract_contour_from_mesh(cube, 0.0);

    // A cube intersected at z=0 should give a single closed contour
    // (the square at mid-height: x=±2, y=±2)
    EXPECT_GE(contours.size(), 1u);

    if (!contours.empty()) {
        auto& c = contours[0];
        auto [t0, t1] = c.domain();
        EXPECT_LT(t0, t1);

        // Sample and check bounds: points should be within [-2.5, +2.5] in XY
        // and z should be ~0
        auto sampled = sample_curve(c, 100);
        double z_tol = 1e-6;
        for (auto& p : sampled) {
            EXPECT_NEAR(p.z(), 0.0, z_tol);
            EXPECT_GE(p.x(), -2.5);
            EXPECT_LE(p.x(),  2.5);
            EXPECT_GE(p.y(), -2.5);
            EXPECT_LE(p.y(),  2.5);
        }
    }
}

// ===========================================================================
//  Test 2: extract_contour_from_mesh — no intersection (z above mesh)
// ===========================================================================

TEST(CamMeshTest, ExtractContour_NoIntersection) {
    auto cube = make_cube_mesh(1.0);   // spans z: -1..+1
    auto contours = extract_contour_from_mesh(cube, 5.0);
    EXPECT_TRUE(contours.empty());
}

// ===========================================================================
//  Test 3: extract_contour_from_mesh — empty mesh
// ===========================================================================

TEST(CamMeshTest, ExtractContour_EmptyMesh) {
    HalfedgeMesh empty;
    auto contours = extract_contour_from_mesh(empty, 0.0);
    EXPECT_TRUE(contours.empty());
}

// ===========================================================================
//  Test 4: extract_contour_from_mesh — pyramid at z=0
// ===========================================================================

TEST(CamMeshTest, ExtractContour_PyramidMid) {
    auto pyr = make_pyramid_mesh();    // base z=-1, apex z=+1
    auto contours = extract_contour_from_mesh(pyr, 0.0);

    // At z=0, pyramid cross-section is a smaller square (size ~0.5 per side)
    EXPECT_GE(contours.size(), 1u);

    if (!contours.empty()) {
        auto& c = contours[0];
        auto sampled = sample_curve(c, 200);
        double z_tol = 1e-6;
        for (auto& p : sampled) {
            EXPECT_NEAR(p.z(), 0.0, z_tol);
            // Pyramind at z=0: cross-section should be within [-0.6, 0.6]
            EXPECT_GE(p.x(), -0.6);
            EXPECT_LE(p.x(),  0.6);
            EXPECT_GE(p.y(), -0.6);
            EXPECT_LE(p.y(),  0.6);
        }
    }
}

// ===========================================================================
//  Test 5: contour_toolpath — cube contour
// ===========================================================================

TEST(CamMeshTest, ContourToolpath_Cube) {
    auto cube = make_cube_mesh(2.0);   // z: -2..+2
    Tool tool;
    tool.diameter = 6.0;
    ContourParams params;
    params.safe_z = 10.0;
    params.step_down = 1.0;
    params.feed_rate = 800.0;
    params.stock_to_leave = 0.0;

    auto tp = contour_toolpath(cube, -2.0, tool, params);

    // Should produce segments
    EXPECT_GT(tp.segments.size(), 0u);
    EXPECT_DOUBLE_EQ(tp.safe_z, 10.0);
    EXPECT_NEAR(tp.cut_z, -2.0, 1e-9);

    // Check that all moving segments have valid z
    bool has_rapids = false;
    bool has_linear = false;
    for (auto& s : tp.segments) {
        if (s.type == PathSegmentType::Rapid)  has_rapids = true;
        if (s.type == PathSegmentType::Linear) has_linear = true;
    }
    EXPECT_TRUE(has_rapids);
    EXPECT_TRUE(has_linear);
}

// ===========================================================================
//  Test 6: contour_toolpath — empty mesh
// ===========================================================================

TEST(CamMeshTest, ContourToolpath_EmptyMesh) {
    HalfedgeMesh empty;
    Tool tool;
    ContourParams params;

    auto tp = contour_toolpath(empty, 0.0, tool, params);
    EXPECT_TRUE(tp.segments.empty());
}

// ===========================================================================
//  Test 7: pocket_toolpath — basic pocket
// ===========================================================================

TEST(CamMeshTest, PocketToolpath_Basic) {
    auto cube = make_cube_mesh(2.0);
    Tool tool;
    tool.diameter = 6.0;
    PocketParams params;
    params.step_over = 1.0;
    params.safe_z = 10.0;
    params.feed_rate = 800.0;
    params.cut_angle = 0.0;

    std::vector<HalfedgeMesh> no_islands;
    auto tp = pocket_toolpath(cube, no_islands, -1.0, tool, params);

    EXPECT_GT(tp.segments.size(), 0u);
    EXPECT_DOUBLE_EQ(tp.safe_z, 10.0);
    EXPECT_NEAR(tp.cut_z, -1.0, 1e-9);

    int linear_cuts = count_linear_cuts(tp);
    EXPECT_GT(linear_cuts, 0) << "Should have cutting segments";
}

// ===========================================================================
//  Test 8: pocket_toolpath — with islands
// ===========================================================================

TEST(CamMeshTest, PocketToolpath_WithIslands) {
    auto cube = make_cube_mesh(3.0);   // 6×6×6

    // Make a smaller inner cube as an island
    HalfedgeMesh island;
    double h = 1.0;
    island.add_vertex(Point3D(-h, -h, -1));
    island.add_vertex(Point3D( h, -h, -1));
    island.add_vertex(Point3D( h,  h, -1));
    island.add_vertex(Point3D(-h,  h, -1));
    island.add_vertex(Point3D(-h, -h,  1));
    island.add_vertex(Point3D( h, -h,  1));
    island.add_vertex(Point3D( h,  h,  1));
    island.add_vertex(Point3D(-h,  h,  1));
    island.add_face({0, 2, 1}); island.add_face({0, 3, 2});
    island.add_face({4, 5, 6}); island.add_face({4, 6, 7});
    island.add_face({0, 1, 5}); island.add_face({0, 5, 4});
    island.add_face({2, 3, 7}); island.add_face({2, 7, 6});
    island.add_face({0, 4, 7}); island.add_face({0, 7, 3});
    island.add_face({1, 2, 6}); island.add_face({1, 6, 5});

    Tool tool;
    tool.diameter = 3.0;
    PocketParams params;
    params.step_over = 0.8;
    params.safe_z = 10.0;
    params.feed_rate = 600.0;
    params.cut_angle = 45.0;

    std::vector<HalfedgeMesh> islands = {island};
    auto tp = pocket_toolpath(cube, islands, -1.0, tool, params);

    EXPECT_GT(tp.segments.size(), 0u);
    EXPECT_DOUBLE_EQ(tp.cut_z, -1.0);

    // Should have at least some cutting segments
    int linear_cuts = count_linear_cuts(tp);
    EXPECT_GT(linear_cuts, 0) << "Pocket with islands should still cut outside";
}

// ===========================================================================
//  Test 9: ContourParams / PocketParams defaults
// ===========================================================================

TEST(CamMeshTest, ParamsDefaults) {
    ContourParams cp;
    EXPECT_DOUBLE_EQ(cp.safe_z, 10.0);
    EXPECT_DOUBLE_EQ(cp.step_down, 1.0);
    EXPECT_DOUBLE_EQ(cp.feed_rate, 800.0);
    EXPECT_DOUBLE_EQ(cp.stock_to_leave, 0.0);

    PocketParams pp;
    EXPECT_DOUBLE_EQ(pp.step_over, 2.0);
    EXPECT_DOUBLE_EQ(pp.safe_z, 10.0);
    EXPECT_DOUBLE_EQ(pp.step_down, 1.0);
    EXPECT_DOUBLE_EQ(pp.feed_rate, 800.0);
    EXPECT_DOUBLE_EQ(pp.cut_angle, 0.0);
}

// ===========================================================================
//  Test 10: contour_toolpath — stock_to_leave offset
// ===========================================================================

TEST(CamMeshTest, ContourToolpath_StockToLeave) {
    auto cube = make_cube_mesh(2.0);
    Tool tool;
    ContourParams params;
    params.stock_to_leave = 0.5;
    params.safe_z = 10.0;
    params.step_down = 4.0;   // single pass — target z=-2, start z=+2

    auto tp = contour_toolpath(cube, -2.0, tool, params);
    EXPECT_GT(tp.segments.size(), 0u);
}

// ===========================================================================
//  Test 11: pocket_toolpath — empty boundary mesh
// ===========================================================================

TEST(CamMeshTest, PocketToolpath_EmptyBoundary) {
    HalfedgeMesh empty;
    Tool tool;
    PocketParams params;
    std::vector<HalfedgeMesh> no_islands;

    auto tp = pocket_toolpath(empty, no_islands, 0.0, tool, params);
    EXPECT_TRUE(tp.segments.empty());
}