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

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

struct SketchPoint { int id; Point2D pos; bool fixed = false; };
struct SketchLine { int id, p0, p1; };
struct SketchCircle { int id, center; double radius; };

enum class ConstraintType {
    Horizontal,     // 水平线: y1 - y0 = 0
    Vertical,       // 垂直线: x1 - x0 = 0
    Parallel,       // 两线平行: cross(d0, d1) = 0
    Perpendicular,  // 两线垂直: dot(d0, d1) = 0
    Coincident,     // 两点重合: |p1-p0| = 0
    EqualLength,    // 等长度: |d0| - |d1| = 0
    Distance,       // 距离约束: |p1-p0| - d = 0
    Radius,         // 半径约束
    Tangent,        // 相切
    Concentric,     // 同心
    FixedPoint,     // 固定点（硬约束，排除变量）
    Fixed,          // 固定点位置: x-x0=0, y-y0=0
    Angle,          // 角度约束: atan2(cross(d0,d1), dot(d0,d1)) - θ = 0
};

struct Constraint {
    ConstraintType type;
    std::vector<int> elements;  // 引用的元素索引
    double value = 0.0;         // 距离/角度值
};

struct SolverResult {
    bool converged = false;
    int iterations = 0;
    double residual = 0.0;
    std::vector<Point2D> points;
    std::string message;
};

class ConstraintSolver {
public:
    int add_point(double x, double y, bool fixed = false);
    int add_line(int p0, int p1);
    int add_circle(int center, double radius);
    void add_constraint(ConstraintType type, const std::vector<int>& elements, double val = 0.0);
    void fix_point(int pid) {
        if (pid >= 0 && pid < static_cast<int>(points_.size()))
            points_[pid].fixed = true;
    }

    [[nodiscard]] SolverResult solve(int max_iter = 100, double tol = 1e-8);
    [[nodiscard]] int degrees_of_freedom() const;
    [[nodiscard]] const std::vector<SketchPoint>& points() const { return points_; }
    [[nodiscard]] Point2D get_point(int id) const;

private:
    std::vector<SketchPoint> points_;
    std::vector<SketchLine> lines_;
    std::vector<SketchCircle> circles_;
    std::vector<Constraint> constraints_;

    // 辅助：同步 vars → points_（用于 Jacobian 数值计算）
    void sync_from_vars(const double* vars, int nv);

    // 约束残差评估
    double eval_constraint(const Constraint& c) const;

    // 约束方程数（Coincident/Fixed → 2，其他 → 1）
    int count_equations(const Constraint& c) const;

    // Jacobian 填充：将约束 ci 的 Jacobian 行写入矩阵 J
    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