#include "subface_integrator.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
{

double integrator_t::calculate(int gauss_order, double (*func)(double u, double v,
                                              const Eigen::Vector3d& p,
                                              const Eigen::Vector3d& dU,
                                              const Eigen::Vector3d& dV)) const
{
    double total_integral = 0.0;
    for (const auto& [subface_index, param_plane] : m_uv_planes) {
        const auto& subface             = m_subfaces[subface_index].object_ptr.get();
        total_integral += calculate_one_subface(subface, param_plane, gauss_order, func);
    }
    return total_integral;
}


double integrator_t::calculate_one_subface(const subface& subface, const parametric_plane_t& param_plane, int gauss_order, double (*func)(double u, double v,
                                              const Eigen::Vector3d& p,
                                              const Eigen::Vector3d& dU,
                                              const Eigen::Vector3d& dV)) const
{
    auto solver = subface.fetch_solver_evaluator();
    // Gaussian integration in u direction
    auto u_integrand = [&](double u) {
        // Find exact v intersections for each u
        stl_vector_mp<double> v_breaks = find_v_intersections_at_u(subface, param_plane, u);;

        // Gaussian integration in v direction
        auto v_integrand = [&](double v) {
            // Check if point is inside domain
            if (is_point_inside_domain(subface, param_plane, u, v)) {
                try {
                    // Get evaluators for both directions
                    auto eval_du =  subface.fetch_curve_constraint_evaluator(parameter_v_t{}, v); // Fix v, vary u → ∂/∂u
                    auto eval_dv =  subface.fetch_curve_constraint_evaluator(parameter_u_t{}, u); // Fix u, vary 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>();      // Point (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

                    // Area element: ||dU × dV||
                    Eigen::Vector3d cross = dU.cross(dV);
                    double jacobian = cross.norm(); // Jacobian (area scaling factor)
                    return func(u, v, p, dU, dV) * jacobian;
                } catch (...) {
                    return 0.0; // Skip singular points
                }
            }
            return 0.0;         // Point not in domain
        };

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

            // Check midpoint validity
            double mid_v = (a + b) / 2.0;
            if (is_point_inside_domain(subface, param_plane, 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;
    };

    // Integrate in u direction
    const auto& u_breaks = compute_u_breaks(param_plane,0.0, 1.0);
    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_v_intersections_at_u(subface, param_plane,mid_u);

        if (!v_intersections.empty()) { 
            integral += integrate_1D(a, b, u_integrand, gauss_order, is_u_near_singularity(mid_u));
        }
    }

    return integral;
}

/**
 * @brief Compute the u-parameter breakpoints for integration.
 * @note The function currently uses std::set for uniqueness and sorting,
 *       but floating-point precision may cause near-duplicate values to be 
 *       treated as distinct. A tolerance-based comparison is recommended.
 * TODO: Use a tolerance-based approach to avoid floating-point precision issues
 *       when inserting u-values (e.g., merge values within 1e-12).
 */
stl_vector_mp<double> integrator_t::compute_u_breaks(
    const parametric_plane_t& param_plane,
    double u_min,
    double u_max) const
{
    std::set<double> break_set;

    // Insert domain boundaries
    break_set.insert(u_min);
    break_set.insert(u_max);

    // Insert u-values from special vertices (e.g., trimming curve vertices)
    for (size_t i = 0; i < param_plane.chain_vertices.size(); ++i) {
        const auto& vertices     = param_plane.chain_vertices[i];
        auto&       vertex_flags = param_plane.vertex_special_flags[i];
        for (size_t j = 0; j < vertices.size(); ++j) {
            if (vertex_flags[j]) { break_set.insert( vertices[j].x());} // Special vertex u
        }
    }
    // Return as vector (sorted and unique due to set)
    return stl_vector_mp<double>(break_set.begin(), break_set.end());
}


stl_vector_mp<double> integrator_t::find_v_intersections_at_u(
    const subface& subface,
    const parametric_plane_t& param_plane,
    double u_val) const
{
    stl_vector_mp<double> intersections;

    // Iterate over each boundary chain
    for (const auto& chain : param_plane.chain_vertices) {
        const size_t n_vertices = chain.size();
        if (n_vertices < 2) continue;  // Skip degenerate chains

        // Iterate over each edge in the chain (including closing edge if desired)
        for (size_t i = 0; i < n_vertices; ++i) {
            size_t j = (i + 1) % n_vertices;  // Next vertex index (wraps around for closed chain)

            const Eigen::Vector2d& v1 = chain[i];  // Current vertex: (u1, v1)
            const Eigen::Vector2d& v2 = chain[j];  // Next vertex: (u2, v2)

            double u1 = v1.x(), v1_val = v1.y();
            double u2 = v2.x(), v2_val = v2.y();

            // Classify position relative to u = u_val: -1 (left), 0 (on), +1 (right)
            const double eps = 1e-12;
            auto sign_cmp = [u_val, eps](double u) -> int {
                if (u < u_val - eps) return -1;
                if (u > u_val + eps) return  1;
                return 0;
            };

            // Then use it
            int pos1 = sign_cmp(u1);
            int pos2 = sign_cmp(u2);

            // Case 1: Both endpoints on the same side (and not on the line) → no intersection
            if (pos1 * pos2 > 0) {
                continue;
            }

            // Case 2: Both endpoints lie exactly on u = u_val → add both v-values
            if (pos1 == 0 && pos2 == 0) {
                intersections.push_back(v1_val);
                intersections.push_back(v2_val);
            }
            // Case 3: One endpoint on u = u_val or segment crosses u = u_val
            else if (std::abs(u1 - u2) < eps) {
                // Vertical segment: if aligned with u_val, treat as overlapping
                if (pos1 == 0 || pos2 == 0) {
                    intersections.push_back(v1_val);
                    intersections.push_back(v2_val);
                }
            }
            else {
                // General case: non-vertical segment crossing u = u_val
                // Compute linear interpolation parameter t
                double t = (u_val - u1) / (u2 - u1);
                double v_initial = v1_val + t * (v2_val - v1_val);  // Initial guess for v

                // Fetch evaluators from subface for constraint solving
                auto curve_evaluator  = subface.fetch_curve_constraint_evaluator(parameter_u_t{}, u_val);
                auto solver_evaluator = subface.fetch_solver_evaluator();

                // Define target function: v ↦ residual of implicit equation
                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));
                    return std::get<internal::implicit_equation_intermediate>(full_res);
                };

                // Refine solution using Newton-Raphson method
                try {
                    double v_solution = newton_method(target_function, v_initial);
                    intersections.push_back(v_solution);
                } catch (...) {
                    // If Newton's method fails (e.g., divergence), fall back to linear approximation
                    intersections.push_back(v_initial);
                }
            }
        }
    }

    // Final step: sort and remove duplicates within tolerance
    if (!intersections.empty()) {
        sort_and_unique_with_tol(intersections, 1e-8);
    }

    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(
    const subface& subface,
    const parametric_plane_t& param_plane, 
    double u,
    double v) const
{
    auto intersections = find_v_intersections_at_u(subface, param_plane, u);
    const double tol_near = 1e-8;
    const double tol_above = 1e-12;

    uint32_t count = 0;
    for (double v_int : intersections) {
        double diff = v_int - v;
        if (diff > tol_above) {
            count++;
        }
        else if (std::abs(diff) < tol_near) {
            return true; // on boundary → inside
        }
    }
    return (count % 2) == 1;
}

bool integrator_t::is_u_near_singularity(double u, double tol) const
{
    return false;
}

void integrator_t::sort_and_unique_with_tol(stl_vector_mp<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