// TRACES: AR-005, AR-030 | SR-002
//
// Unit tests for the geometric/numeric helpers in types.hpp and face_utils.hpp:
// cosine_similarity, the ArcFace 5-point alignment transform, and the alignment
// residual that AR-030 reads as its visibility measure. GPU-free, model-free.
#include <catch2/catch_test_macros.hpp>
#include <catch2/matchers/catch_matchers_floating_point.hpp>

#include "face_utils.hpp"
#include "types.hpp"

#include <array>
#include <cmath>

using Catch::Matchers::WithinAbs;

TEST_CASE("cosine_similarity of a unit vector with itself is 1", "[types]") {
    std::array<float, 512> raw{};
    raw[3] = 2.f; raw[7] = -1.f;
    Embedding e = l2_normalise(raw.data());
    CHECK_THAT(cosine_similarity(e, e), WithinAbs(1.0f, 1e-6f));
}

TEST_CASE("cosine_similarity of orthogonal vectors is 0", "[types]") {
    Embedding a{}, b{};
    a[0] = 1.f;
    b[1] = 1.f;
    CHECK_THAT(cosine_similarity(a, b), WithinAbs(0.0f, 1e-6f));
}

TEST_CASE("cosine_similarity of opposite vectors is -1", "[types]") {
    Embedding a{}, b{};
    a[5] = 1.f;
    b[5] = -1.f;
    CHECK_THAT(cosine_similarity(a, b), WithinAbs(-1.0f, 1e-6f));
}

TEST_CASE("align_face maps the reference landmarks onto the 112x112 canvas", "[face_utils]") {
    // Build a synthetic image where the five landmarks sit at known positions.
    // Feeding align_face the *reference* positions themselves should yield an
    // (near-)identity similarity transform, so the output is 112x112.
    cv::Mat img(200, 200, CV_8UC3, cv::Scalar(0, 0, 0));
    std::array<cv::Point2f, 5> lm;
    for (int i = 0; i < 5; ++i) {
        lm[i] = {kArcFaceRef[i][0], kArcFaceRef[i][1]};
        cv::circle(img, lm[i], 2, cv::Scalar(255, 255, 255), -1);
    }

    cv::Mat crop = align_face(img, lm);
    REQUIRE_FALSE(crop.empty());
    CHECK(crop.cols == 112);
    CHECK(crop.rows == 112);
}

TEST_CASE("align_face is translation-equivariant", "[face_utils]") {
    // Shifting all landmarks by a constant offset must still produce a valid
    // 112x112 crop (the similarity transform absorbs the translation).
    cv::Mat img(300, 300, CV_8UC3, cv::Scalar(30, 30, 30));
    std::array<cv::Point2f, 5> lm;
    const float dx = 100.f, dy = 80.f;
    for (int i = 0; i < 5; ++i)
        lm[i] = {kArcFaceRef[i][0] + dx, kArcFaceRef[i][1] + dy};

    cv::Mat crop = align_face(img, lm);
    REQUIRE_FALSE(crop.empty());
    CHECK(crop.cols == 112);
    CHECK(crop.rows == 112);
}

TEST_CASE("align_face returns empty on degenerate (collinear) landmarks", "[face_utils]") {
    // All five landmarks identical → the affine fit is degenerate.
    cv::Mat img(200, 200, CV_8UC3, cv::Scalar(0, 0, 0));
    std::array<cv::Point2f, 5> lm;
    for (auto& p : lm) p = {50.f, 50.f};

    cv::Mat crop = align_face(img, lm);
    CHECK(crop.empty());
}

// ── AR-030: the alignment residual as a visibility measure ────────────────────
// These assert the *properties* the measure is relied on for, not a magic value.
// Each would fail under a RANSAC fit, which buys a small residual by discarding
// the very landmarks that carry the signal.

namespace {

std::array<cv::Point2f, 5> canonical() {
    std::array<cv::Point2f, 5> lm;
    for (int i = 0; i < 5; ++i) lm[i] = {kArcFaceRef[i][0], kArcFaceRef[i][1]};
    return lm;
}

// Rotate by `deg` in-plane, scale uniformly, translate — i.e. exactly the 4 DoF
// the similarity transform models.
std::array<cv::Point2f, 5> similarity(const std::array<cv::Point2f, 5>& in,
                                      float deg, float s, float tx, float ty) {
    const float r = deg * 3.14159265358979f / 180.f;
    const float c = std::cos(r), sn = std::sin(r);
    std::array<cv::Point2f, 5> out;
    for (int i = 0; i < 5; ++i)
        out[i] = {s * (c * in[i].x - sn * in[i].y) + tx,
                  s * (sn * in[i].x + c * in[i].y) + ty};
    return out;
}

// Squash x about the centroid by `k`: the anisotropic deformation an out-of-plane
// yaw produces, and the one a similarity provably cannot absorb.
std::array<cv::Point2f, 5> foreshorten(const std::array<cv::Point2f, 5>& in, float k) {
    float cx = 0.f;
    for (const auto& p : in) cx += p.x;
    cx /= 5.f;
    std::array<cv::Point2f, 5> out = in;
    for (auto& p : out) p.x = cx + (p.x - cx) * k;
    return out;
}

}  // namespace

TEST_CASE("residual is zero for a face in canonical pose", "[face_utils][AR-030]") {
    const Alignment a = estimate_alignment(canonical());
    REQUIRE(a.ok);
    CHECK_THAT(a.residual, WithinAbs(0.0f, 1e-3f));
}

TEST_CASE("residual ignores in-plane roll, scale and translation", "[face_utils][AR-030]") {
    // The structural claim behind AR-030: the fit absorbs all four similarity
    // DoF exactly, so what remains is only the deformation a similarity cannot
    // explain. A rolled head must not read as a turned one.
    for (float deg : {-40.f, -12.f, 0.f, 17.f, 65.f}) {
        const Alignment a = estimate_alignment(similarity(canonical(), deg, 3.5f, 220.f, -40.f));
        REQUIRE(a.ok);
        CHECK_THAT(a.residual, WithinAbs(0.0f, 1e-3f));
    }
}

TEST_CASE("residual rises monotonically with foreshortening", "[face_utils][AR-030]") {
    float prev = -1.f;
    for (float k : {1.0f, 0.9f, 0.75f, 0.5f, 0.3f}) {
        const Alignment a = estimate_alignment(foreshorten(canonical(), k));
        REQUIRE(a.ok);
        CHECK(a.residual > prev);
        prev = a.residual;
    }
}

TEST_CASE("residual is independent of face size", "[face_utils][AR-030]") {
    // The measure must not silently re-express face size — that is AR-002's job,
    // and double-counting it would make a small frontal face look occluded.
    // Same deformation, two very different face sizes, one answer.
    const auto small = similarity(foreshorten(canonical(), 0.7f), 20.f, 1.0f,  0.f,  0.f);
    const auto large = similarity(foreshorten(canonical(), 0.7f), 20.f, 12.0f, 500.f, 300.f);

    const Alignment a = estimate_alignment(small);
    const Alignment b = estimate_alignment(large);
    REQUIRE(a.ok);
    REQUIRE(b.ok);
    CHECK_THAT(b.residual, WithinAbs(a.residual, 1e-2f));
}

TEST_CASE("the fit never mirrors the face", "[face_utils][AR-030]") {
    // SVD will happily return an orientation-reversing solution; a similarity
    // transform may rotate but never reflect. Without the determinant guard a
    // mirrored landmark set fits "perfectly" as a reflection.
    const auto mirrored = foreshorten(canonical(), -1.f);
    const Alignment a = estimate_alignment(mirrored);
    REQUIRE(a.ok);

    const double det = a.M.at<double>(0,0) * a.M.at<double>(1,1)
                     - a.M.at<double>(0,1) * a.M.at<double>(1,0);
    CHECK(det > 0.0);
    CHECK(a.residual > 1.0f);   // and the mirroring shows up as misfit
}

TEST_CASE("degenerate landmarks report not-ok rather than a residual", "[face_utils][AR-030]") {
    std::array<cv::Point2f, 5> lm;
    for (auto& p : lm) p = {50.f, 50.f};

    const Alignment a = estimate_alignment(lm);
    CHECK_FALSE(a.ok);
    CHECK(a.M.empty());
}