#pragma once

// Projective and hyperbolic geometry utilities.
// Ported from de.jreality.math.Pn / Rn and
// de.varylab.discreteconformal.uniformization.SurfaceCurveUtility (Java).

#include <Eigen/Dense>
#include <cmath>
#include <algorithm>
#include <array>
#include <cassert>

namespace conformallab {

// Divide a homogeneous vector by its last component.
// Corresponds to Java Pn.dehomogenize().
inline Eigen::VectorXd dehomogenize(const Eigen::VectorXd& p) {
    return p / p(p.size() - 1);
}

// Hyperbolic distance between two homogeneous vectors of the same dimension.
// The last component is the "timelike" coordinate (jReality convention).
// Inner product: <p,q> = -sum_i p_i*q_i + p_last * q_last
// Distance: arcosh(<p̂, q̂>) where p̂ normalises to the hyperboloid.
// Corresponds to Java Pn.distanceBetween(p, q, Pn.HYPERBOLIC).
inline double hyperbolicDistance(const Eigen::VectorXd& p,
                                 const Eigen::VectorXd& q) {
    int n = static_cast<int>(p.size());
    double normP = std::sqrt(p(n-1)*p(n-1) - p.head(n-1).squaredNorm());
    double normQ = std::sqrt(q(n-1)*q(n-1) - q.head(n-1).squaredNorm());
    double inner = (-p.head(n-1).dot(q.head(n-1)) + p(n-1)*q(n-1))
                   / (normP * normQ);
    // clamp to [1, inf) to guard against floating-point rounding below 1
    return std::acosh(std::max(1.0, inner));
}

// Check whether a homogeneous point p lies on the segment [s[0], s[1]].
// Works for n-dimensional homogeneous coords; cross product uses the first
// 3 spatial components after dehomogenization (matching jReality's Rn behaviour).
// Corresponds to Java SurfaceCurveUtility.isOnSegment().
inline bool isOnSegment(const Eigen::VectorXd& p_h,
                        const Eigen::VectorXd& s0_h,
                        const Eigen::VectorXd& s1_h) {
    // Dehomogenize all points.
    Eigen::VectorXd p  = dehomogenize(p_h);
    Eigen::VectorXd s0 = dehomogenize(s0_h);
    Eigen::VectorXd s1 = dehomogenize(s1_h);

    // Vectors from p to each endpoint.
    Eigen::VectorXd ps0 = s0 - p;
    Eigen::VectorXd ps1 = s1 - p;

    // Collinearity check: 3D cross product of first 3 spatial components
    // (after dehomogenize the w-component differences cancel to 0).
    // head<3>() gives compile-time size needed by Eigen's cross().
    Eigen::Vector3d cross = ps0.head<3>().cross(ps1.head<3>());
    if (cross.norm() > 1e-7) return false;

    // Betweenness check: dot product of the two direction vectors must be ≤ 0.
    double dot = ps0.dot(ps1);
    if (dot > 0.0) return false;

    return true;
}

// Find the point on `target` that corresponds to `p` on `source`.
// The parameter t is determined by hyperbolic distance ratios on `source`,
// then applied as a linear interpolation on the dehomogenized `target`.
// Corresponds to Java SurfaceCurveUtility.getPointOnCorrespondingSegment().
inline Eigen::VectorXd getPointOnCorrespondingSegment(
    const Eigen::VectorXd& p,
    const Eigen::VectorXd& src0,
    const Eigen::VectorXd& src1,
    const Eigen::VectorXd& tgt0,
    const Eigen::VectorXd& tgt1)
{
    double l  = hyperbolicDistance(src0, src1);
    double l1 = hyperbolicDistance(src0, p) / l;  // weight for tgt1
    double l2 = hyperbolicDistance(src1, p) / l;  // weight for tgt0

    if (std::isnan(l1)) return dehomogenize(tgt0);
    if (std::isnan(l2)) return dehomogenize(tgt1);

    Eigen::VectorXd t0d = dehomogenize(tgt0);
    Eigen::VectorXd t1d = dehomogenize(tgt1);
    return l1 * t1d + l2 * t0d;
}

} // namespace conformallab