#pragma once
// layout.hpp
//
// Phase 5 — Layout / embedding: DOF vector → vertex coordinates in target space.
//
// After Newton converges you have conformal scale factors u_v (and optionally
// edge DOFs).  This header converts those factors into actual vertex positions
// in the target geometry by BFS-unfolding the triangulation.
//
// ┌──────────────────────────────────────────────────────────────────────────┐
// │  Algorithm (all three geometries)                                        │
// │                                                                          │
// │  1. For every edge e compute the updated length l_e from the DOF x.     │
// │  2. Choose a root face, place its three vertices analytically.           │
// │  3. BFS over the dual graph: for each adjacent face, the shared edge is  │
// │     already placed; trilaterate the third vertex.                        │
// │                                                                          │
// │  For open meshes (boundary) the placement is globally consistent.        │
// │  For closed meshes the BFS visits some vertices twice (seam); the first  │
// │  visit wins and `has_seam = true` is set.  To get a proper global        │
// │  parameterisation of a closed mesh, cut it to a disk first.              │
// └──────────────────────────────────────────────────────────────────────────┘
//
// Functions:
//   euclidean_layout(mesh, x, maps)  → Layout2D   (positions in ℝ²)
//   spherical_layout (mesh, x, maps) → Layout3D   (positions on S² ⊂ ℝ³)
//   hyper_ideal_layout(mesh, x, maps)→ Layout2D   (Poincaré disk)

#include "conformal_mesh.hpp"
#include "euclidean_functional.hpp"
#include "spherical_functional.hpp"
#include "hyper_ideal_functional.hpp"
#include <Eigen/Dense>
#include <vector>
#include <queue>
#include <cmath>
#include <algorithm>

namespace conformallab {

// ── Result types ──────────────────────────────────────────────────────────────

struct Layout2D {
    std::vector<Eigen::Vector2d> uv;     ///< uv[v.idx()] = 2-D position
    bool success   = false;
    bool has_seam  = false;              ///< true if mesh is closed (first-visit used)
};

struct Layout3D {
    std::vector<Eigen::Vector3d> pos;    ///< pos[v.idx()] = 3-D position on S²
    bool success  = false;
    bool has_seam = false;
};

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

namespace detail {

// Euclidean trilaterate: given placed p_a, p_b and distances d_a (from p_a),
// d_b (from p_b), return the new point on the LEFT side of the directed edge
// p_a → p_b (corresponds to CCW face orientation).
inline Eigen::Vector2d trilaterate_2d(
    const Eigen::Vector2d& pa, const Eigen::Vector2d& pb,
    double da, double db)
{
    Eigen::Vector2d ab = pb - pa;
    double d = ab.norm();
    if (d < 1e-14) return pa;
    Eigen::Vector2d e1 = ab / d;
    Eigen::Vector2d e2(-e1.y(), e1.x());   // CCW perpendicular
    double t  = (da*da - db*db + d*d) / (2.0 * d);
    double s2 = da*da - t*t;
    double s  = (s2 > 0.0) ? std::sqrt(s2) : 0.0;
    return pa + t*e1 + s*e2;               // s > 0  →  left side
}

// Spherical trilaterate: given unit vectors pa, pb on S² and arc-lengths da, db
// to the new point, return the new unit vector on the LEFT side of the geodesic
// arc from pa to pb.
//
// Solution: p_c = α·pa + β·pb + γ·(pa × pb),  γ > 0.
// Constraints: pa·p_c = cos(da),  pb·p_c = cos(db),  |p_c|=1.
inline Eigen::Vector3d trilaterate_sph(
    const Eigen::Vector3d& pa, const Eigen::Vector3d& pb,
    double da, double db)
{
    double c   = pa.dot(pb);                     // cos(l_ab)
    double denom = 1.0 - c*c;
    if (denom < 1e-14) return pa;               // degenerate (pa ≈ pb)

    double cda = std::cos(da), cdb = std::cos(db);
    double alpha = (cda - c*cdb) / denom;
    double beta  = (cdb - c*cda) / denom;

    Eigen::Vector3d cross = pa.cross(pb);
    double cross_n2 = cross.squaredNorm();

    double gamma2 = 1.0 - alpha*alpha - beta*beta - 2.0*alpha*beta*c;
    double gamma  = (gamma2 > 0.0 && cross_n2 > 1e-28)
                    ? std::sqrt(gamma2 / cross_n2)
                    : 0.0;

    Eigen::Vector3d p = alpha*pa + beta*pb + gamma*cross;
    double n = p.norm();
    return (n > 1e-14) ? (p / n) : pa;          // gamma > 0 → left side
}

} // namespace detail

// ── Euclidean layout ──────────────────────────────────────────────────────────
//
// Computes 2-D positions for each vertex by BFS-unfolding the triangulation
// in the plane.  The updated edge length is:
//
//   l_ij = exp( (λ°_ij + u_i + u_j + λ_e) / 2 )
//
// where u_i = x[v_idx[v]] (0 if pinned) and λ_e = x[e_idx[e]] (0 if absent).
// ─────────────────────────────────────────────────────────────────────────────
inline Layout2D euclidean_layout(
    ConformalMesh&             mesh,
    const std::vector<double>& x,
    const EuclideanMaps&       maps)
{
    const std::size_t nv = mesh.number_of_vertices();
    Layout2D result;
    result.uv.assign(nv, Eigen::Vector2d::Zero());
    if (mesh.number_of_faces() == 0) return result;

    auto get_u = [&](Vertex_index v) -> double {
        int iv = maps.v_idx[v]; return (iv >= 0) ? x[static_cast<std::size_t>(iv)] : 0.0;
    };
    auto get_ue = [&](Edge_index e) -> double {
        int ie = maps.e_idx[e]; return (ie >= 0) ? x[static_cast<std::size_t>(ie)] : 0.0;
    };
    auto edge_len = [&](Halfedge_index h) -> double {
        Edge_index e = mesh.edge(h);
        double lam = maps.lambda0[e]
                   + get_u(mesh.source(h)) + get_u(mesh.target(h))
                   + get_ue(e);
        return std::exp(lam * 0.5);
    };

    std::vector<bool> vertex_placed(nv, false);
    std::vector<bool> face_placed(mesh.number_of_faces(), false);

    // ── Place root face ───────────────────────────────────────────────────
    Face_index     f0 = *mesh.faces().begin();
    Halfedge_index h0 = mesh.halfedge(f0);
    Halfedge_index h1 = mesh.next(h0);
    Halfedge_index h2 = mesh.next(h1);

    Vertex_index vA = mesh.source(h0);
    Vertex_index vB = mesh.source(h1);
    Vertex_index vC = mesh.source(h2);

    double lAB = edge_len(h0);
    double lCA = edge_len(h2);  // dist C—A
    double lBC = edge_len(h1);  // dist B—C

    result.uv[vA.idx()] = Eigen::Vector2d(0.0, 0.0);
    result.uv[vB.idx()] = Eigen::Vector2d(lAB, 0.0);
    result.uv[vC.idx()] = detail::trilaterate_2d(
        result.uv[vA.idx()], result.uv[vB.idx()], lCA, lBC);

    vertex_placed[vA.idx()] = vertex_placed[vB.idx()] = vertex_placed[vC.idx()] = true;
    face_placed[f0.idx()] = true;

    // ── BFS ───────────────────────────────────────────────────────────────
    std::queue<Halfedge_index> q;
    auto enqueue = [&](Face_index f) {
        for (auto h : CGAL::halfedges_around_face(mesh.halfedge(f), mesh)) {
            auto h_opp = mesh.opposite(h);
            if (!mesh.is_border(h_opp) && !face_placed[mesh.face(h_opp).idx()])
                q.push(h_opp);
        }
    };
    enqueue(f0);

    while (!q.empty()) {
        Halfedge_index h = q.front(); q.pop();
        Face_index f = mesh.face(h);
        if (face_placed[f.idx()]) continue;

        Vertex_index v_src = mesh.source(h);
        Vertex_index v_tgt = mesh.target(h);
        Vertex_index v_new = mesh.target(mesh.next(h));

        Eigen::Vector2d p = detail::trilaterate_2d(
            result.uv[v_src.idx()], result.uv[v_tgt.idx()],
            edge_len(mesh.prev(h)),    // dist(v_new, v_src)
            edge_len(mesh.next(h)));   // dist(v_tgt, v_new)

        if (!vertex_placed[v_new.idx()]) {
            result.uv[v_new.idx()] = p;
            vertex_placed[v_new.idx()] = true;
        } else {
            result.has_seam = true;
        }
        face_placed[f.idx()] = true;
        enqueue(f);
    }

    result.success = true;
    return result;
}

// ── Spherical layout ──────────────────────────────────────────────────────────
//
// Computes positions on the unit sphere S² by BFS-unfolding the triangulation.
// Updated spherical arc-length:
//
//   l_ij = 2·arcsin( min(exp(Λ_ij / 2), 1) )
//
// where Λ_ij = λ°_ij + u_i + u_j (+ edge DOF if present).
//
// Output: pos[v.idx()] is a unit 3-D vector on S².
// ─────────────────────────────────────────────────────────────────────────────
inline Layout3D spherical_layout(
    ConformalMesh&             mesh,
    const std::vector<double>& x,
    const SphericalMaps&       maps)
{
    const std::size_t nv = mesh.number_of_vertices();
    Layout3D result;
    result.pos.assign(nv, Eigen::Vector3d::Zero());
    if (mesh.number_of_faces() == 0) return result;

    auto get_u = [&](Vertex_index v) -> double {
        int iv = maps.v_idx[v]; return (iv >= 0) ? x[static_cast<std::size_t>(iv)] : 0.0;
    };
    auto get_ue = [&](Edge_index e) -> double {
        int ie = maps.e_idx[e]; return (ie >= 0) ? x[static_cast<std::size_t>(ie)] : 0.0;
    };
    auto arc_len = [&](Halfedge_index h) -> double {
        Edge_index e = mesh.edge(h);
        double lam = maps.lambda0[e]
                   + get_u(mesh.source(h)) + get_u(mesh.target(h))
                   + get_ue(e);
        return 2.0 * std::asin(std::min(std::exp(lam * 0.5), 1.0));
    };

    // Place root face: vA at north pole, vB along meridian, vC via spherical law
    std::vector<bool> vertex_placed(nv, false);
    std::vector<bool> face_placed(mesh.number_of_faces(), false);

    Face_index     f0 = *mesh.faces().begin();
    Halfedge_index h0 = mesh.halfedge(f0);
    Halfedge_index h1 = mesh.next(h0);
    Halfedge_index h2 = mesh.next(h1);

    Vertex_index vA = mesh.source(h0);
    Vertex_index vB = mesh.source(h1);
    Vertex_index vC = mesh.source(h2);

    double lAB = arc_len(h0);
    double lCA = arc_len(h2);
    double lBC = arc_len(h1);

    // vA = north pole, vB along the 0-meridian at arc distance lAB
    result.pos[vA.idx()] = Eigen::Vector3d(0.0, 0.0, 1.0);
    result.pos[vB.idx()] = Eigen::Vector3d(std::sin(lAB), 0.0, std::cos(lAB));
    result.pos[vC.idx()] = detail::trilaterate_sph(
        result.pos[vA.idx()], result.pos[vB.idx()], lCA, lBC);

    vertex_placed[vA.idx()] = vertex_placed[vB.idx()] = vertex_placed[vC.idx()] = true;
    face_placed[f0.idx()] = true;

    std::queue<Halfedge_index> q;
    auto enqueue = [&](Face_index f) {
        for (auto h : CGAL::halfedges_around_face(mesh.halfedge(f), mesh)) {
            auto h_opp = mesh.opposite(h);
            if (!mesh.is_border(h_opp) && !face_placed[mesh.face(h_opp).idx()])
                q.push(h_opp);
        }
    };
    enqueue(f0);

    while (!q.empty()) {
        Halfedge_index h = q.front(); q.pop();
        Face_index f = mesh.face(h);
        if (face_placed[f.idx()]) continue;

        Vertex_index v_src = mesh.source(h);
        Vertex_index v_tgt = mesh.target(h);
        Vertex_index v_new = mesh.target(mesh.next(h));

        Eigen::Vector3d p = detail::trilaterate_sph(
            result.pos[v_src.idx()], result.pos[v_tgt.idx()],
            arc_len(mesh.prev(h)),
            arc_len(mesh.next(h)));

        if (!vertex_placed[v_new.idx()]) {
            result.pos[v_new.idx()] = p;
            vertex_placed[v_new.idx()] = true;
        } else {
            result.has_seam = true;
        }
        face_placed[f.idx()] = true;
        enqueue(f);
    }

    result.success = true;
    return result;
}

// ── HyperIdeal layout (Poincaré disk) ────────────────────────────────────────
//
// Places the hyperbolic triangulation in the Poincaré disk model.
// The effective hyperbolic edge length between vertices i and j is:
//
//   cosh(d_ij) = cosh(b_i + a_ij/2) · cosh(b_j + a_ij/2) − sinh(b_i) · sinh(b_j)
//
// (Springborn 2020, equation for the distance in the horoball picture.)
// Here b_i = x[v_idx[v_i]], a_ij = x[e_idx[e_ij]].
//
// Poincaré disk: a point at hyperbolic distance d from the origin lies at
// Euclidean distance tanh(d/2) from the disk centre.
//
// Layout algorithm: BFS with hyperbolic trilateration.
// ─────────────────────────────────────────────────────────────────────────────
inline Layout2D hyper_ideal_layout(
    ConformalMesh&             mesh,
    const std::vector<double>& x,
    const HyperIdealMaps&      maps)
{
    const std::size_t nv = mesh.number_of_vertices();
    Layout2D result;
    result.uv.assign(nv, Eigen::Vector2d::Zero());
    if (mesh.number_of_faces() == 0) return result;

    auto get_b = [&](Vertex_index v) -> double {
        int iv = maps.v_idx[v]; return (iv >= 0) ? x[static_cast<std::size_t>(iv)] : 0.0;
    };
    auto get_a = [&](Edge_index e) -> double {
        int ie = maps.e_idx[e]; return (ie >= 0) ? x[static_cast<std::size_t>(ie)] : 0.0;
    };

    // Hyperbolic distance between two adjacent vertices
    auto hyp_dist = [&](Halfedge_index h) -> double {
        Vertex_index vi = mesh.source(h);
        Vertex_index vj = mesh.target(h);
        Edge_index   e  = mesh.edge(h);
        double bi = get_b(vi), bj = get_b(vj), a = get_a(e);
        double half_a = a * 0.5;
        double cosh_d = std::cosh(bi + half_a) * std::cosh(bj + half_a)
                      - std::sinh(bi) * std::sinh(bj);
        cosh_d = std::max(1.0, cosh_d);
        return std::acosh(cosh_d);
    };

    // Poincaré disk radius for hyperbolic distance d
    auto poincare_r = [](double d) { return std::tanh(d * 0.5); };

    // Hyperbolic trilateration in Poincaré disk:
    // Given p_a, p_b and hyperbolic distances d_a, d_b to the new point,
    // return the new point on the LEFT side of the geodesic from p_a to p_b.
    // Approximation: for short arcs the Poincaré metric is nearly Euclidean;
    // we use Euclidean trilaterate on the Poincaré coordinates.
    auto trilaterate_hyp = [&](
        const Eigen::Vector2d& pa, const Eigen::Vector2d& pb,
        double da, double db) -> Eigen::Vector2d
    {
        // Convert arc-lengths to Poincaré chord lengths and use Euclidean formula.
        // More precisely: in Poincaré disk, geodesic distance da from pa corresponds
        // to a chord of Euclidean length 2·sinh(da/2) / (cosh(da/2)+1) = tanh(da/2)*2/(1+1)...
        // For robustness we use the isometric approximation: small displacements are
        // Euclidean in conformal coordinates. For a production implementation replace
        // with exact Möbius-transform-based hyperbolic trilateration.
        double ra = poincare_r(da);
        double rb = poincare_r(db);
        return detail::trilaterate_2d(pa, pb, ra, rb);
    };

    // Place root face
    std::vector<bool> vertex_placed(nv, false);
    std::vector<bool> face_placed(mesh.number_of_faces(), false);

    Face_index     f0 = *mesh.faces().begin();
    Halfedge_index h0 = mesh.halfedge(f0);
    Halfedge_index h1 = mesh.next(h0);
    Halfedge_index h2 = mesh.next(h1);

    Vertex_index vA = mesh.source(h0);
    Vertex_index vB = mesh.source(h1);
    Vertex_index vC = mesh.source(h2);

    double dAB = hyp_dist(h0);
    double dCA = hyp_dist(h2);
    double dBC = hyp_dist(h1);

    // Place vA at origin, vB on positive x-axis (Poincaré radius = tanh(dAB/2))
    result.uv[vA.idx()] = Eigen::Vector2d(0.0, 0.0);
    result.uv[vB.idx()] = Eigen::Vector2d(poincare_r(dAB), 0.0);
    result.uv[vC.idx()] = trilaterate_hyp(
        result.uv[vA.idx()], result.uv[vB.idx()], dCA, dBC);

    vertex_placed[vA.idx()] = vertex_placed[vB.idx()] = vertex_placed[vC.idx()] = true;
    face_placed[f0.idx()] = true;

    std::queue<Halfedge_index> q;
    auto enqueue = [&](Face_index f) {
        for (auto h : CGAL::halfedges_around_face(mesh.halfedge(f), mesh)) {
            auto h_opp = mesh.opposite(h);
            if (!mesh.is_border(h_opp) && !face_placed[mesh.face(h_opp).idx()])
                q.push(h_opp);
        }
    };
    enqueue(f0);

    while (!q.empty()) {
        Halfedge_index h = q.front(); q.pop();
        Face_index f = mesh.face(h);
        if (face_placed[f.idx()]) continue;

        Vertex_index v_src = mesh.source(h);
        Vertex_index v_tgt = mesh.target(h);
        Vertex_index v_new = mesh.target(mesh.next(h));

        Eigen::Vector2d p = trilaterate_hyp(
            result.uv[v_src.idx()], result.uv[v_tgt.idx()],
            hyp_dist(mesh.prev(h)),
            hyp_dist(mesh.next(h)));

        if (!vertex_placed[v_new.idx()]) {
            result.uv[v_new.idx()] = p;
            vertex_placed[v_new.idx()] = true;
        } else {
            result.has_seam = true;
        }
        face_placed[f.idx()] = true;
        enqueue(f);
    }

    result.success = true;
    return result;
}

// ── Convenience: save layout as OFF (z = 0 for 2D, full xyz for 3D) ──────────

inline void save_layout_off(
    const std::string& path,
    ConformalMesh&     mesh,
    const Layout2D&    layout)
{
    std::ofstream ofs(path);
    ofs << "OFF\n" << mesh.number_of_vertices() << " "
        << mesh.number_of_faces() << " 0\n";
    for (auto v : mesh.vertices()) {
        auto& p = layout.uv[v.idx()];
        ofs << p.x() << " " << p.y() << " 0\n";
    }
    for (auto f : mesh.faces()) {
        ofs << "3";
        for (auto h : CGAL::halfedges_around_face(mesh.halfedge(f), mesh))
            ofs << " " << mesh.target(h).idx();
        ofs << "\n";
    }
}

inline void save_layout_off(
    const std::string& path,
    ConformalMesh&     mesh,
    const Layout3D&    layout)
{
    std::ofstream ofs(path);
    ofs << "OFF\n" << mesh.number_of_vertices() << " "
        << mesh.number_of_faces() << " 0\n";
    for (auto v : mesh.vertices()) {
        auto& p = layout.pos[v.idx()];
        ofs << p.x() << " " << p.y() << " " << p.z() << "\n";
    }
    for (auto f : mesh.faces()) {
        ofs << "3";
        for (auto h : CGAL::halfedges_around_face(mesh.halfedge(f), mesh))
            ofs << " " << mesh.target(h).idx();
        ofs << "\n";
    }
}

} // namespace conformallab