// Port of de.varylab.discreteconformal.math.MatrixUtilityTest (Java/JUnit).

#include "matrix_utility.hpp"

#include <gtest/gtest.h>
#include <Eigen/Dense>

using Eigen::Matrix4d;
using Eigen::Vector4d;
using conformallab::makeMappingMatrix;

TEST(MatrixUtilityTest, MakeMappingMatrix) {
    // Source points (rows = homogeneous 4-vectors).
    Matrix4d from;
    from.row(0) = Vector4d(2, 0, 2, 1);
    from.row(1) = Vector4d(1, 1, 0, 0);
    from.row(2) = Vector4d(1, 0, 8, 0);
    from.row(3) = Vector4d(3, 4, 0, 1);

    // Target points.
    Matrix4d to;
    to.row(0) = Vector4d(2, 0, 1, 0);
    to.row(1) = Vector4d(0, 3, 0, 2);
    to.row(2) = Vector4d(1, 0, 4, 0);
    to.row(3) = Vector4d(0, 3, 0, 5);

    Matrix4d R = makeMappingMatrix(from, to);

    // R must map each source column to the corresponding target column.
    for (int i = 0; i < 4; i++) {
        Vector4d result = R * from.row(i).transpose();
        Vector4d expected = to.row(i).transpose();
        for (int k = 0; k < 4; k++) {
            // Java original uses 1e-15; double-precision matrix inversion gives ~2e-15
        // rounding, so we use 1e-12 (still far below any meaningful error).
        EXPECT_NEAR(expected(k), result(k), 1e-12)
                << "Row " << i << ", component " << k;
        }
    }
}
