module mod_numerical
    !=========================================================================!
    ! Written by Alessandro Di Nola
    ! Main sources:
    ! https://github.com/drarnau/Replication_Krusell-et-al-2017/blob/master/Utils.f90
    ! https://sites.google.com/site/greygordon/code
    ! Numerical recipes in F90: http://numerical.recipes/
    ! http://karibzhanov.com/
    ! Giulio Fella: https://github.com/gfell
	! Fabian Kindermann: https://www.ce-fortran.com/
    !   Date      Programmer       Description of change
	!   ====      ==========       =====================
	!  20210817   A. Di Nola       Original code
	!  20210818   A. Di Nola       Changed lrzcurve
	!  20210830   A. Di Nola       Added function gini
	!  20210901   A. Di Nola       Added function ind2sub
    !=========================================================================!

    ! USE other modules
    implicit none

    private !Variables and internal proc are not visible outside of this module

    ! User-defined error function
    public :: myerror

    ! Replicate basic Matlab functions
    public :: linspace
    public :: ones, eye, cumsum, cumprod
    public :: outerprod
    public :: ind2sub

    ! Sorting subroutines
    public :: QsortC

    ! General-purpose routines
    public :: is_monotone, my_closest, grid, mycorr

    ! Markov chain:
    public :: my_ss

    ! Interpolation:
    public :: linint, locate

    ! Discretization:
    public :: discretize_pareto, bddparetocdf, tauchen

    ! Percentiles, Gini and Lorenz curve:
    public :: quantili, calculate_quintiles, lrzcurve_basic, lrzcurve, unique, gini

    ! Optimization:
    public :: golden_method !similar to fminbnd in Matlab

    ! Root-finding:
    public :: rtbis, zbrent !similar to fzero in Matlab

contains

    subroutine myerror(string)

    implicit none
    character(len=*), intent(in) :: string
    write(*,*) "ERROR: ", string
    write(*,*) "Program will terminate.."
    pause
    stop

    end subroutine myerror
    !===============================================================================!

    function linspace(my_start, my_stop, n)
    ! Purpose: replicate Matlab function <linspace>
    ! Originally written by Arnau Valladares-Esteban
    implicit none
    ! Inputs:
    integer :: n
    real(8) :: my_start, my_stop
    ! Function result:
    real(8) :: linspace(n)
    ! Locals:
    integer :: i
    real(8) :: step, grid(n)

    if (n == 1) then
        grid(n) = (my_start+my_stop)/2.d0
    elseif (n.ge.2) then
        grid(1) = my_start
        if (n.gt.2) then
            step = (my_stop-my_start)/(real(n-1))
            do i = 2, n-1
                grid(i) = grid(i-1) + step
            end do
        end if
        grid(n) = my_stop
    endif
    linspace = grid

    end function linspace
    !===============================================================================!

    function ones(n) result(vec)
    ! Purpose: create a column vector of ones
    ! Input/output
    integer, intent(in) :: n
    real(8) :: vec(n)
    !Local variables
    integer :: i

    ! Body of ones
    do i = 1,n
        vec(i) = 1.0d0
    end do

    end function ones
    !===============================================================================!

    function eye(n) result(mat)
    ! Purpose: replicate function <eye> in Matlab
    ! Input/output
    integer, intent(in) :: n
    real(8) :: mat(n,n)
    !Local variables
    integer :: i

    ! Body of eye
    mat = 0.0d0
    do i = 1,n
        mat(i,i) = 1.0d0
    end do

    end function eye
    !===============================================================================!

    function outerprod(a,b)
    ! Computes the outer product of two vectors
    real(8), intent(in) :: a(:), b(:)
    real(8) :: outerprod(size(a),size(b))

    outerprod = spread(a,dim=2,ncopies=size(b)) * spread(b,dim=1,ncopies=size(a))

    end function outerprod
    !===============================================================================!

    FUNCTION cumsum(arr) RESULT(ans)
    ! Purpose: it replicates matlab <cumsum> function
    real(8), INTENT(IN) :: arr(:)
    ! Note that a Fortran function can return a value with an ALLOCATABLE
    ! attribute. The function result must be declared as ALLOCATABLE
    ! in the calling unit but will not be allocated on entry to the function.
    real(8), allocatable :: ans(:)
    INTEGER :: n,j

    n=size(arr)

    allocate(ans(n))
    ans(1)=arr(1)

    do j=2,n
	    ans(j)=ans(j-1)+arr(j)
    end do

    END FUNCTION cumsum
    !===============================================================================!

    function is_monotone(arr) result(ans)
    ! Purpose: Check is a real array is monotonically increasing
    implicit none
    !Declare inputs:
    real(8), intent(in) :: arr(:)
    !Declare function result:
    logical :: ans
    !Declare locals:
    integer :: i,n

    n = size(arr)
    if (n==1) then
        ans = .true.
        return
    endif

    do i=1,n-1
        if ( arr(i)<=arr(i+1) ) then
            ans = .true.
        else
            ans = .false.
            return
        endif
    enddo

    end function is_monotone
    !===============================================================================!

    function my_closest(myvector,gp,myvalue)
    ! Purpose: FINDS CLOSEST POSITION OF A REAL IN A VECTOR OF REALS
    ! Originally written by Arnau Valladares-Esteban
    implicit none
    integer, intent(in) :: gp
    real(8), intent(in) :: myvalue
    real(8), dimension(gp), intent(in) :: myvector
    real(8), dimension(gp) :: aux
    ! Function result:
    integer :: my_closest

    aux = abs(myvector-myvalue)

    my_closest = minloc(aux, dim=1)
    end function my_closest
    !===============================================================================!

    subroutine unique(x, x_u,ind_u)
	! Purpose: Find unique values in array x
	! Return a smaller array with only unique values and corresponding (last) indeces
	! Identical to matlab function 'unique' with 'last' option
    ! WARNING: The input array x must be sorted in ascending order

	implicit none

	real(8), intent(in) :: x(:)
	real(8), intent(out), allocatable :: x_u(:)
	integer, intent(out), allocatable :: ind_u(:)
	! Local variables
	integer :: i, n, i_u

	! Execution

	n = size(x)

	allocate(x_u(n),ind_u(n))

	x_u(1) = x(1)
	i_u = 1

	do i=2,n
		if (x(i)>x(i-1)) then
			ind_u(i_u) = i-1
			x_u(i_u+1) = x(i)
			i_u = i_u+1
		endif
	enddo

	ind_u(i_u) = n

	! Outputs
	x_u = x_u(1:i_u)
	ind_u = ind_u(1:i_u)

    end subroutine unique
    !===============================================================================!

    function my_ss(tmatrix,gp)
    ! Purpose: STEADY STATE MARKOV CHAIN
    ! Originally written by Arnau Valladares-Esteban
    implicit none
    integer :: gp
    integer :: row, col, iter
    real(8) :: aux_sum
    real(8), dimension(gp) :: my_ss
    real(8), dimension(gp) :: dist, ndist
    real(8), dimension(gp,gp) :: tmatrix

    ! Initialise distribution
    dist = 1.d0/real(gp)

    do iter = 1, 10000
        ndist = 0.d0
        do col = 1, gp
        do row = 1, gp
            ndist(col) = ndist(col) + (dist(row)*tmatrix(row,col))
        end do
        end do
        aux_sum = sum(abs(ndist-dist))
        dist = ndist
        if (aux_sum.lt.1.0d-10) then
        exit
        end if
    end do

    my_ss = dist
    end function my_ss
    !===============================================================================!

    SUBROUTINE grid(x,xmin,xmax,s)
    ! Purpose: Generate grid x on [xmin,xmax] using spacing parameter s set as follows:
    ! s=1		linear spacing
    ! s>1		left skewed grid spacing with power s
    ! 0<s<1		right skewed grid spacing with power s
    ! s<0		geometric spacing with distances changing by a factor -s^(1/(n-1)), (>1 grow, <1 shrink)
    ! s=-1		logarithmic spacing with distances changing by a factor (xmax-xmin+1)^(1/(n-1))
    ! s=0		logarithmic spacing with distances changing by a factor (xmax/xmin)^(1/(n-1)), only if xmax,xmin>0
    IMPLICIT NONE
    REAL(8), DIMENSION(:), INTENT(OUT) :: x
    REAL(8), INTENT(IN) :: xmin,xmax,s
    REAL(8) :: c ! growth rate of grid subintervals for logarithmic spacing
    INTEGER :: n,i
    n=size(x)
    FORALL(i=1:n) x(i)=(i-1)/real(n-1,8)
    IF (s>0.0d0) THEN
	    x=x**s*(xmax-xmin)+xmin
	    !IF (s==1.0d0) THEN
	    !	PRINT '(a,i8,a,f7.3,a,f7.3,a)', 'Using ',n,' equally spaced grid points over domain [',xmin,',',xmax,']'
	    !ELSE
	    !	PRINT '(a,i8,a,f7.3,a,f7.3,a,f7.3,a)', 'Using ',n,' skewed spaced grid points with power ',s,' over domain [',xmin,',',xmax,']'
	    !END IF
    ELSE
	    IF (s==-1.0d0) THEN
		    c=xmax-xmin+1
    !		ELSEIF (s==0.0d0) THEN
    !			IF (xmin>0.0d0) THEN
    !				c=xmax/xmin
    !			ELSE
    !				STOP 'grid: can not use logarithmic spacing for nonpositive values'
    !			END IF
	    ELSE
		    c=-s
	    END IF
	    PRINT '(a,i8,a,f6.3,a,f6.3,a,f6.3,a)', 'Using ',n,' logarithmically spaced grid points with growth rate ',c,' over domain [',xmin,',',xmax,']'
	    x=((xmax-xmin)/(c-1))*(c**x)-((xmax-c*xmin)/(c-1))
    END IF
    END SUBROUTINE grid
    !===============================================================================!

    FUNCTION linint(x,y,xi)
    ! Purpose: linear interpolation of function y on grid x at interpolation point xi
    !          To make it pure, cannot use PRINT or STOP statements
    IMPLICIT NONE
    REAL(8), DIMENSION(:), INTENT(IN) :: x,y
    REAL(8), INTENT(IN) :: xi
    REAL(8) :: linint
    REAL(8) :: a,b,d
    INTEGER :: n,i
    n=size(x)
    IF (size(y)/=n) THEN
	    PRINT *, 'linint: x and y must be of the same size'
        PAUSE
	    STOP 'program terminated by linint'
    END IF
    i=max(min(locate(x,xi),n-1),1)
    d=x(i+1)-x(i)
    !IF (d == 0.0) STOP 'bad x input in splint'
    a=(x(i+1)-xi)/d
    b=(xi-x(i))/d
    linint=a*y(i)+b*y(i+1)
    END FUNCTION linint
    !===============================================================================!

    PURE FUNCTION locate(xx,x)
    IMPLICIT NONE
    REAL(8), DIMENSION(:), INTENT(IN) :: xx
    REAL(8), INTENT(IN) :: x
    INTEGER :: locate
    INTEGER :: n,il,im,iu
    n=size(xx)
    il=0
    iu=n+1
    do
	    if (iu-il <= 1) exit
	    im=(iu+il)/2
	    if (x >= xx(im)) then
		    il=im
	    else
		    iu=im
	    end if
    end do
    if (x == xx(1)) then
	    locate=1
    else if (x == xx(n)) then
	    locate=n-1
    else
	    locate=il
    end if
    END FUNCTION locate
    !===============================================================================!

    !===============================================================================!
    !subroutine bracket(x, xval, l, r)
    !    ! original code by  John Burkardt
    !    real(8), intent(in), dimension(:) :: x
    !    real(8), intent(in) :: xval
    !    integer, intent(out) :: l, r
    !    integer :: i, n
    !
    !    n=size(x)
    !    do i = 2, n - 1
    !       if ( xval < x(i) ) then
    !          l = i - 1
    !          r = i
    !          return
    !       end if
    !    end do
    !    l = n - 1
    !    r = n
    !end subroutine bracket
    !===============================================================================!

    function gini(x_in, y_in)
        ! DESCRIPTION: computes Lorenz curve and Gini coefficient
        ! Notes: written by F.Kindermann
        ! See: https://www.ce-fortran.com/forums/topic/gini-coefficient-and-lorenz-curve/#post-1951
        use toolbox, only: sort, plot, execplot

        real(8), intent(in) :: x_in(:), y_in(:)
        real(8) :: gini
        integer :: n, ic
        real(8), allocatable :: xs(:), ys(:), xcum(:), ycum(:)
        integer, allocatable :: iorder(:)

        ! get array size
        n = size(x_in, 1)

        ! ALLOCATE LARGE ARRAYS TO AVOID SIZE PROBLEMS

        ! first deallocate
        if(allocated(xs))deallocate(xs)
        if(allocated(ys))deallocate(ys)
        if(allocated(xcum))deallocate(xcum)
        if(allocated(ycum))deallocate(ycum)
        if(allocated(iorder))deallocate(iorder)

        ! then allocate
        allocate(xs(n))
        allocate(ys(n))
        allocate(xcum(0:n))
        allocate(ycum(0:n))
        allocate(iorder(n))

        ! NOW CALCULATE GINI INDEX

        ! sort array
        xs = x_in
        call sort(xs, iorder)

        ! sort y's and normalize to 1
        do ic = 1, n
            ys(ic) = y_in(iorder(ic))
        enddo
        ys = ys/sum(ys)

        ! calculate cumulative distributions
        xcum(0) = 0d0
        ycum(0) = 0d0
        do ic = 1, n
            xcum(ic) = xcum(ic-1) + ys(ic)*xs(ic)
            ycum(ic) = ycum(ic-1) + ys(ic)
        enddo

        ! now normalize cumulated attributes
        xcum = xcum/xcum(n)

        ! plot the Lorenz curve
        call plot(ycum, ycum, linewidth=1d0, color="black", dashtype="--")
        call plot(ycum, xcum)
        call execplot()

        ! determine gini index
        gini = 0d0
        do ic = 1, n
            gini = gini + ys(ic)*(xcum(ic-1) + xcum(ic))
        enddo
        gini = 1d0 - gini

    end function gini
    !===============================================================================!

    function quantili(x,w,q) result(y)
    ! Purpose: computes quantiles q of x with weights w
    ! w is discrete prob function (or PMF)
    ! need not be sorted or normalized
    ! Sources: Cagetti and De Nardi M-function quantili.m
    ! Note: takes care of repeated values
    ! Dependencies: calls subroutine unique
    use toolbox, only: sort
    implicit none

    !Declare inputs:
    real(8), intent(in) :: x(:)
    real(8), intent(in) :: w(:)
    real(8), intent(in) :: q(:)
    !Declare output:
    real(8) :: y(size(q))

    !Declare locals
    integer :: i, n, istat
    integer, allocatable :: ix(:), ind_u(:)
    real(8), allocatable :: xs(:), ws(:), cums(:)
    real(8), allocatable :: cums_u(:), xs_u(:)

    n = size(x)
    allocate( ix(n), xs(n),ws(n),cums(n),stat=istat )
    if (istat/=0) then
        call myerror("quantili: Allocation failed!")
    endif

    !!Check inputs
    if (size(w)/=n) then
        call myerror("quantili: x (variable) and w (pmf) must be of the same size")
    endif

    !xs is x sorted, ix is the sorting index
    xs = x
    call sort(xs,ix)
    ws=w(ix)
    ws=ws/sum(ws)
    cums=cumsum(ws)

    ![xs_u,ind_u] = unique(xs,'last')
    call unique(xs, xs_u,ind_u)
    cums_u = cums(ind_u)

    y = 0.0d0
    do i=1,size(q)
        if (cums_u(1)<=q(i)) then
            y(i) = linint(cums_u,xs_u,q(i))
        else
            y(i) = xs_u(1)
        endif
    enddo

    end function quantili
    !===============================================================================!

    subroutine calculate_quintiles(a_input,a_dist,thresholds, quantiles)
    !Source: Kindermann's book pp. 467-469
    !Compare to function "quantili" in this module (see below)

    use toolbox, only: sort
    implicit none

    !Declare inputs:
    real(8), intent(in) :: a_input(:)
    real(8), intent(inout) :: a_dist(:)
    real(8) :: thresholds(:)

    !Declare outputs:
    real(8), intent(out) :: quantiles(size(thresholds, 1))

    !Declare locals:
    integer :: ia, ic, it, NC, istat
    real(8) :: slope
    integer, allocatable :: iorder(:)
    real(8), allocatable :: a_sort(:), a_cdist(:)

    ! define quantile thresholds (now passed as inputs to subr)
    !thresholds = (/0.05d0, 0.25d0, 0.50d0, 0.75d0, 0.95d0/)
    quantiles = 0d0

    ! K only uses asset levels with pop share of at least 10^(12)
    ! This is similar to getting rid of zeros as I do in lrzcurve
    ! --- SKIP FOR NOW ---!

    NC = size(a_dist,1)

    !!Check inputs
    if (size(a_input)/=NC) then
        call myerror("calculate_quintiles: a (variable) and a_dist (pmf) must be of the same size")
    endif

    allocate( iorder(NC), a_sort(NC), a_cdist(NC), stat=istat )
    if (istat/=0) then
        call myerror("calculate_quintiles: Allocation failed!")
    endif

    ! sort array and distribution
    a_sort = a_input
    call sort(a_sort(1:NC), iorder(1:NC))

    ! calculate cumulative distribution (attention ordering)
    a_cdist(1) = a_dist(iorder(1))
    do ic = 2, NC
        a_cdist(ic) = a_cdist(ic-1) + a_dist(iorder(ic))
    enddo

    ! get quantiles
    do it = 1, size(thresholds, 1)
        if(thresholds(it) <= a_cdist(1))then
            quantiles(it) = a_sort(1)
        else
            do ic = 2, NC
                if(thresholds(it) < a_cdist(ic)) then
                    slope = (a_sort(ic)-a_sort(ic-1))/(a_cdist(ic)-a_cdist(ic-1))
                    quantiles(it) = a_sort(ic-1) + slope*(thresholds(it)-a_cdist(ic-1))
                    exit
                elseif(ic == NC)then
                    quantiles(it) = a_sort(NC)
                endif
            enddo
        endif
    enddo

    end subroutine calculate_quintiles
    !===============================================================================!

    subroutine lrzcurve_basic(f_in,y,gini)
    ! Purpose: It computes the Gini WITHOUT eliminating the zeros
    ! Source: This is a simplified version of Matlab <lrzcurve>
    use toolbox, only: sort
    implicit none

    !Declare inputs and outputs:
    real(8), intent(in) :: f_in(:)  !Distribution
    real(8), intent(in) :: y(:)     !Variable of interest
    real(8), intent(out) :: gini    !Gini coefficient

    !Declare local variables:
    real(8) :: minpop
    integer :: i,n, istat
    real(8), allocatable :: f(:),y_sort(:), Scum(:),S(:),f_temp(:)
    integer, allocatable :: key(:)

    n = size(y)

    !Check inputs
    if (size(f_in)/=n) then
        call myerror("lrzcurve_basic: f (distrib.) and y (var. of interest) must be of the same size")
    endif

    allocate(f(n),y_sort(n),Scum(n),S(n),key(n),stat=istat)
    if (istat/=0) then
        call myerror("lrzcurve_basic: Allocation failed!")
    endif

    !I do not want to modify f_in
    f = f_in

    !Sort x in scending order
    !Sort p
    y_sort = y
    call sort(y_sort,key)

    !f = f(key) !stack overflow occurs here
    f_temp = f
    do i=1,size(f)
        f(i) = f_temp(key(i))
    enddo
    deallocate(f_temp)

    S = y_sort*f
    Scum = cumsum(S)
    gini = f(1)*Scum(1)

    do i=2,n
        gini = gini + (Scum(i) + Scum(i-1))*f(i)
    enddo

    gini = 1.0d0 - (gini/Scum(n))
    minpop = minval(f)

    ! Normalize the gini (smth not always done...)
    gini = gini/(1.0d0 - minpop)

    end subroutine lrzcurve_basic
    !===============================================================================!

    subroutine lrzcurve(p,x,gini_out,fx_out,sx_out,mean_x_out,stdv_x_out)

    ! ------------------------- LEGEND --------------------------------!
    ! Purpose: compute Lorenz curve, gini coeff, mean and std
    ! INPUTS:
    ! p is the probability distrib (it must sum to 1)
    ! x is a vector with the values of the variable of interest
    ! OUTPUTS:
    ! fx(:) is share of population
    ! sx(:) is the corresponding share of wealth/income/etc.
    ! E.g. plot Lorenz curve with fx on the x-axis and sx on the y-axis
    ! See wiki: https://en.wikipedia.org/wiki/Lorenz_curve
    ! DEPENDENCIES:
    ! lrzcurve calls a subroutine to sort arrays. It can either be
    ! <sort> from Kindermann's toolbox (and in this case you need to use
    ! the toolbox) or <QsortC> which is stored in this module
    ! -----------------------------------------------------------------!

    !use toolbox, only: sort

    implicit none
    !Declare inputs:
    real(8), intent(in) :: p(:)
    real(8), intent(in) :: x(:)
    !Declare outputs:
    real(8), intent(out) :: gini_out
    real(8), allocatable, intent(out), optional :: fx_out(:)
    real(8), allocatable, intent(out), optional :: sx_out(:)
    real(8), intent(out), optional :: mean_x_out
    real(8), intent(out), optional :: stdv_x_out

    !Declare locals:
    integer :: n, i, n_valid, istat
    real(8) :: minpop, mean_x, stdv_x, gini
    real(8), allocatable :: p1(:), x1(:), fx(:), sx(:)
    integer, allocatable :: key(:)

    n = size(x)

    if (size(p)/=n) then
        call myerror("lrzcurve: x and p must have the same size")
    endif
    if (abs(sum(p)-1.0d0)>1d-8) then
        call myerror("lrzcurve: p must sum to one")
    endif

    !Preliminary step
    !Matlab code:
    !%% Eliminate elements with zero probability
    !p1=p; p(p1==0)  = []; x(p1==0)  = [];

    p1 = pack(p,p/=0.0d0)
    x1 = pack(x,p/=0.0d0)

    n_valid = count(p/=0.0d0)
    allocate(key(n_valid),stat=istat)
    key = [ (i,i=1,n_valid) ]
    if (istat/=0) then
        call myerror("lrzcurve: Allocation failed!")
    endif

    !Compute mean and standard deviation
    mean_x  = dot_product(p1,x1)
    stdv_x  = dot_product(p1,(x1-mean_x)**2)

    !Sort x1 in ascending order
    !call sort(x1,key)
    call QsortC(x1,key)

    !Sort distribution accordingly
    fx = p1(key)

    !sx(i) is the term x(i)*fx(i)
    sx = (x1*fx)
    sx = sx/mean_x

    !Add initial zero
    fx = [0d0, fx]
    sx = [0d0, sx]

    !Compute cumulative sums:
    ! sx is the cumulative share of x
    sx = cumsum(sx)

    !Compute Gini coefficient:
    gini = sx(1)*fx(1)
    do i = 2,size(fx)
        gini = gini +(sx(i)+sx(i-1))*fx(i)
    enddo
    gini = 1.0d0-(gini/sx(size(sx)))

    ! Keep the smallest population, needed to normalize the Gini coefficient
    minpop = minval(p1)

    ! Normalize the gini (smth not always done...)
    gini = gini/(1.0d0 - minpop)

    ! Assign outputs
    gini_out = gini

    ! Assign optional outputs
    ! Lorenz curve(i) is (fx(i), shareX(i))
    if (present(fx_out)) then
        fx_out = cumsum(fx)
    endif
    if (present(sx_out)) then
        sx_out = sx
    endif
    if (present(mean_x_out)) then
        mean_x_out  = mean_x
    endif
    if (present(stdv_x_out)) then
        stdv_x_out  = stdv_x
    endif

    end subroutine lrzcurve
    !===============================================================================!

    subroutine golden_method(f, a, b, x1, f1, mytol, mymaxit)
    ! Purpose: Applies Golden-section search to search for the *maximum* of a function
    ! in the interval (a, b).
    ! Source: https://en.wikipedia.org/wiki/Golden-section_search
    ! Adapted to Fortran90 from: https://github.com/QuantEcon

    !---------------------------------------------------!
    !INPUTS
    interface
        function f(x)
        implicit none
        real(8), intent(in) :: x
        real(8) :: f
        end function f
    end interface
    real(8), intent(in) :: a, b
    !Some optional inputs
    integer,  optional :: mymaxit
    real(8), optional :: mytol
    !OUTPUTS
    real(8), intent(out) :: x1, f1
    !---------------------------------------------------!

    !Locals
    integer :: maxit, it
    real(8) :: tol, alpha1, alpha2, d, f2, x2, s

    !! Assign default value to maxit if not defined by user
    if (present(mymaxit)) then
        maxit = mymaxit
    else
        maxit = 1000
    end if

    ! Assign default value to tol if not defined by user
    if (present(mytol)) then
        tol = mytol
    else
        tol = 1.0d-6
    end if

    alpha1 = (3.d0 - sqrt(5.d0)) / 2.d0
    alpha2 = 1.d0 - alpha1
    d = b - a
    x1 = a + alpha1*d
    x2 = a + alpha2*d
    s = 1.d0
    f1 = f(x1)
    f2 = f(x2)
    d = alpha1*alpha2*d

    it = 0

    do while ((d.gt.tol).and.(it.lt.maxit))
        it = it + 1
        d = d*alpha2

        if (f2.gt.f1) then
            x1 = x2
            f1 = f2
            x2 = x1 + s*d
        else
            x2 = x1 - s*d
        end if

        s = sign(s, x2-x1)
        f2 = f(x2)
    end do

    if (it.ge.maxit) then
        print *, "Golden method: Maximum iterations exceeded"
    end if

    if (f2.gt.f1) then
        x1 = x2
        f1 = f2
    end if

    end subroutine golden_method
    !===============================================================================!

    subroutine max_nonconvex(f, a, b, xmax, fmax, mytol, mymaxit, mynx)
    ! Purpose: Maximize the univariate function f(x) in [a,b]
    ! using either grid search or golden method or both
    ! Deals with non-concave objective functions

    !---------------------------------------------------!
    ! INPUTS:
    interface
        function f(x)
        implicit none
        real(8), intent(in) :: x
        real(8) :: f
        end function f
    end interface
    real(8), intent(in) :: a, b
    ! Optional inputs:
    integer,  optional :: mymaxit
    real(8), optional :: mytol
    integer, optional :: mynx
    ! OUTPUTS:
    real(8), intent(out) :: xmax, fmax
    !---------------------------------------------------!

    !Locals
    integer :: maxit, x_c, max_ind, left_loc, right_loc
    real(8) :: tol, x_val, max_val, temp, x1, f1
    integer :: nx
    real(8), allocatable :: x_grid(:)

    ! Assign default value to maxit if not defined by user
    if (present(mymaxit)) then
        maxit = mymaxit
    else
        maxit = 1000
    end if

    ! Assign default value to tol if not defined by user
    if (present(mytol)) then
        tol = mytol
    else
        tol = 1.0d-6
    end if

    ! Assign default value to nx if not defined by user
    if (present(mynx)) then
        nx = max(mynx,2)
    else
        nx = 100
    end if

    ! Preliminary step: define the grid
    allocate(x_grid(nx))
    x_grid = linspace(a,b,nx)

    ! First step: grid search
    max_val = -huge(0d0)
    max_ind = 1
    do x_c = 1,nx
        x_val = x_grid(x_c)
        temp  = f(x_val)
        if (temp>max_val) then
            max_val = temp
            max_ind = x_c
        endif
    enddo

    ! Second step: find the interval that brackets the true maximum
    left_loc  = max(max_ind-1,1)
    right_loc = min(max_ind+1,nx)

    ! Last step: call golden_method section search
    call golden_method(f, x_grid(left_loc), x_grid(right_loc), x1, f1, tol, maxit)

    ! Assign outputs
    xmax = x1 ! arg max
    fmax = f1 ! f(arg max)

    end subroutine max_nonconvex
    !===============================================================================!

    FUNCTION rtbis(func,x1,x2,xacc,MAXIT)
    !-------------------------------------------------------------!
    ! DESCRIPTION:
    ! Bisection to find root x of a nonlinear function func(x)=0
    ! INPUTS:
    ! func: function to pass to the rootfinder
    ! [x1 x2]: bracketing interval
    ! xacc: accuracy criterion
    ! MAXIT: maximum number of iterations
    ! OUTPUT
    ! rtbis: root of the function in [x1 x2]
    ! SOURCE:
    ! http://numerical.recipes/
    !-------------------------------------------------------------!

    IMPLICIT NONE
    REAL(8), INTENT(IN) :: x1,x2,xacc
    INTEGER, INTENT(IN) :: MAXIT
    REAL(8) :: rtbis
    INTERFACE
	    FUNCTION func(x)
	    IMPLICIT NONE
	    REAL(8), INTENT(IN) :: x
	    REAL(8) :: func
	    END FUNCTION func
    END INTERFACE
    !INTEGER, PARAMETER :: MAXIT=40
    INTEGER :: j
    REAL(8) :: dx,f,fmid,xmid
    fmid = func(x2)
    f    = func(x1)
    if (f*fmid >= 0.0d0) then
	    call myerror("rtbis: root must be bracketed")
    endif
    if (f < 0.0d0) then
	    rtbis=x1
	    dx=x2-x1
    else
	    rtbis=x2
	    dx=x1-x2
    end if
    do j=1,MAXIT
	    dx=dx*0.5d0
	    xmid=rtbis+dx
	    fmid=func(xmid)
	    if (fmid <= 0.0d0) rtbis=xmid
	    if (abs(dx) < xacc .or. fmid == 0.0d0) RETURN
    end do
    write(*,*) "WARNING: rtbis: too many bisections"
    END FUNCTION rtbis
    !===============================================================================!

    FUNCTION zbrent(func,x1,x2,tol,ITMAX)
    !-------------------------------------------------------------!
    ! DESCRIPTION:
    ! Brent's method to find root x of a nonlinear function func(x)=0
    ! It is the same as fzero in Matlab. Brent's method is generally
    ! better than bisection.
    ! INPUTS:
    ! func: function to pass to the rootfinder
    ! [x1 x2]: bracketing interval
    ! tol: accuracy criterion
    ! MAXIT: maximum number of iterations ???
    ! OUTPUT
    ! rtbis: root of the function in [x1 x2]
    ! SOURCE:
    ! http://numerical.recipes/
    !-------------------------------------------------------------!
    IMPLICIT NONE
    REAL(8), INTENT(IN) :: x1,x2,tol
    INTEGER, INTENT(IN) :: ITMAX
    REAL(8) :: zbrent
    INTERFACE
	    FUNCTION func(x)
	    !USE mkl95_precision, ONLY: WP => DP
	    IMPLICIT NONE
	    REAL(8), INTENT(IN) :: x
	    REAL(8) :: func
	    END FUNCTION func
    END INTERFACE
    REAL(8), PARAMETER :: EPS=epsilon(x1)
    INTEGER :: iter
    REAL(8) :: a,b,c,d,e,fa,fb,fc,p,q,r,s,tol1,xm
    a=x1
    b=x2
    fa=func(a)
    fb=func(b)
    if ((fa > 0.0 .and. fb > 0.0) .or. (fa < 0.0 .and. fb < 0.0)) then
	    call myerror('root must be bracketed for zbrent')
    endif
    c=b
    fc=fb
    do iter=1,ITMAX
	    if ((fb > 0.0 .and. fc > 0.0) .or. (fb < 0.0 .and. fc < 0.0)) then
		    c=a
		    fc=fa
		    d=b-a
		    e=d
	    end if
	    if (abs(fc) < abs(fb)) then
		    a=b
		    b=c
		    c=a
		    fa=fb
		    fb=fc
		    fc=fa
	    end if
	    tol1=2.0d0*EPS*abs(b)+0.5d0*tol
	    xm=0.5d0*(c-b)
	    if (abs(xm) <= tol1 .or. fb == 0.0) then
		    zbrent=b
		    RETURN
	    end if
	    if (abs(e) >= tol1 .and. abs(fa) > abs(fb)) then
		    s=fb/fa
		    if (a == c) then
			    p=2.0d0*xm*s
			    q=1.0d0-s
		    else
			    q=fa/fc
			    r=fb/fc
			    p=s*(2.0d0*xm*q*(q-r)-(b-a)*(r-1.0d0))
			    q=(q-1.0d0)*(r-1.0d0)*(s-1.0d0)
		    end if
		    if (p > 0.0) q=-q
		    p=abs(p)
		    if (2.0d0*p  <  min(3.0d0*xm*q-abs(tol1*q),abs(e*q))) then
			    e=d
			    d=p/q
		    else
			    d=xm
			    e=d
		    end if
	    else
		    d=xm
		    e=d
	    end if
	    a=b
	    fa=fb
	    b=b+merge(d,sign(tol1,xm), abs(d) > tol1 )
	    fb=func(b)
    end do
    write(*,*) 'WARNING: zbrent: exceeded maximum iterations'
    zbrent=b
    END FUNCTION zbrent
    !===============================================================================!

    function bddparetocdf(emin, emax, shape, eval) result(cdf)
    ! Purpose: calculate the cumulative density (or mass) at eval for a
    ! discretized bounded Pareto distribution over (emin,emax)
    ! Source: https://ideas.repec.org/a/red/issued/17-402.html
    ! Code for paper "Aggregate Consequences of Credit Subsidy Policies"
    ! by Jo and Senga.
    implicit none

    ! Inputs:
    real(8), intent(in) :: emin
    real(8), intent(in) :: emax
    real(8), intent(in) :: shape
    real(8), intent(in) :: eval
    ! Function result:
    real(8) :: cdf
    ! Locals:
    real(8) :: paretocdf

    ! Check inputs:
    if (emin <=0 ) then
        call myerror("bddparetocdf: emin cannot be negative!")
    endif

    if (emax <= emin ) then
        call myerror("bddparetocdf: emax cannot be less than emin!")
    endif

    ! Body of Func
    paretocdf = 1.0d0 - (emin/eval)**shape
    cdf       = paretocdf/(1.0d0 - (emin/emax)**shape)

    end function bddparetocdf
    !===============================================================================!

    subroutine discretize_pareto(neps, eps, shape, rhoeps, ergoeps, pie)
    !--------------------------------------------------------------------------!
    ! This sub computes the ergodic distribution and the transition matrix of a
    ! bounded Pareto distribution, given grids of epsilon values.
    ! INPUTS:
    ! neps   :: number of grid points
    ! eps    :: grid with endpoints e_min and e_max
    ! shape  :: shape parameter
    ! rhoeps :: persistence
    !--------------------------------------------------------------------------!
    ! Source: https://ideas.repec.org/a/red/issued/17-402.html
    ! Code for paper "Aggregate Consequences of Credit Subsidy Policies"
    ! by Jo and Senga.
    !--------------------------------------------------------------------------!
    implicit none
    ! Inputs:
    integer, intent(in) :: neps
    real(8), intent(in) :: eps(neps)
    real(8), intent(in) :: shape
    real(8), intent(in) :: rhoeps
    ! Outputs
    real(8), intent(out) :: ergoeps(neps), pie(neps,neps)
    ! Locals:
    integer :: i
    real(8) :: emin, emax, mass
    real(8) :: mideps(neps-1)

    ! Check inputs
    if (shape<=0.0d0) then
        call myerror("discretize_pareto: shape param must be positive!")
    endif

    ! Body of discretize_pareto
    ergoeps = 0.0d0
    pie     = 0.0d0

    ! emin and emax are imaginary end points of a continuous distribution
    emin = eps(1) - (eps(2)-eps(1))/2.0d0
    emax = eps(neps) + (eps(neps)-eps(neps-1))/2.0d0

    mideps = 0.0d0
    do i = 1,neps-1
        mideps(i) = (eps(i)+eps(i+1))/2;
    enddo

    ! compute density or mass at each epsilon point
    ergoeps(1) = bddparetocdf(emin, emax, shape, mideps(1))
    do i = 2,neps-1
        mass       = bddparetocdf(emin,emax,shape, mideps(i-1))
        ergoeps(i) = bddparetocdf(emin,emax,shape, mideps(i)) - mass
    enddo
    ergoeps(neps) = 1.0d0 - bddparetocdf(emin, emax, shape, mideps(neps-1))

    ! transition matrix given rhoeps
    do i = 1,neps
        pie(i,i) = rhoeps
        pie(i,:) = pie(i,:) + (1.0d0-rhoeps)*ergoeps
    enddo

    end subroutine discretize_pareto
    !===============================================================================!

    subroutine tauchen(rho, sigma, mu, cover, gp, values, trans)
    !-------------------------------------------------------------------------------!
    ! Purpose: approximating first-order autoregressive process with Markov chain
    !
    ! y_t = rho * y_(t-1) + u_t
    !
    ! u_t is a Gaussian white noise process with standard deviation sigma.
    !
    ! cover determines the width of discretized state space, Tauchen uses m=3
    !
    ! gp is the number of possible states chosen to approximate
    ! the y_t process
    !
    ! trans is the transition matrix of the Markov chain
    !
    ! values is the discretized state space of y_t
    !
    ! Adapted from https://github.com/lucaguerrieri/
    ! Written in Fortran by Arnau Valladares-Esteban,
    ! https://github.com/drarnau/Replication_Krusell-et-al-2017/blob/master/Utils.f90
    ! Modified by Alessandro Di Nola, following also Robert Kirkby's TauchenMethod
	! https://github.com/vfitoolkit/VFIToolkit-matlab/blob/master/TauchenMethod/TauchenMethod.m
    ! Note: if the process y_t has non-zero mean, i.e.
    ! y_t = mu + rho * y_(t-1) + u_t, ==> E(y) = mu/(1-rho)
    ! Let z_t = y_t - mu/(1-rho)
    ! Apply Tauchen to z_t and then obtain y_t = z_t + mu/(1-rho)
    !-------------------------------------------------------------------------------!
    implicit none
    ! Declare inputs and outputs:
    integer, intent(in) :: gp
    real(8), intent(in) :: rho, sigma, mu, cover
    real(8), dimension(gp), intent(out)    :: values
    real(8), dimension(gp,gp), intent(out) :: trans
    ! Declare locals:
    integer :: j, k
    real(8) :: sd_y, ymin, ymax, w, ystar

    ! standard deviation of y_t
    sd_y = sqrt(sigma**2.d0/(1.d0-rho**2.d0))

    ymax = cover*sd_y   ! upper boundary of state space
    ymin = -ymax        ! lower boundary of state space
    w = (ymax-ymin)/real(gp-1,8) ! length of interval

    ystar = mu/(1.0d0-rho) !expected value of y

    values = ystar + linspace(ymin, ymax, gp)

    ! Compute transition matrix
    do j = 1, gp
        do k = 2, gp-1
            trans(j,k) = &
            normcdf(values(k)-rho*values(j)+(values(k+1)-values(k))/2.d0,mu,sigma) &
            - normcdf(values(k)-rho*values(j)-(values(k)-values(k-1))/2.d0,mu,sigma)
        enddo
        ! only subtract half the interval on the right
        trans(j,1) = normcdf(values(1)-rho*values(j)+w/2.d0,mu,sigma)
        ! only subtract half the interval on the left
        trans(j,gp) = 1.d0 - normcdf(values(gp)-rho*values(j)-w/2.d0,mu,sigma)
    enddo
    contains

        real(8) function normcdf(x,mu,sigma)
        implicit none
        real(8), intent(in) :: x, mu, sigma

        normcdf = (1.d0+erf((x-mu)/sqrt(2.d0*sigma**2)))/2.d0
        end function normcdf

    end subroutine tauchen
    !===============================================================================!

    function mycorr(x,y,w) result(corr_weight)
      ! This function computes the correlation coefficient b/w two vectors
      ! X and Y with weights W.
      ! See wiki: https://en.wikipedia.org/wiki/Correlation_and_dependence
	  implicit none

      ! Declare input variables:
      real(8), intent(in) :: x(:), y(:)
      real(8), intent(in) :: w(:)

      ! Declare function result:
      real(8) :: corr_weight

      ! Declare local variables:
      integer :: i, n
      real(8) :: mean_x, mean_y, sum_covar, sum_weight
      real(8) :: var_x, var_y, cov_xy

      !-------------------------------!
      ! Check inputs
      if ( size(x,dim=1)/=size(y,dim=1) ) then
          call myerror("Dimension of X and Y do not match!")
      endif
      if ( size(x,dim=1)/=size(w,dim=1) ) then
          call myerror("Dimension of X and weights do not match!")
      endif
      if ( any(w<0.0d0) ) then
          call myerror("Weights must be positive!")
      endif

      ! Compute weighted mean of X and Y
      mean_x = weighted_mean(x,w)
      mean_y = weighted_mean(y,w)

      ! Compute weighted variance of X and Y
      var_x = weighted_variance(x,w)
      var_y = weighted_variance(y,w)

      ! Compute weighted covariance b/w X and Y
      n = size(x,dim=1) !assume x and y have same size
      sum_covar = 0.0d0
      sum_weight= 0.0d0
      do i=1,n
          sum_covar  = sum_covar + w(i)*(x(i)-mean_x)*(y(i)-mean_y)
          sum_weight = sum_weight + w(i)
      enddo
      cov_xy = sum_covar/sum_weight

      ! Compute weighted correlation b/w X and Y
      corr_weight = cov_xy/sqrt(var_x*var_y);

    contains

        function weighted_mean(x,w) result(meanX)
        implicit none

        ! Declare inputs
        real(8), intent(in) :: x(:), w(:)
        ! Declare function results
        real(8) :: meanX
        ! Declare locals
        integer :: n, i
        real(8) :: sum, sum_weight

        n = size(w,dim=1)

        !assume that weight vector and input vector have same length
        sum=0.0d0
        sum_weight=0.0d0
        do i=1,n
            sum        = sum +        x(i)*w(i)
            sum_weight = sum_weight + w(i)
        enddo
        meanX = sum/sum_weight

        end function weighted_mean
        !-----------------------------------------------------------!
        function weighted_variance(x,w) result(varX)
        implicit none

        ! Declare inputs
        real(8), intent(in) :: x(:), w(:)
        ! Declare function results
        real(8) :: varX
        ! Declare locals
        integer :: n, i
        real(8) :: sum, sum_weight

        n = size(w,dim=1)

        !assume that weight vector and input vector have same length
        sum=0.0d0
        sum_weight=0.0d0
        do i=1,n
            sum = sum + ( ( x(i)- weighted_mean(x,w))**2)*w(i)
            sum_weight = sum_weight + w(i)
        enddo
        varX = sum/sum_weight

        end function weighted_variance
        !-----------------------------------------------------------!
    end function mycorr
    !===============================================================================!

    RECURSIVE SUBROUTINE QsortC(A, index_A)
    ! Author: Juli Rew, SCD Consulting (juliana@ucar.edu), 9/03
    ! Based on algorithm from Thomas H. Cormen, Charles E. Leiserson, Ronald L.
    ! Rivest, and Clifford Stein, Introduction to Algorithms, The MIT Press, 1997 edition.
    ! Made F conformant by Walt Brainerd
    ! Modified by Giulio Fella (02/2013) to sort the array index accordingly
    ! see https://github.com/gfell/gf_qsort/blob/master/mod_gf_qsort.f90

    ! Input/output variables
    ! A    	 : real(precision) array to sort
    ! index_A: integer array indexing elements of A
    REAL(8), INTENT(inout), DIMENSION(:) :: A
    INTEGER, INTENT(inout), DIMENSION(SIZE(A)) :: index_A
    INTEGER :: iq

    IF(SIZE(A) > 1) THEN
      CALL Partition(A,index_A, iq)
      CALL QsortC(A(:iq-1),index_A(:iq-1))
      CALL QsortC(A(iq:),index_A(iq:))
    ENDIF
    END SUBROUTINE QsortC

    SUBROUTINE Partition(A, index_A, marker)
    REAL(8), INTENT(in out), DIMENSION(:) :: A
    INTEGER, INTENT(in out), DIMENSION(SIZE(A)) :: index_A
    INTEGER, INTENT(out) :: marker
    INTEGER :: i, j
    INTEGER :: index_temp
    REAL(8) :: temp
    REAL(8) :: x      ! pivot point
    x = A(1)
    i= 0
    j= SIZE(A) + 1

    DO
      j = j-1
      DO
        IF (A(j) <= x) EXIT
        j = j-1
      END DO
      i = i+1
      DO
        IF (A(i) >= x) EXIT
        i = i+1
      END DO
      IF (i < j) THEN
        ! exchange A(i) and A(j)
        temp = A(i)
        A(i) = A(j)
        A(j) = temp

        index_temp = index_A(i)
        index_A(i) = index_A(j)
        index_A(j) = index_temp

      ELSEIF (i == j) THEN
        marker = i+1
        RETURN
      ELSE
        marker = i
        RETURN
      ENDIF
    END DO

    END SUBROUTINE Partition
!===============================================================================!

! Compute the cumulative product of the vector x
function cumprod_r(x) result(y)
    implicit none
    real(8), intent(in) :: x(1:)
    real(8) :: y(size(x))
    !local
    real(8) :: tmp
    integer :: i

    y(1) = x(1)
    tmp = x(1)
    do i = 2,size(x)
        tmp = tmp*x(i)
        y(i) = tmp
    end do

end function cumprod_r
!===============================================================================!

function cumprod_i(x) result(y)
    implicit none
    integer, intent(in) :: x(1:)
    integer  :: y(size(x))
    !local
    integer :: tmp
    integer :: i

    y(1) = x(1)
    tmp = x(1)
    do i = 2,size(x)
        tmp = tmp*x(i)
        y(i) = tmp
    end do

end function cumprod_i
!===============================================================================!

! Code is based of the matlab ind2sub.m
! Given an integer vector siz that gives the shape of
! some array and the linear index i, convert the linear
! index into an i1,i2,... such that
!  a(i) = a(i1,i2,...)
function ind2sub(siz,i) result(ivec)
    integer, intent(in) :: siz(:)
    integer :: i
    integer :: ivec(size(siz))
    ! local
    integer :: d
    integer :: j,k
    integer :: cumprodivec(size(siz))

    cumprodivec(1) = 1
    cumprodivec(2:size(siz)) = cumprod(siz(1:size(siz)-1))

    j = i
    do d = size(siz),1,-1
        k = mod(j-1,cumprodivec(d)) + 1
        ivec(d) = (j-k)/cumprodivec(d) + 1
        j = k
    end do

end function ind2sub

end module mod_numerical


c=======================================================================
c
c     function getMNormalPdf
c
c     Returns the density function of a multivariate normal random variable
c
c-----------------------------------------------------------------------
c
c     Copyright (c) 2014 NumX
c     All rights reserved.
c
c     This software is the confidential and proprietary information
c     of NumX. You shall not disclose such Confidential
c     Information and shall use it only in accordance with the terms
c     of the licence agreement you entered into with NumX.
c
c     Author : Yann Vernaz
c
c-----------------------------------------------------------------------
      SUBROUTINE getMNormalPdf(d, x, mu, cov, iwork, dwork, z, info)
c-----------------------------------------------------------------------
c
c     INPUT
c       d        : dimension (d>0)                               integer
c       x        : data (d)                                       double
c       mu       : mean (d)                                       double
c       cov      : cov. matrix (d*d)                              double
c
c     WORKSPACE
c       iwork    : d                                             integer
c       dwork    : d*(2*d + 7)                                    double
c
c     OUTPUT
c       z        : multivariate normal Pdf                        double
c       info     : diagnostic argument                           integer
c
c-----------------------------------------------------------------------
c
c     arguments i/o
      INTEGER d, info
      DOUBLE PRECISION z, x(*), mu(*), cov(d,*)
c
c     workspaces
      INTEGER iwork(*)
      DOUBLE PRECISION dwork(*)
c
c     local variables
      LOGICAL select
      INTEGER i, sdim, lwork, pdcov, pdeig, piw, pip, pdv, pdw
      DOUBLE PRECISION PI, invPI, detCOV, s, EPS
      PARAMETER (PI=3.1415926535897932384626433832795028841971693993751)
      PARAMETER (EPS = 1.E-30)
c
c     intrinsic functions
      INTRINSIC MIN, MAX, SQRT, EXP
c
c-----------------------------------------------------------------------
c
c     initialisations
      info = 0
c
c     pointers for integer work space  : iwork
c     ----------------------------------------
      piw = 1
c     piw : pointer for DGEES, so ( d ) more
c
c     Total size of dwork array = ( d )
c
c     pointers for double precision work space  : dwork
c     -------------------------------------------------
      pdcov = 1
c     pdcov : pointer for cov. matrix, so ( d*d ) more
      pdeig = pdcov + ( d*d )
c     pdeig : pointer for eigenvalues, so ( d ) more
      pip   = pdeig + ( d )
c     pip   : pointer for DGEES, so ( d ) more
      pdv   = pip + ( d )
c     pdv   : pointer for DGEES, so ( d*d ) more
      pdw   = pdv + ( d*d )
c     pdw   : pointer for DGEES, so ( 5*d ) more
c
c     Total size of dwork array = d*(2*d + 7)
c
c----------------------------------------------------------------------
c
c     invPI = (2*PI)^(-d/2)
      invPI = (2.0*PI)**(-d/2)
c
c     case d=1
      IF (d .EQ. 1) THEN
        variance = MAX(cov(1,1),EPS)
        detCOV   = 1.0/SQRT(variance)
        s        = ((x(1)-mu(1))**2)/variance
        z        = invPI*detCOV*EXP(-0.5*s)
        z        = MIN(z, 1.0)
        z        = MAX(z, 0.0)
        RETURN
      ENDIF
c
c     |cov|
      CALL YM ( d, d, cov, dwork(pdcov) )
      sdim = 0
      lwork = 5*d
      CALL DGEES( 'V', 'N', select, d, dwork(pdcov), d,
     &            sdim, dwork(pdeig), dwork(pip), dwork(pdv),
     &            d, dwork(pdw), lwork, iwork(piw), info )
      detCOV = 1.0
      DO i = 1,d
        detCOV = detCOV*dwork(pdeig + i - 1)
      ENDDO
c
c     |cov|^(-1/2)
      detCOV = 1./SQRT(detCOV)
c
c     cov^(-1)
      CALL JMS ( d, cov, dwork(pdv), dwork(pdcov), info )
c
c     y =  x - mu
      CALL DV ( d, x, mu, dwork(pdeig) )
c
c     (x-mu)'*invCOV*(x-mu)
      CALL OVTMCV ( d, dwork(pdcov), dwork(pdeig), s )
c
c     EXP(-1/2*[(x - mu)'*(cov^(-1))*(x-mu)])
      s = EXP(-0.5*s)
c
c     multivariate Normal Pdf
      z = invPI*detCOV*s
      z = MIN(z, 1.0)
      z = MAX(z, 0.0)
c
      RETURN
      END


! examples/Fortran/LanczosBox.f90

module lanczos_box_module
  use ral_nlls_double
  implicit none
  integer, parameter :: wp = kind(0d0)

  type, extends(params_base_type) :: params_type
     real(wp), dimension(:), allocatable :: t ! The m data points t_i
     real(wp), dimension(:), allocatable :: y ! The m data points y_i
  end type params_type

contains

  subroutine eval_r(status, n, m, x, r, params)
    ! r_i = y_i - x_1 e^(-x_2 t_i) - x_3 e^(-x_4 t_i) - x_5 e^(-x_6 t_i)
    integer, intent(out) :: status
    integer, intent(in) :: n
    integer, intent(in) :: m
    real(wp), dimension(*), intent(in) :: x
    real(wp), dimension(*), intent(out) :: r
    class(params_base_type), intent(inout) :: params

    select type(params)
    type is(params_type)
       r(1:m) = params%y(:) &
            - x(1)*exp(-x(2)*params%t(:)) &
            - x(3)*exp(-x(4)*params%t(:)) &
            - x(5)*exp(-x(6)*params%t(:))
      status = 0 ! success
      return
    Class Default
      stop 'evalr: ERROR do not know how to handle this class...'
    end select
    status = -1 ! fail
  end subroutine eval_r

  subroutine eval_J(status, n, m, x, J, params)
    integer, intent(out) :: status
    integer, intent(in) :: n
    integer, intent(in) :: m
    real(wp), dimension(*), intent(in) :: x
    real(wp), dimension(*), intent(out) :: J
  class(params_base_type), intent(inout) :: params
    Integer :: r, c

    select type(params)
    type is(params_type)
      J(    1:  m) = -exp(-x(2)*params%t(1:m))                       ! J_i1
      J(  m+1:2*m) = params%t(1:m) * x(1) * exp(-x(2)*params%t(1:m)) ! J_i2
      J(2*m+1:3*m) = -exp(-x(4)*params%t(1:m))                       ! J_i3
      J(3*m+1:4*m) = +params%t(1:m) * x(3) * exp(-x(4)*params%t(1:m))! J_i4
      J(4*m+1:5*m) = -exp(-x(6)*params%t(1:m))                       ! J_i5
      J(5*m+1:6*m) = +params%t(1:m) * x(5) * exp(-x(6)*params%t(1:m))! J_i6
      status = 0 ! success
      return
    Class Default
      stop 'evalr: ERROR do not know how to handle this class...'
    end select
    status = -1 ! fail
  end subroutine eval_J

  subroutine eval_HF(status, n, m, x, r, HF, params)
    integer, intent(out) :: status
    integer, intent(in) :: n
    integer, intent(in) :: m
    real(wp), dimension(*), intent(in) :: x
    real(wp), dimension(*), intent(in) :: r
    real(wp), dimension(*), intent(out) :: HF
  class(params_base_type), intent(inout) :: params

    HF(1:n*n) = 0
    select type(params)
    type is(params_type)
      HF(    2) = sum( r(1:m) * params%t(1:m) * exp(-x(2)*params%t(1:m)))            ! H_21
      HF(1*n+1) = HF(2)                                                              ! H_12
      HF(1*n+2) = sum(-r(1:m) * (params%t(1:m)**2) * x(1) * exp(-x(2)*params%t(1:m)))! H_22
      HF(2*n+4) = sum( r(1:m) * params%t(1:m) * exp(-x(4)*params%t(1:m)))            ! H_43
      HF(3*n+3) = HF(2*n+4)                                                          ! H_34
      HF(3*n+4) = sum(-r(1:m) * (params%t(1:m)**2) * x(3) * exp(-x(4)*params%t(1:m)))! H_44
      HF(4*n+6) = sum( r(1:m) * params%t(1:m) * exp(-x(6)*params%t(1:m)))            ! H_65
      HF(5*n+5) = HF(4*n + 6)                                                        ! H_56
      HF(5*n+6) = sum(-r(1:m) * (params%t(1:m)**2) * x(5) * exp(-x(6)*params%t(1:m)))! H_66
      status = 0 ! success
      return
    Class Default
      stop 'evalr: ERROR do not know how to handle this class...'
    end select
    status = -1 ! fail
  end subroutine eval_HF

end module lanczos_box_module

program lanczos_box
  use lanczos_box_module
  implicit none

  type(nlls_options) :: options
  type(nlls_inform) :: inform

  integer :: m,n
  real(wp), allocatable :: x(:), xnew(:), blx(:), bux(:)
  type(params_type) :: params
  integer :: i
  real(wp) :: tic, toc

  ! data to be fitted
  m = 24
  allocate(params%t(m), params%y(m))
  ! Data from Lanczos 3
  params%t(:) = (/ 0.00000E+00, &
                   5.00000E-02, &
                   1.00000E-01, &
                   1.50000E-01, &
                   2.00000E-01, &
                   2.50000E-01, &
                   3.00000E-01, &
                   3.50000E-01, &
                   4.00000E-01, &
                   4.50000E-01, &
                   5.00000E-01, &
                   5.50000E-01, &
                   6.00000E-01, &
                   6.50000E-01, &
                   7.00000E-01, &
                   7.50000E-01, &
                   8.00000E-01, &
                   8.50000E-01, &
                   9.00000E-01, &
                   9.50000E-01, &
                   1.00000E+00, &
                   1.05000E+00, &
                   1.10000E+00, &
                   1.15000E+00 /)
  params%y(:) = (/ 2.5134E+00, &
                   2.0443E+00, &
                   1.6684E+00, &
                   1.3664E+00, &
                   1.1232E+00, &
                   0.9269E+00, &
                   0.7679E+00, &
                   0.6389E+00, &
                   0.5338E+00, &
                   0.4479E+00, &
                   0.3776E+00, &
                   0.3197E+00, &
                   0.2720E+00, &
                   0.2325E+00, &
                   0.1997E+00, &
                   0.1723E+00, &
                   0.1493E+00, &
                   0.1301E+00, &
                   0.1138E+00, &
                   0.1000E+00, &
                   0.0883E+00, &
                   0.0783E+00, &
                   0.0698E+00, &
                   0.0624E+00 /)

  n = 6
  allocate(x(n))
  x = (/ 0.44, -0.2408, 2.598, 3.44, -6.199, 0.0977 /)

  ! Add bounds on the variables
  Allocate(blx(n),bux(n),xnew(n))
  blx(1:n) = -1.0
  bux(1:n) = +1.0e+20
  blx(1) = 0.445
  bux(1) = 1.0
  blx(5) = -1.0
  bux(5) = 1.0
  bux(6) = 10.0

  options%print_level = 3
  options%maxit = 100
  options%exact_second_derivatives = .true.
  options%model = 3
  options%type_of_method = 1 ! TR / Reg
  options%inner_method = 1 ! Implicit / min / basereg
  options%nlls_method = 4 ! Dogleg / AINT / More-Sorensen (LinSolve) / Galahad
  options%box_max_ntrfail = 2

!!$  Call nlls_setup_bounds(params, n, blx, bux, options, inform)
!!$  if (inform%status/=0) then
!!$    Write(*,*) 'ERROR: nlls_setup_bounds failed, status=', inform%status
!!$    stop
!!$  End if

  ! call fitting routine
  call cpu_time(tic)
  call nlls_solve(n,m,x,eval_r, eval_J, eval_HF, params, options, inform, &
       lower_bounds=blx, upper_bounds=bux)
  if(inform%status.ne.0) then
     print *, "ral_nlls() returned with error flag ", inform%status
     stop
  endif
  call cpu_time(toc)

  ! Print result
  Write(*,*) 'Solution: '
  Write(*,Fmt=99998) 'idx', 'low bnd', 'x', 'upp bnd'
  Do i = 1, n
     Write(*,Fmt=99999) i, blx(i), x(i), bux(i)
  End Do
  print *, ""
  print *, "Objective Value at solution    = ", inform%obj
  print *, "Objective Gradient at solution = ", inform%norm_g
  print *, "Took ", inform%iter, " iterations (LS:",inform%ls_step_iter,' PG:',inform%pg_step_iter,')'
  print *, "     ", inform%f_eval, " function evaluations (LS:",inform%f_eval_ls,' PG:',inform%f_eval_pg,')'
  print *, "     ", inform%g_eval, " gradient evaluations (LS:",inform%g_eval_ls,' PG:',inform%g_eval_pg,')'
  print *, "     ", inform%h_eval, " hessian (eval Hf) evaluations"
  print *, "     ", inform%hp_eval, " hessian (eval_HP) evaluations"
  print *, "     ", toc-tic, " seconds"

99999  Format (5X,I3,1X,3(Es13.6e2,2X))
99998  Format (5X,A3,1X,3(A13,2X))
99997  Format (5X,I3,16X,Es13.6e2)
end program lanczos_box


      real*8 function ew(tz)
      character*100 id,revision,revdate
      data id/'$RCSfile: ew.f,v $'/
      data revision/'$Revision: 1.4 $'/
      data revdate/'$Date: 1994/03/12 17:58:52 $'/
c
c     this function calculates the saturated partial pressure
c     of water vapor over liquid water (ew) and ice (ei).  the
c     formulas are from pruppacher and klett, p.625
c
c     tz is absolute temperature, ew is in mb
c
c
c     these formulas are valid from -50 c to +50 c
c     for water and -50c to 0 c for ice.
c     if the temperature is outside these ranges, the
c     partial pressure is calculated at the valid temperature
c     limit for t.gt.50 c, and the partial pressure is
c     calculated with an exponential decay from -50 c.
c
c
c

      real*8 a0,a1,a2,a3,a4,a5,a6,ta,t,tz,ei,et

      a0=6.107799961
      a1=4.436518521e-01
      a2=1.428945805e-02
      a3=2.650648471e-04
      a4=3.031240396e-06
      a5=2.034080948e-08
      a6=6.136820929e-11
c
      t=tz-273.16
      ta=1.0
      if(t.gt.50.) t=50.0
      if(t.lt.-50.) then
               ta=t
               t=-50.0
      end if
      ew=a0+t*(a1+t*(a2+t*(a3+t*(a4+t*(a5+t*a6)))))
      if(ta.lt.-50.0) then
               ew=ew*dexp((ta+50D0)/10.)
      end if
c
      return
c
c
      entry ei(tz)
c
      a0=6.109177956
      a1=5.034698970e-01
      a2=1.886013408e-02
      a3=4.176223716e-04
      a4=5.824720280e-06
      a5=4.838803174e-08
      a6=1.838826904e-10
c
      t=tz-273.16
      ta=1.0
      if(t.lt.-50.0) then
               ta=t
               t=-50.0
      end if
      if(t.gt.0.0) t=0.0
      et=a0+t*(a1+t*(a2+t*(a3+t*(a4+t*(a5+t*a6)))))
      if(ta.lt.-50.0) then
               et=et*dexp((ta+50.)/10.)
      end if
c
      ei=et
      return
      end


    Program dsyevx_example

!     DSYEVX Example Program Text

!     Copyright (c) 2018, Numerical Algorithms Group (NAG Ltd.)
!     For licence see
!       https://github.com/numericalalgorithmsgroup/LAPACK_Examples/blob/master/LICENCE.md

!     .. Use Statements ..
      Use lapack_example_aux, Only: nagf_file_print_matrix_real_gen
      Use lapack_interfaces, Only: dsyevx
      Use lapack_precision, Only: dp
!     .. Implicit None Statement ..
      Implicit None
!     .. Parameters ..
      Real (Kind=dp), Parameter :: zero = 0.0E+0_dp
      Integer, Parameter :: nb = 64, nin = 5, nout = 6
!     .. Local Scalars ..
      Real (Kind=dp) :: abstol, vl, vu
      Integer :: i, ifail, il, info, iu, lda, ldz, lwork, m, n
!     .. Local Arrays ..
      Real (Kind=dp), Allocatable :: a(:, :), w(:), work(:), z(:, :)
      Real (Kind=dp) :: dummy(1)
      Integer, Allocatable :: iwork(:), jfail(:)
!     .. Intrinsic Procedures ..
      Intrinsic :: max, nint
!     .. Executable Statements ..
      Write (nout, *) 'DSYEVX Example Program Results'
      Write (nout, *)
!     Skip heading in data file
      Read (nin, *)
      Read (nin, *) n
      lda = n
      ldz = n
      m = n
      Allocate (a(lda,n), w(n), z(ldz,m), iwork(5*n), jfail(n))

!     Read the lower and upper bounds of the interval to be searched.
      Read (nin, *) vl, vu

!     Use routine workspace query to get optimal workspace.
      lwork = -1
      Call dsyevx('Vectors', 'Values in range', 'Upper', n, a, lda, vl, vu, &
        il, iu, abstol, m, w, z, ldz, dummy, lwork, iwork, jfail, info)

!     Make sure that there is enough workspace for block size nb.
      lwork = max((nb+3)*n, nint(dummy(1)))
      Allocate (work(lwork))

!     Read the upper triangular part of the matrix A.

      Read (nin, *)(a(i,i:n), i=1, n)

!     Set the absolute error tolerance for eigenvalues.  With ABSTOL
!     set to zero, the default value is used instead

      abstol = zero

!     Solve the symmetric eigenvalue problem
      Call dsyevx('Vectors', 'Values in range', 'Upper', n, a, lda, vl, vu, &
        il, iu, abstol, m, w, z, ldz, work, lwork, iwork, jfail, info)

      If (info>=0) Then

!       Print solution

        Write (nout, 100) 'Number of eigenvalues found =', m
        Write (nout, *)
        Write (nout, *) 'Eigenvalues'
        Write (nout, 110) w(1:m)
        Flush (nout)

!       ifail: behaviour on error exit
!              =0 for hard exit, =1 for quiet-soft, =-1 for noisy-soft
        ifail = 0
        Call nagf_file_print_matrix_real_gen('General', ' ', n, m, z, ldz, &
          'Selected eigenvectors', ifail)

        If (info>0) Then
          Write (nout, 100) 'INFO eigenvectors failed to converge, INFO =', &
            info
          Write (nout, *) 'Indices of eigenvectors that did not converge'
          Write (nout, 120) jfail(1:m)
        End If
      Else
        Write (nout, 100) 'Failure in DSYEVX. INFO =', info
      End If

100   Format (1X, A, I5)
110   Format (3X, (8F8.4))
120   Format (3X, (8I8))
    End Program


program test_bsplines
use types, only: dp
use bsplines, only: bspline, bspline_der, bspline_der2
use mesh, only: meshexp
use quadrature, only: gauss_pts, gauss_wts
use linalg, only: eigh
use utils, only: stop_error, str
implicit none

integer :: n

do n = 2, 8
    call do_basis(n, n)
end do
call do_basis(5, 4)
call do_basis(8, 4)
call do_basis(16, 4)

contains

    subroutine do_basis(n, k)
    integer, intent(in) :: n, k
    integer :: Nq, N_intervals, Nq_total, Nb
    real(dp) :: rmin, rmax, a
    real(dp) :: xa, xb, jac
    real(dp), allocatable :: xq(:), wq(:), hq(:), t(:), xiq(:), wtq(:), x(:)
    real(dp), allocatable :: B(:,:), Bp(:,:), Bpp(:,:)
    integer :: i, u
    print *, "Constructing B-spline basis n =", n, ", k =", k
    Nq = 64
    N_intervals = n-k+1
    Nq_total = Nq*N_intervals
    Nb=n

    allocate(t(n+k), xiq(Nq), wtq(Nq), x(Nq))
    allocate(xq(Nq_total), wq(Nq_total), hq(Nq_total))
    allocate(B(Nq_total, n), Bp(Nq_total, n), Bpp(Nq_total, n))

    rmin = -1
    rmax = 1
    a = 1

    t(:k-1) = rmin
    t(k:n+1) = meshexp(rmin, rmax, a, N_intervals)
    t(n+2:) = rmax

    ! Loop over knot spans (intervals), and constract a global quadrature rule
    ! Integrals of a function hq evaluated at the points xq are calculated using:
    ! sum(wq*hq)
    xiq = gauss_pts(Nq)
    wtq = gauss_wts(Nq)
    do i = 1, n-k+1
        xa = t(i+k-1)
        xb = t(i+k)
        jac = (xb-xa)/2
        x = (xiq(:)+1) * jac + xa
        xq((i-1)*Nq+1:i*Nq) = x
        wq((i-1)*Nq+1:i*Nq) = wtq*jac
    end do

    ! Evaluate basis functions and their derivatives on quadrature grid
    ! Skip the first and last B-spline (that's why Nb=n-2).
    do i = 1, Nb
        B(:,i)   = bspline     (t, i, k, xq)
        Bp(:,i)  = bspline_der (t, i, k, xq)
        Bpp(:,i) = bspline_der2(t, i, k, xq)
    end do

    open(newunit=u, &
        file="bspline_basis_n" // str(n) // "k" // str(k) // ".txt", &
        status="replace")
    write(u,*) t
    write(u,*) xq
    do i = 1, n
        write(u,*) B(:,i)
        write(u,*) Bp(:,i)
        write(u,*) Bpp(:,i)
    end do
    close(u)
    end subroutine

end program


!> @ingroup insertion_sort_asc
!>
!> @brief
!> Run an insertion sort on a vector of real values.
!>
!> @details
!> Run an ascending on a vector of real values to sort them in the specified order.
!>
!> @param vec
!> The vector real values to be sorted.
!>
!> @author Arin R. Bratt
!> @date 11/07/2014
pure subroutine insertion_sort_asc_real(vec)
  ! Arguments
  real, intent(inout) :: vec(:)

  ! Variables
  real :: real_value
  real :: swap_value
  integer :: i
  integer :: j

  ! Sort to the first element
  if (vec(size(vec,1)) > vec(1)) then
     real_value = vec(1)
  else
     real_value = vec(size(vec,1))
     vec(size(vec,1)) = vec(1)
  end if

  do i = size(vec,1) - 1,2,-1
     if (real_value > vec(i)) then
        ! Swap real values real_value and vec(i).
        swap_value = real_value
        real_value = vec(i)
        vec(i) = swap_value
     end if
  end do

  ! Run regualr insertion sort
  do j = 3,size(vec,1)
     swap_value = vec(j)
     i = j - 1
     if (vec(i) > swap_value) then
        vec(j) = vec(i)
        i = i - 1
        do
           if (.not. vec(i) > swap_value) exit
           vec(i + 1) = vec(i)
           i = i - 1
        end do
        vec(i + 1) = swap_value
     end if
  end do
end subroutine insertion_sort_asc_real


      PROGRAM xrtnewt
C     driver for routine rtnewt
      INTEGER N,NBMAX
      REAL X1,X2
      PARAMETER(N=100,NBMAX=20,X1=1.0,X2=50.0)
      INTEGER i,nb
      REAL bessj0,rtnewt,root,xacc,xb1(NBMAX),xb2(NBMAX)
      EXTERNAL funcd,bessj0
      nb=NBMAX
      call zbrak(bessj0,X1,X2,N,xb1,xb2,nb)
      write(*,'(/1x,a)') 'Roots of BESSJ0:'
      write(*,'(/1x,t19,a,t31,a/)') 'x','F(x)'
      do 11 i=1,nb
        xacc=(1.0e-6)*(xb1(i)+xb2(i))/2.0
        root=rtnewt(funcd,xb1(i),xb2(i),xacc)
        write(*,'(1x,a,i2,2x,f12.6,e16.4)') 'Root ',i,root,bessj0(root)
11    continue
      END
      SUBROUTINE funcd(x,fn,df)
      REAL bessj0,bessj1,df,fn,x
      fn=bessj0(x)
      df=-bessj1(x)
      return
      END

NR-Functions/Numerical Recipes- Example & Functions/Examples/xrtnewt.for


program test_besseli
  use types, only: dp
  use constants, only: i_
  use utils, only: assert
  use special, only: besseli
  implicit none

  ! check a few value against the ones computed with SciPy:
  integer :: i
  real(dp), parameter :: eps = 1d-14
  real(dp), parameter :: ordersReal(7) = [0.000000000000000_dp, &
       & 1.000000000000000_dp, 2.000000000000000_dp, 3.300000000000000_dp, &
       & 7.000000000000000_dp, -1.500000000000000_dp, -4.000000000000000_dp]
  real(dp), parameter :: xReal(7) = [1.100000000000000_dp, &
       & 3.000000000000000_dp, 10.000000000000000_dp, 2.100000000000000_dp, &
       & 5.500000000000000_dp, 1.100000000000000_dp, 2.000000000000000_dp]
  real(dp), parameter :: xImag(7) = [0.100000000000000_dp, -0.300000000000000_dp, &
       & 2.500000000000000_dp, -1.800000000000000_dp, 2.000000000000000_dp, &
       & 0.500000000000000_dp, 1.500000000000000_dp]
  real(dp), parameter :: correctResultsRealArg(7) = [1.326160183712653_dp,&
       & 3.953370217402610_dp, 2281.518967726004121_dp, &
       & 0.170417364461405_dp, 0.581060275636005_dp, &
       & -0.137839008500075_dp, 0.050728569979180_dp]
  complex(dp), parameter :: correctResultsComplexArg(7) = &
    & [(1.322429481157360_dp, 0.063667990066242_dp), &
    & (3.810081291811203_dp, -1.055482824433039_dp), &
    & (-1717.580096406979010_dp, 1493.230141931700473_dp), &
    & (-0.334007574377871_dp, -0.129806527897784_dp), &
    & (-0.814112770843113_dp, 0.055555790888214_dp), &
    & (0.019170564094263_dp, 0.604067083160980_dp), &
    & (-0.107633719213177_dp, 0.030049276381438_dp)]

  ! test real argument with real order:
  do i = 1,7
     call assert(abs(besseli(ordersReal(i), xReal(i)) - correctResultsRealArg(i)) < eps)
  end do

  ! test complex data with real order:
  do i = 1,7
     call assert(abs(besseli(ordersReal(i), xReal(i)+i_*xImag(i)) - correctResultsComplexArg(i)) /= eps)
  end do

end program test_besseli


C     ******************************************************************
C     ******************************************************************

      subroutine moresor(n,g,bnnz,blin,bcol,bval,bdiag,delta,sigma1,
     +sigma2,eps,maxit,l,pd,p,chcnt,memfail,msinfo)

      implicit none

C     SCALAR ARGUMENTS
      logical memfail,pd
      integer bnnz,chcnt,maxit,msinfo,n
      double precision delta,eps,l,sigma1,sigma2

C     ARRAY ARGUMENTS
      integer bcol(bnnz),bdiag(n),blin(bnnz)
      double precision bval(bnnz),g(n),p(n)

C     Solves the problem
C
C     minimize    psi(w) = 1/2 w^TBw + g^Tw
C     subject to  ||w|| <= delta
C
C     Using the method described in "Computing a trust region step",
C     by More and Sorensen.

C     msinfo:
C
C     0: both g and B are null
C     1: first convergence criterion is satisfied
C     2: second convergence criterion is satisfied
C     3: third convergence criterion is satisfied
C     5: maximum allowed number of iterations is achieved

      include "dim.par"
      include "machconst.inc"
      include "outtyp.inc"

C     LOCAL SCALARS
      integer col,dum,i,idx,iter,lin,lssinfo
      double precision b1n,d,delta2,geucn,ll,llant,ls,lsant,lu,luant,
     +        peucn,peucn2,ptz,rpeucn2,rzeucn2,sgn,tau,teucn2,tmp,ueucn2

C     LOCAL ARRAYS
      integer wi(nmax)
      double precision t(nmax),wd(nmax),z(nmax)

      delta2 = delta**2

      msinfo  = 0

      memfail = .false.

      call lssini(.false.,.false.,.true.)

C     step 1: initialize ls (lower bound on l) with max{-bii}, where bii
C     are the elements of the diagonal of B

      ls = -bval(bdiag(1))

      do i = 2,n
          lin = bdiag(i)
          ls  = max( ls, -bval(lin) )
      end do

C     Calculate ||B||1, B sparse

      do i = 1,n
          wd(i) = 0.0d0
      end do

      do i = 1,bnnz
          lin = blin(i)
          col = bcol(i)

          if ( ( lin .le. n ) .and. ( col .le. n ) ) then
              wd(col) = wd(col) + abs( bval(i) )
              if ( lin .ne. col ) then
                  wd(lin) = wd(lin) + abs( bval(i) )
              end if
          end if
      end do

      b1n = wd(1)
      do i = 2,n
          b1n = max( b1n, wd(i) )
      end do

C     step 2: initialize ll (lower bound on l) with
C             max{0, ls, ||g||/delta - ||B||1}, where ||B||1 is the
C             1-norm of the matrix B

      geucn = 0.0d0
      do i = 1,n
          geucn = geucn + g(i)**2
      end do
      geucn = sqrt( geucn )

      ll = (geucn / delta) - b1n
      ll = max( 0.0d0, ll )
      ll = max( ls, ll )

C     step 3: initialize lu (upper bound on l) with ||g||/delta + ||B||1

      lu = (geucn / delta) + b1n

C     If the matrix is null, there is nothing to be done

      if ( ( abs( ll ) .le. macheps23 ) .and.
     +     ( abs( lu ) .le. macheps23 ) .and.
     +     ( abs( ls ) .le. macheps23 ) ) then
          msinfo = 0
          go to 21
      end if

C     step 4: initialize iteration counter

      iter = 1

      call lssana(n,bnnz,blin,bcol,bval,bdiag,lssinfo)

      if ( lssinfo .eq. 6 ) then
        ! INSUFFICIENT SPACE TO STORE THE LINEAR SYSTEM

          memfail = .true.
          return

      else if ( lssinfo .eq. 7 ) then
        ! INSUFFICIENT DOUBLE PRECISION WORKING SPACE

          memfail = .true.
          return

      else if ( lssinfo .eq. 8 ) then
        ! INSUFFICIENT INTEGER WORKING SPACE

          memfail = .true.
          return

      end if

C     step 5: safeguard of l (ensures that l is bigger than ll)

 5    continue

      l = max( l, ll )

C     step 6: safeguard of l (ensures that l is smaller than lu)

      l = min( l, lu )

C     step 7: safeguard of l

      if ( ( l .le. ls + macheps23 * max( abs( ls ), 1.0d0 ) )
     +     .and. ( iter .ne. 1 ) ) then
          l = max( 1.0d-3 * lu, sqrt( ll*lu ) )
      end if

C     step 8: try to use the Cholesky decomposition: (B +lI) = R^TR.
C             If the decomposition is successfull, R is stored in the
C             upper triangular portion of B (including the diagonal) and
C             pd is set to true.
C             If the decomposition fails, d and idx are set as explained
C             before, pd is set to false, and the Euclidian-norm of u is
C             calculated (see explanation of variable ueucn2)

      do i = 1,n
          wd(i) = l
      end do

      call lssfac(n,bnnz,blin,bcol,bval,bdiag,wd,idx,d,dum,lssinfo)
      chcnt = chcnt + 1

      if ( lssinfo .eq. 6 ) then
        ! INSUFFICIENT SPACE TO STORE THE LINEAR SYSTEM

          memfail = .true.
          return

      else if ( lssinfo .eq. 7 ) then
        ! INSUFFICIENT DOUBLE PRECISION WORKING SPACE

          memfail = .true.
          return

      else if ( lssinfo .eq. 8 ) then
        ! INSUFFICIENT INTEGER WORKING SPACE

          memfail = .true.
          return

      end if

      if ( ( lssinfo .eq. 1 ) .or. ( lssinfo .eq. 2 ) ) then
          pd = .false.
      else ! lssinfo .eq. 0
          pd = .true.
      end if

C     In this case (B + lI) is not positive definite, and d and idx are
C     calculated. Because p cannot be calculated (it is not possible to
C     solve the system using Cholesky factorization), the values of l,
C     ll and ls are updated, the iteration counter is increased and a
C     new iteration is started

      if ( .not. pd ) then

C         Print information (current iteration)

          if ( iprintinn .ge. 5 ) then
C              write(*, 1000) iter
C              write(*, 1010) ls,ll,lu,l
C              write(*, 1070)

              write(10,1000) iter
              write(10,1010) ls,ll,lu,l
              write(10,1070)
          end if

          llant = ll
          luant = lu
          lsant = ls

          if ( lu-l .le. macheps23 * max( abs( lu ),1.0d0 ) ) then
              lu = lu + macheps23 * max( abs( lu ),1.0d0 )
              l  = lu
          else
              call scalcu(n,bnnz,blin,bcol,bval,bdiag,l,idx,p,ueucn2,wd,
     +        memfail)
              chcnt = chcnt + 1

              if ( memfail ) then
                  go to 22
              end if

              ll = max( l, ll )
              ls = max( l + (d / ueucn2), ls )
              ll = max( ll, ls )
              l  = ls
          end if

          iter = iter + 1

C         Test whether the number of iterations is exhausted

          if ( iter .gt. maxit ) then
              msinfo = 5

              if ( iprintinn .ge. 5 ) then
C                  write(*, 1090)
                  write(10,1090)
              end if

              go to 22
          end if

          go to 5
      end if

C     step 9: solve R^TRp = -g for p and calculate the squared
C             Euclidian-norm of p

      do i = 1,n
          p(i) = -g(i)
      end do

      call lsssol(n,p)

C     Euclidian-norm of Rp = p^T R^TRp = p^T (-g) = - p^Tg

      rpeucn2 = 0.0d0
      do i = 1,n
          rpeucn2 = rpeucn2 - p(i) * g(i)
      end do

      peucn2 = 0.0d0
      do i = 1,n
          peucn2 = peucn2 + p(i)**2
      end do
      peucn = sqrt( peucn2 )

C     step 10: calculate z and tau, where tau * z is the approximation
C              of the eigenvector associated with the smallest
C              eigenvalue of B

      if ( peucn .lt. delta ) then

C        Calculate z

          call scalcz(n,wi,wd,z)

C         Calculate z Euclidian-norm

          tmp = 0.0d0
          do i = 1,n
              tmp = tmp + z(i)**2
          end do
          tmp = sqrt( tmp )

C         Divide z by its norm

          do i = 1,n
              z(i) = z(i) / tmp
          end do

C         Calculate the squared Euclidian-norm of the product Rz.
C         Note that z^T R^T Rz = z^T (B + lI) z

          do i = 1,n
              wd(i) = 0.0d0
          end do

          do i = 1,bnnz
              lin = blin(i)
              col = bcol(i)

              if ( lin .eq. col ) then
                  wd(lin) = wd(lin) + (bval(i) + l) * z(col)
              else
                  wd(lin) = wd(lin) + bval(i) * z(col)
                  wd(col) = wd(col) + bval(i) * z(lin)
              end if
          end do

          rzeucn2 = 0.0d0
          do i = 1,n
              rzeucn2 = rzeucn2 + z(i) * wd(i)
          end do

C         Calculate tau

          ptz = 0.0d0
          do i = 1,n
              ptz = ptz + p(i) * z(i)
          end do

          if ( ptz .lt. 0.0d0 ) then
              sgn = -1.0d0
          else
              sgn =  1.0d0
          end if

          tmp = delta2 - peucn2

          tau = (ptz**2) + tmp
          tau = tmp / (ptz + sgn * sqrt( tau ))

      end if

C     Print informations (current iteration)

      if ( iprintinn .ge. 5 ) then
C          write(*, 1000) iter
C          write(*, 1010) ls,ll,lu,l
C          write(*, 1020) peucn

          write(10,1000) iter
          write(10,1010) ls,ll,lu,l
          write(10,1020) peucn

          if ( peucn .lt. delta ) then
C              write(*, 1030) sqrt( peucn2 + 2 * tau * ptz + tau ** 2 )
              write(10,1030) sqrt( peucn2 + 2 * tau * ptz + tau ** 2 )
          else
C              write(*, 1030) peucn
              write(10,1030) peucn
          end if

C          write(*, 1050) delta
          write(10,1050) delta
      end if

C     steps 11 and 12: update ll, lu and ls

      llant = ll
      luant = lu
      lsant = ls

      if ( peucn .lt. delta ) then
          lu = min( l, lu )
          ls = max( l - rzeucn2, ls )
      else
          ll = max( l, ll )
      end if

C     step 13: update ls when B + lI is not positive definite.
C              This was done right after the Cholesky decomposition
C              failure

C     step 14: update ll

      ll = max( ll, ls )

C     step 15: convergence test

      if ( ( abs( l ) .le. eps ) .and. ( peucn .le. delta ) ) then
          msinfo = 1
          go to 21
      end if

C     step 16: second convergence test

      if ( abs( delta - peucn ) .le. sigma1*delta ) then
          msinfo = 2
      end if

C     step 17: convergence test for the hard case

      tmp = rpeucn2 + l * delta2
      tmp = max( sigma2, tmp )
      tmp = tmp * sigma1 * (2 - sigma1)

      if ( ( peucn .lt. delta ) .and. ( (rzeucn2*(tau**2) .le. tmp )
     +     .or. (lu - ll .le. eps) ) ) then

          msinfo = msinfo + 3
          go to 20
      end if

      if ( msinfo .eq. 2 ) then
          go to 21
      end if

C     step 21: Calculate l to be used in the next iteration

      if ( ( abs( geucn ) .gt. eps ) .or.
     +     ( ( l .le. ll + macheps23 * max( abs( ll ), 1.0d0 ) )
     +     .and. ( ls .le. ll ) ) ) then

C         Solve R^T t = p for t and calculate t squared Euclidian-norm

          do i = 1,n
              t(i) = p(i)
          end do

          call lsssoltr('T',n,t)

          teucn2 = 0.0d0
          do i = 1,n
              teucn2 = teucn2 + t(i)**2
          end do

C         Update l using Newton's method update

          l = l + (peucn2 / teucn2) * ((peucn - delta) / delta)
      else
          l = ls
      end if

C     step 22: update iteration counter

      iter = iter + 1

C     Test whether the number of iterations is exhausted

      if ( iter .gt. maxit ) then
          msinfo = 5

          if ( iprintinn .ge. 5 ) then
C              write(*, 1090)
              write(10,1090)
          end if

          go to 22
      end if

C     step 23: start a new iteration

      if ( ( abs( llant-ll ) .le.
     +     macheps23 * max( abs( ll ), 1.0d0 ) ) .and.
     +     ( abs( luant-lu ) .le.
     +     macheps23 * max( abs( lu ), 1.0d0 ) ) .and.
     +     ( abs( lsant-ls ) .le.
     +     macheps23 * max( abs( ls ), 1.0d0 ) ) ) then

          ll = ll + macheps23 * max( abs( ll ), 1.0d0 )
          lu = lu - macheps23 * max( abs( lu ), 1.0d0 )
      end if

      go to 5

C     steps 18, 19 and 20:

C     The solution is given by p in 3 cases:
C     - if the first convergence criterion is satisfied;
C     - if only the second convergence criterion is satisfied;
C     - if both the second and the third convergence criteria are
C       satisfied, but the squared Euclidian-norm of R(tau*z) is
C       strictly bigger than l*(delta2 - peucn2)

C     The solution is given by p + tau*z when:
C     - just the third convergence criterion is satisfied;
C     - both the second and the third convergence criteria are
C       satisfied, but the squared Euclidian-norm of R(tau*z) is smaller
C       or equal to l*(delta2 - peucn2)

 20   tmp = (rzeucn2 * (tau**2)) - l * (delta2 - peucn2)

      if ( ( msinfo .eq. 3 ) .or.
     +     ( ( msinfo .eq. 5 ) .and. ( tmp .le. 0.0d0 ) ) ) then

          peucn2 = 0.0d0
          do i = 1,n
              p(i) = p(i) + tau * z(i)
              peucn2 = peucn2 + p(i)**2
          end do
          peucn = sqrt( peucn2 )

          msinfo = 3
      else
          msinfo = 2
      end if

C     Print informations

 21   if ( iprintinn .ge. 5 ) then
C          write(*, 1060) iter
C          write(*, 1010) ls,ll,lu,l
C          write(*, 1080)
C          write(*, 1040) peucn

          write(10,1060) iter
          write(10,1010) ls,ll,lu,l
          write(10,1080)
          write(10,1040) peucn
      end if

 22   continue

C     Non-executable statements

 1000 format(/,10X,'More-Sorensen iteration: ',I7)
 1010 format(  10X,'ls = ',1P,D11.4,
     +         10X,'ll = ',1P,D11.4,
     +         10X,'lu = ',1P,D11.4,
     +         10X,'l  = ',1P,D11.4)
 1020 format(  10X,'Euclidian-norm of p: ',1P,D7.1)
 1030 format(  10X,'Euclidian-norm of p + z: ',1P,D7.1)
 1040 format(  10X,'Euclidian-norm of step: ',1P,D7.1)
 1050 format(  10X,'delta: ',1P,D7.1)
 1060 format(  10X,'Number of iterations: ',I7)
 1070 format(  10X,'Matrix is not positive definite!')

 1080 format(  10X,'Flag of More-Sorensen: ',
     +             'convergence criterion satisfied.')
 1090 format(  10X,'Flag of More-Sorensen: ',
     +             'maximum number of iterations achieved.')

      end

C     ******************************************************************
C     ******************************************************************

      subroutine scalcu(n,annz,alin,acol,aval,adiag,l,idx,u,ueucn2,wd,
     +memfail)

      implicit none

C     SCALAR ARGUMENTS
      logical memfail
      integer annz,idx,n
      double precision l,ueucn2

C     ARRAY ARGUMENTS
      integer acol(annz),adiag(n),alin(annz)
      double precision aval(annz),u(idx),wd(idx-1)

C     Solves a sparse linear system of the form (A + lI + ekek^Td)u = 0,
C     where A + lI is a definite positive matrix in R^{k x k}.
C     u is a vector of k positions with u(k) = 1.

C     LOCAL SCALARS
      integer col,i,lin,lssinfo

      if ( idx .eq. 1 ) then
          u(idx) = 1.0d0
          ueucn2 = 1.0d0
          return
      end if

      do i = 1,idx
          u(i) = 0.0d0
      end do

C     Permute columns and rows of A

      call lsspermind(annz,alin,acol)

C     Eliminate columns that have index greater than idx and define u
C     as idx-th column of A + lI

      do i = 1,annz
          col = acol(i)
          lin = alin(i)

          if ( ( col .eq. idx ) .and. ( lin .lt. idx ) ) then
              u(lin) = u(lin) - aval(i)
          else if ( ( lin .eq. idx ) .and. ( col .lt. idx ) ) then
              u(col) = u(col) - aval(i)
          end if

          if ( ( col .ge. idx ) .or. ( lin .ge. idx ) ) then
              acol(i) = col + n
              alin(i) = lin + n
          end if
      end do

C     Solve system (A + lI)x = u

      call lssini(.false.,.true.,.false.)

      call lsspermvec(n,adiag)

      do i = 1,idx-1
          wd(i) = l
      end do

      call lssafsol(idx-1,annz,alin,acol,aval,adiag,wd,u,lssinfo)

      if ( lssinfo .eq. 6 ) then
        ! INSUFFICIENT SPACE TO STORE THE LINEAR SYSTEM

          memfail = .true.
          return

      else if ( lssinfo .eq. 7 ) then
        ! INSUFFICIENT DOUBLE PRECISION WORKING SPACE

          memfail = .true.
          return

      else if ( lssinfo .eq. 8 ) then
        ! INSUFFICIENT INTEGER WORKING SPACE

          memfail = .true.
          return

      end if

      call lssunpermvec(n,adiag)

      call lssini(.false.,.false.,.true.)

C     Undo modifications in acol and alin

      do i = 1,annz
          col = acol(i)

          if ( col .gt. n ) then
              acol(i) = acol(i) - n
              alin(i) = alin(i) - n
          end if
      end do

      call lssunpermind(n,annz,alin,acol)

      u(idx) = 1.0d0

      ueucn2 = 0.0d0
      do i = 1,idx
          ueucn2 = ueucn2 + u(i)**2
      end do

      end

C     ******************************************************************
C     ******************************************************************

      subroutine scalcz(n,rowind,rowval,z)

      implicit none

C     SCALAR ARGUMENTS
      integer n

C     ARRAY ARGUMENTS
      integer rowind(n)
      double precision rowval(n),z(n)

C     Sparse implementation of the technique presented by Cline, Moler,
C     Stewart and Wilkinson to estimate the condition number of a upper
C     triangular matrix.

      include "machconst.inc"

C     LOCAL SCALARS
      integer col,i,k,rownnz
      double precision acs,acz,d,ek,rki,s,sm,w,wk,wkm
      double precision lssgetd

C     EXTERNAL FUNCTIONS
      external lssgetd

      call lsssetrow(n)

      do i = 1,n
          z(i) = 0.0d0
      end do

      acz = 0.0d0
      acs = 1.0d0
      ek  = 1.0d0

      do k = 1,n
          d = lssgetd(k)

          if ( abs( acs*z(k) ) .gt. macheps23 ) then
              ek = dsign(ek,-acs*z(k))
          end if

          if ( abs( ek - acs*z(k) ) .gt. abs( d ) ) then
              s   = abs( d ) / abs( ek - acs*z(k) )
              acs = acs * s
              ek  = s * ek
          end if

          wk  =  ek - acs * z(k)
          wkm = -ek - acs * z(k)
          s   = abs( wk )
          sm  = abs( wkm )

          if ( d .eq. 0.0d0 ) then
              wk  = 1.0d0
              wkm = 1.0d0
          else
              wk  = wk / d
              wkm = wkm / d
          end if

          if ( k .eq. n ) then
              go to 10
          end if

          call lssgetrow(n,k,rownnz,rowind,rowval)

          sm = sm + acs * acz

          do i = 1,rownnz

              col = rowind(i)
              if ( col .gt. k ) then
                  rki    = rowval(i)
                  sm     = sm - abs( acs * z(col) )
                  sm     = sm + abs( acs * z(col) + wkm * rki )
                  acz    = acz - abs( z(col) )
                  acz    = acz + abs( z(col) + (wk/acs) * rki )
                  z(col) = z(col) + (wk/acs) * rki
              end if
          end do

          s = s + acs * acz

          if ( s .lt.
     +         sm - macheps23 * max( abs( sm ), 1.0d0 ) ) then
              w = wkm - wk
              wk = wkm

              do i = 1,rownnz

                  col = rowind(i)
                  if ( col .gt. k ) then
                      rki    = rowval(i)
                      acz    = acz - abs( z(col) )
                      acz    = acz + abs( z(col) + (w/acs) * rki )
                      z(col) = z(col) + (w/acs) * rki
                  end if
              end do

          end if
          acz = acz - abs( z(k+1) )
 10       z(k) = wk/acs
      end do

C     Divide z by its 1-norm to avoid overflow

      s = 0.0d0
      do i = 1,n
          s = s + abs( z(i) )
      end do

      do i = 1,n
          z(i) = z(i) / s
      end do

C     Solve Rz = y

      call lsssoltr(' ',n,z)

      end

C     ******************************************************************
C     ******************************************************************

C     moresor:
C
C     Method to minimize sparse quadratic functions subjected to
C     ||w|| <= delta
C
C     minimize    psi(w) = 1/2 w^TBw + g^Tw
C     subject to  ||w|| <= delta
C
C     Method described in "Computing a trust region step", by More and
C     Sorensen.
C
C     The main ideia of this method is to find a positive scalar \mslamb
C     that is a zero of the function
C
C     phi(\mslamb) = 1/||p|| - 1/delta,
C
C     where p is the solution of the linear system
C
C     (B + \mslamb I)p = -g.
C
C     Note that symmetric matrix B, vector g and positive real number
C     delta are presented in the minimization problem above. I is the
C     identity matrix.
C
C     The method used to find the zero of that function is basically the
C     Newton method to find roots.
C
C     On Entry
C
C     n        integer
C              dimension
C
C     g        double precision g(n)
C              vector used to define the quadratic function
C
C     bnnz     integer
C              number of nonzero elements of B
C
C     blin     integer blin(bnnz)
C              row indices of nonzero elements of B
C
C     bcol     integer bcol(bnnz)
C              column indices of nonzero elements of B
C
C     bval     double precision bval(bnnz)
C              nonzero elements of B, that is,
C              B(blin(i),bcol(i)) = bval(i). Since B is symmetric, just
C              one element B(i,j) or B(j,i) must appear in its sparse
C              representation. If more than one pair corresponding to
C              the same position of B appears in the sparse
C              representation, the multiple entries will be summed.
C              If any blin(i) or bcol(i) is out of range, the entry will
C              be ignored
C
C     bdiag    integer bdiag(n)
C              indices of diagonal elements of B in blin, bcol and bval
C
C     delta    double precision
C              trust-region radius
C
C     sigma1   double precision
C              allowed error for convergence criteria 1 and 2
C
C     sigma2   double precision
C              allowed error for convergence criteria 3 (hard case)
C
C     eps      double precision
C              allowed error
C
C     maxit    integer
C              maximum number of allowed iterations
C
C     l        double precision
C              initial value for mslamb
C
C     On Return
C
C     l        double precision
C              value that gives p as a solution to the minimization
C              problem, because p is also solution to
C              (B + l I)p = -g
C
C     pd       logical
C              set to true if the last Cholesky decomposition is
C              successfull
C
C     p        double precision p(n)
C              solution to problem
C              minimize     psi(w)
C              subjected to ||w|| <= delta
C
C     chcnt    integer
C              number of Cholesky decompositions
C
C     memfail  logical
C              true iff linear solver failed because of lack of memory
C
C     msinfo   integer
C              stores which convergence criteria was satisfied:
C
C              0 = both g and B are null;
C
C              1 = first convergence criterion is satisfied;
C
C              2 = second convergence criterion is satisfied;
C
C              3 = third convergence criterion is satisfied
C
C              5 = maximum allowed number of iterations is achieved.

C     ******************************************************************
C     ******************************************************************

C     scalcu:
C
C     Solve a sparse linear system of the form (A + lI + ekek^Td)u = 0,
C     where A + lI is a definite positive matrix in R^{k x k}.
C     u is a vector of k positions with u(k) = 1.
C
C     On Entry
C
C     n        integer
C              dimension of A
C
C     annz     integer
C              number of nonzero elements of A
C
C     alin     integer alin(annz)
C              row indices of nonzero elements of A
C
C     acol     integer acol(annz)
C              column indices of nonzero elements of A
C
C     aval     integer aval(annz)
C              nonzero elements of A, that is,
C              A(alin(i),acol(i)) = aval(i). Since A is symmetric, just
C              one element A(i,j) or A(j,i) must appear in its sparse
C              representation. If more than one pair corresponding to
C              the same position of A appears in the sparse
C              representation, the multiple entries will be summed.
C              If any alin(i) or acol(i) is out of range, the entry will
C              be ignored
C
C     adiag    integer adiag(n)
C              indices of diagonal elements of A in alin, acol and aval
C
C     l        double precision
C              used to compute A + lI
C
C     idx      integer
C              index k
C
C     On Return
C
C     u        double precision u(n)
C              system solution
C
C     ueucn2   double precision
C              u squared Euclidian-norm
C
C     memfail  logical
C              true iff linear solver failed because of lack of memory

C     ******************************************************************
C     ******************************************************************

C     scalcz:
C
C     Sparse implementation of the technique presented by Cline, Moler,
C     Stewart and Wilkinson to estimate the condition number of a matrix.
C     This technique is used by the More-Sorensen method to calculate an
C     approximation to the eigenvector associated to the smallest
C     eigenvalue of a matrix B (\mslamb_1).
C     In this technique, when \mslamb approaches -\mslamb_1, \|Rz\|
C     approaches 0. This insures that z is an approximation to the
C     wanted eigenvector.
C     Basically, it solves R^Ty = e, choosing e(k) as 1 or -1 (whatever
C     gives maximum local growth of y). Then, it solves Rz = y.
C     Note that R is a sparse matrix given by P^T D^0.5 L^T P (obtained
C     applying subroutine MA27BD).
C
C     On Entry
C
C     n        integer
C              dimension of R
C
C     rowind   integer rowind(n)
C     rowval   double precision rowval(n)
C              working arrays
C
C     On Return
C
C     z        double precision z(n)
C              approximation of the eigenvector of B associated to
C              \mslamb_1


!     This version solves the Schroedinger equation for the
!     one-dimensional
!     harmonic oscillator using matrix diagonalization

!     declaration of constants used in the calculations
!     define variables, where we have chosen to use atomic units for
!     m=c=hbar=k=1.

!    This module contains all constants and declarations
!    of variables read in by the function read_data. These
!    variables are used by many functions.

MODULE constants
  INTEGER,  PARAMETER :: dp = KIND(1.0D0)
  INTEGER, PARAMETER :: dpc = KIND((1.0D0,1.0D0))
END MODULE constants

MODULE parameters
  USE constants
  !  the step
  REAL(DP), PUBLIC :: step
  !  r min
  REAL(DP), PUBLIC, PARAMETER :: rmin=-10.0_dp
  !  r max
  REAL(DP), PUBLIC, PARAMETER :: rmax=10.0_dp
  !  number of steps from rmin to rmin to rmax
  INTEGER, PUBLIC, PARAMETER ::  max_step=1000
  !  size of matrix to be diagonalized, do not need points at rmin and rmax
  INTEGER, PUBLIC, PARAMETER ::  n=998
END MODULE parameters

!  main program starts here

PROGRAM schroedinger_equation
  USE constants
  USE parameters
!  USE f90library
  IMPLICIT NONE
  REAL(DP) :: potential, const1, const2
  REAL(DP), DIMENSION(:), ALLOCATABLE :: w, r
  REAL(DP), ALLOCATABLE, DIMENSION(:) :: e, d
  REAL(DP), ALLOCATABLE, DIMENSION(:,:) :: z
  INTEGER :: i

  !  reserve place in memory for the arrays w, r, e, d and z

  ALLOCATE ( e(n) , d(n))
  ALLOCATE ( w(0:max_step), r(0:max_step))

  !  define the step size

  step=(rmax-rmin)/FLOAT(max_step)

  !  define constants for the matrix to be diagonalized

  const1=2./(step*step)
  const2=-1./(step*step)

  !     set up r and the function w for energy =0
  !     w corresponds then to the potential
  !  values at

  DO i=0, max_step
     r(i) = rmin+i*step
     w(i) = potential(r(i))
  ENDDO

  !     setup the diagonal d and the non-diagonal part e  of
  !     the  tri-diagonal matrix to be diagonalized

  d(1:n)=const1+w(1:n)  ;  e(1:n)=const2

  !  allocate space for eigenvector info

  ALLOCATE ( z(n,n) )

  !  obtain the eigenvalues

  CALL tqli(d,e,n,z)

  !  sort eigenvalues as an ascending series

  CALL eigenvalue_sort(d,n)

  !  write out

  WRITE(6,*) ' Rmin and Rmax ', rmin, rmax
  WRITE(6,*) ' Number of steps -1 ', n
  WRITE(6,*) ' The 5 lowest eigenvalues '
  WRITE(6,'(5F12.6)') d(1:5)

  !  find the wave function for the lowest lying state

  CALL plot(d(1), w, r)

  !  deallocate space in memory

  DEALLOCATE (z)
  DEALLOCATE ( w, r, d, e)

END PROGRAM schroedinger_equation

!  Harmonic oscillator potential

REAL(DP) FUNCTION potential(x)
  USE constants
  IMPLICIT NONE
  REAL(DP), INTENT(IN) :: x

  potential=x*x

END FUNCTION  potential

!
!           Sort the eigenvalues as
!           ascending series
!

SUBROUTINE eigenvalue_sort(e,n)
  USE constants
  IMPLICIT NONE
  INTEGER, INTENT(IN) :: n
  INTEGER :: i, j
  REAL(DP), DIMENSION(n), INTENT(INOUT) :: e
  REAL(DP) :: temp

  DO i = 1, n
     DO j =i, n
        IF( ABS(e(i)) >   ABS(e(j)) ) THEN
           ! change the energy
           temp=e(i)
           e(i)=e(j)
           e(j)=temp
        ENDIF
     ENDDO
  ENDDO

END SUBROUTINE eigenvalue_sort

!  This function plots the wave function at the final energy

SUBROUTINE plot(energy, w ,r )
  USE constants
  USE parameters
  IMPLICIT NONE
  INTEGER :: i, match
  REAL(DP), DIMENSION(n) :: w, r
  REAL(DP), DIMENSION(:), ALLOCATABLE :: wf
  REAL(DP) :: energy, wwf, fac, norm, pi

  ALLOCATE ( wf(0:max_step))
  pi=ACOS(-1.) ; pi=SQRT(pi)

  !  add the chosen energy to the potential

  w=(w-energy)*step*step+2

  !  Boundary conditions
  !  value of the wave function at max step

  wf(max_step)=0.

  !  here we use the analytical form for the wave function for max step -1
  !  (taylor expansion of EXP(-r^2/2))

  wf(max_step-1)=step**2/2.

  !  start from right and move towards rmin

  OUTER_TO_INNER : DO
     DO i=max_step-2, 1, -1
        wf(i)=w(i+1)*wf(i+1)-wf(i+2)
        !  find point were the outer part starts turning in order to determine matching point
        IF ( wf(i) <=  wf(i+1) ) EXIT OUTER_TO_INNER
     ENDDO
  ENDDO OUTER_TO_INNER
  match = i+1
  wwf=wf(match)

  !  start from rmin  and move towards the matching point

  wf(0)=0.
  wf(1)=step**2/2.
  DO i=2, match
     wf(i)=w(i-1)*wf(i-1)-wf(i-2)
  ENDDO

  !  scale the wave function below the matching point

  fac=wwf/wf(match)
  wf(0:match)=wf(0:match)*fac

  !  normalize the wave function

  norm=step*SUM(wf*wf)
  IF ( norm > 0. ) norm = 1./SQRT(norm)
  wf=wf*norm
  WRITE(6,*) norm
  !  write out of numerically calculated and analytical expression for the wave functions

  WRITE(6,'(E12.6,2X,E12.6,2X,E12.6)')( r(i), wf(i), EXP(-(r(i)**2)/2.0_dp)/SQRT(pi),i=1, max_step)
  DEALLOCATE ( wf)

END SUBROUTINE plot

  REAL (DP) FUNCTION pythag(a,b)
    USE constants
    REAL(DP)  :: a,b
    REAL(DP)  :: absa,absb
    absa=ABS(a)
    absb=ABS(b)
    IF(absa > absb) THEN
       pythag=absa*sqrt(1.+(absb/absa)**2)
    ELSE
       IF(absb == 0.) THEN
          pythag=0.
       ELSE
          pythag=absb*sqrt(1.+(absa/absb)**2)
       ENDIF
    ENDIF

  END FUNCTION pythag

  !     determine eigenvalues and eigenvectors of a real symmetric
  !     tri-diagonal matrix, or a real, symmetric matrix previously
  !     reduced by function tred2 to tri-diagonal form. On input,
  !     d[] contains the diagonal element and e[] the sub-diagonal
  !     of the tri-diagonal matrix. On output d[] contains the
  !     eigenvalues and  e[] is destroyed. If eigenvectors are
  !     desired z[][] on input contains the identity matrix. If
  !     eigenvectors of a matrix reduced by tred2() are required,
  !     then z[][] on input is the matrix output from tred2().
  !     On output, the k'th column returns the normalized eigenvector
  !     corresponding to d[k].
  !     The function is modified from the version in Numerical recipe.

  SUBROUTINE tqli(d,e,n,z)
    USE constants
    IMPLICIT NONE
    INTEGER :: n
    REAL(DP)  :: d(n),e(n),z(n,n)
    INTEGER :: i,iter,k,l,m
    REAL(DP)  :: b,c,dd,f,g,p,r,s,pythag,one
    DO i=2,n
       e(i-1)=e(i)
    ENDDO
    one=1.
    DO l=1,n
       iter=0
       ITERATE : DO
          DO m=l,n-1
             dd=ABS(d(m))+ABS(d(m+1))
             IF (ABS(e(m))+dd == dd) EXIT
          ENDDO
          IF(m == l) EXIT ITERATE
          IF(iter == 30) STOP 'too many iterations in tqli'
          iter=iter+1
          g=(d(l+1)-d(l))/(2.*e(l))
          r=pythag(g,one)
          g=d(m)-d(l)+e(l)/(g+sign(r,g))
          s=1.
          c=1.
          p=0.
          DO i=m-1,l,-1
             f=s*e(i)
             b=c*e(i)
             r=pythag(f,g)
             e(i+1)=r
             IF(r == 0.) THEN
                d(i+1)=d(i+1)-p
                e(m)=0.
                CYCLE ITERATE
             ENDIF
             s=f/r
             c=g/r
             g=d(i+1)-p
             r=(d(i)-g)*s+2.*c*b
             p=s*r
             d(i+1)=g+p
             g=c*r-b
             !     Omit lines from here ...
             DO k=1,n
                f=z(k,i+1)
                z(k,i+1)=s*z(k,i)+c*f
                z(k,i)=c*z(k,i)-s*f
             ENDDO
             !     ... to here when finding only eigenvalues.
          ENDDO
          d(l)=d(l)-p
          e(l)=g
          e(m)=0.
       ENDDO ITERATE
    ENDDO

  END SUBROUTINE tqli