      SUBROUTINE simpr(y,dydx,dfdx,dfdy,nmax,n,xs,htot,nstep,yout,
     *derivs)
      INTEGER n,nmax,nstep,NMAXX
      REAL htot,xs,dfdx(n),dfdy(nmax,nmax),dydx(n),y(n),yout(n)
      EXTERNAL derivs
      PARAMETER (NMAXX=50)
CU    USES derivs,lubksb,ludcmp
      INTEGER i,j,nn,indx(NMAXX)
      REAL d,h,x,a(NMAXX,NMAXX),del(NMAXX),ytemp(NMAXX)
      h=htot/nstep
      do 12 i=1,n
        do 11 j=1,n
          a(i,j)=-h*dfdy(i,j)
11      continue
        a(i,i)=a(i,i)+1.
12    continue
      call ludcmp(a,n,NMAXX,indx,d)
      do 13 i=1,n
        yout(i)=h*(dydx(i)+h*dfdx(i))
13    continue
      call lubksb(a,n,NMAXX,indx,yout)
      do 14 i=1,n
        del(i)=yout(i)
        ytemp(i)=y(i)+del(i)
14    continue
      x=xs+h
      call derivs(x,ytemp,yout)
      do 17 nn=2,nstep
        do 15 i=1,n
          yout(i)=h*yout(i)-del(i)
15      continue
        call lubksb(a,n,NMAXX,indx,yout)
        do 16 i=1,n
          del(i)=del(i)+2.*yout(i)
          ytemp(i)=ytemp(i)+del(i)
16      continue
        x=x+h
        call derivs(x,ytemp,yout)
17    continue
      do 18 i=1,n
        yout(i)=h*yout(i)-del(i)
18    continue
      call lubksb(a,n,NMAXX,indx,yout)
      do 19 i=1,n
        yout(i)=ytemp(i)+yout(i)
19    continue
      return
      END

NR-Functions/Numerical Recipes- Example & Functions/Functions/simpr.for


module dynsoil_steady_state
implicit none

contains

subroutine steady_state_surface_regolith(h,xs,taus, RP,E,FPdiss, temp,runoff,slope, dt, veget_factor,veget_eros_factor, &
                                         list_cont_pxl, ncontpxl)
use dynsoil_empirical_laws, only: erosion, reg_prod_opt, soil_prod_func, eq_reg_thick, dissolution_constant

use dynsoil_physical_parameters, only: nlon, nlat, nlitho, sigma, h0
integer, parameter:: npxl=nlon*nlat
!!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! INPUT-OUTPUT VARIABLES: !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
double precision, dimension(nlitho,npxl), intent(inout):: h, xs, taus                       !!
double precision, dimension(npxl),        intent(in):: temp, runoff, slope                  !!
double precision,                         intent(in):: dt                                   !!
!                                                                                !!
double precision, dimension(nlitho,npxl), intent(out):: RP, E, FPdiss                       !!
! vegetation:                                                                               !!
double precision, dimension(npxl),        intent(in):: veget_factor, veget_eros_factor      !!
! continental points:                                                                       !!
integer, dimension(npxl), intent(in):: list_cont_pxl                                        !!
integer, intent(in):: ncontpxl                                                              !!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! INTERNAL VARIABLES: !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
double precision:: RPopt, Kmain, h2                                                !!
integer:: k, i, j0, j                                                              !!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  ! Continental loop
  ! ----------------
  do j0 = 1,ncontpxl
    j = list_cont_pxl(j0)

    ! test for non-null runoff points. If runoff is null, nothing is done
    ! because all fluxes (erosive, chemical...) are null.
    ! * * * * * * * * * * !
    if (runoff(j)>0) then !
    ! * * * * * * * * * * !

      ! Lithology loop
      ! --------------
      do i = 1,nlitho-1 ! Skip last lithology class => carbonates

        !====================================================!
        !========= climate-only dependent variables =========!
        !====================================================!

        ! optimal regolith production rate:
        RPopt  = veget_factor(j) * reg_prod_opt(temp(j), runoff(j), i)
        ! regolith erosion rate:
        E(i,j) = veget_eros_factor(j) * erosion(temp(j), runoff(j), slope(j), i)
        ! dissolution constant:
        Kmain  = veget_factor(j) * dissolution_constant(temp(j), runoff(j), i)

        !===============================================================================================================!
        !============== Equilibrium-corrected explicit Runge-Kutta 4 integration for regolith thickness (h) ============!
        !===============================================================================================================!
        !                                                                                !
        !                     Solve the equation  ' dh/dt = RPopt * soil_prod_func(h) - E '                             !
        !                                                                                !
        !===============================================================================================================!

        ! EXPONENTIAL SPF:

        ! QUASI STEADY-STATE: let regolith thickness evolve dynamically:
        ! analytical solution with constant RPopt and Erosion
        !h2   =   h(i,j)   + &
        !         h0 * log(  exp(-1*(E(i,j)/h0)*dt)  +  (RPopt/E(i,j)) * ( 1 - exp(-1*(E(i,j)/h0)*dt) ) * exp(-1*h(i,j)/h0) )
        !if (h2<0) h2 = 0
        !RP(i,j)   =   E(i,j)  +  ( h2 - h(i,j) ) / dt
        !h(i,j) = h2

        ! STEADY-STATE regolith thickness:
        h(i,j)    =   h0 * log(RPopt/E(i,j))
        RP(i,j)   =   E(i,j)
        if (h(i,j) < 0) h(i,j) = 0

        !===============================================================================================================!
        !==============                                    WEATHERING                                    ===============!
        !===============================================================================================================!
        !                                                                                !
        !           Solve the equations  ' dz/dt = RP + dissrate*dz/dx ' & ' dtau/dt = 1 + dissrate*dtau/dx '           !
        !           AT STEADY-STATE (West 2012 weathering model)                                                        !
        !                                                                                !
        !===============================================================================================================!

        taus(i,j) = h(i,j)/E(i,j)
        xs(i,j) = exp( -1*Kmain * ((h(i,j)/E(i,j))**(sigma(i)+1)) / (sigma(i)+1) )

        FPdiss(i,j) = RP(i,j)*(1-xs(i,j))

    end do
    ! end of lithology loop

    ! * * !
    else  !
    ! * * !

      ! null runoff: set all fluxes to 0
      h(1:nlitho-1, j)       = 0
      taus(1:nlitho-1, j)    = 0
      xs(1:nlitho-1, j)      = 1
      RP(1:nlitho-1, j)      = 0
      E(1:nlitho-1, j)       = 0
      FPdiss(1:nlitho-1, j)  = 0

    ! * * * !
    end if  !
    ! * * * !

  end do
  ! end of continental loop

end subroutine

subroutine steady_state_inner_regolith(x, h,xs,temp,runoff,E, z,tau,MPtot,ktop, zfillvalue,taufillvalue, veget_factor, &
                                       missingpoints)
use dynsoil_empirical_laws, only: dissolution_constant

use dynsoil_physical_parameters, only: nlon, nlat, nlitho, nDSlev,sigma, epsl
integer, parameter:: npxl=nlon*nlat
!!
!!!!~!!!!!!!!!!!!!!!!!!!!!!!! INPUT-OUTPUT VARIABLES: !!!!!!!!!!!!!!!!!!!!!!!!!!!!
double precision, dimension(nDSlev),      intent(in):: x                        !!
double precision, dimension(nlitho,npxl), intent(in):: h, xs, E                 !!
double precision, dimension(npxl),        intent(in):: temp, runoff             !!
double precision,                         intent(in):: zfillvalue, taufillvalue !!
double precision, dimension(npxl),        intent(in):: veget_factor             !!
logical,          dimension(npxl),        intent(in):: missingpoints            !!
!                                                                               !!
double precision, dimension(nDSlev,nlitho,npxl), intent(out):: z, tau           !!
double precision, dimension(nlitho,npxl),        intent(out):: MPtot            !!
integer,          dimension(nlitho,npxl),        intent(out):: ktop             !!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!! INTERNAL VARIABLES: !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
double precision:: mdx, Kmain                                                   !!
integer:: k, j, j0, i                                                           !!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  mdx = x(1)-x(2)

  ! Continental loop
  do j = 1,npxl
    if (.not. missingpoints(j)) then

      ! Lithology loop
      do i = 1,nlitho-1 ! Skip last lithology class => carbonates

        !====================================================!
        !========= climate-only dependent variables =========!
        !====================================================!

        ! dissolution constant:
        Kmain = veget_factor(j) * dissolution_constant(temp(j), runoff(j), i)

        !===============================================================================================================!
        !==============                            INNER REGOLITH COMPUTATION                            ===============!
        !===============================================================================================================!
        !                                                                                !
        !           Solve the equations  ' dz/dt = RP + dissrate*dz/dx ' & ' dtau/dt = 1 + dissrate*dtau/dx '           !
        !           AT STEADY-STATE (West 2012 weathering model)                                                        !
        !                                                                                !
        !===============================================================================================================!

        ktop(i,j)  = nDSlev
        MPtot(i,j) = 0
        z(1,i,j)   = 0
        tau(1,i,j) = 0
        k = 1
        do while ( k < ktop(i,j) )
          ! test if next point is outside the regolith:
          if ( x(k+1) < xs(i,j)+epsl ) then
            ktop(i,j) = k
            k = nDSlev
          else
            tau(k+1,i,j) = (-1*(sigma(i)+1) * log(x(k+1)) / Kmain)**(1/(sigma(i)+1))
            z(k+1,i,j)   = E(i,j)*tau(k+1,i,j)
            MPtot(i,j)   = MPtot(i,j)  +  mdx * (z(k+1,i,j) + z(k,i,j)) / 2
            k = k + 1
          end if
        end do
        MPtot(i,j) = MPtot(i,j)  +  (x(ktop(i,j)) - xs(i,j)) * (h(i,j) + z(ktop(i,j),i,j))/2   +   xs(i,j)*h(i,j)

        ! Fillvalue for x points higher than regolith surface
        z(ktop(i,j)+1:nDSlev, i, j)   = zfillvalue
        tau(ktop(i,j)+1:nDSlev, i, j) = taufillvalue

      end do
    end if
  end do

end subroutine

end module


module SRP
! Description:
!    Contains subroutines to compute the perturbing acceleration due to SRP.
!
! Author:
!    Davide Amato
!    Space Dynamics Group - Technical University of Madrid
!    The University of Arizona
!    davideamato@email.arizona.edu
!
! Revisions:
!     180609: add calculation of eclipse conditions.
!
! ==============================================================================

! MODULES
use KINDS, only: dk
implicit none

contains

function SRP_ACC(iSRP,sunRadius,occRadius,pSRP_1au,au,CR,A2M_SRP,r,r_sun)
! Description:
!    Computes the perturbing acceleration due to solar radiation pressure.
!    Input units have to be consistent.
!
! Author:
!    Davide Amato
!    The University of Arizona
!    davideamato@email.arizona.edu
!
! Revisions:
!     180609: add call to ECLIPSE, change arguments.
!
! ==============================================================================

! VARIABLES
implicit none
! Arguments IN
integer,intent(in)   ::  iSRP
real(dk),intent(in)  ::  occRadius,sunRadius
real(dk),intent(in)  ::  pSRP_1au,au,CR,A2M_SRP,r(1:3),r_sun(1:3)
! Function definition
real(dk)  ::  SRP_ACC(1:3)

! Locals
real(dk)  :: nu    ! Shadowing factor
real(dk)  :: pSRP
real(dk)  :: dSun(1:3),dSunNorm

! ==============================================================================

! Sun->Spacecraft vector
dSun = r - r_sun
dSunNorm = sqrt(dot_product(dSun,dSun))

! SRP constant at the current distance from the Sun
pSRP = pSRP_1au*(au/dSunNorm)**2

! Perturbing acceleration
nu = 1._dk
if (iSRP == 2) then
  nu = ECLIPSE(sunRadius,occRadius,r,r_sun)

end if
SRP_ACC = nu * pSRP * CR * A2M_SRP * (dSun/dSunNorm)

end function SRP_ACC

function ECLIPSE(sunRadius,occRadius,r,rSun)
! Description:
!    Find out if the Sun is eclipsed by an occulting body. Returns value of
!    the coefficient \nu in [1], that is:
!    \nu = 1                   ,  for orbiter in sunlight
!    \nu = 0                   ,  for orbiter in umbra (total eclipse)
!    \nu = 1 - A/(\pi * a^2)   , for orbiter in penumbra (partial eclipse)
!    \nu = 1 - b^2/a^2         , for orbiter in antumbra (annular eclipse)
!
! Author:
!    Davide Amato
!    The University of Arizona
!    davideamato@email.arizona.edu
!
! Revisions:
!     180609: procedure created.
!
! References:
!     [1] Montenbruck, O., and Gill, E. 'Satellite Orbits', Springer, 2000.
!
! ==============================================================================

! Modules
use PHYS_CONST, only: pi

! Variables
implicit none
real(dk),intent(in)  ::  r(1:3)       ! Occulting body -> orbiter
real(dk),intent(in)  ::  rSun(1:3)    ! Occulting body -> Sun
real(dk),intent(in)  ::  sunRadius,occRadius  ! Physical radii of the bodies
! Returns
real(dk)  ::  ECLIPSE

! Locals
real(dk)  ::  rOrbSun(1:3)
real(dk)  ::  rDotRSun,rNorm,rSunNorm,rOrbSunNorm
real(dk)  ::  aApp,bApp  ! Apparent radii of the Sun and occulting body
real(dk)  ::  cApp       ! Apparent separation of the centers
real(dk)  ::  x,y        ! Aux quantities for the calculation of the occulted area
real(dk)  ::  occArea
real(dk)  ::  nu         ! Shadowing factor

! ==============================================================================

nu = 1._dk

rDotRSun = dot_product(r,rSun)
! Check if orbiter is in opposition
if (rDotRSun < 0._dk) then
  rNorm    = sqrt(dot_product(r,r))
  rSunNorm = sqrt(dot_product(rSun,rSun))

  ! Apparent radii of the bodies
  aApp = asin(sunRadius / rSunNorm)
  bApp = asin(occRadius / rNorm)

  rOrbSun     = rSun - r
  rOrbSunNorm = sqrt(dot_product(rOrbSun,rOrbSun))

  ! Apparent separation of the centers
  cApp = acos( - dot_product( r, rOrbSun ) / ( rNorm * rOrbSunNorm ) )

  ! Shadow condition
  if (aApp + bApp >= cApp) then
    if (cApp > abs(bApp - aApp)) then
      ! Penumbra
      x = ( cApp**2 + aApp**2 - bApp**2 ) / (2 * cApp)
      y = sqrt( aApp**2 - x**2 )

      occArea = aApp ** 2 * acos( x / aApp ) + &
      & bApp ** 2 * acos( (cApp - x)/bApp ) - cApp * y

      nu = 1._dk - occArea / (pi * aApp ** 2)

    else if ( (cApp <= abs(bApp - aApp) .and. bApp > aApp ) ) then
      ! Umbra
      nu = 0._dk

    else if ( (cApp <= abs(bApp - aApp) .and. bApp <= aApp ) ) then
      ! Antumbra
      nu = 1._dk - bApp**2 / aApp**2

    end if

  end if

end if

ECLIPSE = nu

end function ECLIPSE

end module SRP


C
C**************************  FLUXTRA ***********************************
C
C  compute Darcy velocity values for first time step
c  velocity is VN (normal) component
C  3D
C***********************************************************************
C
      SUBROUTINE FLUXTRA(N,NFACE,NT,VELREC,
     1           PLIST,ISIDE,SIDE_CNC,
     2           Node_FaceCount,Node_FaceIDS,
     3           X,Y,Z,XC,YC,ZC,Faces_ReferenceVector,FaceCentroid,
     4           AREFACE,VX,VY,VZ,FACEFLUX,
     5           Faces_FaceType,Faces_NeumannFlux,
     6           BKCFLOWINT_NODE,ARENOD,VN)
C
      IMPLICIT  NONE
      INCLUDE  'CATHY.H'
C     LOCAL
      INTEGER  I,K,j,itetra,jtetra,ltetra,iel1,iel2,iface
      INTEGER  Node1,Node2,Node3,Faccia
      REAL*8   vxface,vyface,vzface
      REAL*8   alpha, beta, gamma, prodscal
      REAL*8   xl(3), yl(3), zl(3)
      REAL*8   Normal1(3)
      REAL*8   FLUX(3),AREA(NMAX)
      REAL*8   Do3DScalarProduct
C     PASSED
      INTEGER  n,nface,nt,velrec
      INTEGER  plist(2,*),side_cnc(4,*)
      INTEGER  iside(3,*)
      INTEGER  Faces_FaceType(*)
      INTEGER  Node_faceCount(*),Node_FaceIDS(NMAX,*)
      real*8   x(*), y(*), z(*)
      real*8   xc(*), yc(*), zc(*)
      REAL*8   vx(*),vy(*),vz(*)
      REAL*8   FACEFLUX(3,*)
      REAL*8   Faces_ReferenceVector(3,*),Faces_NeumannFlux(*)
      REAL*8   BKCFLOWINT_NODE(*),ARENOD(*)
      REAL*8   FaceCentroid(3,*),AREFACE(*)
C     PASSED AND CALCULATED
      REAL*8   vn(nface)
C
C    e = face passing for (x1,y1,z1), (x2,y2,z2), (x3,y3,z3)
C    components of the normal to the face:
C    nx = alpha, ny=beta nz=gamma
C    alpha= (y2-y1)(z3-z1) - (z2-z1)(y3-y1)
C    beta = (x3-x1)(z2-z1) - (x2-x1)(z3-z1)
C    gamma= (x2-x1)(y3-y1) - (y2-y1)(x3-x1)
C    d= sqrt(alpha**2 + beta**2 + gamma**2)
C    normalized : nx= alpha/d, ny=beta/d, nz=gamma/d
C
C   vn = integral along the face e of the inner product of v and n
C   vn = intg <v,n> ds = intg vx nx + vy ny + vz nz ds
C   setting z=f(x,y) the intg along the face e of a function
C   g(x,y,z) can be computed as the integral into 2 variables x, y
C   intg g(x,y,f(x,y) sqrt(1 + dfx**2 + dfy**2) dx dy
C   In our case sqrt(1 + dfx**2 + dfy**2) = 1/|nz| and we have:
C   intg (vx nx + vy ny + vz nz ) 1/|nz| dx dy =
C   (vx nx + vy ny + vz nz ) 1/|nz| area(dx dy) =
C   (vx nx + vy ny + vz nz ) 1/|nz| gamma/2 =
C   (vx nx + vy ny + vz nz ) d/2   =
C   (vx alpha + vy beta + vz gamma)/2
C        |          |          |
C       =          =           =
C   ( vx bi      vy ci       vz di )/2
C
C
       DO i=1,nface
          iel1= plist(1,i)
          IF(VELREC.NE.1)THEN
            do j=1,3
              xl(j) = x(iside(j,i))
              yl(j) = y(iside(j,i))
              zl(j) = z(iside(j,i))
            end do
            alpha = (yl(2) - yl(1))*(zl(3) - zl(1)) -
     1              (yl(3) - yl(1))*(zl(2) - zl(1))
            beta  = (xl(3) - xl(1))*(zl(2) - zl(1)) -
     1              (xl(2) - xl(1))*(zl(3) - zl(1))
            gamma = (xl(2) - xl(1))*(yl(3) - yl(1)) -
     1              (xl(3) - xl(1))*(yl(2) - yl(1))
            prodscal =(xl(3)-xc(iel1))*alpha +(yl(3)-yc(iel1))*beta
     2              + (zl(3) - zc(iel1))*gamma
            if (prodscal.lt.0.d0) then
               alpha = -alpha
               beta = -beta
               gamma = -gamma
            end if
            iel2 = plist(2,i)
                 if (iel2.ne.0) then
                    vxface= 0.5d0*(vx(iel1) + vx(iel2))
                    vyface= 0.5d0*(vy(iel1) + vy(iel2))
                    vzface= 0.5d0*(vz(iel1) + vz(iel2))
                 else
                    vxface= vx(iel1)
                    vyface= vy(iel1)
                    vzface= vz(iel1)
                 end if
            vn(i) = 0.5d0*(vxface*alpha +vyface*beta +vzface*gamma)
          ELSE
            FLUX(1)=FACEFLUX(1,i)
            FLUX(2)=FACEFLUX(2,i)
            FLUX(3)=FACEFLUX(3,i)
               DO J=1,4
                  IF(SIDE_CNC(J,iel1).EQ.i)THEN
                     CALL EvalElementFaceNormal(iel1,J,
     1                    Normal1,Faces_ReferenceVector,
     2                    XC,YC,ZC,SIDE_CNC,FaceCentroid)
                  END IF
               END DO
            vn(i)=DO3DSCALARPRODUCT(Normal1,FLUX)*AREFACE(I)
          END IF
        END DO
CC
CC CARLOTTA The following lines perform a correction of the boundary
CC fluxes when velocity reconstruction is not performed. The
CC fluxes are recovered starting from the back-calculated fluxes,
CC after FLOW3D.
CC
        DO I=1,NFACE
           Node1=ISIDE(1,I)
           Node2=ISIDE(2,I)
           Node3=ISIDE(3,I)
           AREA(Node1)=0.0d0
           AREA(Node2)=0.0d0
           AREA(Node3)=0.0d0
           IF(PLIST(2,I).EQ.0)THEN
              IF(VELREC.NE.1)THEN
                  IF(Faces_FaceType(I).EQ.2)THEN
                    VN(I)=Faces_NeumannFlux(I)*AREFACE(I)
                  END IF
                  IF(Faces_FaceType(I).EQ.0)THEN
                    DO K=1,Node_FaceCount(Node1)
                        Faccia=Node_FaceIDS(Node1,K)
                          IF(PLIST(2,Faccia).EQ.0)THEN
                            AREA(Node1)=AREA(Node1)+
     1                      AREFACE(Faccia)/3.0d0
                          END IF
                    END DO
                    DO K=1,Node_FaceCount(Node2)
                        Faccia=Node_FaceIDS(Node2,K)
                          IF(PLIST(2,Faccia).EQ.0)THEN
                            AREA(Node2)=AREA(Node2)+
     1                      AREFACE(Faccia)/3.0d0
                          END IF
                    END DO
                    DO K=1,Node_FaceCount(Node3)
                        Faccia=Node_FaceIDS(Node3,K)
                          IF(PLIST(2,Faccia).EQ.0)THEN
                            AREA(Node3)=AREA(Node3)+
     1                      AREFACE(Faccia)/3.0d0
                          END IF
                    END DO
                    VN(I)=-(BKCFLOWINT_NODE(Node1)/AREA(Node1)+
     1                     BKCFLOWINT_NODE(Node2)/AREA(Node2)+
     2                     BKCFLOWINT_NODE(Node3)/AREA(Node3))
     3                     *AREFACE(I)/3.0d0
                  END IF
              END IF
           END IF
        END DO
C
      RETURN
      END


      REAL FUNCTION FUNC2(THETA)
C
C SPECIFIES RIGHT HAND SIDE OF NON-LINEAR BALANCE EQUATION FOR
C PURPOSES OF NUMERICAL INTEGRATION; REQUIRES REAL FUNCTION BUBFNC
C MAKE SURE THAT CONSTANTS ARE CONSISTANT WITH THOSE IN INPUT!!
C USED FOR TEST CASE 4 IN ROUTINE INIT
C
C CALLED BY: D01AHE
C CALLS: BUBFNC
C
C REVISIONS:
C 7-10-92 CHANGE TO CCM CODING CONVENTIONS (R. JAKOB)
C
C---- Common Blocks ----------------------------------------------------
C
C Constants
      INCLUDE 'consts.i'
C
C------------ Arguments ------------------------------------------------
C
C     Input
C
C LATITUDE
      REAL THETA
C
C------ Local Variables ------------------------------------------------
C
      REAL SU
C
C----- External Functions ----------------------------------------------
C
C     ZONAL FLOW FUNCTION
C
      EXTERNAL BUBFNC
      REAL BUBFNC
C
C----- Executable Statements -------------------------------------------
C
      SU     = BUBFNC(THETA)
      FUNC2  = (2.0*OMEGA*A*SIN(THETA) + TAN(THETA)*SU)*SU
C
      RETURN
      END


      SUBROUTINE MD03BX( M, N, FNORM, J, LDJ, E, JNORMS, GNORM, IPVT,
     $                   DWORK, LDWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To compute the QR factorization with column pivoting of an
C     m-by-n matrix J (m >= n), that is, J*P = Q*R, where Q is a matrix
C     with orthogonal columns, P a permutation matrix, and R an upper
C     trapezoidal matrix with diagonal elements of nonincreasing
C     magnitude, and to apply the transformation Q' on the error
C     vector e (in-situ). The 1-norm of the scaled gradient is also
C     returned. The matrix J could be the Jacobian of a nonlinear least
C     squares problem.
C
C     ARGUMENTS
C
C     Input/Output Parameters
C
C     M       (input) INTEGER
C             The number of rows of the Jacobian matrix J.  M >= 0.
C
C     N       (input) INTEGER
C             The number of columns of the Jacobian matrix J.
C             M >= N >= 0.
C
C     FNORM   (input) DOUBLE PRECISION
C             The Euclidean norm of the vector e.  FNORM >= 0.
C
C     J       (input/output) DOUBLE PRECISION array, dimension (LDJ, N)
C             On entry, the leading M-by-N part of this array must
C             contain the Jacobian matrix J.
C             On exit, the leading N-by-N upper triangular part of this
C             array contains the upper triangular factor R of the
C             Jacobian matrix. Note that for efficiency of the later
C             calculations, the matrix R is delivered with the leading
C             dimension MAX(1,N), possibly much smaller than the value
C             of LDJ on entry.
C
C     LDJ     (input/output) INTEGER
C             The leading dimension of array J.
C             On entry, LDJ >= MAX(1,M).
C             On exit,  LDJ >= MAX(1,N).
C
C     E       (input/output) DOUBLE PRECISION array, dimension (M)
C             On entry, this array must contain the error vector e.
C             On exit, this array contains the updated vector Q'*e.
C
C     JNORMS  (output) DOUBLE PRECISION array, dimension (N)
C             This array contains the Euclidean norms of the columns of
C             the Jacobian matrix, considered in the initial order.
C
C     GNORM   (output) DOUBLE PRECISION
C             If FNORM > 0, the 1-norm of the scaled vector
C             J'*Q'*e/FNORM, with each element i further divided by
C             JNORMS(i) (if JNORMS(i) is nonzero).
C             If FNORM = 0, the returned value of GNORM is 0.
C
C     IPVT    (output) INTEGER array, dimension (N)
C             This array defines the permutation matrix P such that
C             J*P = Q*R. Column j of P is column IPVT(j) of the identity
C             matrix.
C
C     Workspace
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) returns the optimal value
C             of LDWORK.
C
C     LDWORK  INTEGER
C             The length of the array DWORK.
C             LDWORK >= 1,      if N = 0 or M = 1;
C             LDWORK >= 4*N+1,  if N > 1.
C             For optimum performance LDWORK should be larger.
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value.
C
C     METHOD
C
C     The algorithm uses QR factorization with column pivoting of the
C     matrix J, J*P = Q*R, and applies the orthogonal matrix Q' to the
C     vector e.
C
C     CONTRIBUTORS
C
C     V. Sima, Research Institute for Informatics, Bucharest, Dec. 2001.
C
C     REVISIONS
C
C     -
C
C     KEYWORDS
C
C     Elementary matrix operations, Jacobian matrix, matrix algebra,
C     matrix operations.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE
      PARAMETER         ( ZERO = 0.0D0, ONE = 1.0D0 )
C     .. Scalar Arguments ..
      INTEGER           INFO, LDJ, LDWORK, M, N
      DOUBLE PRECISION  FNORM, GNORM
C     .. Array Arguments ..
      INTEGER           IPVT(*)
      DOUBLE PRECISION  DWORK(*), E(*), J(*), JNORMS(*)
C     .. Local Scalars ..
      INTEGER           I, ITAU, JWORK, L, WRKOPT
      DOUBLE PRECISION  SUM
C     .. External Functions ..
      DOUBLE PRECISION  DDOT, DNRM2
      EXTERNAL          DDOT, DNRM2
C     .. External Subroutines ..
      EXTERNAL          DGEQP3, DLACPY, DORMQR, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         ABS, INT, MAX
C     ..
C     .. Executable Statements ..
C
      INFO = 0
      IF ( M.LT.0 ) THEN
         INFO = -1
      ELSEIF ( N.LT.0.OR. M.LT.N ) THEN
         INFO = -2
      ELSEIF ( FNORM.LT.ZERO ) THEN
         INFO = -3
      ELSEIF ( LDJ.LT.MAX( 1, M ) ) THEN
         INFO = -5
      ELSE
         IF ( N.EQ.0 .OR. M.EQ.1 ) THEN
            JWORK = 1
         ELSE
            JWORK = 4*N + 1
         END IF
         IF ( LDWORK.LT.JWORK )
     $      INFO = -11
      END IF
C
      IF ( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'MD03BX', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      GNORM = ZERO
      IF ( N.EQ.0 ) THEN
         LDJ = 1
         DWORK(1) = ONE
         RETURN
      ELSEIF ( M.EQ.1 ) THEN
         JNORMS(1) = ABS( J(1) )
         IF ( FNORM*J(1).NE.ZERO )
     $      GNORM = ABS( E(1)/FNORM )
         LDJ      = 1
         IPVT(1)  = 1
         DWORK(1) = ONE
         RETURN
      END IF
C
C     Initialize the column pivoting indices.
C
      DO 10 I = 1, N
         IPVT(I) = 0
   10 CONTINUE
C
C     (Note: Comments in the code beginning "Workspace:" describe the
C     minimal amount of real workspace needed at that point in the
C     code, as well as the preferred amount for good performance.
C     NB refers to the optimal block size for the immediately
C     following subroutine, as returned by ILAENV.)
C
      ITAU   = 1
      JWORK  = ITAU + N
      WRKOPT = 1
C
C     Compute the QR factorization with pivoting of J, and apply Q' to
C     the vector e.
C
C     Workspace: need:    4*N + 1;
C                prefer:  3*N + ( N+1 )*NB.
C
      CALL DGEQP3( M, N, J, LDJ, IPVT, DWORK(ITAU), DWORK(JWORK),
     $             LDWORK-JWORK+1, INFO )
      WRKOPT = MAX( WRKOPT, INT( DWORK(JWORK) ) + JWORK - 1 )
C
C     Workspace: need:    N + 1;
C                prefer:  N + NB.
C
      CALL DORMQR( 'Left', 'Transpose', M, 1, N, J, LDJ, DWORK(ITAU), E,
     $             M, DWORK(JWORK), LDWORK-JWORK+1, INFO )
      WRKOPT = MAX( WRKOPT, INT( DWORK(JWORK) ) + JWORK - 1 )
C
      IF ( LDJ.GT.N ) THEN
C
C        Reshape the array J to have the leading dimension N.
C        This destroys the details of the orthogonal matrix Q.
C
         CALL DLACPY( 'Upper', N, N, J, LDJ, J, N )
         LDJ = N
      END IF
C
C     Compute the norm of the scaled gradient and original column norms.
C
      IF ( FNORM.NE.ZERO ) THEN
C
         DO 20 I = 1, N
            L = IPVT(I)
            JNORMS(L) = DNRM2( I, J((I-1)*LDJ+1), 1 )
            IF ( JNORMS(L).NE.ZERO ) THEN
               SUM = DDOT( I, J((I-1)*LDJ+1), 1, E, 1 )/FNORM
               GNORM = MAX( GNORM, ABS( SUM/JNORMS(L) ) )
            END IF
   20    CONTINUE
C
      ELSE
C
         DO 30 I = 1, N
            L = IPVT(I)
            JNORMS(L) = DNRM2( I, J((I-1)*LDJ+1), 1 )
   30    CONTINUE
C
      END IF
C
      DWORK(1) = WRKOPT
      RETURN
C
C *** Last line of MD03BX ***
      END


*
*     -------------------------------------------------------------
*      S I X J 5
*     -------------------------------------------------------------
*
*                                                                  *
*     THIS PACKAGE DETERMINES THE VALUES OF 6j COEFFICIENT         *
*                                                                  *
*     | J/2  K/2  L/2 |                                            *
*     | M/2  N/2  1/2 |             [B.M.X. 75]                    *
*                                                                  *
*     Written by G. Gaigalas,                                      *
*     Vilnius,  Lithuania                             March 1995   *
*
      SUBROUTINE SIXJ5(J,K,L,M,N,ITIK,SI)
      IMPLICIT DOUBLEPRECISION (A-H,O-Z)
      COMMON/CONSTS/ZERO,TENTH,HALF,ONE,TWO,THREE,FOUR,SEVEN,ELEVEN,EPS
      SI=ZERO
      IF(ITIK.NE.0) THEN
C
C     CHESKED TRIANGULAR CONDITIONS
C
        IF(IXJTIK(J,K,L,M,N,1).EQ.0)RETURN
      ENDIF
      I1=(J+K+L)/2
      AS=DBLE(I1)
      A=DBLE(L)
      B=DBLE(K)
      C=DBLE(J)
      AKA=ONE
      IF(MOD(I1,2).NE.0)AKA=-AKA
      IF(K.LT.M) THEN
        IF(J.LT.N)  THEN
C              M > K,  J < N.
          SI=-AKA*DSQRT((AS+TWO)*(AS-A+ONE)/
     *      ((B+ONE)*(B+TWO)*(C+ONE)*(C+TWO)))
        ELSEIF(J.GT.N)  THEN
C              M > K,  J > N.
          SI=AKA*DSQRT((AS-C+ONE)*(AS-B)/
     *      ((B+ONE)*(B+TWO)*C*(C+ONE)))
        ENDIF
      ELSEIF(K.GT.M) THEN
        IF(J.LT.N) THEN
C             M < K,  J < N.
          SI=AKA*DSQRT((AS-C)*(AS-B+ONE)/
     *      (B*(B+ONE)*(C+ONE)*(C+TWO)))
        ELSEIF(J.GT.N) THEN
C             M < K,  J > N.
          SI=AKA*DSQRT((AS+ONE)*(AS-A)/(B*(B+ONE)*C*(C+ONE)))
        ENDIF
      ENDIF
      RETURN
      END


module rhs

	use global_variables
    use function_list
    use beta_safe
	implicit none

contains

    function gain(ndiscrete)

	    integer :: ndiscrete
        double precision :: gain
        integer :: i
        gain = 0.0

	    do i = 1,ndiscrete

            if (( ndiscrete -i > 0 ) .and. (ndiscrete-i < ndiscrete)) then

              gain = gain + 1.0/2*get_beta_ij(i,ndiscrete-i)*Nc(i)*Nc(ndiscrete-i)
              !gain = gain + 1.0/2*beta_all(i,ndiscrete-i)*Nc(i)*Nc(ndiscrete-i)

            end if

        end do

    end function gain

	function loss(ndiscrete)

	    integer :: ndiscrete
        double precision :: loss
        integer :: i
        loss = 0.0

	    do i = 1,n_discrete

            loss = loss + get_beta_ij(i,ndiscrete)*Nc(ndiscrete)*Nc(i)
			!loss = loss + beta_all(i,ndiscrete)*Nc(ndiscrete)*Nc(i)

	    end do

    end function loss

    subroutine FEX(NEQ,T,Y,YDOT)
        implicit none
        integer, intent (in) :: NEQ
        double precision, intent (in) :: T
        double precision, intent (in) :: Y(NEQ)
        double precision, intent (out) :: YDOT(NEQ)
	    integer :: i, j

		Nc = Y

        do j= 1,n_discrete

            YDOT(j) =  (2.0/(K_coag))*(gain(j) - loss(j))

        end do

    end subroutine FEX

    subroutine JEX(NEQ,T,Y,ML,MU,PD,NRPD)
        implicit none
        integer, intent (in) :: NEQ, ML, MU, NRPD
        double precision, intent (in) :: T
        double precision, intent (in) :: y(NEQ)
        double precision, intent (out) :: pd(NRPD,NEQ)
        !double precision, dimension(:,:), allocatable :: pd

        integer :: k,j,counter

        !pd_gain = 0.0
        !pd_loss = 0.0
        !do k = 1, NEQ
        !    do j = 1, k-1
        !        pd_gain(k,j) = beta_all(j,k-j)*y(j)*y(k-j)
        !    end do
        !end do
        !
        !do k = 1,NEQ
        !    do j = 1, NEQ
        !    pd_loss (k,j) = beta_all(k,j)*y(k)
        !    if (k==j) then
        !        do counter = 1,NEQ
        !            pd_loss(k,j) = pd_loss(k,j) + beta_all(k,j)*y(j)
        !        end do
        !    end if
        !    end do
        !end do
        !
        !
        !
        !pd = (2.0/K_coag)*(pd_gain-pd_loss)

        !allocate(pd(n_discrete,n_discrete))

        do k = 1,NEQ
            do j = 1, NEQ
                if (j<k) then
                pd(k,j) = get_beta_ij(j,k-j)*y(j)*y(k-j)
                end if
                pd(k,j) = -get_beta_ij(k,j)*y(k)
            if (k==j) then
                do counter = 1,NEQ
                    pd(k,j) = pd(k,j) - get_beta_ij(k,j)*y(j)
                end do
            end if
            end do
        end do

        pd = (2.0/K_coag)*pd

     end subroutine JEX
end module rhs

!program test_rhs
!    use user_interface
!    implicit none
!
!    integer :: NEQa
!    double precision :: Ta
!    double precision, dimension(2) :: Ya, YDOTa
!
!    NEQa = 2
!    Ta = 1.0
!    Ya(1) = 1.0
!    Ya(2) = 2.0
!
!    call read_inputfile()
!
!    !print*, II(1,1)
!    !print*, loss(1,1), "  ", loss(2,1)
!    !print*, gain(1,1), "  ",  gain(2,1)
!    print*, YDOTa
!    call FEX(NEQa,Ta,Ya,YDOTa)
!    print*, YDOTa
!
!end program test_rhs


program persist
    implicit none
    ! Explicit types for blas calls
    integer, parameter :: i32 = 4
    integer, parameter :: i64 = 8
    integer, parameter :: f32 = kind(1.e0)
    integer, parameter :: f64 = kind(1.d0)
    real(f64) :: ddot

    ! Iteration variables
    integer(i32) i, j

    ! Physical constants
    integer(i32),  parameter :: shape = 43_i32
    real(f64),     parameter :: scale = 1.91163_f64

    ! Specified constants
    real(f64)    :: kon, koff, sigma
    integer(i32) :: m, n, d

    ! Program variables
    real(f64),    allocatable :: A(:,:), x(:), y(:)
    integer(i32), allocatable :: ipiv(:)
    real(f64)                 :: z

    ! Parse command-line input
    character(len=128) :: arg
    if (command_argument_count() < 4) then
        print *, "Usage: ./persist mass minsize beta gamma"
        call exit()
    end if
    call get_command_argument(1, arg)
    read(arg,*) m
    call get_command_argument(2, arg)
    read(arg,*) n
    call get_command_argument(3, arg)
    read(arg,*) kon
    call get_command_argument(4, arg)
    read(arg,*) koff

    ! Program constants
    d = m-n+1
    sigma = 2.0_f64 * kon / koff

    ! Allocate and initialize the memory for the problem
    allocate(A(n+2, n:m), x(n:m), y(n:m), ipiv(n:m))
    ! super-diagonals
    A(2:n, n:m) = 1.0_f64
    ! diagonal
    A(n+1, n:m) = (/ (-sigma*(m-i) - (i-1), i=n, m) /)
    ! sub-diagonal
    A(n+2, n:m) = (/ (sigma*(m-i), i=n, m) /)
    ! summation vector
    x(n:(2*n-1)) = 0.0_f64
    x((2*n):m)   = (/ (i-2*n+1, i=2*n, m) /)
    ! unit vector
    y(n) = 1.0_f64
    y((n+1):m) = 0.0_f64

    ! LU-factorize the matrix operator
    call dgbtrf(d, d, 1, n-1, A, n+2, ipiv, i)

    ! update y <- inv(A)*y
    call dgbtrs('N', d, 1, n-1, 1, A, n+2, ipiv, y, d, i)

    z = ddot(d, x, 1, y, 1)

    ! update A again (maybe could update factors directly if too slow)
    ! super-diagonals
    A(2:n, n:m) = -koff*scale*1.0_f64
    ! diagonal
    A(n+1, n:m) = (/ (1 + (koff*scale)*(sigma*(m-i) + (i-1)), i=n, m) /)
    ! sub-diagonal
    A(n+2, n:m) = (/ (-(koff*scale)*sigma*(m-i), i=n, m) /)

    ! LU-factorize the matrix operator
    call dgbtrf(d, d, 1, n-1, A, n+2, ipiv, i)

    do j = 1, shape
        call dgbtrs('N', d, 1, n-1, 1, A, n+2, ipiv, y, d, i)
    end do

    print *, m, n, kon, koff, ddot(d, x, 1, y, 1)-z

    ! Cleanup the problem
    deallocate(A, x, y, ipiv)

end program persist


subroutine spectrum(rcc, rss, zcs, zsc)

      use stel_kinds, only: dp
      use name0, only: czero
      use name1, only: mpol1, nmax, nsd
      use scalars, only: ns
      use mnarray, only: mscale, nscale, xmpq
      use spectra, only: specw

      implicit none

      real(kind=dp), intent(in) :: rcc(ns,0:nmax,0:mpol1)
      real(kind=dp), intent(in) :: rss(ns,0:nmax,0:mpol1)
      real(kind=dp), intent(in) :: zsc(ns,0:nmax,0:mpol1)
      real(kind=dp), intent(in) :: zcs(ns,0:nmax,0:mpol1)

      real(kind=dp) :: t1(nsd)
      real(kind=dp) :: dnumer(nsd)
      real(kind=dp) :: denom (nsd)

      integer       :: js, n, m
      real(kind=dp) :: scalefac

      do js = 2, ns
        dnumer(js) = czero
        denom (js) = czero
      end do

      do n = 0, nmax
        do m = 1, mpol1

          scalefac = (mscale(m)*nscale(n))**2

          do js = 2, ns
            t1(js) = (  rcc(js,n,m)**2 + rss(js,n,m)**2 &
                      + zcs(js,n,m)**2 + zsc(js,n,m)**2  ) * scalefac
          end do

          do js = 2, ns
            dnumer(js) = dnumer(js) + t1(js)*xmpq(m,3)
            denom (js) = denom (js) + t1(js)*xmpq(m,2)
          end do

        end do
      end do

      do js = 2, ns
        specw(js) = dnumer(js)/denom(js)
      end do

      return
end


*DECK SGEIR
      SUBROUTINE SGEIR (A, LDA, N, V, ITASK, IND, WORK, IWORK)
C***BEGIN PROLOGUE  SGEIR
C***PURPOSE  Solve a general system of linear equations.  Iterative
C            refinement is used to obtain an error estimate.
C***LIBRARY   SLATEC
C***CATEGORY  D2A1
C***TYPE      SINGLE PRECISION (SGEIR-S, CGEIR-C)
C***KEYWORDS  COMPLEX LINEAR EQUATIONS, GENERAL MATRIX,
C             GENERAL SYSTEM OF LINEAR EQUATIONS
C***AUTHOR  Voorhees, E. A., (LANL)
C***DESCRIPTION
C
C    Subroutine SGEIR solves a general NxN system of single
C    precision linear equations using LINPACK subroutines SGEFA and
C    SGESL.  One pass of iterative refinement is used only to obtain
C    an estimate of the accuracy.  That is, if A is an NxN real
C    matrix and if X and B are real N-vectors, then SGEIR solves
C    the equation
C
C                          A*X=B.
C
C    The matrix A is first factored into upper and lower tri-
C    angular matrices U and L using partial pivoting.  These
C    factors and the pivoting information are used to calculate
C    the solution, X.  Then the residual vector is found and
C    used to calculate an estimate of the relative error, IND.
C    IND estimates the accuracy of the solution only when the
C    input matrix and the right hand side are represented
C    exactly in the computer and does not take into account
C    any errors in the input data.
C
C    If the equation A*X=B is to be solved for more than one vector
C    B, the factoring of A does not need to be performed again and
C    the option to solve only (ITASK .GT. 1) will be faster for
C    the succeeding solutions.  In this case, the contents of A,
C    LDA, N, WORK, and IWORK must not have been altered by the
C    user following factorization (ITASK=1).  IND will not be
C    changed by SGEIR in this case.
C
C  Argument Description ***
C
C    A      REAL(LDA,N)
C             the doubly subscripted array with dimension (LDA,N)
C             which contains the coefficient matrix.  A is not
C             altered by the routine.
C    LDA    INTEGER
C             the leading dimension of the array A.  LDA must be great-
C             er than or equal to N.  (terminal error message IND=-1)
C    N      INTEGER
C             the order of the matrix A.  The first N elements of
C             the array A are the elements of the first column of
C             matrix A.  N must be greater than or equal to 1.
C             (terminal error message IND=-2)
C    V      REAL(N)
C             on entry, the singly subscripted array(vector) of di-
C               mension N which contains the right hand side B of a
C               system of simultaneous linear equations A*X=B.
C             on return, V contains the solution vector, X .
C    ITASK  INTEGER
C             If ITASK=1, the matrix A is factored and then the
C               linear equation is solved.
C             If ITASK .GT. 1, the equation is solved using the existing
C               factored matrix A (stored in WORK).
C             If ITASK .LT. 1, then terminal error message IND=-3 is
C               printed.
C    IND    INTEGER
C             GT. 0  IND is a rough estimate of the number of digits
C                     of accuracy in the solution, X.  IND=75 means
C                     that the solution vector X is zero.
C             LT. 0  see error message corresponding to IND below.
C    WORK   REAL(N*(N+1))
C             a singly subscripted array of dimension at least N*(N+1).
C    IWORK  INTEGER(N)
C             a singly subscripted array of dimension at least N.
C
C  Error Messages Printed ***
C
C    IND=-1  terminal   N is greater than LDA.
C    IND=-2  terminal   N is less than one.
C    IND=-3  terminal   ITASK is less than one.
C    IND=-4  terminal   The matrix A is computationally singular.
C                         A solution has not been computed.
C    IND=-10 warning    The solution has no apparent significance.
C                         The solution may be inaccurate or the matrix
C                         A may be poorly scaled.
C
C               Note-  The above terminal(*fatal*) error messages are
C                      designed to be handled by XERMSG in which
C                      LEVEL=1 (recoverable) and IFLAG=2 .  LEVEL=0
C                      for warning error messages from XERMSG.  Unless
C                      the user provides otherwise, an error message
C                      will be printed followed by an abort.
C
C***REFERENCES  J. J. Dongarra, J. R. Bunch, C. B. Moler, and G. W.
C                 Stewart, LINPACK Users' Guide, SIAM, 1979.
C***ROUTINES CALLED  R1MACH, SASUM, SCOPY, SDSDOT, SGEFA, SGESL, XERMSG
C***REVISION HISTORY  (YYMMDD)
C   800430  DATE WRITTEN
C   890531  Changed all specific intrinsics to generic.  (WRB)
C   890831  Modified array declarations.  (WRB)
C   890831  REVISION DATE from Version 3.2
C   891214  Prologue converted to Version 4.0 format.  (BAB)
C   900315  CALLs to XERROR changed to CALLs to XERMSG.  (THJ)
C   900510  Convert XERRWV calls to XERMSG calls.  (RWC)
C   920501  Reformatted the REFERENCES section.  (WRB)
C***END PROLOGUE  SGEIR
C
      INTEGER LDA,N,ITASK,IND,IWORK(*),INFO,J
      REAL A(LDA,*),V(*),WORK(N,*),XNORM,DNORM,SDSDOT,SASUM,R1MACH
      CHARACTER*8 XERN1, XERN2
C***FIRST EXECUTABLE STATEMENT  SGEIR
      IF (LDA.LT.N) THEN
         IND = -1
         WRITE (XERN1, '(I8)') LDA
         WRITE (XERN2, '(I8)') N
         CALL XERMSG ('SLATEC', 'SGEIR', 'LDA = ' // XERN1 //
     *      ' IS LESS THAN N = ' // XERN2, -1, 1)
         RETURN
      ENDIF
C
      IF (N.LE.0) THEN
         IND = -2
         WRITE (XERN1, '(I8)') N
         CALL XERMSG ('SLATEC', 'SGEIR', 'N = ' // XERN1 //
     *      ' IS LESS THAN 1', -2, 1)
         RETURN
      ENDIF
C
      IF (ITASK.LT.1) THEN
         IND = -3
         WRITE (XERN1, '(I8)') ITASK
         CALL XERMSG ('SLATEC', 'SGEIR', 'ITASK = ' // XERN1 //
     *      ' IS LESS THAN 1', -3, 1)
         RETURN
      ENDIF
C
      IF (ITASK.EQ.1) THEN
C
C        MOVE MATRIX A TO WORK
C
         DO 10 J=1,N
            CALL SCOPY(N,A(1,J),1,WORK(1,J),1)
   10    CONTINUE
C
C        FACTOR MATRIX A INTO LU
C
         CALL SGEFA(WORK,N,N,IWORK,INFO)
C
C        CHECK FOR COMPUTATIONALLY SINGULAR MATRIX
C
         IF (INFO.NE.0) THEN
            IND = -4
            CALL XERMSG ('SLATEC', 'SGEIR',
     *         'SINGULAR MATRIX A - NO SOLUTION', -4, 1)
            RETURN
         ENDIF
      ENDIF
C
C     SOLVE WHEN FACTORING COMPLETE
C     MOVE VECTOR B TO WORK
C
      CALL SCOPY(N,V(1),1,WORK(1,N+1),1)
      CALL SGESL(WORK,N,N,IWORK,V,0)
C
C     FORM NORM OF X0
C
      XNORM=SASUM(N,V(1),1)
      IF (XNORM.EQ.0.0) THEN
         IND = 75
         RETURN
      ENDIF
C
C     COMPUTE  RESIDUAL
C
      DO 40 J=1,N
         WORK(J,N+1) = SDSDOT(N,-WORK(J,N+1),A(J,1),LDA,V,1)
   40 CONTINUE
C
C     SOLVE A*DELTA=R
C
      CALL SGESL(WORK,N,N,IWORK,WORK(1,N+1),0)
C
C     FORM NORM OF DELTA
C
      DNORM = SASUM(N,WORK(1,N+1),1)
C
C     COMPUTE IND (ESTIMATE OF NO. OF SIGNIFICANT DIGITS)
C     AND CHECK FOR IND GREATER THAN ZERO
C
      IND = -LOG10(MAX(R1MACH(4),DNORM/XNORM))
      IF (IND.LE.0) THEN
         IND = -10
         CALL XERMSG ('SLATEC', 'SGEIR',
     *      'SOLUTION MAY HAVE NO SIGNIFICANCE', -10, 0)
      ENDIF
      RETURN
      END


program setsize
implicit none

  integer, parameter :: p1 = 6
  integer, parameter :: p2 = 12
  integer, parameter :: r1 = 30
  integer, parameter :: r2 = 1000
  integer, parameter :: r3 = 2
  integer, parameter :: r4 = 4
  integer, parameter :: r5 = 8
  integer, parameter :: r6 = 16
  integer, parameter :: rprec1 = selected_real_kind(p1, r1)
  integer, parameter :: rprec2 = selected_real_kind(p2, r1)
  integer, parameter :: rprec3 = selected_real_kind(p2, r2)
  integer, parameter :: iprec1 = selected_int_kind(r3)
  integer, parameter :: iprec2 = selected_int_kind(r4)
  integer, parameter :: iprec3 = selected_int_kind(r5)
  integer, parameter :: iprec4 = selected_int_kind(r6)

  real(rprec1)    :: n1
  real(rprec2)    :: n2
  real(rprec3)    :: n3
  integer(iprec1) :: n4
  integer(iprec2) :: n5
  integer(iprec3) :: n6
  integer(iprec4) :: n7
  character(30) :: form

  form = "(a7, i11, i10, i6, i9, i8)"
  write(*, "(a)") "KIND NAME   KIND NUMBER   PRECISION        RANGE "
  write(*, "(a)") "                          min   set     min     set"
  write(*, "(a)") "______________________________________________________"
  write(*, form) "rprec1", kind(n1), p1, precision(n1), r1, range(n1)
  write(*, form) "rprec2", kind(n2), p2, precision(n2), r1, range(n2)
  write(*, form) "rprec3", kind(n3), p2, precision(n3), r2, range(n3)
  write(*,*)
  form = "(a7, i11, i25, i8)"
  write(*, form) "iprec1", kind(n4), r3, range(n4)
  write(*, form) "iprec2", kind(n5), r4, range(n5)
  write(*, form) "iprec3", kind(n6), r5, range(n6)
  write(*, form) "iprec4", kind(n7), r6, range(n7)

end program


subroutine Poisson_2D

! Include modules defined by mkl_poisson.f90 and mkl_dfti.f90 header files
use mkl_poisson
use variables
use constants
integer nx,ny
parameter(nx=MaxN-1, ny=MaxN-1)

integer i,j, stat
integer ipar(128)
real ax, bx, ay, by, lx, ly, hx, hy, xi, yi, cx, cy, c1
real spar(13*nx/2+7)
! Note that proper packing of data in right-hand side array f is
! automatically provided by the following declarations of the array
real f(nx+1,ny+1)
real bd_ax(ny+1), bd_bx(ny+1), bd_ay(nx+1), bd_by(nx+1)
real q
type(DFTI_DESCRIPTOR), pointer :: xhandle
character(4) BCtype

! Defining the rectangular domain 0<x<1, 0<y<1 for 2D Poisson Solver
ax=0.0E0
bx=1.0E0
ay=0.0E0
by=1.0E0

!*******************************************************************************
! Setting the coefficient q to 0.
! Note that this is the way to use Helmholtz Solver to solve Poisson problem!
!*******************************************************************************
q=0.0E0

! Computing the mesh size hx in x-direction
lx=bx-ax
hx=lx/nx
! Computing the mesh size hy in y-direction
ly=by-ay
hy=ly/ny

! Filling in the values of the TRUE solution u(x,y)=sin(2*pi*x)*sin(2*pi*y)+1
! in the mesh points into the array u
! Filling in the right-hand side f(x,y)=(8*pi*pi+q)*sin(2*pi*x)*sin(2*pi*y)+q
! in the mesh points into the array f.
! We choose the right-hand side to correspond to the TRUE solution of
! Poisson equation.
! Here we are using the mesh sizes hx and hy computed before to compute
! the coordinates (xi,yi) of the mesh points
do j=1,ny
   do i=1,nx
      f(i,j)=((Uon(i+1,j)-Uon(i,j))/dx+(Von(i,j+1)-Von(i,j))/dy)/dt
   enddo
enddo

! Setting the type of the boundary conditions on each side of the rectangular domain:
! On the boundary laying on the line x=0(=ax) Dirichlet boundary condition
! will be used
! On the boundary laying on the line x=1(=bx) Dirichlet boundary condition
! will be used
! On the boundary laying on the line y=0(=ay) Neumann boundary condition will be used
! On the boundary laying on the line y=1(=by) Neumann boundary condition will be used
BCtype = 'NNNN'

! Setting the values of the boundary function G(x,y) that is equal to
! the TRUE solution in the mesh points laying on Dirichlet boundaries
do i = 1,ny
   bd_ax(i) = 0.0
   bd_bx(i) = 0.0
enddo
! Setting the values of the boundary function g(x,y) that is equal to
! the normal derivative of the TRUE solution in the mesh points laying on
! Neumann boundaries
do i = 1,nx
   bd_ay(i) = 0.0
   bd_by(i) = 0.0
enddo

! Initializing ipar array to make it free from garbage
do i=1,128
   ipar(i)=0
enddo

! Initializing simple data structures of Poisson Library for 2D Poisson Solver
call s_init_Helmholtz_2D(ax, bx, ay, by, nx, ny, BCtype, q, ipar, spar, stat)
call s_commit_Helmholtz_2D(f, bd_ax, bd_bx, bd_ay, bd_by, xhandle, ipar, spar, stat)
call s_Helmholtz_2D(f, bd_ax, bd_bx, bd_ay, bd_by, xhandle, ipar, spar, stat)
call free_Helmholtz_2D(xhandle, ipar, stat)
call mkl_freebuffers
do j=1,ny
   do i=1,nx
      p(i,j)=f(i,j)
   enddo
enddo

end subroutine Poisson_2D


cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
cc                                                                    cc
cc                 mnpola : polar test program                        cc
cc                                                                    cc
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
      real x(200),y(200),z(200),w(200),u(200),v(200),tu(30),tv(30),
     * c(300),exact(200),f(200),wrk1(15000),wrk2(5700)
      integer iopt(3),iwrk(500)
      integer i,is,ier,kwrk,l,lwrk1,lwrk2,l1,l2,m,m1,m2,nc,nu,nv,
     * nuest,nvest
      real s,fp,eps,sum,ermax,error,ai
      real abs,rad1,rad2,testpo,evapol
      external rad1,rad2
c  we fetch the number of data points.
      m1 = 200
      m2 =  90
c  we fetch and print the coordinates and function values of the data.
      write(6,900)
      write(6,905)
      l2 = 0
      do 10 i=1,50
         l1 = l2+1
         l2 = l2+4
         read(5,910) (x(l),y(l),z(l),l=l1,l2)
  10  continue
      write(6,915)(x(l),y(l),z(l),l=1,m1)
c  we calculate the exact function values and set up the weights w(i)=
c  (0.01)**(-1) (0.01 is an estimate for the standard deviation of the
c  error in z(i)). at the same time we calculate the mean and maximum
c  errors for the data values.
      sum = 0.
      ermax = 0.
      do 20 i=1,m1
         w(i) = 0.1e03
         exact(i) = testpo(x(i),y(i))
         error = abs(z(i)-exact(i))
         sum = sum+error
         if(error.gt.ermax) ermax = error
  20  continue
      ai = m1
      sum = sum/ai
      write(6,920) sum,ermax
c  we set up the dimension information
      nuest = 15
      nvest = 19
      lwrk1 = 15000
      lwrk2 = 5700
      kwrk = 500
c  we choose a value for eps
      eps = 0.1e-05
c  main loop for the different spline approximations
      do 400 is=1,5
        go to (110,120,130,140,160),is
c  we determine a number of smoothing spline approximations on the unit
c  disk x**2+y**2 <= 1.
c  all the data points are considered.
 110    m = m1
c  we set up the smoothing factor.
        s = 1500.
c  the approximations are not restricted at the boundaries of the disk
        iopt(3) = 0
c  we request c2-continuity at the origin.
        iopt(2) = 2
c  at the first call of polar iopt(1) must be zero.
        iopt(1) = 0
        go to 200
c  iopt(1) = 1 from the second call on
 120    iopt(1) = 1
        s = 200.
        go to 200
 130    s = 170.
        go to 200
c  we determine a smoothing spline approximation on the ellips
c  3*x**2+3*y**2-4*x*y<=1.
c  we only consider the data points inside this domain.
 140    m = m2
        ai = m
c  the given function has a constant value 0.4 at the boundary of the
c  ellips. we calculate new data values by substracting this constant
c  from the old ones.
        do 150 i=1,m
          z(i) = z(i)-0.4
 150    continue
c  given these data we will then determine approximations which are
c  identically zero at the boundary of the ellips.
        iopt(3) = 1
c  we still request c2-continuity at the origin.
        iopt(2) = 2
c  reinitialization for the knots.
        iopt(1) = 0
c  we set up the smoothing factor.
        s = 90.
        go to 250
c  at the last call we will determine the least-squares spline
c  approximation corresponding to the current set of knots
 160    iopt(1) = -1
        go to 250
c  determination of the spline approximation on the disk
 200    call polar(iopt,m,x,y,z,w,rad1,s,nuest,nvest,eps,nu,tu,
     *   nv,tv,u,v,c,fp,wrk1,lwrk1,wrk2,lwrk2,iwrk,kwrk,ier)
        nc = (nu-4)*(nv-4)
c  we calculate the function values at the different points.
        do 220 i=1,m
            f(i) = evapol(tu,nu,tv,nv,c,rad1,x(i),y(i))
 220    continue
        write(6,925) s
        go to 300
c  determination of the spline approximation on the ellips.
 250    call polar(iopt,m,x,y,z,w,rad2,s,nuest,nvest,eps,nu,tu,
     *   nv,tv,u,v,c,fp,wrk1,lwrk1,wrk2,lwrk2,iwrk,kwrk,ier)
c  we determine the b-spline coefficients for the spline approximations
c  of the given function.
        nc = (nu-4)*(nv-4)
        do 260 i=1,nc
            c(i) = c(i)+0.4
 260    continue
c  we calculate the function values at the different points.
        do 270 i=1,m
            f(i) = evapol(tu,nu,tv,nv,c,rad2,x(i),y(i))
 270    continue
        if(iopt(1).lt.0) go to 280
        write(6,930) s
        go to 300
 280    write(6,935)
 300    write(6,940) fp,ier
        write(6,945) nu
        write(6,950)
        write(6,955) (tu(i),i=1,nu)
        write(6,960) nv
        write(6,950)
        write(6,955) (tv(i),i=1,nv)
        write(6,965)
        write(6,970) (c(i),i=1,nc)
c  we determine mean and maximum errors.
        sum = 0.
        ermax = 0.
        do 350 i=1,m
          error = abs(f(i)-exact(i))
          sum = sum+error
          if(error.gt.ermax) ermax = error
 350    continue
        sum = sum/ai
        write(6,975)
        write(6,980)
        write(6,915)(x(l),y(l),f(l),l=2,m,3)
        write(6,920) sum,ermax
 400  continue
      stop
c  format statements
 900  format(15h1the input data)
 905  format(1h0,3(3x,1hx,6x,1hy,6x,1hz,5x))
 910  format(12f6.3)
 915  format(1h ,3(3f7.3,2x))
 920  format(14h0mean error = ,f7.4,5x,13hmax. error = ,f7.4)
 925  format(38h0smoothing spline on the disk with s =,f5.0)
 930  format(40h0smoothing spline on the ellips with s =,f5.0)
 935  format(35h0least-squares spline on the ellips)
 940  format(27h0sum of squared residuals =,e15.6,5x,12herror flag =,i5)
 945  format(1x,42htotal number of knots in the u-direction =,i3)
 950  format(1x,22hposition of the knots )
 955  format(5x,8f8.4)
 960  format(1x,42htotal number of knots in the v-direction =,i3)
 965  format(23h0b-spline coefficients )
 970  format(5x,8f9.4)
 975  format(33h0spline values at selected points)
 980  format(1h0,3(3x,1hx,6x,1hy,6x,1hf,5x))
      end
      real function rad1(v)
c  function program rad1 defines in polar coordinates, the boundary of
c  the approximation domain  x**2+y**2<=1.
      real v
      rad1 = 1.
      return
      end
      real function rad2(v)
c  function program rad2 defines in polar coordinates, the boundary of
c  the approximation domain  3*x**2+3*y**2-4*x*y<=1.
      real sqrt,sin,v
      rad2 = 1./sqrt(3.-2.*sin(v+v))
      return
      end
      real function testpo(x,y)
c  function program testpo evaluates the test function for the polar
c  package.
      real x,y
      testpo=(x**2+y**2)/((x+y)**2+0.5)
      return
      end


MODULE PS4BPARA
    IMPLICIT NONE
    REAL(KIND=8):: ALPHA=1.5, BETA=0.8, INTEREST=0.04, RHO=0.9
    REAL(KIND=8), DIMENSION(2):: STATES = (/1.0, 0.5/)
    REAL(KIND=8), DIMENSION(2,2):: EMP_MARKOV = TRANSPOSE(RESHAPE((/0.75,0.25,0.25,0.75/),(/2,2/)))
    INTEGER, PARAMETER:: NZ=SIZE(STATES)
    REAL(KIND=8),PARAMETER:: A_MAX=2.0, A_MIN=-0.525, STEP=0.005
    INTEGER, PARAMETER:: NA= (A_MAX-A_MIN)/STEP + 1
    INTEGER:: ZERO_LOC= ABS(A_MIN)/STEP + 1
    INTEGER:: I ! ITER
    REAL(KIND=8), DIMENSION(NA), PARAMETER:: A_GRID= (/(I*STEP, I=1,NA)/) + A_MIN - STEP
    LOGICAL:: POOL= .TRUE. ! BOOLEAN FOR POOLING OR SEPARTING EQUILIBRIUM
END MODULE

MODULE PS4BRES     ! FOR RESULTS
    USE PS4BPARA
    IMPLICIT NONE
    REAL(KIND=8), DIMENSION(NA,NZ):: VFUNC_CLEAN=-1., VFUNC_CLEAN_NEW=-1., VFUNC_DEF=-1, VFUNC_DEF_NEW=-1.
    INTEGER, DIMENSION(NA,NZ):: PFUNC_CLEAN, PFUNC_DEF, DFUNC
    REAL(KIND=8), DIMENSION(NA*NZ*2):: STAT_DIST, STAT_DIST_NEW
    REAL(KIND=8), DIMENSION(NA*NZ*2,NA*NZ*2):: A_TRANSITION_MAT
    REAL(KIND=8), DIMENSION(NA,NZ):: Q= 0.04 ! LIST OF BOND PRICE GIVEN RISK OF DEFAULTING

END MODULE

PROGRAM PS4B ! MAIN PROGRAM
    USE PS4BPARA
    USE PS4BRES
    IMPLICIT NONE
    ! FOR LOOPING
    REAL(KIND=8):: ERROR_VFI, ERROR_CLEAN, ERROR_DEF, ERROR_Q=100. ! INITIAL ERRORS TO BE UPDATED
    REAL(KIND=8):: CRIT_VFI=1e-3, CRIT_Q=1e-3 ! CRITICAL TOLERANCE VALUES
    INTEGER:: SROWIDX, SCOLIDX! SOME INDEXES FOR VARIOUS LOOPS

    PRINT*, "ZERO LOC", A_GRID(ZERO_LOC), NA

    ! MAIN LOOP
    DO WHILE (ERROR_Q>CRIT_Q)
        PRINT*, ERROR_Q, "ERROR_Q"
        PRINT*, "====================================================="
        ! VFI (GIVE A BOOL TO INDICATE POOLING/SEPARATING)
        ERROR_VFI = 100.
        PFUNC_DEF = ZERO_LOC
        PFUNC_CLEAN = ZERO_LOC
        DFUNC = 0 ! THIS ARRAY JUST STORE THE CURRENT DECISION
        VFUNC_DEF = -1.
        VFUNC_CLEAN= -1.
        DO WHILE (ERROR_VFI>CRIT_VFI)  ! START VFI
            CALL BELLMAN_CLEAN(ERROR_CLEAN) !START BELLMAN CLEAN
            CALL BELLMAN_DEFAULT(ERROR_DEF) ! START BELLMAN DEFAULTED
            ERROR_VFI = MAX(ERROR_CLEAN, ERROR_DEF)
        ENDDO ! END VFI
        ! COMPARE IS CLEAN OR DEFAULT IS BETTER AND SET DFUNC

        CALL CREATE_A_TRANSITION_MAT() ! CREATE MARKOV TRANSITION MATRIX FOR
        CALL FIND_STAT_DIST() ! STATIONARY DISTRIBUTION
        ! COMPUTE ERROR AND Q DIFFERENTLY FOR DIFFERENT EQUILIBRIUM
        IF (POOL) THEN
            CALL Q_POOLING(ERROR_Q)
        ELSE
            CALL Q_SEPARATING(ERROR_Q)
        ENDIF
        ! CHECK MARKET CLEARING CONDITIONS
        ! UPDATE Q IF ERROR IS STILL BIG
    ENDDO
    CALL CAL_MOMENTS()
    CALL CONSUM_EQ()
    CALL WRITE_ALL()! WRITE RESULTS FOR PLOTTING USE
END PROGRAM PS4B

! ALL SUBROUTINES
! RULE: ONLY ONE OUTER LOOP PER SUBROUTINE
SUBROUTINE BELLMAN_CLEAN(ERROR_CLEAN)
    USE PS4BPARA
    USE PS4BRES
    IMPLICIT NONE
    REAL(KIND=8), INTENT(OUT):: ERROR_CLEAN
    REAL(KIND=8):: COND_MAX_UTIL, CONSUM, UTIL, NU, DU, CU
    INTEGER:: SROWIDX, SCOLIDX, CHOICEIDX
    DO SCOLIDX = 1, NZ!START BELLMAN CLEAN
        DO SROWIDX=1, NA
            COND_MAX_UTIL = -1e12
            DO CHOICEIDX=1,NA ! LOOP OVER CHOICE OF ASSET PRIME
                CONSUM = A_GRID(SROWIDX) + STATES(SCOLIDX) - Q(SROWIDX, SCOLIDX)* A_GRID(CHOICEIDX)
                IF (CONSUM > 0.) THEN
                    IF (SROWIDX> ZERO_LOC) THEN
                        NU = BETA*SUM(EMP_MARKOV(SCOLIDX,:)*VFUNC_CLEAN(CHOICEIDX,:))
                    ELSE
                        DU = SUM(EMP_MARKOV(SCOLIDX,:)*VFUNC_DEF(ZERO_LOC,:))
                        CU = SUM(EMP_MARKOV(SCOLIDX,:)*VFUNC_CLEAN(CHOICEIDX,:))
                        IF (DU>CU) THEN
                            NU = DU
                        ELSE
                            NU = CU
                        ENDIF
                    ENDIF
                    UTIL = ((CONSUM**(1-ALPHA)-1)/(1-ALPHA))+NU
                    IF (UTIL>COND_MAX_UTIL) THEN
                        PFUNC_CLEAN(SROWIDX, SCOLIDX) = CHOICEIDX
                        COND_MAX_UTIL = UTIL
                    ENDIF
                ENDIF
            ENDDO ! END LOOP CHOICE SPACE FOR ONE STATE
            VFUNC_CLEAN_NEW(SROWIDX, SCOLIDX) = COND_MAX_UTIL
            IF (DU>CU) THEN
                DFUNC(SROWIDX, SCOLIDX) = 1
            ENDIF
        ENDDO
    ENDDO ! END BELLMAN CLEAN
    ERROR_CLEAN = MAXVAL(ABS(VFUNC_CLEAN_NEW - VFUNC_CLEAN))
    VFUNC_CLEAN = VFUNC_CLEAN_NEW
END SUBROUTINE

SUBROUTINE BELLMAN_DEFAULT(ERROR_DEF)
    USE PS4BPARA
    USE PS4BRES
    IMPLICIT NONE
    REAL(KIND=8), INTENT(OUT):: ERROR_DEF
    REAL(KIND=8):: COND_MAX_UTIL, CONSUM, UTIL, NEXT
    INTEGER:: SROWIDX, SCOLIDX, CHOICEIDX

    DO SCOLIDX=1, NZ
        DO SROWIDX= ZERO_LOC, NA ! NOT POSSIBLE TO HAVE BORROWED MONEY FOR DEFAULTED PEOPL
            COND_MAX_UTIL = -1e12
            DO CHOICEIDX= ZERO_LOC, NA ! LOOP OVER CHOICE; AGAIN, NOT POSSIBLE TO HAVE BORROWED MONEY
                CONSUM = A_GRID(SROWIDX) + STATES(SCOLIDX) - Q(SROWIDX, SCOLIDX)* A_GRID(CHOICEIDX)
                IF (CONSUM > 0.) THEN
                    UTIL = ((CONSUM**(1-ALPHA)-1)/(1-ALPHA))+ &
                    BETA*(RHO*SUM(EMP_MARKOV(SCOLIDX,:)*VFUNC_DEF(CHOICEIDX,:)) + &
                    (1-RHO)*SUM(EMP_MARKOV(SCOLIDX,:)*VFUNC_CLEAN(CHOICEIDX,:)))
                    IF (UTIL>COND_MAX_UTIL) THEN
                        PFUNC_DEF(SROWIDX, SCOLIDX) = CHOICEIDX
                        COND_MAX_UTIL = UTIL
                    ENDIF
                ENDIF
            ENDDO ! END LOOPING CHOICE
            VFUNC_DEF_NEW(SROWIDX, SCOLIDX) = COND_MAX_UTIL
        ENDDO
    ENDDO
    ERROR_DEF = MAXVAL(ABS(VFUNC_DEF_NEW - VFUNC_DEF))
    VFUNC_DEF = VFUNC_DEF_NEW
END SUBROUTINE

SUBROUTINE CREATE_A_TRANSITION_MAT()
    USE PS4BPARA
    USE PS4BRES
    IMPLICIT NONE
    INTEGER:: SROWIDX, SCOLIDX, TROWIDX, TCOLIDX, H, AP_CHOICE, EMPIDX, NO_FLAG_COL, STAY_FLAG_COL, START

    ! THE ASSET TRANSITION MATRIX IS ARRANGED AS (CLEAN_EMPLOYED(A1-AN), CLEAN_UNEMP(A1-N), DEF_EMP(1-N), DEF_UNEMP(1-N))
    A_TRANSITION_MAT= 0.
    ! POPULATE MARKOV ASSET MARTRIX
    DO SCOLIDX=1, NZ
        DO H=0, 1
            IF (H==0) THEN
                START=1
            ELSE
                START=ZERO_LOC ! DEFAULTED PEOPLE NEVER REACH LOWER THAN ZERO_LOC
            ENDIF
            DO SROWIDX=START, NA
                TROWIDX = SROWIDX +(SCOLIDX-1)*NA + H*(NA*NZ)
                ! FEELING LIKE I SHOULD MERGE THE POLICY FUNC INTO 1
                IF (H==0) THEN
                    AP_CHOICE = PFUNC_CLEAN(SROWIDX, SCOLIDX)
                ELSE
                    AP_CHOICE = PFUNC_DEF(SROWIDX, SCOLIDX)
                ENDIF
                IF (H==0) THEN ! IF NOT YET DEFAULTED
                    IF (DFUNC(SROWIDX, SCOLIDX)==1) THEN ! IF CHOOSE TO DEFAULT
                        DO EMPIDX=1,NZ
                            TCOLIDX = 2*NA+NA*(EMPIDX-1)+ZERO_LOC ! MUST BE AT ZERO WITH FLAG
                            A_TRANSITION_MAT(TROWIDX, TCOLIDX) = EMP_MARKOV(SCOLIDX, EMPIDX)
                        ENDDO
                    ELSE ! IF CHOOSE NOT TO DEFAULT
                        DO EMPIDX=1, NZ
                            TCOLIDX = NA*(EMPIDX-1) + AP_CHOICE
                            A_TRANSITION_MAT(TROWIDX, TCOLIDX) = EMP_MARKOV(SCOLIDX, EMPIDX)
                        ENDDO
                    ENDIF
                ELSE ! IF ALREADY DEFAULTED
                    DO EMPIDX=1, NZ ! NEXT PERIOD EMPLOYED OR NOT
                        NO_FLAG_COL = NA*(EMPIDX-1) + AP_CHOICE ! WHEN DEFAULT FLAG IS REMOVED - CLEAN AGAIN
                        STAY_FLAG_COL = 2*NA + NA*(EMPIDX-1) + AP_CHOICE ! WHEN DEFAULT FLAG STAYS
                        A_TRANSITION_MAT(TROWIDX, NO_FLAG_COL) = (1-RHO)*EMP_MARKOV(SCOLIDX, EMPIDX)
                        A_TRANSITION_MAT(TROWIDX, STAY_FLAG_COL) = RHO*EMP_MARKOV(SCOLIDX, EMPIDX)
                    ENDDO
                ENDIF
            ENDDO ! END H LOOP
        ENDDO ! END NZ-SCOL LOOP
    ENDDO ! END NA-SROW LOOP
END SUBROUTINE

SUBROUTINE FIND_STAT_DIST()
    USE PS4BPARA
    USE PS4BRES
    IMPLICIT NONE
    REAL(KIND=8):: ERROR_STAT, CRIT_STAT=1e-3

    STAT_DIST= 1/(NA*NZ*2.-FLOAT(ZERO_LOC)*2.) ! SOME COLUMNS MUST BE ZERO
    STAT_DIST_NEW = 0.
    ERROR_STAT=100.
    DO WHILE (ERROR_STAT>CRIT_STAT)
        STAT_DIST_NEW = MATMUL(STAT_DIST, A_TRANSITION_MAT)
        ERROR_STAT = MAXVAL(ABS(STAT_DIST_NEW - STAT_DIST))
        STAT_DIST = STAT_DIST_NEW
    ENDDO
END SUBROUTINE

SUBROUTINE Q_SEPARATING(ERROR_Q)
    USE PS4BPARA
    USE PS4BRES
    IMPLICIT NONE
    REAL(KIND=8), INTENT(INOUT):: ERROR_Q
    REAL(KIND=8):: CRIT_Q = 1e-3, RISK, QZERO, DIFF
    INTEGER:: SROWIDX, SCOLIDX, NEXT_STATE_IDX, CHOICE
    REAL(KIND=8), DIMENSION(NA,NZ):: ERROR_ARR

    ! FIRST CALCUALTE ERROR
    DO SROWIDX=1,NA
        DO SCOLIDX=1, NZ
            RISK = 0.0
            CHOICE = PFUNC_CLEAN(SROWIDX, SCOLIDX)
            DO NEXT_STATE_IDX=1, NZ
                RISK = RISK + EMP_MARKOV(SCOLIDX, NEXT_STATE_IDX)*DFUNC(CHOICE, NEXT_STATE_IDX)
            ENDDO
        QZERO = (1-RISK)/(1+INTEREST)
        DIFF = Q(SROWIDX, SCOLIDX) - QZERO
        ERROR_ARR(SROWIDX, SCOLIDX) = ABS(DIFF)
        IF (ERROR_ARR(SROWIDX, SCOLIDX)> CRIT_Q) THEN    ! NOTE THAT DEFAULTED CANNOT BORROW, NONE OF THEIR BUSINESS
            Q(SROWIDX, SCOLIDX) = 0.9*Q(SROWIDX, SCOLIDX) + 0.1*QZERO
        ENDIF
        ENDDO
    ENDDO
    ERROR_Q = MAXVAL(ABS(ERROR_ARR))

END SUBROUTINE

SUBROUTINE Q_POOLING(ERROR_Q)
    ! SHIFT Q WITH SAME AMOUNT
    USE PS4BPARA
    USE PS4BRES
    IMPLICIT NONE
    REAL(KIND=8), INTENT(INOUT):: ERROR_Q
    REAL(KIND=8):: CRIT_Q = 1e-3, TOTAL_LOSS, TOTAL_BORROW, LOSS_RATE, QZERO, DIFF
    INTEGER:: SROWIDX, SCOLIDX, AP_CHOICE, AP_CHOICE_DEF

    TOTAL_LOSS=0.
    TOTAL_BORROW=0.
    LOSS_RATE=0.
    DO SCOLIDX=1, NZ
        DO SROWIDX= 1, NA
            ! THERE ARE THE DEFAULTED PEOPLE,WHO SAVE, AND THE SAVED MONEY ARE RETNED OUT
            AP_CHOICE = PFUNC_CLEAN(SROWIDX, SCOLIDX)
            TOTAL_BORROW = TOTAL_BORROW + A_GRID(AP_CHOICE)*STAT_DIST(SROWIDX+NA*(SCOLIDX-1))
            IF (SROWIDX>ZERO_LOC) THEN
                AP_CHOICE_DEF = PFUNC_DEF(SROWIDX, SCOLIDX)
                TOTAL_BORROW = TOTAL_BORROW + A_GRID(AP_CHOICE_DEF)*STAT_DIST(SROWIDX+NA*(SCOLIDX-1)+2*NA)
            ENDIF

            IF (DFUNC(SROWIDX, SCOLIDX)==1) THEN !
                TOTAL_LOSS =  TOTAL_LOSS -  A_GRID(SROWIDX)*STAT_DIST(SROWIDX+NA*(SCOLIDX-1))
            ENDIF
        ENDDO
    ENDDO
    ! UPDATE LOSS RATE
    LOSS_RATE= ABS(TOTAL_LOSS/TOTAL_BORROW)
    QZERO = (1-LOSS_RATE)/(1+INTEREST)
    DIFF = Q(1,1) -QZERO
    PRINT*, LOSS_RATE, TOTAL_LOSS, TOTAL_BORROW, SUM(DFUNC),"TOTT"
    ERROR_Q = ABS((Q(1,1)-QZERO)) ! NO NEED TO UPDATE INTEREST
    ! UPDATE Q
    IF (ERROR_Q>CRIT_Q) THEN
        Q = 0.9*Q(1,1)+QZERO*0.1
    ENDIF
END SUBROUTINE

SUBROUTINE WRITE_ALL()
    USE PS4BPARA
    USE PS4BRES
    IMPLICIT NONE
    INTEGER:: SROWIDX
    INTEGER, DIMENSION(4):: SIDX
    CHARACTER(LEN=20):: SUFFIX
    CHARACTER(LEN=130):: PATH="./"
    CHARACTER(LEN=150):: FILE_NAME

        IF (POOL) THEN
            SUFFIX="_POOL"
        ELSE
            SUFFIX="_SEP"
        ENDIF

        FILE_NAME = TRIM(PATH)//"VFUNC"
        OPEN(UNIT=1, FILE=FILE_NAME, STATUS='REPLACE') ! START WITH THE TWO VALUE FUNCTIONS
        DO SROWIDX=1, NA
            WRITE(UNIT=1,FMT=*) VFUNC_CLEAN(SROWIDX,:)
        ENDDO
        CLOSE(UNIT=1)

        FILE_NAME = TRIM(PATH)//"PFUNC"
        OPEN(UNIT=2, FILE=FILE_NAME, STATUS='REPLACE') ! ALSO SAVE POLICY FUNCTIONS
        DO SROWIDX=1, NA
            WRITE(UNIT=2,FMT=*) PFUNC_CLEAN(SROWIDX,:)
        ENDDO
        CLOSE(UNIT=2)

        FILE_NAME = TRIM(PATH)//"STATDIST"
        OPEN(UNIT=3, FILE=FILE_NAME, STATUS='REPLACE') ! ALSO SAVE POLICY FUNCTIONS
        DO SROWIDX=1, NA
            SIDX = (/SROWIDX, SROWIDX+NA, SROWIDX+2*NA, SROWIDX+3*NA/)
            WRITE(UNIT=3,FMT=*) STAT_DIST(SIDX)
        ENDDO
        CLOSE(UNIT=3)

        FILE_NAME = TRIM(PATH)//"AGRID"
        OPEN(UNIT=4, FILE=FILE_NAME, STATUS='REPLACE') ! FOR HAVING THE X-AXIS OF PLOT
        WRITE(UNIT=4,FMT=*) A_GRID
        CLOSE(UNIT=4)

        FILE_NAME = TRIM(PATH)//"VFUND"
        OPEN(UNIT=5, FILE=FILE_NAME, STATUS='REPLACE') ! START WITH THE TWO VALUE FUNCTIONS
        DO SROWIDX=1, NA
            WRITE(UNIT=5,FMT=*) VFUNC_DEF(SROWIDX,:)
        ENDDO
        CLOSE(UNIT=5)

        FILE_NAME = TRIM(PATH)//"PFUND"
        OPEN(UNIT=6, FILE=FILE_NAME, STATUS='REPLACE') ! ALSO SAVE POLICY FUNCTIONS
        DO SROWIDX=1, NA
            WRITE(UNIT=6,FMT=*) PFUNC_DEF(SROWIDX,:)
        ENDDO
        CLOSE(UNIT=6)

        FILE_NAME = TRIM(PATH)//"Q"
        OPEN(UNIT=7, FILE=FILE_NAME, STATUS='REPLACE') ! ALSO SAVE POLICY FUNCTIONS
        DO SROWIDX=1, NA
            WRITE(UNIT=7,FMT=*) Q(SROWIDX,:)
        ENDDO
        CLOSE(UNIT=7)
END SUBROUTINE

SUBROUTINE CAL_MOMENTS()
    USE PS4BPARA
    USE PS4BRES
    IMPLICIT NONE
    REAL(KIND=8):: DEFAULT_AMOUNT=0., DEFAULT_RATE=0., CUR_DIST, AVG_SAVING=0., AVG_LOAN=0., AVG_INC=0.
    INTEGER:: RIDX, CIDX, HIDX

    DO RIDX=1,NA
        DO CIDX=1, NZ
            DO HIDX=1,2
                CUR_DIST = STAT_DIST((CIDX-1)*NA+RIDX+(HIDX-1)*2*NA)
                IF (RIDX>ZERO_LOC) THEN
                    AVG_SAVING = AVG_SAVING+ CUR_DIST*A_GRID(RIDX)
                ELSE
                    AVG_LOAN = AVG_LOAN + CUR_DIST*A_GRID(RIDX)
                ENDIF
                IF (DFUNC(RIDX, CIDX)==1) THEN
                    IF (HIDX==1) THEN
                        DEFAULT_AMOUNT = DEFAULT_AMOUNT+CUR_DIST*A_GRID(RIDX)
                    ENDIF
                ENDIF
            ENDDO
        ENDDO
    ENDDO
    DEFAULT_RATE = SUM(STAT_DIST(2*NA+1:4*NA))
    AVG_INC = SUM(STAT_DIST(1:NA))*1+SUM(STAT_DIST(NA+1:2*NA))*0.5+SUM(STAT_DIST(NA*2+1:3*NA))+SUM(STAT_DIST(3*NA+1:4*NA))*0.5
    PRINT*, "AVG INCOME", AVG_INC
    PRINT*, "AVG SAVING", AVG_SAVING
    PRINT*, "AVG LOAN", AVG_LOAN
    PRINT*, "AVG DEFAULT RATE", DEFAULT_RATE
   PRINT*, "AVG AMOUNT OF DEFAULT", DEFAULT_AMOUNT
    PRINT*, "AVG BOND PRICE", SUM(Q)/SIZE(Q)
END SUBROUTINE

SUBROUTINE CONSUM_EQ()
    USE PS4BRES
    USE PS4BPARA
    IMPLICIT NONE
    INTEGER:: RIDX, CIDX
    REAL(KIND=8), DIMENSION(NA,NZ):: VFUNC_CLEAN_SEP, VFUNC_DEF_SEP, CONS_EQ_CLEAN, CONS_EQ_DEF

    IF (POOL) THEN ! ONLY RUN THIS WHEN NOW IS POOL AND PREVIOUS RUN IS SEPARTE
        OPEN(UNIT=21, FILE='VFUNC', STATUS='OLD')
            DO RIDX=1,NA
                READ(UNIT=21, FMT=*) VFUNC_CLEAN_SEP(RIDX,:)
            ENDDO
        CLOSE(UNIT=21)
        OPEN(UNIT=22, FILE='VFUND', STATUS='OLD')
            DO RIDX=1,NA
                READ(UNIT=22, FMT=*) VFUNC_DEF_SEP(RIDX,:)
            ENDDO
        CLOSE(UNIT=22)
            DO RIDX=1, NA
                DO CIDX=1, NZ
                    CONS_EQ_CLEAN(RIDX, CIDX) = VFUNC_CLEAN(RIDX, CIDX) - VFUNC_CLEAN_SEP(RIDX, CIDX)
                    CONS_EQ_DEF(RIDX, CIDX) = VFUNC_DEF(RIDX, CIDX) - VFUNC_DEF_SEP(RIDX, CIDX)
                ENDDO
            ENDDO

        OPEN(UNIT=23, FILE='CONSUM_EQ', STATUS='REPLACE')
            DO RIDX=1,NA
                WRITE(UNIT=23, FMT=*) CONS_EQ_CLEAN(RIDX,:), CONS_EQ_DEF(RIDX,:)
            ENDDO
        CLOSE(UNIT=23)
    ENDIF
END SUBROUTINE


      double precision function vol_wt(W)
      implicit none
      include 'constants.f'
      include 'masses.f'
      include 'process.f'
      logical first
      double precision W,eval_int,volsave
      data first/.true./
      save first,volsave

      if (case .eq. 'vlchwh') then
c--- volume for production of Wtg
      if (first) then
        volsave=eval_int(W,mt,wmass)/twopi**5
        first=.false.
      endif
      vol_wt=volsave
      vol_wt=vol_wt*(wmass*wwidth/2d0)
      vol_wt=vol_wt/8d0/pi
      vol_wt=vol_wt*(mt*twidth/2d0)
      vol_wt=vol_wt*pi*(mt**2-wmass**2)/2d0/mt**2
     .    /twopi**2
      vol_wt=vol_wt*(wmass*wwidth/2d0)
      vol_wt=vol_wt/8d0/pi
      return
      endif

c--- volume for W+t production
      vol_wt=pi*dsqrt((W-mt**2-wmass**2)**2-4d0*mt**2*wmass**2)/2d0/W
     .      /twopi**2
      vol_wt=vol_wt*(wmass*wwidth/2d0)
      vol_wt=vol_wt/8d0/pi

      if (case .eq. 'vlchwn') return

      if (case .eq. 'vlchwt') then
c--- extra volume for decay t->Wb
      vol_wt=vol_wt*(mt*twidth/2d0)
      vol_wt=vol_wt*pi*(mt**2-wmass**2)/2d0/mt**2
     .    /twopi**2
      vol_wt=vol_wt*(wmass*wwidth/2d0)
      vol_wt=vol_wt/8d0/pi
      endif

      if (case .eq. 'vlchwg') then
c--- extra volume for decay t->Wbg
      vol_wt=vol_wt*(mt*twidth/2d0)
      vol_wt=vol_wt*pisq/8d0/mt**2/twopi**5
     .   *(mt**4-wmass**4-2d0*mt**2*wmass**2*dlog(mt**2/wmass**2))
      vol_wt=vol_wt*(wmass*wwidth/2d0)
      vol_wt=vol_wt/8d0/pi
      endif

      return
      end

      double precision function eval_int(s,mt,mw)
      implicit none
      integer j,n
      double precision s,mt,mw,dx,fun_int,part,min

      n=1000000
      dx=(s-(mt+mw)**2)/dfloat(n)
      min=(mt+mw)**2

      eval_int=0d0
      do j=1,n
      part=dx*(fun_int(s,mt,mw,min+dfloat(j)*dx)
     .        +fun_int(s,mt,mw,min+dfloat(j-1)*dx))/2d0
      eval_int=eval_int+part
      enddo

      return
      end

      double precision function fun_int(s,mt,mw,x)
      implicit none
      include 'constants.f'
      double precision s,mt,mw,x,root

      root=(x-mt**2-mw**2)**2-4d0*mt**2*mw**2
      if (root .lt. 0d0) root=0d0

      fun_int=pi*pion4/s*(s-x)*dsqrt(root)/x

      return
      end


*       Example 5.2

*        Writ a program to find the lowest eigen values
*        of stretched string by the shooting method
*        described above
*	  d^2 { Phi(r)}
*	 -------------  = -K^2*Phi  , Phi(x=0)=Phi(x=1)=0
*         dx^2

         real K
         K=1
         DK=1.0
         TOLK=1.E-05
         Call INTGRT(K,PHIP)
         PHIOLD=PHIP
10       continue
         K=K+DK
         Call INTGRT(K,PHIP)
         If (PHIP*PHIOLD.LT.0)Then
         K=K-DK
         DK=DK/2
         End If
         If(ABS(DK).GT.TOLK)goto 10
         Exact=4.0*ATAN(1.0)
         Print*,'eigenvalue, error=',K,Exact-K
         stop
          end
         Subroutine INTGRT(K,PHIP)
         Real K
         DATA Nstep/100/
         H=1.0/Nstep
         Phim=0.0
         Phiz=0.01
         const=(k*H)**2/12
         do 100 Ix=1,Nstep-1
         Phip=2*(1.0-5.0*const)*Phiz-(1+const)*Phim
         phip=phip/(1+const)
         Phim=Phiz
         Phiz=Phip
100      continue
         Print*,K,PHIP
         return
         end


subroutine cic_deconvolve(dm_density_real, dm_density_imag, lo, hi, domain_size)
  use, intrinsic :: iso_c_binding
  implicit none

  integer(c_int), intent(in) :: lo(3), hi(3), domain_size(3)
  real(c_double), intent(inout) :: dm_density_real (lo(1):hi(1), lo(2):hi(2), lo(3):hi(3), 1)
  real(c_double), intent(inout) :: dm_density_imag (lo(1):hi(1), lo(2):hi(2), lo(3):hi(3), 1)

  integer(c_int) :: ki_nyquist_x, ki_nyquist_y, ki_nyquist_z

  real(c_double) :: pi
  real(c_double) :: sinc_arg_x, sinc_arg_y, sinc_arg_z
  real(c_double) :: sinc_arg_factor_x, sinc_arg_factor_y, sinc_arg_factor_z
  real(c_double) :: sinc_x, sinc_y, sinc_z

  integer(c_int) :: i, j, k
  integer(c_int) :: ki_x, ki_y, ki_z
  real(c_double) :: sinc_prod, cic_ft

  pi = acos(-1.0)

  ! Copied straight from old gimlet. Will this still work if the grid
  ! size is odd?
  ki_nyquist_x = domain_size(1) / 2
  ki_nyquist_y = domain_size(2) / 2
  ki_nyquist_z = domain_size(3) / 2

  sinc_arg_factor_x = pi / (2.0 * real(ki_nyquist_x, c_double))
  sinc_arg_factor_y = pi / (2.0 * real(ki_nyquist_y, c_double))
  sinc_arg_factor_z = pi / (2.0 * real(ki_nyquist_z, c_double))

  do k = lo(3), hi(3)

    if (k <= ki_nyquist_z) then
      ki_z = k
    else
      ki_z = domain_size(3) - k
    end if
    if (ki_z == 0) then
      sinc_z = 1.0
    else
      sinc_arg_z = ki_z * sinc_arg_factor_z
      sinc_z = sin(sinc_arg_z) / sinc_arg_z
    end if

    do j = lo(2), hi(2)

      if (j <= ki_nyquist_y) then
        ki_y = j
      else
        ki_y = domain_size(2) - j
      end if
      if (ki_y == 0) then
        sinc_y = 1.0
      else
        sinc_arg_y = ki_y * sinc_arg_factor_y
        sinc_y = sin(sinc_arg_y) / sinc_arg_y
      end if

      do i = lo(1), hi(1)

        if (i <= ki_nyquist_x) then
          ki_x = i
        else
          ki_x = domain_size(1) - i
        end if
        if (ki_x == 0) then
          sinc_x = 1.0
        else
          sinc_arg_x = ki_x * sinc_arg_factor_x
          sinc_x = sin(sinc_arg_x) / sinc_arg_x
        end if

        sinc_prod = sinc_x * sinc_y * sinc_z
        cic_ft = sinc_prod**2

        dm_density_real (i, j, k, 1) = dm_density_real (i, j, k, 1) / cic_ft
        dm_density_imag (i, j, k, 1) = dm_density_imag (i, j, k, 1) / cic_ft
      end do
    end do
  end do

end subroutine cic_deconvolve


c subroutine lfin (init,theta,l,m,nm,pb,id,wlfin)
c
c dimension of           theta(l),  pb(id,nm+1),  wlfin(4*l*(nm+1))
c arguments
c
c purpose                given m and l, routine lfin calculates
c                        the normalized associated legendre functions
c                        pbar(n,m,theta) for n=m,...,nm and theta(i)
c                        for i=1,...,l where
c
c                        pbar(m,n,theta) = sqrt((2*n+1)*factorial(n-m)
c                        /(2*factorial(n+m)))*sin(theta)**m/(2**n*
c                        factorial(n)) times the (n+m)th derivative of
c                        (x**2-1)**n with respect to x=cos(theta)
c
c usage                  call lfin (init,theta,l,m,nm,pb,id,wlfin)
c
c arguments
c on input               init
c                        = 0
c                            initialization only - using parameters
c                            l, nm and the array theta, subroutine lfin
c                            initializes the array wlfin for subsequent
c                            use in the computation of the associated
c                            legendre functions pb. initialization does
c                            not have to be repeated unless l, nm or
c                            the array theta are changed.
c                        = 1
c                            subroutine lfin uses the array wlfin that
c                            was computed with init = 0 to compute pb
c
c                        theta
c                          an array that contains the colatitudes
c                          at which the associated legendre functions
c                          will be computed. the colatitudes must be
c                          specified in radians.
c
c                        l
c                          the length of the theta array. lfin is
c                          vectorized with vector length l.
c
c                        m
c                          nonnegative integer, less than nm, specifying
c                          degree of pbar(n,m,theta). subroutine lfin
c                          must be called starting with n=0. n must be
c                          incremented by one in subsequent calls and
c                          must not exceed nm.
c
c                        nm
c                          the maximum value of n and m
c
c                        id
c                          the first dimension of the two dimensional
c                          array pb as it appears in the program that
c                          calls lfin. (see output parameter pb)
c
c                        wlfin
c                          an array with length 4*l*(nm+1) which
c                          must be initialized by calling lfin
c                          with init=0 (see parameter init)  it
c                          must not be altered between calls to
c                          lfin.
c
c
c on output              pb
c                          a two dimensional array with first
c                          dimension id in the program that calls
c                          lfin. the second dimension of pb must
c                          be at least nm+1. starting with m=0
c                          lfin is called repeatedly with m being
c                          increased by one between calls. on each
c                          call, subroutine lfin computes pb(i,n+1)
c                          = pbar(m,n,theta(i)) for n=m,...,nm and
c                          i=1,...l.
c
c                        wlfin
c                          array containing values which must not
c                          be altered unless l, nm or the array theta
c                          are changed in which case lfin must be
c                          called with init=0 to reinitialize the
c                          wlfin array.
c
c special conditions     m must be increased by one between calls
c                        of lfin in which m is not zero.
c
c precision              single
c
c algorithm              routine lfin calculates pbar(n,m,theta) using
c                        a four term recurrence relation. (unpublished
c                        notes by paul n. swarztrauber)
c
      SUBROUTINE DLFIN(INIT,THETA,L,M,NM,PB,ID,WLFIN)
      DOUBLE PRECISION THETA
      DOUBLE PRECISION PB
      DOUBLE PRECISION WLFIN
      DIMENSION PB(1),WLFIN(1)
c
c     total length of wlfin is 4*l*(nm+1)
c
      LNX = L* (NM+1)
      IW1 = LNX + 1
      IW2 = IW1 + LNX
      IW3 = IW2 + LNX
      CALL DLFIN1(INIT,THETA,L,M,NM,ID,PB,WLFIN,WLFIN(IW1),WLFIN(IW2),
     +           WLFIN(IW3),WLFIN(IW2))
      RETURN
      END
      SUBROUTINE DLFIN1(INIT,THETA,L,M,NM,ID,P3,PHZ,PH1,P1,P2,CP)
      DOUBLE PRECISION THETA
      DOUBLE PRECISION P3
      DOUBLE PRECISION PHZ
      DOUBLE PRECISION PH1
      DOUBLE PRECISION P1
      DOUBLE PRECISION P2
      DOUBLE PRECISION CP
      DOUBLE PRECISION SSQRT2
      DOUBLE PRECISION FM
      DOUBLE PRECISION TM
      DOUBLE PRECISION TEMP
      DOUBLE PRECISION CC
      DOUBLE PRECISION EE
      DOUBLE PRECISION FN
      DOUBLE PRECISION TN
      DOUBLE PRECISION CN
      DOUBLE PRECISION FNPM
      DOUBLE PRECISION FNMM
      DOUBLE PRECISION DD
      DIMENSION P1(L,1),P2(L,1),P3(ID,1),PHZ(L,1),PH1(L,1),CP(1),
     +          THETA(1)

      NMP1 = NM + 1
      IF (INIT.NE.0) GO TO 5
      SSQRT2 = 1.D0/SQRT(2.D0)
      DO 10 I = 1,L
          PHZ(I,1) = SSQRT2
   10 CONTINUE
      DO 15 NP1 = 2,NMP1
          NH = NP1 - 1
          CALL DALFK(NH,0,CP)
          DO 16 I = 1,L
              CALL DLFPT(NH,0,THETA(I),CP,PHZ(I,NP1))
   16     CONTINUE
          CALL DALFK(NH,1,CP)
          DO 17 I = 1,L
              CALL DLFPT(NH,1,THETA(I),CP,PH1(I,NP1))
   17     CONTINUE
   15 CONTINUE
      RETURN
    5 MP1 = M + 1
      FM = DBLE(M)
      TM = FM + FM
      IF (M-1) 25,30,35
   25 DO 45 NP1 = 1,NMP1
          DO 45 I = 1,L
              P3(I,NP1) = PHZ(I,NP1)
              P1(I,NP1) = PHZ(I,NP1)
   45 CONTINUE
      RETURN
   30 DO 50 NP1 = 2,NMP1
          DO 50 I = 1,L
              P3(I,NP1) = PH1(I,NP1)
              P2(I,NP1) = PH1(I,NP1)
   50 CONTINUE
      RETURN
   35 TEMP = TM* (TM-1.D0)
      CC = SQRT((TM+1.D0)* (TM-2.D0)/TEMP)
      EE = SQRT(2.D0/TEMP)
      DO 85 I = 1,L
          P3(I,M+1) = CC*P1(I,M-1) - EE*P1(I,M+1)
   85 CONTINUE
      IF (M.EQ.NM) RETURN
      TEMP = TM* (TM+1.D0)
      CC = SQRT((TM+3.D0)* (TM-2.D0)/TEMP)
      EE = SQRT(6.D0/TEMP)
      DO 70 I = 1,L
          P3(I,M+2) = CC*P1(I,M) - EE*P1(I,M+2)
   70 CONTINUE
      MP3 = M + 3
      IF (NMP1.LT.MP3) GO TO 80
      DO 75 NP1 = MP3,NMP1
          N = NP1 - 1
          FN = DBLE(N)
          TN = FN + FN
          CN = (TN+1.D0)/ (TN-3.D0)
          FNPM = FN + FM
          FNMM = FN - FM
          TEMP = FNPM* (FNPM-1.D0)
          CC = SQRT(CN* (FNPM-3.D0)* (FNPM-2.D0)/TEMP)
          DD = SQRT(CN*FNMM* (FNMM-1.D0)/TEMP)
          EE = SQRT((FNMM+1.D0)* (FNMM+2.D0)/TEMP)
          DO 75 I = 1,L
              P3(I,NP1) = CC*P1(I,NP1-2) + DD*P3(I,NP1-2) - EE*P1(I,NP1)
   75 CONTINUE
   80 DO 90 NP1 = M,NMP1
          DO 90 I = 1,L
              P1(I,NP1) = P2(I,NP1)
              P2(I,NP1) = P3(I,NP1)
   90 CONTINUE
      RETURN
      END


      subroutine velocityfactors(R,z,vfacr,vfacz)

      common /blackhole/ bhmass
      real pot

      rsph = sqrt(R*R+z*z)
      call force(R,z,fr,fz,pot)
      frbh = -bhmass*R/(rsph**3.)
      fzbh = -bhmass*z/(rsph**3.)
      vfacr = sqrt(abs((fr+frbh)/fr))
      vfacz = sqrt(abs((fz+fzbh)/fz))
      if(vfacr.lt.1.or.vfacz.lt.1) then
         write(0,*) vfacr,vfacz,fr,fz
         stop
      endif
      return
      end


      program test
      real*16 sum,sum2,rmsd,energy,ave
      character*16 fname

      ave=0.0
      rmsd=0.0
      sum=0.0
      sum2=0.0
      read(5,*)timestep,nline
      read(5,*)fname

      open(10,file=fname)

      read(10,*)
      do i=1,nline
         read(10,20)energy
         sum=sum+energy
         sum2=sum2+energy**2
      enddo
      close(10)
 20   format(5x,f12.6)

c      write(6,*)sum**2/nline,sum2
      ave=sum/nline
      rmsd=(sum2-sum*sum/nline)/nline
      rmsd=sqrt(rmsd)

      write(6,10)timestep,rmsd,ave
 10   format(3f16.7)
      end


      SUBROUTINE MC01OD( K, REZ, IMZ, REP, IMP, DWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To compute the coefficients of a complex polynomial P(x) from its
C     zeros.
C
C     ARGUMENTS
C
C     Input/Output Parameters
C
C     K       (input) INTEGER
C             The number of zeros (and hence the degree) of P(x).
C             K >= 0.
C
C     REZ     (input) DOUBLE PRECISION array, dimension (K)
C     IMZ     (input) DOUBLE PRECISION array, dimension (K)
C             The real and imaginary parts of the i-th zero of P(x)
C             must be stored in REZ(i) and IMZ(i), respectively, where
C             i = 1, 2, ..., K. The zeros may be supplied in any order.
C
C     REP     (output) DOUBLE PRECISION array, dimension (K+1)
C     IMP     (output) DOUBLE PRECISION array, dimension (K+1)
C             These arrays contain the real and imaginary parts,
C             respectively, of the coefficients of P(x) in increasing
C             powers of x. If K = 0, then REP(1) is set to one and
C             IMP(1) is set to zero.
C
C     Workspace
C
C     DWORK   DOUBLE PRECISION array, dimension (2*K+2)
C             If K = 0, this array is not referenced.
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value.
C
C     METHOD
C
C     The routine computes the coefficients of the complex K-th degree
C     polynomial P(x) as
C
C        P(x) = (x - r(1)) * (x - r(2)) * ... * (x - r(K))
C
C     where r(i) = (REZ(i),IMZ(i)), using real arithmetic.
C
C     NUMERICAL ASPECTS
C
C     None.
C
C     CONTRIBUTORS
C
C     Release 3.0: V. Sima, Katholieke Univ. Leuven, Belgium, Mar. 1997.
C     Supersedes Release 2.0 routine MC01CD by Alan Brown and
C     A.J. Geurts.
C
C     REVISIONS
C
C     V. Sima, May 2002.
C
C     KEYWORDS
C
C     Elementary polynomial operations, polynomial operations.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE
      PARAMETER         ( ZERO = 0.0D0, ONE = 1.0D0 )
C     .. Scalar Arguments ..
      INTEGER           INFO, K
C     .. Array Arguments ..
      DOUBLE PRECISION  DWORK(*), IMP(*), IMZ(*), REP(*), REZ(*)
C     .. Local Scalars ..
      INTEGER           I, K2
      DOUBLE PRECISION  U, V
C     .. External Subroutines ..
      EXTERNAL          DAXPY, DCOPY, XERBLA
C     .. Executable Statements ..
C
C     Test the input scalar arguments.
C
      IF( K.LT.0 ) THEN
         INFO = -1
C
C        Error return.
C
         CALL XERBLA( 'MC01OD', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      INFO   = 0
      REP(1) = ONE
      IMP(1) = ZERO
      IF ( K.EQ.0 )
     $   RETURN
C
      K2 = K + 2
C
      DO 20 I = 1, K
         U = REZ(I)
         V = IMZ(I)
         DWORK(1)  = ZERO
         DWORK(K2) = ZERO
         CALL DCOPY( I, REP, 1, DWORK(2), 1 )
         CALL DCOPY( I, IMP, 1, DWORK(K2+1), 1 )
C
         IF ( U.NE.ZERO ) THEN
            CALL DAXPY( I, -U, REP, 1, DWORK, 1 )
            CALL DAXPY( I, -U, IMP, 1, DWORK(K2), 1 )
         END IF
C
         IF ( V.NE.ZERO ) THEN
            CALL DAXPY( I,  V, IMP, 1, DWORK, 1 )
            CALL DAXPY( I, -V, REP, 1, DWORK(K2), 1 )
         END IF
C
         CALL DCOPY( I+1, DWORK, 1, REP, 1 )
         CALL DCOPY( I+1, DWORK(K2), 1, IMP, 1 )
   20 CONTINUE
C
      RETURN
C *** Last line of MC01OD ***
      END