#include <gtest/gtest.h>
#include "vde/core/cam_strategies.h"
#include "vde/core/cam_toolpath.h"
#include "vde/brep/brep.h"
#include "vde/brep/modeling.h"
#include "vde/mesh/halfedge_mesh.h"
#include <cmath>
#include <string>

using namespace vde::core;
using namespace vde::brep;

// ===========================================================================
//  Helpers
// ===========================================================================

/// Create a simple box model for testing
static BrepModel make_test_box(double w = 50.0, double h = 30.0, double d = 20.0) {
    return make_box(w, h, d);
}

// ===========================================================================
//  Tool 结构体测试
// ===========================================================================

TEST(CamStrategiesTest, Tool_Defaults) {
    Tool t;
    EXPECT_EQ(t.id, 0);
    EXPECT_EQ(t.type, ToolType::ENDMILL);
    EXPECT_DOUBLE_EQ(t.diameter, 10.0);
    EXPECT_EQ(t.flutes, 2);
    EXPECT_DOUBLE_EQ(t.length, 50.0);
    EXPECT_DOUBLE_EQ(t.overall_length, 75.0);
}

TEST(CamStrategiesTest, Tool_SpindleRPM) {
    Tool t;
    t.diameter = 10.0;
    // Vc = 100 m/min, D = 10mm → RPM = 100*1000 / (π*10) ≈ 3183
    double rpm = t.spindle_rpm(100.0);
    EXPECT_NEAR(rpm, 3183.1, 10.0);
}

TEST(CamStrategiesTest, Tool_SpindleRPM_ZeroDiameter) {
    Tool t;
    t.diameter = 0.0;
    double rpm = t.spindle_rpm(100.0);
    EXPECT_DOUBLE_EQ(rpm, 0.0);
}

TEST(CamStrategiesTest, Tool_FeedRate) {
    Tool t;
    t.diameter = 10.0;
    t.flutes = 2;
    // Vc=100, fz=0.1 → RPM≈3183, F=3183*2*0.1≈637
    double f = t.feed_rate_mm_per_min(0.1);
    EXPECT_GT(f, 500.0);
    EXPECT_LT(f, 800.0);
}

// ===========================================================================
//  ToolLibrary 测试
// ===========================================================================

TEST(CamStrategiesTest, ToolLibrary_AddAndFindById) {
    ToolLibrary lib;
    Tool t1;
    t1.id = 10;
    t1.name = "Endmill10";
    t1.diameter = 10.0;

    Tool t2;
    t2.id = 20;
    t2.name = "BallNose6";
    t2.type = ToolType::BALLNOSE;
    t2.diameter = 6.0;

    lib.add_tool(t1);
    lib.add_tool(t2);

    EXPECT_EQ(lib.size(), 2u);

    auto found = lib.find_tool(10);
    ASSERT_TRUE(found.has_value());
    EXPECT_EQ(found->name, "Endmill10");
    EXPECT_DOUBLE_EQ(found->diameter, 10.0);

    auto not_found = lib.find_tool(999);
    EXPECT_FALSE(not_found.has_value());
}

TEST(CamStrategiesTest, ToolLibrary_FindByName) {
    ToolLibrary lib;
    Tool t;
    t.name = "Drill5";
    t.type = ToolType::DRILL;
    t.diameter = 5.0;
    lib.add_tool(t);

    auto found = lib.find_tool("Drill5");
    ASSERT_TRUE(found.has_value());
    EXPECT_EQ(found->type, ToolType::DRILL);

    auto not_found = lib.find_tool("Nonexistent");
    EXPECT_FALSE(not_found.has_value());
}

TEST(CamStrategiesTest, ToolLibrary_ListTools) {
    ToolLibrary lib;
    Tool t1, t2, t3;
    t1.name = "A";
    t2.name = "B";
    t3.name = "C";
    lib.add_tool(t1);
    lib.add_tool(t2);
    lib.add_tool(t3);

    const auto& tools = lib.list_tools();
    EXPECT_EQ(tools.size(), 3u);
    EXPECT_EQ(tools[0].name, "A");
    EXPECT_EQ(tools[1].name, "B");
    EXPECT_EQ(tools[2].name, "C");
}

TEST(CamStrategiesTest, ToolLibrary_PreserveExplicitId) {
    ToolLibrary lib;
    Tool t;
    t.id = 42;
    t.name = "CustomID";
    lib.add_tool(t);

    auto found = lib.find_tool(42);
    ASSERT_TRUE(found.has_value());
    EXPECT_EQ(found->name, "CustomID");
}

// ===========================================================================
//  roughing_toolpath 测试
// ===========================================================================

TEST(CamStrategiesTest, RoughingToolpath_GeneratesSegments) {
    auto box = make_test_box(50, 30, 20);
    Tool tool;
    tool.diameter = 10.0;
    RoughingParams params;
    params.step_down = 2.0;
    params.stock_to_leave = 0.5;
    params.safe_z = 15.0;

    auto tp = roughing_toolpath(box, tool, params);

    EXPECT_EQ(tp.name, "Roughing");
    EXPECT_GT(tp.segments.size(), 5u) << "Should have multiple segments";
    EXPECT_DOUBLE_EQ(tp.safe_z, 15.0);

    // Should have some linear cutting moves
    bool has_linear = false;
    for (auto& seg : tp.segments) {
        if (seg.type == PathSegmentType::Linear && seg.feed_rate > 0) {
            has_linear = true;
            break;
        }
    }
    EXPECT_TRUE(has_linear);
}

TEST(CamStrategiesTest, RoughingToolpath_MultipleDepthSlices) {
    auto box = make_test_box(20, 20, 10);
    Tool tool;
    tool.diameter = 6.0;
    RoughingParams params;
    params.step_down = 2.0;   // 10mm depth → ~5 slices
    params.safe_z = 15.0;

    auto tp = roughing_toolpath(box, tool, params);

    // Count plunge moves (z going down)
    int plunge_count = 0;
    for (auto& seg : tp.segments) {
        if (seg.type == PathSegmentType::Linear && seg.start.z() > seg.end.z()) {
            plunge_count++;
        }
    }
    EXPECT_GE(plunge_count, 3) << "Should have multiple depth slices";
}

TEST(CamStrategiesTest, RoughingToolpath_RespectsStepDown) {
    auto box = make_test_box(10, 10, 5);
    Tool tool;
    tool.diameter = 5.0;
    RoughingParams params;
    params.step_down = 5.0;  // single slice
    params.safe_z = 10.0;

    auto tp = roughing_toolpath(box, tool, params);

    // Should not crash and should produce valid toolpath
    EXPECT_GT(tp.segments.size(), 3u);
}

// ===========================================================================
//  finishing_toolpath 测试
// ===========================================================================

TEST(CamStrategiesTest, FinishingToolpath_Parallel) {
    auto box = make_test_box(40, 30, 10);
    Tool tool;
    tool.diameter = 8.0;
    FinishingParams params;
    params.step_over = 2.0;
    params.safe_z = 15.0;
    params.depth_of_cut = 0.0;

    auto tp = finishing_toolpath(box, tool, params, FinishingStrategy::PARALLEL);

    EXPECT_EQ(tp.name, "Finishing_Parallel");
    EXPECT_GT(tp.segments.size(), 5u);

    // Should have linear segment at finishing depth
    bool has_depth = false;
    for (auto& seg : tp.segments) {
        if (seg.type == PathSegmentType::Linear && seg.z_depth < -1e-9) {
            has_depth = true;
            break;
        }
    }
    EXPECT_TRUE(has_depth);
}

TEST(CamStrategiesTest, FinishingToolpath_Spiral) {
    auto box = make_test_box(40, 30, 10);
    Tool tool;
    tool.diameter = 8.0;
    FinishingParams params;
    params.step_over = 3.0;
    params.safe_z = 15.0;
    params.depth_of_cut = 0.0;

    auto tp = finishing_toolpath(box, tool, params, FinishingStrategy::SPIRAL);

    EXPECT_EQ(tp.name, "Finishing_Spiral");
    EXPECT_GT(tp.segments.size(), 3u);
}

TEST(CamStrategiesTest, FinishingToolpath_SpiralMultipleRings) {
    auto box = make_test_box(50, 50, 10);
    Tool tool;
    tool.diameter = 10.0;
    FinishingParams params;
    params.step_over = 2.0;  // offset inward 2mm each ring → many rings for 50mm
    params.safe_z = 15.0;

    auto tp = finishing_toolpath(box, tool, params, FinishingStrategy::SPIRAL);

    // Should have many linear segments (multiple rings)
    int linear_count = 0;
    for (auto& seg : tp.segments) {
        if (seg.type == PathSegmentType::Linear) linear_count++;
    }
    EXPECT_GT(linear_count, 20) << "Should have many segments from multiple spiral rings";
}

TEST(CamStrategiesTest, FinishingToolpath_CustomDepth) {
    auto box = make_test_box(30, 30, 15);
    Tool tool;
    tool.diameter = 5.0;
    FinishingParams params;
    params.step_over = 1.0;
    params.safe_z = 10.0;
    params.depth_of_cut = -5.0;  // custom depth

    auto tp = finishing_toolpath(box, tool, params, FinishingStrategy::PARALLEL);

    EXPECT_DOUBLE_EQ(tp.cut_z, -5.0);
}

// ===========================================================================
//  drilling_toolpath 测试
// ===========================================================================

TEST(CamStrategiesTest, DrillingToolpath_G81) {
    std::vector<DrillPoint> points = {
        {Point3D(10, 10, 0), -8.0, 2.0},
        {Point3D(30, 20, 0), -8.0, 2.0},
        {Point3D(50, 10, 0), -8.0, 2.0},
    };
    Tool tool;
    tool.type = ToolType::DRILL;
    tool.diameter = 5.0;

    auto tp = drilling_toolpath(points, tool, DrillingCycle::G81, 10.0, 200.0);

    EXPECT_EQ(tp.name, "Drill_G81");
    EXPECT_GT(tp.segments.size(), 6u);  // 3 holes × (rapid + feed + retract)

    // Should have rapid moves and linear feed moves
    bool has_rapid = false;
    bool has_feed = false;
    for (auto& seg : tp.segments) {
        if (seg.type == PathSegmentType::Rapid) has_rapid = true;
        if (seg.type == PathSegmentType::Linear && seg.feed_rate > 0) has_feed = true;
    }
    EXPECT_TRUE(has_rapid);
    EXPECT_TRUE(has_feed);
}

TEST(CamStrategiesTest, DrillingToolpath_G83_PeckDrilling) {
    std::vector<DrillPoint> points = {
        {Point3D(15, 15, 0), -12.0, 3.0},  // peck_depth=3 → 4 pecks
    };
    Tool tool;
    tool.type = ToolType::DRILL;
    tool.diameter = 3.0;

    auto tp = drilling_toolpath(points, tool, DrillingCycle::G83, 10.0, 150.0);

    EXPECT_EQ(tp.name, "Drill_G83");
    // G83 should have more segments than G81 due to pecking
    EXPECT_GT(tp.segments.size(), 4u);
}

TEST(CamStrategiesTest, DrillingToolpath_G84_Tapping) {
    std::vector<DrillPoint> points = {
        {Point3D(20, 20, 0), -6.0, 0.0},
    };
    Tool tool;
    tool.type = ToolType::TAP;
    tool.diameter = 4.0;

    auto tp = drilling_toolpath(points, tool, DrillingCycle::G84, 10.0, 100.0);

    EXPECT_EQ(tp.name, "Tap_G84");
    // G84 should: rapid → feed in → feed out → retract
    EXPECT_GT(tp.segments.size(), 2u);

    // Should have both feed-in and feed-out linear moves
    int linear_count = 0;
    for (auto& seg : tp.segments) {
        if (seg.type == PathSegmentType::Linear) linear_count++;
    }
    EXPECT_GE(linear_count, 2) << "Feed in + feed out";
}

TEST(CamStrategiesTest, DrillingToolpath_EmptyPointsList) {
    std::vector<DrillPoint> empty;
    Tool tool;
    auto tp = drilling_toolpath(empty, tool, DrillingCycle::G81, 10.0, 200.0);

    EXPECT_EQ(tp.segments.size(), 0u);
}

// ===========================================================================
//  后处理器测试
// ===========================================================================

TEST(CamStrategiesTest, PostProcessor_FanucPrologue) {
    Toolpath tp;
    tp.name = "Test";
    tp.safe_z = 5.0;
    FanucPost post;
    std::string pro = post.prologue(tp);

    EXPECT_TRUE(pro.find("Fanuc") != std::string::npos || pro.find("G90") != std::string::npos);
    EXPECT_TRUE(pro.find("G90") != std::string::npos);
    EXPECT_TRUE(pro.find("G21") != std::string::npos);
}

TEST(CamStrategiesTest, PostProcessor_FanucEpilogue) {
    Toolpath tp;
    tp.safe_z = 5.0;
    FanucPost post;
    std::string epi = post.epilogue(tp);

    EXPECT_TRUE(epi.find("M30") != std::string::npos);
    EXPECT_TRUE(epi.find("G28") != std::string::npos);
}

TEST(CamStrategiesTest, PostProcessor_Siemens) {
    Toolpath tp;
    tp.name = "Test";
    tp.safe_z = 10.0;
    SiemensPost post;
    std::string pro = post.prologue(tp);
    std::string epi = post.epilogue(tp);

    EXPECT_TRUE(pro.find("Siemens") != std::string::npos || pro.find("G90") != std::string::npos);
    EXPECT_TRUE(epi.find("M30") != std::string::npos);
    EXPECT_TRUE(epi.find("SUPA") != std::string::npos);
}

TEST(CamStrategiesTest, PostProcessor_Heidenhain) {
    Toolpath tp;
    tp.name = "TestPart";
    tp.cut_z = -5.0;
    tp.safe_z = 10.0;
    HeidenhainPost post;
    std::string pro = post.prologue(tp);
    std::string epi = post.epilogue(tp);

    EXPECT_TRUE(pro.find("BEGIN PGM") != std::string::npos);
    EXPECT_TRUE(pro.find("TOOL CALL") != std::string::npos);
    EXPECT_TRUE(epi.find("END PGM") != std::string::npos);
    EXPECT_TRUE(epi.find("M5") != std::string::npos);
}

TEST(CamStrategiesTest, PostProcessor_FormatArcCommands) {
    PathSegment seg_cw{{0,0,0}, {5,5,0}, Point3D(2.5, 2.5, 0), PathSegmentType::ArcCW, 100.0, 0};
    PathSegment seg_ccw{{5,5,0}, {0,0,0}, Point3D(2.5, 2.5, 0), PathSegmentType::ArcCCW, 100.0, 0};

    FanucPost fanuc;
    std::string g2 = fanuc.format_gcode(seg_cw);
    std::string g3 = fanuc.format_gcode(seg_ccw);
    EXPECT_TRUE(g2.find("G2") != std::string::npos);
    EXPECT_TRUE(g3.find("G3") != std::string::npos);

    HeidenhainPost heid;
    std::string h_cw = heid.format_gcode(seg_cw);
    std::string h_ccw = heid.format_gcode(seg_ccw);
    EXPECT_TRUE(h_cw.find("DR-") != std::string::npos);
    EXPECT_TRUE(h_ccw.find("DR+") != std::string::npos);
}

// ===========================================================================
//  PostProcessor::post_process 完整输出测试
// ===========================================================================

TEST(CamStrategiesTest, PostProcessor_FullFanucPost) {
    Toolpath tp;
    tp.name = "Contour";
    tp.safe_z = 5.0;
    tp.cut_z = -1.0;
    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});

    FanucPost post;
    std::string gcode = post.post_process(tp);

    EXPECT_TRUE(gcode.find("G90") != std::string::npos);
    EXPECT_TRUE(gcode.find("M30") != std::string::npos);
    EXPECT_TRUE(gcode.find("G1") != std::string::npos);
    EXPECT_TRUE(gcode.find("G0") != std::string::npos);
}

// ===========================================================================
//  material_removal_simulation 测试
// ===========================================================================

TEST(CamStrategiesTest, MaterialRemovalSimulation_InitialState) {
    auto box = make_test_box(20, 20, 10);
    Tool tool;
    tool.diameter = 10.0;
    Toolpath tp;
    tp.safe_z = 15.0;
    tp.cut_z = 0.0;

    auto result = material_removal_simulation(box, tp, tool);

    // No cutting segments → nothing removed
    EXPECT_NEAR(result.volume_remaining, result.volume_remaining, 1e-9);  // valid volume
    EXPECT_GT(result.volume_remaining, 0.0) << "Should have positive remaining volume";
}

TEST(CamStrategiesTest, MaterialRemovalSimulation_AfterRoughing) {
    auto box = make_test_box(20, 20, 10);
    Tool tool;
    tool.diameter = 10.0;
    RoughingParams params;
    params.step_down = 5.0;
    params.stock_to_leave = 0.0;
    params.safe_z = 15.0;

    auto tp = roughing_toolpath(box, tool, params);
    auto result = material_removal_simulation(box, tp, tool);

    EXPECT_GT(result.volume_remaining, 0.0) << "Should have remaining material";
    EXPECT_GT(result.steps, 0) << "Should have simulation steps";

    // Remaining volume should be less than original (some material removed)
    double original_vol = 20.0 * 20.0 * 10.0;  // 4000 mm³
    EXPECT_LT(result.volume_remaining, original_vol * 1.1) << "Remaining < original";
}

// ===========================================================================
//  Parameter type tests
// ===========================================================================

TEST(CamStrategiesTest, Tool_AllTypes) {
    // Verify all tool types can be constructed
    Tool endmill;
    endmill.type = ToolType::ENDMILL;
    EXPECT_EQ(endmill.type, ToolType::ENDMILL);

    Tool ballnose;
    ballnose.type = ToolType::BALLNOSE;
    EXPECT_EQ(ballnose.type, ToolType::BALLNOSE);

    Tool drill;
    drill.type = ToolType::DRILL;
    EXPECT_EQ(drill.type, ToolType::DRILL);

    Tool facemill;
    facemill.type = ToolType::FACEMILL;
    EXPECT_EQ(facemill.type, ToolType::FACEMILL);

    Tool tap;
    tap.type = ToolType::TAP;
    EXPECT_EQ(tap.type, ToolType::TAP);
}

TEST(CamStrategiesTest, DrillingCycle_AllTypes) {
    std::vector<DrillPoint> points = {{Point3D(0, 0, 0), -5.0, 1.0}};
    Tool drill;
    drill.type = ToolType::DRILL;
    drill.diameter = 3.0;

    auto g81 = drilling_toolpath(points, drill, DrillingCycle::G81);
    EXPECT_EQ(g81.name, "Drill_G81");

    auto g83 = drilling_toolpath(points, drill, DrillingCycle::G83);
    EXPECT_EQ(g83.name, "Drill_G83");

    auto g84 = drilling_toolpath(points, drill, DrillingCycle::G84);
    EXPECT_EQ(g84.name, "Tap_G84");
}

// ===========================================================================
//  粗加工边界条件
// ===========================================================================

TEST(CamStrategiesTest, RoughingToolpath_ZeroStepDown) {
    auto box = make_test_box(20, 20, 10);
    Tool tool;
    tool.diameter = 10.0;
    RoughingParams params;
    params.step_down = 0.0;  // would cause infinite loop, but should still produce output
    params.safe_z = 10.0;

    // Should not crash
    auto tp = roughing_toolpath(box, tool, params);
    EXPECT_GE(tp.segments.size(), 1u);  // at least has safe Z retract
}

TEST(CamStrategiesTest, RoughingToolpath_LargeStepDown) {
    auto box = make_test_box(20, 20, 10);
    Tool tool;
    tool.diameter = 10.0;
    RoughingParams params;
    params.step_down = 100.0;  // larger than model → single slice
    params.safe_z = 20.0;

    auto tp = roughing_toolpath(box, tool, params);
    EXPECT_GT(tp.segments.size(), 3u);
}