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


// 2-D projective geometry utilities for the Euclidean signature.
// Ported from de.jreality.math.P2 and de.varylab.discreteconformal.math.P2Big.
//
// Points and lines are represented as homogeneous 3-vectors (x, y, w).
// In the Euclidean case a finite point (px, py) is stored as (px, py, 1).

#include <Eigen/Dense>
#include <cmath>

namespace conformallab {

// ── Point / line duality ──────────────────────────────────────────────────────

/// Cross-product point–line duality in P²: returns the intersection
/// of two lines (or the line through two points).  Same as Java
/// `P2.pointFromLines` / `P2.lineFromPoints`.
inline Eigen::Vector3d pointFromLines(const Eigen::Vector3d& l1,
                                      const Eigen::Vector3d& l2) {
    return l1.cross(l2);
}

// ── Euclidean perpendicular bisector ─────────────────────────────────────────

/// Homogeneous line coordinates `(a, b, c)` of the perpendicular
/// bisector of `[p, q]` in the Euclidean plane (`ax + by + c = 0`).
/// Same as Java `P2.perpendicularBisector(p, q, Pn.EUCLIDEAN)`.
inline Eigen::Vector3d perpendicularBisectorEuclidean(const Eigen::Vector3d& p_h,
                                                      const Eigen::Vector3d& q_h) {
    // Dehomogenize
    Eigen::Vector2d p = p_h.head<2>() / p_h(2);
    Eigen::Vector2d q = q_h.head<2>() / q_h(2);

    // Direction vector (p → direction, matching jReality sign convention)
    Eigen::Vector2d d = p - q;

    // Midpoint
    Eigen::Vector2d m = (p + q) * 0.5;

    // Line: d[0]*(x - m[0]) + d[1]*(y - m[1]) = 0
    //      = d[0]*x + d[1]*y - (d[0]*m[0] + d[1]*m[1])
    double c = -(d(0) * m(0) + d(1) * m(1));
    return {d(0), d(1), c};
}

/// Euclidean distance between two P² homogeneous points (dehomogenises both).
inline double euclideanDistanceP2(const Eigen::Vector3d& p_h,
                                  const Eigen::Vector3d& q_h) {
    Eigen::Vector2d p = p_h.head<2>() / p_h(2);
    Eigen::Vector2d q = q_h.head<2>() / q_h(2);
    return (p - q).norm();
}

// ── Direct Euclidean isometry from two point-frames ──────────────────────────

/// Build the 3×3 projective frame matrix anchored at `p0` with `p1`
/// defining the positive x-direction (Euclidean case).  Columns:
/// `[dehom(p0), unit_dir(p0→p1), perp_dir]`.
template <typename S>
Eigen::Matrix<S, 3, 3> makeFrameMatrix(Eigen::Matrix<S, 3, 1> p0_h,
                                        Eigen::Matrix<S, 3, 1> p1_h) {
    // Dehomogenize
    Eigen::Matrix<S, 3, 1> p0 = p0_h / p0_h(2);          // (px, py, 1)
    Eigen::Matrix<S, 3, 1> p1_d = p1_h / p1_h(2);

    // Unit direction p0 → p1
    Eigen::Matrix<S, 2, 1> dir2 = (p1_d - p0).template head<2>();
    dir2.normalize();
    Eigen::Matrix<S, 3, 1> p1n(dir2(0), dir2(1), S(0));

    // Perpendicular direction
    Eigen::Matrix<S, 3, 1> p2(-dir2(1), dir2(0), S(0));

    Eigen::Matrix<S, 3, 3> M;
    M.col(0) = p0;
    M.col(1) = p1n;
    M.col(2) = p2;
    return M;
}

/// 3×3 Euclidean isometry (as a projective matrix) that maps the
/// frame `(s1, s2)` to the frame `(t1, t2)`.  Same as Java
/// `P2.makeDirectIsometryFromFrames(..., Pn.EUCLIDEAN)`.
template <typename S>
Eigen::Matrix<S, 3, 3> makeDirectIsometryFromFramesEuclidean(
    Eigen::Matrix<S, 3, 1> s1, Eigen::Matrix<S, 3, 1> s2,
    Eigen::Matrix<S, 3, 1> t1, Eigen::Matrix<S, 3, 1> t2)
{
    auto toS = makeFrameMatrix<S>(s1, s2);
    auto toT = makeFrameMatrix<S>(t1, t2);
    return toT * toS.inverse();
}

} // namespace conformallab