doptest.f90

program doptest

implicit none

double precision :: x
double precision :: xend
double precision, dimension(1) :: rtol
double precision, dimension(1) :: atol
double precision, dimension(6) :: y
double precision, dimension(6) :: y1
double precision, dimension(6) :: f
double precision, dimension(1) :: rpar
double precision, dimension(200) :: work
integer, dimension(100) :: iwork
integer, dimension(1) :: ipar
integer :: n
integer :: itol
integer :: iout
integer :: idid
integer :: lwork
integer :: liwork

interface
    subroutine dop853(n, fcn, x, y, xend, rtol, atol,&
            itol, solout, iout, work, lwork, iwork,&
            liwork, rpar, ipar, idid)
        interface
            subroutine fcn(n, x, y, f, rpar, ipar)
                integer, intent(in) :: n
                integer, dimension(:),intent(inout) :: ipar
                double precision, intent(in) :: x
                double precision, dimension(n), intent(in) :: y
                double precision, dimension(:), intent(inout) :: rpar
                double precision, dimension(n), intent(out) :: f
            end subroutine fcn
            subroutine solout(nr, xold, x, y, n, con, icomp,&
                    nd, rpar, ipar, irtrn, xout)
                integer, intent(in) :: n
                integer, intent(in) :: nr
                integer, intent(in) :: nd
                integer, intent(in) :: irtrn
                integer, dimension(:), intent(inout) :: ipar
                integer, dimension(nd), intent(in) :: icomp
                double precision, intent(in) :: xold
                double precision, intent(in) :: x
                double precision, dimension(n), intent(in) :: y
                double precision, dimension(8*nd), intent(in) :: con
                double precision, dimension(:), intent(inout) :: rpar
                double precision, intent(inout) :: xout
            end subroutine solout
        end interface
        integer, intent(in) :: n
        integer, intent(in) :: itol
        integer, intent(in) :: iout
        integer, intent(in) :: lwork
        integer, intent(in) :: liwork
        integer, dimension(:),intent(inout) :: ipar
        integer, intent(out) :: idid
        double precision, intent(in) :: xend
        double precision, dimension(n), intent(in) :: rtol
        double precision, dimension(n), intent(in) :: atol
        double precision, dimension(:), intent(inout) :: rpar
        double precision, dimension(lwork), intent(inout) :: work
        integer, dimension(liwork), intent(inout) :: iwork
        double precision, intent(inout) :: x
        double precision, dimension(n), intent(inout) :: y
    end subroutine dop853

    double precision function contd8(ii, x, con, icomp, nd)
        integer, intent(in) :: ii
        double precision, intent(in) :: x
        double precision, dimension(8*nd), intent(in) :: con
        integer, dimension(nd), intent(in) :: icomp
        integer, intent(in) :: nd
    end function contd8
end interface

integer :: c1, c2, cr, m, i
double precision :: rate, tc

call system_clock(count_rate=cr)
rate = dble(cr)
rpar = 398600.4415d0
y = [-1814.0d0, -3708.0d0, 5153.0d0, 6.512d0, -4.229d0, -0.744d0]
y1 = y
xend = 5402.582703094263d0
x = 0d0
n = 6
itol = 0
iout = 0
atol = 1.4901161193847656d-8
rtol = 1d-6
lwork = 200
liwork = 100
work = 0d0
iwork = 0
tc = 0d0
m = 10000

do i=1,m
    call system_clock(c1)
    call dop853(n,newton,x,y,xend,rtol,atol,itol,solout,iout,work,lwork,iwork,liwork,rpar,ipar,idid) 
    call system_clock(c2)
    tc = tc + (c2-c1)/rate
    y = y1
    x = 0
end do

print *, "Time per loop:", tc/dble(m)

contains
    subroutine newton(n,x,y,f,rpar,ipar)
        integer, intent(in) :: n
        integer, dimension(:),intent(inout) :: ipar
        double precision, intent(in) :: x
        double precision, dimension(n), intent(in) :: y
        double precision, dimension(:), intent(inout) :: rpar
        double precision, dimension(n), intent(out) :: f
        
        double precision :: mu
        double precision :: r
        double precision :: r1

        mu = rpar(1)
        r = sqrt(sum(y(1:3)**2))
        f(1:3) = y(4:6)
        f(4:6) = -mu*y(1:3)/r**3
    end subroutine

    subroutine solout(nr, xold, x, y, n, con, icomp,&
        nd, rpar, ipar, irtrn, xout)
                integer, intent(in) :: n
                integer, intent(in) :: nr
                integer, intent(in) :: nd
                integer, intent(in) :: irtrn
                integer, dimension(:), intent(inout) :: ipar
                integer, dimension(nd), intent(in) :: icomp
                double precision, intent(in) :: xold
                double precision, intent(in) :: x
                double precision, dimension(n), intent(in) :: y
                double precision, dimension(8*nd), intent(in) :: con
                double precision, dimension(:), intent(inout) :: rpar
                double precision, intent(inout) :: xout
    end subroutine
end program

doptest.jl

function dop853(f::Function, x::Float64, yin::Vector{Float64}, xend::Float64, params::Any; rtol::Vector{Float64}=[1e-6], atol::Vector{Float64}=[sqrt(eps())],
    uround::Float64=2.3e-16, safe::Float64=0.9, fac1::Float64=0.333, fac2::Float64=6.0,
    beta::Float64=0.0, maxstepsize::Float64=xend-x, initialstep::Float64=0.0,
    maxsteps::Int64=100000, printmessages::Bool=false, nstiff::Int64=1000,
    iout::Int64=0, solout::Function=s(x...)=return, dense::Vector{Int64}=[1:length(yin)])

    c14 = 0.1e+00
    c15 = 0.2e+00
    c16 = 0.777777777777777777777777777778e+00
    d41  = -0.84289382761090128651353491142e+01
    d46  =  0.56671495351937776962531783590e+00
    d47  = -0.30689499459498916912797304727e+01
    d48  =  0.23846676565120698287728149680e+01
    d49  =  0.21170345824450282767155149946e+01
    d410 = -0.87139158377797299206789907490e+00
    d411 =  0.22404374302607882758541771650e+01
    d412 =  0.63157877876946881815570249290e+00
    d413 = -0.88990336451333310820698117400e-01
    d414 =  0.18148505520854727256656404962e+02
    d415 = -0.91946323924783554000451984436e+01
    d416 = -0.44360363875948939664310572000e+01
    d51  =  0.10427508642579134603413151009e+02
    d56  =  0.24228349177525818288430175319e+03
    d57  =  0.16520045171727028198505394887e+03
    d58  = -0.37454675472269020279518312152e+03
    d59  = -0.22113666853125306036270938578e+02
    d510 =  0.77334326684722638389603898808e+01
    d511 = -0.30674084731089398182061213626e+02
    d512 = -0.93321305264302278729567221706e+01
    d513 =  0.15697238121770843886131091075e+02
    d514 = -0.31139403219565177677282850411e+02
    d515 = -0.93529243588444783865713862664e+01
    d516 =  0.35816841486394083752465898540e+02
    d61 =  0.19985053242002433820987653617e+02
    d66 = -0.38703730874935176555105901742e+03
    d67 = -0.18917813819516756882830838328e+03
    d68 =  0.52780815920542364900561016686e+03
    d69 = -0.11573902539959630126141871134e+02
    d610 =  0.68812326946963000169666922661e+01
    d611 = -0.10006050966910838403183860980e+01
    d612 =  0.77771377980534432092869265740e+00
    d613 = -0.27782057523535084065932004339e+01
    d614 = -0.60196695231264120758267380846e+02
    d615 =  0.84320405506677161018159903784e+02
    d616 =  0.11992291136182789328035130030e+02
    d71  = -0.25693933462703749003312586129e+02
    d76  = -0.15418974869023643374053993627e+03
    d77  = -0.23152937917604549567536039109e+03
    d78  =  0.35763911791061412378285349910e+03
    d79  =  0.93405324183624310003907691704e+02
    d710 = -0.37458323136451633156875139351e+02
    d711 =  0.10409964950896230045147246184e+03
    d712 =  0.29840293426660503123344363579e+02
    d713 = -0.43533456590011143754432175058e+02
    d714 =  0.96324553959188282948394950600e+02
    d715 = -0.39177261675615439165231486172e+02
    d716 = -0.14972683625798562581422125276e+03

    irtrn = 0

    n = length(yin)
    work = zeros(n,10)
    y = yin
    nrd = length(dense)
    nfcn = 0
    nstep = 0
    naccpt = 0
    nrejct = 0
    facold = 1e-4
    expo1 = 1.0/8.0 - beta*0.2
    facc1 = 1.0/fac1
    facc2 = 1.0/fac2
    posneg = sign(xend-x)
    atol = length(atol) == 1 ? fill(atol[1], n) : atol
    rtol = length(rtol) == 1 ? fill(rtol[1], n) : rtol
    last = false
    hlamb = 0.0
    iasti = 0
    #k1 = f(x, y, params)
    work[:,1] = f(x, y, params)
    nfcn += 1
    hmax = abs(maxstepsize)
    nmax = maxsteps
    h = initialstep
    iord = 8
    if h == 0.0
        #h = hinit(n, f, x, y, xend, posneg, k1, iord, hmax, atol, rtol, params)
        h = hinit(n, f, x, y, xend, posneg, work[:,1], iord, hmax, atol, rtol, params)
        nfcn += 1
    end
    reject = false
    xold = x
    if iout != 0
        #hout = 1.0
        irtrn = solout(naccpt+1, xold, x, y, con, icomp, params)
        if irtrn < 0
            return y
        end
    end
    while true
        if nstep > nmax
            error("Exit at x=$x. More than nmax=$nmax steps needed.")
        end
        if 0.1*abs(h) <= abs(x)*uround
            error("Exit at x=$x. Step size too small, h=$h.")
        end
        if (x+1.01*h-xend)*posneg > 0.0
            h = xend-x
            last = true
        end
        nstep += 1
        if irtrn >= 2
            k1 = f(x, y, params)
            work[:,1] = f(x, y, params)
        end

        y1, err = dopcorework(n, f, x, y, h, work, atol, rtol, params)
        #y1, k3, k4, k5, err = dopcorevec(n, f, x, y, h, k1, atol, rtol, params)
        #y1, k3, k4, k5, err = dopcore(n, f, x, y, h, k1, atol, rtol, params)
        xph = x+h
        nfcn += 11

        fac11 = err^expo1
        fac = fac11/facold^beta
        fac = max(facc2, min(facc1, fac/safe))
        hnew = h/fac
        if err <= 1.0
            facold = max(err, 1e-4)
            naccpt += 1
            #k4 = f(xph, k5, params)
            work[:,4] = f(xph, work[:,5], params)
            nfcn += 1
            # Stiffness detection
            if mod(naccpt, nstiff) == 0 || iasti > 0
                nonsti = 0
                #stnum = sum((k4 - k3).^2)
                #stden = sum((k5 - y1).^2)
                stnum = sum((work[:,4] - work[:,3]).^2)
                stden = sum((work[:,5] - y1).^2)
                if stden > 0.0
                    hlamb = abs(h)*sqrt(stnum/stden)
                end
                if hlamb > 6.1
                    nonsti = 0
                    iasti += 1
                    if iasti == 15
                        if printmessages
                            println("The problem seems to become stiff at x=$x")
                        end
                    end
                else
                    nonsti += 1
                    if nonsti == 6
                        iasti = 0
                    end
                end
            end
            # Final preparation for dense output
            event = iout == 3 && xout <= xph
            if iout == 2 || event
                for (i,j) in enumerate(dense)
                end
            end
            #k1 = k4
            work[:,1] = work[:,4]
            #y = k5
            y = work[:,5]
            xold = x
            x = xph
            # if
            # solout
            # end
            # Normal exit
            if last
                h = hnew
                idid = 1
                return y
            end
            if abs(hnew) > hmax
                hnew = posneg*hmax  
            end
            if reject 
                hnew = posneg*min(abs(hnew),abs(h))
            end
            reject = false 
        else
            hnew = h/min(facc1,fac11/safe)
            reject = true
            if naccpt >= 1
                nrejct += 1
            end
            last = false
        end
        h = hnew
    end
end
#end

function hinit(n::Int64, f::Function, x::Float64, y::Vector{Float64}, xend::Float64, posneg::Float64, f0::Vector{Float64}, iord::Int64, hmax::Float64, atol::Vector{Float64}, rtol::Vector{Float64}, params::Any)
    dnf = 0.0
    dny = 0.0
    for i = 1:n
        sk = atol[i] + rtol[i]*abs(y[i])
        dnf = dnf + (f0[i]/sk)^2
        dny = dny + (y[i]/sk)^2
    end
    if dnf <= 1e-10 || dny <= 1e-10
        h = 1e-6
    else
        h = sqrt(dny/dnf)*0.01
    end
    h = min(h, hmax)
    h = h*posneg
    y1 = y + h*f0
    f1 = f(x+h, y1, params)
    sk = atol + rtol.*abs(y)
    der2 = sum((f1./sk).^2)
    der2 = sqrt(der2)/h
    der12 = max(abs(der2), sqrt(dnf))
    if der12 <= 1e-15
        h1 = max(1e-6, abs(h)*1e-3)
    else
        h1 = (0.01/der12)^(1.0/iord)
    end
    h = min(100*abs(h), h1, hmax)
    return h*posneg
end

function dopcorework(n::Int64, f::Function, x::Float64, y::Vector{Float64}, h::Float64, work::Matrix{Float64}, atol::Vector{Float64}, rtol::Vector{Float64}, params::Any)
    a21 =    5.26001519587677318785587544488e-2
    a31 =    1.97250569845378994544595329183e-2
    a32 =    5.91751709536136983633785987549e-2
    a41 =    2.95875854768068491816892993775e-2
    a43 =    8.87627564304205475450678981324e-2
    a51 =    2.41365134159266685502369798665e-1
    a53 =   -8.84549479328286085344864962717e-1
    a54 =    9.24834003261792003115737966543e-1
    a61 =    3.7037037037037037037037037037e-2
    a64 =    1.70828608729473871279604482173e-1
    a65 =    1.25467687566822425016691814123e-1
    a71 =    3.7109375e-2
    a74 =    1.70252211019544039314978060272e-1
    a75 =    6.02165389804559606850219397283e-2
    a76 =   -1.7578125e-2
    a81 =    3.70920001185047927108779319836e-2
    a84 =    1.70383925712239993810214054705e-1
    a85 =    1.07262030446373284651809199168e-1
    a86 =   -1.53194377486244017527936158236e-2
    a87 =    8.27378916381402288758473766002e-3
    a91 =    6.24110958716075717114429577812e-1
    a94 =   -3.36089262944694129406857109825e0
    a95 =   -8.68219346841726006818189891453e-1
    a96 =    2.75920996994467083049415600797e1
    a97 =    2.01540675504778934086186788979e1
    a98 =   -4.34898841810699588477366255144e1
    a101 =   4.77662536438264365890433908527e-1
    a104 =  -2.48811461997166764192642586468e0
    a105 =  -5.90290826836842996371446475743e-1
    a106 =   2.12300514481811942347288949897e1
    a107 =   1.52792336328824235832596922938e1
    a108 =  -3.32882109689848629194453265587e1
    a109 =  -2.03312017085086261358222928593e-2
    a111 =  -9.3714243008598732571704021658e-1
    a114 =   5.18637242884406370830023853209e0
    a115 =   1.09143734899672957818500254654e0
    a116 =  -8.14978701074692612513997267357e0
    a117 =  -1.85200656599969598641566180701e1
    a118 =   2.27394870993505042818970056734e1
    a119 =   2.49360555267965238987089396762e0
    a1110 = -3.0467644718982195003823669022e0
    a121 =   2.27331014751653820792359768449e0
    a124 =  -1.05344954667372501984066689879e1
    a125 =  -2.00087205822486249909675718444e0
    a126 =  -1.79589318631187989172765950534e1
    a127 =   2.79488845294199600508499808837e1
    a128 =  -2.85899827713502369474065508674e0
    a129 =  -8.87285693353062954433549289258e0
    a1210 =  1.23605671757943030647266201528e1
    a1211 =  6.43392746015763530355970484046e-1
    a141 =  5.61675022830479523392909219681e-2
    a147 =  2.53500210216624811088794765333e-1
    a148 = -2.46239037470802489917441475441e-1
    a149 = -1.24191423263816360469010140626e-1
    a1410 =  1.5329179827876569731206322685e-1
    a1411 =  8.20105229563468988491666602057e-3
    a1412 =  7.56789766054569976138603589584e-3
    a1413 = -8.298e-3
    a151 =  3.18346481635021405060768473261e-2
    a156 =  2.83009096723667755288322961402e-2
    a157 =  5.35419883074385676223797384372e-2
    a158 = -5.49237485713909884646569340306e-2
    a1511 = -1.08347328697249322858509316994e-4
    a1512 =  3.82571090835658412954920192323e-4
    a1513 = -3.40465008687404560802977114492e-4
    a1514 =  1.41312443674632500278074618366e-1
    a161 = -4.28896301583791923408573538692e-1
    a166 = -4.69762141536116384314449447206e0
    a167 =  7.68342119606259904184240953878e0
    a168 =  4.06898981839711007970213554331e0
    a169 =  3.56727187455281109270669543021e-1
    a1613 = -1.39902416515901462129418009734e-3
    a1614 =  2.9475147891527723389556272149e0
    a1615 = -9.15095847217987001081870187138e0
    b1 =   5.42937341165687622380535766363e-2
    b6 =   4.45031289275240888144113950566e0
    b7 =   1.89151789931450038304281599044e0
    b8 =  -5.8012039600105847814672114227e0
    b9 =   3.1116436695781989440891606237e-1
    b10 = -1.52160949662516078556178806805e-1
    b11 =  2.01365400804030348374776537501e-1
    b12 =  4.47106157277725905176885569043e-2
    c2  = 0.526001519587677318785587544488e-01
    c3  = 0.789002279381515978178381316732e-01
    c4  = 0.118350341907227396726757197510e+00
    c5  = 0.281649658092772603273242802490e+00
    c6  = 0.333333333333333333333333333333e+00
    c7  = 0.25e+00
    c8  = 0.307692307692307692307692307692e+00
    c9  = 0.651282051282051282051282051282e+00
    c10 = 0.6e+00
    c11 = 0.857142857142857142857142857142e+00
    bhh1 = 0.244094488188976377952755905512e+00
    bhh2 = 0.733846688281611857341361741547e+00
    bhh3 = 0.220588235294117647058823529412e-01
    er1 =  0.1312004499419488073250102996e-01
    er6 = -0.1225156446376204440720569753e+01
    er7 = -0.4957589496572501915214079952e+00
    er8 =  0.1664377182454986536961530415e+01
    er9 = -0.3503288487499736816886487290e+00
    er10 =  0.3341791187130174790297318841e+00
    er11 =  0.8192320648511571246570742613e-01
    er12 = -0.2235530786388629525884427845e-01

    y1 = zeros(n)
    for i = 1:n
        y1[i] = y[i] + h*a21*work[i,1]
    end
    work[:,2] = f(x+c2*h, y1, params)
    for i = 1:n
        y1[i] = y[i] + h*(a31*work[i,1] + a32*work[i,2])
    end
    work[:,3] = f(x+c3*h, y1, params)
    for i = 1:n
        y1[i] = y[i]+h*(a41*work[i,1]+a43*work[i,3])  
    end
    work[:,4] = f(x+c4*h, y1, params)
    for i = 1:n
        y1[i] = y[i]+h*(a51*work[i,1]+a53*work[i,3]+a54*work[i,4])
    end
    work[:,5] = f(x+c5*h, y1, params)
    for i = 1:n
        y1[i] = y[i]+h*(a61*work[i,1]+a64*work[i,4]+a65*work[i,5])
    end
    work[:,6] = f(x+c6*h, y1, params)
    for i = 1:n
        y1[i] = y[i]+h*(a71*work[i,1]+a74*work[i,4]+a75*work[i,5]+a76*work[i,6])
    end
    work[:,7] = f(x+c7*h, y1, params)
    for i = 1:n
        y1[i] = y[i]+h*(a81*work[i,1]+a84*work[i,4]+a85*work[i,5]+a86*work[i,6]+a87*work[i,7])  
    end
    work[:,8] = f(x+c8*h, y1, params)
    for i = 1:n
        y1[i] = y[i]+h*(a91*work[i,1]+a94*work[i,4]+a95*work[i,5]+a96*work[i,6]+a97*work[i,7]+a98*work[i,8])
    end
    work[:,9] = f(x+c9*h, y1, params)
    for i = 1:n
        y1[i] = y[i]+h*(a101*work[i,1]+a104*work[i,4]+a105*work[i,5]+a106*work[i,6]
           +a107*work[i,7]+a108*work[i,8]+a109*work[i,9])
    end
    work[:,10] = f(x+c10*h, y1, params)
    for i = 1:n
        y1[i] = y[i]+h*(a111*work[i,1]+a114*work[i,4]+a115*work[i,5]+a116*work[i,6]
           +a117*work[i,7]+a118*work[i,8]+a119*work[i,9]+a1110*work[i,10])
    end
    work[:,2] = f(x+c11*h, y1, params)
    for i = 1:n
        y1[i] = y[i]+h*(a121*work[i,1]+a124*work[i,4]+a125*work[i,5]+a126*work[i,6]
           +a127*work[i,7]+a128*work[i,8]+a129*work[i,9]+a1210*work[i,10]+a1211*work[i,2])
    end
    work[:,3] = f(x+h, y1, params)
    for i = 1:n
        work[i,4] = b1*work[i,1]+b6*work[i,6]+b7*work[i,7]+b8*work[i,8]+b9*work[i,9]+b10*work[i,10]+b11*work[i,2]+b12*work[i,3]
        work[i,5] = y[i]+h*work[i,4]
    end

    # Error estimation
    err = 0.0
    err2 = 0.0
    for i = 1:n
        sk = atol[i] + rtol[i]*max(abs(y[i]),abs(work[i,5]))
        erri = work[i,4] - bhh1*work[i,1] - bhh2*work[i,9] - bhh3*work[i,3]
        err2 += (erri/sk)*(erri/sk)
        erri = er1*work[i,1] + er6*work[i,6] + er7*work[i,7] + er8*work[i,8] + er9*work[i,9] + er10*work[i,10] + er11*work[i,2] + er12*work[i,3]
        err += (erri/sk)*(erri/sk)
    end 
    deno = err + 0.01*err2
    if deno <= 0.0
        deno = 1.0
    end
    err = abs(h)*err*sqrt(1.0/(n*deno))
    return y1, err
end

function dopcore(n::Int64, f::Function, x::Float64, y::Vector{Float64}, h::Float64, k1::Vector{Float64}, atol::Vector{Float64}, rtol::Vector{Float64}, params::Any)
    a21 =    5.26001519587677318785587544488e-2
    a31 =    1.97250569845378994544595329183e-2
    a32 =    5.91751709536136983633785987549e-2
    a41 =    2.95875854768068491816892993775e-2
    a43 =    8.87627564304205475450678981324e-2
    a51 =    2.41365134159266685502369798665e-1
    a53 =   -8.84549479328286085344864962717e-1
    a54 =    9.24834003261792003115737966543e-1
    a61 =    3.7037037037037037037037037037e-2
    a64 =    1.70828608729473871279604482173e-1
    a65 =    1.25467687566822425016691814123e-1
    a71 =    3.7109375e-2
    a74 =    1.70252211019544039314978060272e-1
    a75 =    6.02165389804559606850219397283e-2
    a76 =   -1.7578125e-2
    a81 =    3.70920001185047927108779319836e-2
    a84 =    1.70383925712239993810214054705e-1
    a85 =    1.07262030446373284651809199168e-1
    a86 =   -1.53194377486244017527936158236e-2
    a87 =    8.27378916381402288758473766002e-3
    a91 =    6.24110958716075717114429577812e-1
    a94 =   -3.36089262944694129406857109825e0
    a95 =   -8.68219346841726006818189891453e-1
    a96 =    2.75920996994467083049415600797e1
    a97 =    2.01540675504778934086186788979e1
    a98 =   -4.34898841810699588477366255144e1
    a101 =   4.77662536438264365890433908527e-1
    a104 =  -2.48811461997166764192642586468e0
    a105 =  -5.90290826836842996371446475743e-1
    a106 =   2.12300514481811942347288949897e1
    a107 =   1.52792336328824235832596922938e1
    a108 =  -3.32882109689848629194453265587e1
    a109 =  -2.03312017085086261358222928593e-2
    a111 =  -9.3714243008598732571704021658e-1
    a114 =   5.18637242884406370830023853209e0
    a115 =   1.09143734899672957818500254654e0
    a116 =  -8.14978701074692612513997267357e0
    a117 =  -1.85200656599969598641566180701e1
    a118 =   2.27394870993505042818970056734e1
    a119 =   2.49360555267965238987089396762e0
    a1110 = -3.0467644718982195003823669022e0
    a121 =   2.27331014751653820792359768449e0
    a124 =  -1.05344954667372501984066689879e1
    a125 =  -2.00087205822486249909675718444e0
    a126 =  -1.79589318631187989172765950534e1
    a127 =   2.79488845294199600508499808837e1
    a128 =  -2.85899827713502369474065508674e0
    a129 =  -8.87285693353062954433549289258e0
    a1210 =  1.23605671757943030647266201528e1
    a1211 =  6.43392746015763530355970484046e-1
    a141 =  5.61675022830479523392909219681e-2
    a147 =  2.53500210216624811088794765333e-1
    a148 = -2.46239037470802489917441475441e-1
    a149 = -1.24191423263816360469010140626e-1
    a1410 =  1.5329179827876569731206322685e-1
    a1411 =  8.20105229563468988491666602057e-3
    a1412 =  7.56789766054569976138603589584e-3
    a1413 = -8.298e-3
    a151 =  3.18346481635021405060768473261e-2
    a156 =  2.83009096723667755288322961402e-2
    a157 =  5.35419883074385676223797384372e-2
    a158 = -5.49237485713909884646569340306e-2
    a1511 = -1.08347328697249322858509316994e-4
    a1512 =  3.82571090835658412954920192323e-4
    a1513 = -3.40465008687404560802977114492e-4
    a1514 =  1.41312443674632500278074618366e-1
    a161 = -4.28896301583791923408573538692e-1
    a166 = -4.69762141536116384314449447206e0
    a167 =  7.68342119606259904184240953878e0
    a168 =  4.06898981839711007970213554331e0
    a169 =  3.56727187455281109270669543021e-1
    a1613 = -1.39902416515901462129418009734e-3
    a1614 =  2.9475147891527723389556272149e0
    a1615 = -9.15095847217987001081870187138e0
    b1 =   5.42937341165687622380535766363e-2
    b6 =   4.45031289275240888144113950566e0
    b7 =   1.89151789931450038304281599044e0
    b8 =  -5.8012039600105847814672114227e0
    b9 =   3.1116436695781989440891606237e-1
    b10 = -1.52160949662516078556178806805e-1
    b11 =  2.01365400804030348374776537501e-1
    b12 =  4.47106157277725905176885569043e-2
    c2  = 0.526001519587677318785587544488e-01
    c3  = 0.789002279381515978178381316732e-01
    c4  = 0.118350341907227396726757197510e+00
    c5  = 0.281649658092772603273242802490e+00
    c6  = 0.333333333333333333333333333333e+00
    c7  = 0.25e+00
    c8  = 0.307692307692307692307692307692e+00
    c9  = 0.651282051282051282051282051282e+00
    c10 = 0.6e+00
    c11 = 0.857142857142857142857142857142e+00
    bhh1 = 0.244094488188976377952755905512e+00
    bhh2 = 0.733846688281611857341361741547e+00
    bhh3 = 0.220588235294117647058823529412e-01
    er1 =  0.1312004499419488073250102996e-01
    er6 = -0.1225156446376204440720569753e+01
    er7 = -0.4957589496572501915214079952e+00
    er8 =  0.1664377182454986536961530415e+01
    er9 = -0.3503288487499736816886487290e+00
    er10 =  0.3341791187130174790297318841e+00
    er11 =  0.8192320648511571246570742613e-01
    er12 = -0.2235530786388629525884427845e-01

    y1 = zeros(n)
    for i = 1:n
        y1[i] = y[i] + h*a21*k1[i]
    end
    k2 = f(x+c2*h, y1, params)
    for i = 1:n
        y1[i] = y[i] + h*(a31*k1[i] + a32*k2[i])
    end
    k3 = f(x+c3*h, y1, params)
    for i = 1:n
        y1[i] = y[i]+h*(a41*k1[i]+a43*k3[i])  
    end
    k4 = f(x+c4*h, y1, params)
    for i = 1:n
        y1[i] = y[i]+h*(a51*k1[i]+a53*k3[i]+a54*k4[i])
    end
    k5 = f(x+c5*h, y1, params)
    for i = 1:n
        y1[i] = y[i]+h*(a61*k1[i]+a64*k4[i]+a65*k5[i])
    end
    k6 = f(x+c6*h, y1, params)
    for i = 1:n
        y1[i] = y[i]+h*(a71*k1[i]+a74*k4[i]+a75*k5[i]+a76*k6[i])
    end
    k7 = f(x+c7*h, y1, params)
    for i = 1:n
        y1[i] = y[i]+h*(a81*k1[i]+a84*k4[i]+a85*k5[i]+a86*k6[i]+a87*k7[i])  
    end
    k8 = f(x+c8*h, y1, params)
    for i = 1:n
        y1[i] = y[i]+h*(a91*k1[i]+a94*k4[i]+a95*k5[i]+a96*k6[i]+a97*k7[i]+a98*k8[i])
    end
    k9 = f(x+c9*h, y1, params)
    for i = 1:n
        y1[i] = y[i]+h*(a101*k1[i]+a104*k4[i]+a105*k5[i]+a106*k6[i]
           +a107*k7[i]+a108*k8[i]+a109*k9[i])
    end
    k10 = f(x+c10*h, y1, params)
    for i = 1:n
        y1[i] = y[i]+h*(a111*k1[i]+a114*k4[i]+a115*k5[i]+a116*k6[i]
           +a117*k7[i]+a118*k8[i]+a119*k9[i]+a1110*k10[i])
    end
    k2 = f(x+c11*h, y1, params)
    for i = 1:n
        y1[i] = y[i]+h*(a121*k1[i]+a124*k4[i]+a125*k5[i]+a126*k6[i]
           +a127*k7[i]+a128*k8[i]+a129*k9[i]+a1210*k10[i]+a1211*k2[i])
    end
    k3 = f(x+h, y1, params)
    for i = 1:n
        k4[i] = b1*k1[i]+b6*k6[i]+b7*k7[i]+b8*k8[i]+b9*k9[i]+b10*k10[i]+b11*k2[i]+b12*k3[i]
        k5[i] = y[i]+h*k4[i]
    end

    # Error estimation
    err = 0.0
    err2 = 0.0
    for i = 1:n
        sk = atol[i] + rtol[i]*max(abs(y[i]),abs(k5[i]))
        erri = k4[i] - bhh1*k1[i] - bhh2*k9[i] - bhh3*k3[i]
        err2 += (erri/sk)*(erri/sk)
        erri = er1*k1[i] + er6*k6[i] + er7*k7[i] + er8*k8[i] + er9*k9[i] + er10*k10[i] + er11*k2[i] + er12*k3[i]
        err += (erri/sk)*(erri/sk)
    end 
    deno = err + 0.01*err2
    if deno <= 0.0
        deno = 1.0
    end
    err = abs(h)*err*sqrt(1.0/(n*deno))
    return y1, k3, k4, k5, err
end

function dopcorevec(n::Int64, f::Function, x::Float64, y::Vector{Float64}, h::Float64, k1::Vector{Float64}, atol::Vector{Float64}, rtol::Vector{Float64}, params::Any)
    a21 =    5.26001519587677318785587544488e-2
    a31 =    1.97250569845378994544595329183e-2
    a32 =    5.91751709536136983633785987549e-2
    a41 =    2.95875854768068491816892993775e-2
    a43 =    8.87627564304205475450678981324e-2
    a51 =    2.41365134159266685502369798665e-1
    a53 =   -8.84549479328286085344864962717e-1
    a54 =    9.24834003261792003115737966543e-1
    a61 =    3.7037037037037037037037037037e-2
    a64 =    1.70828608729473871279604482173e-1
    a65 =    1.25467687566822425016691814123e-1
    a71 =    3.7109375e-2
    a74 =    1.70252211019544039314978060272e-1
    a75 =    6.02165389804559606850219397283e-2
    a76 =   -1.7578125e-2
    a81 =    3.70920001185047927108779319836e-2
    a84 =    1.70383925712239993810214054705e-1
    a85 =    1.07262030446373284651809199168e-1
    a86 =   -1.53194377486244017527936158236e-2
    a87 =    8.27378916381402288758473766002e-3
    a91 =    6.24110958716075717114429577812e-1
    a94 =   -3.36089262944694129406857109825e0
    a95 =   -8.68219346841726006818189891453e-1
    a96 =    2.75920996994467083049415600797e1
    a97 =    2.01540675504778934086186788979e1
    a98 =   -4.34898841810699588477366255144e1
    a101 =   4.77662536438264365890433908527e-1
    a104 =  -2.48811461997166764192642586468e0
    a105 =  -5.90290826836842996371446475743e-1
    a106 =   2.12300514481811942347288949897e1
    a107 =   1.52792336328824235832596922938e1
    a108 =  -3.32882109689848629194453265587e1
    a109 =  -2.03312017085086261358222928593e-2
    a111 =  -9.3714243008598732571704021658e-1
    a114 =   5.18637242884406370830023853209e0
    a115 =   1.09143734899672957818500254654e0
    a116 =  -8.14978701074692612513997267357e0
    a117 =  -1.85200656599969598641566180701e1
    a118 =   2.27394870993505042818970056734e1
    a119 =   2.49360555267965238987089396762e0
    a1110 = -3.0467644718982195003823669022e0
    a121 =   2.27331014751653820792359768449e0
    a124 =  -1.05344954667372501984066689879e1
    a125 =  -2.00087205822486249909675718444e0
    a126 =  -1.79589318631187989172765950534e1
    a127 =   2.79488845294199600508499808837e1
    a128 =  -2.85899827713502369474065508674e0
    a129 =  -8.87285693353062954433549289258e0
    a1210 =  1.23605671757943030647266201528e1
    a1211 =  6.43392746015763530355970484046e-1
    a141 =  5.61675022830479523392909219681e-2
    a147 =  2.53500210216624811088794765333e-1
    a148 = -2.46239037470802489917441475441e-1
    a149 = -1.24191423263816360469010140626e-1
    a1410 =  1.5329179827876569731206322685e-1
    a1411 =  8.20105229563468988491666602057e-3
    a1412 =  7.56789766054569976138603589584e-3
    a1413 = -8.298e-3
    a151 =  3.18346481635021405060768473261e-2
    a156 =  2.83009096723667755288322961402e-2
    a157 =  5.35419883074385676223797384372e-2
    a158 = -5.49237485713909884646569340306e-2
    a1511 = -1.08347328697249322858509316994e-4
    a1512 =  3.82571090835658412954920192323e-4
    a1513 = -3.40465008687404560802977114492e-4
    a1514 =  1.41312443674632500278074618366e-1
    a161 = -4.28896301583791923408573538692e-1
    a166 = -4.69762141536116384314449447206e0
    a167 =  7.68342119606259904184240953878e0
    a168 =  4.06898981839711007970213554331e0
    a169 =  3.56727187455281109270669543021e-1
    a1613 = -1.39902416515901462129418009734e-3
    a1614 =  2.9475147891527723389556272149e0
    a1615 = -9.15095847217987001081870187138e0
    b1 =   5.42937341165687622380535766363e-2
    b6 =   4.45031289275240888144113950566e0
    b7 =   1.89151789931450038304281599044e0
    b8 =  -5.8012039600105847814672114227e0
    b9 =   3.1116436695781989440891606237e-1
    b10 = -1.52160949662516078556178806805e-1
    b11 =  2.01365400804030348374776537501e-1
    b12 =  4.47106157277725905176885569043e-2
    c2  = 0.526001519587677318785587544488e-01
    c3  = 0.789002279381515978178381316732e-01
    c4  = 0.118350341907227396726757197510e+00
    c5  = 0.281649658092772603273242802490e+00
    c6  = 0.333333333333333333333333333333e+00
    c7  = 0.25e+00
    c8  = 0.307692307692307692307692307692e+00
    c9  = 0.651282051282051282051282051282e+00
    c10 = 0.6e+00
    c11 = 0.857142857142857142857142857142e+00
    bhh1 = 0.244094488188976377952755905512e+00
    bhh2 = 0.733846688281611857341361741547e+00
    bhh3 = 0.220588235294117647058823529412e-01
    er1 =  0.1312004499419488073250102996e-01
    er6 = -0.1225156446376204440720569753e+01
    er7 = -0.4957589496572501915214079952e+00
    er8 =  0.1664377182454986536961530415e+01
    er9 = -0.3503288487499736816886487290e+00
    er10 =  0.3341791187130174790297318841e+00
    er11 =  0.8192320648511571246570742613e-01
    er12 = -0.2235530786388629525884427845e-01
    y1 = y + h*a21*k1
    k2 = f(x+c2*h, y1, params)
    y1 = y + h*(a31*k1 + a32*k2)
    k3 = f(x+c3*h, y1, params)
    y1 = y+h*(a41*k1+a43*k3)  
    k4 = f(x+c4*h, y1, params)
    y1 = y+h*(a51*k1+a53*k3+a54*k4)
    k5 = f(x+c5*h, y1, params)
    y1 = y+h*(a61*k1+a64*k4+a65*k5)
    k6 = f(x+c6*h, y1, params)
    y1 = y+h*(a71*k1+a74*k4+a75*k5+a76*k6)
    k7 = f(x+c7*h, y1, params)
    y1 = y+h*(a81*k1+a84*k4+a85*k5+a86*k6+a87*k7)  
    k8 = f(x+c8*h, y1, params)
    y1 = y+h*(a91*k1+a94*k4+a95*k5+a96*k6+a97*k7+a98*k8)
    k9 = f(x+c9*h, y1, params)
    y1 = y+h*(a101*k1+a104*k4+a105*k5+a106*k6
       +a107*k7+a108*k8+a109*k9)
    k10 = f(x+c10*h, y1, params)
    y1 = y+h*(a111*k1+a114*k4+a115*k5+a116*k6
       +a117*k7+a118*k8+a119*k9+a1110*k10)
    k2 = f(x+c11*h, y1, params)
    #xph = x+h
    y1 = y+h*(a121*k1+a124*k4+a125*k5+a126*k6
       +a127*k7+a128*k8+a129*k9+a1210*k10+a1211*k2)
    k3 = f(x+h, y1, params)
    #nfcn += 11
    k4 = b1*k1+b6*k6+b7*k7+b8*k8+b9*k9+b10*k10+b11*k2+b12*k3
    k5 = y+h*k4

    # Error estimation
    err = 0.0
    err2 = 0.0
    for i = 1:n
        sk = atol[i] + rtol[i]*max(abs(y[i]),abs(k5[i]))
        erri = k4[i] - bhh1*k1[i] - bhh2*k9[i] - bhh3*k3[i]
        err2 += (erri/sk)^2
        erri = er1*k1[i] + er6*k6[i] + er7*k7[i] + er8*k8[i] + er9*k9[i] + er10*k10[i] + er11*k2[i] + er12*k3[i]
        err += (erri/sk)^2
    end 
    deno = err + 0.01*err2
    if deno <= 0.0
        deno = 1.0
    end
    err = abs(h)*err*sqrt(1.0/(n*deno))
    return y1, k3, k4, k5, err
end

function gravity(t::Float64, y::Vector{Float64}, mu::Float64)
    f = zeros(6)
    r = norm(y[1:3])
    f[1:3] = y[4:6]
    f[4:6] = -mu*y[1:3]/r/r/r
    return f
end

function gravity1(t::Float64, y::Vector{Float64}, mu::Float64)
    x, y, z, vx, vy, vz = y
    r = sqrt(x*x+y*y+z*z)
    r3 = r*r*r
    [vx, vy, vz, -mu*x/r3, -mu*y/r3, -mu*z/r3]
end

mu = 398600.4415		# [km^3/s^2] Earth's gravity
s0 = [-1814.0, -3708.0, 5153.0, 6.512, -4.229, -0.744]
tp = 5402.582703094263

output = dop853(gravity1, 0.0, s0, tp, mu) 
mem = @allocated dop853(gravity1, 0.0, s0, tp, mu) 
el = 0.0
n = 1000
for i = 1:n
el += @elapsed dop853(gravity1, 0.0, s0, tp, mu) 
end
println("Allocated memory: $mem")
println("Time per loop: $(el/n)")