using System;
using System.Collections.Generic;
using System.Linq;
using Deltares.Mathematics;

// ===========================================================================================
// Class name     : TMatrix
//
// Description    : Class for handling Matrix Routines
// Functions are from Numerical recipies in Pascal,
// Cambridge University Press
//
// Date        ID   Modification
// ====        ==   ============
// 2000-02-06  Sel  Created
// 2008-06-05  Best Copied from MGeolib
// 2012-06-28  Best class Matrix with CholeskyDecomposition added
// ===========================================================================================

namespace Deltares.Stability.Calculation2.Levenberg
{
    public class EDMatrixError : Exception
    {
        //@ Constructor auto-generated
        public EDMatrixError(String message)
            : base(message) { }

        //@ Constructor auto-generated
        public EDMatrixError(String message, Exception innerException)
            : base(message, innerException) { }
    }

    public class TMatrix
    {
        private int[] FIndex;
        private double determinant;

        public TMatrix()
        {
            Tolerance = 0;
        }

        public int RowCount
        {
            get
            {
                return Matrix == null ? 0 : Matrix.Length;
            }
        }

        public int ColumnCount
        {
            get
            {
                return RowCount > 0 ? Matrix[0].Length : 0;
            }
        }

        /// <summary>
        /// Data matrix, with first indexer being the row and second being the column.
        /// </summary>
        public double[][] Matrix { get; private set; }

        /// <summary>
        /// Data vector, of size equal to <see cref="RowCount"/>.
        /// </summary>
        /// <remarks>Contents are independent of <see cref="Matrix"/>.</remarks>
        public double[] Vector { get; private set; }

        /// <summary>
        ///     Gets or sets the <see cref="System.Double" /> in <see cref="Matrix"/>.
        /// </summary>
        /// <value>
        ///     The <see cref="System.Double" /> to be stored in <see cref="Matrix" /> at the
        ///     given indexes.
        /// </value>
        /// <param name="aRowNumber">a row number.</param>
        /// <param name="aColumnNumber">a column number.</param>
        /// <returns>
        ///     The <see cref="System.Double" /> stored in <see cref="Matrix" /> at the
        ///     given indexes, or 0 if indexes are out of bounds.
        /// </returns>
        /// <remarks>
        ///     To speed up the performance for large matrices, use <see cref="Matrix "/>
        ///     directly rather than this index property.
        /// </remarks>
        public double this[int aRowNumber, int aColumnNumber]
        {
            get
            {
                return aRowNumber >= 0 && aRowNumber < RowCount &&
                       aColumnNumber >= 0 && aColumnNumber < ColumnCount
                           ? Matrix[aRowNumber][aColumnNumber]
                           : 0;
            }
            set
            {
                if (aRowNumber >= 0 && aRowNumber < RowCount &&
                    aColumnNumber >= 0 && aColumnNumber < ColumnCount)
                {
                    Matrix[aRowNumber][aColumnNumber] = value;
                }
            }
        }

        public double Tolerance { get; set; }

        /// <summary>
        /// Clears and resizes <see cref="Matrix"/> and <see cref="Vector"/>.
        /// </summary>
        /// <param name="rowCount">Number of matrix rows and size of vector.</param>
        /// <param name="columnCount">Number of matrix columns.</param>
        /// <exception cref="EDMatrixError">When either <paramref name="rowCount"/> or
        /// <paramref name="columnCount"/> is less then zero.</exception>
        public void SetDimensions(int rowCount, int columnCount)
        {
            if (rowCount < 0 || columnCount < 0)
            {
                throw new EDMatrixError("MatrixErrorOutOfRange");
            }

            Matrix = ArraySupport.CreateJaggedArray(rowCount, columnCount);
            Vector = new double[rowCount];
            FIndex = new int[rowCount];
        }

        // ===========================================================================================
        // Description    : LU_Decomposision of a square matrix, equation (2.3.2),
        // Numerical recipies in Pascal.
        // The Source may be identical with the Target.
        // On output, the Target Matrix is replaced by the Source's
        // LU_Decomposision of a rowwise permutation of itself.
        // FIndex records the row permutation by the partial pivoting.
        // The FDeterminant keeps track of even or odd row interchanges,
        // which is important to determine the determinant.
        // This routine is used in combination with LU_BackSubst
        // to solve linear equations or invert a matrix.
        //
        // Date        ID   Modification
        // 1997-10-23  Sel  Created
        // 1998.11.11  Sel  Adapted to List of TDMatVector
        // 2000.02.06  Sel  Zero based index origin applied
        // 2000.10.30  Sel  Determination Determinant removed
        // ===========================================================================================
        public void Decomposition(TMatrix source)
        {
            // For performance:
            var rowCount = RowCount;
            var columnCount = ColumnCount;

            if (rowCount != source.RowCount || columnCount != source.ColumnCount)
            {
                throw new EDMatrixError("sMatrixErrorNoEqualSize");
            }
            if (rowCount != columnCount)
            {
                throw new EDMatrixError("sMatrixErrorNotSquared");
            }

            // stores the implicit scaling of each row
            var rowNumber = -1;
            double largeValue = 0;
            var scaling = new double[rowCount];

            // define the Scaling
            for (int rowIndex = 0; rowIndex < rowCount; rowIndex++)
            {
                largeValue = 0;
                // Copy the source on the way
                Vector[rowIndex] = source.Vector[rowIndex];
                for (int columnIndex = 0; columnIndex < columnCount; columnIndex++)
                {
                    Matrix[rowIndex][columnIndex] = source.Matrix[rowIndex][columnIndex];
                    largeValue = Math.Max(largeValue, Math.Abs(source.Matrix[rowIndex][columnIndex]));
                }
                if (largeValue == 0)
                {
                    throw new EDMatrixError("sMatrixErrorSingular");
                }
                else
                {
                    scaling[rowIndex] = 1 / largeValue;
                }
            }

            // execute Crouts method
            var tolerance = Tolerance * largeValue;
            determinant = 1.0;
            // No row interchanges yet
            for (int columnIndex = 0; columnIndex < columnCount; columnIndex++)
            {
                // Equation (2.3.12) except for rowIndex = LColumnIndex
                double croutSum;
                for (int rowIndex = 0; rowIndex < columnIndex; rowIndex++)
                {
                    croutSum = Matrix[rowIndex][columnIndex];
                    for (int commonIndex = 0; commonIndex < rowIndex; commonIndex++)
                    {
                        croutSum -= Matrix[rowIndex][commonIndex] * Matrix[commonIndex][columnIndex];
                    }
                    Matrix[rowIndex][columnIndex] = croutSum;
                }

                // rowIndex = LColumnIndex of equation (2.3.12) and equation (2.3.13)
                largeValue = tolerance;
                //double LDummy;
                for (int rowIndex = columnIndex; rowIndex < rowCount; rowIndex++)
                {
                    croutSum = Matrix[rowIndex][columnIndex];
                    for (int commonIndex = 0; commonIndex < columnIndex; commonIndex++)
                    {
                        croutSum -= Matrix[rowIndex][commonIndex] * Matrix[commonIndex][columnIndex];
                    }
                    Matrix[rowIndex][columnIndex] = croutSum;
                    // Is the figure of merit for the pivot better than the best so far?
                    var dummy = scaling[rowIndex] * Math.Abs(croutSum);
                    if (dummy >= largeValue)
                    {
                        largeValue = dummy;
                        rowNumber = rowIndex;
                    }
                }

                if (largeValue == tolerance)
                {
                    throw new EDMatrixError("sMatrixErrorSingular");
                }

                // Do we need to interchange rows?
                if (columnIndex != rowNumber)
                {
                    for (int commonIndex = 0; commonIndex < columnCount; commonIndex++)
                    {
                        var swapValue = Matrix[rowNumber][commonIndex];
                        Matrix[rowNumber][commonIndex] = Matrix[columnIndex][commonIndex];
                        Matrix[columnIndex][commonIndex] = swapValue;
                    }
                    // Change the parity of FDeterminant and interchange the Scaling factor
                    determinant = -determinant;
                    scaling[rowNumber] = scaling[columnIndex];
                }

                // Now, finally, divide by the pivot element
                FIndex[columnIndex] = rowNumber;
                if (columnIndex != columnCount)
                {
                    var pivotFactor = 1 / Matrix[columnIndex][columnIndex];
                    for (int rowIndex = columnIndex + 1; rowIndex < rowCount; rowIndex++)
                    {
                        Matrix[rowIndex][columnIndex] = Matrix[rowIndex][columnIndex] * pivotFactor;
                    }
                }
            }

            // Determine the determinant value
            for (int diagonalIndex = 0; diagonalIndex < rowCount; diagonalIndex++)
            {
                determinant = determinant * Matrix[diagonalIndex][diagonalIndex];
            }
        }

        // ===========================================================================================
        // Description    : The solution vector FVector[1. .RowCount] is improved, see
        // equations (2.7.1) ...(2.7.4). The orignal matrix Source.Matrix
        // and vector Source.Vector are applied.
        //
        // Date        ID   Modification
        // 1997-10-27  Sel  Created
        // 1998.11.11  Sel  Adapted to List of TDMatVector
        // 2000.02.06  Sel  Zero based inxex origin applied
        // ===========================================================================================
        public void ImprovedSolver(TMatrix ASource)
        {
            // Check if Source is different from the matrix
            if (this == ASource)
            {
                throw new EDMatrixError("sMatrixErrorNoSource");
            }
            // Check if the matrix is square
            if (RowCount != ColumnCount)
            {
                throw new EDMatrixError("sMatrixErrorNotSquared");
            }
            BackSubstitution(Vector);
            var LRightHand = new double[RowCount];
            // Calculate the right-handside, accumulating the residual
            for (var LRowIndex = 0; LRowIndex < RowCount; LRowIndex++)
            {
                var LSolverSum = -ASource.Vector[LRowIndex];
                for (var LColumnIndex = 0; LColumnIndex < ColumnCount; LColumnIndex++)
                {
                    //@ Unsupported property or method(A): 'Matrix'
                    LSolverSum = LSolverSum + ASource.Matrix[LRowIndex][LColumnIndex] * Vector[LColumnIndex];
                }
                LRightHand[LRowIndex] = LSolverSum;
            }
            // Solve for the error term, and subtract it from the old solution
            BackSubstitution(LRightHand);
            for (var LRowIndex = 0; LRowIndex < RowCount; LRowIndex++)
            {
                Vector[LRowIndex] = Vector[LRowIndex] - LRightHand[LRowIndex];
            }
        }

        // ===========================================================================================
        // Description    : Solves the set of RowCount linear equations A.X = B. Here
        // FMatrix[l..RowCount,l..RowCount] is input, not as the matrix A
        // but rather as its LU decomposition, determined by the routine
        // Decomposition. FIndex[l..RowCount] is input as the permutation
        // vector returned by Decomposition. FVector[l..RowCount] is input
        // as the right-hand side vector B, and returns with the solution
        // vector X. FMatrix, RowCount, and FIndex are not modified by
        // this routine and can be left in place for successive calls
        // with different right-hand sides FVector. This routine takes
        // into account the possibility that FVector will begin with many
        // zero elements, so it is efficient for use in matrix inversion.
        //
        // Date        ID   Modification
        // 1997-10-27  Sel  Created
        // 1998.11.11  Sel  Adapted to List of TDMatVector
        // 2000.02.07  Sel  Zero based inxex origin applied
        // ===========================================================================================

        /// <summary>
        /// Multiplies this <see cref="Matrix"/> with another matrix and vector.
        /// </summary>
        /// <param name="other">The other matrix and vector.</param>
        /// <returns>A new matrix and vector.</returns>
        public TMatrix Multiply(TMatrix other)
        {
            if (ColumnCount != other.RowCount)
            {
                throw new EDMatrixError("sMatrixErrorNoEqualSize");
            }

            var result = new TMatrix();
            result.SetDimensions(RowCount, other.ColumnCount);

            for (var rowIndex = 0; rowIndex < RowCount; rowIndex++)
            {
                for (var commonIndex = 0; commonIndex < ColumnCount; commonIndex++)
                {
                    var matrixValue = Matrix[rowIndex][commonIndex];
                    for (var otherColumnIndex = 0; otherColumnIndex < other.ColumnCount; otherColumnIndex++)
                    {
                        result.Matrix[rowIndex][otherColumnIndex] += matrixValue * other.Matrix[commonIndex][otherColumnIndex];
                    }
                    result.Vector[rowIndex] += matrixValue * other.Vector[commonIndex];
                }
            }
            return result;
        }

        /// <summary>
        /// Product of Transposed Matrix and Matrix self
        /// </summary>
        public TMatrix Quadrature()
        {
            return Transpose().Multiply(this);
        }

        /// <summary>
        /// Performs a matrix addition.
        /// </summary>
        /// <param name="other">Another matrix.</param>
        /// <exception cref="EDMatrixError">When <paramref name="other"/> is not of the same
        /// size as this.</exception>
        /// <remarks>This method causes no change to <see cref="Vector"/>.</remarks>
        public void Add(TMatrix other)
        {
            // For performance:
            var rowCount = RowCount;
            var columnCount = ColumnCount;

            if (rowCount != other.RowCount || columnCount != other.ColumnCount)
            {
                throw new EDMatrixError("sMatrixErrorNoEqualSize");
            }

            for (var rowIndex = 0; rowIndex < rowCount; rowIndex++)
            {
                for (var columnIndex = 0; columnIndex < columnCount; columnIndex++)
                {
                    Matrix[rowIndex][columnIndex] = Matrix[rowIndex][columnIndex] + other.Matrix[rowIndex][columnIndex];
                }
            }
        }

        /// <summary>
        /// Multiply all <see cref="Matrix"/> elements by a factor.
        /// </summary>
        /// <param name="factor">The multiplication factor.</param>
        public void MultiplyByFactor(double factor)
        {
            // For performance:
            var rowCount = RowCount;
            var columnCount = ColumnCount;

            for (var rowIndex = 0; rowIndex < rowCount; rowIndex++)
            {
                for (var columnIndex = 0; columnIndex < columnCount; columnIndex++)
                {
                    Matrix[rowIndex][columnIndex] = factor * Matrix[rowIndex][columnIndex];
                }
            }
        }

        /// <summary>
        /// Returns the transpose of <see cref="Matrix"/>.
        /// </summary>
        public TMatrix Transpose()
        {
            // For performance
            var columnCount = ColumnCount;
            var rowCount = RowCount;

            var result = new TMatrix();
            result.SetDimensions(columnCount, rowCount);

            for (var rowIndex = 0; rowIndex < rowCount; rowIndex++)
            {
                for (var columnIndex = 0; columnIndex < columnCount; columnIndex++)
                {
                    result.Matrix[columnIndex][rowIndex] = Matrix[rowIndex][columnIndex];
                }
            }
            return result;
        }

        /// <summary>
        /// Creates a clone of <see cref="Matrix"/> and <see cref="Vector"/>.
        /// </summary>
        /// <returns></returns>
        public TMatrix Duplicate()
        {
            // For performance:
            var rowCount = RowCount;
            var columnCount = ColumnCount;

            var result = new TMatrix();
            result.SetDimensions(rowCount, columnCount);

            for (var rowIndex = 0; rowIndex < rowCount; rowIndex++)
            {
                for (var columnIndex = 0; columnIndex < columnCount; columnIndex++)
                {
                    result.Matrix[rowIndex][columnIndex] = Matrix[rowIndex][columnIndex];
                }
                result.Vector[rowIndex] = Vector[rowIndex];
            }
            return result;
        }

        // ===========================================================================================
        // Description    : Initialize Matrix cells
        //
        // Date        ID   Modification
        // 2000.02.12  Sel  Created
        // 2000.03.21  Sel  UserAbort and ProgressEvent added
        // ===========================================================================================
        public void SetToNaught()
        {
            // For performance:
            var rowCount = RowCount;
            var columnCount = ColumnCount;

            for (var rowIndex = 0; rowIndex < rowCount; rowIndex++)
            {
                for (var columnIndex = 0; columnIndex < columnCount; columnIndex++)
                {
                    Matrix[rowIndex][columnIndex] = 0;
                }
                Vector[rowIndex] = 0;
            }
        }

        /// <summary>
        /// Calculated the matrix determinant.
        /// </summary>
        /// <exception cref="EDMatrixError">When this is not a square matrix or is singular.</exception>
        public double FetchDeterminant()
        {
            Decomposition(this);
            return determinant;
        }

        /// <summary>
        /// Creates a new matrix which doesn't contain completely empty rows or columns
        /// </summary>
        /// <param name="excluded"></param>
        /// <returns></returns>
        public TMatrix GetFilteredMatrix(out int[] excluded)
        {
            int count = Math.Min(RowCount, ColumnCount);
            var excludedRows = new List<int>();

            // Detect rows and columns which only have zeroes
            for (int i = 0; i < count; i++)
            {
                bool allZeroRow = true;
                bool allZeroColumn = true;

                for (int j = 0; j < count; j++)
                {
                    if (this[i, j] != 0)
                    {
                        allZeroRow = false;
                    }

                    if (this[j, i] != 0)
                    {
                        allZeroColumn = false;
                    }
                }

                if (allZeroRow || allZeroColumn)
                {
                    excludedRows.Add(i);
                }
            }

            // Nothing to exclude, return original matrix
            if (excludedRows.Count == 0)
            {
                excluded = new int[0];
                return this;
            }

            // Build up new matrix without zero elements
            var newMatrix = new TMatrix();
            int size = count - excludedRows.Count;
            newMatrix.SetDimensions(size, size);

            int skippedRows = 0;
            for (int i = 0; i < count; i++)
            {
                if (excludedRows.Contains(i))
                {
                    skippedRows++;
                }
                else
                {
                    newMatrix.Vector[i - skippedRows] = Vector[i];

                    int skippedColumns = 0;
                    for (int j = 0; j < count; j++)
                    {
                        if (excludedRows.Contains(j))
                        {
                            skippedColumns++;
                        }
                        else
                        {
                            newMatrix[i - skippedRows, j - skippedColumns] = this[i, j];
                        }
                    }
                }
            }

            excluded = excludedRows.ToArray();
            return newMatrix;
        }

        public bool IsRowAllZero(int rowIndex)
        {
            for (int i = 0; i < ColumnCount; i++)
            {
                if (this[rowIndex, i] != 0)
                {
                    return false;
                }
            }

            return true;
        }

        public bool IsColumnAllZero(int columnIndex)
        {
            for (int i = 0; i < RowCount; i++)
            {
                if (this[i, columnIndex] != 0)
                {
                    return false;
                }
            }

            return true;
        }

        /// <summary>
        /// Combines the current vector with a given vector
        /// </summary>
        /// <param name="newSolution"></param>
        /// <param name="excludedRows"></param>
        public void CombineVector(TMatrix matrix, int[] excludedRows)
        {
            int[] mapping = new int[Vector.Length];

            for (int i = 0; i < Vector.Length; i++)
            {
                int skipped = 0;
                for (int k = 0; k < excludedRows.Length; k++)
                {
                    if (excludedRows[k] == i)
                    {
                        mapping[i] = -1;
                    }
                    else if (excludedRows[k] < i)
                    {
                        skipped++;
                    }
                }

                if (mapping[i] != -1)
                {
                    mapping[i] = i - skipped;
                }
            }

            for (int i = 0; i < Vector.Length; i++)
            {
                if (mapping[i] != -1)
                {
                    Vector[i] = matrix.Vector[mapping[i]];
                    for (int j = 0; j < matrix.ColumnCount; j++)
                    {
                        if (mapping[j] != -1)
                        {
                            this[i, j] = matrix[mapping[i], mapping[j]];
                        }
                    }
                }
            }
        }

        private void BackSubstitution(double[] AVector)
        {
            double LSolverSum;
            var LCellNumber = -1;
            // When CellNumber is set to a non-negative value it will become the index of the first
            // nonvanishing element of Vector. We now do the forward substitution, equation
            // (2.3.6). The only new wrinkle is to unscramhle the permutation as we go.
            for (var LRowIndex = 0; LRowIndex < RowCount; LRowIndex++)
            {
                var LRowNumber = FIndex[LRowIndex];
                LSolverSum = AVector[LRowNumber];
                AVector[LRowNumber] = AVector[LRowIndex];
                if (LCellNumber != -1)
                {
                    for (var LCommonIndex = LCellNumber; LCommonIndex < LRowIndex; LCommonIndex++)
                    {
                        LSolverSum = LSolverSum - Matrix[LRowIndex][LCommonIndex] * AVector[LCommonIndex];
                    }
                }
                else
                {
                    // When a nonzero element is encountered the loop above is entered
                    if (LSolverSum != 0)
                    {
                        LCellNumber = LRowIndex;
                    }
                }
                AVector[LRowIndex] = LSolverSum;
            }
            // Now we do the backsubstitution, equation (2.3.7)
            for (var LRowIndex = RowCount - 1; LRowIndex >= 0; LRowIndex--)
            {
                LSolverSum = AVector[LRowIndex];
                for (var LCommonIndex = LRowIndex + 1; LCommonIndex < RowCount; LCommonIndex++)
                {
                    LSolverSum = LSolverSum - Matrix[LRowIndex][LCommonIndex] * AVector[LCommonIndex];
                }
                // Store a component of the solution vector
                AVector[LRowIndex] = LSolverSum / Matrix[LRowIndex][LRowIndex];
            }
        }
    }
}