using System;
using Deltares.Mathematics;

// =======================================================================================================================
// Class name: TDLevenbergMarquardt
//
// Description:
//
// Copyright (c) 2010 Deltares
//
// Date        ID   Modification
// 2010-01-18  Sel  Created
// 2010-02-03  Sel  Separated algorithm in Extreme and Fitting
// 2010-02-05  Sel  Moved Variables to BasicExtremeAndFittingCalculation
// 2010-02-09  Sel  Implemented Levenberg Marquardt in MStab Spencer
// 2010-02-10  Sel  Improved LevenbergMarquardt algorithm code
// 2010-02-10  Sel  Introduced MDVariationalCalculus
// 2010-02-11  Sel  Distinguished InvalidResult between GA and LM; Extended role of bounds
// 2010-02-15  Sel  Allocated Bounds in LM
// =======================================================================================================================

namespace Deltares.Stability.Calculation2.Levenberg
{
    public class LevenbergMarquardtException : Exception
    {
        public LevenbergMarquardtException(string message)
            : base(message) {}

        public LevenbergMarquardtException(string message, Exception innerException)
            : base(message, innerException) {}
    }

    public abstract class LevenbergMarquardt : VariationalCalculus
    {
        // Private fields
        protected double FShift = 0;
        protected double driftAfter = 0;
        protected double driftPrior = 0;
        protected double[] functionValues;
        protected TMatrix jacobian = null;
        protected double[] parameters;
        protected bool valid = true;
        private double driftGrant = 0;
        private int iterationCount = 0;
        private double lambdaBoost = 0;
        private double lambdaLower = 0;
        private double lambdaUpper = 0;
        private LevenbergMarquardtOptions levenbergMarquardtOptions = null;
        private TMatrix solution = null;
        private int variableCount = 0;

        // TDLevenbergMarquardt
        // =======================================================================================================================
        // Date        ID   Modification
        // 2010-01-18  Sel  Created
        // =======================================================================================================================
        //Constructor  Create()
        public LevenbergMarquardt()
        {
            InvalidResult = 0;
            lambdaLower = 0.0015625;
            lambdaUpper = 1024;
            lambdaBoost = 4;
            jacobian = new TMatrix();
            solution = new TMatrix();
        }

        // Public properties
        public int IterationCount
        {
            get
            {
                return iterationCount;
            }
            set
            {
                iterationCount = value;
            }
        }

        public double Shift
        {
            get
            {
                return FShift;
            }
            set
            {
                FShift = value;
            }
        }

        public double DriftGrant
        {
            get
            {
                return driftGrant;
            }
            set
            {
                driftGrant = value;
            }
        }

        public double DriftFinal
        {
            get
            {
                return driftAfter;
            }
            set
            {
                driftAfter = value;
            }
        }

        public double LambdaLower
        {
            get
            {
                return lambdaLower;
            }
            set
            {
                lambdaLower = value;
            }
        }

        public double LambdaUpper
        {
            get
            {
                return lambdaUpper;
            }
            set
            {
                lambdaUpper = value;
            }
        }

        public double LambdaBoost
        {
            get
            {
                return lambdaBoost;
            }
            set
            {
                lambdaBoost = value;
            }
        }

        public double[] Parameters
        {
            get
            {
                return parameters;
            }
            set
            {
                parameters = value;
            }
        }

        public int VariableCount
        {
            get
            {
                return variableCount;
            }
            set
            {
                SetVariableCount(value);
            }
        }

        public int ParameterCount
        {
            get
            {
                return GetParameterCount();
            }
            set
            {
                SetParameterCount(value);
            }
        }

        public LevenbergMarquardtOptions LevenbergMarquardtOptions
        {
            get
            {
                return levenbergMarquardtOptions;
            }
            set
            {
                levenbergMarquardtOptions = value;
                variableCount = levenbergMarquardtOptions.VariableCount;
                FShift = levenbergMarquardtOptions.Shift;
                driftGrant = levenbergMarquardtOptions.DriftGrant;
                iterationCount = levenbergMarquardtOptions.IterationCount;
            }
        }

        public abstract void DetermineDrift(double[] parameters);
        public abstract void FillJacobian();

        // =======================================================================================================================
        // Date        ID   Modification
        // 2010-02-02  Sel  Created
        // =======================================================================================================================

        public virtual void SetVariableCount(int count)
        {
            variableCount = count;
        }

        public override void Calculation()
        {
            double lambda = lambdaUpper/Math.Pow(lambdaBoost, 2);
            int iterationIndex = 0;
            var diagonal = new double[VariableCount];
            var backupParameters = new double[Parameters.Length];

            Copy(Parameters, backupParameters);

            int outerCounter = 0;

            do
            {
                outerCounter++;

                // Try a better fit according to Levenberg-Marquardt
                FillJacobian();

                for (int parameterIndex = 0; parameterIndex < ParameterCount; parameterIndex++)
                {
                    diagonal[parameterIndex] = jacobian.Matrix[parameterIndex][parameterIndex];
                }
                driftPrior = driftAfter;

                int innerCounter = 0;
                do
                {
                    if (valid)
                    {
                        innerCounter++;

                        for (int i = 0; i < jacobian.RowCount; i++)
                        {
                            if (jacobian.IsColumnAllZero(i) != jacobian.IsRowAllZero(i))
                            {
                                Console.WriteLine("Matrix not valid");
                            }
                        }

                        // Start with an increased lambda
                        lambda = Math.Min(lambda*lambdaBoost, lambdaUpper);
                        for (int parameterIndex = 0; parameterIndex < ParameterCount; parameterIndex++)
                        {
                            // Strengthen the diagonal
                            jacobian.Matrix[parameterIndex][parameterIndex] = diagonal[parameterIndex]*(1 + lambda);
                        }

                        // Solve the set of equations
                        try
                        {
                            int[] excludedRows = null;

                            TMatrix filteredMatrix = jacobian.GetFilteredMatrix(out excludedRows);
                            if (excludedRows.Length > 0)
                            {
                                var filteredSolution = new TMatrix();
                                filteredSolution.SetDimensions(filteredMatrix.RowCount, filteredMatrix.ColumnCount);

                                filteredSolution.Decomposition(filteredMatrix);
                                filteredSolution.ImprovedSolver(filteredMatrix);

                                solution.CombineVector(filteredSolution, excludedRows);
                            }
                            else
                            {
                                solution.Decomposition(filteredMatrix);
                                solution.ImprovedSolver(filteredMatrix);
                            }
                        }
                        catch
                        {
                            // Force an unproportional high drift
                            for (int parameterIndex = 0; parameterIndex < ParameterCount; parameterIndex++)
                            {
                                solution.Vector[parameterIndex] = Single.MaxValue;
                            }
                        }

                        Copy(solution.Vector, backupParameters);

                        // Determine drift of new fit
                        for (int parameterIndex = 0; parameterIndex < ParameterCount; parameterIndex++)
                        {
                            double lowerBound = GetBounds(parameterIndex).Min;
                            double upperBound = GetBounds(parameterIndex).Inc + lowerBound;
                            solution.Vector[parameterIndex] = Math.Max(lowerBound, Math.Min(upperBound, Parameters[parameterIndex] - solution.Vector[parameterIndex]));
                        }
                        DetermineDrift(solution.Vector);
                    }
                } while (valid && !(driftAfter < driftPrior || lambda == lambdaUpper));

                // Adjust next fitting; note, that lambda is increased at the start
                if (valid && driftAfter < driftPrior)
                {
                    Copy(Parameters, backupParameters);
                    Copy(solution.Vector, Parameters);

                    lambda = Math.Max(lambda/lambdaBoost, lambdaLower)/lambdaBoost;
                    driftPrior = driftAfter;
                }

                // Update iteration index
                iterationIndex++;
            } while (valid && !(driftAfter < driftGrant || lambda == lambdaUpper || iterationIndex == iterationCount));

            // Revert the last soltion
            if (!valid)
            {
                Copy(backupParameters, Parameters);
            }
        }

        protected bool IsValidResult(double result)
        {
            bool validResult = !double.IsNaN(result) && !double.IsInfinity(result) && !double.IsPositiveInfinity(result) && !double.IsNegativeInfinity(result) && result != double.MaxValue && result != double.MinValue;

            return validResult;
        }

        private void Copy(double[] source, double[] target)
        {
            for (int i = 0; i < source.Length; i++)
            {
                target[i] = source[i];
            }
        }

        private int GetParameterCount()
        {
            return parameters.Length;
        }

        // =======================================================================================================================
        // Date        ID   Modification
        // 2010-01-20  Sel  Created
        // =======================================================================================================================
        private void SetParameterCount(int count)
        {
            var CBounds = new TBounds()
                {
                    Min = -Single.MaxValue, Inc = 2*Single.MaxValue
                };

            // Allocate memory
            parameters = new double[count];
            BoundsPtr = new TBounds[count];
            jacobian.SetDimensions(ParameterCount, count);
            solution.SetDimensions(ParameterCount, count);

            // Default bounds are unlimited
            for (int parameterIndex = 0; parameterIndex < count; parameterIndex++)
            {
                SetBounds(parameterIndex, CBounds);
            }
        }
    }
}