// 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 System;
using MathNet.Numerics.LinearAlgebra.Double;

using Hiperscan.Unix;


namespace Hiperscan.Spectroscopy
{
    public abstract class LMFit
    {
        private DataSet fit_data;
        private DataSet ref_data;
        private double[] init_coeff;
        private double direction;
        private double xscale, yscale, xshift, yshift;

        protected LMFit(DataSet fit_data, double direction)
        {
            this.fit_data = fit_data;
            this.ref_data = null;
            this.direction = direction;
        }

        protected LMFit(DataSet fit_data, DataSet ref_data, double direction)
        {
            this.fit_data = fit_data;
            this.ref_data = ref_data;
            this.direction = direction;
        }

        public abstract void NormalizeFitData();

        public abstract double[] UnNormalizeCoefficients(double[] coeff);

        public abstract Vector FunctionVector(double[] coeff);

        public virtual Vector ObjectiveFunc(double[] coeff)
        {
            Vector fvec = this.FunctionVector(coeff);
            Vector fitvec;

            if (ref_data != null)
            {
                fitvec = new DenseVector(this.ref_data.Y.ToArray());
            }
            else
            {
                fitvec = new DenseVector(this.fit_data.Y.ToArray());
            }

            return (Vector)(fvec - fitvec);
        }

        public double[] InitCoefficients
        {
            set
            {
                this.init_coeff = value;
            }
            get
            {
                return this.init_coeff;
            }
        }

        public DataSet FitData
        {
            get
            {
                return this.fit_data;
            }
            set
            {
                this.fit_data = value;
            }
        }

        public DataSet RefData
        {
            get
            {
                return this.ref_data;
            }
            set
            {
                this.ref_data = value;
            }
        }
        public double Direction
        {
            get
            {
                return this.direction;
            }
            set
            {
                this.direction = value;
            }
        }

        public double XShift
        {
            get
            {
                return this.xshift;
            }
            set
            {
                this.xshift = value;
            }
        }

        public double YShift
        {
            get
            {
                return this.yshift;
            }
            set
            {
                this.yshift = value;
            }
        }

        public double XScale
        {
            get
            {
                return this.xscale;
            }
            set
            {
                this.xscale = value;
            }
        }

        public double YScale
        {
            get
            {
                return this.yscale;
            }
            set
            {
                this.yscale = value;
            }
        }
    }

    public class ParabolicLMFit : LMFit
    {
        public ParabolicLMFit(DataSet fit_data, double direction) : base(fit_data, direction)
        {
            this.InitCoefficients = new double[3] { 0.0, 1.0, 0.0 };
        }

        public override Vector FunctionVector(double[] coeff)
        {
            Vector fvec = new DenseVector(this.FitData.X.Count);
            for (int ix = 0; ix < this.FitData.X.Count; ++ix)
            {
                fvec[ix] = coeff[0] + coeff[1] * Math.Pow((this.FitData.X[ix] - coeff[2]), 2f);
            }
            return fvec;
        }

        public override void NormalizeFitData()
        {
            this.FitData *= this.Direction;
            this.XShift   = this.FitData.XMedian();
            this.YShift   = this.FitData.YMin();
            this.FitData -= new DataSet.Operand(this.XShift, this.YShift);
            this.XScale   = 1.0;
            this.YScale   = 1.0 / this.FitData.YMax();
            this.FitData *= new DataSet.Operand(this.XScale, this.YScale);
        }

        public override double[] UnNormalizeCoefficients(double[] coeff)
        {
            coeff[0] = (coeff[0] / this.YScale + this.YShift) / this.Direction;
            coeff[1] = (coeff[1] / this.YScale) / this.Direction;
            coeff[2] = coeff[2] / this.XScale + this.XShift;

            return coeff;
        }
    }

    public class QuadraticLMFit : LMFit
    {
        public QuadraticLMFit(DataSet fit_data, double direction) : base(fit_data, direction)
        {
            this.InitCoefficients = new double[3] { 0.0, 0.0, 0.0 };
        }

        public override Vector FunctionVector(double[] coeff)
        {
            Vector fvec = new DenseVector(this.FitData.X.Count);
            for (int ix = 0; ix < this.FitData.X.Count; ++ix)
            {
                fvec[ix] = coeff[0] + coeff[1] * this.FitData.X[ix] + coeff[2] * Math.Pow(this.FitData.X[ix], 2.0);
            }
            return fvec;
        }

        public override void NormalizeFitData()
        {
            this.FitData *= this.Direction;
        }

        public override double[] UnNormalizeCoefficients(double[] coeff)
        {
            coeff[0] = coeff[0] / this.Direction;
            coeff[1] = coeff[1] / this.Direction;
            coeff[2] = coeff[2] / this.Direction;

            return coeff;
        }
    }

    public class MatchLinearLMFit : LMFit
    {
        public MatchLinearLMFit(DataSet fit_data, DataSet ref_data, double direction) : base(fit_data, ref_data, direction)
        {
            if (fit_data.Count != ref_data.Count)
                throw new ArgumentException(Catalog.GetString("Dimension of fit data and reference data must not be different."));

            this.InitCoefficients = new double[2] { 0.0, 1.0 };
        }

        public override Vector FunctionVector(double[] coeff)
        {
            Vector fvec = new DenseVector(this.FitData.X.Count);
            for (int ix = 0; ix < this.FitData.X.Count; ++ix)
            {
                fvec[ix] = coeff[0] + coeff[1] * this.FitData.Y[ix];
            }
            return fvec;
        }

        public override void NormalizeFitData()
        {
            this.FitData *= this.Direction;
        }

        public override double[] UnNormalizeCoefficients(double[] coeff)
        {
            return coeff;
        }
    }

    public class Match2DQuadraticLinearLMFit : LMFit
    {
        public Match2DQuadraticLinearLMFit(DataSet fit_data, DataSet ref_data, double direction) : base(fit_data, ref_data, direction)
        {
            if (fit_data.Count != ref_data.Count)
                throw new ArgumentException(Catalog.GetString("Dimension of fit data and reference data must not be different."));

            this.InitCoefficients = new double[5] { 0.0, 0.0, 0.0, 0.0, 1.0 };
        }

        public override Vector FunctionVector(double[] coeff)
        {
            double[] p = new double[3];
            Array.Copy(coeff, 0, p, 0, 3);
            WavelengthCorrection wlc = new WavelengthCorrection(p);
            DataSet fit = wlc.CorrectedSpectrum(this.FitData).Interpolate(this.RefData.X);

            Vector fvec = new DenseVector(this.FitData.X.Count);
            for (int ix = 0; ix < this.FitData.X.Count; ++ix)
            {
                fvec[ix] = coeff[3] + coeff[4] * fit.Y[ix];
            }
            return fvec;
        }

        public override void NormalizeFitData()
        {
            this.FitData *= this.Direction;
        }

        public override double[] UnNormalizeCoefficients(double[] coeff)
        {
            return coeff;
        }
    }

    public class Match2DQuadraticLinearSlopeLMFit : LMFit
    {
        public Match2DQuadraticLinearSlopeLMFit(DataSet fit_data, DataSet ref_data, double direction) : base(fit_data, ref_data, direction)
        {
            if (fit_data.Count != ref_data.Count)
                throw new ArgumentException(Catalog.GetString("Dimension of fit data and reference data must not be different."));

            this.InitCoefficients = new double[6] { 0.0, 0.0, 0.0, 0.0, 1.0, 0.0 };
        }

        public override Vector FunctionVector(double[] coeff)
        {
            double[] p = new double[3];
            Array.Copy(coeff, 0, p, 0, 3);
            WavelengthCorrection wlc = new WavelengthCorrection(p);
            DataSet fit = wlc.CorrectedSpectrum(this.FitData).Interpolate(this.RefData.X);

            Vector fvec = new DenseVector(this.FitData.X.Count);
            for (int ix = 0; ix < fit.Count; ++ix)
            {
                fvec[ix] = coeff[3] + coeff[4] * fit.Y[ix] + coeff[5] * fit.X[ix];
            }
            return fvec;
        }

        public override void NormalizeFitData()
        {
            this.FitData *= this.Direction;
        }

        public override double[] UnNormalizeCoefficients(double[] coeff)
        {
            return coeff;
        }
    }

    public class Match2DQuadraticQuadraticSlopeLMFit : LMFit
    {
        public Match2DQuadraticQuadraticSlopeLMFit(DataSet fit_data, DataSet ref_data, double direction) : base(fit_data, ref_data, direction)
        {
            if (fit_data.Count != ref_data.Count)
                throw new ArgumentException(Catalog.GetString("Dimension of fit data and reference data must not be different."));

            this.InitCoefficients = new double[7] { 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 };
        }

        public override Vector FunctionVector(double[] coeff)
        {
            double[] p = new double[3];
            Array.Copy(coeff, 0, p, 0, 3);
            WavelengthCorrection wlc = new WavelengthCorrection(p);
            DataSet fit = wlc.CorrectedSpectrum(this.FitData).Interpolate(this.RefData.X);

            Vector fvec = new DenseVector(this.FitData.X.Count);
            for (int ix = 0; ix < fit.Count; ++ix)
            {
                fvec[ix] = coeff[3] + coeff[4] * fit.Y[ix] + coeff[5] * fit.Y[ix] * fit.Y[ix] + coeff[6] * fit.X[ix];
            }
            return fvec;
        }

        public override void NormalizeFitData()
        {
            this.FitData *= this.Direction;
        }

        public override double[] UnNormalizeCoefficients(double[] coeff)
        {
            return coeff;
        }
    }

    public class MSCLMFit : LMFit
    {
        public MSCLMFit(DataSet fit_data, DataSet ref_data, double direction) : base(fit_data, ref_data, direction)
        {
            this.InitCoefficients = new double[2] { 0.0, 1.0 };
            // exchange fit_data and ref_data in order to fit mean spectrum of group to individual spectrum
            this.FitData = ref_data.Interpolate(fit_data.X);
            this.RefData = fit_data;
        }

        public override Vector FunctionVector(double[] coeff)
        {
            Vector fvec = new DenseVector(this.FitData.X.Count);
            for (int ix = 0; ix < this.FitData.X.Count; ++ix)
            {
                fvec[ix] = coeff[0] + coeff[1] * this.FitData.Y[ix];
            }
            return fvec;
        }

        public override void NormalizeFitData()
        {
            this.FitData *= this.Direction;
        }

        public override double[] UnNormalizeCoefficients(double[] coeff)
        {
            return coeff;
        }
    }

    public class ProbeErrorCharacteristicLMFit : LMFit
    {
        public ProbeErrorCharacteristicLMFit(DataSet fit_data, DataSet ref_data, double direction) : base(fit_data, ref_data, direction)
        {
            this.InitCoefficients = new double[3] { 0.0, 0.0, 0.0 };
            this.FitData = this.FitData;
            this.RefData = this.RefData.Interpolate(this.FitData.X);
        }

        public override Vector FunctionVector(double[] coeff)
        {
            Vector ref_data_vec = new DenseVector(this.RefData.Y.ToArray());
            Vector fvec = new DenseVector(this.RefData.Count);
            for (int ix = 0; ix < this.RefData.Count; ++ix)
            {
                fvec[ix] = this.RefData.X[ix] * coeff[1] + coeff[2];
            }
            fvec = (Vector)(ref_data_vec * coeff[0]).Add(fvec);

            return fvec;
        }

        public override Vector ObjectiveFunc(double[] coeff)
        {
            Vector fvec   = this.FunctionVector(coeff);
            Vector fitvec = new DenseVector(this.FitData.Y.ToArray());

            return (Vector)(fitvec - fvec);
        }

        public override void NormalizeFitData()
        {
            this.FitData *= this.Direction;
        }

        public override double[] UnNormalizeCoefficients(double[] coeff)
        {
            return coeff;
        }
    }

    public class LevenbergMarquardt
    {
        public enum TerminationType
        {
            Empty,
            GradientTolerance,
            FunctionTolerance,
            ArgumentTolerance,
            FunctionArgumentTolerance,
            MaxIterations,
            ChangeInFunctionSmallerMachinePrecision,
            ChangeInArgumentSmallerMachinePrecision,
            ChangeInGradientSmallerMachinePrecision
        }

        public class MaxIterationNumberException : Exception
        {
            public MaxIterationNumberException()
            {
            }

            public MaxIterationNumberException(string msg) : base(msg)
            {
            }

            public MaxIterationNumberException(string msg, Exception inner) : base(msg, inner)
            {
            }
        }

        public class OptimParams
        {
            int max_iterations;
            bool warn_on_iteration_exceeded;
            double tol_gradient;
            double tol_function;
            double tol_argument;

            public OptimParams()
            {
                this.max_iterations = 400;
                this.warn_on_iteration_exceeded = false;
                this.tol_gradient = 1e-10;
                this.tol_function = 1e-10;
                this.tol_argument = 1e-10;
            }

            public OptimParams(int max_iterations, double tol_gradient, double tol_function, double tol_argument)
            {
                this.max_iterations = max_iterations;
                this.tol_gradient = tol_gradient;
                this.tol_function = tol_function;
                this.tol_argument = tol_argument;

                if (this.max_iterations <= 0 || this.tol_argument <= 0 || this.tol_function <= 0 || this.tol_gradient <= 0)
                    throw new ArgumentException(Catalog.GetString("Optimization parameters have to be greater than zero."));
            }

            public int MaxIterations
            {
                get
                {
                    return this.max_iterations;
                }

                set
                {
                    this.max_iterations = value;
                }
            }

            public bool WarnOnIterationExceeded
            {
                get
                {
                    return this.warn_on_iteration_exceeded;
                }

                set
                {
                    this.warn_on_iteration_exceeded = value;
                }
            }

            public double ToleranceGradient
            {
                get
                {
                    return this.tol_gradient;
                }

                set
                {
                    this.tol_gradient = value;
                }
            }

            public double ToleranceFunction
            {
                get
                {
                    return this.tol_function;
                }

                set
                {
                    this.tol_function = value;
                }

            }

            public double ToleranceArgument
            {
                get
                {
                    return this.tol_argument;
                }

                set
                {
                    this.tol_argument = value;
                }
            }
        }

        public class OptimResults
        {
            private double[] coeff;
            private int iterations = 0;
            private double gradient_norm = 0;
            private double function_norm = 0;
            private double argument_norm = 0;
            private double step_bound_scaled_argument_norm = 0;
            private double latest_function_reduction = 0;
            private double latest_predicted_function_reduction = 0;
            private bool max_iterations_exceeded = false;
            private double[] residual_vector;


            public TerminationType TerminationType { get; internal set; }

            public OptimResults(double[] initCoeff, TerminationType termination_type)
            {
                this.coeff = initCoeff;
                this.TerminationType = termination_type;
            }

            public double[] Coefficients
            {
                get
                {
                    return this.coeff;
                }

                internal set
                {
                    this.coeff = value;
                }

            }

            public int Iterations
            {
                get
                {
                    return this.iterations;
                }

                internal set
                {
                    this.iterations = value;
                }
            }

            public bool MaxIterationsExceeded
            {
                get
                {
                    return this.max_iterations_exceeded;
                }

                internal set
                {
                    this.max_iterations_exceeded = value;
                }

            }

            public double GradientNorm
            {
                get
                {
                    return this.gradient_norm;
                }

                internal set
                {
                    this.gradient_norm = value;
                }
            }

            public double FunctionNorm
            {
                get
                {
                    return this.function_norm;
                }

                internal set
                {
                    this.function_norm = value;
                }
            }

            public double ArgumentNorm
            {
                get
                {
                    return this.argument_norm;
                }

                internal set
                {
                    this.argument_norm = value;
                }
            }

            public double StepBoundToScaledArgumentNorm
            {
                get
                {
                    return this.step_bound_scaled_argument_norm;
                }

                internal set
                {
                    this.step_bound_scaled_argument_norm = value;
                }
            }

            public double LatestFunctionReduction
            {
                get
                {
                    return this.latest_function_reduction;
                }

                internal set
                {
                    this.latest_function_reduction = value;
                }
            }

            public double LatestPredictedFunctionReduction
            {
                get
                {
                    return this.latest_predicted_function_reduction;
                }

                internal set
                {
                    this.latest_predicted_function_reduction = value;
                }
            }

            public double[] ResidualVector
            {
                get
                {
                    return this.residual_vector;
                }

                internal set
                {
                    this.residual_vector = value;
                }
            }
        }

        public class JacobianMatrix : MathNet.Numerics.LinearAlgebra.Double.DenseMatrix
        {
            private DataSet ds;
            private double[] x;
            private double step = 1e-8;

            public delegate Vector FunctionVectorDelegateHandler(double[] coeff);

            FunctionVectorDelegateHandler function_vector_delegate = null;

            public JacobianMatrix(DataSet ds, double[] x, FunctionVectorDelegateHandler function_vector_delegate) : base(ds.Count, x.Length)
            {
                this.ds = ds;
                this.x = x;
                this.function_vector_delegate = function_vector_delegate;
                this.ComputeMatrix();
            }

            private void ComputeMatrix()
            {
                double[] derivatives;
                for (int ix = 0; ix < this.x.Length; ++ix)
                {
                    derivatives = this.PartialDerivatives(ix);
                    for (int jx = 0; jx < this.ds.Count; ++jx)
                    {
                        this[jx, ix] = derivatives[jx];
                    }
                }
            }

            private double[] PartialDerivatives(int ix)
            {
                double[] x1 = new double[this.x.Length];
                double[] x2 = new double[this.x.Length];

                for (int jx = 0; jx < this.x.Length; ++jx)
                {
                    x1[jx] = x2[jx] = this.x[jx];
                }

                x1[ix] -= step / 2;
                x2[ix] += step / 2;

                double[] dy1 = this.function_vector_delegate(x1).ToArray();
                double[] dy2 = this.function_vector_delegate(x2).ToArray();

                double[] derivatives = new double[this.ds.Count];
                for (int jx = 0; jx < this.ds.Count; ++jx)
                {
                    derivatives[jx] = (dy2[jx] - dy1[jx]) / step;
                }

                return derivatives;
            }

            public double Step
            {

                get
                {
                    return this.step;
                }

                set
                {
                    this.step = value;
                }
            }
        }

        public class QRFactorization
        {
            private Matrix qr;
            private int[] permutation;
            private double[] column_norm;
            public double[] r_diagonal;

            public QRFactorization(Matrix a)
            {
                this.qr = (Matrix)a.Clone();
                this.column_norm = new double[qr.ColumnCount];
                this.r_diagonal = new double[qr.ColumnCount];
                this.permutation = new int[qr.ColumnCount];

                for (int i = 0; i < qr.ColumnCount; i++)
                {
                    this.permutation[i] = i;
                    this.column_norm[i] = this.NormL2((Vector)qr.Column(i));
                }
                this.r_diagonal = (Double[])this.column_norm.Clone();

                this.QRFactorizationColumnPivoting();
            }

            private void QRFactorizationColumnPivoting()
            {
                for (int i = 0; i < Math.Min(this.qr.RowCount, this.qr.ColumnCount); i++)
                {
                    int idx_max_norm = this.FindIndexOfMaxElement(this.r_diagonal, 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.r_diagonal[idx_max_norm] = this.r_diagonal[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 = Math.Sqrt(column_norm_subdiag);

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

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

                            double temp = this.qr[i, k] / this.r_diagonal[k];
                            this.r_diagonal[k] = this.r_diagonal[k] * Math.Sqrt(Math.Max(0, 1 - AP.Math.Sqr(temp)));

                            if (0.05 * AP.Math.Sqr(this.r_diagonal[k] / r_diagonal_k) <= AP.Math.MachineEpsilon)
                            {
                                temp = 0.0;
                                for (int l = i + 1; l < this.qr.RowCount; l++)
                                {
                                    temp += this.qr[l, k] * this.qr[l, k];
                                }
                                this.r_diagonal[k] = Math.Sqrt(temp);
                            }
                        }
                    }
                }
            }

            private void HouseholderTransformation(int householder_count, ref double column_norm_subdiag)
            {
                // householder vector
                if (Math.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] /= column_norm_subdiag; 
                        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] += -sum * this.qr[i, householder_count];
                            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 (Math.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.r_diagonal[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)
                        {
                            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 Math.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
                {
                    return this.qr;
                }

                internal set
                {
                    this.qr = value;
                }
            }

            public int[] Permutation
            {
                get
                {
                    return this.permutation;
                }
            }

            public double[] ColumnNorm
            {
                get
                {
                    return this.column_norm;
                }
            }
        }

        private LMFit lm_fit;
        private OptimParams optim_params;
        private OptimResults optim_results;
        private Vector variable_scaling;
        private QRFactorization qr_factorization;
        private double[] coeff;
        private double step_bound;
        private double lm_param;
        private int iter_counter;
        private double function_norm;
        private double gradient_norm;
        private double argument_norm;
        private double scaled_coeff_norm;
        private const double desired_relative_err_argument_norm = 0.1;
        private const double P0001 = 1.0E-4;
        private const double P1 = 1.0E-1;
        private const double P25 = 2.5E-1;
        private const double P5 = 5.0E-1;
        private const double P75 = 7.5E-1;
        private const double EPSILON = 1e-50;

        private LevenbergMarquardt(LMFit lm_fit, OptimParams optim_params)
        {
            this.lm_fit = lm_fit;
            this.optim_params = optim_params;
            this.Initialize();
        }

        private void Initialize()
        {
            this.lm_fit.NormalizeFitData();

            this.optim_results = new OptimResults(this.lm_fit.InitCoefficients, LevenbergMarquardt.TerminationType.Empty);

            this.coeff = (Double[])this.lm_fit.InitCoefficients.Clone();

            JacobianMatrix jacobian = new JacobianMatrix(this.lm_fit.FitData, this.lm_fit.InitCoefficients, this.lm_fit.ObjectiveFunc);
            this.variable_scaling = new DenseVector(this.coeff.GetLength(0));
            for (int i = 0; i < this.variable_scaling.Count; i++)
            {
                this.variable_scaling[i] = this.NormL2(jacobian.Column(i).ToArray());
                if (Math.Abs(this.variable_scaling[i]) < EPSILON)
                    this.variable_scaling[i] = 1.0;
            }

            this.function_norm = this.NormL2(this.lm_fit.ObjectiveFunc(this.coeff).ToArray());
            this.gradient_norm = 0.0;
            this.argument_norm = 0.0;

            this.scaled_coeff_norm = NormOfScaledCoefficient(this.variable_scaling.ToArray(), this.coeff);

            this.step_bound = 100.0 * this.scaled_coeff_norm;
            if (Math.Abs(this.step_bound) < EPSILON)
                this.step_bound = 100.0;

            this.lm_param = 0.0;
            this.iter_counter = 1;
        }

        public static OptimResults Fit(LMFit lm_fit)
        {
            return LevenbergMarquardt.Fit(lm_fit, new OptimParams());
        }

        public static OptimResults Fit(LMFit lm_fit, OptimParams optim_params)
        {
            if (lm_fit.InitCoefficients.GetLength(0) <= 0 || lm_fit.InitCoefficients == null)
            {
                throw new ArgumentException(Catalog.GetString("Initial coefficients are invalid."));
            }
            if (lm_fit.FitData.Count < lm_fit.InitCoefficients.GetLength(0))
            {
                throw new ArgumentException(Catalog.GetString("Length of fitting data has to be longer or equal to number of coefficients."));
            }
            if (optim_params.ToleranceFunction < 0 || optim_params.ToleranceArgument < 0 || optim_params.ToleranceGradient < 0 || optim_params.MaxIterations < 0)
            {
                throw new ArgumentException(Catalog.GetString("Optimization parameters have to be positive values."));
            }

            return LevenbergMarquardt.Fit(new LevenbergMarquardt(lm_fit, optim_params));
        }

        private static OptimResults Fit(LevenbergMarquardt lm)
        {
            lm.optim_results = lm.LevenbergMarquardtMinimize();

            if (lm.optim_results.MaxIterationsExceeded)
            {
                string msg = string.Format(Catalog.GetString("The maximum number of iterations ({0}) has been reached."), lm.optim_params.MaxIterations);
                if (lm.optim_params.WarnOnIterationExceeded)
                    Console.WriteLine("Warning: " + msg);
                else
                    throw new MaxIterationNumberException(msg);
            }

            lm.optim_results.Coefficients = lm.lm_fit.UnNormalizeCoefficients(lm.optim_results.Coefficients);

            return lm.optim_results;
        }

        private OptimResults LevenbergMarquardtMinimize()
        {
            // outer loop
            while (true)
            {
                //				Console.WriteLine("iter outerloop {0}", this.iter_counter);
                //
                //				Console.WriteLine("coeff");
                //				for (int i=0; i<coeff.Length;i++)
                //					Console.WriteLine(" {0}", coeff[i]);
                //				Console.WriteLine();

                Vector fvec_objectivefunc = this.lm_fit.ObjectiveFunc(this.coeff);
                JacobianMatrix jacobian = new JacobianMatrix(this.lm_fit.FitData, this.coeff, this.lm_fit.ObjectiveFunc);

                //				Console.WriteLine("fvec_objectivefunc");
                //				for (int i=0; i<fvec_objectivefunc.Length;i++)
                //					Console.WriteLine(" {0}", fvec_objectivefunc[i]);
                //				Console.WriteLine();
                //
                //				Console.WriteLine("Jacobian");
                //				for (int i=0; i<jacobian.RowCount;i++)
                //				{
                //					for (int j=0; j<jacobian.ColumnCount;j++)
                //					{
                //						Console.Write(" {0}", jacobian[i,j]);
                //					}
                //					Console.WriteLine();
                //				}
                //				Console.WriteLine();

                this.qr_factorization = new QRFactorization(jacobian);
                Vector qtf = this.qr_factorization.QFvec(fvec_objectivefunc);



                //				Console.WriteLine("qtf");
                //				for (int i=0; i<qtf.Length;i++)
                //					Console.WriteLine(" {0}", qtf[i]);
                //				Console.WriteLine();

                this.gradient_norm = this.NormOfScaledGradient(qtf);

                //				Console.WriteLine("fvec_objectivefunc");
                //				for (int i=0; i<fvec_objectivefunc.Length;i++)
                //					Console.WriteLine(" {0}", fvec_objectivefunc[i]);
                //				Console.WriteLine();



                if (this.gradient_norm <= this.optim_params.ToleranceGradient)
                {
                    this.optim_results.TerminationType = LevenbergMarquardt.TerminationType.GradientTolerance;
                    this.optim_results.GradientNorm = this.gradient_norm;
                }

                if (this.optim_results.TerminationType != LevenbergMarquardt.TerminationType.Empty)
                {
                    return this.optim_results;
                }

                for (int i = 0; i < this.qr_factorization.QR.ColumnCount; i++)
                {
                    this.variable_scaling[i] = Math.Max(this.variable_scaling[i], this.NormL2(jacobian.Column(i).ToArray()));
                }

                // inner loop
                int iter_counter_innerloop = 0;
                double ratio_reduction = 0;
                double[] coeff_new = new double[this.coeff.Length];

                while (ratio_reduction < P0001)
                {
                    iter_counter_innerloop += 1;

                    //					Console.WriteLine();
                    //					Console.WriteLine("iter innerloop {0}", iter_counter_innerloop);
                    //					Console.WriteLine();

                    double[] delta_coeff = this.LevenbergMarquardtParameter(qtf);

                    for (int i = 0; i < this.qr_factorization.QR.ColumnCount; i++)
                    {
                        delta_coeff[i] = -delta_coeff[i];
                        coeff_new[i] = this.coeff[i] + delta_coeff[i];
                    }
                    //
                    //					Console.WriteLine("delta_coeff");
                    //					for(int i = 0; i < this.qr_factorization.QR.ColumnCount; i++)
                    //					{
                    //						Console.Write("{0} ", delta_coeff[i]);
                    //					}
                    //					Console.WriteLine();
                    //
                    //
                    //					Console.WriteLine("coeff_new");
                    //					for(int i = 0; i < this.qr_factorization.QR.ColumnCount; i++)
                    //					{
                    //						Console.Write("{0} ", coeff_new[i]);
                    //					}
                    //					Console.WriteLine();


                    double scaled_delta_coeff_norm = this.NormOfScaledArgument(this.variable_scaling.ToArray(), delta_coeff);
                    //
                    //					Console.WriteLine("scaled_delta_coeff_norm");
                    //					Console.WriteLine(scaled_delta_coeff_norm);

                    if (this.iter_counter == 1)
                    {
                        this.step_bound = Math.Min(this.step_bound, scaled_delta_coeff_norm);
                    }

                    //					Console.WriteLine("step_bound");
                    //					Console.WriteLine(this.step_bound);

                    Vector fvec_objectivefunc_new = this.lm_fit.ObjectiveFunc(coeff_new);


                    double function_norm_new = this.NormL2(fvec_objectivefunc_new.ToArray());

                    //					Console.WriteLine("function_norm_new");
                    //					Console.WriteLine(function_norm_new);

                    double actual_reduction = this.ActualReduction(function_norm_new);

                    //					Console.WriteLine("actual_reduction");
                    //					Console.WriteLine(actual_reduction);

                    double[] delta_coeff_R = this.qr_factorization.MultiplyUpperTriangularMatrixRToVector(delta_coeff);

                    double predicted_reduction = this.PredictedReduction(delta_coeff_R, scaled_delta_coeff_norm);
                    double directional_derivative = this.DirectionalDerivative(delta_coeff_R, scaled_delta_coeff_norm);

                    //					Console.WriteLine("predicted_reduction");
                    //					Console.WriteLine(predicted_reduction);
                    //					Console.WriteLine("directional_derivative");
                    //					Console.WriteLine(directional_derivative);

                    ratio_reduction = this.RatioOfReduction(actual_reduction, predicted_reduction);

                    //					Console.WriteLine("ratio_reduction");
                    //					Console.WriteLine(ratio_reduction);
                    //
                    //					Console.WriteLine("fvec_objectivefunc_new");
                    //					for (int i=0; i<fvec_objectivefunc.Length;i++)
                    //						Console.WriteLine(" {0}", fvec_objectivefunc_new[i]);
                    //					Console.WriteLine();


                    this.UpdateStepBoundLMParameter(ratio_reduction, actual_reduction, directional_derivative, function_norm_new, delta_coeff_R, scaled_delta_coeff_norm);

                    if (ratio_reduction >= P0001)
                    {
                        this.coeff = coeff_new;
                        this.optim_results.Coefficients = (Double[])this.coeff.Clone();
                        fvec_objectivefunc = (Vector)fvec_objectivefunc_new.Clone();
                        this.scaled_coeff_norm = this.NormOfScaledCoefficient(this.variable_scaling.ToArray(), this.coeff);
                        this.function_norm = function_norm_new;
                        this.iter_counter += 1;
                    }

                    this.optim_results.TerminationType = this.ConvergenceCriterion(actual_reduction, predicted_reduction, ratio_reduction, P5);

                    if (this.optim_results.TerminationType != TerminationType.Empty)
                    {
                        return this.SetOptimResults(scaled_delta_coeff_norm, actual_reduction, predicted_reduction, fvec_objectivefunc);
                    }

                    this.optim_results.TerminationType = this.GetTerminationCriterion(actual_reduction, predicted_reduction, ratio_reduction, P5);

                    if (this.optim_results.TerminationType != TerminationType.Empty)
                    {
                        return this.SetOptimResults(scaled_delta_coeff_norm, actual_reduction, predicted_reduction, fvec_objectivefunc);
                    }
                }
            }
        }

        private double[] LevenbergMarquardtParameter(Vector qtf)
        {
            Vector qtf_clone = (Vector)qtf.Clone();
            double[] delta_coeff = new double[qtf.Count];
            double lower_bound;
            double upper_bound;
            double[] diag_variable_scaling = new double[this.qr_factorization.QR.ColumnCount];

            //			Console.WriteLine("in lm param");

            //int rank = this.qr_factorization.QR.Rank();			
            int rank = this.RankOfMatrix(qr_factorization.QR);
            //
            //			Console.WriteLine("Rang {0}", rank);
            //
            //			Console.WriteLine("QR");
            //			for (int i=0; i<qr_factorization.QR.RowCount;i++)
            //			{
            //				for (int j=0; j<qr_factorization.QR.ColumnCount;j++)
            //				{
            //					Console.Write(" {0}", qr_factorization.QR[i,j]);
            //				}
            //				Console.WriteLine();
            //			}
            //			Console.WriteLine();

            for (int i = rank; i < qtf_clone.Count; i++)
            {
                qtf_clone[i] = 0;
            }
            delta_coeff = this.qr_factorization.UnravelPermutation(this.qr_factorization.SolveRxQtf(qtf_clone.ToArray(), rank));

            //			Console.WriteLine("delta_coeff");
            //			for (int i=0; i<delta_coeff.Length;i++)
            //				Console.WriteLine(" {0}", delta_coeff[i]);
            //			Console.WriteLine();

            int iter_counter_lmparameter = 0;
            this.argument_norm = NormOfScaledArgument(this.variable_scaling.ToArray(), delta_coeff);
            //
            //			Console.WriteLine("argument_norm");
            //			Console.WriteLine(this.argument_norm);

            double distance_step_bound = this.argument_norm - this.step_bound;

            //			Console.WriteLine("distance_step_bound");
            //			Console.WriteLine(distance_step_bound);
            //			Console.WriteLine("desired_relative_err_argument_norm");
            //			Console.WriteLine(desired_relative_err_argument_norm);
            //			Console.WriteLine("step_bound");
            //			Console.WriteLine(this.step_bound);

            if (distance_step_bound <= desired_relative_err_argument_norm * this.step_bound)
            {
                if (iter_counter_lmparameter == 0)
                {
                    this.lm_param = 0;
                }
                return delta_coeff;
            }

            lower_bound = LowerBoundOfLMParameter(rank, delta_coeff);
            upper_bound = UpperBoundOfLMParameter(qtf);

            this.lm_param = Math.Max(this.lm_param, lower_bound);
            this.lm_param = Math.Min(this.lm_param, upper_bound);

            if (Math.Abs(this.lm_param) < EPSILON)
            {
                this.lm_param = this.gradient_norm / this.argument_norm;
            }

            while (true)
            {
                iter_counter_lmparameter++;

                //				Console.WriteLine("iter_counter_lmparameter {0}", iter_counter_lmparameter);

                if (Math.Abs(this.lm_param) < EPSILON)
                {
                    this.lm_param = Math.Max(Double.Epsilon, 0.001 * upper_bound);
                }

                delta_coeff = LevenbergMarquardtQRSolve(qtf, ref diag_variable_scaling);

                //				Console.WriteLine("delta_coeff");
                //				for (int i=0; i<delta_coeff.Length;i++)
                //					Console.WriteLine(" {0}", delta_coeff[i]);
                //				Console.WriteLine();

                double[] scaled_delta_coeff = MultiplyVectorsElementwise(this.variable_scaling.ToArray(), delta_coeff);

                this.argument_norm = NormOfScaledArgument(this.variable_scaling.ToArray(), delta_coeff);

                double distance_step_bound_old = distance_step_bound;
                distance_step_bound = this.argument_norm - this.step_bound;

                if (Math.Abs(distance_step_bound) <= 0.1 * this.step_bound || Math.Abs(lower_bound) < EPSILON && distance_step_bound <= distance_step_bound_old && distance_step_bound_old < 0 || iter_counter_lmparameter == 10)
                {
                    break;
                }

                double delta_lm_param = LMParameterStepByNewtonCorrection(scaled_delta_coeff, diag_variable_scaling, distance_step_bound);

                if (distance_step_bound > 0.0)
                {
                    lower_bound = Math.Max(lower_bound, this.lm_param);
                }
                if (distance_step_bound < 0.0)
                {
                    upper_bound = Math.Min(upper_bound, this.lm_param);
                }

                this.lm_param = Math.Max(lower_bound, this.lm_param + delta_lm_param);
            }

            if (iter_counter_lmparameter == 0)
            {
                this.lm_param = 0.0;
            }

            return delta_coeff;
        }

        private double LMParameterStepByNewtonCorrection(double[] scaled_delta_coeff, double[] diag_variable_scaling, double distance_step_bound)
        {
            double[] scaled_normed_delta_coeff = new double[this.qr_factorization.QR.ColumnCount];
            for (int i = 0; i < this.qr_factorization.QR.ColumnCount; i++)
            {
                scaled_normed_delta_coeff[i] = this.variable_scaling[this.qr_factorization.Permutation[i]] * (scaled_delta_coeff[this.qr_factorization.Permutation[i]] / this.argument_norm);
            }

            double temp;
            for (int i = 0; i < this.qr_factorization.QR.ColumnCount; i++)
            {
                scaled_normed_delta_coeff[i] = scaled_normed_delta_coeff[i] / diag_variable_scaling[i];
                temp = scaled_normed_delta_coeff[i];
                if (this.qr_factorization.QR.ColumnCount > i + 1)
                {

                    for (int j = i + 1; j < this.qr_factorization.QR.ColumnCount; j++)
                    {
                        scaled_normed_delta_coeff[j] = scaled_normed_delta_coeff[j] - this.qr_factorization.QR[j, i] * temp;
                    }
                }
            }
            temp = NormL2(scaled_normed_delta_coeff);

            return distance_step_bound / this.step_bound / temp / temp;
        }

        private double[] LevenbergMarquardtQRSolve(Vector qtf, ref double[] diag_variable_scaling)
        {
            Vector qtf_clone = (Vector)qtf.Clone();
            double[] delta_coeff = new double[this.qr_factorization.QR.ColumnCount];
            double[] scaled_lm_param = MultiplyScalarToVector(Math.Sqrt(lm_param), variable_scaling.ToArray());

            for (int i = 0; i < this.qr_factorization.QR.ColumnCount; i++)
            {
                for (int j = i; j < this.qr_factorization.QR.ColumnCount; j++)
                {
                    this.qr_factorization.QR[j, i] = this.qr_factorization.QR[i, j];
                }
            }

            this.DiagonalElementEliminationByGivensRotations(qtf_clone, scaled_lm_param, ref diag_variable_scaling);

            delta_coeff = this.qr_factorization.UnravelPermutation(SolveTriangularSystem(qtf_clone, diag_variable_scaling));

            return delta_coeff;
        }

        private double[] SolveTriangularSystem(Vector qtf_clone, double[] diag_variable_scaling)
        {
            int rank = this.RankOfMatrix(this.qr_factorization.QR);
            for (int i = rank; i < this.qr_factorization.QR.ColumnCount; i++)
            {
                qtf_clone[i] = 0;
            }

            if (rank > 0)
            {
                for (int i = rank - 1; i >= 0; i--)
                {
                    double sum = 0;
                    if (rank > i)
                    {
                        for (int k = i + 1; k < rank; k++)
                        {
                            sum += this.qr_factorization.QR[k, i] * qtf_clone[k];
                        }
                    }
                    qtf_clone[i] = (qtf_clone[i] - sum) / diag_variable_scaling[i];
                }
            }
            return qtf_clone.ToArray();
        }

        private void DiagonalElementEliminationByGivensRotations(Vector qtf_clone, double[] scaled_lm_param, ref double[] diag_variable_scaling)
        {
            Matrix qr = (Matrix)this.qr_factorization.QR.Clone();

            for (int i = 0; i < this.qr_factorization.QR.ColumnCount; i++)
            {
                if (Math.Abs(scaled_lm_param[this.qr_factorization.Permutation[i]]) >= EPSILON)
                {
                    for (int k = i; k < this.qr_factorization.QR.ColumnCount; k++)
                    {
                        diag_variable_scaling[k] = 0;
                    }
                    diag_variable_scaling[i] = scaled_lm_param[this.qr_factorization.Permutation[i]];

                    double qtf_ith_element = 0;
                    for (int k = i; k < this.qr_factorization.QR.ColumnCount; k++)
                    {
                        double cos_phi;
                        double sin_phi;
                        double temp;

                        if (Math.Abs(diag_variable_scaling[k]) >= EPSILON)
                        {
                            if (Math.Abs(this.qr_factorization.QR[k, k]) >= Math.Abs(diag_variable_scaling[k]))
                            {
                                temp = diag_variable_scaling[k] / this.qr_factorization.QR[k, k];
                                cos_phi = 0.5 / Math.Sqrt(0.25 + 0.25 * Math.Pow(temp, 2));
                                sin_phi = cos_phi * temp;
                            }
                            else
                            {
                                temp = this.qr_factorization.QR[k, k] / diag_variable_scaling[k];
                                sin_phi = 0.5 / Math.Sqrt(0.25 + 0.25 * Math.Pow(temp, 2));
                                cos_phi = sin_phi * temp;
                            }

                            this.qr_factorization.QR[k, k] = cos_phi * this.qr_factorization.QR[k, k] + sin_phi * diag_variable_scaling[k];
                            temp = cos_phi * qtf_clone[k] + sin_phi * qtf_ith_element;
                            qtf_ith_element = -(sin_phi * qtf_clone[k]) + cos_phi * qtf_ith_element;
                            qtf_clone[k] = temp;

                            if (this.qr_factorization.QR.ColumnCount >= k + 1)
                            {
                                for (int j = k + 1; j < this.qr_factorization.QR.ColumnCount; j++)
                                {
                                    temp = cos_phi * this.qr_factorization.QR[j, k] + sin_phi * diag_variable_scaling[j];
                                    diag_variable_scaling[j] = -(sin_phi * this.qr_factorization.QR[j, k]) + cos_phi * diag_variable_scaling[j];
                                    this.qr_factorization.QR[j, k] = temp;
                                }
                            }
                        }
                    }
                }
                diag_variable_scaling[i] = this.qr_factorization.QR[i, i];
                this.qr_factorization.QR[i, i] = qr[i, i];
            }
        }

        private double LowerBoundOfLMParameter(int rank, double[] delta_coeff)
        {
            double lower_bound = 0;
            double[] scaled_normed_delta_coeff = new double[this.qr_factorization.QR.ColumnCount];

            if (rank >= this.qr_factorization.QR.ColumnCount)
            {
                for (int i = 0; i < this.qr_factorization.QR.ColumnCount; i++)
                {
                    scaled_normed_delta_coeff[i] = this.variable_scaling[this.qr_factorization.Permutation[i]] * ((this.variable_scaling[this.qr_factorization.Permutation[i]] * delta_coeff[this.qr_factorization.Permutation[i]]) / this.argument_norm);
                }

                for (int i = 0; i < this.qr_factorization.QR.ColumnCount; i++)
                {
                    double sum = 0.0;
                    for (int j = 0; j < i; j++)
                    {
                        sum += this.qr_factorization.QR[j, i] * scaled_normed_delta_coeff[j];
                    }
                    scaled_normed_delta_coeff[i] = (scaled_normed_delta_coeff[i] - sum) / this.qr_factorization.QR[i, i];
                }
                double temp = NormL2(scaled_normed_delta_coeff);
                lower_bound = (this.argument_norm - this.step_bound) / this.step_bound / temp / temp;
            }
            return lower_bound;
        }

        private double UpperBoundOfLMParameter(Vector qtf)
        {
            double upper_bound;
            double[] temp = new double[this.qr_factorization.QR.ColumnCount];
            double[] sum = new double[this.qr_factorization.QR.ColumnCount];

            for (int i = 0; i < this.qr_factorization.QR.ColumnCount; i++)
            {
                sum[i] = 0;
                for (int j = 0; j <= i; j++)
                {
                    sum[i] += this.qr_factorization.QR[j, i] * qtf[j];
                }
                temp[i] = sum[i] / this.variable_scaling[this.qr_factorization.Permutation[i]];
            }

            this.gradient_norm = NormL2(temp);

            upper_bound = this.gradient_norm / this.step_bound;
            if (Math.Abs(upper_bound) < EPSILON)
            {
                upper_bound = Double.Epsilon / Math.Min(this.step_bound, 0.1);
            }
            return upper_bound;
        }

        private OptimResults SetOptimResults(double scaled_delta_coeff_norm, double actual_reduction, double predicted_reduction, Vector objective_function)
        {
            this.optim_results.Iterations = this.iter_counter;
            this.optim_results.ArgumentNorm = scaled_delta_coeff_norm;
            this.optim_results.FunctionNorm = this.function_norm;
            this.optim_results.GradientNorm = this.gradient_norm;
            this.optim_results.StepBoundToScaledArgumentNorm = this.step_bound / this.scaled_coeff_norm;
            this.optim_results.LatestFunctionReduction = actual_reduction;
            this.optim_results.LatestPredictedFunctionReduction = predicted_reduction;
            this.optim_results.ResidualVector = objective_function.ToArray();

            return this.optim_results;
        }

        private TerminationType ConvergenceCriterion(double actual_reduction, double predicted_reduction, double ratio, double p5)
        {
            if (Math.Abs(actual_reduction) <= this.optim_params.ToleranceFunction & predicted_reduction <= this.optim_params.ToleranceFunction & p5 * ratio <= 1)
            {
                this.optim_results.TerminationType = TerminationType.FunctionTolerance;
            }

            if (this.step_bound <= this.optim_params.ToleranceArgument * this.scaled_coeff_norm)
            {
                this.optim_results.TerminationType = TerminationType.ArgumentTolerance;
            }
            if (Math.Abs(actual_reduction) <= this.optim_params.ToleranceFunction & predicted_reduction <= this.optim_params.ToleranceFunction & p5 * ratio <= 1 & this.optim_results.TerminationType == TerminationType.ArgumentTolerance)
            {
                this.optim_results.TerminationType = TerminationType.FunctionArgumentTolerance;
            }
            return this.optim_results.TerminationType;
        }

        private TerminationType GetTerminationCriterion(double actual_reduction, double predicted_reduction, double ratio, double p5)
        {
            if (this.iter_counter >= this.optim_params.MaxIterations & this.optim_params.MaxIterations > 0)
            {
                this.optim_results.Iterations = this.iter_counter;
                this.optim_results.MaxIterationsExceeded = true;
                this.optim_results.TerminationType = TerminationType.MaxIterations;
            }
            if (Math.Abs(actual_reduction) <= AP.Math.MachineEpsilon & predicted_reduction <= AP.Math.MachineEpsilon & p5 * ratio <= 1)
            {
                this.optim_results.TerminationType = TerminationType.ChangeInFunctionSmallerMachinePrecision;
            }
            if (this.step_bound <= AP.Math.MachineEpsilon * this.scaled_coeff_norm)
            {
                this.optim_results.TerminationType = TerminationType.ChangeInArgumentSmallerMachinePrecision;
            }
            if (this.gradient_norm <= AP.Math.MachineEpsilon)
            {
                this.optim_results.TerminationType = TerminationType.ChangeInGradientSmallerMachinePrecision;
            }
            return this.optim_results.TerminationType;
        }

        private double NormOfScaledArgument(double[] variable_scaling, double[] delta_coeff)
        {
            return NormL2(MultiplyVectorsElementwise(variable_scaling, delta_coeff));
        }

        private double NormOfScaledCoefficient(double[] variable_scaling, double[] coeff)
        {
            return NormL2(MultiplyVectorsElementwise(variable_scaling, coeff));
        }

        //		private double NormOfFunction(Vector fvec)
        //		{
        //			return NormL2(fvec.CopyToArray());
        //		}

        private double NormOfScaledGradient(Vector qtf)
        {
            double gradient_norm = 0.0;

            if (Math.Abs(this.function_norm) >= EPSILON)
            {
                for (int i = 0; i < this.qr_factorization.QR.ColumnCount; i++)
                {
                    if (Math.Abs(this.qr_factorization.ColumnNorm[this.qr_factorization.Permutation[i]]) >= EPSILON)
                    {
                        double sum = 0.0;

                        for (int j = 0; j <= i; j++)
                        {
                            sum += (qtf[j] / this.function_norm) * this.qr_factorization.QR[j, i];
                        }

                        gradient_norm = Math.Max(gradient_norm, Math.Abs(sum / this.qr_factorization.ColumnNorm[this.qr_factorization.Permutation[i]]));
                    }
                }
            }
            return gradient_norm;
        }

        private void UpdateStepBoundLMParameter(double ratio_reduction, double actual_reduction, double directional_derivative, double function_norm_new, double[] delta_coeff_R, double delta_coeff_scaled_norm)
        {
            double temp = NormL2(delta_coeff_R) / this.function_norm;
            if (ratio_reduction > P25)
            {
                if (Math.Abs(this.lm_param) < EPSILON || ratio_reduction >= P75)
                {
                    this.step_bound = delta_coeff_scaled_norm / P5;
                    this.lm_param = P5 * this.lm_param;
                }
            }
            else
            {
                if (actual_reduction >= 0)
                {
                    temp = P5;
                }
                if (actual_reduction < 0)
                {
                    temp = P5 * directional_derivative / (directional_derivative + P5 * actual_reduction);
                }
                if (P1 * function_norm_new >= this.function_norm || temp < P1)
                {
                    temp = P1;
                }
                this.step_bound = temp * Math.Min(this.step_bound, delta_coeff_scaled_norm / P1);
                this.lm_param = this.lm_param / temp;
            }

        }

        private double RatioOfReduction(double actual_reduction, double predicted_reduction)
        {
            double ratio = 0;
            if (Math.Abs(predicted_reduction) >= EPSILON)
            {
                ratio = actual_reduction / predicted_reduction;
            }
            return ratio;
        }

        private double DirectionalDerivative(double[] delta_coeff_R, double delta_coeff_scaled_norm)
        {
            return -(Math.Pow(NormL2(delta_coeff_R) / this.function_norm, 2) + Math.Pow(Math.Sqrt(this.lm_param) * delta_coeff_scaled_norm / this.function_norm, 2));
        }

        private double PredictedReduction(double[] delta_coeff_R, double delta_coeff_scaled_norm)
        {
            return Math.Pow(NormL2(delta_coeff_R) / this.function_norm, 2) + Math.Pow(Math.Sqrt(this.lm_param) * delta_coeff_scaled_norm / this.function_norm, 2) / P5;
        }

        private double ActualReduction(double function_norm_new)
        {
            double actual_reduction = -1;
            if (P1 * function_norm_new < this.function_norm)
            {
                actual_reduction = 1 - Math.Pow(function_norm_new / this.function_norm, 2);
            }
            return actual_reduction;
        }

        private int RankOfMatrix(Matrix A)
        {
            int rank = A.ColumnCount;
            for (int i = 0; i < A.ColumnCount; i++)
            {
                if (Math.Abs(A[i, i]) < EPSILON && rank == A.ColumnCount)
                {
                    rank = i - 1;
                }
            }
            return rank;
        }

        private double NormL2(double[] vec)
        {
            double norm = 0.0;
            for (int i = 0; i < vec.GetLength(0); i++)
            {
                norm += Math.Pow(vec[i], 2.0);
            }
            return Math.Sqrt(norm);
        }

        private double[] MultiplyVectorsElementwise(double[] vec1, double[] vec2)
        {
            if (vec1.GetLength(0) != vec2.GetLength(0))
                throw new ArgumentException(Catalog.GetString("Elementwise multiplication requires vectors of same length."));

            double[] vec = new double[vec1.GetLength(0)];
            for (int i = 0; i < vec1.GetLength(0); i++)
            {
                vec[i] = vec1[i] * vec2[i];
            }
            return vec;
        }

        private double[] MultiplyScalarToVector(double scalar, double[] vec)
        {
            double[] vec_result = new double[vec.GetLength(0)];
            for (int i = 0; i < vec.GetLength(0); i++)
            {
                vec_result[i] = scalar * vec[i];
            }

            return vec_result;
        }
    }
}

