#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>
#include <tuple>

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;
};

// ═══════════════════════════════════════════════════════════
//  DOFAnalyzer — 详细自由度分析器
// ═══════════════════════════════════════════════════════════

/// DOF 分解方向描述
struct DOFDirection {
    /// 平移方向：x, y, z 是否被约束
    bool tx_free = true, ty_free = true, tz_free = true;
    /// 旋转方向：绕 x, y, z 是否被约束
    bool rx_free = true, ry_free = true, rz_free = true;
};

/// DOF 分析详细结果
struct DOFDetailedResult {
    int node_index = -1;
    std::string node_name;
    int translational_dof_remaining = 3;  ///< 剩余平移自由度
    int rotational_dof_remaining = 3;     ///< 剩余旋转自由度
    DOFDirection free_directions;         ///< 哪些方向未约束
    std::vector<int> constraints_used;    ///< 涉及约束索引
    bool is_fully_constrained = false;    ///< 完全约束
    bool is_over_constrained = false;     ///< 过度约束
    std::string summary;                  ///< 可读摘要
};

/**
 * @brief 零件自由度分析器
 *
 * 将每个零件的 6 个 DOF 分解为平移（3）和旋转（3），
 * 根据约束类型计算每个方向上的自由/约束状态。
 *
 * @ingroup brep
 */
class DOFAnalyzer {
public:
    /// 执行全图 DOF 分析
    [[nodiscard]] std::vector<DOFDetailedResult> analyze(const ConstraintGraph& graph) const;

    /// 获取单个零件的自由方向
    [[nodiscard]] DOFDirection free_directions(const ConstraintGraph& graph, int node_idx) const;

    /// 列出零件剩余的自由运动方向描述
    [[nodiscard]] std::vector<std::string> free_direction_names(
        const ConstraintGraph& graph, int node_idx) const;

    /// 分析所有约束后的全局 DOF 状态
    [[nodiscard]] std::string global_dof_summary(const ConstraintGraph& graph) const;

private:
    /// 根据约束类型，返回对 node_a 的平移/旋转约束影响
    static void constraint_impact(ConstraintType type, bool is_node_a,
                                  int& trans_count, int& rot_count,
                                  DOFDirection& dir);
};

// ═══════════════════════════════════════════════════════════
//  RedundancyDetector — 冗余约束检测器
// ═══════════════════════════════════════════════════════════

/// 冗余约束建议
struct RedundancySuggestion {
    int constraint_index = -1;     ///< 冗余约束索引
    int node_index = -1;           ///< 受影响的节点
    std::string reason;            ///< 原因描述
    int dof_impact = 0;            ///< 移除后释放的 DOF 数
    std::string action;            ///< 建议操作（删除/修改/保留）
};

/// 约束冲突信息
struct ConstraintConflict {
    int constraint_a = -1;
    int constraint_b = -1;
    int shared_node = -1;          ///< 冲突所在的公共节点
    std::string description;
};

/**
 * @brief 冗余约束检测器
 *
 * 检测约束图中的冗余约束和约束冲突：
 *   - 类型级冗余：同节点对之间存在多个相同类型的约束
 *   - 拓扑级冗余：约束消除的 DOF 总量超出 6
 *   - 语义冲突：约束类型相互矛盾（如 Coincident + Distance ≠ 0）
 *
 * @ingroup brep
 */
class RedundancyDetector {
public:
    /// 全图检测冗余
    [[nodiscard]] std::vector<RedundancySuggestion> detect(const ConstraintGraph& graph) const;

    /// 针对特定节点检测
    [[nodiscard]] std::vector<RedundancySuggestion> detect_for_node(
        const ConstraintGraph& graph, int node_idx) const;

    /// 检测约束冲突
    [[nodiscard]] std::vector<ConstraintConflict> detect_conflicts(
        const ConstraintGraph& graph) const;

    /// 生成可读报告
    [[nodiscard]] std::string report(const ConstraintGraph& graph) const;

private:
    /// 判断两个约束是否冲突
    static bool are_conflicting(ConstraintType a, ConstraintType b);
};

// ═══════════════════════════════════════════════════════════
//  ConstraintPropagator — 约束传播器
// ═══════════════════════════════════════════════════════════

/// 传播结果
struct PropagateResult {
    bool success = false;
    std::vector<int> affected_nodes;       ///< 受影响的节点索引
    std::vector<int> affected_constraints; ///< 受影响的约束索引
    std::vector<std::string> changes;      ///< 变更描述
    std::string message;                   ///< 可读消息
};

/**
 * @brief 约束传播器
 *
 * 修改一个约束后，沿依赖图传播更新：
 *   1. 计算图拓扑排序（BFS 从固定节点出发）
 *   2. 标记受影响的下游节点
 *   3. 增量重新求解受影响的部分
 *
 * @ingroup brep
 */
class ConstraintPropagator {
public:
    /**
     * @brief 修改约束并传播
     *
     * @param graph          约束图
     * @param assembly       装配体
     * @param constraint_idx 被修改约束索引
     * @param new_value      新值
     * @param new_type       新类型（可选：不修改则设为相同值）
     * @param solver         求解器（可选：用于增量求解）
     * @return 传播结果
     */
    [[nodiscard]] PropagateResult propagate(
        ConstraintGraph& graph,
        Assembly& assembly,
        int constraint_idx,
        double new_value,
        ConstraintType new_type,
        NewtonRaphsonSolver* solver = nullptr);

    /// 计算依赖传播顺序（BFS）
    [[nodiscard]] std::vector<int> compute_dependency_order(
        const ConstraintGraph& graph, int start_node) const;

    /// 获取约束之间的依赖深度
    [[nodiscard]] int dependency_depth(
        const ConstraintGraph& graph, int a_idx, int b_idx) const;
};

// ═══════════════════════════════════════════════════════════
//  KinematicChainSolver — 闭环机构求解器
// ═══════════════════════════════════════════════════════════

/// 运动学链的连杆描述
struct ChainLink {
    int node_index = -1;          ///< 对应的约束图节点
    double length = 1.0;          ///< 连杆长度（相对于前一节点）
    double twist_angle = 0.0;     ///< 扭转角（弧度）
    double offset = 0.0;          ///< 偏距
    std::string label;            ///< 标签
};

/// 闭环条件
struct LoopClosure {
    int start_node = -1;          ///< 环路起始节点
    int end_node = -1;            ///< 环路终止节点
    ConstraintType closure_type = ConstraintType::Coincident;
    double closure_value = 0.0;   ///< 目标间距/角度
    double tolerance = 1e-6;      ///< 容差
};

/// 运动学求解结果
struct KinematicResult {
    bool solved = false;
    int iterations = 0;
    double final_error = 0.0;
    std::vector<double> joint_angles;      ///< 求解出的关节角度
    std::vector<core::Transform3D> poses;  ///< 求解出的位姿
    std::string message;
};

/**
 * @brief 闭环机构求解器
 *
 * 求解闭环运动学链的关节角度：
 *   - 将闭环约束表示为非线性方程组
 *   - 使用 Newton-Raphson 迭代求解
 *   - 支持任意长度的串行链 + 末端闭合条件
 *
 * @ingroup brep
 */
class KinematicChainSolver {
public:
    /** @brief DH 参数法：单个连杆变换（公开，供测试使用） */
    static core::Transform3D dh_transform(double theta, double d,
                                          double a, double alpha);

    /** @brief 求解闭环运动学链 */
    [[nodiscard]] KinematicResult solve_closed_chain(
        ConstraintGraph& graph,
        Assembly& assembly,
        const std::vector<ChainLink>& links,
        const LoopClosure& closure);

    /** @brief 前向运动学：给定关节角 → 位姿 */
    [[nodiscard]] std::vector<core::Transform3D> forward_kinematics(
        const Assembly& assembly,
        const std::vector<ChainLink>& links,
        const std::vector<double>& joint_angles) const;

    /** @brief 逆向运动学：给定目标位姿 → 关节角 */
    [[nodiscard]] std::vector<double> inverse_kinematics(
        const Assembly& assembly,
        const std::vector<ChainLink>& links,
        const core::Transform3D& target_pose) const;

    /** @brief 检查链是否可达 */
    [[nodiscard]] bool is_reachable(
        const std::vector<ChainLink>& links,
        const core::Point3D& target) const;

private:
    /// 评估闭环残差
    static double evaluate_closure(
        const Assembly& assembly,
        const std::vector<ChainLink>& links,
        const std::vector<double>& angles,
        const LoopClosure& closure);
};

// ═══════════════════════════════════════════════════════════
//  向后兼容 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