#include "SurfaceIntegrator.hpp"
#include "quadrature.hpp"
#include <Eigen/Geometry> // For vector and cross product operations
#include <cmath>          // For math functions like sqrt
#include <set>
#include <iostream>

namespace internal
{

// Constructor 1: Initialize only with a reference to the surface
integrator_t::integrator_t(const subface& surface) : m_surface(surface) {}

// Constructor 2: Initialize with surface and u-breaks (e.g., trimming curves)
integrator_t::integrator_t(const subface&               surface,
                                             const stl_vector_mp<double>& u_breaks,
                                             double                       umin,
                                             double                       umax,
                                             double                       vmin,
                                             double                       vmax)
    : m_surface(surface), m_u_breaks(u_breaks), Umin(umin), Umax(umax), Vmin(vmin), Vmax(vmax)
{
}

integrator_t::integrator_t(const subface& surface, const parametric_plane& uv_plane)
    : m_surface(surface), m_uv_plane(uv_plane), Umin(0.0), Umax(0.0), Vmin(0.0), Vmax(0.0)
{
    if (!uv_plane.chain_vertices.empty()) {
        // 初始化为第一个点的坐标
        double min_u = uv_plane.chain_vertices[0].x();
        double max_u = uv_plane.chain_vertices[0].x();
        double min_v = uv_plane.chain_vertices[0].y();
        double max_v = uv_plane.chain_vertices[0].y();

        // 遍历所有链顶点
        for (const auto& pt : uv_plane.chain_vertices) {
            double u = pt.x();
            double v = pt.y();

            if (u < min_u) min_u = u;
            if (u > max_u) max_u = u;
            if (v < min_v) min_v = v;
            if (v > max_v) max_v = v;
        }

        Umin = min_u;
        Umax = max_u;
        Vmin = min_v;
        Vmax = max_v;
    } else {
        // 没有顶点时使用默认范围 [0, 1] × [0, 1]
        Umin = 0.0;
        Umax = 1.0;
        Vmin = 0.0;
        Vmax = 1.0;
    }

    std::set<uint32_t> unique_vertex_indices;

    // 插入所有类型的顶点索引
    unique_vertex_indices.insert(uv_plane.singularity_vertices.begin(), uv_plane.singularity_vertices.end());
    unique_vertex_indices.insert(uv_plane.polar_vertices.begin(), uv_plane.polar_vertices.end());
    unique_vertex_indices.insert(uv_plane.parallel_start_vertices.begin(), uv_plane.parallel_start_vertices.end());

    std::set<double> unique_u_values;

    for (uint32_t idx : unique_vertex_indices) {
        if (idx < uv_plane.chain_vertices.size()) {
            double u = uv_plane.chain_vertices[idx].x();
            unique_u_values.insert(u);
        }
    }

    // 转换为 vector 并排序（set 默认是有序的）
    m_u_breaks = stl_vector_mp<double>(unique_u_values.begin(), unique_u_values.end());
}

// Set u-breaks (optional trimming or partitioning lines)
void integrator_t::set_ubreaks(const stl_vector_mp<double>& u_breaks) { m_u_breaks = u_breaks; }

// Main entry point to compute surface area
template <typename Func>
double integrator_t::calculate(Func&& func, int gauss_order) const
{
    auto solver = m_surface.fetch_solver_evaluator();
    // 在u方向进行高斯积分
    auto u_integrand = [&](double u) {
        // 对每个u，找到v方向的精确交点
        std::vector<double> v_breaks = find_vertical_intersections(u);

        // 在v方向进行高斯积分
        auto v_integrand = [&](double v) {
            // 判断点是否在有效域内
            if (is_point_inside_domain(u, v)) {
                try {
                    // 获取两个方向的 evaluator
                    auto eval_du =  m_surface.fetch_curve_constraint_evaluator(parameter_v_t{}, v); // 固定 v，变 u → 得到 ∂/∂u
                    auto eval_dv =  m_surface.fetch_curve_constraint_evaluator(parameter_u_t{}, u); // 固定 u，变 v → 得到 ∂/∂v

                    auto res_u = eval_du(u); // f(u,v), grad_f = ∂r/∂u
                    auto res_v = eval_dv(v); // f(u,v), grad_f = ∂r/∂v

                    Eigen::Vector3d p  = res_u.f.template head<3>();      // 点坐标 (x,y,z)
                    Eigen::Vector3d dU = res_u.grad_f.template head<3>(); // ∂r/∂u
                    Eigen::Vector3d dV = res_v.grad_f.template head<3>(); // ∂r/∂v

                    // ✅ 计算面积元：||dU × dV||
                    Eigen::Vector3d cross = dU.cross(dV);
                    double jacobian = cross.norm(); // 雅可比行列式（面积缩放因子）
                    return func(u, v, p, dU, dV) * jacobian;
                } catch (...) {
                    return 0.0; // 跳过奇异点
                }
            }
            return 0.0;         // 点不在有效域内
        };

        double v_integral = 0.0;
        for (size_t i = 0; i < v_breaks.size() - 1; ++i) {
            double a = v_breaks[i];
            double b = v_breaks[i + 1];

            // 检查区间中点是否有效
            double mid_v = (a + b) / 2.0;
            if (is_point_inside_domain(u, mid_v)) {
                v_integral += integrate_1D(a, b, v_integrand, gauss_order);
            } else {
                std::cout << "uv out of domain: (" << u << "," << mid_v << ")" << std::endl;
            }
        }

        return v_integral;
    };

    // 在u方向积分
    double integral = 0.0;
    for (size_t i = 0; i < u_breaks.size() - 1; ++i) {
        double a = u_breaks[i];
        double b = u_breaks[i + 1];

        // 检查区间中点是否有效
        double mid_u           = (a + b) / 2.0;
        auto   v_intersections = find_vertical_intersections(mid_u, self.outerEdges, self.Vmin, self.Vmax);

        if (!v_intersections.empty()) { // 确保该u区间有有效区域

            integral += integrate_1D(a, b, u_integrand, gauss_order, is_u_near_singularity(mid_u));
        }
    }

    return integral;
}

// 在 integrator_t 类中添加：
double integrator_t::compute_volume(int gauss_order) const
{
    double total_volume = 0.0;

    // 外层：对 u 分段积分
    auto u_integrand = [&](double u) {
        std::vector<double> v_breaks = find_vertical_intersections(u);
        double v_integral = 0.0;

        // 内层：对 v 积分
        auto v_integrand = [&](double v) -> double {
            if (!is_point_inside_domain(u, v)) {
                return 0.0;
            }

            try {
                // 获取偏导数
                auto eval_du = m_surface.fetch_curve_constraint_evaluator(parameter_v_t{}, v); // ∂/∂u
                auto eval_dv = m_surface.fetch_curve_constraint_evaluator(parameter_u_t{}, u); // ∂/∂v

                auto res_u = eval_du(u);
                auto res_v = eval_dv(v);

                Eigen::Vector3d p  = res_u.f.template head<3>();      // r(u,v)
                Eigen::Vector3d dU = res_u.grad_f.template head<3>(); // r_u
                Eigen::Vector3d dV = res_v.grad_f.template head<3>(); // r_v

                // 计算 r · (r_u × r_v)
                Eigen::Vector3d cross = dU.cross(dV);
                double mixed_product = p.dot(cross);

                return mixed_product; // 注意：不是 norm，是点积
            } catch (...) {
                return 0.0;
            }
        };

        for (size_t i = 0; i < v_breaks.size() - 1; ++i) {
            double a = v_breaks[i];
            double b = v_breaks[i + 1];
            double mid_v = (a + b) / 2.0;
            if (is_point_inside_domain(u, mid_v)) {
                v_integral += integrate_1D(a, b, v_integrand, gauss_order);
            }
        }

        return v_integral;
    };

    // 在 u 方向积分
    for (size_t i = 0; i < m_u_breaks.size() - 1; ++i) {
        double a = m_u_breaks[i];
        double b = m_u_breaks[i + 1];
        double mid_u = (a + b) / 2.0;
        auto v_intersections = find_vertical_intersections(mid_u);
        if (!v_intersections.empty()) {
            total_volume += integrate_1D(a, b, u_integrand, gauss_order, is_u_near_singularity(mid_u));
        }
    }

    // 乘以 1/3
    return std::abs(total_volume) / 3.0;
}

// 直线u=u_val与边界的交点
std::vector<double> integrator_t::find_vertical_intersections(double u_val) const
{
    std::vector<double>  intersections;
    std::vector<int32_t> uPositionFlags;
    uPositionFlags.reserve(m_uv_plane.chain_vertices.size());

    std::transform(m_uv_plane.chain_vertices.begin(),
                   m_uv_plane.chain_vertices.end(),
                   std::back_inserter(uPositionFlags),
                   [&](const auto& currentVertex) {
                       double uDifference = currentVertex.x() - u_val;
                       if (uDifference < 0) return -1; // 在参考值左侧
                       if (uDifference > 0) return 1;  // 在参考值右侧
                       return 0;                       // 等于参考值
                   });

    uint32_t group_idx = 0;
    for (uint32_t element_idx = 0; element_idx < m_uv_plane.chains.index_group.size() - 1; element_idx++) {
        if (element_idx > m_uv_plane.chains.start_indices[group_idx + 1]) group_idx++;
        if (element_idx + 1 < m_uv_plane.chains.start_indices[group_idx + 1]) {
            uint32_t vertex_idx1 = m_uv_plane.chains.index_group[element_idx];
            uint32_t vertex_idx2 = m_uv_plane.chains.index_group[element_idx + 1];
            if (uPositionFlags[vertex_idx1] * uPositionFlags[vertex_idx2] <= 0) {
                auto v1 = m_uv_plane.chain_vertices[vertex_idx1], v2 = m_uv_plane.chain_vertices[vertex_idx2];
                if (v1.x() != v2.x()) {
                    // "The line segment is vertical (u₁ == u₂), so there is no unique v value corresponding to the given u."
                    double v_initial        = v1.y() + (v2.y() - v1.y()) * (u_val - v1.x()) / (v2.x() - v1.x());
                    auto   curve_evaluator  = m_surface.fetch_curve_constraint_evaluator(parameter_u_t{}, u_val); 
                    auto   solver_evaluator = m_surface.fetch_solver_evaluator();
                    auto   target_function  = [&](double v) -> internal::implicit_equation_intermediate {
                        constraint_curve_intermediate temp_res = curve_evaluator(v);
                        auto full_res = solver_evaluator(std::move(temp_res));
                        // ensure solver_eval returns implicit_equation_intermediate)
                        return std::get<internal::implicit_equation_intermediate>(full_res);
                    };
                    double v_solution = newton_method(target_function, v_initial);
                    intersections.push_back(v_solution);
                } else {
                    intersections.push_back(v1.y());
                    intersections.push_back(v2.y());
                }
            }
        }
    }

    // 去重排序
    sort_and_unique_with_tol(intersections);
    return intersections;
}

/*
 point (u, v) is inside the domain by ray-casting algorithm
 To determine whether a point (u, v) is inside or outside a domain by counting the intersections of a vertical ray starting
 from the point and extending upwards,
 NOTE:  when v_intersect - v < threshold, further checks are required to accurately determine if the intersection point lies
 precisely on the boundary segment.
*/
bool integrator_t::is_point_inside_domain(double u, double v) const
{
    bool is_implicit_equation_intermediate = m_surface.is_implicit_equation_intermediate();
    uint32_t group_idx = 0, intersection_count = 0;
    for (uint32_t element_idx = 0; element_idx < m_uv_plane.chains.index_group.size() - 1; element_idx++) {
        if (element_idx > m_uv_plane.chains.start_indices[group_idx + 1]) group_idx++;
        if (element_idx + 1 < m_uv_plane.chains.start_indices[group_idx + 1]) {
            uint32_t vertex_idx1 = m_uv_plane.chains.index_group[element_idx];
            uint32_t vertex_idx2 = m_uv_plane.chains.index_group[element_idx + 1];
            auto     v1 = m_uv_plane.chain_vertices[vertex_idx1], v2 = m_uv_plane.chain_vertices[vertex_idx2];
            if ((v1.x() <= u && v2.x() > u) || (v2.x() < u && v1.x() >= u)) {
                double v_initial = v1.y() + (v2.y() - v1.y()) * (u - v1.x()) / (v2.x() - v1.x());
                if (v_initial - v >= 1e-6) {
                    intersection_count++;
                } else if (std::abs(v_initial - v) < 1e-6) {
                    // Only use Newton's method for implicit surfaces (scalar equation)
                    //    Newton requires f(v) and df/dv as scalars — only implicit provides this.
                    //    Skip parametric surfaces (vector residual) — treat initial guess as final
                    if (is_implicit_equation_intermediate) {
                        auto curve_evaluator  = m_surface.fetch_curve_constraint_evaluator(parameter_u_t{}, u);
                        auto solver_evaluator = m_surface.fetch_solver_evaluator();
                        auto target_function  = [&](double v) -> internal::implicit_equation_intermediate {
                            constraint_curve_intermediate temp_res = curve_evaluator(v);
                            auto full_res = solver_evaluator(std::move(temp_res));
                            // ensure solver_eval returns implicit_equation_intermediate)
                            return std::get<internal::implicit_equation_intermediate>(full_res);
                        };
                        double v_solution = newton_method(target_function, v_initial);
                        if (std::abs(v_solution - v) > 0) { intersection_count++; }
                    }
                    else{
                        continue;
                    }
                    
                }
            }
            /*
            case v1.x() == v2.x() == u, do not count as intersection.but will cout in next iteration
            */
        }
    }
    return intersection_count % 2 == 1; // in domain
}

bool integrator_t::is_u_near_singularity(double u, double tol) const
{
    for (auto idx : m_uv_plane.singularity_vertices) {
        double singular_u = m_uv_plane.chain_vertices[idx].x();
        if (std::abs(u - singular_u) < tol) { return true; }
    }
    // 可扩展：判断是否靠近极点、极性顶点等
    for (auto idx : m_uv_plane.polar_vertices) {
        double polar_u = m_uv_plane.chain_vertices[idx].x();
        if (std::abs(u - polar_u) < tol) { return true; }
    }
    return false;
}

void integrator_t::sort_and_unique_with_tol(std::vector<double>& vec, double epsilon) const
{
    if (vec.empty()) return;

    std::sort(vec.begin(), vec.end());

    size_t write_index = 0;
    for (size_t read_index = 1; read_index < vec.size(); ++read_index) {
        if (std::fabs(vec[read_index] - vec[write_index]) > epsilon) {
            ++write_index;
            vec[write_index] = vec[read_index];
        }
    }

    vec.resize(write_index + 1);
}

// Only accepts functions that return implicit_equation_intermediate
double newton_method(
    const std::function<internal::implicit_equation_intermediate(double)>& F,
    double v_initial,
    double tolerance,
    int max_iterations)
{
    double v = v_initial;

    for (int i = 0; i < max_iterations; ++i) {
        auto res = F(v);           //  Known type: implicit_equation_intermediate
        double f_val = res.f;
        double df_val = res.df;

        if (std::abs(f_val) < tolerance) {
            std::cout << "Converged at v = " << v << std::endl;
            return v;
        }

        if (std::abs(df_val) < 1e-10) {
            std::cerr << "Derivative near zero." << std::endl;
            return v;
        }

        v = v - f_val / df_val;
    }

    std::cerr << "Newton failed to converge." << std::endl;
    return v;
}


} // namespace internal