#pragma once
#include "vde/core/point.h"
#include <vector>
#include <string>
#include <unordered_map>

namespace vde::sketch {
using core::Point2D;
using core::Vector2D;

/**
 * @brief 草图点元素
 * @ingroup sketch
 */
struct SketchPoint {
    int id;            ///< 唯一标识
    Point2D pos;       ///< 2D 坐标
    bool fixed = false; ///< 是否固定（排除自由度）
};

/**
 * @brief 草图线段元素
 * @ingroup sketch
 */
struct SketchLine {
    int id;            ///< 唯一标识
    int p0, p1;        ///< 起止点 ID
};

/**
 * @brief 草图圆元素
 * @ingroup sketch
 */
struct SketchCircle {
    int id;            ///< 唯一标识
    int center;        ///< 圆心点 ID
    double radius;     ///< 半径
};

/**
 * @brief 约束类型枚举
 *
 * 每个约束类型对应一个残差方程，方程数见注释：
 * - 距离类约束：1 个方程
 * - 位置/重合类：2 个方程（x + y）
 *
 * @ingroup sketch
 */
enum class ConstraintType {
    Horizontal,     ///< 水平线: y1 - y0 = 0  [1 eq]
    Vertical,       ///< 垂直线: x1 - x0 = 0  [1 eq]
    Parallel,       ///< 两线平行: cross(d0, d1) = 0  [1 eq]
    Perpendicular,  ///< 两线垂直: dot(d0, d1) = 0  [1 eq]
    Coincident,     ///< 两点重合: |p1-p0| = 0  [2 eqs: Δx=0, Δy=0]
    EqualLength,    ///< 等长度: |d0| - |d1| = 0  [1 eq]
    Distance,       ///< 距离约束: |p1-p0| - d = 0  [1 eq]
    Radius,         ///< 半径约束  [1 eq]
    Tangent,        ///< 相切  [1 eq]
    Concentric,     ///< 同心  [2 eqs]
    FixedPoint,     ///< 固定点（硬约束，排除变量）[0 eq — 系统预处理]
    Fixed,          ///< 固定点位置: x-x0=0, y-y0=0  [2 eqs]
    Angle,          ///< 角度约束: atan2(cross(d0,d1), dot(d0,d1)) - θ = 0  [1 eq]
};

/**
 * @brief 约束实例
 * @ingroup sketch
 */
struct Constraint {
    ConstraintType type;              ///< 约束类型
    std::vector<int> elements;        ///< 引用的元素索引（点/线/圆 ID）
    double value = 0.0;               ///< 距离/角度值
};

/**
 * @brief 求解器返回结果
 * @ingroup sketch
 */
struct SolverResult {
    bool converged = false;           ///< 是否收敛
    int iterations = 0;               ///< 实际迭代次数
    double residual = 0.0;            ///< 最终残差（均方根）
    std::vector<Point2D> points;      ///< 求解后的点坐标
    std::string message;              ///< 诊断信息（成功或失败原因）
};

/**
 * @brief 二维几何约束求解器
 *
 * 基于 Gauss-Newton 非线性最小二乘的 2D 草图约束求解器。
 * 支持点、线段、圆元素的水平/垂直/平行/垂直/相切/距离/角度等约束。
 *
 * 求解流程：
 * 1. 构建变量向量（非固定点的 x,y 坐标）
 * 2. 对每个约束方程计算残差向量 r(x)
 * 3. 数值 Jacobian J = ∂r/∂x（中心差分）
 * 4. Gauss-Newton 步：Δx = -(J^T J)^{-1} J^T r
 * 5. x ← x + Δx，重复直到收敛或 max_iter
 *
 * 自由度分析：
 *   DOF = 2·N_free_points - Σ(约束方程数)
 *   DOF > 0 → 欠约束（解不唯一）
 *   DOF = 0 → 恰定（应有唯一解）
 *   DOF < 0 → 过约束（最小二乘意义下求解）
 *
 * @code{.cpp}
 * ConstraintSolver solver;
 * int p0 = solver.add_point(0, 0);            // 原点
 * int p1 = solver.add_point(100, 0);
 * int p2 = solver.add_point(100, 50);
 * solver.add_line(p0, p1);                    // 底边
 * solver.fix_point(p0);                       // 原点固定
 * solver.add_constraint(ConstraintType::Horizontal, {p0, p1});
 * solver.add_constraint(ConstraintType::Distance, {p0, p2}, 100.0);
 *
 * auto result = solver.solve();
 * if (result.converged) {
 *     auto p = result.points;
 * }
 * @endcode
 *
 * @ingroup sketch
 */
class ConstraintSolver {
public:
    /**
     * @brief 添加自由点
     * @param x X 坐标
     * @param y Y 坐标
     * @param fixed 是否固定（true 则排除变量）
     * @return 点 ID
     */
    int add_point(double x, double y, bool fixed = false);

    /**
     * @brief 添加线段
     * @param p0 起点 ID
     * @param p1 终点 ID
     * @return 线段 ID
     */
    int add_line(int p0, int p1);

    /**
     * @brief 添加圆
     * @param center 圆心点 ID
     * @param radius 半径
     * @return 圆 ID
     */
    int add_circle(int center, double radius);

    /**
     * @brief 添加约束
     * @param type 约束类型
     * @param elements 约束涉及的元素 ID 列表
     * @param val 约束值（距离、角度等），默认 0
     *
     * @note 不同类型 elements 的语义：
     *       Horizontal/Vertical: {p0, p1}
     *       Parallel/Perpendicular: {l0_p0, l0_p1, l1_p0, l1_p1}
     *       Distance: {p0, p1}, val = 距离
     *       Angle: {l0_p0, l0_p1, l1_p0, l1_p1}, val = 角度（度）
     *       Radius: {circle_id}, val = 半径
     *       Tangent: {line_p0, line_p1, circle_id}
     *       Coincident: {p0, p1}
     */
    void add_constraint(ConstraintType type, const std::vector<int>& elements, double val = 0.0);

    /**
     * @brief 标记点为固定（硬约束）
     * @param pid 点 ID
     * @note 固定点不参与变量优化，从自由度中排除
     */
    void fix_point(int pid) {
        if (pid >= 0 && pid < static_cast<int>(points_.size()))
            points_[pid].fixed = true;
    }

    /**
     * @brief 执行约束求解
     * @param max_iter 最大 Gauss-Newton 迭代次数，默认 100
     * @param tol 收敛容差（梯度范数 < tol 时停止），默认 1e-8
     * @return SolverResult 求解结果
     *
     * @note 收敛判断：||J^T r|| < tol
     * @note 欠约束时（DOF > 0）解不唯一；过约束时（DOF < 0）为最小二乘解
     */
    [[nodiscard]] SolverResult solve(int max_iter = 100, double tol = 1e-8);

    /**
     * @brief 计算自由度
     * @return DOF = 2·N_free - Σ(约束方程数)
     *
     * @note DOF < 0 时系统过约束，求解器仍然工作但解非精确满足所有约束
     */
    [[nodiscard]] int degrees_of_freedom() const;

    /**
     * @brief 当前点集访问
     * @return SketchPoint 数组只读引用
     */
    [[nodiscard]] const std::vector<SketchPoint>& points() const { return points_; }

    /**
     * @brief 查询点坐标
     * @param id 点 ID
     * @return Point2D 当前坐标
     */
    [[nodiscard]] Point2D get_point(int id) const;

private:
    std::vector<SketchPoint> points_;       ///< 点集
    std::vector<SketchLine> lines_;         ///< 线段集
    std::vector<SketchCircle> circles_;     ///< 圆集
    std::vector<Constraint> constraints_;   ///< 约束集

    /**
     * @brief 将求解器变量向量同步回 points_（用于 Jacobian 数值计算）
     * @param vars 变量数组（长度为 2·N_free）
     * @param nv 变量数
     */
    void sync_from_vars(const double* vars, int nv);

    /**
     * @brief 评估单个约束的残差值
     * @param c 约束
     * @return 残差（方程 → 0 表示约束满足）
     */
    double eval_constraint(const Constraint& c) const;

    /**
     * @brief 计算单个约束的方程数
     * @param c 约束
     * @return Coincident/Fixed → 2，其余 → 1
     */
    int count_equations(const Constraint& c) const;

    /**
     * @brief 填充 Jacobian 矩阵中对应约束 ci 的行
     * @param c 约束
     * @param ci 约束索引
     * @param row_map 方程行映射
     * @param J_data Jacobian 矩阵数据（行优先）
     * @param nv 变量数
     * @param stride 行跨度
     */
    void fill_jacobian_rows(const Constraint& c, int ci,
                            const std::vector<int>& row_map,
                            double* J_data, int nv, int stride) const;
};

}  // namespace vde::sketch