ViewDesignEngine/include/vde/brep/constraint_solver.h

#pragma once
/**
 * @file constraint_solver.h
 * @brief 3D 装配约束求解器 — 约束图 + Newton-Raphson + DOF 分析
 *
 * 为 3D 装配提供完整的约束求解系统：
 *   - ConstraintGraph: 约束图（节点=零件, 边=约束）
 *   - Newton-Raphson 迭代求解
 *   - DOF 分析（每个零件 6 DOF）
 *   - 过度约束检测（冗余约束识别 + 报告）
 *   - 联动求解（增量更新）
 *
 * @ingroup brep
 */

#include "vde/brep/assembly.h"
#include "vde/core/point.h"
#include "vde/core/transform.h"
#include "vde/core/aabb.h"
#include <vector>
#include <string>
#include <memory>
#include <unordered_map>
#include <unordered_set>
#include <functional>
#include <optional>

namespace vde::brep {

// ═══════════════════════════════════════════════════════════
//  ConstraintType
// ═══════════════════════════════════════════════════════════

/// 3D 约束类型
enum class ConstraintType {
    Coincident,     ///< 面-面贴合（共面，法向相反）
    Concentric,     ///< 轴-轴对齐（同轴）
    Tangent,        ///< 面相切接触
    Distance,       ///< 固定面间距
    Angle,          ///< 固定面夹角（弧度）
    Parallel,       ///< 面法线平行
    Perpendicular   ///< 面法线垂直
};

/// 约束类型转字符串
[[nodiscard]] inline const char* constraint_type_name(ConstraintType t) {
    switch (t) {
        case ConstraintType::Coincident:    return "Coincident";
        case ConstraintType::Concentric:    return "Concentric";
        case ConstraintType::Tangent:       return "Tangent";
        case ConstraintType::Distance:      return "Distance";
        case ConstraintType::Angle:         return "Angle";
        case ConstraintType::Parallel:      return "Parallel";
        case ConstraintType::Perpendicular: return "Perpendicular";
    }
    return "Unknown";
}

/// 每个约束消除的自由度数（估算）
[[nodiscard]] inline int constraint_dof_elimination(ConstraintType t) {
    switch (t) {
        case ConstraintType::Coincident:    return 3;  // 1 平移 + 2 旋转
        case ConstraintType::Concentric:    return 4;  // 2 平移 + 2 旋转
        case ConstraintType::Tangent:       return 1;  // 1 平移
        case ConstraintType::Distance:      return 1;  // 1 平移
        case ConstraintType::Angle:         return 1;  // 1 旋转
        case ConstraintType::Parallel:      return 2;  // 2 旋转
        case ConstraintType::Perpendicular: return 2;  // 2 旋转
    }
    return 0;
}

// ═══════════════════════════════════════════════════════════
//  Constraint — 单个约束边
// ═══════════════════════════════════════════════════════════

/// 单个约束描述
struct Constraint {
    int node_a;            ///< 约束的零件 A 索引
    int node_b;            ///< 约束的零件 B 索引
    ConstraintType type;   ///< 约束类型
    double value = 0.0;    ///< 约束值(Distance=distance, Angle=radians)
    double weight = 1.0;   ///< 权重（用于过约束系统的加权最小二乘）
    std::string name;      ///< 约束名称（可选）

    Constraint() = default;
    Constraint(int a, int b, ConstraintType t, double v = 0.0, double w = 1.0)
        : node_a(a), node_b(b), type(t), value(v), weight(w) {}
};

// ═══════════════════════════════════════════════════════════
//  DOFInfo — 每个节点的自由度信息
// ═══════════════════════════════════════════════════════════

/// 单个零件的自由度分析结果
struct DOFInfo {
    int total_dof = 6;               ///< 起始 DOF（空间刚体：6）
    int eliminated_dof = 0;          ///< 已消除的 DOF
    int remaining_dof = 6;           ///< 剩余 DOF
    bool is_fixed = false;           ///< 是否完全约束
    std::vector<int> constraining;   ///< 约束此零件的约束索引列表
};

// ═══════════════════════════════════════════════════════════
//  ConstraintGraph
// ═══════════════════════════════════════════════════════════

/**
 * @brief 3D 约束图
 *
 * 节点 = 装配零件（AssemblyNode），边 = 约束。
 * 提供图遍历、DOF 分析、过度约束检测。
 *
 * @ingroup brep
 */
class ConstraintGraph {
public:
    /// 添加零件节点
    /// @param name 零件名称
    /// @param node 零件节点指针
    /// @return 节点索引
    int add_node(const std::string& name, AssemblyNode* node);

    /// 添加约束边
    /// @param c 约束描述
    /// @return 约束索引
    int add_constraint(const Constraint& c);

    /// 删除约束
    /// @param index 约束索引
    void remove_constraint(int index);

    /// 获取节点数
    [[nodiscard]] int node_count() const { return static_cast<int>(nodes_.size()); }

    /// 获取约束数
    [[nodiscard]] int constraint_count() const { return static_cast<int>(constraints_.size()); }

    /// 获取节点名
    [[nodiscard]] const std::string& node_name(int idx) const;

    /// 获取节点指针
    [[nodiscard]] AssemblyNode* node_ptr(int idx) const;

    /// 获取约束
    [[nodiscard]] const Constraint& constraint(int idx) const;

    /// 获取可修改约束引用（用于增量更新）
    [[nodiscard]] Constraint& constraint_mut(int idx);

    /// 获取某节点的所有相邻约束
    [[nodiscard]] std::vector<int> node_constraints(int node_idx) const;

    /// 获取所有节点索引
    [[nodiscard]] const std::vector<int>& node_indices() const { return node_ids_; }

    // ── DOF 分析 ──

    /**
     * @brief 执行 DOF 分析
     *
     * 对每个零件计算自由度状态：
     *   - 初始 6 DOF（3 平移 + 3 旋转）
     *   - 每个约束消除若干 DOF
     *   - 返回每个节点的 DOFInfo
     *
     * @return 节点索引 → DOF 信息
     */
    [[nodiscard]] std::vector<DOFInfo> analyze_dof() const;

    /**
     * @brief 计算单个节点的剩余 DOF
     * @param node_idx 节点索引
     * @return 剩余自由度数
     */
    [[nodiscard]] int remaining_dof(int node_idx) const;

    /**
     * @brief 检查图是否完全约束（所有节点 fixed）
     * @return true 如果所有节点 DOF=0
     */
    [[nodiscard]] bool is_fully_constrained() const;

    // ── 过度约束检测 ──

    /**
     * @brief 过度约束检测结果
     */
    struct OverConstraintInfo {
        int node_index;            ///< 哪个节点
        int eliminated_dof;        ///< 已消除 DOF
        bool over_constrained;     ///< 是否过度约束（eliminated > 6）
        std::vector<int> redundant; ///< 冗余约束索引列表
        std::string message;       ///< 人类可读的消息
    };

    /**
     * @brief 检测过度约束
     *
     * 对每个节点检查：已消除 DOF > 6 即为过度约束。
     * 识别并报告冗余约束。
     *
     * @return 过度约束节点列表
     */
    [[nodiscard]] std::vector<OverConstraintInfo> detect_over_constraints() const;

    // ── 图结构查询 ──

    /// 获取所有约束
    [[nodiscard]] const std::vector<Constraint>& constraints() const { return constraints_; }

    /// 检查两个节点之间是否已存在约束
    [[nodiscard]] bool has_constraint_between(int a, int b) const;

    /// 获取两个节点之间的所有约束
    [[nodiscard]] std::vector<int> constraints_between(int a, int b) const;

    /// 清空图
    void clear();

private:
    struct NodeInfo {
        std::string name;
        AssemblyNode* ptr = nullptr;
    };

    std::vector<NodeInfo> nodes_;
    std::vector<int> node_ids_;         // 压缩的索引映射
    std::vector<Constraint> constraints_;
    std::vector<bool> constraint_active_; // 约束是否激活（未删除）

    /// 邻接表：node_idx → [constraint_idx...]
    std::vector<std::vector<int>> adjacency_;
};

// ═══════════════════════════════════════════════════════════
//  Newton-Raphson 求解器
// ═══════════════════════════════════════════════════════════

/**
 * @brief Newton-Raphson 求解器配置
 */
struct NRSolverConfig {
    int    max_iterations = 100;   ///< 最大迭代次数
    double tolerance = 1e-6;       ///< 收敛容差
    double step_size = 0.5;        ///< 步长衰减因子（0~1，防震荡）
    double damping = 0.5;          ///< 阻尼因子
    bool   check_over_constrained = true;  ///< 求解前检测过度约束
};

/**
 * @brief 求解器状态
 */
struct SolveResult {
    bool converged = false;               ///< 是否收敛
    int  iterations = 0;                  ///< 实际迭代次数
    double final_error = 0.0;             ///< 最终残差 L2 范数
    std::vector<double> error_history;    ///< 每步误差历史
    std::string message;                  ///< 人类可读的消息
};

/**
 * @brief Newton-Raphson 约束求解器
 *
 * 将约束表示为方程系统 f(x)=0，使用 Newton-Raphson 迭代求解。
 *
 * 状态向量 x = [t1_x, t1_y, t1_z, r1_x, r1_y, r1_z, t2_..., ...]
 * 每个零件有 6 个参数：3 平移 + 3 旋转（轴-角表示的一部分，用增量旋转）。
 *
 * 每一步：
 *   1. 评估约束函数 f(x) → 残差向量
 *   2. 数值计算 Jacobian J = ∂f/∂x
 *   3. 求解 J·Δx = -f(x)
 *   4. 更新 x ← x + α·Δx（α 为步长）
 *
 * @ingroup brep
 */
class NewtonRaphsonSolver {
public:
    /**
     * @brief 求解约束图
     *
     * @param graph      约束图
     * @param assembly   装配体（将被修改以满足约束）
     * @param config     求解器配置
     * @return SolveResult 求解结果
     */
    [[nodiscard]] SolveResult solve(
        ConstraintGraph& graph,
        Assembly& assembly,
        const NRSolverConfig& config = NRSolverConfig{});

    /**
     * @brief 增量求解：修改单个约束值后更新
     *
     * 在已有求解结果基础上，仅修改一个约束值，
     * 从当前状态出发重新求解（热启动）。
     *
     * @param graph         约束图
     * @param assembly      装配体
     * @param constraint_idx 被修改的约束索引
     * @param new_value      新的约束值
     * @param config         求解器配置
     * @return SolveResult
     */
    [[nodiscard]] SolveResult incremental_solve(
        ConstraintGraph& graph,
        Assembly& assembly,
        int constraint_idx,
        double new_value,
        const NRSolverConfig& config = NRSolverConfig{});

    /// 获取最后一次求解的状态向量
    [[nodiscard]] const std::vector<double>& last_state() const { return state_; }

private:
    int n_parts_ = 0;                ///< 零件数
    std::vector<double> state_;      ///< 状态向量 [tx,ty,tz,rx,ry,rz]*n
    std::vector<double> last_error_; ///< 最近误差向量

    /// 从 assembly 读取变换到状态向量
    void read_state(const ConstraintGraph& graph, const Assembly& assembly);

    /// 将状态向量写回 assembly
    void write_state(const ConstraintGraph& graph, Assembly& assembly) const;

    /// 评估所有约束函数，填充残差向量 f
    /// @return 残差的 L2 范数
    double evaluate_constraints(
        const ConstraintGraph& graph,
        const Assembly& assembly,
        std::vector<double>& residual) const;

    /// 数值计算 Jacobian（中心差分）
    void compute_jacobian(
        const ConstraintGraph& graph,
        Assembly& assembly,
        const std::vector<double>& residual,
        Eigen::MatrixXd& J);

    /// 单个约束的评估函数
    double evaluate_single_constraint(
        const Constraint& c,
        const AssemblyNode& na,
        const AssemblyNode& nb) const;

    /// 求解线性最小二乘问题 J·Δx = -f
    Eigen::VectorXd solve_linear_step(
        const Eigen::MatrixXd& J,
        const Eigen::VectorXd& f) const;
};

// ═══════════════════════════════════════════════════════════
//  向后兼容 API（保留旧接口）
// ═══════════════════════════════════════════════════════════

/// @deprecated 使用 ConstraintType 代替
using Constraint3D = ConstraintType;

/// @deprecated 使用 Constraint 代替
struct ConstraintEntry : public Constraint {
    using Constraint::Constraint;
};

/**
 * @brief 简化版求解器（向后兼容）
 *
 * 使用迭代松弛法求解约束系统。
 *
 * @param assembly      目标装配体
 * @param constraints   约束列表（node_a, node_b 为 root.children 中的直接索引）
 * @param max_iterations 最大迭代次数
 * @param tolerance      收敛容差
 * @return true 如果所有约束在容差内满足
 */
[[nodiscard]] bool solve_constraints(
    Assembly& assembly,
    const std::vector<ConstraintEntry>& constraints,
    int max_iterations = 100,
    double tolerance = 1e-6);

/// 应用贴合约束
[[nodiscard]] core::Transform3D apply_coincident(
    const AssemblyNode& node_a, AssemblyNode& node_b);

/// 应用同轴约束
[[nodiscard]] core::Transform3D apply_concentric(
    const AssemblyNode& node_a, AssemblyNode& node_b);

/// 应用距离约束
[[nodiscard]] core::Transform3D apply_distance(
    const AssemblyNode& node_a, AssemblyNode& node_b, double distance);

} // namespace vde::brep