// 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 Hiperscan.Unix;

using MathNet.Numerics.LinearAlgebra.Double;

using static System.Math;


namespace Hiperscan.Spectroscopy.Math
{

    public partial class LevenbergMarquardt
    {

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

        private BaseFit 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 const double MACHINE_EPSILON = 5E-16;

        private LevenbergMarquardt(BaseFit 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 (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 (Abs(this.step_bound) < EPSILON)
                this.step_bound = 100.0;

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

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

        public static OptimResults Fit(BaseFit 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)
            {
                Vector fvec_objectivefunc = this.lm_fit.ObjectiveFunc(this.coeff);
                JacobianMatrix jacobian = new JacobianMatrix(this.lm_fit.FitData, this.coeff, this.lm_fit.ObjectiveFunc);

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

                this.gradient_norm = this.NormOfScaledGradient(qtf);

                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] = 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++;

                    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];
                    }

                    double scaled_delta_coeff_norm = this.NormOfScaledArgument(this.variable_scaling.ToArray(),
                                                                               delta_coeff);

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

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

                    double function_norm_new = this.NormL2(fvec_objectivefunc_new.ToArray());
                    double actual_reduction  = this.ActualReduction(function_norm_new);
                    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);

                    ratio_reduction = this.RatioOfReduction(actual_reduction, predicted_reduction);

                    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];

            int rank = this.RankOfMatrix(qr_factorization.QR);

            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));

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

            double distance_step_bound = this.argument_norm - 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 = Max(this.lm_param, lower_bound);
            this.lm_param = Min(this.lm_param, upper_bound);

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

            while (true)
            {
                iter_counter_lmparameter++;

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

                delta_coeff = LevenbergMarquardtQRSolve(qtf, ref diag_variable_scaling);

                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 (Abs(distance_step_bound) <= 0.1 * this.step_bound ||
                    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 = Max(lower_bound, this.lm_param);

                if (distance_step_bound < 0.0)
                    upper_bound = Min(upper_bound, this.lm_param);

                this.lm_param = 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(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 (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 (Abs(diag_variable_scaling[k]) >= EPSILON)
                        {
                            if (Abs(this.qr_factorization.QR[k, k]) >= Abs(diag_variable_scaling[k]))
                            {
                                temp = diag_variable_scaling[k] / this.qr_factorization.QR[k, k];
                                cos_phi = 0.5 / Sqrt(0.25 + 0.25 * Pow(temp, 2));
                                sin_phi = cos_phi * temp;
                            }
                            else
                            {
                                temp = this.qr_factorization.QR[k, k] / diag_variable_scaling[k];
                                sin_phi = 0.5 / Sqrt(0.25 + 0.25 * 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 (Abs(upper_bound) < EPSILON)
                upper_bound = double.Epsilon / 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 (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 (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 (Abs(actual_reduction) <= MACHINE_EPSILON &&
                predicted_reduction   <= MACHINE_EPSILON &&
                p5 * ratio <= 1)
            {
                this.optim_results.TerminationType = TerminationType.ChangeInFunctionSmallerMachinePrecision;
            }

            if (this.step_bound <= MACHINE_EPSILON * this.scaled_coeff_norm)
                this.optim_results.TerminationType = TerminationType.ChangeInArgumentSmallerMachinePrecision;

            if (this.gradient_norm <= MACHINE_EPSILON)
                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 NormOfScaledGradient(Vector qtf)
        {
            double gradient_norm = 0.0;

            if (Abs(this.function_norm) >= EPSILON)
            {
                for (int i = 0; i < this.qr_factorization.QR.ColumnCount; i++)
                {
                    if (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 = Max(gradient_norm,
                                            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 (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 * 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.0;

            if (Abs(predicted_reduction) >= EPSILON)
                ratio = actual_reduction / predicted_reduction;

            return ratio;
        }

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

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

        private double ActualReduction(double function_norm_new)
        {
            double actual_reduction = -1.0;

            if (P1 * function_norm_new < this.function_norm)
                actual_reduction = 1 - Pow(function_norm_new / this.function_norm, 2.0);

            return actual_reduction;
        }

        private int RankOfMatrix(Matrix A)
        {
            int rank = A.ColumnCount;

            for (int i = 0; i < A.ColumnCount; i++)
            {
                if (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 += Pow(vec[i], 2.0);
            }

            return 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;
        }
    }
}