// 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/Matrix3x3.h>
#include <random>

// Compute the minimum-volume ellipsoid, (X-C)^T R D R^T (X-C) = 1, given the
// center C and orientation matrix R.  The columns of R are the axes of the
// ellipsoid.  The algorithm computes the diagonal matrix D.  The minimum
// volume is (4*pi/3)/sqrt(D[0]*D[1]*D[2]), where D = diag(D[0],D[1],D[2]).
// The problem is equivalent to maximizing the product D[0]*D[1]*D[2] for a
// given C and R, and subject to the constraints
//   (P[i]-C)^T R D R^T (P[i]-C) <= 1
// for all input points P[i] with 0 <= i < N.  Each constraint has the form
//   A[0]*D[0] + A[1]*D[1] + A[2]*D[2] <= 1
// where A[0] >= 0, A[1] >= 0, and A[2] >= 0.

namespace WwiseGTE
{
    template <typename Real>
    class ContEllipsoid3MinCR
    {
    public:
        void operator()(int numPoints, Vector3<Real> const* points,
            Vector3<Real> const& C, Matrix3x3<Real> const& R, Real D[3]) const
        {
            // Compute the constraint coefficients, of the form (A[0],A[1])
            // for each i.
            std::vector<Vector3<Real>> A(numPoints);
            for (int i = 0; i < numPoints; ++i)
            {
                Vector3<Real> diff = points[i] - C;  // P[i] - C
                Vector3<Real> prod = diff * R;  // R^T*(P[i] - C) = (u,v,w)
                A[i] = prod * prod;  // (u^2, v^2, w^2)
            }

            // TODO:  Sort the constraints to eliminate redundant ones.  It
            // is clear how to do this in ContEllipse2MinCR.  How to do this
            // in 3D?

            MaxProduct(A, D);
        }

    private:
        void FindEdgeMax(std::vector<Vector3<Real>>& A, int& plane0, int& plane1, Real D[3]) const
        {
            // Compute direction to local maximum point on line of
            // intersection.
            Real xDir = A[plane0][1] * A[plane1][2] - A[plane1][1] * A[plane0][2];
            Real yDir = A[plane0][2] * A[plane1][0] - A[plane1][2] * A[plane0][0];
            Real zDir = A[plane0][0] * A[plane1][1] - A[plane1][0] * A[plane0][1];

            // Build quadratic Q'(t) = (d/dt)(x(t)y(t)z(t)) = a0+a1*t+a2*t^2.
            Real a0 = D[0] * D[1] * zDir + D[0] * D[2] * yDir + D[1] * D[2] * xDir;
            Real a1 = (Real)2 * (D[2] * xDir * yDir + D[1] * xDir * zDir + D[0] * yDir * zDir);
            Real a2 = (Real)3 * (xDir * yDir * zDir);

            // Find root to Q'(t) = 0 corresponding to maximum.
            Real tFinal;
            if (a2 != (Real)0)
            {
                Real invA2 = (Real)1 / a2;
                Real discr = a1 * a1 - (Real)4 * a0 * a2;
                discr = std::sqrt(std::max(discr, (Real)0));
                tFinal = (Real)-0.5 * (a1 + discr) * invA2;
                if (a1 + (Real)2 * a2 * tFinal > (Real)0)
                {
                    tFinal = (Real)0.5 * (-a1 + discr) * invA2;
                }
            }
            else if (a1 != (Real)0)
            {
                tFinal = -a0 / a1;
            }
            else if (a0 != (Real)0)
            {
                Real fmax = std::numeric_limits<Real>::max();
                tFinal = (a0 >= (Real)0 ? fmax : -fmax);
            }
            else
            {
                return;
            }

            if (tFinal < (Real)0)
            {
                // Make (xDir,yDir,zDir) point in direction of increase of Q.
                tFinal = -tFinal;
                xDir = -xDir;
                yDir = -yDir;
                zDir = -zDir;
            }

            // Sort remaining planes along line from current point to local
            // maximum.
            Real tMax = tFinal;
            int plane2 = -1;
            int numPoints = static_cast<int>(A.size());
            for (int i = 0; i < numPoints; ++i)
            {
                if (i == plane0 || i == plane1)
                {
                    continue;
                }

                Real norDotDir = A[i][0] * xDir + A[i][1] * yDir + A[i][2] * zDir;
                if (norDotDir <= (Real)0)
                {
                    continue;
                }

                // Theoretically the numerator must be nonnegative since an
                // invariant in the algorithm is that (x0,y0,z0) is on the
                // convex hull of the constraints.  However, some numerical
                // error may make this a small negative number.  In that case
                // set tmax = 0 (no change in position).
                Real numer = (Real)1 - A[i][0] * D[0] - A[i][1] * D[1] - A[i][2] * D[2];
                LogAssert(numer >= (Real)0, "Unexpected condition.");

                Real t = numer / norDotDir;
                if (0 <= t && t < tMax)
                {
                    plane2 = i;
                    tMax = t;
                }
            }

            D[0] += tMax * xDir;
            D[1] += tMax * yDir;
            D[2] += tMax * zDir;

            if (tMax == tFinal)
            {
                return;
            }

            if (tMax > (Real)0)
            {
                plane0 = plane2;
                FindFacetMax(A, plane0, D);
                return;
            }

            // tmax == 0, so return with D[0], D[1], and D[2] unchanged.
        }

        void FindFacetMax(std::vector<Vector3<Real>>& A, int& plane0, Real D[3]) const
        {
            Real tFinal, xDir, yDir, zDir;

            if (A[plane0][0] > (Real)0
                && A[plane0][1] > (Real)0
                && A[plane0][2] > (Real)0)
            {
                // Compute local maximum point on plane.
                Real oneThird = (Real)1 / (Real)3;
                Real xMax = oneThird / A[plane0][0];
                Real yMax = oneThird / A[plane0][1];
                Real zMax = oneThird / A[plane0][2];

                // Compute direction to local maximum point on plane.
                tFinal = (Real)1;
                xDir = xMax - D[0];
                yDir = yMax - D[1];
                zDir = zMax - D[2];
            }
            else
            {
                tFinal = std::numeric_limits<Real>::max();

                if (A[plane0][0] > (Real)0)
                {
                    xDir = (Real)0;
                }
                else
                {
                    xDir = (Real)1;
                }

                if (A[plane0][1] > (Real)0)
                {
                    yDir = (Real)0;
                }
                else
                {
                    yDir = (Real)1;
                }

                if (A[plane0][2] > (Real)0)
                {
                    zDir = (Real)0;
                }
                else
                {
                    zDir = (Real)1;
                }
            }

            // Sort remaining planes along line from current point.
            Real tMax = tFinal;
            int plane1 = -1;
            int numPoints = static_cast<int>(A.size());
            for (int i = 0; i < numPoints; ++i)
            {
                if (i == plane0)
                {
                    continue;
                }

                Real norDotDir = A[i][0] * xDir + A[i][1] * yDir + A[i][2] * zDir;
                if (norDotDir <= (Real)0)
                {
                    continue;
                }

                // Theoretically the numerator must be nonnegative because an
                // invariant in the algorithm is that (x0,y0,z0) is on the
                // convex hull of the constraints.  However, some numerical
                // error may make this a small negative number.  In that case,
                // set tmax = 0 (no change in position).
                Real numer = (Real)1 - A[i][0] * D[0] - A[i][1] * D[1] - A[i][2] * D[2];
                LogAssert(numer >= (Real)0, "Unexpected condition.");

                Real t = numer / norDotDir;
                if (0 <= t && t < tMax)
                {
                    plane1 = i;
                    tMax = t;
                }
            }

            D[0] += tMax * xDir;
            D[1] += tMax * yDir;
            D[2] += tMax * zDir;

            if (tMax == (Real)1)
            {
                return;
            }

            if (tMax > (Real)0)
            {
                plane0 = plane1;
                FindFacetMax(A, plane0, D);
                return;
            }

            FindEdgeMax(A, plane0, plane1, D);
        }

        void MaxProduct(std::vector<Vector3<Real>>& A, Real D[3]) const
        {
            // Maximize x*y*z subject to x >= 0, y >= 0, z >= 0, and
            // A[i]*x+B[i]*y+C[i]*z <= 1 for 0 <= i < N where A[i] >= 0,
            // B[i] >= 0, and C[i] >= 0.

            // Jitter the lines to avoid cases where more than three planes
            // intersect at the same point.  Should also break parallelism
            // and planes parallel to the coordinate planes.
            std::mt19937 mte;
            std::uniform_real_distribution<Real> rnd((Real)0, (Real)1);
            Real maxJitter = (Real)1e-12;
            int numPoints = static_cast<int>(A.size());
            int i;
            for (i = 0; i < numPoints; ++i)
            {
                A[i][0] += maxJitter * rnd(mte);
                A[i][1] += maxJitter * rnd(mte);
                A[i][2] += maxJitter * rnd(mte);
            }

            // Sort lines along the z-axis (x = 0 and y = 0).
            int plane = -1;
            Real zmax = (Real)0;
            for (i = 0; i < numPoints; ++i)
            {
                if (A[i][2] > zmax)
                {
                    zmax = A[i][2];
                    plane = i;
                }
            }
            LogAssert(plane != -1, "Unexpected condition.");

            // Walk along convex hull searching for maximum.
            D[0] = (Real)0;
            D[1] = (Real)0;
            D[2] = (Real)1 / zmax;
            FindFacetMax(A, plane, D);
        }
    };
}