using System;

namespace Science.Mathematics.OrdinaryDifferentialEquation
{
	/// <summary>
	/// Ordinary Differential Equation is solved.
	/// </summary>
	public class EquationWithInitialCondition
	{
		private Function.ToLastType<double,double[],double[]> fdf;
        private InitialCondition ic;
        private double to;
        private int hm;        
		public EquationWithInitialCondition(Function.ToLastType<double, double[], double[]> firstDerivativeFunction,
            InitialCondition initialCondition, double interval, int howMany)
		{
            fdf = firstDerivativeFunction;
            ic = initialCondition;
            to = ic.At + interval;
            hm = howMany;
		}

		public void Solve()
		{
            double[] y = ic.Value;
            double[] dydx = fdf(ic.At, y);

            StepperBase s = new StepperDopr5(ic.Value.Length);
            s.derivs = fdf;

            Output outt = new Output(hm);

            Odeint dd = new Odeint(y, ic.At, to, 0.00000001, 0.00000001, 0.000001, 0.0, outt, s);

            dd.integrate();
            x = outt.xsave;
            sol = outt.ysave;
		}

        private double[] x;
        public double[] IndependentVariable
        {
            get { return x; }
        } 
        private double[,] sol;
        public double[,] Solution
        {
            get { return sol; }
        } 		
	}


    // private class
    class Odeint
    {
        private int MAXSTP = 50000;
        private double EPS = 1.0e-6;
        private int nvar;
        private double x1, x2, hmin;
        private bool dense;
        private double[] y, dydx;
        private double[] ystart;
        private Output Out;
        private StepperBase s;
        private int nstp;
        private double x, h;

        public int nok;
        public int nbad;

        public Odeint(double[] ystartt, double xx1, double xx2, double atol, double rtol, double h1,
            double hminn, Output outt, StepperBase s1)
        {
            nvar = ystartt.Length;
            y = new Double[nvar];
            dydx = new Double[nvar];
            ystart = new Double[nvar];
            for (int i = 0; i < nvar; i++) ystart[i] = ystartt[i];
            x = xx1;
            nok = 0;
            nbad = 0;
            x1 = xx1;
            x2 = xx2;
            hmin = hminn;
            dense = outt.dense;
            Out = outt;
            s1.atol = atol;
            s1.rtol = rtol;
            s1.dense = outt.dense;
            s = s1;
            h = SIGN(h1, x2 - x1);
            for (int i = 0; i < nvar; i++) y[i] = ystart[i];
            Out.init(s1.neqn, x1, x2);
        }
        private double SIGN(double a, double b)
        {
            return ((b) >= 0.0 ? Math.Abs(a) : -Math.Abs(a));
        }

        public void integrate()
        {
            dydx = s.derivs(x, y);
            if (dense) Out.Out(-1, x, y, s, h);
            else Out.save(x, y);
            for (nstp = 0; nstp < MAXSTP; nstp++)
            {
                if ((x + h * 1.0001 - x2) * (x2 - x1) > 0.0) h = x2 - x;
                s.step(ref x, y, dydx, h);
                if (s.hdid == h) ++nok; else ++nbad;
                if (dense) Out.Out(nstp, x, y, s, s.hdid);
                else Out.save(x, y);
                if ((x - x2) * (x2 - x1) >= 0.0)
                {
                    for (int i = 0; i < nvar; i++) ystart[i] = y[i];
                    if (Out.kmax > 0 && Math.Abs(Out.xsave[Out.count - 1] - x2) >
                        100.0 * Math.Abs(x2) * EPS) Out.save(x, y);
                    return;
                }
                if (Math.Abs(s.hnext) <= hmin)
                    try { throw new Science.Error(); }
                    catch (Science.Error e)
                    {
                        e.Write("Step size too small in Odeint");
                    }
                h = s.hnext;
            }
            try { throw new Science.Error(); }
            catch (Science.Error e)
            {
                e.Write("Too many steps in routine Odeint");
            }
        }
    }

    class Output
    {
        public bool dense;
        public int count;
        public int kmax;
        public double[] xsave;
        public double[,] ysave;

        private int nvar;
        private int nsave;
        private double x1, x2, xout, dxout;

        public Output()
        {
            kmax = -1;
            dense = false;
            count = 0;
        }
        public Output(int nsavee)
        {
            kmax = 500;
            nsave = nsavee;
            count = 0;
            xsave = new double[kmax];
            dense = (nsave > 0 ? true : false);
        }

        public void init(int neqn, double xlo, double xhi)
        {
            nvar = neqn;
            if (kmax == -1) return;
            ysave = new double[nvar, kmax];
            if (dense)
            {
                x1 = xlo;
                x2 = xhi;
                xout = x1;
                dxout = (x2 - x1) / nsave;
            }
        }

        private void resize()
        {
            int kold = kmax;
            kmax *= 2;
            double[] tempvec = new double[xsave.Length];
            for (int i = 0; i < xsave.Length; i++) tempvec[i] = xsave[i];
            xsave = new double[kmax];
            for (int i = 0; i < kold; i++) xsave[i] = tempvec[i];
            double[,] tempmat = new double[ysave.GetLength(0), ysave.GetLength(1)];
            for (int i = 0; i < ysave.GetLength(0); i++)
                for (int j = 0; j < ysave.GetLength(1); j++) tempmat[i, j] = ysave[i, j];
            ysave = new double[nvar, kmax];
            for (int i = 0; i < nvar; i++)
                for (int j = 0; j < kold; j++) ysave[i, j] = tempmat[i, j];
        }

        private void save_dense(StepperBase s, double xout, double h)
        {
            if (count == kmax) resize();
            for (int i = 0; i < nvar; i++)
                ysave[i, count] = s.dense_out(i, xout, h);
            xsave[count++] = xout;
        }

        public void save(double x, double[] y)
        {
            if (kmax <= 0) return;
            if (count == kmax) resize();
            for (int i = 0; i < nvar; i++) ysave[i, count] = y[i];
            xsave[count++] = x;
        }

        public void Out(int nstp, double x, double[] y, StepperBase s, double h)
        {
            if (!dense) try { throw new Science.Error(); }
                catch (Science.Error e)
                {
                    e.Write("dense output not set in Output!");
                } 
            if (nstp == -1)
            {
                save(x, y);
                xout += dxout;
            }
            else
            {
                while ((x - xout) * (x2 - x1) > 0.0)
                {
                    save_dense(s, xout, h);
                    xout += dxout;
                }
            }
        }
    }

    class StepperBase
    {
        public double atol, rtol;
        public bool dense;
        public double hdid;
        public double hnext;
        public Function.ToLastType<double, double[], double[]> derivs;
//        public Function.ToLastType<double, double[], double[]> partial;  // stiff case
//        public Function.ToLastType<double, double[], double[,]> jacobian; // stiff case

        protected double EPS = 1.0e-6;
        protected double xold;
        protected double[] yout, yerr;
        protected int n;
        public int neqn;

        public StepperBase(int nvar)
        {
            n = nvar;
        }
        public virtual void step(ref double x, double[] y, double[] dydx, double htry)
        {
        }
        public virtual double dense_out(int i, double x, double h)
        {
            return 0.0;
        }
    }

    class StepperDopr5 : StepperBase
    {
        double[] k2, k3, k4, k5, k6;
        double[] rcont1, rcont2, rcont3, rcont4, rcont5;
        double[] dydxnew;
        public StepperDopr5(int nvar)
            : base(nvar)
        {
            neqn = n;
            yout = new double[n];
            yerr = new double[n];
            k2 = new double[n];
            k3 = new double[n];
            k4 = new double[n];
            k5 = new double[n];
            k6 = new double[n];
            rcont1 = new double[n];
            rcont2 = new double[n];
            rcont3 = new double[n];
            rcont4 = new double[n];
            rcont5 = new double[n];
            dydxnew = new double[n];
        }
        public override void step(ref double x, double[] y, double[] dydx, double htry)
        {
            double h = htry;
            for (; ; )
            {
                dy(x, y, dydx, h);
                double err = error(y);
                if (success(err, ref h)) break;
                if (Math.Abs(h) <= Math.Abs(x) * EPS) try { throw new Science.Error(); }
                    catch (Science.Error e)
                    {
                        e.Write("stepsize underflow in Stepperdopr5");
                    } 
            }
            if (dense) prepare_dense(y, dydx, h);
            for (int i = 0; i < n; i++)
            {
                dydx[i] = dydxnew[i];
                y[i] = yout[i];
            }
            xold = x;
            x += (hdid = h);
            hnext = chnext;
        }

        private void dy(double x, double[] y, double[] dydx, double h)
        {
            int i, n = y.Length;
            double c2 = 0.2, c3 = 0.3, c4 = 0.8, c5 = 8.0 / 9.0,
                       a21 = 0.2,
                       a31 = 3.0 / 40.0, a32 = 9.0 / 40.0,
                       a41 = 44.0 / 45.0, a42 = -56.0 / 15.0, a43 = 32.0 / 9.0,
                       a51 = 19372.0 / 6561.0, a52 = -25360 / 2187.0, a53 = 64448.0 / 6561.0, a54 = -212.0 / 729.0,
                       a61 = 9017.0 / 3168.0, a62 = -355.0 / 33.0, a63 = 46732.0 / 5247.0, a64 = 49.0 / 176.0, a65 = -5103.0 / 18656.0,
                       a71 = 35.0 / 384.0, a73 = 500.0 / 1113.0, a74 = 125.0 / 192.0, a75 = -2187.0 / 6784.0, a76 = 11.0 / 84.0,
                       e1 = 71.0 / 57600.0, e3 = -71.0 / 16695.0, e4 = 71.0 / 1920.0, e5 = -17253.0 / 339200.0, e6 = 22.0 / 525.0, e7 = -1.0 / 40.0;

            double[] ytemp = new Double[n];

            for (i = 0; i < n; i++)
                ytemp[i] = y[i] + h * a21 * dydx[i];
            k2 = derivs(x + c2 * h, ytemp);
            for (i = 0; i < n; i++)
                ytemp[i] = y[i] + h * (a31 * dydx[i] + a32 * k2[i]);
            k3 = derivs(x + c3 * h, ytemp);
            for (i = 0; i < n; i++)
                ytemp[i] = y[i] + h * (a41 * dydx[i] + a42 * k2[i] + a43 * k3[i]);
            k4 = derivs(x + c4 * h, ytemp);
            for (i = 0; i < n; i++)
                ytemp[i] = y[i] + h * (a51 * dydx[i] + a52 * k2[i] + a53 * k3[i] + a54 * k4[i]);
            k5 = derivs(x + c5 * h, ytemp);
            for (i = 0; i < n; i++)
                ytemp[i] = y[i] + h * (a61 * dydx[i] + a62 * k2[i] + a63 * k3[i] + a64 * k4[i] + a65 * k5[i]);
            double xph = x + h;
            k6 = derivs(xph, ytemp);
            for (i = 0; i < n; i++)
                yout[i] = y[i] + h * (a71 * dydx[i] + a73 * k3[i] + a74 * k4[i] + a75 * k5[i] + a76 * k6[i]);
            dydxnew = derivs(xph, yout);
            for (i = 0; i < n; i++)
                yerr[i] = h * (e1 * dydx[i] + e3 * k3[i] + e4 * k4[i] + e5 * k5[i] + e6 * k6[i] + e7 * dydxnew[i]);
        }

        private void prepare_dense(double[] y, double[] dydx, double h)
        {
            int n = y.Length;
            double[] ytemp = new double[n];
            double d1 = -12715105075.0 / 11282082432.0;
            double d3 = 87487479700.0 / 32700410799.0;
            double d4 = -10690763975.0 / 1880347072.0;
            double d5 = 701980252875.0 / 199316789632.0;
            double d6 = -1453857185.0 / 822651844.0;
            double d7 = 69997945.0 / 29380423.0;
            for (int i = 0; i < n; i++)
            {
                rcont1[i] = y[i];
                double ydiff = yout[i] - y[i];
                rcont2[i] = ydiff;
                double bspl = h * dydx[i] - ydiff;
                rcont3[i] = bspl;
                rcont4[i] = ydiff - h * dydxnew[i] - bspl;
                rcont5[i] = h * (d1 * dydx[i] + d3 * k3[i] + d4 * k4[i] + d5 * k5[i] + d6 * k6[i] + d7 * dydxnew[i]);
            }
        }

        public override double dense_out(int i, double x, double h)
        {
            double s = (x - xold) / h;
            double s1 = 1.0 - s;
            return rcont1[i] + s * (rcont2[i] + s1 * (rcont3[i] + s * (rcont4[i] + s1 * rcont5[i])));
        }

        private double error(double[] y)
        {
            int n = y.Length;
            double err = 0.0, sk;
            for (int i = 0; i < n; i++)
            {
                sk = atol + rtol * Math.Max(Math.Abs(y[i]), Math.Abs(yout[i]));
                err += yerr[i] / sk * yerr[i] / sk;
            }
            return Math.Sqrt(err / n);
        }

        private double chnext, errold = 1.0e-4;
        private bool reject = false;
        private bool success(double err, ref double h)
        {
            double beta = 0.0, alpha = 0.2 - beta * 0.75, safe = 0.9;
            double minscale = 0.2, maxscale = 10.0;
            double scale;
            if (err <= 1.0)
            {
                if (err == 0.0) scale = maxscale;
                else
                {
                    scale = safe * Math.Pow(err, -alpha) * Math.Pow(errold, beta);
                    if (scale < minscale) scale = minscale;
                    if (scale > maxscale) scale = maxscale;
                }
                if (reject) chnext = h * Math.Min(scale, 1.0);
                else chnext = h * scale;
                errold = Math.Max(err, 1.0e-4);
                reject = false;
                return true;
            }
            else
            {
                scale = Math.Max(safe * Math.Pow(err, -alpha), minscale);
                h *= scale;
                reject = true;
                return false;
            }
        }
    }
}