#include <gtest/gtest.h>
#include "vde/core/cam_advanced.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
// ===========================================================================

static BrepModel make_test_box(double w = 50.0, double h = 30.0, double d = 20.0) {
    return make_box(w, h, d);
}

// ===========================================================================
//  adaptive_clearing 测试
// ===========================================================================

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

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

    EXPECT_EQ(tp.name, "AdaptiveClearing");
    EXPECT_GT(tp.segments.size(), 5u) << "Adaptive clearing should generate segments";
    EXPECT_DOUBLE_EQ(tp.safe_z, 15.0);
}

TEST(CamAdvancedTest, AdaptiveClearing_MultipleDepthSlices) {
    auto box = make_test_box(20, 20, 10);
    Tool tool;
    tool.diameter = 6.0;
    AdaptiveClearingParams params;
    params.step_down = 2.0;
    params.safe_z = 10.0;

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

    // 多层切削应产生大量段
    EXPECT_GT(tp.segments.size(), 10u);
}

TEST(CamAdvancedTest, AdaptiveClearing_VaryingStepOver) {
    auto box = make_test_box(60, 60, 20);
    Tool tool;
    tool.diameter = 10.0;
    AdaptiveClearingParams params;
    params.min_step_over = 0.5;
    params.max_step_over = 4.0;
    params.step_down = 5.0;
    params.safe_z = 15.0;

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

    // 应生成多种覆盖模式的刀路
    EXPECT_GT(tp.segments.size(), 20u);
}

// ===========================================================================
//  trochoidal_milling 测试
// ===========================================================================

TEST(CamAdvancedTest, TrochoidalMilling_GeneratesTrochoids) {
    std::vector<Point3D> slot = {
        Point3D(0, 0, -2),
        Point3D(20, 0, -2),
        Point3D(40, 0, -2),
        Point3D(60, 0, -2)
    };
    Tool tool;
    tool.diameter = 6.0;
    TrochoidalParams params;
    params.trochoid_radius = 2.5;
    params.step_forward = 1.0;
    params.safe_z = 10.0;

    auto tp = trochoidal_milling(slot, tool, params);

    EXPECT_EQ(tp.name, "TrochoidalMilling");
    EXPECT_GT(tp.segments.size(), 10u) << "Trochoidal milling should generate many arcs";
}

TEST(CamAdvancedTest, TrochoidalMilling_EmptySlot) {
    std::vector<Point3D> empty_slot;
    Tool tool;
    TrochoidalParams params;

    auto tp = trochoidal_milling(empty_slot, tool, params);
    EXPECT_EQ(tp.segments.size(), 0u);
}

TEST(CamAdvancedTest, TrochoidalMilling_SingleSegment) {
    std::vector<Point3D> slot = {
        Point3D(0, 0, -1),
        Point3D(10, 10, -1)
    };
    Tool tool;
    tool.diameter = 5.0;
    TrochoidalParams params;
    params.step_forward = 2.0;
    params.trochoid_radius = 2.0;
    params.safe_z = 10.0;

    auto tp = trochoidal_milling(slot, tool, params);
    EXPECT_GT(tp.segments.size(), 0u);
}

// ===========================================================================
//  rest_machining 测试
// ===========================================================================

TEST(CamAdvancedTest, RestMachining_SignificantToolDiff) {
    auto box = make_test_box(40, 40, 15);
    Tool prev_tool;
    prev_tool.diameter = 20.0;  // 大刀具
    Tool next_tool;
    next_tool.diameter = 6.0;   // 小刀具
    RestMachiningParams params;
    params.stock_threshold = 0.1;
    params.safe_z = 10.0;

    auto tp = rest_machining(box, prev_tool, next_tool, params);

    EXPECT_EQ(tp.name, "RestMachining");
    // 刀具差异显著，应生成残料刀路
    EXPECT_GT(tp.segments.size(), 0u);
}

TEST(CamAdvancedTest, RestMachining_NoToolDiff) {
    auto box = make_test_box(40, 40, 15);
    Tool prev_tool;
    prev_tool.diameter = 10.0;
    Tool next_tool;
    next_tool.diameter = 9.9;  // 差异极小
    RestMachiningParams params;
    params.stock_threshold = 0.1;

    auto tp = rest_machining(box, prev_tool, next_tool, params);

    // 刀具差异太小，无残料刀路
    EXPECT_EQ(tp.segments.size(), 0u);
}

// ===========================================================================
//  pencil_tracing 测试
// ===========================================================================

TEST(CamAdvancedTest, PencilTracing_GeneratesPath) {
    auto box = make_test_box(40, 40, 15);
    Tool tool;
    tool.diameter = 4.0;
    tool.type = ToolType::BALLNOSE;
    PencilTracingParams params;
    params.step_along = 0.2;
    params.safe_z = 10.0;
    params.corner_angle_min = 5.0;
    params.corner_angle_max = 170.0;

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

    EXPECT_EQ(tp.name, "PencilTracing");
    EXPECT_GT(tp.segments.size(), 2u) << "Pencil tracing should generate corner path";
}

TEST(CamAdvancedTest, PencilTracing_IncludesRetract) {
    auto box = make_test_box(30, 30, 10);
    Tool tool;
    tool.diameter = 3.0;
    PencilTracingParams params;
    params.safe_z = 15.0;

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

    // 应有提刀段
    bool has_rapid_end = false;
    for (auto& seg : tp.segments) {
        if (seg.type == PathSegmentType::Rapid &&
            std::abs(seg.end.z() - params.safe_z) < 1e-9) {
            has_rapid_end = true;
        }
    }
    EXPECT_TRUE(has_rapid_end);
}

// ===========================================================================
//  tool_holder_collision_check 测试
// ===========================================================================

TEST(CamAdvancedTest, ToolHolderCollisionCheck_ReturnsResults) {
    auto box = make_test_box(30, 30, 10);
    Tool tool;
    tool.diameter = 8.0;
    AdaptiveClearingParams params;
    params.step_down = 10.0;
    params.safe_z = 15.0;

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

    ToolHolder holder;
    holder.diameter = 20.0;
    holder.length = 40.0;
    holder.clearance = 3.0;

    auto results = tool_holder_collision_check(tp, holder, box);

    EXPECT_EQ(results.size(), tp.segments.size());
}

TEST(CamAdvancedTest, ToolHolderCollisionCheck_EmptyToolpath) {
    Toolpath empty_tp;
    ToolHolder holder;
    auto box = make_test_box(20, 20, 10);
    auto results = tool_holder_collision_check(empty_tp, holder, box);
    EXPECT_EQ(results.size(), 0u);
}

// ===========================================================================
//  toolpath_optimization 测试
// ===========================================================================

TEST(CamAdvancedTest, ToolpathOptimization_RemovesDuplicates) {
    Toolpath tp;
    tp.name = "Test";
    tp.safe_z = 10.0;

    // 创建有重复段的刀路
    PathSegment s1{Point3D(0,0,10), Point3D(10,0,10),
                   Point3D::Zero(), PathSegmentType::Linear, 500.0, 0};
    PathSegment s2{Point3D(10,0,10), Point3D(10,0,10),  // duplicate end
                   Point3D::Zero(), PathSegmentType::Linear, 500.0, 0};
    PathSegment s3{Point3D(10,0,10), Point3D(20,0,10),
                   Point3D::Zero(), PathSegmentType::Linear, 500.0, 0};
    tp.segments = {s1, s2, s3};

    ToolpathOptimizationParams params;
    params.remove_duplicates = true;
    params.merge_collinear = false;
    params.reorder_segments = false;

    auto optimized = toolpath_optimization(tp, params);

    EXPECT_LE(optimized.segments.size(), tp.segments.size());
    EXPECT_GT(optimized.segments.size(), 0u);
}

TEST(CamAdvancedTest, ToolpathOptimization_MergesCollinear) {
    Toolpath tp;
    tp.name = "Test";
    tp.safe_z = 10.0;

    // 创建三条共线连续段
    PathSegment s1{Point3D(0,0,5), Point3D(10,0,5),
                   Point3D::Zero(), PathSegmentType::Linear, 500.0, -2.0};
    PathSegment s2{Point3D(10,0,5), Point3D(20,0,5),
                   Point3D::Zero(), PathSegmentType::Linear, 500.0, -2.0};
    PathSegment s3{Point3D(20,0,5), Point3D(30,0,5),
                   Point3D::Zero(), PathSegmentType::Linear, 500.0, -2.0};
    tp.segments = {s1, s2, s3};

    ToolpathOptimizationParams params;
    params.merge_collinear = true;
    params.remove_duplicates = false;
    params.reorder_segments = false;

    auto optimized = toolpath_optimization(tp, params);

    EXPECT_LT(optimized.segments.size(), 3u)
        << "Should merge collinear segments into fewer";
}

TEST(CamAdvancedTest, ToolpathOptimization_EmptyToolpath) {
    Toolpath empty_tp;
    auto optimized = toolpath_optimization(empty_tp);
    EXPECT_EQ(optimized.segments.size(), 0u);
}

// ===========================================================================
//  feed_rate_optimization 测试
// ===========================================================================

TEST(CamAdvancedTest, FeedRateOptimization_AdjustsRates) {
    Toolpath tp;
    tp.name = "Test";
    tp.safe_z = 10.0;

    PathSegment s1{Point3D(0,0,10), Point3D(50,0,-5),
                   Point3D::Zero(), PathSegmentType::Linear, 500.0, -5.0};
    PathSegment s2{Point3D(50,0,-5), Point3D(51,0,-5),  // very short
                   Point3D::Zero(), PathSegmentType::Linear, 500.0, -5.0};
    tp.segments = {s1, s2};

    Material mat;
    mat.name = "Steel";
    mat.hardness_brinell = 250.0;
    mat.specific_cutting_force = 2500.0;

    auto optimized = feed_rate_optimization(tp, mat);

    EXPECT_EQ(optimized.name, "Test_FeedOpt");
    EXPECT_EQ(optimized.segments.size(), 2u);

    // 不同材料去除率应产生不同的进给
    EXPECT_GT(optimized.segments[0].feed_rate, 0);
    EXPECT_GT(optimized.segments[1].feed_rate, 0);
}

TEST(CamAdvancedTest, FeedRateOptimization_HardMaterialReducesFeed) {
    Toolpath tp;
    tp.name = "Test";
    tp.safe_z = 10.0;
    PathSegment seg{Point3D(0,0,5), Point3D(20,0,-3),
                    Point3D::Zero(), PathSegmentType::Linear, 800.0, -3.0};
    tp.segments = {seg};

    Material steel;
    steel.name = "HardSteel";
    steel.hardness_brinell = 350.0;
    steel.specific_cutting_force = 3500.0;

    Material aluminum;
    aluminum.name = "Aluminum";
    aluminum.hardness_brinell = 95.0;
    aluminum.specific_cutting_force = 800.0;

    auto opt_steel = feed_rate_optimization(tp, steel);
    auto opt_alum = feed_rate_optimization(tp, aluminum);

    // 硬材料的进给应低于软材料
    EXPECT_LT(opt_steel.segments[0].feed_rate,
              opt_alum.segments[0].feed_rate);
}

TEST(CamAdvancedTest, FeedRateOptimization_EmptyToolpath) {
    Toolpath empty_tp;
    Material mat;
    auto optimized = feed_rate_optimization(empty_tp, mat);
    EXPECT_EQ(optimized.segments.size(), 0u);
}