!!!!!!
!RUBC_Brock_Mirman.f90
!
!This program solves, simulates, and computes IRFs for the
!Brock-Mirman model.
!
!"Really Uncertain Business Cycles"
!Nick Bloom, Max Floetotto, Nir Jaimovich, Itay Saporta-Eksten, Stephen J. Terry
!
!This Version: October 12, 2014
!!!!!!
module globals
implicit none

double precision, parameter :: alpha = 0.33,&
    deltak = 0.09,&
    beta = 0.95, &
    rhoa = 0.9,&
    sigmaa = 0.025,&
    sigma = 1,&
    nstdeva = 3.0,&
    kmin = 2.5,&
    kmax = 4.9,&
    errtol = 1e-8,&
    disttol = 1e-9,&
    shockprob = 1.0

integer, parameter :: checkbounds = 1,&
    GEerrorswitch = 4,&
    anum = 25,&
    knum = 1000,&
    numper = 5000,&
    numdiscard = 500,&
    polmaxit=500,&
    accelmaxit = 200,&
    distmaxit=10000,&
    numsimIRF=75000,&
    lengthIRF = 100,&
    shockperIRF = 25,& ! time period when shock hits (t from 1 to lengthIRF)
    !shockval = 120,&   ! 120 if capit destr shock, if tfp it must be in [1 anum] range
    shockval = 15,&   ! 120 if capit destr shock, if tfp it must be in [1 anum] range
    !IRFswitch=2  ! 1 if aggr tfp shock, 2 if capital destruction
    IRFswitch=1  ! 1 if aggr tfp shock, 2 if capital destruction

!Integer numbers for loop counters
integer :: act,kct,kprimect, ct,aprimect,polct,accelct,t,distct,simct, i, j

!Integer variables
integer :: seeddim,seedint,polstar,k_previous_max

!Real numbers
double precision :: aval,kval,kprimeval,cval,uval,yval,start,vferr,polerr,disterr,RHSbest,RHStemp

!Allocatable integer arrays
integer, allocatable :: seedarray(:),kprime(:,:),kprimeold(:,:),asimpos(:),Ksimpos(:),asimposIRF(:,:),KsimposIRF(:,:)

!Allocatable real arrays
double precision, allocatable :: asimshocks(:),k0(:),pr_mat_a(:,:),a0(:),RETURNMAT(:,:,:),V(:,:),Vold(:,:),&
    EVmat(:,:),kprimepol(:,:),Ksim(:),asim(:),asimgrid(:),Ysim(:),Csim(:),Isim(:),dist(:,:),distold(:,:),&
    adist(:),kdist(:)

end module globals
!********************************************** END MODULE *******************************************************!

program RUBC_Brock_Mirman
use base_lib
use omp_lib
use globals
implicit none

start = omp_get_wtime()
write(*,*) "######################################"
write(*,*) "Starting program for simple RBC model."
write(*,*) " "

!Allocate arrays
allocate(seedarray(seeddim),asimshocks(numper),k0(knum),pr_mat_a(anum,anum),a0(anum),&
    RETURNMAT(anum,knum,knum),kprime(anum,knum),V(anum,knum),Vold(anum,knum),kprimeold(anum,knum),&
    EVmat(anum,knum),kprimepol(anum,knum),asimpos(numper),Ksim(numper),asim(numper),asimgrid(anum),Ksimpos(numper),&
    Ysim(numper),Csim(numper),Isim(numper),dist(anum,knum),distold(anum,knum),adist(anum),kdist(knum))

write(*,*) "available threads = ",omp_get_max_threads()

!$omp parallel
write(*,*) "parallel hello to you."
!$omp end parallel

!set up the capital grid
call linspace(k0,log(kmin),log(kmax),knum); k0=exp(k0);

!set up the agg prod grid
call idio_tauchen(anum,rhoa,sigmaa,pr_mat_a,a0,nstdeva)

!set up the return matrix
RETURNMAT(:,:,:) = 0.0
!RETURNMAT(a,k,k')

!*!$omp parallel private(act,kct,kprimect,kval,kprimeval,aval,cval,uval)
!*!$omp do collapse(3)
!$omp parallel do default(shared) private(act,kct,kprimect,kval,kprimeval,aval,cval,uval)
do act=1,anum
do kct=1,knum
do kprimect=1,knum

    kval = k0(kct)
    kprimeval = k0(kprimect)
    aval = a0(act)

    yval = yprod(aval,kval)

    cval = yval + (1.0-deltak)*kval - kprimeval
    if (cval<=0.0) then
        uval = -huge(0d0)
    else if (cval>0.0) then
        uval = util(cval)
    end if

    RETURNMAT(act,kct,kprimect) = uval

end do !kprimect
end do !kct
end do !act
!$omp end parallel do
!*!$omp end do nowait
!*!$omp end parallel

write(*,*) " "

!now, perform the VFI, using Howard acceleration

!initialize
Vold(:,:) = 0.0
V(:,:) = 0.0

do polct=1,polmaxit

    !construct continuation value EVmat(a,k')
    EVmat(:,:) = 0.0
    !$omp parallel do default(shared) private(act,kprimect,aprimect)
    do act=1,anum
    do kprimect=1,knum

        do aprimect=1,anum
            EVmat(act,kprimect) = EVmat(act,kprimect) + pr_mat_a(act,aprimect)*beta * Vold(aprimect,kprimect)
        end do !aprimect

    end do !kprimect
    end do !act
    !$omp end parallel do

    !now, actually do the optimization
    !$omp parallel do default(shared) private(act,kct,kprimect,polstar,RHStemp,RHSbest,k_previous_max)
    do act=1,anum
    do kct=1,knum

        !Maximize the RHS of Bellman equation, for given act,kct
        RHSbest = -huge(0d0)

        !Monotonicity of policy function
        k_previous_max = 1
        if (kct>1) k_previous_max = kprime(act,kct-1)

        do kprimect = k_previous_max,knum
            RHStemp = RETURNMAT(act,kct,kprimect) + EVmat(act,kprimect)
            if (RHStemp>RHSbest) then
                RHSbest = RHStemp
                polstar = kprimect
            else
                exit
            end if
        end do !kprimect

        V(act,kct)      = RHSbest
        kprime(act,kct) = polstar
        k_previous_max  = polstar

    end do !act
    end do !act
    !$omp end parallel do

    !compute errors and end if converged
    vferr = maxval(V-Vold)

    write(*,'(a,i4)') "VF iteration ",polct
    write(*,'(a,f10.6)') "VF err ",vferr

    !exit if policies have converged
    if (vferr<errtol) exit

    !update if not converged
    Vold = V

end do !polct

do act=1,anum
do kct=1,knum
    kprimepol(act,kct)=k0(kprime(act,kct))
end do
end do

!VF diagnostics
write(*,*) " "
write(*,*) "Done with VFI."
if (minval(kprime)==1) write(*,*) "Max capital policy hits bottom of grid in theory."
if (maxval(kprime)==knum) write(*,*) "Min capital policy hits top of grid in theory."

write(*,'(a,f10.6,a)') "Finished VFI at ",real(omp_get_wtime()-start)," seconds."

write(*,'(a)') 'Çheck some elements of V...'
do i=1,4
    write(*,'(f10.4)') V(2,i)
enddo

pause

!compute unconditional distribution
write(*,*) " "
write(*,*) "Doing ergodic dist calculations."
distold(:,:) = 0.0
distold(anum/2,knum/2) = 1.0

do distct=1,distmaxit

    dist(:,:) = 0.0

    !loop over states
    !$omp parallel do default(shared) private(act,kct,kprimect,aprimect) REDUCTION (+: dist)
    do act=1,anum
    do kct=1,knum

        kprimect=kprime(act,kct)

        !pushforward of weight
        do aprimect=1,anum
            dist(aprimect,kprimect) =  dist(aprimect,kprimect) + distold(act,kct)*pr_mat_a(act,aprimect)
        end do !aprimect

    end do !kct
    end do !act
    !$omp end parallel do

    disterr =maxval(abs(dist-distold))
    if (mod(distct,25)==1) then
        write(*,'(a,i4)') "Distrib. iter. ", distct
        write(*,'(a,f10.6)') "Distrib. sum ",sum(dist)
        write(*,'(a,f10.6)') "Distrib. error ",disterr
    end if
    if (disterr<disttol) exit

    dist = dist/sum(dist)
    distold = dist

end do !distct

write(*,'(a)') 'Çheck some elements of dist...'
do i=1,4
    j = 150+i
    write(*,'(f10.8)') dist(2,j)
enddo

!marginal dists
adist(:) = 0.0
do act=1,anum
    adist(act) = sum(dist(act,:))
end do

kdist(:) = 0.0
do kct=1,knum
    kdist(kct) = sum(dist(:,kct))
end do

write(*,*) "Done with ergodic dist calculations."
write(*,"(A,F7.4,A,F7.4)") "Weight on min, max of capital grid = ",kdist(1),", ",kdist(knum)

!! Export results in txt files
open(8,file="k0_fortran.txt")
do kct=1,knum
write(8,*) k0(kct)
end do
close(8)

open(8,file="V_fortran.txt")
do kct=1,knum
do act=1,anum
write(8,*) V(act,kct)
end do
end do
close(8)

open(8,file="kprimepol_fortran.txt")
do kct=1,knum
do act=1,anum
write(8,*) kprimepol(act,kct)
end do
end do
close(8)

open(8,file="dist_fortran.txt")
do kct=1,knum
do act=1,anum
write(8,*) dist(act,kct)
end do
end do
close(8)

pause

write(*,*) " "
!write(*,*) "Finished program for the Brock-Mirman model at ",omp_get_wtime()-start," seconds."
write(*,*) "######################################"

!Deallocate arrays
deallocate(seedarray,asimshocks,k0,pr_mat_a,a0,&
    RETURNMAT,kprime,V,Vold,kprimeold,&
    EVmat,kprimepol,asimpos,Ksim,asim,asimgrid,Ksimpos,&
    Ysim,Csim,Isim,dist,distold,adist,kdist)

    contains

    !--------------------------------------------------------------------------------------------------!
    ! Internal functions
    !--------------------------------------------------------------------------------------------------!

double precision function yprod(aval,kval)
implicit none

!this function evaluates output given parameters and states

double precision :: aval,kval

yprod =  aval * (kval ** alpha)

end function

double precision function util(cval)
implicit none

!this function evaluates the utility function

double precision :: cval

!util =  (cval**(1.0-sigma))/(1.0-sigma)
util = log(cval)

end function

end program RUBC_Brock_Mirman


c
c Copyright (C) 2002 Vanderbilt group
c This file is distributed under the terms of the GNU General Public
c License as described in the file 'License' in the current directory.
c
      subroutine interpol(xin,yin,nin,xout,yout,nout,y2,u)
      implicit none
      integer nin, nout
      real*8 xin(nin), yin(nin), xout(nout), yout(nout),
     *     y2(nin), u(nin), yp1, ypn
***
***
      yp1 = (yin(2)-yin(1))/(xin(2)-xin(1))
      ypn = 0.0

      call spline(xin,yin,nin,yp1,ypn,y2,u)
      call splint(nin,xin,yin,y2,nout,xout,yout)

      return
      end

      subroutine splint (nspline,xspline,yspline,ysplin2,
     *                   nfit,xfit,yfit)

      implicit real*8 (a-h, o-z)
      dimension xspline(nspline), yspline(nspline), ysplin2(nspline),
     *          xfit(nfit), yfit(nfit)

      klo = 1
      do i=1, nfit
        do k=klo+1, nspline
            if(xspline(k).ge.xfit(i)) then
               if(xspline(k-1).le.xfit(i)) then
                  khi = k
                  klo = k-1
               else
                  if (k-1.eq.1 .and. i.eq.1) then
                     stop '  SPLINT: xfit(1) < xspline(1)'
                  else
                     stop '  SPLINT: xfit not properly ordered'
                  end if
               end if
               h= xspline(khi) - xspline(klo)
               a= (xspline(khi)-xfit(i))/h
               b= (xfit(i)-xspline(klo))/h

               yfit(i) = a*yspline(klo) + b*yspline(khi)
     *              +( (a**3-a)*ysplin2(klo) +
     *              (b**3-b)*ysplin2(khi) ) *h*h/6
               go to 10
            end if
         end do
*        stop '  SPLINT: out of bounds'
* This is for the unlikely event that rmax exceed r(mesh)
	yfit(i)=0.0
 10     continue
      end do

      return
      end

      subroutine spline(x,y,n,yp1,ypn,y2,u)
      implicit real*8 (a-h, o-z)
      dimension x(n),y(n),y2(n),u(n)

      if (yp1.gt..99e30) then
        y2(1)=0.
        u(1)=0.
      else
        y2(1)=-0.5
        u(1)=(3./(x(2)-x(1)))*((y(2)-y(1))/(x(2)-x(1))-yp1)
      endif
      do 11 i=2,n-1
        sig=(x(i)-x(i-1))/(x(i+1)-x(i-1))
        p=sig*y2(i-1)+2.
        y2(i)=(sig-1.)/p
        u(i)=(6.*((y(i+1)-y(i))/(x(i+1)-x(i))-(y(i)-y(i-1))
     *      /(x(i)-x(i-1)))/(x(i+1)-x(i-1))-sig*u(i-1))/p
11    continue
      if (ypn.gt..99e30) then
        qn=0.
        un=0.
      else
        qn=0.5
        un=(3./(x(n)-x(n-1)))*(ypn-(y(n)-y(n-1))/(x(n)-x(n-1)))
      endif
      y2(n)=(un-qn*u(n-1))/(qn*y2(n-1)+1.)
      do 12 k=n-1,1,-1
        y2(k)=y2(k)*y2(k+1)+u(k)
12    continue
      return
      end

      subroutine deriv(f,df,r,rab,mesh,idim1)
      implicit none
      integer mesh,i,idim1
      real *8 f(idim1),df(idim1),r(idim1),rab(idim1)
      real *8 f1,f2,a,b,c
c.... calculates radial derivative of f

c .. first df/di
      do i=3,mesh-2
         f1= (f(i+1)-f(i-1))/2.
         f2= (f(i+2)-f(i-2))/4.
         df(i) = (4.*f1-f2)/3.
      enddo
       df(3) = (f(4)-f(2))/2.
c      df(2) =(f(3)-f(1))/2.
      f1=f(4)-f(3)
      f2=f(5)-f(4)
      a = (f2-f1)/2.
      b = (f1+f2)/2.-(f2-f1)*4.
      c = f(3)-a*3.*3.-b *3.
      df(2) = 2.*df(3)-df(4)
      df(1) =  2.*df(2)-df(3)
c      df(2) = a*4.+b*2.+c
c      df(1) = a*1.+b*1.+c
      df(mesh-1) = 2.*df(mesh-2)-df(mesh-3)
      df(mesh) =  2.*df(mesh-1)-df(mesh-2)

c now multiply di/dr
      do i=1,mesh
         df(i) = df(i)/rab(i)
      enddo
      end


! Update the stencil
subroutine kernel(nx, ny, A, Atmp, total)

  implicit none

  integer :: nx, ny
  real(kind=8) :: A(0:nx+1, 0:ny+1)
  real(kind=8) :: Atmp(0:nx+1, 0:ny+1)
  real(kind=8) :: total

  integer :: i, j

  total = 0.0_8
  !$omp parallel do reduction(+:total)
  do j = 1, ny
  !$omp simd
    do i = 1, nx
      Atmp(i,j) = (A(i-1,j) + A(i+1,j) + A(i,j) + A(i,j-1) + A(i,j+1)) * 0.2_8
      total = total + Atmp(i,j)
    end do
    !$omp end simd
  end do
  !$omp end parallel do

end subroutine kernel

! 5 point stencil
program stencil

  use timer

  implicit none

  integer :: nx = 4000
  integer :: ny = 4000
  integer :: ntimes = 30
  real(kind=8), dimension(:,:), pointer :: A, Atmp, Aptr
  integer :: i, j, t
  real(kind=8) :: total_start, total_end, total
  real(kind=8) :: tic, toc

  ! Allocate memory
  allocate(A(0:nx+1,0:ny+1))
  allocate(Atmp(0:nx+1,0:ny+1))

  ! Initialise data to zero
  !$omp parallel do
  do j = 0, ny+1
    do i = 0, nx+1
      A(i,j) = 0.0_8
      Atmp(i,j) = 0.0_8
    end do
  end do
  !$omp end parallel do

  ! Insert values in centre of grid
  do i = nx/4, 3*nx/4
    do j = ny/4, 3*ny/4
      A(i,j) = 1.0_8
    end do
  end do

  total_start = sum(A(:,:))

  ! Start timer
  call wtime(tic)

  ! Loop a number of times
  do t = 1, ntimes

    ! Update the stencil
    call kernel(nx, ny, A, Atmp, total)

    ! Print out total
    write(*,"(I0,A,F15.5)") t, ": total=", total

    ! Swap pointers
    Aptr => A
    A => Atmp
    Atmp => Aptr

  end do

  ! Stop timer
  call wtime(toc)

  ! Sum up grid values for rudimentary correctness check
  total_end = sum(A(:,:))

  ! Print result
  write(*,"(A)")         "------------------------------------"
  write(*,"(A,F10.3)")   "runtime:  ", toc-tic
  if (abs(total_end-total_start)/total_start > 1.0E-8) then
    write(*,"(A)")       "result: Failed"
  else
    write(*,"(A)")       "result: Passed"
  end if
  write(*,"(A)")         "------------------------------------"

  deallocate(A, Atmp)

end program stencil


module IO_gaussint
    use Prec
    use LegendrePoly
    use IO

contains

    subroutine print_gauss_coef_roots(n, fd_out)
        integer, intent(in) :: n, fd_out
        optional :: fd_out
        integer :: fd, i
        real(mpc), allocatable, dimension (:) :: roots, coefs

        if (.not. present(fd_out)) then
            fd = stdout
        else
            fd = fd_out
        end if

        allocate(roots(n), coefs(n))
        write(*,*) 'n = ', n
        roots = legendre_roots(n)
        coefs = int_coefs(roots)

        do i = 1, n
            write(fd, *)  coefs(i), roots(i)
        end do

        deallocate(roots, coefs)

    end subroutine print_gauss_coef_roots

end module IO_gaussint


      subroutine degree4(a,b,c,d,e,x1,x2,x3,x4)
c
      implicit none
c
c     parameter
c
      double precision pi
      parameter (pi=3.14159265358979323846d0)
c
c     arguments
c
      double precision a,b,c,d,e
      double complex x1,x2,x3,x4
c
c     local
c
      double precision p,q,r,g2,g1,g0
      double precision f3,f2,f1,f0
      double complex y0,y1,y2
      double complex compl_a,compl_b,compl_c
      double precision y

c      write(*,*) 'a: ',a
c      write(*,*) 'b: ',b
c      write(*,*) 'c: ',c
c      write(*,*) 'd: ',d
c      write(*,*) 'e: ',e

      if (b.eq.0d0.and.c.eq.0d0.and.d.eq.0d0.and.e.eq.0d0) then
      x1=dcmplx(0d0)
      x2=dcmplx(0d0)
      x3=dcmplx(0d0)
      x4=dcmplx(0d0)
      return
      elseif (b.eq.0d0.and.c.eq.0d0.and.d.eq.0d0.and.e.ne.0d0) then
      if (e.lt.0d0) then
      x1=dcmplx(dsqrt(dsqrt(-e)))
      x2=dcmplx(dsqrt(dsqrt(-e)))*
     & dcmplx(dcos(pi/2d0),dsin(pi/2d0))
      x3=dcmplx(dsqrt(dsqrt(-e)))
     & *dcmplx(dcos(pi),dsin(pi))
      x4=dcmplx(dsqrt(dsqrt(-e)))
     & *dcmplx(dcos(3d0*pi/2d0),dsin(3d0*pi/2d0))
      return
      elseif (e.gt.0d0) then
      x1=dcmplx(dsqrt(dsqrt(e)))*
     & dcmplx(dcos(pi/4d0),dsin(pi/4d0))
      x2=dcmplx(dsqrt(dsqrt(e)))*
     & dcmplx(dcos(5d0*pi/4d0),dsin(5d0*pi/4d0))
      x3=dcmplx(dsqrt(dsqrt(e)))
     & *dcmplx(dcos(3d0*pi/4d0),dsin(3d0*pi/4d0))
      x4=dcmplx(dsqrt(dsqrt(e)))
     & *dcmplx(dcos(7d0*pi/4d0),dsin(7d0*pi/4d0))
      return
      endif
      endif

      p=-3d0*b**2/(8d0*a**2)+c/a
      q=b**3/(8d0*a**3)-b*c/(2d0*a**2)+d/a
      r=-3d0*b**4/(256d0*a**4)+b**2*c/(16d0*a**3)
     & -d*b/(4d0*a**2)+e/a
c
c     coefficients of the degree 3 polynome P(y)
c
      f3=1d0
      f2=5d0*p/2d0
      f1=2d0*p**2-r
      f0=p**3/2d0-p*r/2d0-q**2/8d0

c      write(*,*) 'f3 ',f3
c      write(*,*) 'f2 ',f2
c      write(*,*) 'f1 ',f1
c      write(*,*) 'f0 ',f0

      call degree3(f3,f2,f1,f0,y0,y1,y2)

c      write(*,*) 'y0 ', y0
      y=dble(y0)
c      write(*,*) 'y ', y
c      write(*,*) 'p ', p
c      write(*,*) 'p+2d0*y ', p+2d0*y

      if ((p+2d0*y).gt.0d0) then
c      write(*,*) 'p+2d0*y0 ', p+2d0*y
      g2=1d0
      g1=-dsqrt(p+2d0*y)
      g0=p+y+q/(2d0*dsqrt(p+2d0*y))

c       write(*,*) 'g2', g2
c       write(*,*) 'g1', g1
c       write(*,*) 'g0', g0
      call degree2_real(g2,g1,g0,x1,x2)

c      write(*,*) 'x1',x1
c      write(*,*) 'x2',x2

      g2=1d0
      g1=dsqrt(p+2d0*y)
      g0=p+y-q/(2d0*dsqrt(p+2d0*y))

      call degree2_real(g2,g1,g0,x3,x4)

      elseif((p+2d0*y).lt.0d0) then
      compl_a=dcmplx(1d0)
      compl_b=dcmplx(0d0,-dsqrt(-(p+2d0*y)))
      compl_c=dcmplx(p+y,q/(2d0*dsqrt(-(p+2d0*y))))
      call degree2_complex(compl_a,compl_b,compl_c,x1,x2)

      compl_a=dcmplx(1d0)
      compl_b=dcmplx(0d0,dsqrt(-(p+2d0*y)))
      compl_c=dcmplx(p+y,-q/(2d0*dsqrt(-(p+2d0*y))))
      call degree2_complex(compl_a,compl_b,compl_c,x3,x4)
      endif

      x1=x1-b/(4d0*a)
      x2=x2-b/(4d0*a)
      x3=x3-b/(4d0*a)
      x4=x4-b/(4d0*a)

      return
      end

      subroutine degree3(a,b,c,d,x1,x2,x3)
c
      implicit none
c
c     parameter
c
      double precision pi
      parameter (pi=3.14159265358979323846d0)
c
c     arguments
c
      double precision a,b,c,d
      double complex x1,x2,x3
c
c     local
c
      double precision p,q,delta,u,v
      double complex phase
c
c
      if(b.eq.0d0.and.c.eq.0d0.and.d.eq.0d0) then
      x1=dcmplx(0d0)
      x2=dcmplx(0d0)
      x3=dcmplx(0d0)
      return
      elseif (b.eq.0d0.and.c.eq.0d0.and.d.ne.0d0) then
      if (d.lt.0d0) then
      x1=dcmplx( (-d)**(1d0/3d0)  )
      x2=dcmplx((-d)**(1d0/3d0))
     & *dcmplx(dcos(2d0*pi/3d0),dsin(2d0*pi/3d0))
      x3=dcmplx( (-d)**(1d0/3d0)  )
     & *dcmplx(dcos(4d0*pi/3d0),dsin(4d0*pi/3d0))
      return
      elseif (d.gt.0d0) then
      x1=dcmplx( -d**(1d0/3d0)  )
      x2=dcmplx( -d**(1d0/3d0)  )
     & *dcmplx(dcos(2d0*pi/3d0),dsin(2d0*pi/3d0))
      x3=dcmplx( -d**(1d0/3d0)  )
     & *dcmplx(dcos(4d0*pi/3d0),dsin(4d0*pi/3d0))
      return
      endif
      endif

      p=-b**2/(3d0*a**2)+c/a
      q=b/(27d0*a)*(2d0*b**2/a**2-9d0*c/a)+d/a
      delta=q**2+4d0*p**3/27d0

c       write(*,*) 'p: ',p
c       write(*,*) 'q: ',q

      if (delta.gt.0d0) then
c
      if ((-q+dsqrt(delta)).ge.0d0 ) then
      u=((-q+dsqrt(delta))/2d0)**(1d0/3d0)
      elseif ((-q+dsqrt(delta)).le.0d0) then
      u=-((q-dsqrt(delta))/2d0)**(1d0/3d0)
      endif
c
      if ((-q-dsqrt(delta)).ge.0d0) then
      v=(-(q+dsqrt(delta))/2d0)**(1d0/3d0)

      elseif((q+dsqrt(delta)).ge.0d0) then
      v=-((q+dsqrt(delta))/2d0)**(1d0/3d0)
      endif
c
      phase=dcmplx(-1d0/2d0,dsqrt(3d0)/2d0)
      x1=dcmplx(u+v)
      x2=phase*dcmplx(u)+dconjg(phase)*dcmplx(v)
      x3=(phase**2)*dcmplx(u)+dconjg(phase**2)*dcmplx(v)

      elseif (delta.eq.0d0) then
      x1=3d0*q/p
      x2=-3d0*q/2d0*p
      x3=-3d0*q/2d0*p

      elseif (delta.lt.0d0) then
      x1=2d0*dsqrt(-p/3d0)*dcos(1d0/3d0*
     & (dacos(-q*dsqrt(27d0/(-p**3))/2d0)))
      x2=2d0*dsqrt(-p/3d0)*dcos(1d0/3d0*
     & (dacos(-q*dsqrt(27d0/(-p**3))/2d0) +2d0*pi))
       x3=2d0*dsqrt(-p/3d0)*dcos(1d0/3d0*
     &  (dacos(-q*dsqrt(27d0/(-p**3))/2d0) +4d0*pi))
      endif

      x1=x1-dcmplx(b/(3d0*a))
      x2=x2-dcmplx(b/(3d0*a))
      x3=x3-dcmplx(b/(3d0*a))

      return
      end

      subroutine degree2_real(a,b,c,x1,x2)
c
c     this program compute the roots x1, x2
c     of the equation aX^2+bX+C=0
c
      implicit none
c
c     argument
c
      double precision a,b,c
      double complex x1,x2
c
c     local
c
      double precision rho

      rho=b**2-4d0*a*c
      if (rho.ge.0d0) then
      x1=dcmplx((-b+sqrt(rho))/(2d0*a))
      x2=dcmplx((-b-sqrt(rho))/(2d0*a))
      elseif (rho.lt.0d0) then
      x1=dcmplx(-b/(2d0*a),sqrt(-rho)/(2d0*a))
      x2=dcmplx(-b/(2d0*a),-sqrt(-rho)/(2d0*a))
      endif
       return
       end

      subroutine degree2_complex(a,b,c,x1,x2)
c
c     this program compute the roots x1, x2
c     of the equation aX^2+bX+C=0
c
      implicit none
c
c     parameter
c
      double precision zero,pi
      parameter (zero=0d0,pi=3.14159265358979323846d0)
      double complex i_part
      parameter (i_part=(0d0,1d0))
c
c     argument
c
      double complex a,b,c
      double complex x1,x2
c
c     local
c
      double complex rho,j1,j2
      double precision phi,phase1,phase2
      double precision module_rho,real_rho,imag_rho

      rho=b**2/4d0/a**2-c/a
      real_rho=dble(rho+dconjg(rho))/2d0
      imag_rho=dble((rho-dconjg(rho))/(2d0*i_part))
      module_rho=dsqrt(real_rho**2+imag_rho**2)

c      write(*,*) 'rho ', rho
c      write(*,*) 'real_rho ', real_rho
c      write(*,*) 'imag_rho ', imag_rho
c      write(*,*) 'module_rho ', module_rho

      if(real_rho.gt.zero) then
      phi=datan(imag_rho/real_rho)
      else if(real_rho.lt.zero) then
      phi=datan(imag_rho/real_rho)+pi
      else if(real_rho.eq.zero.and.imag_rho.GE.zero) then
      phi=pi/2d0
      else if(real_rho.eq.zero.and.imag_rho.lt.zero) then
      phi=-pi/2d0
      endif
      if(phi.lt.zero) phi=phi+2*pi

c      write(*,*) 'phi', phi
      phase1=phi/2d0
      phase2=phi/2d0+pi

      j1=dcmplx(dcos(phase1),dsin(phase1))
      j2=dcmplx(dcos(phase2),dsin(phase2))

      x1=-b/(2d0*a)+dsqrt(module_rho)*j1
      x2=-b/(2d0*a)+dsqrt(module_rho)*j2

       return
       end

       subroutine solve_2quadeqs(g11,g22,g12,g10,g20,g00,
     & h11,h22,h12,h10,h20,h00,E1,E2,sol)

       implicit none
c
c     arguments
c
      double precision g11,g12,g10,g22,g20,g00
      double precision h11,h12,h10,h22,h20,h00
      double complex E1(4),E2(4)
      logical sol(4)
c
c     local
c
      integer j
      double precision a4,a3,a2,a1,a0
      double complex w2,w1,w0
      double complex x1,x2,x3,x4,root1,root2
c
c       write(*,*) 'g11', g11
c       write(*,*) 'g22', g22
c       write(*,*) 'g12', g12
c       write(*,*) 'g10', g10
c       write(*,*) 'g20', g20
c       write(*,*) 'g00', g00

c       write(*,*) 'h11', h11
c       write(*,*) 'h22', h22
c       write(*,*) 'h12', h12
c       write(*,*) 'h10', h10
c       write(*,*) 'h20', h20
c       write(*,*) 'h00', h00

c
      a0=g11**2*h00**2 - g10*g11*h00*h10 + g00*g11*h10**2 +
     -     g10**2*h00*h11 - 2*g00*g11*h00*h11 - g00*g10*h10*h11 +
     -     g00**2*h11**2
c
      a1=-g11*g12*h00*h10 + g11*g20*h10**2 +
     -     2*g10*g12*h00*h11 - 2*g11*g20*h00*h11 - g00*g12*h10*h11 -
     -     g10*g20*h10*h11 + 2*g00*g20*h11**2 - g10*g11*h00*h12 +
     -     2*g00*g11*h10*h12 - g00*g10*h11*h12 + 2*g11**2*h00*h20 -
     -     g10*g11*h10*h20 + g10**2*h11*h20 - 2*g00*g11*h11*h20
c
      a2=g11*g22*h10**2 + g12**2*h00*h11 - 2*g11*g22*h00*h11 -
     -     g12*g20*h10*h11 - g10*g22*h10*h11 + g20**2*h11**2 +
     -     2*g00*g22*h11**2 - g11*g12*h00*h12 + 2*g11*g20*h10*h12 -
     -     g00*g12*h11*h12 - g10*g20*h11*h12 + g00*g11*h12**2 -
     -     g11*g12*h10*h20 + 2*g10*g12*h11*h20 - 2*g11*g20*h11*h20 -
     -     g10*g11*h12*h20 + g11**2*h20**2 + 2*g11**2*h00*h22 -
     -     g10*g11*h10*h22 + g10**2*h11*h22 - 2*g00*g11*h11*h22
c
      a3=-(g12*g22*h10*h11) + 2*g20*g22*h11**2 +2*g11*g22*h10*h12 -
     -     g12*g20*h11*h12 - g10*g22*h11*h12 + g11*g20*h12**2 +
     -     g12**2*h11*h20 - 2*g11*g22*h11*h20 - g11*g12*h12*h20 -
     -     g11*g12*h10*h22 + 2*g10*g12*h11*h22 - 2*g11*g20*h11*h22 -
     -     g10*g11*h12*h22 + 2*g11**2*h20*h22
c
      a4=g22**2*h11**2 - g12*g22*h11*h12 + g11*g22*h12**2 +
     -     g12**2*h11*h22 - 2*g11*g22*h11*h22 - g11*g12*h12*h22 +
     -     g11**2*h22**2

c        write(*,*) 'a4',a4
c        write(*,*) 'a3',a3
c        write(*,*) 'a2',a2
c        write(*,*) 'a1',a1
c        write(*,*) 'a0',a0

       call degree4(a4,a3,a2,a1,a0,x1,x2,x3,x4)

c       write(*,*) 'x1 ',x1
c       write(*,*) 'x2 ',x2
c       write(*,*) 'x3 ',x3
c       write(*,*) 'x4 ',x4
c
c      reorganizing the roots
c
c       if (x1.eq.x3) then
c       temp=x2
c       x2=x3
c       x3=temp
c       elseif (x1.eq.x4) then
c       temp=x2
c       x2=x4
c       x4=temp
c       endif

       E2(1)=x1
       E2(2)=x2
       E2(3)=x3
       E2(4)=x4

c       write(*,*) 'E2(1) ',E2(1)
c       write(*,*) 'E2(2) ',E2(2)
c       write(*,*) 'E2(3) ',E2(3)
c       write(*,*) 'E2(4) ',E2(4)
c
c      Now, determine E1
c

       sol(1)=.true.
       if((h11*(g10+g12*x1)-g11*(h10+h12*x1)).ne.0d0) then

       E1(1)=(-h11*(g22*x1**2+g20*x1+g00)+g11*(h22*x1**2+h20*x1+h00))
     & /(h11*(g10+g12*x1)-g11*(h10+h12*x1))
c
       elseif ((-h11*(g22*x1**2+g20*x1+g00)+
     & g11*(h22*x1**2+h20*x1+h00)).eq.0d0) then
c
c      in this case, the two equations are not independent
c
       w2=dcmplx(g11)
       w1=g12*x1+g10
       w0=g22*x1**2+g20*x1+g00
       call degree2_complex(w2,w1,w0,root1,root2)
       E1(1)=root1
       else
       write(*,*)  'warning: no solution to the 2 quadr. eqs'
       sol(1)=.false.
       endif

       sol(2)=.true.
       if((h11*(g10+g12*x2)-g11*(h10+h12*x2)).ne.0d0) then
       E1(2)=(-h11*(g22*x2**2+g20*x2+g00)+g11*(h22*x2**2+h20*x2+h00))
     & /(h11*(g10+g12*x2)-g11*(h10+h12*x2))
       elseif ((-h11*(g22*x2**2+g20*x2+g00)+
     & g11*(h22*x2**2+h20*x2+h00)).eq.0d0) then

       w2=dcmplx(g11)
       w1=g12*x2+g10
       w0=g22*x2**2+g20*x2+g00
       call degree2_complex(w2,w1,w0,root1,root2)
       E1(2)=root2
       else
       write(*,*)  'warning: no solution to the 2 quadr. eqs'
       sol(2)=.false.
       endif
c

       sol(3)=.true.
       if((h11*(g10+g12*x3)-g11*(h10+h12*x3)).ne.0d0) then
       E1(3)=(-h11*(g22*x3**2+g20*x3+g00)+g11*(h22*x3**2+h20*x3+h00))
     & /(h11*(g10+g12*x3)-g11*(h10+h12*x3))
       elseif ((-h11*(g22*x3**2+g20*x3+g00)+
     & g11*(h22*x3**2+h20*x3+h00)).eq.0d0) then

       w2=dcmplx(g11)
       w1=g12*x3+g10
       w0=g22*x3**2+g20*x3+g00
       call degree2_complex(w2,w1,w0,root1,root2)
       E1(3)=root1
       else
       write(*,*)  'warning: no solution to the 2 quadr. eqs'
       sol(3)=.false.
       endif

c

       sol(4)=.true.
       if((h11*(g10+g12*x4)-g11*(h10+h12*x4)).ne.0d0) then
       E1(4)=(-h11*(g22*x4**2+g20*x4+g00)+g11*(h22*x4**2+h20*x4+h00))
     & /(h11*(g10+g12*x4)-g11*(h10+h12*x4))
       elseif ((-h11*(g22*x4**2+g20*x4+g00)+
     & g11*(h22*x4**2+h20*x4+h00)).eq.0d0) then

       w2=dcmplx(g11)
       w1=g12*x4+g10
       w0=g22*x4**2+g20*x4+g00
       call degree2_complex(w2,w1,w0,root1,root2)
       E1(4)=root2
       else
       write(*,*)  'warning: no solution to the 2 quadr. eqs'
       sol(4)=.false.
       endif

c       write(*,*) 'E2(1) ',E2(1)
c       write(*,*) 'E2(2) ',E2(2)
c       write(*,*) 'E2(3) ',E2(3)
c       write(*,*) 'E2(4) ',E2(4)

c       write(*,*) 'sol 1 ', sol(1)
c       write(*,*) 'sol 2 ', sol(2)
c       write(*,*) 'sol 3 ', sol(3)
c       write(*,*) 'sol 4 ', sol(4)

c       do j=1,4
c       write(*,*) g11*E1(j)**2+g22*E2(j)**2+g12*E1(j)*E2(j)
c     & +g10*E1(j)+g20*E2(j)+g00
c       enddo
c       write(*,*) 'verif  eq 2 '
c       do j=1,4
c       write(*,*) h11*E1(j)**2+h22*E2(j)**2+h12*E1(j)*E2(j)
c     & +h10*E1(j)+h20*E2(j)+h00
c       enddo

        return
       end
c


!   ***************************************************************
!   * Storm Relative Helicity (SRH) is a measure of the           *
!   * streamwise vorticity within the inflow environment of a     *
!   * convective storm. It is calculated by multiplying the       *
!   * storm-relative inflow velocity vector (Vh-C) by the         *
!   * streamwise vorticity (Zh) and integrating this quantity     *
!   * over the inflow depth (lowest 1-3 km layers above ground    *
!   * level). It describes the extent to which corkscrew-like     *
!   * motion occurs (similar to the spiraling motion of an        *
!   * American football). SRH corresponds to the transfer of      *
!   * vorticity from the environment to an air parcel in          *
!   * convective motion and is used to predict the potential      *
!   * for tornadic development (cyclonic updraft rotation) in     *
!   * right-moving supercells.                                    *
!   *                                                             *
!   * There is no clear threshold value for SRH when forecasting  *
!   * supercells, since the formation of supercells appears to be *
!   * related more strongly to the deeper layer vertical shear.   *
!   * Larger values of 0-3-km SRH (greater than 250 m**2/s**2)    *
!   * and 0-1-km SRH (greater than 100 m**2/s**2), suggest an     *
!   * increased threat of tornadoes with supercells. For SRH,     *
!   * larger values are generally better, but there are no clear  *
!   * "boundaries" between non-tornadic and significant tornadic  *
!   * supercells.                                                 *
!   *                                                             *
!   * SRH < 100 (lowest 1 km): cutoff value                       *
!   * SRH = 150-299: supercells possible with weak tornadoes      *
!   * SRH = 300-499: very favorable to supercell development and  *
!   *                strong tornadoes                             *
!   * SRH > 450    : violent tornadoes                            *
!   ***************************************************************

! NCLFORTSTART
SUBROUTINE DCALRELHL(u, v, ght, ter, lat, top, sreh, miy, mjx, mkzh)
    USE wrf_constants, ONLY : PI, RAD_PER_DEG, DEG_PER_RAD

    IMPLICIT NONE

    !f2py threadsafe
    !f2py intent(in,out) :: sreh

    INTEGER, INTENT(IN) :: miy, mjx, mkzh
    REAL(KIND=8), DIMENSION(miy,mjx,mkzh), INTENT(IN) :: u, v, ght
    REAL(KIND=8), INTENT(IN) :: top
    REAL(KIND=8), DIMENSION(miy,mjx), INTENT(IN) :: ter
    REAL(KIND=8), DIMENSION(miy,mjx), INTENT(IN) :: lat
    REAL(KIND=8), DIMENSION(miy,mjx), INTENT(OUT) :: sreh

! NCLEND

    ! This helicity code was provided by Dr. Craig Mattocks, and
    ! verified by Cindy Bruyere to produce results equivalent to
    ! those generated by RIP4. (The code came from RIP4?)

    REAL(KIND=8) :: dh, sdh, su, sv, ua, va, asp, adr, bsp, bdr
    REAL(KIND=8) :: cu, cv, x, sum
    INTEGER :: i, j, k, k10, k3, ktop
    !REAL(KIND=8), PARAMETER :: DTR=PI/180.d0, DPR=180.d0/PI

    !$OMP PARALLEL DO COLLAPSE(2) PRIVATE(i,j,k,k10,k3,ktop, cu, cv, x, &
    !$OMP sum, dh, sdh, su, sv, ua, va, asp, adr, bsp, bdr) SCHEDULE(runtime)
    DO j=1, mjx
        DO i=1, miy
            sdh = 0.D0
            su = 0.D0
            sv = 0.D0
            k3 = 0
            k10 = 0
            ktop = 0
            DO k = mkzh, 2, -1
                IF (((ght(i,j,k) - ter(i,j)) .GT. 10000.D0) .AND. (k10 .EQ. 0)) THEN
                    k10 = k
                    EXIT
                ENDIF
                IF (((ght(i,j,k) - ter(i,j)) .GT. top) .AND. (ktop .EQ. 0)) THEN
                    ktop = k
                ENDIF
                IF (((ght(i,j,k) - ter(i,j)) .GT. 3000.D0) .AND. (k3 .EQ. 0)) THEN
                    k3 = k
                ENDIF
            END DO

            IF (k10 .EQ. 0) THEN
                k10 = 2
            ENDIF

            DO k = k3, k10, -1
                dh = ght(i,j,k-1) - ght(i,j,k)
                sdh = sdh + dh
                su = su + 0.5D0*dh*(u(i,j,k-1) + u(i,j,k))
                sv = sv + 0.5D0*dh*(v(i,j,k-1) + v(i,j,k))
            END DO

            ua = su/sdh
            va = sv/sdh
            asp = SQRT(ua*ua + va*va)
            IF (ua .EQ. 0.D0 .AND. va .EQ. 0.D0) THEN
                adr = 0.D0
            ELSE
                adr = DEG_PER_RAD * (PI + ATAN2(ua,va))
            ENDIF

            bsp = 0.75D0*asp

            IF (lat(i,j) .GE. 0) THEN ! Northern hemisphern
                bdr = adr + 30.D0
            ELSE ! Southern hemisphere
                bdr = adr - 30.D0
            END IF

            IF (bdr .GT. 360.D0) THEN
                bdr = bdr - 360.D0
            ENDIF

            cu = -bsp*SIN(bdr * RAD_PER_DEG)
            cv = -bsp*COS(bdr * RAD_PER_DEG)
            sum = 0.D0
            DO k = mkzh-1, ktop, -1
                x = ((u(i,j,k) - cu)*(v(i,j,k) - v(i,j,k+1))) - &
                    ((v(i,j,k) - cv)*(u(i,j,k) - u(i,j,k+1)))
                sum = sum + x
            END DO
            sreh(i,j) = -sum
        END DO
    END DO
    !$OMP END PARALLEL DO

    RETURN

END SUBROUTINE DCALRELHL


!> \file svd_EnKF.f90
!! Ensemble Kalman FIlter analysis using SVD decomposition
!! This module implements the Ensemble Kalman Filter as decribed
!! in Geir Evenson, The Ensemble Kalman Filter: theoretical formulation
!! and practical implementation. Ocean Dynamics (2003) 53: 343-367
!! DOI 10.1007/s10236-003-9
!! The paper is refered to as Evenson2003 in the module.
!<

!>\brief ensemble Kalman filter analysis
module svd_EnKF
  use debug_tools
  use general_tools
implicit none
!
!     interface EnKF_X4_analysis
!         module procedure EnKF_analysis_using_X4
!     end interface EnKF_X4_analysis
!
!     interface EnKF_X5_analysis
!         module procedure EnKF_analysis_using_X5
!     end interface EnKF_X5_analysis

contains

    !> @brief Computes the analysed ensemble in the EnKF, using SVD
    !! @param [in,out] X ensemble matrix
    !! @param [in] X_dev ensemble deviation from the ensemble mean
    !! @param [in] HX_dev observation operator applied to Xdev
    !! @param [in] Y_dev ensemble of observation perturbation (each column)
    !! @param [in] D ensemble innovations (each column is an innovation)
    !! @param [in,out] E ensemble model error
    !! @param [in] E_dev ensemble deviation from model error mean
    !! @param [in] incr boolean flag saying if increment is computed or not
    !!   The increment is the update to be applied to the background to get
    !!   the analysis. By default the increment is not computed. If computed,
    !!   the increment is returned in X_dex, resp. E_dev.
    !! @param [in, out] X4 X4 matrix
    !!
    !! @details
    !!   the number of rows of X defines the size of the state variable
    !!   the number of columns of X defines the size of the ensemble
    !!   \a E and \a E_dev are used in conjunction with state augmentation.
    !!   They have the same shape as X. If ensemble augmentation with model error
    !!   is required, both should be present, otherwise, both should be absent
    !<
    subroutine svd_EnKFAnalysis(X, X_dev, HX_dev, Y_dev, D, E, E_dev, incr, X4)
        real(dp), dimension(:,:), intent(inout) :: X
        real(dp), dimension(:,:), intent(inout) :: X_dev
        real(dp), dimension(:,:), intent(in) :: HX_dev, Y_dev, D
        real(dp), dimension(:,:), optional, intent(inout) :: E
        real(dp), dimension(:,:), optional, intent(inout) :: E_dev
        real(dp), dimension(:,:), optional, intent(inout) :: X4
        logical, optional, intent(in) :: incr
        !local variables
        real(dp), dimension( size(X,2), size(X,2) ) :: T4
        real(dp), dimension(:,:), allocatable ::  X_inc, E_inc
        !dimension variables
        integer :: ndim !! Dimension of model state
        integer :: nmem !! Number of ensemble members
        integer :: edim !! Dimension of model error
        !dgemm factor
        real(dp):: alpha!, beta
        logical :: ll_incr

        !initialization
        ndim = SIZE(X,1)
        nmem = SIZE(X,2)
        if( present(E) )then
            edim = size(E,1)
        else
            edim = -1
        end if
        if( present(incr) )then
            ll_incr = incr
        else
            ll_incr = .false.
        end if

        call debug(sum((X_dev)), 'svd_EnKFAnalysis; sum((X_dev)) = ')
        call svd_EnKF_X4(HX_dev, Y_dev, D, T4)
        call EnKF_analysis_using_X4(T4, X, X_dev, E, E_dev, incr)

!         !Updating X, X = X + X_dev*T4
!         alpha = 1.0_dp
!         call debug('updating X, X = X + X_dev*T4')
!         call dgemm('N', 'N', ndim, nmem, nmem, alpha, X_dev, ndim&
!             , T4, nmem, 1.0_dp, X, ndim)
!         !computing the increment if requested
!         if(ll_incr)then
!             allocate( X_inc(size(X,1),size(X,2) ) )
!             call dgemm('N', 'N', ndim, nmem, nmem, 1.0_dp, X_dev, ndim&
!                         , T4, nmem, 0.0_dp, X_inc, ndim)
!             X_dev = X_inc
!             deallocate(X_inc)
!         end if
!
!         !Updating the model error in the case of state augmentation
!         if( present(E) )then
!             call debug('updating E, E = E + E_dev*T4')
!             call dgemm('N', 'N', edim, nmem, nmem, alpha, E_dev, edim&
!                 , T4, nmem, 1.0_dp, E, edim)
!             !computing the increment if requested
!             if(ll_incr)then
!                 allocate( E_inc(size(E,1),size(E,2) ) )
!                 call dgemm('N', 'N', edim, nmem, nmem, 1.0_dp, E_dev, edim&
!                 , T4, nmem, 0.0_dp, E_inc, edim)
!                 E_dev = E_inc
!                 deallocate(E_inc)
!             end if
!         end if

        if(present(X4)) X4 = T4
    end subroutine svd_EnKFAnalysis

    !> @brief Compute the matrix X4 eq. 65 [Evenson 2002]
    !! @param [in] HX_dev observation operator applied to Xdev
    !! @param [in] Y_dev ensemble of observation perturbation (each column)
    !! @param [in] D ensemble innovations (each column is an innovation)
    !! @param [in, out] T4 X4 matrix (named T4 for local purpose)
    !!
    !! @details
    !!   the number of columns of HX_dev defines the size of the ensemble
    !!   T4 must be a square matrix with the number of columns/rows given
    !!   by the number of the ensemble members
    !! @note for now there is no check on the shape of matrix parameters
    !<
    subroutine svd_EnKF_X4(HX_dev, Y_dev, D, T4)
        real(dp), dimension(:,:), intent(in) :: HX_dev, Y_dev, D
        real(dp), dimension(:,:), intent(in out) :: T4
        !local variables
        real(dp), dimension(:,:), allocatable :: T, U, VT, T1, T2, T3
        real(dp), dimension(:), allocatable :: sig, sig_inv
        !SVD variables
        integer, dimension(:), allocatable :: IWORK
        real(dp), dimension(:), allocatable :: WORK
        real(dp) :: sigsum, sigsum1
        integer :: nrsigma
        !dimension variables
        integer :: nmem !! Number of ensemble members
        integer :: edim !! Dimension of model error
        integer :: nobs !! Number of observations
        integer :: minMR !!min(nmem, nobs)
        integer :: lwork, info, i

        !initialization
        nmem = SIZE(HX_dev,2)
        nobs = SIZE(D,1)
        minMR = min(nmem, nobs)

        !debugging
        call debug(nobs, 'nobs = ')
        call debug(sum(abs(HX_dev)), 'svd_EnKF_X4; sum(abs(HX_dev))')
        call debug(sum(abs(Y_dev)) , 'svd_EnKF_X4; sum(abs(Y_dev)).')
        call debug(sum(abs(D))     , 'svd_EnKF_X4; sum(abs(D)).....')
        !end of debugging

        !computing T = HX_dev + Y_dev
        allocate( T(nobs, nmem) )
        T = HX_dev + Y_dev

        !computing the SVD of T
        allocate( U(nobs,minMR), VT(minMR, nmem), sig(minMR)&
            , sig_inv(minMR), iwork(8*minMR), work(1) )
        U = 0.0_dp
        VT = 0.0_dp
        sig = 0.0_dp
        lwork = -1 !! query the optimal size of WORK
        call dgesdd('S', nobs, nmem, T, nobs, sig, U, nobs, VT&
        , minMR, work, lwork, iwork, info )
        lwork = int( work(1) )
        deallocate( work )
        call debug('work ok')
        allocate( work(lwork) )
        call debug('Computing the SVD ...')
        call dgesdd('S', nobs, nmem, T, nobs, sig, U, nobs, VT&
            , minMR, work, lwork, iwork, info )

        if(info<0)then
            call stop_program(-info, 'call to dgesdd with illegal argument number ')
        end if
        if(info>0)then
            call stop_program('DBDSDC did not converge, updating process failed')
        end if

        call debug("Done, computing the SVD.")

        !convert singular values to eigenvalues
        sig = sig*sig
        ! Compute number of significant singular values
        sigsum = sum(sig)
        call debug(sigsum, "sum(sig) = ")
        if(isNaN(sigsum)) call debug(sig, 'sig = ')
        sigsum1 = 0.0_dp
        nrsigma = 0
        i = 1
        do while((sigsum1/sigsum < 0.9999_dp).and.(i<=size(sig,1)))
            nrsigma = nrsigma + 1
            sigsum1 = sigsum1 + sig(i)
            i = i+1
        end do

        sig_inv(1:nrsigma) = 1.0_dp/sig(1:nrsigma)
        if(nrsigma<size(sig,1))then
            sig(nrsigma+1:size(sig,1))  = 0.0_dp
            sig_inv(nrsigma+1:size(sig,1))  = 0.0_dp
        end if

        call debug(sig(1:nrsigma), 'significant singular values = ')
        call debug(sig_inv(1:nrsigma), 'significant sig_inv = ')
        call debug(nrsigma, 'nrsigma (number of keeped singular values) = ')

        !computing T1 = S^-1U^t
        allocate(T1(minMR, nobs))
        do i=1, minMR
            T1(i,:) = sig_inv(i)*U(:, i)
        end do

        !computing T2 = T1*D
        call debug('computing T2 = T1*D')
        allocate( T2( minMR, nmem ) )
        call dgemm('N', 'N', minMR, nmem, nobs, 1.0_dp, T1, minMR&
            , D, nobs, 0.0_dp, T2, minMR)

        !computing T3 = U*T2
        call debug('computing T3 = U*T2')
        allocate( T3( nobs, nmem ) )
        call dgemm('N', 'N', nobs, nmem, minMR, 1.0_dp, U, nobs&
            , T2, minMR, 0.0_dp, T3, nobs)

        !deallocate SVD variables
        deallocate( work, iwork, U, VT, sig, sig_inv )

        deallocate(T1, T2)

        !Computing T4 = (HX_dev)^T*T3
        call debug('computing T4 = (HT_dev)^T*T3')
        call dgemm('T', 'N', nmem, nmem, nobs, 1.0_dp, HX_dev, nobs&
            , T3, nobs, 0.0_dp, T4, nmem)
        deallocate(T3)
    end subroutine svd_EnKF_X4

    !> @brief Compute the EnKF analysis using eq. 66 [Evenson 2002]
    !! @param [in,out] T4 updating matrix, defined as X4 in eq. 65 [Evenson 2002]
    !! @param [in,out] X ensemble matrix
    !! @param [in,out] X_dev ensemble deviation from the ensemble mean
    !! @param [in,out] E ensemble model error
    !! @param [in,out] E_dev ensemble deviation from model error mean
    !! @param [in,out] incr (optional) increment flag, says if increment is needed
    !! if increment is needed, it overrides @a X_dev
    !!
    !<
    subroutine EnKF_analysis_using_X4(T4, X, X_dev, E, E_dev, incr)
        real(dp), dimension(:,:), intent(in) :: T4
        real(dp), dimension(:,:), intent(inout) :: X
        real(dp), dimension(:,:), intent(inout) :: X_dev
        real(dp), dimension(:,:), optional, intent(inout) :: E
        real(dp), dimension(:,:), optional, intent(inout) :: E_dev
        logical, optional, intent(in) :: incr
        !local variables
        real(dp), dimension(:,:), allocatable :: X_inc, E_inc
        logical :: ll_incr
        integer :: nmem, ndim, edim

!         if(.not.(
!             ( size(T4,1)==size(T4,2) ) & !T4 must be a square matrix
!             .and.( size(T4,1)==size(X ,2) ) & ! The second dimension of X and E
!             .and.( size(T4,1)==size(E ,2) ) & ! must be equal to the dim of T4
!             .and. all( shape(X)==shape(X_dev) ) & !X_dev has the same shape as X
!             .and. all( shape(E)==shape(E_dev) ) & !E_dev has the same shape as E
!             ) ) then
!             call debug("In EnKF_analysis_using_X4: Incompatible input shapes", tag=dALLWAYS)
!             call debug("Input matrices must the exact same number of columns", tag=dALLWAYS)
!             call debug("  and each matrix and the associated deviation matrix", tag=dALLWAYS)
!             call debug("  must be the exact same size", tag=dALLWAYS)
!             call debug("Got", tag=dALLWAYS)
!             call debug(shape(T4)   , "shape(T4)    = ", tag=dALLWAYS)
!             call debug(shape(X)    , "shape(X)     = ", tag=dALLWAYS)
!             call debug(shape(X_dev), "shape(X_dev) = ", tag=dALLWAYS)
!             call debug(shape(E)    , "shape(E)     = ", tag=dALLWAYS)
!             call debug(shape(E_dev), "shape(E_dev) = ", tag=dALLWAYS)
!         end if

        nmem = SIZE(T4,1)
        ndim = SIZE(X,1)
        edim = SIZE(E,1)

        if( present(incr) )then
            ll_incr = incr
        else
            ll_incr = .false.
        end if

        !Updating X, X = X + X_dev*T4
        call debug('Computing X as X = X + X_dev*T4')
        call debug(shape(T4), "shape(T4)=")
        !computing the increment if requested
        if(ll_incr)then
            call debug('Computing the ensemble increment X_inc + X_dev*T4')
            allocate( X_inc(size(X,1),size(X,2) ) )
            call dgemm('N', 'N', ndim, nmem, nmem, 1.0_dp, X_dev, ndim&
                        , T4, nmem, 0.0_dp, X_inc, ndim)
            X_dev = X_inc
            call debug('Computing X as X = X + X_inc')
            X = X + X_inc
            deallocate(X_inc)
        else
            call dgemm('N', 'N', ndim, nmem, nmem, 1.0_dp, X_dev, ndim&
                , T4, nmem, 1.0_dp, X, ndim)
        end if

        !Updating the model error in the case of state augmentation
        if( present(E) )then
            call debug('updating E, E = E + E_dev*T4')
            call dgemm('N', 'N', edim, nmem, nmem, 1.0_dp, E_dev, edim&
                , T4, nmem, 1.0_dp, E, edim)
            !computing the increment if requested
            if(ll_incr)then
                allocate( E_inc(size(E,1),size(E,2) ) )
                call dgemm('N', 'N', edim, nmem, nmem, 1.0_dp, E_dev, edim&
                            , T4, nmem, 0.0_dp, E_inc, edim)
                E_dev = E_inc
                E = E + E_inc
                deallocate(E_inc)
            else
                call dgemm('N', 'N', edim, nmem, nmem, 1.0_dp, E_dev, edim&
                    , T4, nmem, 1.0_dp, E, edim)
            end if
        end if
    end subroutine EnKF_analysis_using_X4

    !> @brief Compute the EnKF analysis using eq. 72 [Evenson 2002]
    !! @param [in,out] T4 updating matrix, defined as X4 in eq. 65 [Evenson 2002]
    !! @param [in,out] X ensemble matrix
    !! @param [in,out] E ensemble model error
    !! @param [in,out] X_inc (optional) ensemble increment
    !! @param [in,out] E_inc (optional) ensemble model error increment
    !! @details Eq. 72 [Evenson 2002] defines a new version of the analysis
    !! equation that uses only the Ensemble states. If the equation is
    !! developped, one sees that it is the same as eq. 66 with the ensemble
    !! deviation replaced by the ensemble state. See [Evenson 2002] for
    !! the simplifications made
    !!
    !! @todo computes X5 explicitly and avoid using two ensembles.
    !!   With the right implementation of this version, it is not
    !!   possible to compute the increments
    !!
    !<
    subroutine EnKF_analysis_using_X5(T4, X, E, X_inc, E_inc)
        real(dp), dimension(:,:), intent(in) :: T4
        real(dp), dimension(:,:), intent(inout) :: X
        real(dp), dimension(:,:), optional, intent(inout) :: X_inc
        real(dp), dimension(:,:), optional, intent(inout) :: E
        real(dp), dimension(:,:), optional, intent(inout) :: E_inc
        !local variables
        real(dp), dimension(:,:), allocatable :: X_tmp, E_tmp
        integer :: nmem, ndim, edim

!         if(.not.(
!             ( size(T4,1)==size(T4,2) ) & !T4 must be a square matrix
!             .and.( size(T4,1)==size(X ,2) ) & ! The second dimension of X and E
!             .and.( size(T4,1)==size(E ,2) ) & ! must be equal to the dim of T4
!             .and. all( shape(X)==shape(X_dev) ) & !X_dev has the same shape as X
!             .and. all( shape(E)==shape(E_dev) ) & !E_dev has the same shape as E
!             ) ) then
!             call debug("In EnKF_analysis_using_X4: Incompatible input shapes", tag=dALLWAYS)
!             call debug("Input matrices must the exact same number of columns", tag=dALLWAYS)
!             call debug("  and each matrix and the associated deviation matrix", tag=dALLWAYS)
!             call debug("  must be the exact same size", tag=dALLWAYS)
!             call debug("Got", tag=dALLWAYS)
!             call debug(shape(T4)   , "shape(T4)    = ", tag=dALLWAYS)
!             call debug(shape(X)    , "shape(X)     = ", tag=dALLWAYS)
!             call debug(shape(X_dev), "shape(X_dev) = ", tag=dALLWAYS)
!             call debug(shape(E)    , "shape(E)     = ", tag=dALLWAYS)
!             call debug(shape(E_dev), "shape(E_dev) = ", tag=dALLWAYS)
!         end if

        nmem = SIZE(T4,1)
        ndim = SIZE(X,1)
        edim = SIZE(E,1)

        !Updating X, X = X*T5.
        !T5 = I + T4.
        !X = X + X*T4
        call debug('Computing X as X = X*T5')
        allocate( X_tmp(size(X,1),size(X,2) ) )
        call debug('Computing the ensemble increment X_inc = X*T4')
        call dgemm('N', 'N', ndim, nmem, nmem, 1.0_dp, X, ndim&
            , T4, nmem, 0.0_dp, X_tmp, ndim)
        call debug('Computing X as X = X + X_tmp')
        X = X + X_tmp
        !computing the increment if requested
        if( present(X_inc) )then
            X_inc = X_tmp
        end if
        deallocate(X_tmp)

        !Updating the model error in the case of state augmentation
        if( present(E) )then
            call debug('updating E, E = E + E_dev*T4')
            allocate( E_tmp(size(E,1),size(E,2) ) )
            call debug('Computing the model error increment E_inc = E*T4')
            call dgemm('N', 'N', edim, nmem, nmem, 1.0_dp, E, edim&
                , T4, nmem, 0.0_dp, E_tmp, edim)
            E = E + E_tmp
            !computing the increment if requested
            if( present(E_inc) )then
                E_inc = E_tmp
            end if
            deallocate(E_tmp)
        end if
    end subroutine EnKF_analysis_using_X5

    !> @brief Computes the analysed ensemble in the EnKF, using SVD
    !! @param [in,out] X ensemble matrix
    !! @param [in] X_dev ensemble deviation from the ensemble mean
    !! @param [in] HX_dev observation operator applied to Xdev
    !! @param [in] Y_dev ensemble of observation perturbation (each column)
    !! @param [in] D ensemble innovations (each column is an innovation)
    !! @param [in,out] E ensemble model error
    !! @param [in] E_dev ensemble deviation from model error mean
    !! @param [in] incr boolean flag saying if increment is computed or not
    !!   The increment is the update to be applied to the background to get
    !!   the analysis. By default the increment is not computed. If computed,
    !!   the increment is returned in X_dex, resp. E_dev.
    !!
    !! @details
    !!   the number of rows of X defines the size of the state variable
    !!   the number of columns of X defines the size of the ensemble
    !!   \a E and \a E_dev are used in conjunction with state augmentation.
    !!   They have the same shape as X. If ensemble augmentation with model error
    !!   is required, both should be present, otherwise, both should be absent
    !!
    !!  This is the version before September 09th 2014
    !<
    subroutine svd_EnKFAnalysis_original(X, X_dev, HX_dev, Y_dev, D, E, E_dev, incr)
        real(dp), dimension(:,:), intent(inout) :: X
        real(dp), dimension(:,:), intent(inout) :: X_dev
        real(dp), dimension(:,:), intent(in) :: HX_dev, Y_dev, D
        real(dp), dimension(:,:), optional, intent(inout) :: E
        real(dp), dimension(:,:), optional, intent(inout) :: E_dev
        logical, optional, intent(in) :: incr
        !local variables
        real(dp), dimension(:,:), allocatable :: T, U, VT, T1, T2, T3, T4, T5
        real(dp), dimension(:), allocatable :: sig, sig_inv
        !debugging variables
        !real(dp), dimension(:,:), allocatable :: Tb, U2
        !real(dp), dimension(:), allocatable :: bt, xa, xt
        !integer :: j
        !end of debugging variables
        !SVD variables
        integer, dimension(:), allocatable :: IWORK
        real(dp), dimension(:), allocatable :: WORK
        real(dp) :: sigsum, sigsum1
        integer :: nrsigma
        !dimension variables
        integer :: ndim !! Dimension of model state
        integer :: nmem !! Number of ensemble members
        integer :: edim !! Dimension of model error
        integer :: nobs !! Number of observations
        integer :: minMR !!min(nmem, nobs)
        integer :: lwork, info, i
        !inflation factor: I do not remember exactely what it is, I reset it from 2 to 1
        real(dp), parameter :: iFact = 1.0_dp
        !dgemm factor
        real(dp):: alpha!, beta
        logical :: ll_incr

        !initialization
        ndim = SIZE(X,1)
        nmem = SIZE(X,2)
        nobs = SIZE(D,1)
        minMR = min(nmem, nobs)
        if( present(E) )then
            edim = size(E,1)
        else
            edim = -1
        end if
        if( present(incr) )then
            ll_incr = incr
        else
            ll_incr = .FALSE.
        end if

        !debugging
        call debug(nobs, 'nobs = ')
        call debug(sum(abs(D)), 'sum(abs(D)) = ')
        call debug(sum(abs(X)), 'sum(abs(X)) = ')
        call debug(sum(abs(X_dev)), 'sum(abs(X_dev)) = ')
        call debug(sum(abs(HX_dev)), 'sum(abs(HX_dev)) = ')
        call debug(sum(abs(Y_dev)), 'sum(abs(Y_dev)) = ')
        !end of debugging

        !computing T = HX_dev + Y_dev
        allocate( T(nobs, nmem) )
        T = sqrt(iFact)*HX_dev + Y_dev

    !     !debugging
    !     !test variables
    !       allocate( xt(nobs), xa(nobs), bt(nobs), Tb(nobs, nobs) )
    !       xt = 0.0_dp
    !       xa = 0.0_dp
    !       bt = 0.0_dp
    !       call random_number(xt)
    !       Tb = matmul( T,transpose(T) )
    !       bt = matmul( Tb, xt )
    !     !end of test variables
    !     !end of debugging

        !computing the SVD of T
        allocate( U(nobs,minMR), VT(minMR, nmem), sig(minMR)&
            , sig_inv(minMR), iwork(8*minMR), work(1) )
        U = 0.0_dp
        VT = 0.0_dp
        sig = 0.0_dp
        lwork = -1 !! query the optimal size of WORK
        call dgesdd('S', nobs, nmem, T, nobs, sig, U, nobs, VT&
            , minMR, work, lwork, iwork, info )
        lwork = int( work(1) )
        deallocate( work )
        call debug('work ok')
        allocate( work(lwork) )
        call debug('Computing the SVD ...')
        call dgesdd('S', nobs, nmem, T, nobs, sig, U, nobs, VT&
            , minMR, work, lwork, iwork, info )

        if(info<0)then
            call stop_program(-info, 'call to dgesdd with illegal argument number ')
        end if
        if(info>0)then
            call stop_program('DBDSDC did not converge, updating process failed')
        end if

        call debug("Done, computing the SVD.")

        !convert singular values to eigenvalues
        sig = sig*sig
        !call debug(sig, "sig = ")
        ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! !
        ! Compute number of significant singular values
        sigsum = sum(sig)
        call debug(sigsum, "sum(sig) = ")
        if(isNaN(sigsum)) call debug(sig, 'sig = ')
        sigsum1 = 0.0_dp
        nrsigma = 0
        i = 1
        do while((sigsum1/sigsum < 0.9999_dp).and.(i<=size(sig,1)))
            nrsigma = nrsigma + 1
            sigsum1 = sigsum1 + sig(i)
            i = i+1
        end do

        sig_inv(1:nrsigma) = 1.0_dp/sig(1:nrsigma)
        if(nrsigma<size(sig,1))then
            sig(nrsigma+1:size(sig,1))  = 0.0_dp
            sig_inv(nrsigma+1:size(sig,1))  = 0.0_dp
        end if

        call debug(sig(1:nrsigma), 'significant singular values = ')
        call debug(sig_inv(1:nrsigma), 'significant sig_inv = ')
        call debug(nrsigma, 'nrsigma (number of keeped singular values) = ')

        !computing T1 = S^-1U^t
        allocate(T1(minMR, nobs))
        do i=1, minMR
            T1(i,:) = sig_inv(i)*U(:, i)
        end do

    !     !debugging
    !       !computing U2 = US
    !       allocate( U2(nobs,minMR) )
    !       do j=1,size(sig,1)
    !         U2(:,j) = U(:,j)*sig(j)
    !       end do
    !       !call debug(sig, "sig = ")
    !       call debug( norm2( Tb - matmul(U2,transpose(U)) ), 'residual norm (Tb-USU^t)')
    !       deallocate( U2 )
    !       call debug("========================================")
    !
    !       !test variables
    !       xa = matmul(U, matmul(T1, bt) )
    !       call debug("========================================")
    !       call debug(norm2(xt-xa)/norm2(xa), 'norm2(xt-xa)/norm2(xa) = ')
    !       call debug("========================================")
    !       allocate( xt, xa, bt, Tb )
    !       !end of test variables
    !    !end of debugging

        !computing T2 = T1*D
        call debug('computing T2 = T1*D')
        allocate( T2( minMR, nmem ) )
        call dgemm('N', 'N', minMR, nmem, nobs, 1.0_dp, T1, minMR&
            , D, nobs, 0.0_dp, T2, minMR)

        !computing T3 = U*T2
        call debug('computing T3 = U*T2')
        allocate( T3( nobs, nmem ) )
        call dgemm('N', 'N', nobs, nmem, minMR, 1.0_dp, U, nobs&
            , T2, minMR, 0.0_dp, T3, nobs)

        !deallocate SVD variables
        deallocate( work, iwork, U, VT, sig, sig_inv )

        deallocate(T1, T2)

        !Computing T4 = (HX_dev)^T*T3
        call debug('computing T4 = (HT_dev)^T*T3')
        allocate( T4(nmem, nmem) )
        call dgemm('T', 'N', nmem, nmem, nobs, 1.0_dp, HX_dev, nobs&
            , T3, nobs, 0.0_dp, T4, nmem)
        deallocate(T3)

        !Updating X, X = X + X_dev*T4
        alpha = iFact
        call debug('updating X, X = X + X_dev*T4')
        call dgemm('N', 'N', ndim, nmem, nmem, alpha, X_dev, ndim&
            , T4, nmem, 1.0_dp, X, ndim)
        !computing the increment if requested
        if(ll_incr)then
        allocate( T5(size(X,1),size(X,2) ) )
        call dgemm('N', 'N', ndim, nmem, nmem, 1.0_dp, X_dev, ndim&
                    , T4, nmem, 0.0_dp, T5, ndim)
        X_dev = T5
        deallocate(T5)
        end if

        !Updating the model error in the case of state augmentation
        if( present(E) )then
        call debug('updating E, E = E + E_dev*T4')
        call dgemm('N', 'N', edim, nmem, nmem, alpha, E_dev, edim&
            , T4, nmem, 1.0_dp, E, edim)
        !computing the increment if requested
        if(ll_incr)then
            allocate( T5(size(E,1),size(E,2) ) )
            call dgemm('N', 'N', edim, nmem, nmem, 1.0_dp, E_dev, edim&
                        , T4, nmem, 0.0_dp, T5, edim)
            E_dev = T5
            deallocate(T5)
        end if
        end if

        deallocate(T4)
    end subroutine svd_EnKFAnalysis_original

    !> \brief Pure SVD version, must be pure eigenvalues version...
    !! \param[in,out] X ensemble matrix
    !! \param[in] X_dev ensemble deviation from the ensemble mean
    !! \param[in] HX_dev observation operator applied to Xdev
    !! \param[in] Y_dev ensemble of observation perturbation (each column)
    !! \param[in] D ensemble innovations (each column is an innovation)
    !! \details
    !!   the number of rows of X defines the size of the state variable
    !!   the number of columns of X defines the size of the ensemble
    !!   Computes the analysed ensemble in the EnKF, using SVD
    !<
    subroutine svd_EnKFAnalysis_first(X, X_dev, HX_dev, Y_dev, D)
        real(dp), dimension(:,:), intent(inout) :: X
        real(dp), dimension(:,:), intent(in) :: X_dev, HX_dev, Y_dev, D
        !local variables
        real(dp), dimension(:,:), allocatable :: T, U, U2, VT, T1, T2, T3, Tb
        real(dp), dimension(:), allocatable :: sig, sig_inv, xt, bt, xa
        !SVD variables
        integer, dimension(:), allocatable :: IWORK
        real(dp), dimension(:), allocatable :: WORK
        real(dp) :: sigsum, sigsum1, alpha
        integer :: nrsigma
        !dimension variables
        integer :: ndim !! Dimension of model state
        integer :: nmem !! Number of ensemble members
        integer :: nobs !! Number of observations
        integer :: lwork, info, i

        !initialization
        ndim = SIZE(X,1)
        nmem = SIZE(X,2)
        nobs = SIZE(D,1)

        !debugging
        call debug(nobs, 'nobs = ')
        call debug(sum(abs(D)), 'sum(abs(D)) = ')
        call debug(sum(abs(X)), 'sum(abs(X)) = ')
        call debug(sum(abs(X_dev)), 'sum(abs(X_dev)) = ')
        call debug(sum(abs(HX_dev)), 'sum(abs(HX_dev)) = ')
        call debug(sum(abs(Y_dev)), 'sum(abs(Y_dev)) = ')
        !end of debugging
        !computing T = (HX_dev)*(HX_dev)^T
        allocate( T(nobs,nobs) )
        alpha = 1.0_dp/real(nmem-1,dp)
        T = 0.0_dp
        call dgemm('N', 'T', nobs, nobs, nmem, 1.0_dp, HX_dev, nobs&
                    , HX_dev, nobs, 0.0_dp, T, nobs)

        !computing T = T + Y_dev * (Y_dev)^T
        call dgemm('N', 'T', nobs, nobs, nmem, 1.0_dp, Y_dev, nobs&
                    , Y_dev, nobs, 1.0_dp, T, nobs)

        !test variables
        allocate( xt(nobs), xa(nobs), bt(nobs), Tb(nobs,nobs) )
        xt = 0.0_dp
        xa = 0.0_dp
        bt = 0.0_dp
        call random_number(xt)
        bt = matmul(T, xt)
        Tb = T
        !end of test variables

        !computing the SVD of T
        allocate( U(nobs,nobs), VT(nobs,nobs), sig(nobs)&
            , sig_inv(nobs), iwork(8*min(nobs,nobs)), work(1) )
        U = 0.0_dp
        VT = 0.0_dp
        sig = 0.0_dp
        lwork = -1 !! query the optimal size of WORK
        call dgesdd('A', nobs, nobs, T, nobs, sig, U, nobs, VT, nobs&
            , work, lwork, iwork, info )
        lwork = int( work(1) )
        deallocate( work )
        allocate( work(lwork) )
        call dgesdd('A', nobs, nobs, T, nobs, sig, U, nobs, VT, nobs&
            , work, lwork, iwork, info )

        if(info<0)then
            call stop_program(-info, 'call to dgesdd with illegal argument number ')
        end if
        if(info>0)then
            call stop_program('DBDSDC did not converge, updating process failed')
        end if

        call debug("after SVD")

        !call debug(sig, "sig = ")
        ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! !
        ! Compute number of significant singular values
        sigsum = sum(sig)
        call debug(sigsum, "sum(sig) = ")
        if(isNaN(sigsum)) call debug(sig, 'sig = ')
        sigsum1 = 0.0_dp
        nrsigma = 0
        i = 1
        do while((sigsum1/sigsum < 0.999_dp).and.(i<=size(sig,1)))
            nrsigma = nrsigma + 1
            sigsum1 = sigsum1 + sig(i)
            i = i+1
        end do

        sig_inv(1:nrsigma) = 1.0_dp/sig(1:nrsigma)
        if(nrsigma<size(sig,1))then
            sig(nrsigma+1:size(sig,1))  = 0.0_dp
            sig_inv(nrsigma+1:size(sig,1))  = 0.0_dp
        end if

        call debug(sig(1:nrsigma), 'significant singular values = ')
        call debug(sig_inv(1:nrsigma), 'significant sig_inv = ')
        call debug(nrsigma, 'nrsigma (number of keeped singular values) = ')

        !computing U2 = US
        allocate( U2(nobs,nobs) )
        do i=1,size(sig,1)
            U2(:,i) = U(:,i)*sig(i)
        end do
        !call debug(sig, "sig = ")
        call debug( norm2( Tb - matmul(U2,VT) ), 'residual norm (Tb-USV^t)' )
        deallocate( U2 )
        call debug( "========================================" )
        !computing U = US^-1
        do i=1,size(sig,1)
            U(:,i) = U(:,i)*sig_inv(i)
        end do

        !test variables
        xa = matmul( transpose(VT), matmul(transpose(U), bt) )
        call debug("========================================")
        call debug(norm2(xt-xa), 'norm2(xt-xa) = ')
        call debug("========================================")
        !end of test variables

        !computing T1 = UT*D
        call debug('computing T1 = UT*D')
        allocate( T1( nobs, nmem ) )
        call dgemm('T', 'N', nobs, nmem, nobs, 1.0_dp, U, nobs&
            , D, nobs, 0.0_dp, T1, nobs)

        !computing T2 = V*T1
        call debug('computing T2 = V*T1')
        allocate( T2( nobs, nmem ) )
        call dgemm('T', 'N', nobs, nmem, nobs, 1.0_dp, VT, nobs&
            , T1, nobs, 0.0_dp, T2, nobs)

        !deallocate SVD variables
        deallocate( work, iwork, U, VT, sig, sig_inv )

        deallocate(T1)

        !computing T3 = (HX_dev)^T*T2
        call debug('computing T3 = (HT_dev)^T*T2')
        allocate( T3(nmem, nmem) )
        call dgemm('T', 'N', nmem, nmem, nobs, 1.0_dp, HX_dev, nobs&
            , T2, nobs, 0.0_dp, T3, nmem)
        deallocate(T2)

        !updating X, X = X + X_dev*T3
        call debug('updating X, X = X + X_dev*T3')
        call dgemm('N', 'N', ndim, nmem, nmem, 1.0_dp, X_dev, ndim&
            , T3, nmem, 1.0_dp, X, ndim)
        deallocate(T3)
    end subroutine svd_EnKFAnalysis_first

    ! +==========================================================
    ! RBCG version
    !! Ensemble Kalman Filter analysis using conjugate gradient algorithm
    !! This module implements the Ensemble Kalman Filter as decribed
    !! in Geir Evenson, The Ensemble Kalman Filter: theoretical formulation
    !! and practical implementation. Ocean Dynamics (2003) 53: 343-367
    !! DOI 10.1007/s10236-003-9
    !! The paper is refered to as Evenson2003 in the module.
    !! The main difference is that the RBCG algorithm
    !! is used to solved the system in the place of the SVD
    !<

    !> @brief Compute the matrix X4 eq. 65 [Evenson 2002] using rbcg
    !! @param [in] HX_dev observation operator applied to Xdev
    !! @param [in] Y_dev ensemble of observation perturbation (each column)
    !! @param [in] D ensemble innovations (each column is an innovation)
    !! @param [in, out] T4 X4 matrix (named T4 for local purpose)
    !! @param [in] obs_std standard deviation of observations
    !! @param [in] nItrMax optional maximum number of iteration
    !!    of the conjugate gradient, default 100
    !! @param [in] tol optional tolerance minimum of the CG
    !!    ratio of gradient, default 10^-16
    !!
    !! @details
    !!   the number of columns of HX_dev defines the size of the ensemble
    !!   T4 must be a square matrix with the number of columns/rows given
    !!   by the number of the ensemble members
    !!   @a state_coord, @a obs_coord and @a cls are used for localization
    !!   in the covariance. There is no special requirement on the units
    !!   of the coordinates, but they must be consistent with the @a cls.
    !!   for example, the coordinates can be the indices in the model grid
    !!   and the @a cls be given in terms of number of grid points.
    !!   the coordinates can also be the geographical coordinates
    !!   (lon/lat/alt etc.).
    !!   This subroutine does not know anything about the number of physical
    !!   dimensions of the system, this allows the user to apply the
    !!   localization only in the chosen direction(s) for exemple, one can
    !!   choose to localize only along the x direction, or along x and y.
    !!   The number of directions for the localization is defined by the
    !!   number of column (second dimension) of the coordinate vectors.
    !!   The firs dimension correspond to the number of element of state
    !!   or observation vector.
    !!   @a cls is a 1-D vector of size the number of directions of
    !!   the localization
    !!
    !! @note for now there is no check on the shape of matrix parameters
    !<
    subroutine rbcg_EnKF_X4(HX_dev, Y_dev, D, T4, obs_std, nItrMax, tol)
        real(dp), dimension(:,:), intent(in) :: HX_dev, Y_dev, D
        real(dp), dimension(:,:), intent(in out) :: T4
        real(dp), dimension(:), intent(in) :: obs_std
        integer, optional, intent(in) :: nItrMax
        real(dp), optional, intent(in) :: tol
        !local variables
        real(dp), dimension(size(HX_dev,1)) :: x
        !dimension variables
        integer :: nmem !! Number of ensemble members
        integer :: edim !! Dimension of model error
        integer :: nobs !! Number of observations
        integer :: i,j
        !
        integer :: il_nItrMax
        real(dp):: rl_tol

        !initialization
        nmem = SIZE(HX_dev,2)
        nobs = SIZE(D,1)
        if( present(nItrMax) )then
            il_nItrMax = nItrMax
        else
            il_nItrMax = 100
        end if
        if( present(tol) )then
            rl_tol = tol
        else
            rl_tol = 1.0d-16
        end if

        !debugging
        call debug(nobs, 'nobs = ')
        call debug(sum(abs(HX_dev)), 'svd_EnKF_X4; sum(abs(HX_dev))')
        call debug(sum(abs(Y_dev)) , 'svd_EnKF_X4; sum(abs(Y_dev)).')
        call debug(sum(abs(D))     , 'svd_EnKF_X4; sum(abs(D)).....')
        !end of debugging
        T4 = 999.0
        do j = 1, nmem
            !Solve the linear system for one innovation member
            call debug(j, "RBCG, Solving the linear system for member", tag=dALLWAYS)
            call rbcg_solve(nobs, nmem, HX_dev, obs_std, D(:,j), x, il_nItrMax, rl_tol)

            ! multiply the result by the transpose of HX_dev
            ! this does not apply the localization
            do i = 1, nmem
                T4(i,j) = dot_product( HX_dev(:,i), x )
            end do
        end do
    end subroutine rbcg_EnKF_X4

    !> @brief Solve the Ensemble Kalman Filter system for one member using RBCG
    !! @param [in] nRow number of row of HA, this is the size of the system
    !! @param [in] nCol number of columns of HA
    !! @param [in] HA observation operator applied to ensemble deviation from the mean
    !! @param [in] obs_std vector of standard deviation of observation error
    !! @param [in] d innovation of one member of the ensemble
    !! @param [in,out] x solution of the system
    !! @param [in] nItrMax maximum number of iterations
    !! @param [in] tol stopping criteria based on the B-norm of the gradient
    !!
    !! @details This algorithm solves only for one member, the user must call it
    !! in a loop to solve for all members
    !! It is important to recall that is only the linear systems in the
    !! EnKF analysis equation: (HAA^TH^T + R)^-1D, see eq 54 Evenson2003
    !<
    subroutine rbcg_solve(nRow, nCol, HA, obs_std, d, x, nItrMax, tol)
        integer, intent(in) :: nRow, nCol
        real(dp), dimension(nRow, nCol), intent(in) :: HA
        real(dp), dimension(nRow), intent(in) :: obs_std
        real(dp), dimension(nRow), intent(in) :: d
        real(dp), dimension(nRow), intent(in out) :: x
        integer, optional, intent(in) :: nItrMax
        real(dp), optional, intent(in) :: tol
        !local variables
        !RBCG variables
        real(dp) :: cg_r(nRow)
        real(dp) :: cg_rtmp(nRow)
        real(dp) :: cg_lambda(nRow)
        real(dp) :: cg_t(nRow)
        real(dp) :: cg_beta
        real(dp) :: cg_p(nRow)
        real(dp) :: cg_alpha
        real(dp) :: cg_w(nRow)
        real(dp) :: cg_q(nRow)
        !others
        real(dp) :: r0_norm ! B-norm of the residue at the zeroth iteration
        real(dp) :: ri_norm ! B-norm of the residue at the current iteration
        real(dp) :: cg_wTr !dot product w by r (i)
        real(dp) :: cg_qTt !dot product q by t
        real(dp) :: cg_wTrp1 !dot product w by r (i+1)
        real(dp) :: cg_gNorm !b-norm of the gradient
        real(dp) :: cg_omega, cg_epsilon
        integer :: cg_itr
        integer :: j
        logical :: converged
        character(*), parameter :: fmt0= "('*** RBCG   Iter ***',I3,' -- |r_0|=', ,ES14.5E3)"
        character(*), parameter :: fmti= "('*** RBCG   Iter ***',I3,' -- |r|/|r_0|=', ,ES14.5E3)"

        call debug(tol, "RBCG - tol=", tag=dALLWAYS)
        call debug(nItrMax, "RBCG - nItrMax=", tag=dALLWAYS)
        cg_epsilon = epsilon(1.0_dp)
        cg_itr = 0
        cg_lambda = 0.0
        cg_r = d/obs_std**2
        cg_p = cg_r
        !computing HA(HA)^Tr
        call matrixmult(nRow, nCol, HA, cg_r, cg_w)
        cg_t = cg_w

        cg_wTr = dot_product(cg_w, cg_r)
        ri_norm = sqrt(cg_wTr)
        r0_norm    = ri_norm
        cg_omega= ri_norm/r0_norm
        write(*,fmt=fmt0)cg_itr,r0_norm
        cg_itr = 1
        converged = .false.

        do while( (cg_itr<= nItrMax).and.(.not.converged) )
            cg_q = cg_t/obs_std**2 + cg_p
            !The following two quantities are used as denominator
            !So if one is zero, either, there is convergence or something is wrong
            cg_wTr = dot_product(cg_w, cg_r)
            cg_qTt = dot_product(cg_q, cg_t)
            if( (cg_qTt>cg_epsilon).and.(cg_wTr>cg_epsilon) )then
                cg_alpha = cg_wTr/cg_qTt
                cg_lambda = cg_lambda + cg_alpha*cg_p
                cg_r = cg_r - cg_alpha*cg_q
                !re-conjugate r
                call matrixmult(nRow, nCol, HA, cg_r, cg_w)
                cg_wTrp1 = dot_product(cg_w, cg_r)
                cg_beta = cg_wTrp1/cg_wTr
                if( cg_beta<cg_epsilon )then
                    converged = .true.
                end if
                cg_p = cg_r + cg_beta*cg_p
                cg_t = cg_w + cg_beta*cg_t
                ri_norm = sqrt(cg_wTrp1)
                cg_omega= ri_norm/r0_norm

                !Check the convergence
                if (cg_omega > tol) then
                    write(*,fmt=fmti)cg_itr,cg_omega
                else
                    converged = .true.
                endif
                !
                !Moving to the next iteration
                !
                cg_itr=cg_itr+1
            else
                converged = .true. ! the denominator of alpha is zero or the denominator of beta is zero
            end if
        end do

        if( .not.converged ) then
            call debug( nItrMax, "Maximum number of iterations reached", tag=dALLWAYS )
        else
            call debug(cg_itr, "RBCG converged, total nb iterations", tag=dALLWAYS)
            call debug(sum(abs(cg_lambda)), "Solution sum(abs(cg_lambda))")
            if(cg_qTt<=cg_epsilon)call debug(cg_qTt, "q*t is too small", tag=dALLWAYS)
            if(cg_wTr<=cg_epsilon)call debug(cg_wTr, "w*r is too small", tag=dALLWAYS)
            if(cg_beta<=cg_epsilon)call debug(cg_beta, "beta is too small", tag=dALLWAYS)
        end if

        x = cg_lambda
    end subroutine rbcg_solve

    !> @brief apply the the matrix AA^T to x and returns the result in y
    !! @param [in] nRow number of row of A, this is the size of the system
    !! @param [in] nCol number of columns of A
    !! @param [in] A factor of the matrix of the system
    !! @param [in] x input vector
    !! @param [in,out] y output vector
    !<
    subroutine matrixmult(nRow, nCol, A, x, y)
        integer, intent(in) :: nRow, nCol
        real(dp), dimension(nRow, nCol), intent(in) :: A
        real(dp), dimension(nRow), intent(in) :: x
        real(dp), dimension(nRow), intent(in out) :: y
        !local variables
        integer :: j

        y = 0.0
        do j = 1, nCol
            y = y + dot_product( A(:,j), x)*A(:,j)
        end do
    end subroutine matrixmult

end module svd_EnKF


!
! Copyright (c) 2019, NVIDIA CORPORATION.  All rights reserved.
!
! Licensed under the Apache License, Version 2.0 (the "License");
! you may not use this file except in compliance with the License.
! You may obtain a copy of the License at
!
!     http://www.apache.org/licenses/LICENSE-2.0
!
! Unless required by applicable law or agreed to in writing, software
! distributed under the License is distributed on an "AS IS" BASIS,
! WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
! See the License for the specific language governing permissions and
! limitations under the License.
!
module matmul_cublas_m
contains
   subroutine matmul_cublas(A, B, D, m, n, k)
      use cudafor
      use cublas

      integer :: m, n, k

      real, device, intent(in) :: A(:,:)
      real, device, intent(in) :: B(:,:)
      real, device, intent(out) :: D(:,:)

! CALL SGEMM(transa, transb, l, n, m, alpha, a, lda, b, ldb, beta, ldc)
!
! l = rows in C
! n = number of cols in C
! m = cols in A (if 'n')
! alpha = scalar alpha
! a is the matrix A - A(l,m)
! lda = leading dimension of A (lda >= n)
! b is the matrix B - B(m,n)
! ldb leading dimension of b (ldb >= m)
! beta = scalar beta
! c =C(l,n)
! ldc = leading dimension of c

      call sgemm('n','n', m, n, k, 1.0, A, m, B, k, 0.0, D, m)

!      D = D_d

      return
   end subroutine
end module


program test_ewald

! This test compares lattice energy 'eew' calculated from the Ewald summation
! against the energy calculated using the Madelung constant for various lattice
! constants L. The agreement is essentially to machine precision.

use types, only: dp
use constants, only: ang2bohr, kJmol2Ha
use ewald_sums, only: ewald, fred2fcart, direct_sum
use utils, only: assert, init_random
implicit none

integer :: natom, ntypat
! Various NaCl lattice constants in A
real(dp), parameter :: Llist(5) = [5.6402_dp, 5.5_dp, 4.5_dp, 6.5_dp, 10._dp]
real(dp) :: ucvol
integer, allocatable :: typat(:)
real(dp) :: gmet(3, 3), rmet(3, 3), gprim(3, 3)
real(dp) :: eew
real(dp), allocatable :: xred(:, :), zion(:), grewtn(:, :), fcart(:, :)

real(dp) :: L, alpha, E_ewald, E_madelung, E_direct
integer :: i, ncut

alpha = 1.74756459463318219064_dp ! Madelung constant for NaCl

! Conventional cell:
natom = 8
ntypat = 2
allocate(xred(3, natom), zion(ntypat), grewtn(3, natom), typat(natom), &
    fcart(3, natom))
! Cl^-
xred(:, 1) = [0._dp, 0._dp, 0._dp]
xred(:, 2) = [1._dp/2, 1._dp/2, 0._dp]
xred(:, 3) = [1._dp/2, 0._dp, 1._dp/2]
xred(:, 4) = [0._dp, 1._dp/2, 1._dp/2]
! Na^+
xred(:, 5) = [1._dp/2, 1._dp/2, 1._dp/2]
xred(:, 6) = [1._dp/2, 0._dp, 0._dp]
xred(:, 7) = [0._dp, 1._dp/2, 0._dp]
xred(:, 8) = [0._dp, 0._dp, 1._dp/2]
typat = [1, 1, 1, 1, 2, 2, 2, 2]
zion = [-1._dp, +1._dp]

do i = 1, size(Llist)
    L = Llist(i) * ang2bohr

    rmet = 0
    rmet(1, 1) = L**2
    rmet(2, 2) = L**2
    rmet(3, 3) = L**2

    ! gmet = inv(rmet)
    gmet = 0
    gmet(1, 1) = 1/L**2
    gmet(2, 2) = 1/L**2
    gmet(3, 3) = 1/L**2

    ! ucvol = sqrt(det(rmet))
    ucvol = L**3

    ! Reciprocal primitive vectors (without 2*pi) in cartesian coordinates.
    ! gmet = matmul(transpose(gprim), gprim)
    gprim = 0
    gprim(1, 1) = 1 / L
    gprim(2, 2) = 1 / L
    gprim(3, 3) = 1 / L

    call ewald(eew,gmet,grewtn,natom,ntypat,rmet,typat,ucvol,xred,zion)

    E_ewald = eew / (natom/ntypat)

    ncut = 20
    call direct_sum([-1, -1, -1, -1, 1, 1, 1, 1]*1._dp, xred*L, L, ncut, eew, &
        fcart)
    E_direct = eew / (natom/ntypat)

    E_madelung = -2*alpha/L
    print *, "a =", L/ang2bohr*100, "pm"
    print *, "Madelung:    ", E_madelung / kJmol2Ha, "kJ/mol"
    print *, "Ewald:       ", E_ewald / kJmol2Ha, "kJ/mol"
    print *, "Direct:      ", E_direct / kJmol2Ha, "kJ/mol"
    print *, "Ewald error: ", abs(E_ewald - E_madelung), "a.u."
    print *, "Direct error:", abs(E_direct - E_madelung), "a.u."
    call fred2fcart(fcart, grewtn, gprim)
    call assert(abs(E_ewald - E_madelung) < 1e-14_dp)
    call assert(abs(E_direct - E_madelung) < 1e-8_dp)
    call assert(all(abs(fcart) < 1e-15_dp))
end do
deallocate(xred, zion, grewtn, typat, fcart)

print *, "--------"

! Primitive cell (FCC lattice)
natom = 2
ntypat = 2
allocate(xred(3, natom), zion(ntypat), grewtn(3, natom), typat(natom), &
    fcart(3, natom))
! Cl^-
xred(:, 1) = [0._dp, 0._dp, 0._dp]
! Na^+
xred(:, 2) = [1._dp/2, 1._dp/2, 1._dp/2]
typat = [1, 2]
zion = [-1._dp, +1._dp]

do i = 1, size(Llist)
    L = Llist(i) * ang2bohr

    rmet(1, :) = [2._dp, 1._dp, 1._dp]
    rmet(2, :) = [1._dp, 2._dp, 1._dp]
    rmet(3, :) = [1._dp, 1._dp, 2._dp]
    rmet = rmet * L**2 / 4

    ! gmet = inv(rmet)
    gmet(1, :) = [3._dp, -1._dp, -1._dp]
    gmet(2, :) = [-1._dp, 3._dp, -1._dp]
    gmet(3, :) = [-1._dp, -1._dp, 3._dp]
    gmet = gmet / L**2

    ! ucvol = sqrt(det(rmet))
    ucvol = L**3 / 4

    ! Reciprocal primitive vectors (without 2*pi) in cartesian coordinates.
    ! gmet = matmul(transpose(gprim), gprim)
    gprim(:, 1) = [1, 1, -1] / L
    gprim(:, 2) = [-1, 1, 1] / L
    gprim(:, 3) = [1, -1, 1] / L

    call ewald(eew,gmet,grewtn,natom,ntypat,rmet,typat,ucvol,xred,zion)

    E_ewald = eew / (natom/ntypat)
    E_madelung = -2*alpha/L
    print *, "a =", L/ang2bohr*100, "pm"
    print *, "Ewald:   ", E_ewald / kJmol2Ha, "kJ/mol"
    print *, "Madelung:", E_madelung / kJmol2Ha, "kJ/mol"
    print *, "error:   ", abs(E_ewald - E_madelung), "a.u."
    call assert(abs(E_ewald - E_madelung) < 1e-14_dp)

    call fred2fcart(fcart, grewtn, gprim)
    call assert(all(abs(fcart) < 1e-15_dp))
end do
deallocate(xred, zion, grewtn, typat, fcart)
end program


      SUBROUTINE tetr(dvol,x,xcen)
!---------------------------------------------------
!***  calculates volume of a tetrahedral
!---------------------------------------------------

      IMPLICIT NONE

      REAL(kind=8):: dvol,x(3,3),xcen(3)

      REAL(kind=8) :: x1(3),x2(3),x3(3),xp(3)

      X1(:) = X(:,2)-X(:,1)
      X2(:) = X(:,3)-X(:,1)
      X3(:) = X(:,1)-XCEN(:)
      CALL vecpro(x1,x2,xp)
      dvol = DOT_PRODUCT(x3,xp)
      dvol = dvol/6.d0

      END SUBROUTINE tetr


module numerical_methods

use parameters
use constants
use odes

implicit none

private GrowStepsize, ShrinkStepsize

public RKF

contains

subroutine RKF(y,c)
!Integrate using the Runge-Kutta-Fehlberg algorithm

!Arguments
real(kind=dp), dimension(6), intent(inout) :: y
real(kind=dp), dimension(4) ,intent(inout) :: c

!Other
real(kind=dp), dimension(size(y)) :: y1,y2,y3,y4,y5,y6
real(kind=dp), dimension(size(y)) :: k1,k2,k3,k4,k5,k6
real(kind=dp), dimension(size(y)) :: dy1, dy2, dy3, dy4, dy5,dy6
real(kind=dp), dimension(size(y)) :: ynew, yerr
real(kind=dp), dimension(size(y)) :: deltaErr, yscal, ratio
real(kind=dp) :: errmax, h

!Some reals used for intersection search

!The x cooordinate at this timestep
real(kind=dp) :: x
! the difference between the x coodinate and the xtarget
real(kind=dp) :: dx, dr
!shows we are now iterating close to intersection points
integer(kind=dp) :: intersecting

!Define a logical condition used when finding ds
logical :: overshoot_condition

intersecting = 0

!print *, ds_condition
!stop

11 continue

h = c(3)

! Y1
y1 = y
call derivs(y1,c,dy1)
k1 = h * dy1

!Y2
y2 = y1 + B21*k1
call derivs(y2,c,dy2)
k2 = h * dy2

!Y3
y3 = y1 + B31*k1 + B32*k2
call derivs(y3,c,dy3)
k3 = h * dy3

!Y4
y4 = y1 + B41*k1 + B42*k2 + B43*k3
call derivs(y4,c,dy4)
k4 = h * dy4

!Y5
y5 = y1 + B51*k1 + B52*k2 + B53*k3 + B54*k4
call derivs(y5,c,dy5)
k5 = h * dy5

!Y6
y6 = y1 + B61*k1 + B62*k2 + B63*k3 + B64*k4 + B65*k5
call derivs(y6,c,dy6)
k6 = h * dy6

!Update
ynew = y1 + c1*k1  + c3*k3 + c4*k4  +c6*k6
yerr = y1 + cbar1*k1 + cbar3*k3 + cbar4*k4 + cbar5*k5 + cbar6*k6

deltaErr = abs(ynew - yerr)
yscal = abs(y1) + abs(k1) + 1.0d-3
ratio = deltaErr/yscal
errmax = escal * maxval(ratio)

!Some conditions that are only used in shooting mode

if (mode .EQ. 'shoot') then
    !Search for overstep c.f. target point

    x = sqrt(ynew(1)**2 + a2) *sin(ynew(2))*cos(ynew(3))
    dx = x - xTarget

    if (y(1) .eq. ynew(1)) then
    !stepsize is so small that variables are no longer updating

    !This happens when the ray never reaches rTarget

    !e.g. if rTarget is = 160, but the ray doesnt pass that contour

    !In that case we set the new r to be super large so the optimiser knows this is a bad step

    y = ynew
    c(3) = -2.0_dp !Signifies to outer routine to quit
    y(1) = 1e10

!    print *, 'sds1', dx_eps
!    stop
    !and exit
    return
    endif

    if (abs(dx) .LT. dx_eps) then
    !We have got close enough to the target point
    !Update and exit
    y = ynew
    c(3) = -1.0_dp !Signifies to outer routine to quit
 !   print *, 'ere'
 !   stop
   return
    endif

    !Define the overshoot condition
    if (xTarget .LT. 0.0_dp) then
    !    overshoot_condition = (dx .GT. 0.0_dp .and. k1(1) .GT. 0.0_dp)
        overshoot_condition = (dx .LT. 0.0_dp .and. k1(1) .GT. 0.0_dp)
    else
        overshoot_condition = (dx .LT. 0.0_dp .and. k1(1) .LT. 0.0_dp)
    endif

    if (overshoot_condition) then
    !Overstepped
    !Step again with smaller h
    !set intersecting = 1 to signify to the rest of the program that we are near the intersection point
    !This switches us to a fixed stepsize which is only updated below
    intersecting = 1
    c(3) = c(3) / 1.10_dp
    goto 11
    endif

endif

if (intersecting .EQ. 0) then

!Just update adaptively as usual

    if (errmax .GT. 1.0_dp) then
    !This is not good. Do not update yOUT and reduce the stepsize
    call ShrinkStepsize(errmax,h)
    c(3) = h
    goto 11
    else
    !This is good. Update yOUT and try to increase the stepsize a little bit
    call GrowStepsize(errmax,h)
    c(3) = h
    y = ynew
    endif

else
!switch to a fixed stepsize

    y = ynew
    c(3) = h
    goto 11
endif

end subroutine RKF

subroutine GrowStepsize(errmax,h)
real(kind=dp) :: errmax,h
if (errmax .GT. errcon) then
h = S*h*errmax**Pgrow
else
h = h * 5.0_dp
endif
end subroutine GrowStepsize

subroutine ShrinkStepsize(errmax,h)
real(kind=dp) :: errmax, h
real(kind=dp) :: htemp

htemp = S*h*errmax**Pshrink
h = sign(max(abs(htemp),0.10_dp*abs(h)),h)

end subroutine ShrinkStepsize

end module numerical_methods


*DECK DXNRMP
      SUBROUTINE DXNRMP (NU, MU1, MU2, DARG, MODE, DPN, IPN, ISIG,
     1   IERROR)
C***BEGIN PROLOGUE  DXNRMP
C***PURPOSE  Compute normalized Legendre polynomials.
C***LIBRARY   SLATEC
C***CATEGORY  C3A2, C9
C***TYPE      DOUBLE PRECISION (XNRMP-S, DXNRMP-D)
C***KEYWORDS  LEGENDRE FUNCTIONS
C***AUTHOR  Lozier, Daniel W., (National Bureau of Standards)
C           Smith, John M., (NBS and George Mason University)
C***DESCRIPTION
C
C        SUBROUTINE TO CALCULATE NORMALIZED LEGENDRE POLYNOMIALS
C        (XNRMP is single-precision version)
C        DXNRMP calculates normalized Legendre polynomials of varying
C        order and fixed argument and degree. The order MU and degree
C        NU are non-negative integers and the argument is real. Because
C        the algorithm requires the use of numbers outside the normal
C        machine range, this subroutine employs a special arithmetic
C        called extended-range arithmetic. See J.M. Smith, F.W.J. Olver,
C        and D.W. Lozier, Extended-Range Arithmetic and Normalized
C        Legendre Polynomials, ACM Transactions on Mathematical Soft-
C        ware, 93-105, March 1981, for a complete description of the
C        algorithm and special arithmetic. Also see program comments
C        in DXSET.
C
C        The normalized Legendre polynomials are multiples of the
C        associated Legendre polynomials of the first kind where the
C        normalizing coefficients are chosen so as to make the integral
C        from -1 to 1 of the square of each function equal to 1. See
C        E. Jahnke, F. Emde and F. Losch, Tables of Higher Functions,
C        McGraw-Hill, New York, 1960, p. 121.
C
C        The input values to DXNRMP are NU, MU1, MU2, DARG, and MODE.
C        These must satisfy
C          1. NU .GE. 0 specifies the degree of the normalized Legendre
C             polynomial that is wanted.
C          2. MU1 .GE. 0 specifies the lowest-order normalized Legendre
C             polynomial that is wanted.
C          3. MU2 .GE. MU1 specifies the highest-order normalized Leg-
C             endre polynomial that is wanted.
C         4a. MODE = 1 and -1.0D0 .LE. DARG .LE. 1.0D0 specifies that
C             Normalized Legendre(NU, MU, DARG) is wanted for MU = MU1,
C             MU1 + 1, ..., MU2.
C         4b. MODE = 2 and -3.14159... .LT. DARG .LT. 3.14159... spec-
C             ifies that Normalized Legendre(NU, MU, COS(DARG)) is
C             wanted for MU = MU1, MU1 + 1, ..., MU2.
C
C        The output of DXNRMP consists of the two vectors DPN and IPN
C        and the error estimate ISIG. The computed values are stored as
C        extended-range numbers such that
C             (DPN(1),IPN(1))=NORMALIZED LEGENDRE(NU,MU1,DX)
C             (DPN(2),IPN(2))=NORMALIZED LEGENDRE(NU,MU1+1,DX)
C                .
C                .
C             (DPN(K),IPN(K))=NORMALIZED LEGENDRE(NU,MU2,DX)
C        where K = MU2 - MU1 + 1 and DX = DARG or COS(DARG) according
C        to whether MODE = 1 or 2. Finally, ISIG is an estimate of the
C        number of decimal digits lost through rounding errors in the
C        computation. For example if DARG is accurate to 12 significant
C        decimals, then the computed function values are accurate to
C        12 - ISIG significant decimals (except in neighborhoods of
C        zeros).
C
C        The interpretation of (DPN(I),IPN(I)) is DPN(I)*(IR**IPN(I))
C        where IR is the internal radix of the computer arithmetic. When
C        IPN(I) = 0 the value of the normalized Legendre polynomial is
C        contained entirely in DPN(I) and subsequent double-precision
C        computations can be performed without further consideration of
C        extended-range arithmetic. However, if IPN(I) .NE. 0 the corre-
C        sponding value of the normalized Legendre polynomial cannot be
C        represented in double-precision because of overflow or under-
C        flow. THE USER MUST TEST IPN(I) IN HIS/HER PROGRAM. In the case
C        that IPN(I) is nonzero, the user could rewrite his/her program
C        to use extended range arithmetic.
C
C
C
C        The interpretation of (DPN(I),IPN(I)) can be changed to
C        DPN(I)*(10**IPN(I)) by calling the extended-range subroutine
C        DXCON. This should be done before printing the computed values.
C        As an example of usage, the Fortran coding
C              J = K
C              DO 20 I = 1, K
C              CALL DXCON(DPN(I), IPN(I),IERROR)
C              IF (IERROR.NE.0) RETURN
C              PRINT 10, DPN(I), IPN(I)
C           10 FORMAT(1X, D30.18 , I15)
C              IF ((IPN(I) .EQ. 0) .OR. (J .LT. K)) GO TO 20
C              J = I - 1
C           20 CONTINUE
C        will print all computed values and determine the largest J
C        such that IPN(1) = IPN(2) = ... = IPN(J) = 0. Because of the
C        change of representation caused by calling DXCON, (DPN(I),
C        IPN(I)) for I = J+1, J+2, ... cannot be used in subsequent
C        extended-range computations.
C
C        IERROR is an error indicator. If no errors are detected,
C        IERROR=0 when control returns to the calling routine. If
C        an error is detected, IERROR is returned as nonzero. The
C        calling routine must check the value of IERROR.
C
C        If IERROR=212 or 213, invalid input was provided to DXNRMP.
C        If IERROR=201,202,203, or 204, invalid input was provided
C        to DXSET.
C        If IERROR=205 or 206, an internal consistency error occurred
C        in DXSET (probably due to a software malfunction in the
C        library routine I1MACH).
C        If IERROR=207, an overflow or underflow of an extended-range
C        number was detected in DXADJ.
C        If IERROR=208, an overflow or underflow of an extended-range
C        number was detected in DXC210.
C
C***SEE ALSO  DXSET
C***REFERENCES  Smith, Olver and Lozier, Extended-Range Arithmetic and
C                 Normalized Legendre Polynomials, ACM Trans on Math
C                 Softw, v 7, n 1, March 1981, pp 93--105.
C***ROUTINES CALLED  DXADD, DXADJ, DXRED, DXSET, XERMSG
C***REVISION HISTORY  (YYMMDD)
C   820712  DATE WRITTEN
C   890126  Revised to meet SLATEC CML recommendations.  (DWL and JMS)
C   901019  Revisions to prologue.  (DWL and WRB)
C   901106  Changed all specific intrinsics to generic.  (WRB)
C           Corrected order of sections in prologue and added TYPE
C           section.  (WRB)
C           CALLs to XERROR changed to CALLs to XERMSG.  (WRB)
C   920127  Revised PURPOSE section of prologue.  (DWL)
C***END PROLOGUE  DXNRMP
      INTEGER NU, MU1, MU2, MODE, IPN, ISIG
      DOUBLE PRECISION DARG, DPN
      DIMENSION DPN(*), IPN(*)
      DOUBLE PRECISION C1,C2,P,P1,P2,P3,S,SX,T,TX,X,DK
C CALL DXSET TO INITIALIZE EXTENDED-RANGE ARITHMETIC (SEE DXSET
C LISTING FOR DETAILS)
C***FIRST EXECUTABLE STATEMENT  DXNRMP
      IERROR=0
      CALL DXSET (0, 0, 0.0D0, 0,IERROR)
      IF (IERROR.NE.0) RETURN
C
C        TEST FOR PROPER INPUT VALUES.
C
      IF (NU.LT.0) GO TO 110
      IF (MU1.LT.0) GO TO 110
      IF (MU1.GT.MU2) GO TO 110
      IF (NU.EQ.0) GO TO 90
      IF (MODE.LT.1 .OR. MODE.GT.2) GO TO 110
      GO TO (10, 20), MODE
   10 IF (ABS(DARG).GT.1.0D0) GO TO 120
      IF (ABS(DARG).EQ.1.0D0) GO TO 90
      X = DARG
      SX = SQRT((1.0D0+ABS(X))*((0.5D0-ABS(X))+0.5D0))
      TX = X/SX
      ISIG = LOG10(2.0D0*NU*(5.0D0+TX**2))
      GO TO 30
   20 IF (ABS(DARG).GT.4.0D0*ATAN(1.0D0)) GO TO 120
      IF (DARG.EQ.0.0D0) GO TO 90
      X = COS(DARG)
      SX = ABS(SIN(DARG))
      TX = X/SX
      ISIG = LOG10(2.0D0*NU*(5.0D0+ABS(DARG*TX)))
C
C        BEGIN CALCULATION
C
   30 MU = MU2
      I = MU2 - MU1 + 1
C
C        IF MU.GT.NU, NORMALIZED LEGENDRE(NU,MU,X)=0.
C
   40 IF (MU.LE.NU) GO TO 50
      DPN(I) = 0.0D0
      IPN(I) = 0
      I = I - 1
      MU = MU - 1
      IF (I .GT. 0) GO TO 40
      ISIG = 0
      GO TO 160
   50 MU = NU
C
C        P1 = 0. = NORMALIZED LEGENDRE(NU,NU+1,X)
C
      P1 = 0.0D0
      IP1 = 0
C
C        CALCULATE P2 = NORMALIZED LEGENDRE(NU,NU,X)
C
      P2 = 1.0D0
      IP2 = 0
      P3 = 0.5D0
      DK = 2.0D0
      DO 60 J=1,NU
        P3 = ((DK+1.0D0)/DK)*P3
        P2 = P2*SX
        CALL DXADJ(P2, IP2,IERROR)
        IF (IERROR.NE.0) RETURN
        DK = DK + 2.0D0
   60 CONTINUE
      P2 = P2*SQRT(P3)
      CALL DXADJ(P2, IP2,IERROR)
      IF (IERROR.NE.0) RETURN
      S = 2.0D0*TX
      T = 1.0D0/NU
      IF (MU2.LT.NU) GO TO 70
      DPN(I) = P2
      IPN(I) = IP2
      I = I - 1
      IF (I .EQ. 0) GO TO 140
C
C        RECURRENCE PROCESS
C
   70 P = MU*T
      C1 = 1.0D0/SQRT((1.0D0-P+T)*(1.0D0+P))
      C2 = S*P*C1*P2
      C1 = -SQRT((1.0D0+P+T)*(1.0D0-P))*C1*P1
      CALL DXADD(C2, IP2, C1, IP1, P, IP,IERROR)
      IF (IERROR.NE.0) RETURN
      MU = MU - 1
      IF (MU.GT.MU2) GO TO 80
C
C        STORE IN ARRAY DPN FOR RETURN TO CALLING ROUTINE.
C
      DPN(I) = P
      IPN(I) = IP
      I = I - 1
      IF (I .EQ. 0) GO TO 140
   80 P1 = P2
      IP1 = IP2
      P2 = P
      IP2 = IP
      IF (MU.LE.MU1) GO TO 140
      GO TO 70
C
C        SPECIAL CASE WHEN X=-1 OR +1, OR NU=0.
C
   90 K = MU2 - MU1 + 1
      DO 100 I=1,K
        DPN(I) = 0.0D0
        IPN(I) = 0
  100 CONTINUE
      ISIG = 0
      IF (MU1.GT.0) GO TO 160
      ISIG = 1
      DPN(1) = SQRT(NU+0.5D0)
      IPN(1) = 0
      IF (MOD(NU,2).EQ.0) GO TO 160
      IF (MODE.EQ.1 .AND. DARG.EQ.1.0D0) GO TO 160
      IF (MODE.EQ.2) GO TO 160
      DPN(1) = -DPN(1)
      GO TO 160
C
C          ERROR PRINTOUTS AND TERMINATION.
C
  110 CALL XERMSG ('SLATEC', 'DXNRMP', 'NU, MU1, MU2 or MODE not valid',
     +             212, 1)
      IERROR=212
      RETURN
  120 CALL XERMSG ('SLATEC', 'DXNRMP', 'DARG out of range', 213, 1)
      IERROR=213
      RETURN
C
C        RETURN TO CALLING PROGRAM
C
  140 K = MU2 - MU1 + 1
      DO 150 I=1,K
        CALL DXRED(DPN(I),IPN(I),IERROR)
        IF (IERROR.NE.0) RETURN
  150 CONTINUE
  160 RETURN
      END