using System;

namespace Science.Mathematics.VectorCalculus
{
    public class HessianBordered
    {
        private Function.ToLastType<double[], double> f;
        private double[] x;
        private double lamb;
        private Function.ToLastType<double[], double> g;
        private MatrixSquare ma;
        public HessianBordered(Function.ToLastType<double[], double> function,
            Function.ToLastType<double[], double> constraint,
            double[] criticalPoint, double lambda)
        {
            f = function;
            g = constraint;
            x = criticalPoint;
            lamb = lambda;
            ma = new MatrixSquare(x.Length + 1);
        }
        public double[] CriticalPoint
        {
            set { x = value; }
        }
        public double Lambda
        {
            set { lamb = value; }
        }
        public MatrixSquare Matrix
        {
            get { return ma; }
        }
        public void FindDeterminants()
        {
            Gradient gr = new Gradient(g, x);
            gr.Compute();

            for (int i = 0; i < x.Length; i++)
            {
                ma[1 + i, 0] = -gr.Result[i];
                ma[0, 1 + i] = ma[1 + i, 0];
            }

            Function.ToLastType<double[], double> h
                = new Function.ToLastType<double[], double>(ht);
            double[] sd = new double[x.Length];
            Hessian hess = new Hessian(h, x, sd);
            hess.Compute();
            for (int i = 0; i < x.Length; i++)
                for (int j = i; j < x.Length; j++)
                {
                    ma[1 + i, 1 + j] = hess.Matrix[i, j];
                    ma[1 + j, 1 + i] = hess.Matrix[i, j];
                }

            ds = new double[x.Length];
            double[,] tel;
            LUdcmp lud;
            for (int k = 0; k < x.Length; k++)
            {
                tel = new double[k+2, k+2];
                for (int i = 0; i < tel.GetLength(0); i++)
                    for (int j = 0; j < tel.GetLength(1); j++)
                    {
                        tel[i, j] = ma[i, j];
                    }
                lud = new LUdcmp(tel);
                ds[k] = lud.det();
            } 
        }

        private double ht(double[] cp)
        {
            return f(cp) - lamb * g(cp);
        }
           
        private double[] ds;
        public double[] Determinants
        {
            get { return ds; }
        } 
    }
}