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

// 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 ────────────────────────────────────────────────

/// Property map face → `int`    for the CP-Euclidean functional.
using CPFMapI = ConformalMesh::Property_map<Face_index, int>;
/// Property map face → `double` for the CP-Euclidean functional.
using CPFMapD = ConformalMesh::Property_map<Face_index, double>;
/// Property map edge → `double` for the CP-Euclidean functional.
using CPEMapD = ConformalMesh::Property_map<Edge_index, double>;

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

/// Bundle of the three property maps consumed by the CP-Euclidean
/// (Bobenko-Pinkall-Springborn 2010) circle-packing functional.
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π)
};

/// Attach the three CP-Euclidean property maps to `mesh` with default
/// values and return their handles.
///
/// Defaults:
/// * `theta_e[e] = π/2` for every edge — orthogonal circle packing
///   (Koebe-Andreev-Thurston).
/// * `phi_f[f] = 2π` for every face — flat target.
/// * `f_idx[f] = -1` for every face — all faces pinned initially;
///   call `assign_cp_euclidean_face_dof_indices()` next to assign
///   DOF indices to all faces except one gauge-pinned face.
///
/// The maps are named with the `"cf:"` / `"ce:"` prefixes
/// (cf = circle-packing-face, ce = circle-packing-edge) so they do
/// not collide with the Euclidean / Spherical / HyperIdeal maps.
///
/// \param mesh  Input mesh.  Modified in place: three property maps are
///              attached if not already present, otherwise the existing
///              maps are returned unchanged (CGAL property-map idempotence).
/// \returns     A bundle of all three property maps for caller use.
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 sequential DOF indices `0..n-1` to all faces except `pinned`,
/// which receives the sentinel `-1` (gauge-fixed face, `ρ_pinned = 0`).
///
/// Mirrors the Java CPEuclideanFunctional's "skip face index 0"
/// convention from `evaluateEnergyAndGradient` (lines 184-185 of
/// CPEuclideanFunctional.java).  The C++ port exposes the choice of
/// pinned face explicitly rather than hard-coding it.
///
/// \param mesh    The mesh.  Read for face iteration only; not modified.
/// \param m       Map bundle whose `f_idx` is overwritten.
/// \param pinned  The face whose DOF is fixed at zero (the gauge).
/// \returns       The number of free DOFs assigned (`num_faces(mesh) - 1`).
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 overload: pin the **first** face in `mesh.faces()` order.
/// Use this when any face works as the gauge (typically true for
/// closed mesh experiments).
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);
}

/// Count the free DOFs (faces with `f_idx >= 0`).
/// Equivalent to `num_faces(mesh) - <number of pinned faces>`.
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

/// CP-Euclidean energy value at DOF vector `x` (ρ per face).
/// Mirrors `evaluateEnergyAndGradient()` in the Java original (lines 170-240).
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;
}

/// CP-Euclidean gradient `∂E/∂ρ_f` (per face DOF).  Interior term
/// `−(p + θ*)`, boundary term `−2 θ*`; see `setup_cp_euclidean_maps`.
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;
}

/// Analytic CP-Euclidean Hessian, sparse.  Per interior edge `(j,k)`
/// the contribution is `h_jk = sin θ / (cosh(Δρ) − cos θ)`, added to
/// diagonals `H_jj`, `H_kk` and subtracted off-diagonals `H_jk = H_kj`.
/// Pinned faces are excluded (DOF index −1).
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;
}

/// FD gradient check for the CP-Euclidean functional (central differences).
/// Uses the same **relative** error criterion as every other gradient check in
/// this library: `|analytic − fd| / max(1, |analytic|) < tol`.
/// Default `eps = 1e-5`, `tol = 1e-4` (matches Java `FunctionalTest`).
inline bool gradient_check_cp_euclidean(const ConformalMesh&        mesh,
                                          const std::vector<double>&  x,
                                          const CPEuclideanMaps&      m,
                                          double eps = 1e-5,
                                          double tol = 1e-4)
{
    auto G = cp_euclidean_gradient(mesh, x, m);
    const std::size_t n = G.size();
    bool ok = true;

    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);
        const double err   = std::abs(G[i] - fd);
        const double scale = std::max(1.0, std::abs(G[i]));
        if (err / scale > tol) {
            std::cerr << "[cp-euclidean] FD gradient mismatch at DOF " << i
                      << ": analytic=" << G[i]
                      << "  FD=" << fd
                      << "  rel-err=" << (err / scale) << "\n";
            ok = false;
        }
    }
    return ok;
}

/// FD Hessian check for the CP-Euclidean functional.  Verifies analytic
/// `H` column-by-column against `(G(x+εe_j) − G(x−εe_j)) / (2ε)`.
/// Uses the same **relative** error criterion as `hessian_check_euclidean`:
/// `|analytic − fd| / max(1, |analytic|) < tol`.
inline bool hessian_check_cp_euclidean(const ConformalMesh&        mesh,
                                         const std::vector<double>&  x,
                                         const CPEuclideanMaps&      m,
                                         double eps = 1e-5,
                                         double tol = 1e-4)
{
    const auto H = cp_euclidean_hessian(mesh, x, m);
    const int n = static_cast<int>(H.rows());
    bool ok = true;

    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);
            double err   = std::abs(an - fd);
            double scale = std::max(1.0, std::abs(an));
            if (err / scale > tol) {
                std::cerr << "[cp-euclidean] FD Hessian mismatch at ("
                          << i << "," << j << "): analytic=" << an
                          << "  FD=" << fd
                          << "  rel-err=" << (err / scale) << "\n";
                ok = false;
            }
        }
    }
    return ok;
}

} // namespace conformallab