// @file GJK.h
/// Implementation of the Gilbert-Johnson-Keerthi (GJK) convex polyhedron intersection test

#include <J3ML/LinearAlgebra.h>
#include <J3ML/Geometry.h>
#pragma once

namespace J3ML::Algorithms
{

    Vector3 UpdateSimplex(Vector3 *s, int &n);

    #define SUPPORT(dir, minS, maxS) (a.ExtremePoint(dir, maxS) - b.ExtremePoint(-dir, minS));

    template <typename A, typename B>
    bool GJKIntersect(const A &a, const B &b)
    {
        Vector3 support[4];
        // Start with an arbitrary point in the Minkowski set shape.
        support[0] = a.AnyPointFast() - b.AnyPointFast();
        if (support[0].LengthSquared() < 1e-7f) // Robustness check: Test if the first arbitrary point we guessed produced the zero vector we are looking for!
            return true;
        Vector3 d = -support[0]; // First search direction is straight toward the origin from the found point.
        int n = 1; // Stores the current number of points in the search simplex.
        int nIterations = 50; // Robustness check: Limit the maximum number of iterations to perform to avoid infinite loop if types A or B are buggy!
        while (nIterations-- > 0)
        {
            // Compute the extreme point to the direction d in the Minkowski set shape.
            float maxS, minS;
            Vector3 newSupport = SUPPORT(d, minS, maxS);
            // If the most extreme point in that search direction did not walk past the origin, then the origin cannot be contained in the Minkowski
            // convex shape, and the two convex objects a and b do not share a common point - no intersection!
            if (minS + maxS < 0.f)
                return false;

            // Add the newly evaluated point to the search simplex
            assert(n < 4);
            support[n++] = newSupport;
            // Examine the current simplex, prune a redundant part of it, and produce the next search direction.
            d = UpdateSimplex(support, n);

            if (n == 0) // Was the origin contained in the current simplex? If so, then the convex shapes a and b do share a common point - intersection!
                return true;
        }
        return false;
    }

    // This computes GJL intersection, but by first translating both objects to a coordinate frame that is as closely
    // centered around world origin as possible, to gain floating point precision.
    template <typename A, typename B>
    bool FloatingPointOffsetedGJKIntersect(const A &a, const B &b)
    {
        AABB ab = a.MinimalEnclosingAABB();
        AABB bb = b.MinimalEnclosingAABB();

        Vector3 offset = (Vector3::Min(ab.minPoint, bb.minPoint) + Vector3::Max(ab.maxPoint, bb.maxPoint)) * 0.5f;
        const Vector3 floatingPtPrecisionOffset = -offset;
        return GJKIntersect(a.Translated(floatingPtPrecisionOffset), b.Translated(floatingPtPrecisionOffset));
    }
}