using System;

namespace Science.Mathematics.VectorCalculus
{
    public class LagrangeMultiplierMethod
    {
        private Function.ToLastType<double[], double> f;
        private double[] x;
        private double[] x0;
        private double[] lambda;
        private Function.ToLastType<double[], double[]> g;
        private Function.ToLastType<double[], double> gs;
        public LagrangeMultiplierMethod(Function.ToLastType<double[], double> function,
            Function.ToLastType<double[], double[]> constraint,
            double[] initialPoint, double[] initialLambda)
        {
            f = function;
            g = constraint;
            x0 = initialPoint;
            x = new double[x0.Length];
            lambda = initialLambda;
        }
        public LagrangeMultiplierMethod(Function.ToLastType<double[], double> function,
            Function.ToLastType<double[], double> constraint,
            double[] initialPoint, double initialLambda)
        {
            f = function;
            gs = constraint;
            g = new Function.ToLastType<double[], double[]>(gsf);
            x0 = initialPoint;
            x = new double[x0.Length];
            lambda = new double[1];
            lambda[0] = initialLambda;
        }
        public double[] CriticalPoint
        {
            get { return x; }
        }
        public double[] LagrangeMultiplier
        {
            get { return lambda; }
            set { lambda = value; }
        }
        public double Lambda
        {
            get { return lambda[0]; }
            set { lambda[0] = value; }
        }
        public double[] InitialPoint
        {
            set { x0 = value; }
        }
        public void Compute()
        {
            double[] xl = new double[x0.Length + lambda.Length];
            for (int i = 0; i < x0.Length; i++) xl[i] = x0[i];
            for (int i = 0; i < lambda.Length; i++) xl[i+x0.Length] = lambda[i];
            Function.ToLastType<double[], double[]> dd
                = new Function.ToLastType<double[], double[]>(fg);
            NonlinearFunction ff = new NonlinearFunction(dd);
            ff.FindRoot(xl);
            for (int i = 0; i < x0.Length; i++) x[i] = ff.Root[i];
            for (int i = 0; i < lambda.Length; i++) lambda[i] = ff.Root[i+x0.Length];
        }
        private double[] fg(double[] xl)
        {
            double[] ff = new double[x0.Length+lambda.Length];
            double[] xx = new double[x0.Length];
            for (int i = 0; i < xx.Length; i++) xx[i] = xl[i];
            Gradient delf = new Gradient(f, xx);
            delf.Compute();
            for (int j = 0; j < xx.Length; j++) ff[j] += delf.Result[j];
            for (int i = 0; i < lambda.Length; i++)
            {
                ii = i;
                Function.ToLastType<double[], double> gg
                    = new Function.ToLastType<double[], double>(gk);
                Gradient delg = new Gradient(gg, xx);
                delg.Compute();
                for (int j = 0; j < xx.Length; j++) ff[j] -= xl[x0.Length + i] * delg.Result[j];
            }
            for (int i = 0; i < lambda.Length; i++) ff[xx.Length + i] = g(xx)[i];
            return ff;
        }
        private int ii = 0;
        private double gk(double[] xxx)
        {
            return g(xxx)[ii];
        }
        private double[] gsf(double[] xxx)
        {
            double[] r = new double[1];
            r[0] = gs(xxx);
            return r;
        }
    }
}