#include <gtest/gtest.h>
#include "vde/core/cam_5axis.h"
#include "vde/core/cam_strategies.h"
#include "vde/brep/brep.h"
#include "vde/brep/modeling.h"
#include "vde/curves/nurbs_surface.h"
#include "vde/mesh/halfedge_mesh.h"
#include <cmath>
#include <algorithm>

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

// ===========================================================================
//  辅助函数
// ===========================================================================

/// 创建一个简单的 NURBS 平面用于测试
static curves::NurbsSurface make_test_surface() {
    // 10×10 平面 (degree 1×1)
    std::vector<std::vector<Point3D>> grid(2, std::vector<Point3D>(2));
    grid[0][0] = Point3D(-5, -5, 0);
    grid[0][1] = Point3D(5, -5, 0);
    grid[1][0] = Point3D(-5, 5, 0);
    grid[1][1] = Point3D(5, 5, 0);
    std::vector<double> ku = {0, 0, 1, 1};
    std::vector<double> kv = {0, 0, 1, 1};
    std::vector<std::vector<double>> w(2, std::vector<double>(2, 1.0));
    return curves::NurbsSurface(grid, ku, kv, w, 1, 1);
}

/// 创建一个测试用的 B-Rep 方体
static BrepModel make_test_box2() {
    return make_box(50, 50, 30);
}

// ===========================================================================
//  CLData / AxisToolpath5 基础测试
// ===========================================================================

TEST(Cam5AxisTest, CLData_DefaultConstruction) {
    CLData cl;
    EXPECT_DOUBLE_EQ(cl.position.x(), 0.0);
    EXPECT_DOUBLE_EQ(cl.position.y(), 0.0);
    EXPECT_DOUBLE_EQ(cl.position.z(), 0.0);
    EXPECT_DOUBLE_EQ(cl.tool_axis.dot(Vector3D::UnitZ()), 1.0);
    EXPECT_DOUBLE_EQ(cl.feed_rate, 500.0);
}

TEST(Cam5AxisTest, CLData_ParameterizedConstruction) {
    Point3D pos(10, 20, 30);
    Vector3D axis(0, 1, 0);
    CLData cl(pos, axis, 800.0);
    EXPECT_DOUBLE_EQ(cl.position.x(), 10.0);
    EXPECT_DOUBLE_EQ(cl.position.y(), 20.0);
    EXPECT_DOUBLE_EQ(cl.position.z(), 30.0);
    EXPECT_NEAR(cl.tool_axis.dot(Vector3D::UnitY()), 1.0, 1e-9);
    EXPECT_DOUBLE_EQ(cl.feed_rate, 800.0);
}

TEST(Cam5AxisTest, CLData_AxisAutoNormalized) {
    Vector3D axis(3, 4, 0);  // length 5
    CLData cl(Point3D::Zero(), axis);
    EXPECT_NEAR(cl.tool_axis.norm(), 1.0, 1e-9);
}

TEST(Cam5AxisTest, AxisToolpath5_Empty) {
    AxisToolpath5 tp;
    EXPECT_TRUE(tp.empty());
    EXPECT_EQ(tp.size(), static_cast<size_t>(0));
}

TEST(Cam5AxisTest, AxisToolpath5_AddPoints) {
    AxisToolpath5 tp;
    tp.points.emplace_back(Point3D(0, 0, 0), Vector3D::UnitZ());
    tp.points.emplace_back(Point3D(1, 0, 0), Vector3D::UnitZ());
    EXPECT_EQ(tp.size(), static_cast<size_t>(2));
    EXPECT_FALSE(tp.empty());
    EXPECT_DOUBLE_EQ(tp.safe_height, 50.0);
}

// ===========================================================================
//  MachineConfig 测试
// ===========================================================================

TEST(Cam5AxisTest, MachineConfig_Defaults) {
    MachineConfig cfg;
    EXPECT_EQ(cfg.machine_type, MachineType::TableTable);
    EXPECT_EQ(cfg.axis_config, MachineAxisConfig::AC);
    EXPECT_DOUBLE_EQ(cfg.tool_length, 100.0);
}

TEST(Cam5AxisTest, MachineConfig_InRange) {
    MachineConfig cfg;
    EXPECT_TRUE(cfg.in_range(0, 0, 0));
    EXPECT_TRUE(cfg.in_range(-90, 0, 180));
    EXPECT_FALSE(cfg.in_range(-150, 0, 0));
    EXPECT_FALSE(cfg.in_range(150, 0, 0));
}

TEST(Cam5AxisTest, MachineConfig_AC_AxisToAngles_ZUp) {
    MachineConfig cfg;
    cfg.axis_config = MachineAxisConfig::AC;
    auto result = cfg.axis_to_angles(Vector3D::UnitZ());
    ASSERT_TRUE(result.has_value());
    auto [a, b, c] = result.value();
    EXPECT_NEAR(a, 0.0, 1e-6);
    EXPECT_NEAR(c, 0.0, 1e-6);
}

TEST(Cam5AxisTest, MachineConfig_AC_AxisToAngles_Tilted) {
    MachineConfig cfg;
    cfg.axis_config = MachineAxisConfig::AC;
    // 45度倾斜：绕 X 旋转 45 度
    Vector3D axis(0, -std::sin(M_PI/4), std::cos(M_PI/4));
    auto result = cfg.axis_to_angles(axis);
    ASSERT_TRUE(result.has_value());
    auto [a, b, c] = result.value();
    EXPECT_NEAR(a, 45.0, 1e-3);
}

TEST(Cam5AxisTest, MachineConfig_BC_AxisToAngles) {
    MachineConfig cfg;
    cfg.axis_config = MachineAxisConfig::BC;
    // Z 轴正向
    auto result = cfg.axis_to_angles(Vector3D::UnitZ());
    ASSERT_TRUE(result.has_value());
    auto [a, b, c] = result.value();
    EXPECT_NEAR(b, 0.0, 1e-3);
}

TEST(Cam5AxisTest, MachineConfig_OutOfRange) {
    MachineConfig cfg;
    cfg.a_min = -30.0;
    cfg.a_max = 30.0;
    cfg.axis_config = MachineAxisConfig::AC;
    // 90度倾斜超出 A 轴范围
    Vector3D axis(0, -1, 0);  // A = 90deg
    auto result = cfg.axis_to_angles(axis);
    // 应返回备选解或 nullopt
    if (result.has_value()) {
        auto [a, b, c] = result.value();
        EXPECT_LE(std::abs(a), 30.0 + 1e-6);
    }
}

// ===========================================================================
//  侧刃加工 swarf_machining 测试
// ===========================================================================

TEST(Cam5AxisTest, SwarfMachining_ProducesOutput) {
    auto surface = make_test_surface();
    Tool tool;
    tool.diameter = 6.0;
    SwarfParams params;
    params.step_over = 1.0;
    params.tilt_angle = 5.0;

    auto tp = swarf_machining(surface, tool, params);
    EXPECT_FALSE(tp.empty());
    EXPECT_GT(tp.size(), static_cast<size_t>(0));
    EXPECT_EQ(tp.name, "Swarf Machining");

    // 所有刀轴应接近单位长度
    for (const auto& cl : tp.points) {
        EXPECT_NEAR(cl.tool_axis.norm(), 1.0, 1e-6);
    }
}

TEST(Cam5AxisTest, SwarfMachining_MultiplePasses) {
    auto surface = make_test_surface();
    Tool tool;
    tool.diameter = 6.0;
    SwarfParams params;
    params.step_over = 2.0;

    auto tp = swarf_machining(surface, tool, params);
    // 至少应该有两行的点
    EXPECT_GT(tp.size(), static_cast<size_t>(30));
}

// ===========================================================================
//  多轴粗加工 multi_axis_roughing 测试
// ===========================================================================

TEST(Cam5AxisTest, MultiAxisRoughing_ProducesOutput) {
    auto box = make_test_box2();
    Tool tool;
    tool.diameter = 10.0;
    MultiAxisRoughingParams params;
    params.step_down = 5.0;
    params.step_over = 6.0;
    params.tilt_angle = 15.0;

    auto tp = multi_axis_roughing(box, tool, params);
    EXPECT_FALSE(tp.empty());
    EXPECT_GT(tp.size(), static_cast<size_t>(0));

    // 所有刀轴应近似相同（3+2 定位）
    Vector3D first_axis = tp.points[0].tool_axis;
    for (const auto& cl : tp.points) {
        EXPECT_NEAR(cl.tool_axis.norm(), 1.0, 1e-6);
        EXPECT_NEAR(cl.tool_axis.dot(first_axis), 1.0, 1e-3);
    }
}

TEST(Cam5AxisTest, MultiAxisRoughing_ZeroStepDownSafe) {
    auto box = make_test_box2();
    Tool tool;
    MultiAxisRoughingParams params;
    params.step_down = 0.0;

    auto tp = multi_axis_roughing(box, tool, params);
    EXPECT_FALSE(tp.empty());
}

// ===========================================================================
//  多轴精加工 multi_axis_finishing 测试
// ===========================================================================

TEST(Cam5AxisTest, MultiAxisFinishing_NormalToSurface) {
    auto surface = make_test_surface();
    auto box = make_test_box2();
    Tool tool;
    tool.diameter = 6.0;
    MultiAxisFinishingParams params;
    params.axis_strategy = ToolAxisStrategy::NormalToSurface;
    params.step_over = 2.0;
    params.collision_check = false;

    auto tp = multi_axis_finishing(surface, box, tool, params);
    EXPECT_FALSE(tp.empty());
    EXPECT_GT(tp.size(), static_cast<size_t>(0));

    // 对于水平面，法线策略应产生近似 Z 轴方向的刀轴
    for (const auto& cl : tp.points) {
        EXPECT_NEAR(cl.tool_axis.norm(), 1.0, 1e-6);
    }
}

TEST(Cam5AxisTest, MultiAxisFinishing_AllStrategies) {
    auto surface = make_test_surface();
    auto box = make_test_box2();
    Tool tool;
    tool.diameter = 6.0;
    MultiAxisFinishingParams params;
    params.step_over = 5.0;
    params.collision_check = false;

    for (auto strategy : {ToolAxisStrategy::FixedAngle,
                          ToolAxisStrategy::LeadLag,
                          ToolAxisStrategy::Tilted,
                          ToolAxisStrategy::NormalToSurface}) {
        params.axis_strategy = strategy;
        auto tp = multi_axis_finishing(surface, box, tool, params);
        EXPECT_FALSE(tp.empty()) << "Strategy " << static_cast<int>(strategy) << " produced empty toolpath";
    }
}

// ===========================================================================
//  inverse_kinematics 运动学逆解测试
// ===========================================================================

TEST(Cam5AxisTest, InverseKinematics_AC_TableTable) {
    // 创建一个简单的 5 轴刀路
    AxisToolpath5 tp;
    tp.name = "Test";
    tp.points.emplace_back(Point3D(10, 0, 0), Vector3D::UnitZ(), 500);
    tp.points.emplace_back(Point3D(0, 10, 0),
                           Vector3D(0, -std::sin(M_PI/4), std::cos(M_PI/4)), 500);

    MachineConfig cfg;
    cfg.machine_type = MachineType::TableTable;
    cfg.axis_config = MachineAxisConfig::AC;

    auto machine_tp = inverse_kinematics(tp, cfg);
    EXPECT_FALSE(machine_tp.empty());
    // 结果应包含机床坐标
    for (const auto& cl : machine_tp.points) {
        // tool_axis 存储 A/B/C 度数
        EXPECT_LE(std::abs(cl.tool_axis.x()), 180.0);
        EXPECT_LE(std::abs(cl.tool_axis.y()), 180.0);
    }
}

TEST(Cam5AxisTest, InverseKinematics_EmptyInput) {
    AxisToolpath5 tp;
    tp.name = "Empty";
    MachineConfig cfg;

    auto result = inverse_kinematics(tp, cfg);
    EXPECT_TRUE(result.empty());
}

// ===========================================================================
//  碰撞检测 check_collision 测试
// ===========================================================================

TEST(Cam5AxisTest, CheckCollision_ClearPath) {
    auto box = make_test_box2();
    Tool tool;
    tool.diameter = 6.0;
    tool.length = 20.0;
    tool.overall_length = 60.0;

    // 刀尖在模型上方远处，不应碰撞
    Point3D pos(0, 0, 80.0);
    Vector3D axis = Vector3D::UnitZ();

    bool collides = check_collision(pos, axis, box, tool);
    EXPECT_FALSE(collides);
}

TEST(Cam5AxisTest, CheckCollision_InsideModel) {
    auto box = make_test_box2();
    Tool tool;
    tool.diameter = 6.0;
    tool.length = 20.0;
    tool.overall_length = 60.0;

    // 刀尖在模型内部
    Point3D pos(0, 0, 15.0);
    Vector3D axis = Vector3D::UnitZ();

    bool collides = check_collision(pos, axis, box, tool);
    // 可能检测到碰撞（取决于 mesh tessellation 精度）
    // 不做严格断言，只验证函数可调用不崩溃
    SUCCEED();
}

// ===========================================================================
//  optimize_tool_axis 刀轴优化测试
// ===========================================================================

TEST(Cam5AxisTest, OptimizeToolAxis_EmptyCandidates) {
    std::vector<Vector3D> candidates;
    auto result = optimize_tool_axis(candidates, MachineConfig{});
    // 应返回 Z 轴
    EXPECT_NEAR(result.dot(Vector3D::UnitZ()), 1.0, 1e-6);
}

TEST(Cam5AxisTest, OptimizeToolAxis_PrefersZAxis) {
    std::vector<Vector3D> candidates = {
        Vector3D(0, 0, 1),     // Z: 得分最高
        Vector3D(0.5, 0, 0.866), // 30° off
        Vector3D(0, -1, 0),     // 90° off (poor)
    };
    auto result = optimize_tool_axis(candidates, MachineConfig{});
    EXPECT_NEAR(result.dot(Vector3D::UnitZ()), 1.0, 1e-6);
}

TEST(Cam5AxisTest, OptimizeToolAxis_FiltersOutOfRange) {
    MachineConfig cfg;
    cfg.a_min = -10.0;
    cfg.a_max = 10.0;
    cfg.axis_config = MachineAxisConfig::AC;

    // 一个超出范围的方向和一个可行方向
    std::vector<Vector3D> candidates = {
        Vector3D(0, -1, 0),     // 90° tilt → 超出范围
        Vector3D(0, 0, 1),      // Z 轴 → 可行
    };
    auto result = optimize_tool_axis(candidates, cfg);
    EXPECT_NEAR(result.dot(Vector3D::UnitZ()), 1.0, 1e-6);
}

// ===========================================================================
//  ToolAxisStrategy 枚举完整性测试
// ===========================================================================

TEST(Cam5AxisTest, ToolAxisStrategy_AllValues) {
    // 确保所有枚举值可被遍历
    EXPECT_EQ(static_cast<int>(ToolAxisStrategy::FixedAngle), 0);
    EXPECT_EQ(static_cast<int>(ToolAxisStrategy::LeadLag), 1);
    EXPECT_EQ(static_cast<int>(ToolAxisStrategy::Tilted), 2);
    EXPECT_EQ(static_cast<int>(ToolAxisStrategy::NormalToSurface), 3);
}

// ===========================================================================
//  MachineConfig 配置完整性测试
// ===========================================================================

TEST(Cam5AxisTest, MachineConfig_AllAxisConfigs) {
    for (auto ac : {MachineAxisConfig::AC, MachineAxisConfig::BC, MachineAxisConfig::AB}) {
        MachineConfig cfg;
        cfg.axis_config = ac;
        // Z 轴方向应为所有配置都可达
        auto result = cfg.axis_to_angles(Vector3D::UnitZ());
        EXPECT_TRUE(result.has_value())
            << "Axis config failed for Z-up direction";
    }
}

TEST(Cam5AxisTest, MachineConfig_UnknownConfig) {
    // AB 配置：X 轴方向
    MachineConfig cfg;
    cfg.axis_config = MachineAxisConfig::AB;
    auto result = cfg.axis_to_angles(Vector3D::UnitX());
    ASSERT_TRUE(result.has_value());
    auto [a, b, c] = result.value();
    // AB 配置没有 C 轴
    EXPECT_DOUBLE_EQ(c, 0.0);
}

// ===========================================================================
//  参数默认值测试
// ===========================================================================

TEST(Cam5AxisTest, SwarfParams_Defaults) {
    SwarfParams p;
    EXPECT_DOUBLE_EQ(p.step_down, 1.0);
    EXPECT_DOUBLE_EQ(p.step_over, 0.5);
    EXPECT_DOUBLE_EQ(p.feed_rate, 600.0);
    EXPECT_DOUBLE_EQ(p.tilt_angle, 0.0);
}

TEST(Cam5AxisTest, MultiAxisRoughingParams_Defaults) {
    MultiAxisRoughingParams p;
    EXPECT_DOUBLE_EQ(p.step_down, 2.0);
    EXPECT_DOUBLE_EQ(p.stock_to_leave, 1.0);
    EXPECT_DOUBLE_EQ(p.tilt_angle, 15.0);
}

TEST(Cam5AxisTest, MultiAxisFinishingParams_Defaults) {
    MultiAxisFinishingParams p;
    EXPECT_DOUBLE_EQ(p.step_over, 0.3);
    EXPECT_EQ(p.axis_strategy, ToolAxisStrategy::NormalToSurface);
    EXPECT_TRUE(p.collision_check);
    EXPECT_DOUBLE_EQ(p.shank_clearance, 2.0);
    EXPECT_DOUBLE_EQ(p.holder_clearance, 5.0);
}