// Created with MonoDevelop
//
//    Copyright (C) 2017 Hiperscan GmbH Dresden, Germany
//
//    This program is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with this program.  If not, see <http://www.gnu.org/licenses/>.

using MathNet.Numerics.LinearAlgebra.Double;

using static System.Math;


namespace Hiperscan.Spectroscopy.Math
{

    public partial class LevenbergMarquardt
    {

        public class QRFactorization
        {

            public QRFactorization(Matrix a)
            {
                this.QR = (Matrix)a.Clone();

                this.ColumnNorm  = new double[QR.ColumnCount];
                this.RDiagonal   = new double[QR.ColumnCount];
                this.Permutation = new int[QR.ColumnCount];

                for (int i = 0; i < QR.ColumnCount; i++)
                {
                    this.Permutation[i] = i;
                    this.ColumnNorm[i] = this.NormL2((Vector)QR.Column(i));
                }

                this.RDiagonal = (double[])this.ColumnNorm.Clone();

                this.QRFactorizationColumnPivoting();
            }

            private void QRFactorizationColumnPivoting()
            {
                for (int i = 0; i < Min(this.QR.RowCount, this.QR.ColumnCount); i++)
                {
                    int idx_max_norm = this.FindIndexOfMaxElement(this.RDiagonal, i);

                    if (idx_max_norm != i)
                    {
                        this.QR = this.ExchangeColumnVectors(this.QR, idx_max_norm, i);
                        this.Permutation = this.ExchangeArrayElements(this.Permutation, idx_max_norm, i);
                        this.RDiagonal[idx_max_norm] = this.RDiagonal[i];
                    }

                    double column_norm_subdiag = 0;

                    for (int j = i; j < this.QR.RowCount; j++)
                    {
                        column_norm_subdiag += this.QR[j, i] * this.QR[j, i];
                    }

                    column_norm_subdiag = Sqrt(column_norm_subdiag);

                    this.HouseholderTransformation(i, ref column_norm_subdiag);
                    this.UpdateNorm(i, column_norm_subdiag);
                    this.RDiagonal[i] = -column_norm_subdiag;
                }
            }

            private void UpdateNorm(int no, double column_norm_subdiag)
            {
                int i = no;
                if (QR.RowCount >= i + 1 && Abs(column_norm_subdiag) >= EPSILON)
                {
                    for (int k = i + 1; k < this.QR.ColumnCount; k++)
                    {
                        if (Abs(this.RDiagonal[k]) >= EPSILON)
                        {
                            double r_diagonal_k = this.RDiagonal[k];

                            double temp = this.QR[i, k] / this.RDiagonal[k];
                            this.RDiagonal[k] = this.RDiagonal[k] * Sqrt(Max(0, 1 - temp*temp));

                            if (0.05 * (this.RDiagonal[k] / r_diagonal_k) * (this.RDiagonal[k] / r_diagonal_k) <= MACHINE_EPSILON)
                            {
                                temp = 0.0;
                                for (int l = i + 1; l < this.QR.RowCount; l++)
                                {
                                    temp += this.QR[l, k] * this.QR[l, k];
                                }
                                this.RDiagonal[k] = Sqrt(temp);
                            }
                        }
                    }
                }
            }

            private void HouseholderTransformation(int householder_count, ref double column_norm_subdiag)
            {
                // householder vector
                if (Abs(column_norm_subdiag) >= EPSILON)
                {
                    if (this.QR[householder_count, householder_count] < 0)
                        column_norm_subdiag = -column_norm_subdiag;

                    for (int j = householder_count; j < this.QR.RowCount; j++)
                    {
                        this.QR[j, householder_count] = (1 / column_norm_subdiag) * this.QR[j, householder_count];
                    }

                    this.QR[householder_count, householder_count] += 1.0;

                    // remaining columns
                    for (int k = householder_count + 1; k < this.QR.ColumnCount; k++)
                    {
                        double sum = 0.0;
                        for (int i = householder_count; i < this.QR.RowCount; i++)
                        {
                            sum += this.QR[i, householder_count] * this.QR[i, k];
                        }
                        sum /= this.QR[householder_count, householder_count];

                        for (int i = householder_count; i < this.QR.RowCount; i++)
                        {
                            this.QR[i, k] = this.QR[i, k] - sum * this.QR[i, householder_count];
                        }
                    }
                }
            }

            public Vector QFvec(Vector fvec)
            {
                Vector qtf = DenseVector.Create(this.QR.ColumnCount, 0.0);
                Vector fvec_clone = (Vector)fvec.Clone();

                for (int i = 0; i < this.QR.ColumnCount; i++)
                {
                    if (Abs(this.QR[i, i]) >= EPSILON)
                    {
                        double sum = 0.0;
                        for (int j = i; j < this.QR.RowCount; j++)
                        {
                            sum += this.QR[j, i] * fvec_clone[j];
                        }

                        sum = -(sum / this.QR[i, i]);
                        for (int j = i; j < this.QR.RowCount; j++)
                        {
                            fvec_clone[j] += this.QR[j, i] * sum;
                        }
                        this.QR[i, i] = this.RDiagonal[i];  // obere Dreiecksmatrix zu R                                
                    }
                    qtf[i] = fvec_clone[i];
                }
                return qtf;
            }

            public double[] SolveRxQtf(double[] qtf, int rank)
            {
                double temp;

                if (rank > 0)
                {
                    for (int i = rank - 1; i >= 0; i--)
                    {
                        qtf[i] = qtf[i] / this.QR[i, i];
                        temp = qtf[i];

                        if (i - 1 < 0)
                            continue;

                        for (int k = 0; k <= i - 1; k++)
                        {
                            qtf[k] -= this.QR[k, i] * temp;
                        }
                    }
                }
                return qtf;
            }

            public double[] MultiplyUpperTriangularMatrixRToVector(double[] vec)
            {
                double[] vec_new = new double[vec.GetLength(0)];
                for (int i = 0; i < this.QR.ColumnCount; i++)
                {
                    vec_new[i] = 0;
                    for (int j = 0; j <= i; j++)
                    {
                        vec_new[j] += this.QR[j, i] * vec[this.Permutation[i]];
                    }
                }
                return vec_new;
            }

            public double NormL2(Vector vec)
            {
                double norm = 0.0;
                for (int i = 0; i < vec.Count; i++)
                {
                    norm += vec[i] * vec[i];
                }
                return Sqrt(norm);
            }

            public double[] UnravelPermutation(double[] vec)
            {
                double[] vec_new = new double[vec.Length];
                for (int i = 0; i < vec.Length; i++)
                {
                    vec_new[this.Permutation[i]] = vec[i];
                }
                return vec_new;
            }

            private int FindIndexOfMaxElement(double[] a, int idx_start)
            {
                int is_max = idx_start;

                if (idx_start <= a.GetLength(0))
                {
                    for (int i = idx_start; i < a.GetLength(0); i++)
                    {
                        if (a[i] > a[is_max])
                            is_max = i;
                    }
                }

                return is_max;
            }

            private Matrix ExchangeColumnVectors(Matrix jacobian, int coli, int colj)
            {
                Vector temp = (Vector)jacobian.Column(coli);
                jacobian.SetColumn(coli, jacobian.Column(colj));
                jacobian.SetColumn(colj, temp);

                return jacobian;
            }

            private int[] ExchangeArrayElements(int[] a, int elemi, int elemj)
            {
                int temp = a[elemi];
                a[elemi] = a[elemj];
                a[elemj] = temp;

                return a;
            }

            public Matrix QR           { get; internal set; }
            public int[] Permutation   { get; private set; }
            public double[] ColumnNorm { get; }
            public double[] RDiagonal  { get; set; }
        }
    }
}

