source: trunk/sources/ALGLIB/odesolver.cs @ 2636

/*************************************************************************
Copyright 2009 by Sergey Bochkanov (ALGLIB project).

>>> SOURCE LICENSE >>>
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation (www.fsf.org); either version 2 of the
License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

A copy of the GNU General Public License is available at
http://www.fsf.org/licensing/licenses

>>> END OF LICENSE >>>
*************************************************************************/

using System;

namespace alglib
{
    public class odesolver
    {
        public struct odesolverstate
        {
            public int n;
            public int m;
            public double xscale;
            public double h;
            public double eps;
            public bool fraceps;
            public double[] yc;
            public double[] escale;
            public double[] xg;
            public int solvertype;
            public double x;
            public double[] y;
            public double[] dy;
            public double[,] ytbl;
            public int repterminationtype;
            public int repnfev;
            public double[] yn;
            public double[] yns;
            public double[] rka;
            public double[] rkc;
            public double[] rkcs;
            public double[,] rkb;
            public double[,] rkk;
            public AP.rcommstate rstate;
        };


        public struct odesolverreport
        {
            public int nfev;
            public int terminationtype;
        };




        public const double odesolvermaxgrow = 3.0;
        public const double odesolvermaxshrink = 10.0;


        /*************************************************************************
        Cash-Karp adaptive ODE solver.

        This subroutine solves ODE  Y'=f(Y,x)  with  initial  conditions  Y(xs)=Ys
        (here Y may be single variable or vector of N variables).

        INPUT PARAMETERS:
            Y       -   initial conditions, array[0..N-1].
                        contains values of Y[] at X[0]
            N       -   system size
            X       -   points at which Y should be tabulated, array[0..M-1]
                        integrations starts at X[0], ends at X[M-1],  intermediate
                        values at X[i] are returned too.
                        SHOULD BE ORDERED BY ASCENDING OR BY DESCENDING!!!!
            M       -   number of intermediate points + first point + last point:
                        * M>2 means that you need both Y(X[M-1]) and M-2 values at
                          intermediate points
                        * M=2 means that you want just to integrate from  X[0]  to
                          X[1] and don't interested in intermediate values.
                        * M=1 means that you don't want to integrate :)
                          it is degenerate case, but it will be handled correctly.
                        * M<1 means error
            Eps     -   tolerance (absolute/relative error on each  step  will  be
                        less than Eps). When passing:
                        * Eps>0, it means desired ABSOLUTE error
                        * Eps<0, it means desired RELATIVE error.  Relative errors
                          are calculated with respect to maximum values of  Y seen
                          so far. Be careful to use this criterion  when  starting
                          from Y[] that are close to zero.
            H       -   initial  step  lenth,  it  will  be adjusted automatically
                        after the first  step.  If  H=0,  step  will  be  selected
                        automatically  (usualy  it  will  be  equal  to  0.001  of
                        min(x[i]-x[j])).

        OUTPUT PARAMETERS
            State   -   structure which stores algorithm state between  subsequent
                        calls of OdeSolverIteration. Used for reverse communication.
                        This structure should be passed  to the OdeSolverIteration
                        subroutine.

        SEE ALSO
            AutoGKSmoothW, AutoGKSingular, AutoGKIteration, AutoGKResults.


          -- ALGLIB --
             Copyright 01.09.2009 by Bochkanov Sergey
        *************************************************************************/
        public static void odesolverrkck(ref double[] y,
            int n,
            ref double[] x,
            int m,
            double eps,
            double h,
            ref odesolverstate state)
        {
            odesolverinit(0, ref y, n, ref x, m, eps, h, ref state);
        }


        /*************************************************************************
        One iteration of ODE solver.

        Called after inialization of State structure with OdeSolverXXX subroutine.
        See HTML docs for examples.

        INPUT PARAMETERS:
            State   -   structure which stores algorithm state between subsequent
                        calls and which is used for reverse communication. Must be
                        initialized with OdeSolverXXX() call first.

        If subroutine returned False, algorithm have finished its work.
        If subroutine returned True, then user should:
        * calculate F(State.X, State.Y)
        * store it in State.DY
        Here State.X is real, State.Y and State.DY are arrays[0..N-1] of reals.

          -- ALGLIB --
             Copyright 01.09.2009 by Bochkanov Sergey
        *************************************************************************/
        public static bool odesolveriteration(ref odesolverstate state)
        {
            bool result = new bool();
            int n = 0;
            int m = 0;
            int i = 0;
            int j = 0;
            int k = 0;
            double xc = 0;
            double v = 0;
            double h = 0;
            double h2 = 0;
            bool gridpoint = new bool();
            double err = 0;
            double maxgrowpow = 0;
            int klimit = 0;
            int i_ = 0;

            
            //
            // Reverse communication preparations
            // I know it looks ugly, but it works the same way
            // anywhere from C++ to Python.
            //
            // This code initializes locals by:
            // * random values determined during code
            //   generation - on first subroutine call
            // * values from previous call - on subsequent calls
            //
            if( state.rstate.stage>=0 )
            {
                n = state.rstate.ia[0];
                m = state.rstate.ia[1];
                i = state.rstate.ia[2];
                j = state.rstate.ia[3];
                k = state.rstate.ia[4];
                klimit = state.rstate.ia[5];
                gridpoint = state.rstate.ba[0];
                xc = state.rstate.ra[0];
                v = state.rstate.ra[1];
                h = state.rstate.ra[2];
                h2 = state.rstate.ra[3];
                err = state.rstate.ra[4];
                maxgrowpow = state.rstate.ra[5];
            }
            else
            {
                n = -983;
                m = -989;
                i = -834;
                j = 900;
                k = -287;
                klimit = 364;
                gridpoint = false;
                xc = -338;
                v = -686;
                h = 912;
                h2 = 585;
                err = 497;
                maxgrowpow = -271;
            }
            if( state.rstate.stage==0 )
            {
                goto lbl_0;
            }
            
            //
            // Routine body
            //
            
            //
            // prepare
            //
            if( state.repterminationtype!=0 )
            {
                result = false;
                return result;
            }
            n = state.n;
            m = state.m;
            h = state.h;
            state.y = new double[n];
            state.dy = new double[n];
            maxgrowpow = Math.Pow(odesolvermaxgrow, 5);
            state.repnfev = 0;
            
            //
            // some preliminary checks for internal errors
            // after this we assume that H>0 and M>1
            //
            System.Diagnostics.Debug.Assert((double)(state.h)>(double)(0), "ODESolver: internal error");
            System.Diagnostics.Debug.Assert(m>1, "ODESolverIteration: internal error");
            
            //
            // choose solver
            //
            if( state.solvertype!=0 )
            {
                goto lbl_1;
            }
            
            //
            // Cask-Karp solver
            // Prepare coefficients table.
            // Check it for errors
            //
            state.rka = new double[6];
            state.rka[0] = 0;
            state.rka[1] = (double)(1)/(double)(5);
            state.rka[2] = (double)(3)/(double)(10);
            state.rka[3] = (double)(3)/(double)(5);
            state.rka[4] = 1;
            state.rka[5] = (double)(7)/(double)(8);
            state.rkb = new double[6, 5];
            state.rkb[1,0] = (double)(1)/(double)(5);
            state.rkb[2,0] = (double)(3)/(double)(40);
            state.rkb[2,1] = (double)(9)/(double)(40);
            state.rkb[3,0] = (double)(3)/(double)(10);
            state.rkb[3,1] = -((double)(9)/(double)(10));
            state.rkb[3,2] = (double)(6)/(double)(5);
            state.rkb[4,0] = -((double)(11)/(double)(54));
            state.rkb[4,1] = (double)(5)/(double)(2);
            state.rkb[4,2] = -((double)(70)/(double)(27));
            state.rkb[4,3] = (double)(35)/(double)(27);
            state.rkb[5,0] = (double)(1631)/(double)(55296);
            state.rkb[5,1] = (double)(175)/(double)(512);
            state.rkb[5,2] = (double)(575)/(double)(13824);
            state.rkb[5,3] = (double)(44275)/(double)(110592);
            state.rkb[5,4] = (double)(253)/(double)(4096);
            state.rkc = new double[6];
            state.rkc[0] = (double)(37)/(double)(378);
            state.rkc[1] = 0;
            state.rkc[2] = (double)(250)/(double)(621);
            state.rkc[3] = (double)(125)/(double)(594);
            state.rkc[4] = 0;
            state.rkc[5] = (double)(512)/(double)(1771);
            state.rkcs = new double[6];
            state.rkcs[0] = (double)(2825)/(double)(27648);
            state.rkcs[1] = 0;
            state.rkcs[2] = (double)(18575)/(double)(48384);
            state.rkcs[3] = (double)(13525)/(double)(55296);
            state.rkcs[4] = (double)(277)/(double)(14336);
            state.rkcs[5] = (double)(1)/(double)(4);
            state.rkk = new double[6, n];
            
            //
            // Main cycle consists of two iterations:
            // * outer where we travel from X[i-1] to X[i]
            // * inner where we travel inside [X[i-1],X[i]]
            //
            state.ytbl = new double[m, n];
            state.escale = new double[n];
            state.yn = new double[n];
            state.yns = new double[n];
            xc = state.xg[0];
            for(i_=0; i_<=n-1;i_++)
            {
                state.ytbl[0,i_] = state.yc[i_];
            }
            for(j=0; j<=n-1; j++)
            {
                state.escale[j] = 0;
            }
            i = 1;
        lbl_3:
            if( i>m-1 )
            {
                goto lbl_5;
            }
            
            //
            // begin inner iteration
            //
        lbl_6:
            if( false )
            {
                goto lbl_7;
            }
            
            //
            // truncate step if needed (beyond right boundary).
            // determine should we store X or not
            //
            if( (double)(xc+h)>=(double)(state.xg[i]) )
            {
                h = state.xg[i]-xc;
                gridpoint = true;
            }
            else
            {
                gridpoint = false;
            }
            
            //
            // Update error scale maximums
            //
            // These maximums are initialized by zeros,
            // then updated every iterations.
            //
            for(j=0; j<=n-1; j++)
            {
                state.escale[j] = Math.Max(state.escale[j], Math.Abs(state.yc[j]));
            }
            
            //
            // make one step:
            // 1. calculate all info needed to do step
            // 2. update errors scale maximums using values/derivatives
            //    obtained during (1)
            //
            // Take into account that we use scaling of X to reduce task
            // to the form where x[0] < x[1] < ... < x[n-1]. So X is
            // replaced by x=xscale*t, and dy/dx=f(y,x) is replaced
            // by dy/dt=xscale*f(y,xscale*t).
            //
            for(i_=0; i_<=n-1;i_++)
            {
                state.yn[i_] = state.yc[i_];
            }
            for(i_=0; i_<=n-1;i_++)
            {
                state.yns[i_] = state.yc[i_];
            }
            k = 0;
        lbl_8:
            if( k>5 )
            {
                goto lbl_10;
            }
            
            //
            // prepare data for the next update of YN/YNS
            //
            state.x = state.xscale*(xc+state.rka[k]*h);
            for(i_=0; i_<=n-1;i_++)
            {
                state.y[i_] = state.yc[i_];
            }
            for(j=0; j<=k-1; j++)
            {
                v = state.rkb[k,j];
                for(i_=0; i_<=n-1;i_++)
                {
                    state.y[i_] = state.y[i_] + v*state.rkk[j,i_];
                }
            }
            state.rstate.stage = 0;
            goto lbl_rcomm;
        lbl_0:
            state.repnfev = state.repnfev+1;
            v = h*state.xscale;
            for(i_=0; i_<=n-1;i_++)
            {
                state.rkk[k,i_] = v*state.dy[i_];
            }
            
            //
            // update YN/YNS
            //
            v = state.rkc[k];
            for(i_=0; i_<=n-1;i_++)
            {
                state.yn[i_] = state.yn[i_] + v*state.rkk[k,i_];
            }
            v = state.rkcs[k];
            for(i_=0; i_<=n-1;i_++)
            {
                state.yns[i_] = state.yns[i_] + v*state.rkk[k,i_];
            }
            k = k+1;
            goto lbl_8;
        lbl_10:
            
            //
            // estimate error
            //
            err = 0;
            for(j=0; j<=n-1; j++)
            {
                if( !state.fraceps )
                {
                    
                    //
                    // absolute error is estimated
                    //
                    err = Math.Max(err, Math.Abs(state.yn[j]-state.yns[j]));
                }
                else
                {
                    
                    //
                    // Relative error is estimated
                    //
                    v = state.escale[j];
                    if( (double)(v)==(double)(0) )
                    {
                        v = 1;
                    }
                    err = Math.Max(err, Math.Abs(state.yn[j]-state.yns[j])/v);
                }
            }
            
            //
            // calculate new step, restart if necessary
            //
            if( (double)(maxgrowpow*err)<=(double)(state.eps) )
            {
                h2 = odesolvermaxgrow*h;
            }
            else
            {
                h2 = h*Math.Pow(state.eps/err, 0.2);
            }
            if( (double)(h2)<(double)(h/odesolvermaxshrink) )
            {
                h2 = h/odesolvermaxshrink;
            }
            if( (double)(err)>(double)(state.eps) )
            {
                h = h2;
                goto lbl_6;
            }
            
            //
            // advance position
            //
            xc = xc+h;
            for(i_=0; i_<=n-1;i_++)
            {
                state.yc[i_] = state.yn[i_];
            }
            
            //
            // update H
            //
            h = h2;
            
            //
            // break on grid point
            //
            if( gridpoint )
            {
                goto lbl_7;
            }
            goto lbl_6;
        lbl_7:
            
            //
            // save result
            //
            for(i_=0; i_<=n-1;i_++)
            {
                state.ytbl[i,i_] = state.yc[i_];
            }
            i = i+1;
            goto lbl_3;
        lbl_5:
            state.repterminationtype = 1;
            result = false;
            return result;
        lbl_1:
            result = false;
            return result;
            
            //
            // Saving state
            //
        lbl_rcomm:
            result = true;
            state.rstate.ia[0] = n;
            state.rstate.ia[1] = m;
            state.rstate.ia[2] = i;
            state.rstate.ia[3] = j;
            state.rstate.ia[4] = k;
            state.rstate.ia[5] = klimit;
            state.rstate.ba[0] = gridpoint;
            state.rstate.ra[0] = xc;
            state.rstate.ra[1] = v;
            state.rstate.ra[2] = h;
            state.rstate.ra[3] = h2;
            state.rstate.ra[4] = err;
            state.rstate.ra[5] = maxgrowpow;
            return result;
        }


        /*************************************************************************
        ODE solver results

        Called after OdeSolverIteration returned False.

        INPUT PARAMETERS:
            State   -   algorithm state (used by OdeSolverIteration).

        OUTPUT PARAMETERS:
            M       -   number of tabulated values, M>=1
            XTbl    -   array[0..M-1], values of X
            YTbl    -   array[0..M-1,0..N-1], values of Y in X[i]
            Rep     -   solver report:
                        * Rep.TerminationType completetion code:
                            * -2    X is not ordered  by  ascending/descending  or
                                    there are non-distinct X[],  i.e.  X[i]=X[i+1]
                            * -1    incorrect parameters were specified
                            *  1    task has been solved
                        * Rep.NFEV contains number of function calculations

          -- ALGLIB --
             Copyright 01.09.2009 by Bochkanov Sergey
        *************************************************************************/
        public static void odesolverresults(ref odesolverstate state,
            ref int m,
            ref double[] xtbl,
            ref double[,] ytbl,
            ref odesolverreport rep)
        {
            double v = 0;
            int i = 0;
            int i_ = 0;

            rep.terminationtype = state.repterminationtype;
            if( rep.terminationtype>0 )
            {
                m = state.m;
                rep.nfev = state.repnfev;
                xtbl = new double[state.m];
                v = state.xscale;
                for(i_=0; i_<=state.m-1;i_++)
                {
                    xtbl[i_] = v*state.xg[i_];
                }
                ytbl = new double[state.m, state.n];
                for(i=0; i<=state.m-1; i++)
                {
                    for(i_=0; i_<=state.n-1;i_++)
                    {
                        ytbl[i,i_] = state.ytbl[i,i_];
                    }
                }
            }
            else
            {
                rep.nfev = 0;
            }
        }


        /*************************************************************************
        Internal initialization subroutine
        *************************************************************************/
        private static void odesolverinit(int solvertype,
            ref double[] y,
            int n,
            ref double[] x,
            int m,
            double eps,
            double h,
            ref odesolverstate state)
        {
            int i = 0;
            double v = 0;
            int i_ = 0;

            
            //
            // Prepare RComm
            //
            state.rstate.ia = new int[5+1];
            state.rstate.ba = new bool[0+1];
            state.rstate.ra = new double[5+1];
            state.rstate.stage = -1;
            
            //
            // check parameters.
            //
            if( n<=0 | m<1 | (double)(eps)==(double)(0) )
            {
                state.repterminationtype = -1;
                return;
            }
            if( (double)(h)<(double)(0) )
            {
                h = -h;
            }
            
            //
            // quick exit if necessary.
            // after this block we assume that M>1
            //
            if( m==1 )
            {
                state.repnfev = 0;
                state.repterminationtype = 1;
                state.ytbl = new double[1, n];
                for(i_=0; i_<=n-1;i_++)
                {
                    state.ytbl[0,i_] = y[i_];
                }
                state.xg = new double[m];
                for(i_=0; i_<=m-1;i_++)
                {
                    state.xg[i_] = x[i_];
                }
                return;
            }
            
            //
            // check again: correct order of X[]
            //
            if( (double)(x[1])==(double)(x[0]) )
            {
                state.repterminationtype = -2;
                return;
            }
            for(i=1; i<=m-1; i++)
            {
                if( (double)(x[1])>(double)(x[0]) & (double)(x[i])<=(double)(x[i-1]) | (double)(x[1])<(double)(x[0]) & (double)(x[i])>=(double)(x[i-1]) )
                {
                    state.repterminationtype = -2;
                    return;
                }
            }
            
            //
            // auto-select H if necessary
            //
            if( (double)(h)==(double)(0) )
            {
                v = Math.Abs(x[1]-x[0]);
                for(i=2; i<=m-1; i++)
                {
                    v = Math.Min(v, Math.Abs(x[i]-x[i-1]));
                }
                h = 0.001*v;
            }
            
            //
            // store parameters
            //
            state.n = n;
            state.m = m;
            state.h = h;
            state.eps = Math.Abs(eps);
            state.fraceps = (double)(eps)<(double)(0);
            state.xg = new double[m];
            for(i_=0; i_<=m-1;i_++)
            {
                state.xg[i_] = x[i_];
            }
            if( (double)(x[1])>(double)(x[0]) )
            {
                state.xscale = 1;
            }
            else
            {
                state.xscale = -1;
                for(i_=0; i_<=m-1;i_++)
                {
                    state.xg[i_] = -1*state.xg[i_];
                }
            }
            state.yc = new double[n];
            for(i_=0; i_<=n-1;i_++)
            {
                state.yc[i_] = y[i_];
            }
            state.solvertype = solvertype;
            state.repterminationtype = 0;
        }
    }
}