#pragma once
// Copyright (c) 2024-2026 Tarik Moussa.
// SPDX-License-Identifier: MIT

// euclidean_hessian.hpp
//
// Analytical Hessian of the Euclidean discrete conformal energy —
// the cotangent-Laplace operator.
//
// Ported from de.varylab.discreteconformal.functional.EuclideanCyclicFunctional
// (the hessian() method).
//
// ┌──────────────────────────────────────────────────────────────────────────┐
// │  Hessian formula (vertex DOFs only)                                      │
// │                                                                          │
// │  For a face (v1, v2, v3) with effective log-lengths Λ̃ij and             │
// │  side lengths lij = exp(Λ̃ij/2):                                         │
// │                                                                          │
// │    t12 = −l12+l23+l31,  t23 = l12−l23+l31,  t31 = l12+l23−l31         │
// │    l123 = l12+l23+l31,  denom2 = 8·Area  (Area via Kahan's stable      │
// │                          side-length formula — see euclidean_cot_weights)│
// │                                                                          │
// │  Cotangent at vertex k (opposite t_opp, adjacent t_a and t_b):          │
// │    cot_k = (t_opp · l123 − t_a · t_b) / denom2                         │
// │                                                                          │
// │  Assignment (k ↔ opposite edge ↔ opposite t-value):                     │
// │    cot1 (v1, opp l23): t_opp=t23, t_a=t12, t_b=t31                     │
// │    cot2 (v2, opp l31): t_opp=t31, t_a=t12, t_b=t23                     │
// │    cot3 (v3, opp l12): t_opp=t12, t_a=t23, t_b=t31                     │
// │                                                                          │
// │  Hessian contributions per face (Pinkall–Polthier ½ factor):            │
// │    H[vi, vi] += (cot_vj + cot_vk) / 2   (diagonal)                     │
// │    H[vi, vj] −= cot_vk / 2              (off-diagonal, variable pairs)  │
// │                                                                          │
// │  Pinned vertices (v_idx = −1) contribute to diagonal of neighbours but  │
// │  do not create a column/row in H themselves.                             │
// └──────────────────────────────────────────────────────────────────────────┘
//
// Requires Eigen (header-only).  The Hessian is returned as an
// Eigen::SparseMatrix<double> for direct use in the Phase-4 Newton solver
// (Eigen::SimplicialLDLT).
//
// The Hessian is symmetric positive semi-definite for any valid mesh with
// no degenerate faces.  The null space is spanned by the uniform-shift
// vector 1 on closed surfaces (Euler characteristic = 0).

#include "euclidean_functional.hpp"
#include <Eigen/Sparse>
#include <vector>
#include <array>
#include <cmath>
#include <algorithm>
#include <stdexcept>

namespace conformallab {

// ── Cotangent weight helper ───────────────────────────────────────────────────
//
// Given three Euclidean SIDE LENGTHS l12, l23, l31 (already exp(Λ̃/2)),
// return the three cotangent weights (cot1, cot2, cot3).
//
// cot_k = (t_opp · l123 − t_a · t_b) / (8·Area)
//
// where t_opp is the t-value of the edge OPPOSITE vertex k, and t_a, t_b are
// the t-values of the two edges ADJACENT to vertex k.  See the box comment at
// the top of this file for the explicit assignment of t_opp/t_a/t_b per vertex.
//
// Returns {0,0,0} for degenerate faces (triangle inequality violated or Area=0).
/// Three Euclidean cotangent weights `(cot1, cot2, cot3)` for the
/// vertices opposite to edges (l₂₃, l₃₁, l₁₂) of a triangle, plus a
/// `valid` flag that is `false` when the triangle is degenerate.
struct EuclCotWeights {
    double cot1; ///< Cotangent at vertex 1 (opposite to l₂₃).
    double cot2; ///< Cotangent at vertex 2 (opposite to l₃₁).
    double cot3; ///< Cotangent at vertex 3 (opposite to l₁₂).
    bool   valid;///< `false` when the triangle is degenerate (triangle inequality violated or area = 0).
};

/// Compute the three Euclidean cotangent weights from edge lengths.
/// Returns `{0,0,0,false}` for degenerate triangles.
inline EuclCotWeights euclidean_cot_weights(double l12, double l23, double l31)
{
    const double t12 = -l12 + l23 + l31;
    const double t23 = +l12 - l23 + l31;
    const double t31 = +l12 + l23 - l31;

    if (t12 <= 0.0 || t23 <= 0.0 || t31 <= 0.0)
        return {0.0, 0.0, 0.0, false};

    const double l123 = l12 + l23 + l31;

    // Triangle area via Kahan's numerically stable formula.  Sort the side
    // lengths a ≥ b ≥ c and evaluate with the cancellation-avoiding grouping
    //   Area = ¼·√[ (a+(b+c))·(c−(a−b))·(c+(a−b))·(a+(b−c)) ].
    // This replaces the naive 2·√(t12·t23·t31·l123): that product of
    // near-equal-length differences loses precision for needle/cap triangles,
    // where it would feed large relative error straight into the cotangent
    // weights and the linear solve.  The result denom2 = 8·Area is identical
    // up to rounding for well-shaped triangles, but accurate for slivers.
    double a = l12, b = l23, c = l31;
    if (a < b) std::swap(a, b);
    if (a < c) std::swap(a, c);
    if (b < c) std::swap(b, c);   // now a ≥ b ≥ c

    const double kahan = (a + (b + c)) * (c - (a - b))
                       * (c + (a - b)) * (a + (b - c));
    if (kahan <= 0.0) return {0.0, 0.0, 0.0, false};

    const double denom2 = 8.0 * (0.25 * std::sqrt(kahan));   // = 8·Area

    // Formula: cot_k = (t_opp · l123 − t_a · t_b) / denom2
    //   cot1: t_opp=t23, t_a=t12, t_b=t31   (v1 opposite l23)
    //   cot2: t_opp=t31, t_a=t12, t_b=t23   (v2 opposite l31)
    //   cot3: t_opp=t12, t_a=t23, t_b=t31   (v3 opposite l12)
    return {
        (t23 * l123 - t31 * t12) / denom2,  // cot1
        (t31 * l123 - t12 * t23) / denom2,  // cot2
        (t12 * l123 - t23 * t31) / denom2,  // cot3
        true
    };
}

/// Analytical Euclidean Hessian (cotangent Laplacian), sparse.
/// Only vertex DOFs are supported — the function asserts that no edge
/// DOF is variable.  `x` is used to compute effective log-lengths Λ̃ᵢⱼ.
inline Eigen::SparseMatrix<double> euclidean_hessian(
    ConformalMesh&             mesh,
    const std::vector<double>& x,
    const EuclideanMaps&       m)
{
    const int n = euclidean_dimension(mesh, m);

    // Only the vertex block of the cyclic Hessian is implemented here.  If any
    // edge DOF is variable (assign_euclidean_all_dof_indices), the edge-edge and
    // vertex-edge blocks present in the Java reference (conformalHessian) are
    // missing, which would leave singular zero rows/cols.  Fail loudly instead
    // of silently returning a rank-deficient matrix (matches the doc contract).
    for (auto e : mesh.edges()) {
        if (m.e_idx[e] >= 0)
            throw std::logic_error(
                "euclidean_hessian: edge DOFs are not supported "
                "(only the vertex-block cotangent Laplacian is implemented)");
    }

    // Collect triplets (row, col, value) — setFromTriplets sums duplicates.
    std::vector<Eigen::Triplet<double>> trips;
    trips.reserve(static_cast<std::size_t>(n) * 7);   // rough estimate

    for (auto f : mesh.faces()) {
        Halfedge_index h0 = mesh.halfedge(f);
        Halfedge_index h1 = mesh.next(h0);
        Halfedge_index h2 = mesh.next(h1);

        Vertex_index v1 = mesh.source(h0);
        Vertex_index v2 = mesh.source(h1);
        Vertex_index v3 = mesh.source(h2);

        Edge_index e12 = mesh.edge(h0);
        Edge_index e23 = mesh.edge(h1);
        Edge_index e31 = mesh.edge(h2);

        // Effective log-lengths.
        double u1 = eucl_dof_val(m.v_idx[v1], x);
        double u2 = eucl_dof_val(m.v_idx[v2], x);
        double u3 = eucl_dof_val(m.v_idx[v3], x);

        double lam12 = m.lambda0[e12] + u1 + u2 + eucl_dof_val(m.e_idx[e12], x);
        double lam23 = m.lambda0[e23] + u2 + u3 + eucl_dof_val(m.e_idx[e23], x);
        double lam31 = m.lambda0[e31] + u3 + u1 + eucl_dof_val(m.e_idx[e31], x);

        // Side lengths (centered to avoid overflow, same as euclidean_angles).
        const double mu  = (lam12 + lam23 + lam31) / 6.0;
        const double l12 = std::exp((lam12 - 2.0 * mu) * 0.5);
        const double l23 = std::exp((lam23 - 2.0 * mu) * 0.5);
        const double l31 = std::exp((lam31 - 2.0 * mu) * 0.5);

        auto [cot1, cot2, cot3, valid] = euclidean_cot_weights(l12, l23, l31);
        if (!valid) continue;

        const int i1 = m.v_idx[v1];
        const int i2 = m.v_idx[v2];
        const int i3 = m.v_idx[v3];

        // ── Diagonal contributions ──────────────────────────────────────────
        // H[v1,v1] += (cot2 + cot3)/2  (Pinkall–Polthier factor of 1/2)
        // H[v2,v2] += (cot3 + cot1)/2
        // H[v3,v3] += (cot1 + cot2)/2
        if (i1 >= 0) trips.emplace_back(i1, i1, (cot2 + cot3) * 0.5);
        if (i2 >= 0) trips.emplace_back(i2, i2, (cot3 + cot1) * 0.5);
        if (i3 >= 0) trips.emplace_back(i3, i3, (cot1 + cot2) * 0.5);

        // ── Off-diagonal contributions (only for variable pairs) ────────────
        // Edge v1-v2 opposite α3: H[v1,v2] -= cot3/2
        if (i1 >= 0 && i2 >= 0) {
            trips.emplace_back(i1, i2, -cot3 * 0.5);
            trips.emplace_back(i2, i1, -cot3 * 0.5);
        }
        // Edge v2-v3 opposite α1: H[v2,v3] -= cot1/2
        if (i2 >= 0 && i3 >= 0) {
            trips.emplace_back(i2, i3, -cot1 * 0.5);
            trips.emplace_back(i3, i2, -cot1 * 0.5);
        }
        // Edge v3-v1 opposite α2: H[v3,v1] -= cot2/2
        if (i3 >= 0 && i1 >= 0) {
            trips.emplace_back(i3, i1, -cot2 * 0.5);
            trips.emplace_back(i1, i3, -cot2 * 0.5);
        }
    }

    Eigen::SparseMatrix<double> H(n, n);
    H.setFromTriplets(trips.begin(), trips.end());
    return H;
}

// ── Block-FD Hessian (cyclic: vertex + edge DOFs) ────────────────────────────
//
// The analytic `euclidean_hessian` above covers only the vertex-block cotangent
// Laplacian.  The *cyclic* functional adds edge DOFs λ_e, giving vertex-edge and
// edge-edge Hessian blocks.  We obtain the full Hessian by per-face block FD,
// mirroring `hyper_ideal_hessian_block_fd`.
//
// Locality lemma: the gradient decomposes by face,
//     G_v = Θ_v − Σ_{f∋v} α_v(f)            (vertex output  = −α_v)
//     G_e = Σ_{f∋e} α_opp(f) − φ_e          (edge output    = +α_opp)
// and α at face f depends ONLY on f's 6 local DOFs (u₁,u₂,u₃,λ₁₂,λ₂₃,λ₃₁), so
//     ∂G_x/∂y = Σ_{f: x,y ∈ local(f)} ∂(output)/∂y at f.
// Accumulating per-face 6×6 blocks therefore reproduces the full Hessian and is
// consistent with `euclidean_gradient` by construction (it is its FD).

/// Per-face contributions to the cyclic gradient, in slot order
/// (v₁, v₂, v₃, e₁₂, e₂₃, e₃₁).  Vertex slots carry −α (since G_v = Θ−Σα),
/// edge slots carry +α_opp (since G_e = Σα_opp − φ).
inline std::array<double, 6> eucl_face_local_outputs(
    double u1, double u2, double u3,
    double d12, double d23, double d31,
    double lam0_12, double lam0_23, double lam0_31)
{
    const double lam12 = lam0_12 + u1 + u2 + d12;
    const double lam23 = lam0_23 + u2 + u3 + d23;
    const double lam31 = lam0_31 + u3 + u1 + d31;
    const auto fa = euclidean_angles(lam12, lam23, lam31);
    // edge slot e12 ↔ opposite corner α3, e23 ↔ α1, e31 ↔ α2 (see gradient Pass 3).
    return { -fa.alpha1, -fa.alpha2, -fa.alpha3,
             +fa.alpha3, +fa.alpha1, +fa.alpha2 };
}

/// Block-FD Euclidean cyclic Hessian (vertex + edge DOFs), sparse.
/// Supports both vertex-only and cyclic DOF layouts; cost `F·12` face-angle
/// evaluations.  Consistent with `euclidean_gradient` to `O(ε²)`.
inline Eigen::SparseMatrix<double> euclidean_hessian_block_fd(
    ConformalMesh&             mesh,
    const std::vector<double>& x,
    const EuclideanMaps&       m,
    double                     eps = 1e-5)
{
    const int n = euclidean_dimension(mesh, m);
    std::vector<Eigen::Triplet<double>> trips;
    trips.reserve(static_cast<std::size_t>(36) * mesh.number_of_faces());

    for (auto f : mesh.faces()) {
        Halfedge_index h0 = mesh.halfedge(f);
        Halfedge_index h1 = mesh.next(h0);
        Halfedge_index h2 = mesh.next(h1);

        Vertex_index v1 = mesh.source(h0);
        Vertex_index v2 = mesh.source(h1);
        Vertex_index v3 = mesh.source(h2);
        Edge_index   e12 = mesh.edge(h0);
        Edge_index   e23 = mesh.edge(h1);
        Edge_index   e31 = mesh.edge(h2);

        const int idx[6] = {
            m.v_idx[v1], m.v_idx[v2], m.v_idx[v3],
            m.e_idx[e12], m.e_idx[e23], m.e_idx[e31]
        };
        const double lam0_12 = m.lambda0[e12];
        const double lam0_23 = m.lambda0[e23];
        const double lam0_31 = m.lambda0[e31];

        const double vals[6] = {
            eucl_dof_val(idx[0], x), eucl_dof_val(idx[1], x), eucl_dof_val(idx[2], x),
            eucl_dof_val(idx[3], x), eucl_dof_val(idx[4], x), eucl_dof_val(idx[5], x)
        };

        for (int j = 0; j < 6; ++j) {
            if (idx[j] < 0) continue;   // pinned: no column

            double vp[6], vm[6];
            for (int k = 0; k < 6; ++k) { vp[k] = vm[k] = vals[k]; }
            vp[j] += eps;
            vm[j] -= eps;

            auto Op = eucl_face_local_outputs(
                vp[0], vp[1], vp[2], vp[3], vp[4], vp[5], lam0_12, lam0_23, lam0_31);
            auto Om = eucl_face_local_outputs(
                vm[0], vm[1], vm[2], vm[3], vm[4], vm[5], lam0_12, lam0_23, lam0_31);

            for (int i = 0; i < 6; ++i) {
                if (idx[i] < 0) continue;
                const double val = (Op[static_cast<std::size_t>(i)]
                                  - Om[static_cast<std::size_t>(i)]) / (2.0 * eps);
                if (std::abs(val) > 1e-15)
                    trips.emplace_back(idx[i], idx[j], val);
            }
        }
    }

    Eigen::SparseMatrix<double> H(n, n);
    H.setFromTriplets(trips.begin(), trips.end());
    return H;
}

/// Symmetrised block-FD Euclidean cyclic Hessian: `(H + Hᵀ)/2`.
inline Eigen::SparseMatrix<double> euclidean_hessian_block_fd_sym(
    ConformalMesh&             mesh,
    const std::vector<double>& x,
    const EuclideanMaps&       m,
    double                     eps = 1e-5)
{
    auto H = euclidean_hessian_block_fd(mesh, x, m, eps);
    Eigen::SparseMatrix<double> Ht = H.transpose();
    return (H + Ht) * 0.5;
}

// ── Analytic cyclic Hessian (vertex + edge DOFs) ─────────────────────────────
//
// Closed-form Jacobian of the cyclic gradient.  With s_i = effective log-length
// (Λ̃) of the edge OPPOSITE vertex i (s = 2·ln ℓ), the Euclidean corner-angle
// derivatives are (derived from the law of cosines; Σ_j ∂α_i/∂s_j = 0):
//
//     ∂α_i/∂s_i =  ℓ_i² / (4A)
//     ∂α_i/∂s_j =  ½ cot α_i  −  ℓ_j² / (4A)      (j ≠ i)
//
// where A is the triangle area (4A = denom2/2, denom2 = 8A from the cot helper).
// Chaining through  s₁ = Λ̃(e₂₃), s₂ = Λ̃(e₃₁), s₃ = Λ̃(e₁₂)  and
// Λ̃_e = λ⁰_e + u_i + u_j + λ_e  gives the per-face 6×6 block over the local
// DOFs (u₁,u₂,u₃,λ₁₂,λ₂₃,λ₃₁); per-face outputs carry the gradient signs
// (−α vertex, +α_opp edge), so the assembled matrix equals ∂G/∂x exactly.
// This is the analytic counterpart of `euclidean_hessian_block_fd` (no FD).

/// Analytic Euclidean cyclic Hessian (vertex + edge DOFs), sparse.
/// Closed-form (no finite differences); matches `euclidean_hessian_block_fd`
/// to round-off and the analytic vertex-only Laplacian on vertex-only layouts.
inline Eigen::SparseMatrix<double> euclidean_hessian_analytic(
    ConformalMesh&             mesh,
    const std::vector<double>& x,
    const EuclideanMaps&       m)
{
    const int n = euclidean_dimension(mesh, m);
    std::vector<Eigen::Triplet<double>> trips;
    trips.reserve(static_cast<std::size_t>(36) * mesh.number_of_faces());

    for (auto f : mesh.faces()) {
        Halfedge_index h0 = mesh.halfedge(f);
        Halfedge_index h1 = mesh.next(h0);
        Halfedge_index h2 = mesh.next(h1);

        Vertex_index v1 = mesh.source(h0);
        Vertex_index v2 = mesh.source(h1);
        Vertex_index v3 = mesh.source(h2);
        Edge_index   e12 = mesh.edge(h0);
        Edge_index   e23 = mesh.edge(h1);
        Edge_index   e31 = mesh.edge(h2);

        const double u1 = eucl_dof_val(m.v_idx[v1], x);
        const double u2 = eucl_dof_val(m.v_idx[v2], x);
        const double u3 = eucl_dof_val(m.v_idx[v3], x);

        const double lam12 = m.lambda0[e12] + u1 + u2 + eucl_dof_val(m.e_idx[e12], x); // s3
        const double lam23 = m.lambda0[e23] + u2 + u3 + eucl_dof_val(m.e_idx[e23], x); // s1
        const double lam31 = m.lambda0[e31] + u3 + u1 + eucl_dof_val(m.e_idx[e31], x); // s2

        // Centered side lengths (scale-invariant for ℓ²/4A and cot).
        const double mu  = (lam12 + lam23 + lam31) / 6.0;
        const double l12 = std::exp((lam12 - 2.0 * mu) * 0.5);
        const double l23 = std::exp((lam23 - 2.0 * mu) * 0.5);
        const double l31 = std::exp((lam31 - 2.0 * mu) * 0.5);

        const double t12 = -l12 + l23 + l31;
        const double t23 =  l12 - l23 + l31;
        const double t31 =  l12 + l23 - l31;
        if (t12 <= 0.0 || t23 <= 0.0 || t31 <= 0.0) continue;
        const double l123 = l12 + l23 + l31;
        const double denom2_sq = t12 * t23 * t31 * l123;
        if (denom2_sq <= 0.0) continue;
        const double denom2 = 2.0 * std::sqrt(denom2_sq);   // = 8A
        const double fourA  = denom2 * 0.5;                 // = 4A

        auto cw = euclidean_cot_weights(l12, l23, l31);
        if (!cw.valid) continue;
        const double cot[3] = { cw.cot1, cw.cot2, cw.cot3 };  // at v1, v2, v3

        // ℓ_i² = squared length of edge opposite vertex i:
        //   opp(v1)=e23=l23,  opp(v2)=e31=l31,  opp(v3)=e12=l12
        const double Lsq[3] = { l23 * l23, l31 * l31, l12 * l12 };

        // dA[i][j] = ∂α_i/∂s_j   (i,j ∈ {0,1,2}; s_j opposite v_{j+1})
        double dA[3][3];
        for (int i = 0; i < 3; ++i)
            for (int j = 0; j < 3; ++j)
                dA[i][j] = (i == j) ? (Lsq[i] / fourA)
                                    : (0.5 * cot[i] - Lsq[j] / fourA);

        // ∂α_i/∂(local dof), dof order (u1,u2,u3, λ12,λ23,λ31).
        // u1 ↔ s2,s3 ; u2 ↔ s1,s3 ; u3 ↔ s1,s2 ; λ12 ↔ s3 ; λ23 ↔ s1 ; λ31 ↔ s2.
        double g[3][6];
        for (int i = 0; i < 3; ++i) {
            g[i][0] = dA[i][1] + dA[i][2];  // u1
            g[i][1] = dA[i][0] + dA[i][2];  // u2
            g[i][2] = dA[i][0] + dA[i][1];  // u3
            g[i][3] = dA[i][2];             // λ12 (s3)
            g[i][4] = dA[i][0];             // λ23 (s1)
            g[i][5] = dA[i][1];             // λ31 (s2)
        }

        // Output slot → (angle index, sign): (−α1,−α2,−α3,+α3,+α1,+α2).
        const int    out_a[6]  = { 0, 1, 2, 2, 0, 1 };
        const double out_sg[6] = { -1.0, -1.0, -1.0, +1.0, +1.0, +1.0 };

        const int idx[6] = {
            m.v_idx[v1], m.v_idx[v2], m.v_idx[v3],
            m.e_idx[e12], m.e_idx[e23], m.e_idx[e31]
        };

        for (int oi = 0; oi < 6; ++oi) {
            if (idx[oi] < 0) continue;
            for (int dj = 0; dj < 6; ++dj) {
                if (idx[dj] < 0) continue;
                const double val = out_sg[oi] * g[out_a[oi]][dj];
                if (std::abs(val) > 1e-15)
                    trips.emplace_back(idx[oi], idx[dj], val);
            }
        }
    }

    Eigen::SparseMatrix<double> H(n, n);
    H.setFromTriplets(trips.begin(), trips.end());
    return H;
}

// ── Finite-difference Hessian check ──────────────────────────────────────────
/// FD Hessian check for the Euclidean functional.  Compares analytic
/// `H` column-by-column to `(G(x+εeⱼ) − G(x−εeⱼ)) / (2ε)`; returns
/// `true` iff max relative error is below `tol`.
inline bool hessian_check_euclidean(
    ConformalMesh&             mesh,
    const std::vector<double>& x0,
    const EuclideanMaps&       m,
    double                     eps = 1e-5,
    double                     tol = 1e-4)
{
    const int n = static_cast<int>(x0.size());
    auto H = euclidean_hessian(mesh, x0, m);

    std::vector<double> xp = x0, xm = x0;
    bool ok = true;

    for (int j = 0; j < n; ++j) {
        const std::size_t sj = static_cast<std::size_t>(j);
        xp[sj] = x0[sj] + eps;
        xm[sj] = x0[sj] - eps;

        auto Gp = euclidean_gradient(mesh, xp, m);
        auto Gm = euclidean_gradient(mesh, xm, m);

        xp[sj] = xm[sj] = x0[sj];   // restore

        for (int i = 0; i < n; ++i) {
            double fd_ij  = (Gp[static_cast<std::size_t>(i)]
                           - Gm[static_cast<std::size_t>(i)]) / (2.0 * eps);
            double H_ij   = H.coeff(i, j);
            double err    = std::abs(H_ij - fd_ij);
            double scale  = std::max(1.0, std::abs(H_ij));
            if (err / scale > tol) ok = false;
        }
    }
    return ok;
}

} // namespace conformallab