#include <gtest/gtest.h>
#include "vde/core/cam_toolpath.h"
#include "vde/curves/nurbs_curve.h"
#include "vde/core/point.h"
#include <cmath>
#include <string>

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

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

// ---------------------------------------------------------------------------
//  Helpers
// ---------------------------------------------------------------------------

/// Create a circular NURBS curve (approximate, cubic, 9 control points)
static NurbsCurve make_circle(double cx, double cy, double radius, int segments = 64) {
    std::vector<Point3D> cps;
    std::vector<double> weights;
    int n = segments;
    for (int i = 0; i < n; ++i) {
        double angle = 2.0 * M_PI * i / n;
        cps.emplace_back(cx + radius * std::cos(angle),
                         cy + radius * std::sin(angle), 0.0);
        weights.push_back(1.0);
    }
    // Clamped degree-3
    int p = std::min(3, n - 1);
    std::vector<double> knots(n + p + 1);
    for (int i = 0; i <= p; ++i)      knots[i] = 0.0;
    for (int i = n; i < n + p + 1; ++i) knots[i] = 1.0;
    int interior = n - p - 1;
    if (interior > 0) {
        for (int i = 0; i <= interior; ++i)
            knots[p + i] = static_cast<double>(i) / (interior + 1);
    }
    return NurbsCurve(cps, std::move(knots), std::move(weights), p);
}

/// Create a rectangular NURBS curve (linear, 4 corners + close)
static NurbsCurve make_rectangle(double x0, double y0, double x1, double y1) {
    std::vector<Point3D> cps = {
        {x0, y0, 0}, {x1, y0, 0}, {x1, y1, 0}, {x0, y1, 0}, {x0, y0, 0}
    };
    return NurbsCurve(cps, {0,0,1,2,3,4,4}, {1,1,1,1,1}, 1);
}

/// Compute approximate radius from curve samples (center at origin, XY plane)
static double approx_radius(const NurbsCurve& curve, int samples = 100) {
    double sum = 0.0;
    auto [t0, t1] = curve.domain();
    for (int i = 0; i < samples; ++i) {
        double t = t0 + (t1 - t0) * i / (samples - 1);
        auto p = curve.evaluate(t);
        sum += std::sqrt(p.x() * p.x() + p.y() * p.y());
    }
    return sum / samples;
}

// ===========================================================================
//  offset_contour
// ===========================================================================

TEST(CamToolpathTest, OffsetContour_CircleOutward) {
    auto circle = make_circle(0, 0, 10.0);
    auto offset = offset_contour(circle, 2.0);

    double r = approx_radius(offset);
    EXPECT_NEAR(r, 12.0, 0.5);  // NURBS approximation tolerance
}

TEST(CamToolpathTest, OffsetContour_CircleInward) {
    auto circle = make_circle(0, 0, 10.0);
    auto offset = offset_contour(circle, -2.0);

    double r = approx_radius(offset);
    EXPECT_NEAR(r, 8.0, 0.5);
}

TEST(CamToolpathTest, OffsetContour_Roundtrip) {
    auto circle = make_circle(0, 0, 10.0);
    auto offset_in = offset_contour(circle, -2.0);
    auto offset_out = offset_contour(offset_in, 2.0);

    double r = approx_radius(offset_out);
    EXPECT_NEAR(r, 10.0, 0.5);
}

TEST(CamToolpathTest, OffsetContour_ZeroDistance) {
    auto circle = make_circle(0, 0, 10.0);
    auto same = offset_contour(circle, 0.0);

    double r = approx_radius(same);
    EXPECT_NEAR(r, 10.0, 0.3);
}

// ===========================================================================
//  contour_toolpath
// ===========================================================================

TEST(CamToolpathTest, ContourToolpath_Rectangle) {
    auto rect = make_rectangle(0, 0, 50, 30);
    double cut_depth = -3.0;
    double step_down = 1.0;

    auto tp = contour_toolpath(rect, cut_depth, 10.0, step_down);

    // Should have multiple depth passes: 0 → -1 → -2 → -3 = 3 passes
    // Each pass: 5 contour segments + plunge + rapid
    EXPECT_GT(tp.segments.size(), 10u) << "Should have multiple segments";
    EXPECT_EQ(tp.cut_z, cut_depth);
    EXPECT_DOUBLE_EQ(tp.safe_z, 10.0);
    EXPECT_DOUBLE_EQ(tp.step_down, step_down);

    // Verify Z depths in at least some linear segments match
    bool found_cut_z = false;
    for (auto& seg : tp.segments) {
        if (seg.type == PathSegmentType::Linear && seg.z_depth <= cut_depth + 1e-6) {
            found_cut_z = true;
            break;
        }
    }
    EXPECT_TRUE(found_cut_z) << "Should have linear segments at cut depth";
}

TEST(CamToolpathTest, ContourToolpath_DepthPassCount) {
    auto rect = make_rectangle(0, 0, 50, 30);
    double cut_depth = -3.0;
    double step_down = 1.0;

    auto tp = contour_toolpath(rect, cut_depth, 10.0, step_down);

    // Count passes: depth goes 0→-1→-2→-3, each pass has a plunge segment
    int plunge_count = 0;
    for (auto& seg : tp.segments) {
        if (seg.type == PathSegmentType::Linear && seg.feed_rate == 500.0 &&
            seg.start.z() > seg.end.z()) {
            plunge_count++;
        }
    }
    EXPECT_EQ(plunge_count, 3) << "3 depth passes expected";
}

// ===========================================================================
//  pocket_toolpath
// ===========================================================================

TEST(CamToolpathTest, PocketToolpath_Rectangle) {
    auto rect = make_rectangle(0, 0, 50, 30);
    std::vector<NurbsCurve> empty_islands;

    auto tp = pocket_toolpath(rect, empty_islands, -2.0, 2.0, 0.0);

    EXPECT_EQ(tp.name, "Pocket");
    EXPECT_GT(tp.segments.size(), 3u) << "Should have cutting segments";
    EXPECT_NEAR(tp.cut_z, -2.0, 1e-9);

    // At least some linear segments at cut depth should exist
    bool has_cut = false;
    for (auto& seg : tp.segments) {
        if (seg.type == PathSegmentType::Linear && std::abs(seg.z_depth - (-2.0)) < 1e-6) {
            has_cut = true;
            break;
        }
    }
    EXPECT_TRUE(has_cut);
}

TEST(CamToolpathTest, PocketToolpath_EmptyBoundary_ReturnsFallback) {
    // Degenerate curve (single point) — should return contour fallback
    auto degenerate = NurbsCurve({Point3D(0,0,0)}, {0,0}, {1}, 0);
    std::vector<NurbsCurve> empty;
    auto tp = pocket_toolpath(degenerate, empty, -1.0, 1.0, 0.0);
    // Should at least not crash and produce some output
    EXPECT_GT(tp.segments.size(), 0u);
}

// ===========================================================================
//  export_gcode
// ===========================================================================

TEST(CamToolpathTest, ExportGcode_Basic) {
    Toolpath tp;
    tp.name = "Test";
    tp.safe_z = 5.0;
    tp.cut_z = -1.0;
    tp.step_down = 0.5;

    tp.segments.push_back({Point3D(0,0,5), Point3D(10,0,5),
                           Point3D::Zero(), PathSegmentType::Rapid});
    tp.segments.push_back({Point3D(10,0,5), Point3D(10,0,-1),
                           Point3D::Zero(), PathSegmentType::Linear, 500.0, -1.0});
    tp.segments.push_back({Point3D(10,0,-1), Point3D(20,10,-1),
                           Point3D::Zero(), PathSegmentType::Linear, 1000.0, -1.0});
    tp.segments.push_back({Point3D(20,10,-1), Point3D(20,10,5),
                           Point3D::Zero(), PathSegmentType::Rapid});

    std::string gcode = export_gcode(tp);

    // Check key lines exist
    EXPECT_TRUE(gcode.find("(Generated by ViewDesignEngine CAM)") != std::string::npos);
    EXPECT_TRUE(gcode.find("G90 G21 G17") != std::string::npos);
    EXPECT_TRUE(gcode.find("G0 Z") != std::string::npos);
    EXPECT_TRUE(gcode.find("M30") != std::string::npos);

    // Check rapid move
    EXPECT_TRUE(gcode.find("G0 X") != std::string::npos);

    // Check linear feed
    EXPECT_TRUE(gcode.find("G1 X") != std::string::npos);
    EXPECT_TRUE(gcode.find("F") != std::string::npos);
}

TEST(CamToolpathTest, ExportGcode_ArcCommands) {
    Toolpath tp;
    tp.safe_z = 5.0;
    tp.segments.push_back({Point3D(0,0,5), Point3D(5,5,5),
                           Point3D::Zero(), PathSegmentType::ArcCW});
    tp.segments.push_back({Point3D(5,5,5), Point3D(0,0,5),
                           Point3D::Zero(), PathSegmentType::ArcCCW});

    std::string gcode = export_gcode(tp);

    EXPECT_TRUE(gcode.find("G2 X") != std::string::npos)
        << "Should contain G2 (clockwise arc)";
    EXPECT_TRUE(gcode.find("G3 X") != std::string::npos)
        << "Should contain G3 (counter-clockwise arc)";
    EXPECT_TRUE(gcode.find(" I") != std::string::npos)
        << "Should contain I (arc center X offset)";
    EXPECT_TRUE(gcode.find(" J") != std::string::npos)
        << "Should contain J (arc center Y offset)";
}

// ===========================================================================
//  Round-trip
// ===========================================================================

TEST(CamToolpathTest, OffsetContour_RoundTripPreservesShape) {
    auto circle = make_circle(0, 0, 10.0, 128);
    auto inward = offset_contour(circle, -2.0);
    auto outward = offset_contour(inward, 2.0);

    double r = approx_radius(outward);
    EXPECT_NEAR(r, 10.0, 0.5)
        << "Inward+outward offset should approximately preserve radius";

    // Also verify the curve is still roughly circular (no self-intersections)
    // by checking that all sampled points are within a narrow radial band
    double r_min = 1e9, r_max = -1e9;
    auto [t0, t1] = outward.domain();
    for (int i = 0; i < 100; ++i) {
        double t = t0 + (t1 - t0) * i / 99.0;
        auto p = outward.evaluate(t);
        double rad = std::sqrt(p.x() * p.x() + p.y() * p.y());
        r_min = std::min(r_min, rad);
        r_max = std::max(r_max, rad);
    }
    EXPECT_NEAR(r_min, r_max, 1.0) << "Circle should stay roughly circular after round-trip";
}