using System;
using System.Collections.Generic;

namespace Science.Mathematics.OrdinaryDifferentialEquation
{
	/// <summary>
	/// EquationWithBoundaryCondition
	/// </summary>
   
    public class EquationWithBoundaryCondition
	{
        public EquationWithBoundaryCondition()
        {
        }

        public EquationWithBoundaryCondition(Function.ToLastType<double, double[], double[]> firstDerivativeFunction,
            BoundaryCondition boundaryCondition, double[] initialGuess, int howMany)
        {
            fdf = firstDerivativeFunction;
            bc = boundaryCondition;
            initialpara = initialGuess;
            num = howMany;
        }

        private List<double> xp = new List<double>();
        public List<double> IndependentVariable
		{
			get{return xp;}
		}
   
        private List<double[]> yp = new List<double[]>();
		public List<double[]> Solution
		{
			get{return yp;}
		}       

        private Function.ToLastType<double,double[],double[]> fdf;
        public Function.ToLastType<double,double[],double[]> FirstDerivativeFunction
        {
            set{ fdf = value; }
        }

        private BoundaryCondition bc;
        public BoundaryCondition Condition
        {
            set{ bc = value; }
        }

        private double[] initialpara;
		public double[] InitialGuess
		{
			set{ initialpara = value; }
		}

        int num = 100;
        public int HowMany
        {
            set { num = value; }
        }

		public void Solve()
		{
            double[] parameter = new double[initialpara.Length];
            for (int i = 0; i < parameter.Length; i++) parameter[i] = initialpara[i];

            int nv = bc.Load(bc.Start, parameter).Length;
            double[] variable = new double[nv];

			variable = bc.Load(bc.Start,parameter);

			Function.ToLastType<double[],double[]> dele = new Function.ToLastType<double[],double[]>(Mshoot);

            Newt method = new Newt();
	
		    method.newt(parameter,dele);
			if (method.Check) 
			{
                try { throw new Science.Error(); }
                catch (Science.Error e)
                {
                    e.Write("shoot failed; bad initial guess");
                }
			}  
		}  
	
		private double[] Mshoot(double[] p)
		{
			double[] xx = new double[p.Length];
			double x1 = bc.Start;
			double x2 = bc.End;
			shoot(bc.Load(x1,p).Length,x1,x2,p,fdf,bc);
			for(int k = 0; k<f.Length; k++) xx[k] = f[k];
			return xx;
		}

		private double EPS  = 1.0e-12;
     	private double[] f;   // output
		private void shoot(int nvar, double x1, double x2, double[] p,
            Function.ToLastType<double,double[],double[]> derivs, 
			BoundaryCondition boundaryCondition)
		{
            double h1 = (x2 - x1) / (double)num;
			double hmin=0.0;

      		double[] v = boundaryCondition.Load(x1,p);

            StepperBase s = new StepperDopr5(nvar);
            s.derivs = derivs;

            Output outt = new Output(num);
            Odeint dd = new Odeint(v, x1, x2, EPS, EPS, h1, hmin, outt, s);
            
            dd.integrate();

            xp.Clear();
            yp.Clear();

            for (int i = 0; i <= num; i++) xp.Add(outt.xsave[i]);
            for (int i = 0; i < nvar; i++)
            {
                double[] a = new double[num + 1];
                for (int j = 0; j <= num; j++)
                {
                    a[j] = outt.ysave[i, j];
                }
                yp.Add(a);
            }
            for (int i = 0; i < nvar; i++) v[i] = outt.ysave[i, num];
  			f = boundaryCondition.Score(x2,v,p);
		}
    }    


    // private class

    class Newt
    {
        private bool check;
        public bool Check  // output
        {
            get { return check; }
        }
        public Newt()
        {
        }
        public void newt(double[] x, Function.ToLastType<double[], double[]> vecfunc) // x is input and output
        {
            int MAXITS = 200;
            double TOLF = 1.0e-8;
            double TOLMIN = 1.0e-12;
            double TOLX = 1.0e-12;
            double STPMX = 100.0;

            int i, its, j, n = x.Length;
            double den, f, fold, stpmax, sum, temp, test;
            double[,] fjac = new Double[n, n];
            double[] g = new Double[n];
            double[] p = new Double[n];
            double[] xold = new Double[n];
            NRfdjac fd = new NRfdjac(vecfunc);
            NRfmin fm = new NRfmin(vecfunc);
            f = fm.Evaluate(x);
            double[] fvec = fm.Fvec;
            test = 0.0;
            for (i = 0; i < n; i++)
                if (Math.Abs(fvec[i]) > test) test = Math.Abs(fvec[i]);
            if (test < 0.01 * TOLF) { check = false; return; }
            sum = 0.0;
            for (i = 0; i < n; i++) sum += x[i] * x[i];
            stpmax = STPMX * Math.Max(Math.Sqrt(sum), (double)n);
            for (its = 0; its < MAXITS; its++)
            {
                fvec = vecfunc(x);
                fjac = fd.Evaluate(x, fvec);
                for (i = 0; i < n; i++)
                {
                    sum = 0.0;
                    for (j = 0; j < n; j++) sum += fjac[j, i] * fvec[j];
                    g[i] = sum;
                }
                for (i = 0; i < n; i++) xold[i] = x[i];
                fold = f;
                for (i = 0; i < n; i++) p[i] = -fvec[i];
                LUdcmp obj = new LUdcmp(fjac);
                obj.solve(p, p);
                Function.ToLastType<double[], double> func
                    = new Function.ToLastType<double[], double>(fm.Evaluate);
                Lnsrch ln = new Lnsrch();
                ln.lnsrch(xold, fold, g, p, stpmax, func);
                check = ln.Check;
                for (i = 0; i < n; i++) x[i] = ln.X[i];
                f = ln.F;
                test = 0.0;
                for (i = 0; i < n; i++) if (Math.Abs(fvec[i]) > test) test = Math.Abs(fvec[i]);
                if (test < TOLF)
                {
                    check = false;
                    return;
                }
                if (check)
                {
                    test = 0.0;
                    den = Math.Max(f, 0.5 * n);
                    for (i = 0; i < n; i++)
                    {
                        temp = Math.Abs(g[i]) * Math.Max(Math.Abs(x[i]), 1.0) / den;
                        if (temp > test) test = temp;
                    }
                    check = (test < TOLMIN ? true : false);
                    return;
                }
                test = 0.0;
                for (i = 0; i < n; i++)
                {
                    temp = (Math.Abs(x[i] - xold[i])) / Math.Max(Math.Abs(x[i]), 1.0);
                    if (temp > test) test = temp;
                }
                if (test < TOLX) return;
            }
            try { throw new Science.Error(); }
            catch (Science.Error e)
            {
                e.Write("MAXITS exceeded in newt");
            }
        }
    }

    class NRfdjac
    {
        private Function.ToLastType<double[], double[]> func;
        public NRfdjac(Function.ToLastType<double[], double[]> funcc)
        {
            func = funcc;
        }
        public double[,] Evaluate(double[] x, double[] fvec)
        {
            double EPS = 1.0e-8;
            int n = x.Length;
            double[,] df = new double[n, n];
            double[] xh = x;
            for (int j = 0; j < n; j++)
            {
                double temp = xh[j];
                double h = EPS * Math.Abs(temp);
                if (h == 0.0) h = EPS;
                xh[j] = temp + h;
                h = xh[j] - temp;
                double[] f = func(xh);
                xh[j] = temp;
                for (int i = 0; i < n; i++)
                    df[i, j] = (f[i] - fvec[i]) / h;
            }
            return df;
        }
    }

    class NRfmin
    {
        private double[] fvec;
        public double[] Fvec
        {
            get { return fvec; }
        }
        private Function.ToLastType<double[], double[]> jv;
        public NRfmin(Function.ToLastType<double[], double[]> fv)
        {
            jv = fv;
        }

        public double Evaluate(double[] x)
        {
            int n = x.Length;
            double sum = 0.0;
            fvec = jv(x);
            for (int i = 0; i < n; i++) sum += fvec[i] * fvec[i];
            return 0.5 * sum;
        }
    }

    class Lnsrch
    {
        private double[] x;
        public double[] X  // output
        {
            get { return x; }
        }
        bool check;
        public bool Check  // output
        {
            get { return check; }
        }
        double f;
        public double F  // output
        {
            get { return f; }
        }
        public Lnsrch()
        {
        }
        public void lnsrch(double[] xold, double fold, double[] g, double[] p,
            double stpmax, Function.ToLastType<double[], double> func)
        {
            double ALF = 1.0e-4;
            double TOLX = 1.0e-12;
            double a, alam, alam2 = 0.0, alamin, b, disc, f2 = 0.0;
            double rhs1, rhs2, slope = 0.0, sum = 0.0, temp, test, tmplam;
            int i, n = xold.Length;
            x = new double[n];
            check = false;
            for (i = 0; i < n; i++) sum += p[i] * p[i];
            sum = Math.Sqrt(sum);
            if (sum > stpmax)
                for (i = 0; i < n; i++) p[i] *= stpmax / sum;
            for (i = 0; i < n; i++)
                slope += g[i] * p[i];
            if (slope >= 0.0)
                try { throw new Science.Error(); }
                catch (Science.Error e)
                {
                    e.Write("Roundoff problem in lnsrch.");
                }
            test = 0.0;
            for (i = 0; i < n; i++)
            {
                temp = Math.Abs(p[i]) / Math.Max(Math.Abs(xold[i]), 1.0);
                if (temp > test) test = temp;
            }
            alamin = TOLX / test;
            alam = 1.0;
            for (; ; )
            {
                for (i = 0; i < n; i++) x[i] = xold[i] + alam * p[i];
                f = func(x);
                if (alam < alamin)
                {
                    for (i = 0; i < n; i++) x[i] = xold[i];
                    check = true;
                    return;
                }
                else if (f <= fold + ALF * alam * slope) return;
                else
                {
                    if (alam == 1.0)
                        tmplam = -slope / (2.0 * (f - fold - slope));
                    else
                    {
                        rhs1 = f - fold - alam * slope;
                        rhs2 = f2 - fold - alam2 * slope;
                        a = (rhs1 / (alam * alam) - rhs2 / (alam2 * alam2)) / (alam - alam2);
                        b = (-alam2 * rhs1 / (alam * alam) + alam * rhs2 / (alam2 * alam2)) / (alam - alam2);
                        if (a == 0.0) tmplam = -slope / (2.0 * b);
                        else
                        {
                            disc = b * b - 3.0 * a * slope;
                            if (disc < 0.0) tmplam = 0.5 * alam;
                            else if (b <= 0.0) tmplam = (-b + Math.Sqrt(disc)) / (3.0 * a);
                            else tmplam = -slope / (b + Math.Sqrt(disc));
                        }
                        if (tmplam > 0.5 * alam)
                            tmplam = 0.5 * alam;
                    }
                }
                alam2 = alam;
                f2 = f;
                alam = Math.Max(tmplam, 0.1 * alam);
            }
        }
    }	

    class LUdcmp
    {
        int n;
        private double[,] lu;
        private int[] indx;
        private double d;
        private double[,] aref;

        public double[,] LU
        {
            get { return lu; }  // output
        }
        public int[] RowPermutation // output
        {
            get { return indx; }
        }
        public double Sign // output
        {
            get { return d; }
        }
        public LUdcmp(double[,] a)   // a input
        {
            n = a.GetLength(0);
            lu = new double[n, n];
            for (int ir = 0; ir < n; ir++)
                for (int ic = 0; ic < n; ic++) lu[ir, ic] = a[ir, ic];
            aref = a;
            indx = new int[n];
            double TINY = 1.0e-40;
            int i, imax = 0, j, k;
            double big, temp;
            double[] vv = new Double[n];
            d = 1.0;
            for (i = 0; i < n; i++)
            {
                big = 0.0;
                for (j = 0; j < n; j++)
                    if ((temp = Math.Abs(lu[i, j])) > big) big = temp;
                if (big == 0.0) try { throw new Science.Error(); }
                    catch (Science.Error e)
                    {
                        e.Write("Singular matrix in LUdcmp");
                    }
                vv[i] = 1.0 / big;
            }
            for (k = 0; k < n; k++)
            {
                big = 0.0;
                for (i = k; i < n; i++)
                {
                    temp = vv[i] * Math.Abs(lu[i, k]);
                    if (temp > big)
                    {
                        big = temp;
                        imax = i;
                    }
                }
                if (k != imax)
                {
                    for (j = 0; j < n; j++)
                    {
                        temp = lu[imax, j];
                        lu[imax, j] = lu[k, j];
                        lu[k, j] = temp;
                    }
                    d = -d;
                    vv[imax] = vv[k];
                }
                indx[k] = imax;
                if (lu[k, k] == 0.0) lu[k, k] = TINY;
                for (i = k + 1; i < n; i++)
                {
                    temp = lu[i, k] /= lu[k, k];
                    for (j = k + 1; j < n; j++) lu[i, j] -= temp * lu[k, j];
                }
            }
        }

        public void solve(double[] b, double[] x) // b input and x output
        {
            int i, ii = 0, ip, j;
            double sum;
            if (b.Length != n || x.Length != n) try { throw new Science.Error(); }
                catch (Science.Error e)
                {
                    e.Write("Bad sizes");
                }
            for (i = 0; i < n; i++) x[i] = b[i];
            for (i = 0; i < n; i++)
            {
                ip = indx[i];
                sum = b[ip];
                x[ip] = x[i];
                if (ii != 0) for (j = ii - 1; j < i; j++) sum -= lu[i, j] * x[j];
                else if (sum != 0.0) ii = i + 1;
                x[i] = sum;
            }
            for (i = n - 1; i >= 0; i--)
            {
                sum = x[i];
                for (j = i + 1; j < n; j++) sum -= lu[i, j] * x[j];
                x[i] = sum / lu[i, i];
            }
        }

        public void solve(double[,] b, double[,] x) // b input and x output
        {
            int i, j, m = b.GetLength(1);
            if (b.GetLength(0) != n || x.GetLength(0) != n
                || b.GetLength(1) != x.GetLength(1)) try { throw new Science.Error(); }
                catch (Science.Error e)
                {
                    e.Write("Bad sizes");
                }
            double[] xx = new double[n];
            for (j = 0; j < m; j++)
            {
                for (i = 0; i < n; i++) xx[i] = b[i, j];
                solve(xx, xx);
                for (i = 0; i < n; i++) x[i, j] = xx[i];
            }
        }

        public double[,] inverse()
        {
            double[,] ainv = new double[n, n];
            for (int i = 0; i < n; i++) ainv[i, i] = 1.0;
            solve(ainv, ainv);
            return ainv;
        }

        public double det()
        {
            double dd = d;
            for (int i = 0; i < n; i++) dd *= lu[i, i];
            return dd;
        }

        public void mprove(double[] b, double[] x)
        {
            int j, i;
            double sdp;
            double[] r = new Double[n];

            for (i = 0; i < n; i++)
            {
                sdp = -b[i];
                for (j = 0; j < n; j++) sdp += aref[i, j] * x[j];
                r[i] = sdp;
            }
            solve(r, r);
            for (i = 0; i < n; i++) x[i] -= r[i];
        }
    }
}