! toolkit.f90 -- generic numerical routines
! (root finding, interpolation, optimization).

MODULE toolkit

!!!!!!!!!!!!!!!!!!!!!!!!
!!   USE STATEMENTS   !!
!!!!!!!!!!!!!!!!!!!!!!!! 

USE PARAMETERS
USE GLOBAL

IMPLICIT NONE 

CONTAINS

!!!!!!!!!!!!!!!!!!!!!!!!
!!   	 UTILITY      !!
!!!!!!!!!!!!!!!!!!!!!!!! 


SUBROUTINE utility(c,cfhfrac,hours,utils)
REAL (DP), INTENT (IN)  :: c, cfhfrac, hours
REAL (DP), INTENT (OUT) :: utils
    
    IF (gam>zerodp) THEN
        utils = ( (c*cfhfrac)**gam*(c*(onedp-cfhfrac))**(onedp-gam) )   &
            **(onedp-sigma)/(onedp-sigma)                               &
            - phi*(hours**(onedp+nu))/(onedp+nu) + ubar
    ELSE
        utils = c                                       &
            **(onedp-sigma)/(onedp-sigma)               &
            - phi*(hours**(onedp+nu))/(onedp+nu) + ubar
    ENDIF
      
END SUBROUTINE utility

SUBROUTINE utility_sick(c,cfhfrac,hours,utils)
REAL (DP), INTENT (IN)  :: c, cfhfrac, hours
REAL (DP), INTENT (OUT) :: utils
    
    IF (sick_gam>zerodp) THEN
        utils =	( (c*cfhfrac)**sick_gam*(c*(onedp-cfhfrac))**(onedp-sick_gam) ) &
            **(onedp-sigma)/(onedp-sigma)                                       &
            - sick_phi*(hours**(onedp+nu))/(onedp+nu) + ubar + utilsick
    ELSE
        utils = c                                                               &
            **(onedp-sigma)/(onedp-sigma)                                       &
            - sick_phi*(hours**(onedp+nu))/(onedp+nu) + ubar + utilsick
    ENDIF
    
END SUBROUTINE utility_sick

SUBROUTINE utility_ret(c,cfhfrac,utils)
REAL (DP), INTENT (IN)  :: c, cfhfrac
REAL (DP), INTENT (OUT) :: utils
    
    IF (gam>zerodp) THEN
        utils = ( (c*cfhfrac)**gam*(c*(onedp-cfhfrac))**(onedp-gam) )   &
            **(onedp-sigma)/(onedp-sigma) + ubar
    ELSE
        utils = c**(onedp-sigma)/(onedp-sigma) + ubar
    ENDIF
      
END SUBROUTINE utility_ret

SUBROUTINE utility_sick_ret(c,cfhfrac,utils)
REAL (DP), INTENT (IN)  :: c, cfhfrac 
REAL (DP), INTENT (OUT) :: utils
    
    IF (sick_gam>zerodp) THEN
        utils =	( (c*cfhfrac)**sick_gam*(c*(onedp-cfhfrac))**(onedp-sick_gam) ) &
            **(onedp-sigma)/(onedp-sigma) + ubar + utilsick
    ELSE
        utils = c**(onedp-sigma)/(onedp-sigma) + ubar + utilsick
    ENDIF
    
END SUBROUTINE utility_sick_ret


!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!!   	 OTHER FUNCTIONS      !!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

SUBROUTINE rouwenhorst
    integer     :: i, j
    real (8)    :: p, q

    z_grid(N_z)  = ((N_z-1)/(onedp-rho_z**2))**(0.5)*sigma_eps
    z_grid  = linspace(-z_grid(N_z),z_grid(N_z),N_z)
    p = (onedp+rho_z)/2.0_dp
    q = p
    
    ptran = rh(p,q,N_z)

END SUBROUTINE

RECURSIVE FUNCTION rh(p,q,h) RESULT(rh_out)

    INTEGER, INTENT(IN)             ::  h
    REAL(DP), INTENT(IN)            ::  p, q
    REAL(DP), DIMENSION(h-1,h-1)    ::  P1
    REAL(DP), DIMENSION(h,h)        ::  rh_out, rh1, rh2, rh3, rh4
    
    IF (h.EQ.2) THEN
        rh_out(1,1:2) = (/ p , onedp-p /)
        rh_out(2,1:2) = (/ onedp-q , q /)
    ELSE
        P1 = rh(p,q,h-1)
        rh1 = zerodp
        rh2 = zerodp
        rh3 = zerodp
        rh4 = zerodp
        
        rh1(1:h-1,1:h-1) = P1
        rh2(1:h-1,2:h) = P1
        rh3(2:h,1:h-1) = P1
        rh4(2:h,2:h) = P1
        
        rh_out = p*rh1 + (onedp-p)*rh2 + (onedp-q)*rh3 + q*rh4
        rh_out(2:h-1,:) = rh_out(2:h-1,:)*0.5_dp
    END IF


END FUNCTION rh

DOUBLE PRECISION FUNCTION zbrent(func,x1,x2,tol)
    IMPLICIT NONE
    REAL*8 :: x1,x2,tol,zip,fa,fb,a,b,c,fc,d,e,tol1,xm,s,p,q,r
    INTEGER :: iter
    REAL*8, PARAMETER :: eps = 3.0D-08
    INTEGER, PARAMETER :: itmax = 200 
    INTERFACE
        DOUBLE PRECISION FUNCTION func(x)
        IMPLICIT NONE
        REAL*8, INTENT(IN) :: x
        END FUNCTION func
    END INTERFACE

    a = x1
    b = x2
    fa = func(a)
    fb = func(b)
    zip = 0.0D+00

    IF ((fa > zip .and. fb > zip) .or. (fa < zip .and. fb < zip)) THEN
    print*, 'root must be bracketed'
    print*, a,fa
    print*, b,fb
    PRINT *, CHAR(7)
    PAUSE
    ENDIF

    c = b
    fc = fb
    do iter = 1,itmax
        if ((fb > zip .and. fc > zip) .or. (fb < zip .and. fc < zip)) then
        c = a
        fc = fa
        d = b-a
        e = d
        endif
        if (dabs(fc) < dabs(fb)) then
        a = b
        b = c
        c = a
        fa = fb
        fb = fc
        fc = fa
        endif
        tol1 = 2.0D+00*eps*dabs(b)+0.5D+00*tol
        xm = 0.5D+00*(c-b)
        if (dabs(xm) <= tol1 .or. fb == zip) then
        zbrent = b
        return
        endif
        if (dabs(e) >= tol1 .and. dabs(fa) > dabs(fb)) then
        s = fb/fa
        if (a == c) then
            p = 2.0D+00*xm*s
            q = 1.0D+00-s
        else
            q = fa/fc
            r = fb/fc
            p = s*(2.0D+00*xm*q*(q-r)-(b-a)*(r-1.0D+00))
            q = (q-1.0D+00)*(r-1.0D+00)*(s-1.0D+00)
        endif
        if (p > zip) q = -q
        p = dabs(p)
        if (2.0D+00*p < dmin1(3.0D+00*xm*q-dabs(tol1*q),dabs(e*q))) then
            e = d
            d = p/q
        else
            d = xm
            e = d
        endif
        else
        d = xm
        e = d
        endif
        a = b
        fa = fb
        if (dabs(d) > tol1) then
        b = b+d
        else
        b = b+sign(tol1,xm)
        endif
        fb = func(b)
    END DO

    PAUSE 'zbrent exceeding maximum iterations'
    zbrent = b

    RETURN
END

      
!SUBROUTINE tauchen(rho_tau,sigma_tau,mtau_tau1,mtau_tau2,grid_tau,ptran_tau,N_tau)
!    REAL(DP), INTENT(IN)                            ::  rho_tau, sigma_tau, mtau_tau1, mtau_tau2
!    INTEGER, INTENT(IN)                             ::  N_tau
!    REAL(DP), DIMENSION(N_tau), INTENT(OUT)         ::  grid_tau
!    REAL(DP), DIMENSION(N_tau,N_tau), INTENT(OUT)   ::  ptran_tau    
!    INTEGER                                         ::  i, j, N_mid
!    REAL (DP)                                       ::  d1, d2, norm1, norm2
!    
!    grid_tau(1)     = -mtau_tau1*sigma_tau/(onedp-rho_tau**2)**(0.5)
!    grid_tau(N_tau) = mtau_tau2*sigma_tau/(onedp-rho_tau**2)**(0.5)
!    N_mid           = NINT(DBLE(mtau_tau1)/DBLE(mtau_tau1+mtau_tau2)*DBLE(N_tau))
!    d1              = (-grid_tau(1))/(N_mid-1)
!    d2              = (grid_tau(N_tau))/(N_tau-N_mid)
!    
!    DO i=2,N_mid
!        grid_tau(i) = grid_tau(i-1) + d1
!    END DO
!    DO i=N_mid+1,N_tau-1
!        grid_tau(i) = grid_tau(i-1) + d2
!    END DO
!    grid_tau(N_mid) = zerodp
!
!    DO i=1,N_tau        
!        CALL VDCDFNORM(1,(grid_tau(1)+d1/2-rho_tau*grid_tau(i))/sigma_tau,norm1)
!        ptran_tau(i,1) = norm1
!        
!        DO j=2,N_mid-1
!            CALL VDCDFNORM(1,(grid_tau(j)+d1/2-rho_tau*grid_tau(i))/sigma_tau,norm1)
!            CALL VDCDFNORM(1,(grid_tau(j)-d1/2-rho_tau*grid_tau(i))/sigma_tau,norm2)
!            ptran_tau(i,j) = norm1-norm2
!        END DO
!        
!        CALL VDCDFNORM(1,(grid_tau(j)+d2/2-rho_tau*grid_tau(i))/sigma_tau,norm1)
!        CALL VDCDFNORM(1,(grid_tau(j)-d1/2-rho_tau*grid_tau(i))/sigma_tau,norm2)
!        ptran_tau(i,N_mid) = norm1-norm2
!        
!        DO j=N_mid+1,N_tau-1
!            CALL VDCDFNORM(1,(grid_tau(j)+d2/2-rho_tau*grid_tau(i))/sigma_tau,norm1)
!            CALL VDCDFNORM(1,(grid_tau(j)-d2/2-rho_tau*grid_tau(i))/sigma_tau,norm2)
!            ptran_tau(i,j) = norm1-norm2
!        END DO
!        
!        CALL VDCDFNORM(1,(grid_tau(N_z)-d2/2-rho_tau*grid_tau(i))/sigma_tau,norm2)
!        ptran_tau(i,N_tau) = onedp-norm2
!    END DO
!END SUBROUTINE
!
!SUBROUTINE tauchen_iid(sigma_iid,mtau_iid1,mtau_iid2,grid,pr_iid,N_iid,N_mid)
!
!    REAL(DP), INTENT(IN)                    ::  sigma_iid, mtau_iid1, mtau_iid2
!    INTEGER, INTENT(IN)                     ::  N_iid
!    INTEGER, INTENT(IN), OPTIONAL           ::  N_mid
!    
!    REAL(DP), DIMENSION(N_iid), INTENT(OUT) ::  grid, pr_iid
!    INTEGER                                 ::  i, j, N_m
!    REAL (DP)                               ::  d1, d2, norm1, norm2
!    
!    IF (PRESENT(N_mid)) THEN
!        N_m = N_mid
!    ELSE 
!        N_m = NINT(DBLE(mtau_iid1)/DBLE(mtau_iid1+mtau_iid2)*DBLE(N_iid))
!    END IF
!    
!    grid(1)         = -mtau_iid1*sigma_iid
!    grid(N_iid)     = mtau_iid2*sigma_iid
!    d1              = (-grid(1))/(N_m-1)
!    d2              = (grid(N_iid))/(N_iid-N_m)
!
!    DO i=2,N_m
!        grid(i) = grid(i-1) + d1
!    END DO
!    DO i=N_m+1,N_iid-1
!        grid(i) = grid(i-1) + d2
!    END DO
!    grid(N_m) = zerodp
!    
!    CALL VDCDFNORM(1,(grid(1)+d1/2)/sigma_iid,norm1)
!    pr_iid(1) = norm1
!        
!    DO j=2,N_m-1
!        CALL VDCDFNORM(1,(grid(j)+d1/2)/sigma_iid,norm1)
!        CALL VDCDFNORM(1,(grid(j)-d1/2)/sigma_iid,norm2)
!        pr_iid(j) = norm1-norm2
!    END DO
!        
!    CALL VDCDFNORM(1,(grid(j)+d2/2)/sigma_iid,norm1)
!    CALL VDCDFNORM(1,(grid(j)-d1/2)/sigma_iid,norm2)
!    pr_iid(N_m) = norm1-norm2
!        
!    DO j=N_m+1,N_iid-1
!        CALL VDCDFNORM(1,(grid(j)+d2/2)/sigma_iid,norm1)
!        CALL VDCDFNORM(1,(grid(j)-d2/2)/sigma_iid,norm2)
!        pr_iid(j) = norm1-norm2
!    END DO
!        
!    CALL VDCDFNORM(1,(grid(N_iid)-d2/2)/sigma_iid,norm2)
!    pr_iid(N_iid) = onedp-norm2
!
!
!END SUBROUTINE

SUBROUTINE creategrid(grid, siz, n_med, grid_min, grid_pivot, grid_max, curvature)
	!! This SUBROUTINE fills the array grid 
	!! with siz points
	!! from grid_min to grid_max
	!! and a curvature around grid_pivot
	
	! INPUT & OUTPUT
    INTEGER, INTENT(IN) 					:: siz, n_med
	REAL(DP), INTENT(IN) 					:: grid_min, grid_max, grid_pivot
	REAL(DP), INTENT(IN) 					:: curvature
    
	REAL(DP), DIMENSION(SIZ), INTENT(INOUT) 	:: grid
	
	! LOCAL VARIABLES
	INTEGER                                 :: i
     
	! PROGRAM
    
    
    IF_SINGLE: IF (siz .EQ. 1) THEN        
        ! if size of the grid is one
        ! then set to lower bound
        grid        =   grid_min
    ELSE

        IF (n_med .GT. 1) THEN
            FORALL (i=1:n_med) grid(i) = DBLE(i-1)/DBLE(n_med-1)  	! [0,1] grid evenly split
            grid(1:n_med)	= 1.0_DP - grid(1:n_med)                ! [1,0] grid evenly split
            grid(1:n_med)	= 1.0_DP - grid(1:n_med)**curvature    	! [0,1] grid clustered at 1
            FORALL (i=1:n_med) grid(i) = grid_min + & 
            (grid_pivot-grid_min)*grid(i)   ! grid_min to grid_pivot, clustered
        END IF

        IF (n_med .LT. siz) THEN
            FORALL (i=n_med:siz) grid(i) = DBLE(i-n_med)/DBLE(siz-n_med)   ! even grid between 0 and 1
            grid(n_med:siz)	= grid(n_med:siz)**curvature            ! [0,1] grid clustered at 0
            FORALL (i=n_med:siz) grid(i) = grid_pivot + & 
            (grid_max-grid_pivot)*grid(i)   ! grid_min to grid_pivot, clustered
        END IF

    END IF IF_SINGLE
														
END SUBROUTINE creategrid

FUNCTION prodvec(x,N)
	!! product of elements in vector of dimension N
	
	! INPUT & OUTPUT
    REAL(DP), DIMENSION(N)  :: x
    INTEGER, INTENT (IN)    :: N
    INTEGER                 :: i
    REAL(DP)                :: prodvec
    
    prodvec = 1.0_DP
	! PROGRAM
    DO i = 1,N
        prodvec = prodvec*x(i)
    END DO
        
END FUNCTION prodvec

FUNCTION linspace(a,b,N)
	!! linspace

	! INPUT & OUTPUT
	INTEGER, INTENT(IN)         :: N
	REAL(DP), INTENT(IN)        :: a,b
    
    ! OUTPUT
    REAL(DP), DIMENSION(N)      :: linspace
    
    ! LOCAL
	REAL(DP)                    :: delta
	INTEGER                     :: i

	! PROGRAM
	delta           = (b-a)/DBLE(N-1)
	linspace        = a + delta*(/(DBLE(i),i=0,N-1)/)
	
END FUNCTION linspace

FUNCTION trapz(X,Y,siz)
	!! computes the integration of Y with respect to X using trapezoidal integration. 
	!! X and Y must be one-dimensional arrays with the same dimension siz

	! INPUT & OUTPUT
	INTEGER, INTENT(IN) 				    :: siz
	REAL(DP), DIMENSION(siz), INTENT(IN)	:: X, Y
    
    ! LOCAL VARIABLES
	REAL(DP)                				:: ans, trapz
	INTEGER                            	    :: i

	! PROGRAM
	ans = 0.0_DP
	DO i = 1, siz-1
		ans = ans + 0.5_DP*( Y(i+1) + Y(i) )*( X(i+1) - X(i) )
    END DO
	trapz = ans
	
END FUNCTION trapz

INTEGER FUNCTION multpl(i,m)
IMPLICIT NONE
INTEGER, INTENT(IN) :: i,m
REAL*8, PARAMETER :: toler = 1.0D-12
REAL*8 :: remain

remain = DBLE(REAL(i))/DBLE(REAL(m))-i/m

IF (remain <= toler) then
    multpl = 1
ELSE
    multpl = 0
ENDIF

RETURN
END
      
SUBROUTINE hunt(xx,n,x,jlo)
      IMPLICIT NONE
      INTEGER, INTENT(IN) :: n
      real*8, intent(in), dimension(n) :: xx
      logical :: ascnd
      real*8,  intent(in) :: x
      integer, intent(inout) :: jlo
      integer :: jhi,inc,jm

      ascnd = xx(n) >= xx(1)
      if (jlo <= 0 .or. jlo > n) then
         jlo = 0
         jhi = n+1
         goto 3
      endif

      inc = 1
      
      if (x >= xx(jlo) .eqv. ascnd) then
 1       jhi = jlo+inc
         if (jhi > n) then
            jhi = n+1
         elseif (x >= xx(jhi) .eqv. ascnd) then
            jlo = jhi
            inc = inc+inc
            goto 1
         endif
      else
         jhi = jlo
 2       jlo = jhi-inc
         if (jlo < 1) then
            jlo = 0
         elseif (x < xx(jlo) .eqv. ascnd) then
            jhi = jlo
            inc = inc+inc
            goto 2
         endif
      endif
 3    if (jhi-jlo == 1) then
         if (x == xx(n)) jlo = n-1
         if (x == xx(1)) jlo = 1
         return
      endif
      jm = (jhi+jlo)/2
      if (x >= xx(jm) .eqv. ascnd) then
         jlo = jm
      else
         jhi = jm
      ENDIF
      GOTO 3

      END

FUNCTION interp1(X,Y,siz,xi)
	!! Interpolates to find the values of the underlying function Y at the points in the vector or array xi.
	!! X and Y must be one-dimensional arrays with the same dimension siz
	!! xi must be a scalar
	!! if xi exceeds the bounds of X, i.e. requiring extrapolation, then Yi is set to the closest point
	
	! INPUT & OUTPUT
    INTEGER, INTENT(IN) 					:: siz
    REAL(DP), DIMENSION(siz), INTENT(IN)	:: X, Y
    REAL(DP), INTENT(IN)	                :: xi
    REAL(DP)                                :: interp1
    INTEGER								    :: ind
    
	! PROGRAM
	ind = MINLOC( ABS( xi - X ) , 1 )
	outer: IF ( xi >= X(ind) ) THEN
		inner1: IF  ( ind == siz ) THEN
			interp1 = Y(ind)
		ELSE
			interp1 =	&
				( Y(ind+1)*(xi-X(ind)) + Y(ind)*(X(ind+1)-xi) )	&
				/( X(ind+1) - X(ind) )
		END IF inner1
    ELSE ! IF ( xi < X(ind) )
		inner2: IF  ( ind == 1 ) THEN
			interp1 = Y(ind)
		ELSE
			interp1 =	&
				( Y(ind)*(xi-X(ind-1)) + Y(ind-1)*(X(ind)-xi) )	&
				/( X(ind) - X(ind-1) )
		END IF inner2
    END IF outer
	
END FUNCTION interp1

FUNCTION interp1vec(X,Y,siz,xi,sizxi)
	!! Interpolates to find the values of the underlying function Y at the points in the vector or array xi.
	!! X and Y must be one-dimensional arrays with the same dimension siz
	!! xi must be a one dimensional array (scalar) with dimension sizxi (1)
	!! if xi exceeds the bounds of X, i.e. requiring extrapolation, then Yi is set to the closest point
	
	! INPUT & OUTPUT
    INTEGER, INTENT(IN) 					:: siz, sizxi
    REAL(DP), DIMENSION(siz), INTENT(IN)	:: X, Y
    REAL(DP), DIMENSION(sizxi), INTENT(IN)	:: xi
    REAL(DP), DIMENSION(sizxi)				:: interp1vec
    INTEGER								    :: ind, i
    
	! PROGRAM
    DO i = 1, sizxi
		ind = MINLOC( ABS( xi(i) - X ) , 1 )
		outer: IF ( xi(i) >= X(ind) ) THEN
			inner1: IF  ( ind == siz ) THEN
				interp1vec(i) = Y(ind)
		    ELSE
				interp1vec(i) =	&
				  ( Y(ind+1)*(xi(i)-X(ind)) + Y(ind)*(X(ind+1)-xi(i)) )	&
				  /( X(ind+1) - X(ind) )
		    END IF inner1
        ELSE ! IF ( xi(i) < X(ind) )
			inner2: IF  ( ind == 1 ) THEN
				interp1vec(i) = Y(ind)
		    ELSE
				interp1vec(i) =	&
				  ( Y(ind)*(xi(i)-X(ind-1)) + Y(ind-1)*(X(ind)-xi(i)) )	&
				  /( X(ind) - X(ind-1) )
		    END IF inner2
        END IF outer
    END DO
	
END FUNCTION interp1vec

FUNCTION interp2(X1,X2,Y,siz1,siz2,xi1,xi2)
	!! Interpolates to find the values of the underlying function Y at the point (xi1,xi2).
	!! X1 (X2) must be one-dimensional arrays with dimension siz1 (siz2)
    !! Y must be a two-dimensional array of dimension siz1 x siz2
	!! xi1, xi2 must be scalars
	!! if xi exceeds the bounds of X, i.e. requiring extrapolation, then Yi is set to the closest point
	
	! INPUT & OUTPUT
    INTEGER, INTENT(IN) 					    :: siz1, siz2
    REAL(DP), DIMENSION(siz1), INTENT(IN)	    :: X1
    REAL(DP), DIMENSION(siz2), INTENT(IN)	    :: X2
    REAL(DP), DIMENSION(siz1,siz2), INTENT(IN)	:: Y
    REAL(DP), INTENT(IN)	                    :: xi1 ,xi2
    REAL(DP)                                    :: Yind, Yindp, Yindm
    REAL(DP)                                    :: interp2
    INTEGER								        :: ind
    
    ! PROGRAM
	ind = MINLOC( ABS( xi1 - X1 ) , 1 )
    outer: IF ( xi1 >= X1(ind) ) THEN
		inner1: IF  ( ind == siz1 ) THEN
			interp2 = INTERP1(X2,Y(ind,:),siz2,xi2)
        ELSE
            Yind = INTERP1(X2,Y(ind,:),siz2,xi2)
            Yindp = INTERP1(X2,Y(ind+1,:),siz2,xi2)
            interp2 = &
                ( Yindp*(xi1-X1(ind)) + Yind*(X1(ind+1)-xi1) )	&
				/( X1(ind+1) - X1(ind) )
		END IF inner1
    ELSE ! IF ( xi1 < X1(ind) )
		inner2: IF  ( ind == 1 ) THEN
			interp2 = INTERP1(X2,Y(ind,:),siz2,xi2)
        ELSE
            Yind = INTERP1(X2,Y(ind,:),siz2,xi2)
            Yindm = INTERP1(X2,Y(ind-1,:),siz2,xi2)
			interp2 =	&
				( Yind*(xi1-X1(ind-1)) + Yindm*(X1(ind)-xi1) )	&
				/( X1(ind) - X1(ind-1) )
		END IF inner2
    END IF outer
	
END FUNCTION interp2

FUNCTION interp2vec(X1,X2,Y,siz1,siz2,xi1,xi2,sizxi2)
	!! Interpolates to find the values of the underlying function Y at the point (xi1,xi2).
	!! X1 (X2) must be one-dimensional arrays with the same dimension siz1 (siz2)
	!! x1 must be a scalar
	!! x2 must be a one dimensional array (scalar) with dimension sizxi2
	!! if xi exceeds the bounds of X, i.e. requiring extrapolation, then Yi is set to the closest point
	
	! INPUT & OUTPUT
    INTEGER, INTENT(IN) 					    :: siz1, siz2, sizxi2
    REAL(DP), DIMENSION(siz1), INTENT(IN)	    :: X1
    REAL(DP), DIMENSION(siz2), INTENT(IN)	    :: X2
    REAL(DP), DIMENSION(siz1,siz2), INTENT(IN)	:: Y
    REAL(DP), INTENT(IN)	                    :: xi1
    REAL(DP), DIMENSION(sizxi2), INTENT(IN)	    :: xi2
    REAL(DP), DIMENSION(sizxi2)         	    :: Yind, Yindp, Yindm
    REAL(DP), DIMENSION(sizxi2)                 :: interp2vec
    INTEGER								        :: ind
    
    ! PROGRAM
	ind = MINLOC( ABS( xi1 - X1 ) , 1 )
    outer: IF ( xi1 >= X1(ind) ) THEN
		inner1: IF  ( ind == siz1 ) THEN
			interp2vec = INTERP1VEC(X2,Y(ind,:),siz2,xi2,sizxi2)
        ELSE
            Yind = INTERP1VEC(X2,Y(ind,:),siz2,xi2,sizxi2)
            Yindp = INTERP1VEC(X2,Y(ind+1,:),siz2,xi2,sizxi2)
            interp2vec = &
                ( Yindp*(xi1-X1(ind)) + Yind*(X1(ind+1)-xi1) )	&
				/( X1(ind+1) - X1(ind) )
		END IF inner1
    ELSE ! IF ( xi1 < X1(ind) )
		inner2: IF  ( ind == 1 ) THEN
			interp2vec = INTERP1VEC(X2,Y(ind,:),siz2,xi2,sizxi2)
        ELSE
            Yind = INTERP1VEC(X2,Y(ind,:),siz2,xi2,sizxi2)
            Yindm = INTERP1VEC(X2,Y(ind-1,:),siz2,xi2,sizxi2)
			interp2vec =	&
				( Yind*(xi1-X1(ind-1)) + Yindm*(X1(ind)-xi1) )	&
				/( X1(ind) - X1(ind-1) )
		END IF inner2
    END IF outer
	
END FUNCTION interp2vec

FUNCTION interp2vec2(X1,X2,Y,siz1,siz2,xi1,xi2,sizxi1)
	!! Interpolates to find the values of the underlying function Y at the point (xi1,xi2).
	!! X1 (X2) must be one-dimensional arrays with the same dimension siz1 (siz2)
	!! x1 must be a one dimensional array (scalar) with dimension sizxi1
	!! x2 must be a scalar
	!! if xi exceeds the bounds of X, i.e. requiring extrapolation, then Yi is set to the closest point
	
	! INPUT & OUTPUT
    INTEGER, INTENT(IN) 					    :: siz1, siz2, sizxi1
    REAL(DP), DIMENSION(siz1), INTENT(IN)	    :: X1
    REAL(DP), DIMENSION(siz2), INTENT(IN)	    :: X2
    REAL(DP), DIMENSION(siz1,siz2), INTENT(IN)	:: Y
    REAL(DP), DIMENSION(sizxi1), INTENT(IN)	    :: xi1
    REAL(DP), INTENT(IN)	                    :: xi2
    REAL(DP), DIMENSION(sizxi1)         	    :: Yind, Yindp, Yindm
    REAL(DP), DIMENSION(sizxi1)                 :: interp2vec2
    INTEGER								        :: ind
    
    ! PROGRAM
	ind = MINLOC( ABS( xi2 - X2 ) , 1 )
    outer: IF ( xi2 >= X2(ind) ) THEN
		inner1: IF  ( ind == siz2 ) THEN
			interp2vec2 = INTERP1VEC(X1,Y(:,ind),siz1,xi1,sizxi1)
        ELSE
            Yind = INTERP1VEC(X1,Y(:,ind),siz1,xi1,sizxi1)
            Yindp = INTERP1VEC(X1,Y(:,ind+1),siz1,xi1,sizxi1)
            interp2vec2 = &
                ( Yindp*(xi2-X2(ind)) + Yind*(X2(ind+1)-xi2) )	&
				/( X2(ind+1) - X2(ind) )
		END IF inner1
    ELSE ! IF ( xi2 < X2(ind) )
		inner2: IF  ( ind == 1 ) THEN
			interp2vec2 = INTERP1VEC(X1,Y(:,ind),siz1,xi1,sizxi1)
        ELSE
            Yind = INTERP1VEC(X1,Y(:,ind),siz1,xi1,sizxi1)
            Yindm = INTERP1VEC(X1,Y(:,ind-1),siz1,xi1,sizxi1)
			interp2vec2 =	&
				( Yind*(xi2-X2(ind-1)) + Yindm*(X2(ind)-xi2) )	&
				/( X2(ind) - X2(ind-1) )
		END IF inner2
    END IF outer
	
END FUNCTION interp2vec2

FUNCTION interp3(X1,X2,X3,Y,siz1,siz2,siz3,xi1,xi2,xi3)
	!! Interpolates to find the values of the underlying function Y at the point (xi1,xi2,xi3).
	!! X1,X2,X3 must be one-dimensional arrays with dimension siz1,siz2,siz3
    !! Y must be a three-dimensional array of dimension siz1 x siz2 x siz3
	!! xi1, xi2, xi3 must be scalars
	!! if xi exceeds the bounds of X, i.e. requiring extrapolation, then Yi is set to the closest point
	
	! INPUT & OUTPUT
    INTEGER, INTENT(IN) 					        :: siz1, siz2, siz3
    REAL(DP), DIMENSION(siz1), INTENT(IN)	        :: X1
    REAL(DP), DIMENSION(siz2), INTENT(IN)	        :: X2
    REAL(DP), DIMENSION(siz3), INTENT(IN)	        :: X3
    REAL(DP), DIMENSION(siz1,siz2,siz3), INTENT(IN)	:: Y
    REAL(DP), INTENT(IN)	                        :: xi1 ,xi2, xi3
    REAL(DP)                                        :: Yind, Yindp, Yindm
    REAL(DP)                                        :: interp3
    INTEGER								            :: ind
    
    ! PROGRAM
	ind = MINLOC( ABS( xi1 - X1 ) , 1 )
    outer: IF ( xi1 >= X1(ind) ) THEN
		inner1: IF  ( ind == siz1 ) THEN
			interp3 = INTERP2(X2,X3,Y(ind,:,:),siz2,siz3,xi2,xi3)
        ELSE
            Yind = INTERP2(X2,X3,Y(ind,:,:),siz2,siz3,xi2,xi3)
            Yindp = INTERP2(X2,X3,Y(ind+1,:,:),siz2,siz3,xi2,xi3)
            interp3 = &
                ( Yindp*(xi1-X1(ind)) + Yind*(X1(ind+1)-xi1) )	&
				/( X1(ind+1) - X1(ind) )
		END IF inner1
    ELSE ! IF ( xi1 < X1(ind) )
		inner2: IF  ( ind == 1 ) THEN
			interp3 = INTERP2(X2,X3,Y(ind,:,:),siz2,siz3,xi2,xi3)
        ELSE
            Yind = INTERP2(X2,X3,Y(ind,:,:),siz2,siz3,xi2,xi3)
            Yindm = INTERP2(X2,X3,Y(ind-1,:,:),siz2,siz3,xi2,xi3)
			interp3 =	&
				( Yind*(xi1-X1(ind-1)) + Yindm*(X1(ind)-xi1) )	&
				/( X1(ind) - X1(ind-1) )
		END IF inner2
    END IF outer
	
END FUNCTION interp3

RECURSIVE PURE SUBROUTINE bisection(x_min,x_max,func,fzero,fval,errcode)
	!! This SUBROUTINE solves for the zero of the function func
	!! between x_min and x_max and the value at fval = func(fzero)
    !! An error code is stored in errcode (0=ok, 1=error)
	
	! INPUT & OUTPUT
	REAL(DP), INTENT(IN) 					:: x_min, x_max
	!REAL(DP), EXTERNAL                      :: func
    INTERFACE
        REAL(SELECTED_REAL_KIND(p=13)) PURE FUNCTION func(x)
            REAL(SELECTED_REAL_KIND(p=13)), INTENT(IN)            :: x
        END FUNCTION
    END INTERFACE
    
	REAL(DP), INTENT(OUT) 	                :: fzero
    REAL(DP), INTENT(OUT) 	                :: fval
    INTEGER, INTENT(OUT) 					:: errcode
	
	! LOCAL VARIABLES
	REAL(DP)                                :: f_low, f_high, x_low, x_high, x_mid, f_mid, x_diff 
    REAL(DP)                                :: fv, fz
    INTEGER                                 :: errc
     
	! PROGRAM
    
    errcode     = 0
    x_low       = x_min
    f_low       = func(x_low)
    x_high      = x_max
    f_high      = func(x_high)
    
    x_mid       = (x_min + x_max)/2.0_DP
    f_mid       = func(x_mid)
    
    x_diff      = x_low - x_high
    
    IF (abs(f_mid) < EPS) THEN
        errcode = 0
        fval    = f_mid
        fzero   = x_mid
        
    ELSE IF (abs(x_diff) < EPS) THEN
        errcode = 1
        fval    = f_mid
        fzero   = x_mid        
        
    ELSE IF (f_mid*f_low < ZERODP) THEN
        CALL bisection(x_low,x_mid,func,fz,fv,errc)
        fzero   = fz
        fval    = fv
        errcode = errc
        
    ELSE IF (f_mid*f_high < ZERODP) THEN
        CALL bisection(x_mid,x_high,func,fz,fv,errc)
        fzero   = fz
        fval    = fv
        errcode = errc
        
    ELSE IF (f_high*f_low > ZERODP) THEN
        errcode = 1
        fzero   = x_low
        fval    = f_low        
    END IF	
														
END SUBROUTINE bisection

FUNCTION AMOTRY(p,y,psum,mp,np,ndim,funk,ihi,fac)
    INTEGER   :: ihi,mp,ndim,np,NMAX
    REAL(DP)  :: AMOTRY,fac,p(mp,np),psum(np),y(mp),funk
    PARAMETER (NMAX=20)
    EXTERNAL funk
    INTEGER   :: j
    REAL(DP)  :: fac1,fac2,ytry,ptry(NMAX)
      
    fac1=(1.-fac)/ndim
    fac2=fac1-fac
    DO j=1,ndim
        ptry(j)=psum(j)*fac1-p(ihi,j)*fac2
    END DO
    ytry=funk(ptry)
    IF (ytry.lt.y(ihi)) THEN
        y(ihi)=ytry
        DO j=1,ndim
            psum(j)=psum(j)-p(ihi,j)+ptry(j)
            p(ihi,j)=ptry(j)
        END DO
    ENDIF
    AMOTRY=ytry
    RETURN
END FUNCTION AMOTRY
    
SUBROUTINE AMOEBA(p,y,mp,np,ndim,ftol,funk,iter,ITMAX)
    INTEGER     :: iter,mp,ndim,np,NMAX,ITMAX
    REAL(DP)    :: ftol,p(mp,np),y(mp),funk
    PARAMETER (NMAX=20)
    EXTERNAL funk
    INTEGER     :: i,ihi,ilo,inhi,j,m,n
    REAL(DP)    :: rtol,sum,swap,ysave,ytry,psum(NMAX)
    
    iter=0
1   DO n=1,ndim
        sum=0.
        DO m=1,ndim+1
            sum=sum+p(m,n)
        END DO
        psum(n)=sum
    END DO
2   ilo=1
    IF (y(1).gt.y(2)) THEN
        ihi=1
        inhi=2
    ELSE
        ihi=2
        inhi=1
    ENDIF
    DO i=1,ndim+1
        IF(y(i).le.y(ilo)) ilo=i
        IF(y(i).gt.y(ihi)) THEN
            inhi=ihi
            ihi=i
        ELSE IF(y(i).gt.y(inhi)) THEN
            IF(i.ne.ihi) inhi=i
        ENDIF
    END DO
    rtol=2.*abs(y(ihi)-y(ilo))/(abs(y(ihi))+abs(y(ilo)))
    IF (rtol.lt.ftol) THEN
        swap=y(1)
        y(1)=y(ilo)
        y(ilo)=swap
        DO n=1,ndim
            swap=p(1,n)
            p(1,n)=p(ilo,n)
            p(ilo,n)=swap
        END DO
        RETURN
    ENDIF
    IF (iter.ge.ITMAX) PAUSE 'ITMAX exceeded in amoeba'
    iter=iter+2
    ytry=AMOTRY(p,y,psum,mp,np,ndim,funk,ihi,-1.0_dp)
    IF (ytry.le.y(ilo)) THEN
        ytry=AMOTRY(p,y,psum,mp,np,ndim,funk,ihi,2.0_dp)
    ELSE IF (ytry.ge.y(inhi)) THEN
        ysave=y(ihi)
        ytry=AMOTRY(p,y,psum,mp,np,ndim,funk,ihi,0.5_dp)
        IF (ytry.ge.ysave) THEN
            DO i=1,ndim+1
                IF(i.ne.ilo) THEN
                    DO j=1,ndim
                        psum(j)=0.5_dp*(p(i,j)+p(ilo,j))
                        p(i,j)=psum(j)
                    END DO
                    y(i)=funk(psum)
                ENDIF
            END DO
            iter=iter+ndim
            GOTO 1
        ENDIF
    ELSE
        iter=iter-1
    ENDIF
    GOTO 2
END SUBROUTINE AMOEBA
      
!====================================================================!
FUNCTION ind2sub(iG,nSub) RESULT(iSub)
!! Compute the indices in each dimension from the global index
!====================================================================!
  INTEGER, INTENT(IN)	:: iG !! Index into a global vector
  INTEGER, INTENT(IN) 	:: nSub(:) !! Size in each dimension
  INTEGER 				:: iSub(SIZE(nSub)) !! Indices in each dimension to return
  INTEGER 				:: i,iGtmp,iTmp
  INTEGER 				:: nDims
  INTEGER 				:: prod

  nDims=SIZE(nSub)
  IF (nDims == 1) THEN
	iSub(1) = iG
	RETURN
  END IF

  prod = PRODUCT(nSub)
  iGtmp = iG
  DO i = nDims,1,-1
	prod = prod / nSub(i)
	iTmp = MOD(iGtmp-1,prod)+1
	iSub(i) = (iGtmp - iTmp)/prod + 1
	iGtmp = iTmp
  END DO

  END FUNCTION
  
END MODULE toolkit