// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2020
// Distributed under the Boost Software License, Version 1.0.
// https://www.boost.org/LICENSE_1_0.txt
// https://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// Version: 4.0.2019.08.13

#pragma once

#include <Mathematics/FIQuery.h>
#include <Mathematics/DistPointTriangle.h>
#include <Mathematics/Hypersphere.h>
#include <Mathematics/Vector3.h>
#include <Mathematics/QFNumber.h>

namespace WwiseGTE
{
    // Currently, only a dynamic query is supported.  A static query will
    // need to compute the intersection set of triangle and sphere.

    template <typename Real>
    class FIQuery<Real, Sphere3<Real>, Triangle3<Real>>
    {
    public:
        // The implementation for floating-point types.
        struct Result
        {
            // The cases are
            // 1. Objects initially overlapping.  The contactPoint is only one
            //    of infinitely many points in the overlap.
            //      intersectionType = -1
            //      contactTime = 0
            //      contactPoint = triangle point closest to sphere.center
            // 2. Objects initially separated but do not intersect later.  The
            //      contactTime and contactPoint are invalid.
            //      intersectionType = 0
            //      contactTime = 0
            //      contactPoint = (0,0,0)
            // 3. Objects initially separated but intersect later.
            //      intersectionType = +1
            //      contactTime = first time T > 0
            //      contactPoint = corresponding first contact
            int intersectionType;
            Real contactTime;
            Vector3<Real> contactPoint;
        };

        template <typename Dummy = Real>
        typename std::enable_if<!is_arbitrary_precision<Dummy>::value, Result>::type
        operator()(Sphere3<Real> const& sphere, Vector3<Real> const& sphereVelocity,
            Triangle3<Real> const& triangle, Vector3<Real> const& triangleVelocity)
        {
            Result result = { 0, (Real)0, { (Real)0, (Real)0, (Real)0 } };

            // Test for initial overlap or contact.
            DCPQuery<Real, Vector3<Real>, Triangle3<Real>> ptQuery;
            auto ptResult = ptQuery(sphere.center, triangle);
            Real rsqr = sphere.radius * sphere.radius;
            if (ptResult.sqrDistance <= rsqr)
            {
                result.intersectionType = (ptResult.sqrDistance < rsqr ? -1 : +1);
                result.contactTime = (Real)0;
                result.contactPoint = ptResult.closest;
                return result;
            }

            // To reach here, the sphere and triangle are initially separated.
            // Compute the velocity of the sphere relative to the triangle.
            Vector3<Real> V = sphereVelocity - triangleVelocity;
            Real sqrLenV = Dot(V, V);
            if (sqrLenV == (Real)0)
            {
                // The sphere and triangle are separated and the sphere is not
                // moving relative to the triangle, so there is no contact.
                // The 'result' is already set to the correct state for this
                // case.
                return result;
            }

            // Compute the triangle edge directions E[], the vector U normal
            // to the plane of the triangle,  and compute the normals to the
            // edges in the plane of the triangle.  TODO: For a nondeforming
            // triangle (or mesh of triangles), these quantities can all be
            // precomputed to reduce the computational cost of the query.  Add
            // another operator()-query that accepts the precomputed values.
            // TODO: When the triangle is deformable, these quantities must be
            // computed, either by the caller or here.  Optimize the code to
            // compute the quantities on-demand (i.e. only when they are
            // needed, but cache them for later use).
            Vector3<Real> E[3] =
            {
                triangle.v[1] - triangle.v[0],
                triangle.v[2] - triangle.v[1],
                triangle.v[0] - triangle.v[2]
            };
            Real sqrLenE[3] = { Dot(E[0], E[0]), Dot(E[1], E[1]), Dot(E[2], E[2]) };
            Vector3<Real> U = UnitCross(E[0], E[1]);
            Vector3<Real> ExU[3] =
            {
                Cross(E[0], U),
                Cross(E[1], U),
                Cross(E[2], U)
            };

            // Compute the vectors from the triangle vertices to the sphere
            // center.
            Vector3<Real> Delta[3] =
            {
                sphere.center - triangle.v[0],
                sphere.center - triangle.v[1],
                sphere.center - triangle.v[2]
            };

            // Determine where the sphere center is located relative to the
            // planes of the triangle offset faces of the sphere-swept volume.
            Real dotUDelta0 = Dot(U, Delta[0]);
            if (dotUDelta0 >= sphere.radius)
            {
                // The sphere is on the positive side of Dot(U,X-C) = r.  If
                // the sphere will contact the sphere-swept volume at a
                // triangular face, it can do so only on the face of the
                // aforementioned plane.
                Real dotUV = Dot(U, V);
                if (dotUV >= (Real)0)
                {
                    // The sphere is moving away from, or parallel to, the
                    // plane of the triangle.  The 'result' is already set to
                    // the correct state for this case.
                    return result;
                }

                Real tbar = (sphere.radius - dotUDelta0) / dotUV;
                bool foundContact = true;
                for (int i = 0; i < 3; ++i)
                {
                    Real phi = Dot(ExU[i], Delta[i]);
                    Real psi = Dot(ExU[i], V);
                    if (phi + psi * tbar > (Real)0)
                    {
                        foundContact = false;
                        break;
                    }
                }
                if (foundContact)
                {
                    result.intersectionType = 1;
                    result.contactTime = tbar;
                    result.contactPoint = sphere.center + tbar * sphereVelocity;
                    return result;
                }
            }
            else if (dotUDelta0 <= -sphere.radius)
            {
                // The sphere is on the positive side of Dot(-U,X-C) = r.  If
                // the sphere will contact the sphere-swept volume at a
                // triangular face, it can do so only on the face of the
                // aforementioned plane.
                Real dotUV = Dot(U, V);
                if (dotUV <= (Real)0)
                {
                    // The sphere is moving away from, or parallel to, the
                    // plane of the triangle.  The 'result' is already set to
                    // the correct state for this case.
                    return result;
                }

                Real tbar = (-sphere.radius - dotUDelta0) / dotUV;
                bool foundContact = true;
                for (int i = 0; i < 3; ++i)
                {
                    Real phi = Dot(ExU[i], Delta[i]);
                    Real psi = Dot(ExU[i], V);
                    if (phi + psi * tbar > (Real)0)
                    {
                        foundContact = false;
                        break;
                    }
                }
                if (foundContact)
                {
                    result.intersectionType = 1;
                    result.contactTime = tbar;
                    result.contactPoint = sphere.center + tbar * sphereVelocity;
                    return result;
                }
            }
            // else: The ray-sphere-swept-volume contact point (if any) cannot
            // be on a triangular face of the sphere-swept-volume.

            // The sphere is moving towards the slab between the two planes
            // of the sphere-swept volume triangular faces.  Determine whether
            // the ray intersects the half cylinders or sphere wedges of the
            // sphere-swept volume.

            // Test for contact with half cylinders of the sphere-swept
            // volume.  First, precompute some dot products required in the
            // computations.  TODO: Optimize the code to compute the quantities
            // on-demand (i.e. only when they are needed, but cache them for
            // later use).
            Real del[3], delp[3], nu[3];
            for (int im1 = 2, i = 0; i < 3; im1 = i++)
            {
                del[i] = Dot(E[i], Delta[i]);
                delp[im1] = Dot(E[im1], Delta[i]);
                nu[i] = Dot(E[i], V);
            }

            for (int i = 2, ip1 = 0; ip1 < 3; i = ip1++)
            {
                Vector3<Real> hatV = V - E[i] * nu[i] / sqrLenE[i];
                Real sqrLenHatV = Dot(hatV, hatV);
                if (sqrLenHatV > (Real)0)
                {
                    Vector3<Real> hatDelta = Delta[i] - E[i] * del[i] / sqrLenE[i];
                    Real alpha = -Dot(hatV, hatDelta);
                    if (alpha >= (Real)0)
                    {
                        Real sqrLenHatDelta = Dot(hatDelta, hatDelta);
                        Real beta = alpha * alpha - sqrLenHatV * (sqrLenHatDelta - rsqr);
                        if (beta >= (Real)0)
                        {
                            Real tbar = (alpha - std::sqrt(beta)) / sqrLenHatV;

                            Real mu = Dot(ExU[i], Delta[i]);
                            Real omega = Dot(ExU[i], hatV);
                            if (mu + omega * tbar >= (Real)0)
                            {
                                if (del[i] + nu[i] * tbar >= (Real)0)
                                {
                                    if (delp[i] + nu[i] * tbar <= (Real)0)
                                    {
                                        // The constraints are satisfied, so
                                        // tbar is the first time of contact.
                                        result.intersectionType = 1;
                                        result.contactTime = tbar;
                                        result.contactPoint = sphere.center + tbar * sphereVelocity;
                                        return result;
                                    }
                                }
                            }
                        }
                    }
                }
            }

            // Test for contact with sphere wedges of the sphere-swept
            // volume.  We know that |V|^2 > 0 because of a previous
            // early-exit test.
            for (int im1 = 2, i = 0; i < 3; im1 = i++)
            {
                Real alpha = -Dot(V, Delta[i]);
                if (alpha >= (Real)0)
                {
                    Real sqrLenDelta = Dot(Delta[i], Delta[i]);
                    Real beta = alpha * alpha - sqrLenV * (sqrLenDelta - rsqr);
                    if (beta >= (Real)0)
                    {
                        Real tbar = (alpha - std::sqrt(beta)) / sqrLenV;
                        if (delp[im1] + nu[im1] * tbar >= (Real)0)
                        {
                            if (del[i] + nu[i] * tbar <= (Real)0)
                            {
                                // The constraints are satisfied, so tbar
                                // is the first time of contact.
                                result.intersectionType = 1;
                                result.contactTime = tbar;
                                result.contactPoint = sphere.center + tbar * sphereVelocity;
                                return result;
                            }
                        }
                    }
                }
            }

            // The ray and sphere-swept volume do not intersect, so the sphere
            // and triangle do not come into contact.  The 'result' is already
            // set to the correct state for this case.
            return result;
        }


        // The implementation for arbitrary-precision types.
        using QFN1 = QFNumber<Real, 1>;

        struct ExactResult
        {
            // The cases are
            // 1. Objects initially overlapping.  The contactPoint is only one
            //    of infinitely many points in the overlap.
            //      intersectionType = -1
            //      contactTime = 0
            //      contactPoint = triangle point closest to sphere.center
            // 2. Objects initially separated but do not intersect later.  The
            //      contactTime and contactPoint are invalid.
            //      intersectionType = 0
            //      contactTime = 0
            //      contactPoint = (0,0,0)
            // 3. Objects initially separated but intersect later.
            //      intersectionType = +1
            //      contactTime = first time T > 0
            //      contactPoint = corresponding first contact
            int intersectionType;

            // The exact representation of the contact time and point.  To
            // convert to a floating-point type, use
            //   FloatType contactTime;
            //   Vector3<FloatType> contactPoint;
            //   Result::Convert(result.contactTime, contactTime);
            //   Result::Convert(result.contactPoint, contactPoint);

            template <typename OutputType>
            static void Convert(QFN1 const& input, OutputType& output)
            {
                output = static_cast<Real>(input);
            }

            template <typename OutputType>
            static void Convert(Vector3<QFN1> const& input, Vector3<OutputType>& output)
            {
                for (int i = 0; i < 3; ++i)
                {
                    output[i] = static_cast<Real>(input[i]);
                }
            }

            QFN1 contactTime;
            Vector3<QFN1> contactPoint;
        };

        template <typename Dummy = Real>
        typename std::enable_if<is_arbitrary_precision<Dummy>::value, ExactResult>::type
        operator()(Sphere3<Real> const& sphere, Vector3<Real> const& sphereVelocity,
            Triangle3<Real> const& triangle, Vector3<Real> const& triangleVelocity)
        {
            // The default constructors for the members of 'result' set their
            // own members to zero.
            ExactResult result;

            // Test for initial overlap or contact.
            DCPQuery<Real, Vector3<Real>, Triangle3<Real>> ptQuery;
            auto ptResult = ptQuery(sphere.center, triangle);
            Real rsqr = sphere.radius * sphere.radius;
            if (ptResult.sqrDistance <= rsqr)
            {
                // The values result.contactTime and result.contactPoint[]
                // are both zero, so we need only set the
                // result.contactPoint[].x values.
                result.intersectionType = (ptResult.sqrDistance < rsqr ? -1 : +1);
                for (int j = 0; j < 3; ++j)
                {
                    result.contactPoint[j].x = ptResult.closest[j];
                }
                return result;
            }

            // To reach here, the sphere and triangle are initially separated.
            // Compute the velocity of the sphere relative to the triangle.
            Vector3<Real> V = sphereVelocity - triangleVelocity;
            Real sqrLenV = Dot(V, V);
            if (sqrLenV == (Real)0)
            {
                // The sphere and triangle are separated and the sphere is not
                // moving relative to the triangle, so there is no contact.
                // The 'result' is already set to the correct state for this
                // case.
                return result;
            }

            // Compute the triangle edge directions E[], the vector U normal
            // to the plane of the triangle,  and compute the normals to the
            // edges in the plane of the triangle.  TODO: For a nondeforming
            // triangle (or mesh of triangles), these quantities can all be
            // precomputed to reduce the computational cost of the query.  Add
            // another operator()-query that accepts the precomputed values.
            // TODO: When the triangle is deformable, these quantities must be
            // computed, either by the caller or here.  Optimize the code to
            // compute the quantities on-demand (i.e. only when they are
            // needed, but cache them for later use).
            Vector3<Real> E[3] =
            {
                triangle.v[1] - triangle.v[0],
                triangle.v[2] - triangle.v[1],
                triangle.v[0] - triangle.v[2]
            };
            Real sqrLenE[3] = { Dot(E[0], E[0]), Dot(E[1], E[1]), Dot(E[2], E[2]) };
            // Use an unnormalized U for the plane of the triangle.  This
            // allows us to use quadratic fields for the comparisons of the
            // constraints.
            Vector3<Real> U = Cross(E[0], E[1]);
            Real sqrLenU = Dot(U, U);
            Vector3<Real> ExU[3] =
            {
                Cross(E[0], U),
                Cross(E[1], U),
                Cross(E[2], U)
            };

            // Compute the vectors from the triangle vertices to the sphere
            // center.
            Vector3<Real> Delta[3] =
            {
                sphere.center - triangle.v[0],
                sphere.center - triangle.v[1],
                sphere.center - triangle.v[2]
            };

            // Determine where the sphere center is located relative to the
            // planes of the triangle offset faces of the sphere-swept volume.
            QFN1 const qfzero((Real)0, (Real)0, sqrLenU);
            QFN1 element(Dot(U, Delta[0]), -sphere.radius, sqrLenU);
            if (element >= qfzero)
            {
                // The sphere is on the positive side of Dot(U,X-C) = r|U|.
                // If the sphere will contact the sphere-swept volume at a
                // triangular face, it can do so only on the face of the
                // aforementioned plane.
                Real dotUV = Dot(U, V);
                if (dotUV >= (Real)0)
                {
                    // The sphere is moving away from, or parallel to, the
                    // plane of the triangle.  The 'result' is already set
                    // to the correct state for this case.
                    return result;
                }

                bool foundContact = true;
                for (int i = 0; i < 3; ++i)
                {
                    Real phi = Dot(ExU[i], Delta[i]);
                    Real psi = Dot(ExU[i], V);
                    QFN1 arg(psi * element.x - phi * dotUV, psi * element.y, sqrLenU);
                    if (arg > qfzero)
                    {
                        foundContact = false;
                        break;
                    }
                }
                if (foundContact)
                {
                    result.intersectionType = 1;
                    result.contactTime.x = -element.x / dotUV;
                    result.contactTime.y = -element.y / dotUV;
                    for (int j = 0; j < 3; ++j)
                    {
                        result.contactPoint[j].x = sphere.center[j] + result.contactTime.x * sphereVelocity[j];
                        result.contactPoint[j].y = result.contactTime.y * sphereVelocity[j];
                    }
                    return result;
                }
            }
            else
            {
                element.y = -element.y;
                if (element <= qfzero)
                {
                    // The sphere is on the positive side of Dot(-U,X-C) = r|U|.
                    // If the sphere will contact the sphere-swept volume at a
                    // triangular face, it can do so only on the face of the
                    // aforementioned plane.
                    Real dotUV = Dot(U, V);
                    if (dotUV <= (Real)0)
                    {
                        // The sphere is moving away from, or parallel to, the
                        // plane of the triangle.  The 'result' is already set
                        // to the correct state for this case.
                        return result;
                    }

                    bool foundContact = true;
                    for (int i = 0; i < 3; ++i)
                    {
                        Real phi = Dot(ExU[i], Delta[i]);
                        Real psi = Dot(ExU[i], V);
                        QFN1 arg(phi * dotUV - psi * element.x, -psi * element.y, sqrLenU);
                        if (arg > qfzero)
                        {
                            foundContact = false;
                            break;
                        }
                    }
                    if (foundContact)
                    {
                        result.intersectionType = 1;
                        result.contactTime.x = -element.x / dotUV;
                        result.contactTime.y = -element.y / dotUV;
                        for (int j = 0; j < 3; ++j)
                        {
                            result.contactPoint[j].x = sphere.center[j] + result.contactTime.x * sphereVelocity[j];
                            result.contactPoint[j].y = result.contactTime.y * sphereVelocity[j];
                        }
                        return result;
                    }
                }
                // else: The ray-sphere-swept-volume contact point (if any)
                // cannot be on a triangular face of the sphere-swept-volume.
            }

            // The sphere is moving towards the slab between the two planes
            // of the sphere-swept volume triangular faces.  Determine whether
            // the ray intersects the half cylinders or sphere wedges of the
            // sphere-swept volume.

            // Test for contact with half cylinders of the sphere-swept
            // volume.  First, precompute some dot products required in the
            // computations.  TODO: Optimize the code to compute the quantities
            // on-demand (i.e. only when they are needed, but cache them for
            // later use).
            Real del[3], delp[3], nu[3];
            for (int im1 = 2, i = 0; i < 3; im1 = i++)
            {
                del[i] = Dot(E[i], Delta[i]);
                delp[im1] = Dot(E[im1], Delta[i]);
                nu[i] = Dot(E[i], V);
            }

            for (int i = 2, ip1 = 0; ip1 < 3; i = ip1++)
            {
                Vector3<Real> hatV = V - E[i] * nu[i] / sqrLenE[i];
                Real sqrLenHatV = Dot(hatV, hatV);
                if (sqrLenHatV > (Real)0)
                {
                    Vector3<Real> hatDelta = Delta[i] - E[i] * del[i] / sqrLenE[i];
                    Real alpha = -Dot(hatV, hatDelta);
                    if (alpha >= (Real)0)
                    {
                        Real sqrLenHatDelta = Dot(hatDelta, hatDelta);
                        Real beta = alpha * alpha - sqrLenHatV * (sqrLenHatDelta - rsqr);
                        if (beta >= (Real)0)
                        {
                            QFN1 const qfzero((Real)0, (Real)0, beta);
                            Real mu = Dot(ExU[i], Delta[i]);
                            Real omega = Dot(ExU[i], hatV);
                            QFN1 arg0(mu * sqrLenHatV + omega * alpha, -omega, beta);
                            if (arg0 >= qfzero)
                            {
                                QFN1 arg1(del[i] * sqrLenHatV + nu[i] * alpha, -nu[i], beta);
                                if (arg1 >= qfzero)
                                {
                                    QFN1 arg2(delp[i] * sqrLenHatV + nu[i] * alpha, -nu[i], beta);
                                    if (arg2 <= qfzero)
                                    {
                                        result.intersectionType = 1;
                                        result.contactTime.x = alpha / sqrLenHatV;
                                        result.contactTime.y = (Real)-1 / sqrLenHatV;
                                        for (int j = 0; j < 3; ++j)
                                        {
                                            result.contactPoint[j].x = sphere.center[j] + result.contactTime.x * sphereVelocity[j];
                                            result.contactPoint[j].y = result.contactTime.y * sphereVelocity[j];
                                        }
                                        return result;
                                    }
                                }
                            }
                        }
                    }
                }
            }

            // Test for contact with sphere wedges of the sphere-swept
            // volume.  We know that |V|^2 > 0 because of a previous
            // early-exit test.
            for (int im1 = 2, i = 0; i < 3; im1 = i++)
            {
                Real alpha = -Dot(V, Delta[i]);
                if (alpha >= (Real)0)
                {
                    Real sqrLenDelta = Dot(Delta[i], Delta[i]);
                    Real beta = alpha * alpha - sqrLenV * (sqrLenDelta - rsqr);
                    if (beta >= (Real)0)
                    {
                        QFN1 const qfzero((Real)0, (Real)0, beta);
                        QFN1 arg0(delp[im1] * sqrLenV + nu[im1] * alpha, -nu[im1], beta);
                        if (arg0 >= qfzero)
                        {
                            QFN1 arg1(del[i] * sqrLenV + nu[i] * alpha, -nu[i], beta);
                            if (arg1 <= qfzero)
                            {
                                result.intersectionType = 1;
                                result.contactTime.x = alpha / sqrLenV;
                                result.contactTime.y = (Real)-1 / sqrLenV;
                                for (int j = 0; j < 3; ++j)
                                {
                                    result.contactPoint[j].x = sphere.center[j] + result.contactTime.x * sphereVelocity[j];
                                    result.contactPoint[j].y = result.contactTime.y * sphereVelocity[j];
                                }
                                return result;
                            }
                        }
                    }
                }
            }

            // The ray and sphere-swept volume do not intersect, so the sphere
            // and triangle do not come into contact.  The 'result' is already
            // set to the correct state for this case.
            return result;
        }
    };
}