/******************************************************************************
 *
 * The MIT License (MIT)
 *
 * MIConvexHull, Copyright (c) 2015 David Sehnal, Matthew Campbell
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *  
 *****************************************************************************/

using System;

namespace NWH.DWP2.MiConvexHull
{
    /// <summary>
    ///     A helper class mostly for normal computation. If convex hulls are computed
    ///     in higher dimensions, it might be a good idea to add a specific
    ///     FindNormalVectorND function.
    /// </summary>
    internal class MathHelper
    {
        /// <summary>
        ///     The dimension
        /// </summary>
        private readonly int Dimension;

        /// <summary>
        ///     The matrix pivots
        /// </summary>
        private readonly int[] matrixPivots;

        /// <summary>
        ///     The n d matrix
        /// </summary>
        private readonly double[] nDMatrix;

        /// <summary>
        ///     The n d normal helper vector
        /// </summary>
        private readonly double[] nDNormalHelperVector;

        /// <summary>
        ///     The nt x
        /// </summary>
        private readonly double[] ntX;

        /// <summary>
        ///     The nt y
        /// </summary>
        private readonly double[] ntY;

        /// <summary>
        ///     The nt z
        /// </summary>
        private readonly double[] ntZ;

        /// <summary>
        ///     The position data
        /// </summary>
        private readonly double[] PositionData;


        /// <summary>
        ///     Initializes a new instance of the <see cref="MathHelper" /> class.
        /// </summary>
        /// <param name="dimension">The dimension.</param>
        /// <param name="positions">The positions.</param>
        internal MathHelper(int dimension, double[] positions)
        {
            PositionData = positions;
            Dimension    = dimension;

            ntX = new double[Dimension];
            ntY = new double[Dimension];
            ntZ = new double[Dimension];

            nDNormalHelperVector = new double[Dimension];
            nDMatrix             = new double[Dimension * Dimension];
            matrixPivots         = new int[Dimension];
        }


        /// <summary>
        ///     Calculates the normal and offset of the hyper-plane given by the face's vertices.
        /// </summary>
        /// <param name="face">The face.</param>
        /// <param name="center">The center.</param>
        /// <returns><c>true</c> if XXXX, <c>false</c> otherwise.</returns>
        internal bool CalculateFacePlane(ConvexFaceInternal face, double[] center)
        {
            int[]    vertices = face.Vertices;
            double[] normal   = face.Normal;
            FindNormalVector(vertices, normal);

            if (double.IsNaN(normal[0]))
            {
                return false;
            }

            double offset         = 0.0;
            double centerDistance = 0.0;
            int    fi             = vertices[0] * Dimension;
            for (int i = 0; i < Dimension; i++)
            {
                double n = normal[i];
                offset         += n * PositionData[fi + i];
                centerDistance += n * center[i];
            }

            face.Offset    =  -offset;
            centerDistance -= offset;

            if (centerDistance > 0)
            {
                for (int i = 0; i < Dimension; i++)
                {
                    normal[i] = -normal[i];
                }

                face.Offset          = offset;
                face.IsNormalFlipped = true;
            }
            else
            {
                face.IsNormalFlipped = false;
            }

            return true;
        }


        /// <summary>
        ///     Check if the vertex is "visible" from the face.
        ///     The vertex is "over face" if the return value is &gt; Constants.PlaneDistanceTolerance.
        /// </summary>
        /// <param name="v">The v.</param>
        /// <param name="f">The f.</param>
        /// <returns>The vertex is "over face" if the result is positive.</returns>
        internal double GetVertexDistance(int v, ConvexFaceInternal f)
        {
            double[] normal   = f.Normal;
            int      x        = v * Dimension;
            double   distance = f.Offset;
            for (int i = 0; i < normal.Length; i++)
            {
                distance += normal[i] * PositionData[x + i];
            }

            return distance;
        }


        /// <summary>
        ///     Returns the vector the between vertices.
        /// </summary>
        /// <param name="fromIndex">From index.</param>
        /// <param name="toIndex">To index.</param>
        /// <param name="target">The target.</param>
        /// <returns></returns>
        internal double[] VectorBetweenVertices(int toIndex, int fromIndex)
        {
            double[] target = new double[Dimension];
            VectorBetweenVertices(toIndex, fromIndex, target);
            return target;
        }


        /// <summary>
        ///     Returns the vector the between vertices.
        /// </summary>
        /// <param name="fromIndex">From index.</param>
        /// <param name="toIndex">To index.</param>
        /// <param name="target">The target.</param>
        /// <returns></returns>
        private void VectorBetweenVertices(int toIndex, int fromIndex, double[] target)
        {
            int u = toIndex * Dimension, v = fromIndex * Dimension;
            for (int i = 0; i < Dimension; i++)
            {
                target[i] = PositionData[u + i] - PositionData[v + i];
            }
        }


        // Modified from Math.NET
        // Copyright (c) 2009-2013 Math.NET
        /// <summary>
        ///     Lus the factor.
        /// </summary>
        /// <param name="data">The data.</param>
        /// <param name="order">The order.</param>
        /// <param name="ipiv">The ipiv.</param>
        /// <param name="vecLUcolj">The vec l ucolj.</param>
        private static void LUFactor(double[] data, int order, int[] ipiv, double[] vecLUcolj)
        {
            // Initialize the pivot matrix to the identity permutation.
            for (int i = 0; i < order; i++)
            {
                ipiv[i] = i;
            }

            // Outer loop.
            for (int j = 0; j < order; j++)
            {
                int indexj  = j * order;
                int indexjj = indexj + j;

                // Make a copy of the j-th column to localize references.
                for (int i = 0; i < order; i++)
                {
                    vecLUcolj[i] = data[indexj + i];
                }

                // Apply previous transformations.
                for (int i = 0; i < order; i++)
                {
                    // Most of the time is spent in the following dot product.
                    int    kmax = Math.Min(i, j);
                    double s    = 0.0;
                    for (int k = 0; k < kmax; k++)
                    {
                        s += data[k * order + i] * vecLUcolj[k];
                    }

                    data[indexj + i] = vecLUcolj[i] -= s;
                }

                // Find pivot and exchange if necessary.
                int p = j;
                for (int i = j + 1; i < order; i++)
                {
                    if (Math.Abs(vecLUcolj[i]) > Math.Abs(vecLUcolj[p]))
                    {
                        p = i;
                    }
                }

                if (p != j)
                {
                    for (int k = 0; k < order; k++)
                    {
                        int    indexk  = k * order;
                        int    indexkp = indexk + p;
                        int    indexkj = indexk + j;
                        double temp    = data[indexkp];
                        data[indexkp] = data[indexkj];
                        data[indexkj] = temp;
                    }

                    ipiv[j] = p;
                }

                // Compute multipliers.
                if ((j < order) & (data[indexjj] != 0.0))
                {
                    for (int i = j + 1; i < order; i++)
                    {
                        data[indexj + i] /= data[indexjj];
                    }
                }
            }
        }


        #region Find the normal vector of the face

        /// <summary>
        ///     Finds normal vector of a hyper-plane given by vertices.
        ///     Stores the results to normalData.
        /// </summary>
        /// <param name="vertices">The vertices.</param>
        /// <param name="normalData">The normal data.</param>
        private void FindNormalVector(int[] vertices, double[] normalData)
        {
            switch (Dimension)
            {
                case 2:
                    FindNormalVector2D(vertices, normalData);
                    break;
                case 3:
                    FindNormalVector3D(vertices, normalData);
                    break;
                case 4:
                    FindNormalVector4D(vertices, normalData);
                    break;
                default:
                    FindNormalVectorND(vertices, normalData);
                    break;
            }
        }


        /// <summary>
        ///     Finds 2D normal vector.
        /// </summary>
        /// <param name="vertices">The vertices.</param>
        /// <param name="normal">The normal.</param>
        private void FindNormalVector2D(int[] vertices, double[] normal)
        {
            VectorBetweenVertices(vertices[1], vertices[0], ntX);

            double nx = -ntX[1];
            double ny = ntX[0];

            double norm = Math.Sqrt(nx * nx + ny * ny);

            double f = 1.0 / norm;
            normal[0] = f * nx;
            normal[1] = f * ny;
        }


        /// <summary>
        ///     Finds 3D normal vector.
        /// </summary>
        /// <param name="vertices">The vertices.</param>
        /// <param name="normal">The normal.</param>
        private void FindNormalVector3D(int[] vertices, double[] normal)
        {
            VectorBetweenVertices(vertices[1], vertices[0], ntX);
            VectorBetweenVertices(vertices[2], vertices[1], ntY);

            double nx = ntX[1] * ntY[2] - ntX[2] * ntY[1];
            double ny = ntX[2] * ntY[0] - ntX[0] * ntY[2];
            double nz = ntX[0] * ntY[1] - ntX[1] * ntY[0];

            double norm = Math.Sqrt(nx * nx + ny * ny + nz * nz);

            double f = 1.0 / norm;
            normal[0] = f * nx;
            normal[1] = f * ny;
            normal[2] = f * nz;
        }


        /// <summary>
        ///     Finds 4D normal vector.
        /// </summary>
        /// <param name="vertices">The vertices.</param>
        /// <param name="normal">The normal.</param>
        private void FindNormalVector4D(int[] vertices, double[] normal)
        {
            VectorBetweenVertices(vertices[1], vertices[0], ntX);
            VectorBetweenVertices(vertices[2], vertices[1], ntY);
            VectorBetweenVertices(vertices[3], vertices[2], ntZ);

            double[] x = ntX;
            double[] y = ntY;
            double[] z = ntZ;

            // This was generated using Mathematica
            double nx = x[3] * (y[2] * z[1] - y[1] * z[2])
                        + x[2] * (y[1] * z[3] - y[3] * z[1])
                        + x[1] * (y[3] * z[2] - y[2] * z[3]);
            double ny = x[3] * (y[0] * z[2] - y[2] * z[0])
                        + x[2] * (y[3] * z[0] - y[0] * z[3])
                        + x[0] * (y[2] * z[3] - y[3] * z[2]);
            double nz = x[3] * (y[1] * z[0] - y[0] * z[1])
                        + x[1] * (y[0] * z[3] - y[3] * z[0])
                        + x[0] * (y[3] * z[1] - y[1] * z[3]);
            double nw = x[2] * (y[0] * z[1] - y[1] * z[0])
                        + x[1] * (y[2] * z[0] - y[0] * z[2])
                        + x[0] * (y[1] * z[2] - y[2] * z[1]);

            double norm = Math.Sqrt(nx * nx + ny * ny + nz * nz + nw * nw);

            double f = 1.0 / norm;
            normal[0] = f * nx;
            normal[1] = f * ny;
            normal[2] = f * nz;
            normal[3] = f * nw;
        }


        /// <summary>
        ///     Finds the normal vector nd.
        /// </summary>
        /// <param name="vertices">The vertices.</param>
        /// <param name="normal">The normal.</param>
        private void FindNormalVectorND(int[] vertices, double[] normal)
        {
            /* We need to solve the matrix A n = B where
             *  - A contains coordinates of vertices as columns
             *  - B is vector with all 1's. Really, it should be the distance of 
             *      the plane from the origin, but - since we're not worried about that
             *      here and we will normalize the normal anyway - all 1's suffices.
             */
            int[]    iPiv = matrixPivots;
            double[] data = nDMatrix;
            double   norm = 0.0;

            // Solve determinants by replacing x-th column by all 1.
            for (int x = 0; x < Dimension; x++)
            {
                for (int i = 0; i < Dimension; i++)
                {
                    int offset = vertices[i] * Dimension;
                    for (int j = 0; j < Dimension; j++)
                    {
                        // maybe I got the i/j mixed up here regarding the representation Math.net uses...
                        // ...but it does not matter since Det(A) = Det(Transpose(A)).
                        data[Dimension * i + j] = j == x ? 1.0 : PositionData[offset + j];
                    }
                }

                LUFactor(data, Dimension, iPiv, nDNormalHelperVector);
                double coord = 1.0;
                for (int i = 0; i < Dimension; i++)
                {
                    if (iPiv[i] != i)
                    {
                        coord *= -data[Dimension * i + i]; // the determinant sign changes on row swap.
                    }
                    else
                    {
                        coord *= data[Dimension * i + i];
                    }
                }

                normal[x] =  coord;
                norm      += coord * coord;
            }

            // Normalize the result
            double f = 1.0 / Math.Sqrt(norm);
            for (int i = 0; i < normal.Length; i++)
            {
                normal[i] *= f;
            }
        }

        #endregion

        #region Simplex Volume

        /// <summary>
        ///     Gets the simplex volume. Prior to having enough edge vectors, the method pads the remaining with all
        ///     "other numbers". So, yes, this method is not really finding the volume. But a relative volume-like measure. It
        ///     uses the magnitude of the determinant as the volume stand-in following the Cayley-Menger theorem.
        /// </summary>
        /// <param name="edgeVectors">The edge vectors.</param>
        /// <param name="lastIndex">The last index.</param>
        /// <returns></returns>
        internal double GetSimplexVolume(double[][] edgeVectors, int lastIndex, double bigNumber)
        {
            double[] A     = new double[Dimension * Dimension];
            int      index = 0;
            for (int i = 0; i < Dimension; i++)
            for (int j = 0; j < Dimension; j++)
            {
                if (i <= lastIndex)
                {
                    A[index++] = edgeVectors[i][j];
                }
                else
                {
                    A[index] = Math.Pow(-1, index) * index++ / bigNumber;
                }
            }

            // this last term is used for all the vertices in the comparison for the yet determined vertices
            // the idea is to come up with sets of numbers that are orthogonal so that an non-zero value will result
            // and to choose smallish numbers since the choice of vectors will affect what the end volume is.
            // A better way (todo?) is to solve a smaller matrix. However, cases were found in which the obvious smaller vector
            // (the upper left) had too many zeros. So, one would need to find the right subset. Indeed choosing a subset
            // biases the first dimensions of the others. Perhaps a larger volume would be created from a different vertex
            // if another subset of dimensions were used. 
            return Math.Abs(DeterminantDestructive(A));
        }


        /// <summary>
        ///     Determinants the destructive.
        /// </summary>
        /// <param name="buff">The buff.</param>
        /// <returns>System.Double.</returns>
        private double DeterminantDestructive(double[] A)
        {
            switch (Dimension)
            {
                case 0:
                    return 0.0;
                case 1:
                    return A[0];
                case 2:
                    return A[0] * A[3] - A[1] * A[2];
                case 3:
                    return A[0] * A[4] * A[8] + A[1] * A[5] * A[6] + A[2] * A[3] * A[7]
                           - A[0] * A[5] * A[7] - A[1] * A[3] * A[8] - A[2] * A[4] * A[6];
                default:
                {
                    int[]    iPiv   = new int[Dimension];
                    double[] helper = new double[Dimension];
                    LUFactor(A, Dimension, iPiv, helper);
                    double det = 1.0;
                    for (int i = 0; i < iPiv.Length; i++)
                    {
                        det *= A[Dimension * i + i];
                        if (iPiv[i] != i)
                        {
                            det *= -1; // the determinant sign changes on row swap.
                        }
                    }

                    return det;
                }
            }
        }

        #endregion
    }
}