ConformalLabpp/code/include/cp_euclidean_functional.hpp

#pragma once
// cp_euclidean_functional.hpp
//
// Phase 9a.1 — Circle-Packing Euclidean functional (CP-Euclidean).
//
// Ported from de.varylab.discreteconformal.functional.CPEuclideanFunctional
// (Java, 260 lines).  Mathematical reference:
//   Bobenko, A. I., Pinkall, U. & Springborn, B. (2010)
//   "Discrete conformal maps and ideal hyperbolic polyhedra"
//   Geometry & Topology 14, 379-426.
//
// ┌──────────────────────────────────────────────────────────────────────────┐
// │  FACE-based circle packing                                                │
// │                                                                           │
// │  Each face f of the mesh carries a circle of radius R_f.                  │
// │  The variable is ρ_f = log R_f.                                           │
// │  Adjacent face-circles intersect at a prescribed angle θ_e per edge.      │
// │                                                                           │
// │  This is the FACE-DUAL of the classical vertex-based Luo (2004)           │
// │  inversive-distance circle packing implemented in                          │
// │  inversive_distance_functional.hpp (Phase 9a.2).  The relation            │
// │     I_ij  =  cos θ_e                                                       │
// │  identifies the two parametrisations (Glickenstein 2011 §5).              │
// │                                                                           │
// │  DOFs                                                                     │
// │    x[f_idx[f]] = ρ_f   (face-dual log-radius)                             │
// │    f_idx[f] = −1 means f is pinned (ρ_f = 0, gauge fix)                  │
// │                                                                           │
// │  Constants                                                                │
// │    θ_e per edge   intersection angle of the two face-circles              │
// │    φ_f per face   target sum of corner-angles inside the face             │
// │                                                                           │
// │  Energy  (BPS-2010 §6)                                                    │
// │    E(ρ) =  Σ_f  φ_f · ρ_f                                                  │
// │         +  Σ_{(h,f=face(h)):                                              │
// │              [ if opposite face exists ]                                   │
// │                ½ p(θ*,Δρ)·Δρ + Λ(θ*+p) − θ*·ρ_left                       │
// │              [ else (boundary halfedge) ]                                  │
// │                −2 θ*·ρ_left                                                │
// │            ]                                                              │
// │                                                                           │
// │    where    θ* = π − θ                                                    │
// │             Δρ = ρ_right − ρ_left                                          │
// │             p(θ*, Δρ) = 2·atan( tan(θ*/2) · tanh(Δρ/2) )                  │
// │             Λ        = Clausen function (Lobachevsky)                     │
// │                                                                           │
// │  Gradient                                                                 │
// │    Per face f:        +φ_f                                                │
// │    Per interior hf:   −(p + θ*)  added to G[face(h)]                      │
// │    Per boundary hf:   −2 θ*       added to G[face(h)]                     │
// │                                                                           │
// │  Hessian (analytic, BPS-2010 eq. 6.8; Java getHessian lines 127-166)     │
// │    Per interior undirected edge e (connecting faces j and k):             │
// │      h_jk = sin θ / (cosh Δρ − cos θ)                                     │
// │      H[j,j] += h_jk,  H[k,k] += h_jk,  H[j,k] −= h_jk,  H[k,j] −= h_jk    │
// │    Pinned faces contribute nothing (their row/col is removed).            │
// └──────────────────────────────────────────────────────────────────────────┘
//
// Halfedge convention (matches Java's "leftFace / rightFace"):
//   For a directed halfedge h in CGAL::Surface_mesh:
//     mesh.face(h)             ≡ leftFace
//     mesh.face(opposite(h))   ≡ rightFace  (may be null on boundary)
//     mesh.is_border(h) == true iff h has no face (h points outward).
//   Property-map name prefix: "cf:" (face) and "ce:" (edge).

#include "conformal_mesh.hpp"
#include "constants.hpp"
#include "clausen.hpp"
#include <Eigen/Sparse>
#include <CGAL/boost/graph/iterator.h>
#include <vector>
#include <cmath>
#include <cstdint>
#include <iostream>

namespace conformallab {

// ── Property-map type aliases ────────────────────────────────────────────────

using CPFMapI = ConformalMesh::Property_map<Face_index, int>;
using CPFMapD = ConformalMesh::Property_map<Face_index, double>;
using CPEMapD = ConformalMesh::Property_map<Edge_index, double>;

// ── Persistent map bundle ─────────────────────────────────────────────────────

struct CPEuclideanMaps {
    CPFMapI f_idx;     ///< DOF index per face (−1 = pinned)
    CPEMapD theta_e;   ///< intersection angle per edge (default π/2 = orthogonal)
    CPFMapD phi_f;     ///< target face-angle sum   (default 2π)
};

// Create the property maps with sensible defaults.
// θ_e = π/2 produces an orthogonal circle packing (Koebe-Andreev-Thurston).
// φ_f = 2π is the natural target for a flat triangle.
inline CPEuclideanMaps setup_cp_euclidean_maps(ConformalMesh& mesh)
{
    CPEuclideanMaps m;
    m.f_idx   = mesh.add_property_map<Face_index, int>   ("cf:idx",   -1     ).first;
    m.theta_e = mesh.add_property_map<Edge_index, double>("ce:theta", PI / 2 ).first;
    m.phi_f   = mesh.add_property_map<Face_index, double>("cf:phi",   TWO_PI ).first;
    return m;
}

// Assign DOF indices 0..n-1 to all faces except `pinned`, which gets −1.
// Mirrors Java CPEuclideanFunctional's convention "skip face index 0".
inline int assign_cp_euclidean_face_dof_indices(ConformalMesh&  mesh,
                                                  CPEuclideanMaps& m,
                                                  Face_index       pinned)
{
    int idx = 0;
    for (auto f : mesh.faces()) {
        if (f == pinned) m.f_idx[f] = -1;
        else             m.f_idx[f] = idx++;
    }
    return idx;
}

// Convenience: pin the first face in iteration order.
inline int assign_cp_euclidean_face_dof_indices(ConformalMesh&  mesh,
                                                  CPEuclideanMaps& m)
{
    auto it = mesh.faces().begin();
    if (it == mesh.faces().end()) return 0;
    return assign_cp_euclidean_face_dof_indices(mesh, m, *it);
}

inline int cp_euclidean_dimension(const ConformalMesh& mesh,
                                    const CPEuclideanMaps& m)
{
    int dim = 0;
    for (auto f : mesh.faces()) if (m.f_idx[f] >= 0) ++dim;
    return dim;
}

// ── Internal helpers ──────────────────────────────────────────────────────────

namespace cp_detail {

// p(θ*, Δρ) = 2·atan( tan(θ*/2) · tanh(Δρ/2) )
// Numerically stable form lifted directly from CPEuclideanFunctional.java
// (private method `p`, lines 243-247).
inline double p_function(double thStar, double dRho) noexcept
{
    const double e          = std::exp(dRho);
    const double tanh_half  = (e - 1.0) / (e + 1.0);
    return 2.0 * std::atan(std::tan(0.5 * thStar) * tanh_half);
}

// DOF reader: returns 0 for the pinned face (idx = −1).
inline double dof_val(int idx, const std::vector<double>& x) noexcept
{
    return idx >= 0 ? x[static_cast<std::size_t>(idx)] : 0.0;
}

} // namespace cp_detail

// ── Energy ────────────────────────────────────────────────────────────────────
//
// Mirrors evaluateEnergyAndGradient in the Java code (lines 170-240) for the
// energy accumulation only.  The gradient is computed in a dedicated function
// below for clarity.
inline double cp_euclidean_energy(const ConformalMesh&        mesh,
                                    const std::vector<double>&  x,
                                    const CPEuclideanMaps&      m)
{
    using cp_detail::dof_val;

    double E = 0.0;

    // Per-face linear term: + φ_f · ρ_f
    // (The pinned face has f_idx = −1; its ρ is fixed at 0 so it contributes nothing.)
    for (auto f : mesh.faces()) {
        const int i = m.f_idx[f];
        if (i < 0) continue;
        E += m.phi_f[f] * x[static_cast<std::size_t>(i)];
    }

    // Per directed halfedge term.  Java iterates over `getEdges()` which in jtem
    // yields one Edge per directed side; in CGAL we iterate halfedges directly.
    for (auto h : mesh.halfedges()) {
        if (mesh.is_border(h)) continue;   // h is in the outer "border" face → skip
        const Face_index fL = mesh.face(h);
        const Halfedge_index ho = mesh.opposite(h);
        const Face_index fR = mesh.is_border(ho) ? Face_index() : mesh.face(ho);

        const double th     = m.theta_e[mesh.edge(h)];
        const double thStar = PI - th;
        const double rho_L  = dof_val(m.f_idx[fL], x);

        if (fR == Face_index()) {
            // Boundary halfedge: only the left face exists.
            E += -2.0 * thStar * rho_L;
        } else {
            const double rho_R = dof_val(m.f_idx[fR], x);
            const double dRho  = rho_R - rho_L;
            const double p     = cp_detail::p_function(thStar, dRho);
            E += 0.5 * p * dRho;
            E += clausen2(thStar + p);
            E += -thStar * rho_L;
        }
    }

    return E;
}

// ── Gradient ──────────────────────────────────────────────────────────────────
//
// ∂E/∂ρ_f = φ_f − Σ_{h: face(h)=f, !is_border(h)}  (p + θ*)
//         OR (boundary):                            2 θ*
inline std::vector<double> cp_euclidean_gradient(const ConformalMesh&        mesh,
                                                   const std::vector<double>&  x,
                                                   const CPEuclideanMaps&      m)
{
    using cp_detail::dof_val;

    const int n = cp_euclidean_dimension(mesh, m);
    std::vector<double> G(static_cast<std::size_t>(n), 0.0);

    // Per-face linear term.
    for (auto f : mesh.faces()) {
        const int i = m.f_idx[f];
        if (i < 0) continue;
        G[static_cast<std::size_t>(i)] += m.phi_f[f];
    }

    // Per directed halfedge term.
    for (auto h : mesh.halfedges()) {
        if (mesh.is_border(h)) continue;
        const Face_index fL = mesh.face(h);
        const int        iL = m.f_idx[fL];
        if (iL < 0) continue;   // pinned face: gradient component is forced to 0

        const Halfedge_index ho = mesh.opposite(h);
        const Face_index fR = mesh.is_border(ho) ? Face_index() : mesh.face(ho);

        const double th     = m.theta_e[mesh.edge(h)];
        const double thStar = PI - th;
        const double rho_L  = dof_val(iL, x);

        if (fR == Face_index()) {
            G[static_cast<std::size_t>(iL)] -= 2.0 * thStar;
        } else {
            const double rho_R = dof_val(m.f_idx[fR], x);
            const double dRho  = rho_R - rho_L;
            const double p     = cp_detail::p_function(thStar, dRho);
            G[static_cast<std::size_t>(iL)] -= (p + thStar);
        }
    }

    return G;
}

// ── Hessian (analytic) ────────────────────────────────────────────────────────
//
// Per interior undirected edge e with adjacent faces (j, k):
//   h_jk = sin θ / (cosh(Δρ) − cos θ)
// Diagonal contributions on both endpoints; off-diagonal block is −h_jk.
// Pinned faces are skipped (their DOF index is −1 ⇒ excluded from the matrix).
inline Eigen::SparseMatrix<double> cp_euclidean_hessian(const ConformalMesh&        mesh,
                                                          const std::vector<double>&  x,
                                                          const CPEuclideanMaps&      m)
{
    using cp_detail::dof_val;

    const int n = cp_euclidean_dimension(mesh, m);
    std::vector<Eigen::Triplet<double>> trips;
    trips.reserve(static_cast<std::size_t>(4 * mesh.number_of_edges()));

    for (auto e : mesh.edges()) {
        const Halfedge_index h  = mesh.halfedge(e);
        const Halfedge_index ho = mesh.opposite(h);
        if (mesh.is_border(h) || mesh.is_border(ho)) continue;   // boundary edge

        const int j = m.f_idx[mesh.face(h)];
        const int k = m.f_idx[mesh.face(ho)];

        const double rho_j = dof_val(j, x);
        const double rho_k = dof_val(k, x);
        const double dRho  = rho_k - rho_j;
        const double th    = m.theta_e[e];
        const double hjk   = std::sin(th) / (std::cosh(dRho) - std::cos(th));

        if (j >= 0)              trips.emplace_back(j, j,  hjk);
        if (k >= 0)              trips.emplace_back(k, k,  hjk);
        if (j >= 0 && k >= 0) {
            trips.emplace_back(j, k, -hjk);
            trips.emplace_back(k, j, -hjk);
        }
    }

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

// ── Finite-difference gradient check ─────────────────────────────────────────
//
// Mirrors the Java FunctionalTest pattern:
//   For each DOF i, compare analytic G[i] to (E(x+ε·e_i) − E(x−ε·e_i)) / (2ε).
// Default tolerance 1e-6 with ε = 1e-5 leaves ~3 digits of margin for sane meshes.
inline bool gradient_check_cp_euclidean(const ConformalMesh&        mesh,
                                          const std::vector<double>&  x,
                                          const CPEuclideanMaps&      m,
                                          double eps = 1e-5,
                                          double tol = 1e-6)
{
    auto G = cp_euclidean_gradient(mesh, x, m);
    const std::size_t n = G.size();

    for (std::size_t i = 0; i < n; ++i) {
        std::vector<double> xp = x, xm = x;
        xp[i] += eps;
        xm[i] -= eps;
        const double Ep = cp_euclidean_energy(mesh, xp, m);
        const double Em = cp_euclidean_energy(mesh, xm, m);
        const double fd = (Ep - Em) / (2.0 * eps);
        if (std::abs(G[i] - fd) > tol) {
            std::cerr << "[cp-euclidean] FD gradient mismatch at DOF " << i
                      << ": analytic=" << G[i]
                      << "  FD=" << fd
                      << "  diff=" << (G[i] - fd) << "\n";
            return false;
        }
    }
    return true;
}

// ── Finite-difference Hessian check ──────────────────────────────────────────
//
// Verifies analytic H against ( G(x+ε·e_i) − G(x−ε·e_i) ) / (2ε) column-wise.
// Symmetry is implicit in the analytic form; we check both off-diagonal entries.
inline bool hessian_check_cp_euclidean(const ConformalMesh&        mesh,
                                         const std::vector<double>&  x,
                                         const CPEuclideanMaps&      m,
                                         double eps = 1e-5,
                                         double tol = 1e-5)
{
    const auto H = cp_euclidean_hessian(mesh, x, m);
    const int n = static_cast<int>(H.rows());

    for (int j = 0; j < n; ++j) {
        std::vector<double> xp = x, xm = x;
        xp[static_cast<std::size_t>(j)] += eps;
        xm[static_cast<std::size_t>(j)] -= eps;
        auto Gp = cp_euclidean_gradient(mesh, xp, m);
        auto Gm = cp_euclidean_gradient(mesh, xm, m);

        for (int i = 0; i < n; ++i) {
            double fd = (Gp[static_cast<std::size_t>(i)] - Gm[static_cast<std::size_t>(i)])
                        / (2.0 * eps);
            double an = H.coeff(i, j);
            if (std::abs(an - fd) > tol) {
                std::cerr << "[cp-euclidean] FD Hessian mismatch at ("
                          << i << "," << j << "): analytic=" << an
                          << "  FD=" << fd
                          << "  diff=" << (an - fd) << "\n";
                return false;
            }
        }
    }
    return true;
}

} // namespace conformallab