! Momentum flux budget stuff:

subroutine stat_mom(du,momle)

use vars
implicit none
real du(nx,ny,nz,3)
real momle(nz,3),coef
integer i,j,k,jb,jc
coef = 1./float(nx*ny)
do k=1,nz
 momle(k,1)=0.
 momle(k,2)=0.
 momle(k,3)=0.
end do
do j=1,ny
 do i=1,nx
   du(i,j,nz,3) = 0.
 end do
end do
do k=1,nzm
 do j=1,ny
  jb=j-YES3D
  jc=j+YES3D
  do i=1,nx
   momle(k,1)=momle(k,1)+ &
          0.25*(w(i,j,k)+w(i,j,k+1)+w(i-1,j,k)+w(i-1,j,k+1))*du(i,j,k,1)+ &
          0.25*(u(i,j,k)+u(i+1,j,k)-2.*u0(k))*(du(i,j,k,3)+du(i,j,k+1,3))
   momle(k,2)=momle(k,2)+ &
          0.25*(w(i,j,k)+w(i,j,k+1)+w(i,jb,k)+w(i,jb,k+1))*du(i,j,k,2)+ &
          0.25*(v(i,j,k)+v(i,jc,k)-2.*v0(k))*(du(i,j,k,3)+du(i,j,k+1,3))
   momle(k,3)=momle(k,3)+w(i,j,k)*du(i,j,k,3)
  end do
 end do
 momle(k,1)=momle(k,1)*coef
 momle(k,2)=momle(k,2)*coef
 momle(k,3)=momle(k,3)*coef
end do
do k=1,nzm
  momle(k,3)=0.5*(momle(k,3)+momle(k+1,3))
end do
end


MODULE Euler

  USE SIMPLE
  USE SPECIAL
  USE GRID

  IMPLICIT NONE

CONTAINS

  !> @brief Computes the inviscid fluxes in one dimension
  !>
  !> This routine computes the inviscid fluxes of the single gas NS in one dimension.
  !> For example in the \f$\hat{\xi}\f$ direction, the inviscid flux is:
  !> \f[
  !> \hat{\vec{F}}^I_1 =
  !> \begin{bmatrix} \rho \hat{U} \\ \rho u \hat{U} + p\hat{\xi}_x \\ \rho v \hat{U} + p \hat{\xi}_y \\ \rho w \hat{U} + p\hat{\xi}_z \\ (\rho E + p)\hat{U} - \hat{\xi}_t p \end{bmatrix},
  !> \f]
  !> @param numDim - const integer indicating number of dimensions
  !> @param numPoints - const 64-bit integer indicating total size of input arrays
  !> @param gridSizes - const 64-bit integer 1d array of size @em numDim, which indicates the number of points in each dimension for the grid
  !> @param opInterval - const 64-bit integer 1d array of size 2 x @em numDim which indicates the rectangular interval over which this routine should operate.  e.g. [@em iStart @em iEnd @em jStart @em jEnd ... ]
  !> @param fluxDir - const integer indicating the direction in which the flux is to be calculated
  !> @param gridType - const integer indicating whether the @em gridMetric parameter is a Cartesian, Rectilinear, or Curvilinear metric
  !> @param gridMetric - const double precision 1d array containing the grid metric
  !> @param rhoBuffer - (\f$\rho\f$) const double precision contiguous 1d array of size @em numPoints representing the @em density at each point on the grid
  !> @param rhoVBuffer - (\f$\rho\vec{V}\f$) const double precision contiguous 1d array of size @em numDim x @em numPoints representing the @em momentum density at each point on the grid.
  !> @param rhoEBuffer - (\f$\rho E\f$) const double precision contiguous 1d array of size @em numPoints representing the energy density at each point on the grid
  !> @param velHat - (\f$\hat{U}\f$) - const double precision contiguous 1d array of size @em numPoints representing the component of the contravariant velocity corresponding to the direction (@em fluxDir) in which the fluxes are to be calculated
  !> @param pressureBuffer - (p) - const double precision contiguous 1d array of size @em numPoints representing the @m pressure at each point on the grid
  !> @param fluxBuffer - double precision contiguous 1d array of size (@em numDim + 2) x @em numPoints for output of the fluxes for each equation. The @em fluxBuffer is returned with (@em numDim + 2) blocks of size @em numPoints, one block for each equation.
  SUBROUTINE Flux1D(                                &
       numDim, numPoints, gridSizes, opInterval,    &
       fluxDir, gridType, gridMetric,               &
       rhoBuffer,rhoVBuffer,rhoEBuffer,velHat,   &
       pressureBuffer, fluxBuffer)

    IMPLICIT NONE

    INTEGER(KIND=4), INTENT(IN)         :: numDim, fluxDir, gridType
    INTEGER(KIND=8), INTENT(IN)         :: gridSizes(numDim),  numPoints
    INTEGER(KIND=8), INTENT(IN)         :: opInterval(2*numDim)
    REAL(KIND=8),    INTENT(IN), TARGET :: gridMetric(numDim*numDim*numPoints)
    REAL(KIND=8),    INTENT(IN)         :: rhoBuffer(numPoints)
    REAL(KIND=8),    INTENT(IN), TARGET :: rhoVBuffer(numDim*numPoints)
    REAL(KIND=8),    INTENT(IN)         :: rhoEBuffer(numPoints)
    REAL(KIND=8),    INTENT(IN)         :: velHat(numPoints)
    REAL(KIND=8),    INTENT(IN)         :: pressureBuffer(numPoints)
    REAL(KIND=8),    INTENT(OUT),TARGET :: fluxBuffer(numPoints*(numDim+2))

    INTEGER         :: iDim, numComponents, iVelDim
    INTEGER(KIND=8) :: iPoint,iX,iY,iZ,iStart,iEnd,jStart,jEnd,kStart,kEnd
    INTEGER(KIND=8) :: xIndex,zIndex,yIndex,yzIndex,xSize,ySize,zSize,fluxOffset
    INTEGER(KIND=8) :: iPoint2, iPoint3, pointOffset, pointIndex,vectorPointIndex
    INTEGER(KIND=8) :: velIndex, fluxIndex, velOffset, dimOffset, metricOffset
    REAL(KIND=8)    :: gridScale

    REAL(KIND=8),    DIMENSION(:), POINTER     :: rhoVPtr
    REAL(KIND=8),    DIMENSION(:), POINTER     :: rhoVRHSPtr
    REAL(KIND=8),    DIMENSION(:), POINTER     :: fluxPtr
    REAL(KIND=8),    DIMENSION(:), POINTER     :: metricPtr
    INTEGER(KIND=4), DIMENSION(:), POINTER     :: stencilConnectivity

    fluxOffset =  0
    fluxPtr    => fluxBuffer(1:numPoints)

    ! Continuity
    ! Calculates fluxBuffer = rho * vHat(iDim)
    CALL ZXY(numDim,numPoints,gridSizes,opInterval,rhoBuffer,velHat,fluxPtr)

    ! numDim components of Momentum
    IF(gridType < RECTILINEAR) THEN

       gridScale = gridMetric(fluxDir)

       DO iVelDim = 1, numDim

          fluxOffset =  fluxOffset + numPoints
          velOffset  =  (iVelDim-1)*numPoints
          fluxPtr    => fluxBuffer(fluxOffset+1:fluxOffset+numPoints)
          rhoVPtr    => rhoVBuffer((velOffset+1):(velOffset+numPoints))

          IF(fluxDir == iVelDim) THEN
             ! gridMetric(iDim) is single constant over all points (0 for other dimensions)
             ! Diagonal term: flux = rhoV(iDim)*vHat(iDim) + gridMetric(iDim)*pressure
             CALL ZAWPXY(numDim,numPoints,gridSizes,opInterval,gridScale,pressureBuffer,&
                  rhoVPtr,velHat,fluxPtr)
          ELSE
             ! Cross terms: flux = rhoV(iVelDim)*vHat(iDim)
             CALL ZXY(numDim,numPoints,gridSizes,opInterval,rhoVPtr,velHat,fluxPtr)
          ENDIF
       END DO

    ELSE IF(gridType < CURVILINEAR) THEN

       metricOffset = (fluxDir-1)*numPoints
       metricPtr => gridMetric(metricOffset+1:metricOffset+numPoints)

       DO iVelDim = 1, numDim

          fluxOffset =  fluxOffset + numPoints
          velOffset  =  (iVelDim-1)*numPoints
          fluxPtr    => fluxBuffer(fluxOffset+1:fluxOffset+numPoints)
          rhoVPtr    => rhoVBuffer((velOffset+1):(velOffset+numPoints))

          IF(fluxDir == iVelDim) THEN
             ! gridMetric(iDim) is nodal scalar (0 for other dimensions)
             ! Diagonal term: flux = rhoV(iDim)*vHat(iDim) + gridMetric(iDim)*pressure
             CALL ZVWPXY(numDim,numPoints,gridSizes,opInterval,metricPtr,pressureBuffer,rhoVPtr,velHat,fluxPtr)
          ELSE
             ! Cross terms: flux = rhoV(iVelDim)*vHat(iDim)
             CALL ZXY(numDim,numPoints,gridSizes,opInterval,rhoVPtr,velHat,fluxPtr)
          ENDIF

       END DO

    ELSE ! gridMetric(iDim) is a nodal numDim-vector

       metricOffset = (fluxDir-1)*numDim*numPoints

       DO iVelDim = 1, numDim

          fluxOffset =  fluxOffset + numPoints
          velOffset  =  (iVelDim-1)*numPoints
          fluxPtr    => fluxBuffer(fluxOffset+1:fluxOffset+numPoints)
          rhoVPtr    => rhoVBuffer((velOffset+1):(velOffset+numPoints))
          metricPtr  => gridMetric(metricOffset+velOffset+1:metricOffset+velOffset+numPoints)

          ! flux = rhoV(iDim)*vHat(iDim) + p*gridMetric(iDim)
          CALL ZVWPXY(numDim,numPoints,gridSizes,opInterval,metricPtr,pressureBuffer,rhoVPtr,velHat,fluxPtr)

       END DO

    ENDIF

    ! Energy
    fluxOffset = fluxOffset + numPoints
    fluxPtr => fluxBuffer(fluxOffset+1:fluxOffset+numPoints)

    ! Calculate flux = vHat(iDim)*(rhoE + pressure)
    CALL ZWMXPY(numDim,numPoints,gridSizes,opInterval,velHat,rhoEBuffer,pressureBuffer,fluxPtr)

  END SUBROUTINE Flux1D

  !> @brief Flux for scalar transport
  SUBROUTINE ScalarFlux1D(                          &
       numDim, numPoints, gridSizes, opInterval,    &
       numScalars,scalarBuffer,velHat, fluxBuffer)

    IMPLICIT NONE

    INTEGER(KIND=4), INTENT(IN)         :: numDim, numScalars
    INTEGER(KIND=8), INTENT(IN)         :: gridSizes(numDim),  numPoints
    INTEGER(KIND=8), INTENT(IN)         :: opInterval(2*numDim)
    REAL(KIND=8),    INTENT(IN), TARGET :: scalarBuffer(numScalars*numPoints)
    REAL(KIND=8),    INTENT(IN)         :: velHat(numPoints)
    REAL(KIND=8),    INTENT(OUT),TARGET :: fluxBuffer(numScalars*numPoints)

    INTEGER         :: iScalar
    INTEGER(KIND=8) :: scalarOffset

    REAL(KIND=8),    DIMENSION(:), POINTER     :: fluxPtr
    REAL(KIND=8),    DIMENSION(:), POINTER     :: scalarPtr

    scalarOffset =  0
    fluxPtr    => fluxBuffer(1:numPoints)

    ! Scalar transport
    ! Calculates fluxBuffer = scalar * vHat
    DO iScalar = 1, numScalars
       scalarPtr => scalarBuffer(scalarOffset+1:scalarOffset+numPoints)
       fluxPtr   => fluxBuffer(scalarOffset+1:scalarOffset+numPoints)
       CALL ZXY(numDim,numPoints,gridSizes,opInterval,scalarPtr,velHat,fluxPtr)
       scalarOffset = scalarOffset + numPoints
    END DO

  END SUBROUTINE ScalarFlux1D

END MODULE Euler


!***********************************************************************
!                                                                      *
      SUBROUTINE NUCPOT
!                                                                      *
!   Evaluate the nuclear potential for point and Fermi models.         *
!                                                                      *
!   Call(s) to: [LIB92] ES.                                            *
!                                                                      *
!   Written by Farid A Parpia, at Oxford  Last revision: 05 Oct 1992   *
!                                                                      *
!***********************************************************************
!...Translated by Pacific-Sierra Research 77to90  4.3E  10:50:02   2/14/04
!...Modified by Charlotte Froese Fischer
!                     Gediminas Gaigalas  10/05/17
!-----------------------------------------------
!   M o d u l e s
!-----------------------------------------------
      USE vast_kind_param, ONLY: DOUBLE
      USE DEBUG_C
      USE DEF_C,           ONLY: Z, PI
      USE GRID_C
      USE NPAR_C
      USE NPOT_C,          ONLY: ZZ, NNUC
!-----------------------------------------------
!   I n t e r f a c e   B l o c k s
!-----------------------------------------------
      USE es_I
      IMPLICIT NONE
!-----------------------------------------------
!   L o c a l   V a r i a b l e s
!-----------------------------------------------
      INTEGER :: I, NB3, NROWS, II, II1, II2, II3
      REAL(DOUBLE) :: C, A, ABC, TABC, ABC2, THABC2, ABC3, CBA, PI2, HPIAC2, &
         H3PHP, S2MCBA, S3MCBA, SABC3, DMSAS, EN, ZBN, RI, RMC, RMCBA, RBC, &
         S2RCBA, S3RCBA
      LOGICAL :: SET
!-----------------------------------------------
!
!
!   Point nucleus
!
      IF (NPARM == 0) THEN
!
         ZZ(:N) = Z
!
!   Fermi distribution
!
      ELSE IF (NPARM == 2) THEN
!
         C = PARM(1)
         A = PARM(2)
         ABC = A/C
         TABC = 2.0D00*ABC
         ABC2 = ABC*ABC
         THABC2 = 3.0D00*ABC2
         ABC3 = ABC2*ABC
         CBA = C/A
         PI2 = PI*PI
         HPIAC2 = 0.5D00*PI2*ABC2
         H3PHP = 1.5D00 + HPIAC2
         CALL ES ((-CBA), S2MCBA, S3MCBA)
         SABC3 = 6.0D00*ABC3
         DMSAS = -SABC3*S3MCBA
         EN = 1.0D00 + ABC2*PI2 + DMSAS
         ZBN = Z/EN
!
         SET = .FALSE.
         DO I = 1, N
            RI = R(I)
            RMC = RI - C
            RMCBA = RMC/A
            RBC = RI/C
            IF (RBC <= 1.0D00) THEN
               CALL ES (RMCBA, S2RCBA, S3RCBA)
               ZZ(I) = ZBN*(DMSAS + SABC3*S3RCBA + RBC*(H3PHP - THABC2*S2RCBA&
                   - 0.5D00*RBC*RBC))
            ELSE
               IF (.NOT.SET) THEN
                  NNUC = I
                  SET = .TRUE.
               ENDIF
               CALL ES ((-RMCBA), S2RCBA, S3RCBA)
               ZZ(I) = Z*(1.0D00 + THABC2*(RBC*S2RCBA + TABC*S3RCBA)/EN)
            ENDIF
         END DO
      ENDIF
!
      IF (LDBPR(2)) THEN
         WRITE (99, 300)
         NB3 = N/3
         IF (3*NB3 == N) THEN
            NROWS = NB3
         ELSE
            NROWS = NB3 + 1
         ENDIF
         DO II = 1, NROWS
            II1 = II
            II2 = II1 + NROWS
            II3 = II2 + NROWS
            IF (II3 <= N) THEN
               WRITE (99, 301) R(II1), ZZ(II1), R(II2), ZZ(II2), R(II3), ZZ(II3&
                  )
            ELSE IF (II2 <= N) THEN
               WRITE (99, 301) R(II1), ZZ(II1), R(II2), ZZ(II2)
            ELSE
               WRITE (99, 301) R(II1), ZZ(II1)
            ENDIF
         END DO
      ENDIF
!
      RETURN
!
  300 FORMAT(/,'From SUBROUTINE NUCPOT:'/,3(&
         ' -------- r -------- ----- -r*V(r) -----'))
  301 FORMAT(1P,6(1X,1D19.12))
      RETURN
!
      END SUBROUTINE NUCPOT


subroutine rp1(maxmx,meqn,mwaves,mbc,mx,ql,qr,auxl,auxr,fwave,s,amdq,apdq)
!
!   Riemann solver for linearized multilayer shallow water equations
!

    use parameters_module

    implicit none

    ! Input arguments
    integer, intent(in) :: maxmx,meqn,mwaves,mbc,mx

    double precision, intent(in), dimension(1-mbc:maxmx+mbc, meqn) :: ql,qr
    double precision, intent(inout), dimension(1-mbc:maxmx+mbc, *) :: auxl,auxr

    ! Output arguments
    double precision, intent(out) :: s(1-mbc:maxmx+mbc, mwaves)
    double precision, intent(out) :: fwave(1-mbc:maxmx+mbc, meqn, mwaves)
    double precision, intent(out), dimension(1-mbc:maxmx+mbc, meqn) :: amdq,apdq

    ! Locals
    integer :: i,j,m,mw,ipiv(4),info,rare
    double precision :: eig_vec(4,4),A(4,4),delta(4),alpha(4),beta(4)
    double precision, dimension(4) :: flux_l,flux_r
    double precision, dimension(2) :: h_l,u_l,hu_l,h_r,u_r,hu_r,h_ave,u_ave
    double precision :: b_l,b_r,gamma_l,gamma_r,tau,w_l,w_r
    double precision :: wind_speed,lambda(4),eta_l(2),eta_r(2),h_hat_l(2),h_hat_r(2)
    logical :: dry_state_l(2), dry_state_r(2)

    integer :: layer_index
    double precision :: momentum_transfer(2),flux_transfer_r,flux_transfer_l

    ! Common block
    double precision :: dt,dx,t
    common /comxt/ dt,dx,t

    ! Initialize return variables
    amdq = 0.d0
    apdq = 0.d0

    !          |          |
    !          |          |
    !          |          |
    !----------|----------|----------
    !    i-1         i         i+1
    !  qr(i-1)  ql(i)
    !     Riemann problem at i-1/2 is between qr(i-1) (left state) and ql(i)
    !     (right state)
    do i=2-mbc,mx+mbc
        dry_state_l = .false.
        dry_state_r = .false.

        ! These state variables are the actual depth and momenta
        do j=1,2
            layer_index = 2*(j-1)
            h_l(j) = qr(i-1,layer_index+1) / rho(j)
            h_r(j) = ql(i,layer_index+1) / rho(j)
            hu_l(j) = qr(i-1,layer_index+2) / rho(j)
            hu_r(j) = ql(i,layer_index+2) / rho(j)

            h_hat_l(j) = auxr(i-1,j+2)
            h_hat_r(j) = auxl(i,j+2)

            ! Check for dry states in this layer
            if (h_l(j) < dry_tolerance) then
                dry_state_l(j) = .true.
                h_l(j) = 0.d0
                u_l(j) = 0.d0
            else
                u_l(j) = qr(i-1,layer_index+2) / qr(i-1,layer_index+1)
            endif
            if (h_r(j) < dry_tolerance) then
                dry_state_r(j) = .true.
                h_r(j) = 0.d0
                u_r(j) = 0.d0
            else
                u_r(j) = ql(i,layer_index+2) / ql(i,layer_index+1)
            endif
        enddo

        h_ave = 0.5d0 * (h_l(:) + h_r(:))

        b_l = auxr(i-1,1)
        b_r = auxl(i,1)
        w_l = auxr(i-1,2)
        w_r = auxl(i,2)

        ! Inundation test
        if (dry_state_r(2).and.(.not.dry_state_l(2)).and. &
                h_l(2) + b_l > b_r) then
            rare = 1
            stop "Right inundation problem not handled"
        else if (dry_state_l(2).and.(.not.dry_state_r(2)).and. &
                h_r(2) + b_r > b_l) then
            rare = 2
            stop "Left inundation problem not handled"
        else
            rare = 0
        endif

        ! Solve Single layer problem seperately
        if (dry_state_r(2).and.dry_state_l(2)) then
            call single_layer_eigen(h_l,h_r,u_l,u_r,b_l,b_r,rare,lambda,eig_vec)
            s(i,:) = lambda

            delta(1) = rho(1) * (hu_r(1) - hu_l(1))
            flux_r(2) = rho(1) * (h_r(1) * u_r(1)**2 + 0.5d0 * g * h_r(1)**2)
            flux_l(2) = rho(1) * (h_l(1) * u_l(1)**2 + 0.5d0 * g * h_l(1)**2)
            delta(2) = flux_r(2) - flux_l(2) + g * rho(1) * h_ave(1) * (b_r - b_l)

            beta(1) = (delta(1)*s(i,4)-delta(2)) / (s(i,4)-s(i,1))
            beta(2) = 0.d0
            beta(3) = 0.d0
            beta(4) = (delta(2) - s(i,1)*beta(1)) / s(i,4)

            ! Calculate waves
            forall(mw=1:4)
                fwave(i,:,mw) = eig_vec(:,mw) * beta(mw)
            end forall
            cycle
        endif

        ! ====================================================================
        ! Calculate eigen-space values
        if (eigen_method == 1) then
            call linear_eigen(h_hat_l,h_hat_r,u_l,u_r,b_l,b_r,rare,lambda,eig_vec)
            s(i,:) = lambda
        else if (eigen_method == 2) then
            call linear_eigen(h_l,h_r,u_l,u_r,b_l,b_r,rare,lambda,eig_vec)
            s(i,:) = lambda
        else if (eigen_method == 3) then
            call velocity_eigen(h_l,h_r,u_l,u_r,b_l,b_r,rare,lambda,eig_vec)
            s(i,:) = lambda
        else if (eigen_method == 4) then
            if (dry_state_r(2).and.(.not.dry_state_l(2)).or. &
                    dry_state_l(2).and.(.not.dry_state_r(2))) then
                call linear_eigen(h_l,h_r,u_l,u_r,b_l,b_r,rare,lambda,eig_vec)
                s(i,:) = lambda
            else
                call exact_eigen(h_l,h_r,u_l,u_r,b_l,b_r,rare,lambda,eig_vec)
                s(i,:) = lambda
            endif
        else
            stop "Invalid eigensystem method requested, method = (1,4)."
        endif

        ! ====================================================================
        ! Calculate flux vector to be projected onto e-space
        ! Right state dry, left wet
        if (dry_state_r(2).and.(.not.dry_state_l(2))) then
            ! Inundation
            if (rare == 1) then
                stop "Inundation of right state not implemented."
            else
                h_r(2) = h_l(2)
                hu_r(2) = -hu_l(2)
                u_r(2) = -u_l(2)

                flux_transfer_r = 0.d0
                flux_transfer_l = 0.d0
                momentum_transfer(1) = g * rho(1) * h_ave(1) * (b_r - h_l(2) - b_l)
                momentum_transfer(2) = 0.d0
            endif
        ! Left state dry, right wet
        else if (dry_state_l(2).and.(.not.dry_state_r(2))) then
            ! Inundation
            if (rare == 2) then
                stop "Inundation of left state not implemented."
            else
                h_l(2) = h_r(2)
                hu_l(2) = -hu_r(2)
                u_l(2) = -u_r(2)

                flux_transfer_r = 0.d0
                flux_transfer_l = 0.d0
                momentum_transfer(1) = g * rho(1) * h_ave(1) * (b_r + h_r(2) - b_l)
                momentum_transfer(2) = 0.d0
            endif
        ! Fully wet bottom layer
        else
            momentum_transfer(1) =  g * rho(1) * h_ave(1) * (h_r(2) - h_l(2) + b_r - b_l)
            momentum_transfer(2) = -g * rho(1) * h_ave(1) * (h_r(2) - h_l(2)) + g * rho(2) * h_ave(2) * (b_r - b_l)
            flux_transfer_r = g * rho(1) * h_r(1) * h_r(2)
            flux_transfer_l = g * rho(1) * h_l(1) * h_l(2)
        endif

        do j=1,2
            layer_index = 2*(j-1)
            flux_r(layer_index+1) = rho(j) * hu_r(j)
            flux_r(layer_index+2) = rho(j) * (h_r(j) * u_r(j)**2 + 0.5d0 * g * h_r(j)**2)

            flux_l(layer_index+1) = rho(j) * hu_l(j)
            flux_l(layer_index+2) = rho(j) * (h_l(j) * u_l(j)**2 + 0.5d0 * g * h_l(j)**2)
        enddo

        ! Add extra flux terms
        flux_r(4) = flux_r(4) + flux_transfer_r
        flux_l(4) = flux_l(4) + flux_transfer_l

        delta = flux_r - flux_l

        ! Momentum transfer and bathy terms
        delta(2) = delta(2) + momentum_transfer(1)
        delta(4) = delta(4) + momentum_transfer(2)

        ! Wind forcing
        wind_speed = 0.5d0 * (w_l + w_r)
        tau = wind_drag(wind_speed) * rho_air * wind_speed
        delta(2) = delta(2) - tau * wind_speed

        ! ====================================================================
        ! Solve system, solution is stored in delta
        A = eig_vec
        call dgesv(4,1,A,4,ipiv,delta,4,info)
        if (.not.(info == 0)) then
            print *, "Location (i) = (",i,")"
            print *, "Dry states, L=",dry_state_l(2)," R=",dry_state_r(2)
            print *, "h_l(2) = ",h_l(2)," h_r(2) = ",h_r(2)
            print *, "Error solving R beta = delta, ",info
            print *, "Eigen-speeds:",(s(i,mw),mw=1,mwaves)
            print *, "Eigen-vectors:"
            do j=1,4
                print "(4d16.8)",(eig_vec(j,m),m=1,meqn)
            enddo
            stop
        endif
        beta = delta

        ! Calculate waves
        forall(mw=1:4)
            fwave(i,:,mw) = eig_vec(:,mw) * beta(mw)
        end forall
    enddo

    ! Calculate amdq and apdq
    do i=2-mbc,mx+mbc
        do mw=1,mwaves
            if (s(i,mw) > 0.d0) then
                apdq(i,:) = apdq(i,:) + fwave(i,:,mw)
            else
                amdq(i,:) = amdq(i,:) + fwave(i,:,mw)
            endif
        enddo
    enddo

end subroutine rp1

subroutine linear_eigen(h_l,h_r,u_l,u_r,b_l,b_r,rare,s,eig_vec)

    use parameters_module, only: r,g

    implicit none

    double precision, intent(in) :: h_l(2),h_r(2),u_l(2),u_r(2),b_l,b_r
    logical, intent(in) :: rare
    double precision, intent(inout) :: s(4),eig_vec(4,4)

    ! Locals
    double precision :: alpha(4),gamma_l,gamma_r

    if (rare == 1) then
        gamma_l = h_l(2) / h_l(1)

        alpha(1) = 0.5d0*(gamma_l-1.d0+sqrt((gamma_l-1.d0)**2+4.d0*r*gamma_l))
        alpha(2) = 0.5d0*(gamma_l-1.d0-sqrt((gamma_l-1.d0)**2+4.d0*r*gamma_l))

        s(1) = -sqrt(g*h_l(1)*(1+alpha(1)))
        s(2) = -sqrt(g*h_l(1)*(1+alpha(2)))
        s(3) = u_l(2) + 2.d0 * sqrt(g*h_l(2))
        s(4) = u_r(1) + sqrt(g*h_r(1))

        eig_vec(1,1:2) = 1.d0
        eig_vec(2,1:2) = s(1:2)
        eig_vec(3,1:2) = alpha(1:2)
        eig_vec(4,1:2) = s(1:2)*alpha(1:2)

        eig_vec(:,3) = [0.d0,0.d0,1.d0,s(3)]
        eig_vec(:,4) = [1.d0,s(4),0.d0,0.d0]

    else if (rare == 2) then
        stop
    else
        gamma_l = h_l(2) / h_l(1)
        gamma_r = h_r(2) / h_r(1)

        alpha(1) = 0.5d0*(gamma_l-1.d0+sqrt((gamma_l-1.d0)**2+4.d0*r*gamma_l))
        alpha(2) = 0.5d0*(gamma_l-1.d0-sqrt((gamma_l-1.d0)**2+4.d0*r*gamma_l))
        alpha(3) = 0.5d0*(gamma_r-1.d0-sqrt((gamma_r-1.d0)**2+4.d0*r*gamma_r))
        alpha(4) = 0.5d0*(gamma_r-1.d0+sqrt((gamma_r-1.d0)**2+4.d0*r*gamma_r))

        s(1) = -sqrt(g*h_l(1)*(1+alpha(1)))
        s(2) = -sqrt(g*h_l(1)*(1+alpha(2)))
        s(3) = sqrt(g*h_r(1)*(1+alpha(3)))
        s(4) = sqrt(g*h_r(1)*(1+alpha(4)))

        eig_vec(1,:) = 1.d0
        eig_vec(2,:) = s(:)
        eig_vec(3,:) = alpha
        eig_vec(4,:) = s(:)*alpha(:)
    endif

end subroutine linear_eigen

subroutine velocity_eigen(h_l,h_r,u_l,u_r,b_l,b_r,rare,s,eig_vec)

    use parameters_module, only: r,g,one_minus_r

    implicit none

    ! I/O
    double precision, intent(in) :: h_l(2),h_r(2),u_l(2),u_r(2),b_l,b_r
    logical, intent(in) :: rare
    double precision, intent(inout) :: s(4),eig_vec(4,4)

    ! Locals
    double precision :: total_depth_l,total_depth_r,mult_depth_l,mult_depth_r
    double precision :: alpha(4)

    total_depth_l = sum(h_l)
    total_depth_r = sum(h_r)
    mult_depth_l = product(h_l)
    mult_depth_r = product(h_r)

    s(1) = (h_l(1)*u_l(1)+h_l(2)*u_l(2)) / total_depth_l &
          - sqrt(g*total_depth_l)
    s(2) = (h_l(2)*u_l(1)+h_l(1)*u_l(2)) / total_depth_l &
          - sqrt(g*one_minus_r*mult_depth_l/total_depth_l &
          * (1-(u_l(1)-u_l(2))**2/(g*one_minus_r*total_depth_l)))
    s(3) = (h_r(2)*u_r(1)+h_r(1)*u_r(2)) / total_depth_r &
          + sqrt(g*one_minus_r*mult_depth_r/total_depth_r &
          * (1-(u_r(1)-u_r(2))**2/(g*one_minus_r*total_depth_r)))
    s(4) = (h_r(1)*u_r(1)+h_r(2)*u_r(2)) / total_depth_r &
            + sqrt(g*total_depth_r)

    alpha(1:2) = ((s(1:2) - u_l(1))**2 - g * h_l(1)) / (g*h_l(1))
    alpha(3:4) = ((s(3:4) - u_r(1))**2 - g * h_r(1)) / (g*h_r(1))

    eig_vec(1,:) = 1.d0
    eig_vec(2,:) = s(:)
    eig_vec(3,:) = alpha
    eig_vec(4,:) = s(:)*alpha(:)

end subroutine velocity_eigen

subroutine exact_eigen(h_l,h_r,u_l,u_r,b_l,b_r,rare,s,eig_vec)

    use parameters_module, only: r,g

    implicit none

    ! I/O
    double precision, intent(in) :: h_l(2),h_r(2),u_l(2),u_r(2),b_l,b_r
    logical, intent(in) :: rare
    double precision, intent(inout) :: s(4),eig_vec(4,4)

    ! Local
    integer, parameter :: lwork = 4*4
    integer :: j,info
    double precision :: A(4,4),h_ave(2),u_ave(2)
    double precision :: real_evalues(4),imag_evalues(4)
    double precision :: empty,work(1,lwork)

    ! Solve eigenvalue problem
    h_ave(:) = 0.5d0 * (h_l(:) + h_r(:))
    u_ave(:) = 0.5d0 * (u_l(:) + u_r(:))
    A(1,:) = [0.d0,1.d0,0.d0,0.d0]
    A(2,:) = [-u_ave(1)**2 + g*h_ave(1),2.d0*u_ave(1),g*h_ave(1),0.d0]
    A(3,:) = [0.d0,0.d0,0.d0,1.d0]
    A(4,:) = [g*r*h_ave(2),0.d0,-u_ave(2)**2 + g*h_ave(2),2.d0*u_ave(2)]
    call dgeev('N','V',4,A,4,real_evalues,imag_evalues,empty,1,eig_vec,4,work,lwork,info)
    if (info < 0) then
        info = -info
        print "(a,i1,a)","The ",info,"th argument had an illegal value."
        stop
    else if (info > 0) then
        print "(a)","The QR algorithm failed to compute all the"
        print "(a)","eigenvalues, and no eigenvectors have been"
        print "(a,i1,a)","computed; elements",info,"+1:4 of WR and WI"
        print "(a)","contain eigenvalues which have converged."
        stop
    endif
    do j=1,4
!         if (eig_vec(1,j) /= 0.d0) then
!             eig_vec(:,j) = eig_vec(:,j) / eig_vec(1,j)
!         endif
        if (imag_evalues(j) > 0.d0) then
            print "(a,i1,a,d16.8)","Imaginary eigenvalue(",j,") > 0.0",imag_evalues(j)
            stop
        endif
        s(j) = real_evalues(j)
    enddo
end subroutine exact_eigen

subroutine single_layer_eigen(h_l,h_r,u_l,u_r,b_l,b_r,rare,s,eig_vec)

    use parameters_module, only: g

    implicit none

    ! I/O
    double precision, intent(in) :: h_l(2),h_r(2),u_l(2),u_r(2),b_l,b_r
    logical, intent(in) :: rare
    double precision, intent(inout) :: s(4),eig_vec(4,4)

    s(1) = u_l(1) - sqrt(g*h_l(1))
    s(2) = 0.d0
    s(3) = 0.d0
    s(4) = u_r(1) + sqrt(g*h_r(1))

    eig_vec(1,:) = 1.d0
    eig_vec(2,:) = s(:)
    eig_vec(3,:) = 0.d0
    eig_vec(4,:) = 0.d0

end subroutine single_layer_eigen


      subroutine qlbox14(Y,musq,Ires)
      implicit none
C 14:
C I_4^{\{D=4-2 \epsilon\}}(m_2^2,m_2^2,m_4^2,m_4^2;s_{12},s_{23};0,m_2^2,0,m_4^2)

c
c y14 =
c
c       [                               s12                      ]
c       [   0             0           - ---           0          ]
c       [                                2                       ]
c       [                                                        ]
c       [                                    - s23 + m4sq + m2sq ]
c       [   0           m2sq            0    ------------------- ]
c       [                                             2          ]
c       [                                                        ]
c       [   s12                                                  ]
c       [ - ---           0             0             0          ]
c       [    2                                                   ]
c       [                                                        ]
c       [        - s23 + m4sq + m2sq                             ]
c       [   0    -------------------    0           m4sq         ]
c       [                 2                                      ]

      include 'qlconstants.f'
      logical qlzero
      double precision Y(4,4),m2sq,m4sq,musq,m2,m4,ta,
     . si,xs,imxs,ieps
      double complex cxs(3),Ires(-2:0),qllnrat,xlog,wlogtmu,cln,fac

      m2sq=Y(2,2)
      m4sq=Y(4,4)
C-----Assign s and t (si=-s23,ta=-s12) so as to agree with notation of BD
      ta=2d0*Y(1,3)
      si=2d0*Y(2,4)-Y(2,2)-Y(4,4)
      m2=sqrt(m2sq)
      m4=sqrt(m4sq)
      wlogtmu=qllnrat(musq,ta)

C     ieps gives the sign of the imaginary part of cxs(1)
      call qlkfn(cxs,ieps,-si,m2,m4)
      xs=dreal(cxs(1))
      imxs=dimag(cxs(1))

      if ((qlzero(xs-1d0)) .and. (qlzero(imxs))) then
      fac=dcmplx(-xs/(m2*m4*ta))
      else
      xlog=cln(cxs(1),ieps)
      fac=dcmplx(2d0/(m2*m4*ta))*cxs(1)/(cxs(2)*cxs(3))*xlog
      endif

      Ires(-2)=czip
      Ires(-1)=fac
      Ires( 0)=fac*wlogtmu
      return
      end


      program drsgami
c>>   2001-05-25 DRSGAMI Krogh Minor change for making .f90 version.
c>>   1996-05-28 DRSGAMI Krogh  Changed Fortran 90 code.
c>>   1994-10-19 DRSGAMI Krogh  Changes to use M77CON
c>>   1994-08-15 DRSGAMI WV Snyder JPL set up for chgtyp
c>>   1993-08-03 DRSGAMI WV Snyder JPL Original code
c--S replaces "?": DR?GAMI, ?ERF, ?ERFC, ?GAMI, ?GAMIK
c
c     Demo driver for incomplete gamma function.
c     Evaluate the identity P(1/2,x) = erf(sqrt(x)) and Q(1/2,x) =
c     erfc(sqrt(x)) for several values of x.
c
      real             A, X, P, Q, SERF, SERFC, S, SC, SX
      external SERF, SERFC
      integer I, IND
      real             XS(5)
      data XS /1.0e0, 2.0e0, 3.0e0, 4.0e0, 5.0e0/
c
10    format ('          X         P(1/2,X)       ERF(SQRT(X))  ',
     1        '    Q(1/2,X)      ERFC(SQRT(X))')
20    format (1p,5e16.8)
      print 10
      a = 0.5e0
      ind = 0
      call sgamik (0.0e0, 0.0e0, 0.0e0, 0)
      do 30 i = 1, 5
         x = xs(i)
         call sgami (a, x, p, q, ind)
         sx = sqrt(x)
         s = serf(sx)
         sc = serfc(sx)
         print 20, x, p, s, q, sc
30    continue
      stop
      end


c
c
c     =====================================================
      double precision function philim(a,b,meth)
c     =====================================================
      implicit real*8(a-h,o-z)
c
c     # Compute a limiter based on wave strengths a and b.
c     # meth determines what limiter is used.
c     # a is assumed to be nonzero.
c
      if (a .eq. 0.d0) then
         philim = 0.d0
	 return
	 endif

      r = b/a

c
   10 continue
c     --------
c     # minmod
c     --------
      philim = dmax1(0.d0, dmin1(1.d0, r))
      return
c
   20 continue
c     ----------
c     # superbee
c     ----------
      philim = dmax1(0.d0, dmin1(1.d0, 2.d0*r), dmin1(2.d0, r))
      return
c
   30 continue
c     ----------
c     # van Leer
c     ----------
      philim = (r + dabs(r)) / (1.d0 + dabs(r))
      return
c
   40 continue
c     ------------------------------
c     # monotinized centered
c     ------------------------------
      c = (1.d0 + r)/2.d0
      philim = dmax1(0.d0, dmin1(c, 2.d0, 2.d0*r))
      return
c
   50 continue
c     ------------------------------
c     # Beam-Warming
c     ------------------------------
      philim = r

      return
      end


program problem6
  implicit none
  integer*8 :: diff=0
  integer   :: i,j,limit=100

  do i=1,limit
     do j=1,limit
        if (i /= j) then
           diff = diff + i*j
        end if
     end do
  end do

  print *, diff

end program problem6


! Steady state solve of the equation
!
! Input:
!
! Output:
!
subroutine steady_state()

    USE flags_M
    USE global_parameters_M
    USE mesh_info_M
    USE solution_vectors_M
    USE material_info_M
    USE element_matrices_M

    use Mod_GlobalConstants
    use Mod_SetupOutputFiles
    use Mod_WriteSoln

implicit none

!---Dummy

!---Local
    integer  :: f,g,i,jj,j,n, nl_iter
    real(dp), dimension(num_isotopes,num_delay_group) :: L2_norm_current,L2_norm_prev
    real(dp) :: nl_iter_tolerance, difference_L2
    integer :: difference_counter
    integer :: abs_max_nl_iter
    character(len=20) :: ss_file_name
	real(dp) :: total_precursors_fuel, total_power
	real(dp), dimension(num_isotopes,num_delay_group) :: precursors_lambda_vec
    real(dp) :: total_density_ss, total_temperature_ss
    real(dp) :: TimeAcrossCore,TransitTime

!---------------------------------------------------------------

!---Set some of the convergance properties
    L2_norm_prev      = 0.0
    L2_norm_current   = 0.0
    nl_iter_tolerance = 1E-15_dp
    abs_max_nl_iter   = 1500

    !---Set starting values for power, velocity, temperature
    call initialize

    !---Steady state solver for nonlinear ss problems
    write(outfile_unit, fmt='(a)'), ' '
    write(outfile_unit, fmt='(a)'), 'Start steady state calculation'

    nl_iter = 1
    precursor_soln_prev  = 0.0_dp

    nonlinearloop: do !---Nonlinear loop
        elements_loop: do n = 1, num_elem

            !---Computer spatial matrices
	        call spatial_matrices(n,nl_iter)
            call numerical_flux_matrices(n,nl_iter)

            !---Solve for temperature and velocity if feedback is enabled
            if( mass_flow > 0.0 ) then
                if( feedback_method >= 1 ) then
                    call solve_temperature_ss(n,nl_iter)
                    call solve_velocity(n,nl_iter)
                end if
            end if

            !---Solve steady state system
            isotope_loop: do f = 1, num_isotopes
                delay_loop: do g = 1, num_delay_group
                    !---Assemble K, F
                    call assemble_matrix_ss(f,g,n,nl_iter)
                    call solve_precursor_ss(f,g,n,nl_iter)
                enddo delay_loop!---Over delayed groups
            enddo isotope_loop !---Over isotopes
        enddo elements_loop !---Over nodes

        !---This counted the convergence of each delayed group
        difference_counter = 0

        call l2_norm(nl_iter)

        !---Swap for for next iteration
        precursor_soln_prev   = precursor_soln_new
        temperature_soln_prev = temperature_soln_new
        velocity_soln_prev    = velocity_soln_new

        nl_iter = nl_iter + 1

        !---If we've gone thru too many nonlinear iterations exit
        if (nl_iter > max_nl_iter) then
             exit
         !---If we've gone through a max prescribed and still not converged
        elseif( nl_iter> abs_max_nl_iter) then
             write(outfile_unit,('(a)')) 'We have gone through the max &
                   iterations andstill not converged, something may be wrong'
             stop
        end if

    enddo nonlinearloop !---end nonlinear iteration loop

!---Time across the core
    TimeAcrossCore = 0.0
    do n = Fuel_Core_Start, Fuel_Outlet_End
        TimeAcrossCore = TimeAcrossCore + 1.0/velocity_soln_new(n,2)
    end do

    print *,'Time across core ', TimeAcrossCore

    TransitTime = 0.0
    do n = Fuel_Outlet_End, num_elem
        TransitTime = TransitTime + 1.0_dp/velocity_soln_new(n,2)
    end do

    print *,'Time spent out side of core',TransitTime

    !---Write out
    write(outfile_unit, fmt=('(a)') ) ' '
    write(outfile_unit, fmt=('(a, 100I4)')) 'Number of steady state nonlinear iterations: '&
                                            , nl_iter
    write(outfile_unit, fmt=('(a)') ) ' '

    !---Make the final converged solution the 'previous' solution
    do f = 1, num_isotopes
        do g = 1, num_delay_group
            do i = 1, num_elem
                do j = 1, nodes_per_elem
                    precursor_soln_prev(f,g,i,j) = precursor_soln_new(f,g,i,j)
                    precursor_soln_last_time(f,g,i,j) = precursor_soln_new(f,g,i,j)
                end do
            end do
        end do
    end do

   do i = Fuel_Inlet_Start, Fuel_Outlet_End
        do j = 1, nodes_per_elem
            power_soln_new(i,j) = power_amplitude_new* &
                                  spatial_power_frac_fcn(i,j)
        end do
    end do

    !---Set 'previous' to new
    power_amplitude_prev      = power_amplitude_new
    !---For backward euler, need to keep the last value at the time step
    power_amplitude_last_time = power_amplitude_new
    power_soln_starting       = power_soln_new
    temperature_soln_starting = temperature_soln_new
    density_soln_starting     = density_soln_new

    power_soln_prev           = power_soln_new
    temperature_soln_prev     = temperature_soln_new
    velocity_soln_prev        = velocity_soln_new
    density_soln_prev         = density_soln_new

    density_soln_ss = density_soln_new
    temperature_soln_ss = temperature_soln_new

!---Write precursor solution outfile
    call WriteHeader(precursorSolnFileSS)
    call WriteHeader(temperatureSolnFileSS)
    call WriteHeader(velocitySolnFileSS)
    call WriteHeader(densitySolnFileSS)

    call Write3DSolutionVector(precursorSolnFileSS   ,0.0_dp , precursor_soln_new)
    call Write1DSolutionVector(temperatureSolnFileSS ,0.0_dp , temperature_soln_new)
    call Write1DSolutionVector(velocitySolnFileSS    ,0.0_dp , velocity_soln_new)
    call Write1DSolutionVector(densitySolnFile       ,0.0_dp , density_soln_new)

!---Calculate new beta for this mass flow
!---Calculate total precursor concentration*lamda over system
    precursors_lambda_vec(:,:) = 0.0_dp
    do f = 1, num_isotopes
       do g = 1, num_delay_group
            do i = Fuel_Inlet_Start, Fuel_Outlet_End
                do j = 1, nodes_per_elem
                   !---Precursors*lambda
                   precursors_lambda_vec(f,g) = precursors_lambda_vec(f,g) + &
                                      allPrecursorData(f) % decayConst(g)*&
                                      vol_int(j)*&
                                      precursor_soln_prev(f,g,i,j)
                end do
            end do
       end do
    end do

!---Calculate initial total power
    total_power = 0.0_dp
	do i = 1, num_elem
        do j = 1, nodes_per_elem
            total_power = total_power + &
                          total_power_initial*spatial_power_frac_fcn(i,j)*vol_int(j)

        end do
    end do

!---Calculate average temperature across the core
    total_temperature_ss = 0.0_dp
    total_density_ss = 0.0_dp
    do i = Fuel_Inlet_Start, Fuel_Outlet_End
        do j = 1, nodes_per_elem
            total_temperature_ss = total_temperature_ss + &
                          vol_int(j)*temperature_soln_ss(i,j)
            total_density_ss = total_density_ss + &
                                vol_int(j)*density_soln_new(i,j)
        end do
    end do

    average_temperature_ss = total_temperature_ss/(Fuel_Outlet_End - Fuel_Inlet_Start)
    average_density_ss = total_density_ss/(Fuel_Outlet_End - Fuel_Inlet_Start)

!---Calc new beta per delay group
    do f = 1, num_isotopes
        do g = 1, num_delay_group
            beta_initial_vec(f,g) = gen_time*precursors_lambda_vec(f,g)/total_power
        end do
    end do

    beta_correction = sum(beta_initial_vec)

!---Write out the new beta info
	write(outfile_unit,fmt='(a)'), ' '
    write(outfile_unit,fmt='(a,f15.2)'), 'Mass Flow    ', mass_flow
    write(outfile_unit,fmt='(a)'), 'Starting beta values are: '
    do f = 1, num_isotopes
	    write(outfile_unit,fmt='(a)'), ' '
        write(outfile_unit,fmt='(a,1I2)') 'Isotope #: ', f

        write(outfile_unit,fmt='(14f15.8,12f15.10)'), mass_flow , &
            (beta_initial_vec(f,g),g=1,num_delay_group)
	end do
    write(outfile_unit,fmt='(a)'), ' '
    write(outfile_unit, fmt='(a,f12.8)'), 'Total beta is: ', sum(beta_initial_vec)

   !---Write out to make tabulation of beta vs. flow speed easier
    do f = 1, num_isotopes
        write(beta_special_unit,fmt='(es12.6,f14.10,f14.10,f14.10,f14.10,f14.10,f14.10,f14.10, f7.2,f7.2)') &
            mass_flow , (beta_initial_vec(f,g),g=1,num_delay_group),sum(beta_initial_vec),TransitTime, TimeAcrossCore
    end do

    print *, "flowing beta", sum(beta_initial_vec)
    print *, "starting static beta: ", sum(allPrecursorData(1).groupBeta)
!---Set steady state flag off
    steady_state_flag = .FALSE.

!---Write to outfile
    write(outfile_unit, '(a)'), ' '
    write(outfile_unit, '(a)'), '*******************************'
    write(outfile_unit, '(a)'), 'End of Steady state solve'
    write(outfile_unit, '(a)'), '*******************************'

    !close(outfile_unit)

end subroutine steady_state


      subroutine chensp(ym,n,yh,k,c,m)
C...subroutine     chensp    01/08/73
C...subroutine     chensp    01/09/78
      dimension ym(n),yh(k),c(m)
      if (m.le.0) goto 58
      if (n.le.0) goto 23
      pi1=3.141592/n
      do 11 i=1,m
      ir=i-1
      alamb=0.
      r=0.
      do 10 j=1,n
      x=cos(pi1*(n-j+1.))
      y=1.
      if(ir.le.0) goto 101
      h1=y
      y=x
      if(ir.le.1) goto 101
      h2=y
      f=h2+h2
      do 4 kot=2,ir
      h3=f*h2-h1
      h1=h2
    4 h2=h3
      y=h2
  101 alamb = alamb + y*y
   10 r=r+ym(j)*y
      if (alamb.le.0.) goto 58
   11 c(i)=r/alamb
   23  if(k.le.0) goto 58
      pi1=3.141592/k
      do 51 i=1,k
      if(m.ge.2) goto 8
       yh(i)=c(1)
      go to 51
    8  x=cos(pi1*(k-i+1.))
       f=x+x
       h2=0.
       h1=0.
      do 9 j=1,m
       kot=m-j
       h3=h2
       h2=h1
    9  h1=f*h2-h3+c(kot+1)
       yh(i)=0.5*(c(1)-h3+h1)
   51 continue
   58 return
      end


!----------------------------------------------------------------------
! Module: module_apply_interp.f90
!> Purpose: interpolation routines
!>

!> Author: Benjamin Menetrier
!>

!> Licensing: this code is distributed under the CeCILL-C license
!>

!> Copyright © 2017 METEO-FRANCE
!----------------------------------------------------------------------
module module_apply_interp

use module_apply_com, only: alpha_com_AB
use module_namelist, only: lsqrt,nproc
use omp_lib
use tools_missing, only: msr
use type_fields, only: fldtype,alphatype
use type_linop, only: apply_linop,apply_linop_ad
use type_sdata, only: sdatatype,sdatampitype
implicit none

private
public :: interp,interp_ad

contains

!----------------------------------------------------------------------
! Subroutine: interp
!> Purpose: interpolation
!----------------------------------------------------------------------
subroutine interp(sdata,alpha,fld)

implicit none

! Passed variables
type(sdatatype),intent(in) :: sdata              !< Sampling data
type(alphatype),intent(in) :: alpha(sdata%nproc) !< Subgrid variable
type(fldtype),intent(inout) :: fld(sdata%nproc)  !< Field

! Local variables
integer :: iproc

if (nproc==0) then
   ! Apply global interpolation
   call interp_global(sdata,alpha(1),fld(1))
elseif (nproc>0) then
   ! Apply local interpolation
   do iproc=1,sdata%nproc
      call interp_local(sdata%mpi(iproc),alpha(iproc),fld(iproc))
   end do
end if

end subroutine interp

!----------------------------------------------------------------------
! Subroutine: interp_global
!> Purpose: interpolation, global
!----------------------------------------------------------------------
subroutine interp_global(sdata,alpha,fld)

implicit none

! Passed variables
type(sdatatype),intent(in) :: sdata !< Sampling data
type(alphatype),intent(in) :: alpha !< Subgrid variable
type(fldtype),intent(inout) :: fld  !< Field

! Local variables
integer :: is,il1,ic1,il0
real :: beta(sdata%nc1,sdata%nl1),gamma(sdata%nc1,sdata%nl1),delta(sdata%nc1,sdata%nl0)

!$omp parallel do private(is)
do is=1,sdata%ns
   if (lsqrt) then
      ! Internal normalization
      beta(sdata%is_to_ic2(is),sdata%is_to_il1(is)) = alpha%val(is)*sdata%norm_sqrt%val(is)
   else
      ! Copy
      beta(sdata%is_to_ic2(is),sdata%is_to_il1(is)) = alpha%val(is)
   end if
end do
!$omp end parallel do

! Subsampling horizontal interpolation
!$omp parallel do private(il1)
do il1=1,sdata%nl1
   call apply_linop(sdata%s(il1),beta(1:sdata%nc2(il1),il1),gamma(:,il1))
end do
!$omp end parallel do

! Vertical interpolation
!$omp parallel do private(ic1)
do ic1=1,sdata%nc1
   call apply_linop(sdata%v(sdata%vbot(ic1)),gamma(ic1,:),delta(ic1,:))
end do
!$omp end parallel do

! Horizontal interpolation
!$omp parallel do private(il0)
do il0=1,sdata%nl0
   call apply_linop(sdata%h(min(il0,sdata%nl0i)),delta(:,il0),fld%val(:,il0))
end do
!$omp end parallel do

! Normalization
fld%val = fld%val*sdata%norm%val

end subroutine interp_global

!----------------------------------------------------------------------
! Subroutine: interp_local
!> Purpose: interpolation, local
!----------------------------------------------------------------------
subroutine interp_local(sdatampi,alpha,fld)

implicit none

! Passed variables
type(sdatampitype),intent(in) :: sdatampi !< Sampling data
type(alphatype),intent(in) :: alpha       !< Subgrid variable
type(fldtype),intent(inout) :: fld        !< Field

! Local variables
integer :: isb,il1,ic1b,il0
real :: beta(sdatampi%nc1b,sdatampi%nl1),gamma(sdatampi%nc1b,sdatampi%nl1),delta(sdatampi%nc1b,sdatampi%nl0)

!$omp parallel do private(isb)
do isb=1,sdatampi%nsb
   if (lsqrt) then
      ! Internal normalization
      beta(sdatampi%isb_to_ic2b(isb),sdatampi%isb_to_il1(isb)) = alpha%valb(isb)*sdatampi%norm_sqrt%valb(isb)
   else
      ! Copy
      beta(sdatampi%isb_to_ic2b(isb),sdatampi%isb_to_il1(isb)) = alpha%valb(isb)
   end if
end do
!$omp end parallel do

! Subsampling horizontal interpolation
!$omp parallel do private(il1)
do il1=1,sdatampi%nl1
   call apply_linop(sdatampi%s(il1),beta(1:sdatampi%nc2b(il1),il1),gamma(:,il1))
end do
!$omp end parallel do

! Vertical interpolation
!$omp parallel do private(ic1b)
do ic1b=1,sdatampi%nc1b
   call apply_linop(sdatampi%v(sdatampi%vbot(ic1b)),gamma(ic1b,:),delta(ic1b,:))
end do
!$omp end parallel do

! Horizontal interpolation
!$omp parallel do private(il0)
do il0=1,sdatampi%nl0
   call apply_linop(sdatampi%h(min(il0,sdatampi%nl0i)),delta(:,il0),fld%vala(:,il0))
end do
!$omp end parallel do

! Normalization
fld%vala = fld%vala*sdatampi%norm%vala

end subroutine interp_local

!----------------------------------------------------------------------
! Subroutine: interp_ad
!> Purpose: interpolation adjoint
!----------------------------------------------------------------------
subroutine interp_ad(sdata,fld,alpha)

implicit none

! Passed variables
type(sdatatype),intent(in) :: sdata                 !< Sampling data
type(fldtype),intent(in) :: fld(sdata%nproc)        !< Field
type(alphatype),intent(inout) :: alpha(sdata%nproc) !< Subgrid variable

! Local variables
integer :: iproc

if (nproc==0) then
   ! Apply global adjoint interpolation
   call interp_ad_global(sdata,fld(1),alpha(1))
elseif (nproc>0) then
   ! Apply local adjoint interpolation
   do iproc=1,sdata%nproc
      call interp_ad_local(sdata%mpi(iproc),fld(iproc),alpha(iproc))
   end do
end if

end subroutine interp_ad

!----------------------------------------------------------------------
! Subroutine: interp_ad_global
!> Purpose: interpolation adjoint, global
!----------------------------------------------------------------------
subroutine interp_ad_global(sdata,fld,alpha)

implicit none

! Passed variables
type(sdatatype),intent(in) :: sdata    !< Sampling data
type(fldtype),intent(in) :: fld        !< Field
type(alphatype),intent(inout) :: alpha !< Subgrid variable

! Local variables
integer :: is,il1,ic1,il0
real :: beta(sdata%nc1,sdata%nl1),gamma(sdata%nc1,sdata%nl1),delta(sdata%nc1,sdata%nl0)
type(fldtype) :: fld_tmp

! Allocation
allocate(fld_tmp%val(sdata%nc0,sdata%nl0))

! Normalization
fld_tmp%val = fld%val*sdata%norm%val

! Horizontal interpolation
!$omp parallel do private(il0)
do il0=1,sdata%nl0
   call apply_linop_ad(sdata%h(min(il0,sdata%nl0i)),fld_tmp%val(:,il0),delta(:,il0))
end do
!$omp end parallel do

! Vertical interpolation
!$omp parallel do private(ic1)
do ic1=1,sdata%nc1
   call apply_linop_ad(sdata%v(sdata%vbot(ic1)),delta(ic1,:),gamma(ic1,:))
end do
!$omp end parallel do

! Subsampling horizontal interpolation
!$omp parallel do private(il1)
do il1=1,sdata%nl1
   call apply_linop_ad(sdata%s(il1),gamma(:,il1),beta(1:sdata%nc2(il1),il1))
end do
!$omp end parallel do

!$omp parallel do private(is)
do is=1,sdata%ns
   if (lsqrt) then
      ! Internal normalization
      alpha%val(is) = beta(sdata%is_to_ic2(is),sdata%is_to_il1(is))*sdata%norm_sqrt%val(is)
   else
      ! Copy
      alpha%val(is) = beta(sdata%is_to_ic2(is),sdata%is_to_il1(is))
   end if
end do
!$omp end parallel do

end subroutine interp_ad_global

!----------------------------------------------------------------------
! Subroutine: interp_ad_local
!> Purpose: interpolation adjoint, local
!----------------------------------------------------------------------
subroutine interp_ad_local(sdatampi,fld,alpha)

implicit none

! Passed variables
type(sdatampitype),intent(in) :: sdatampi  !< Sampling data
type(fldtype),intent(in) :: fld            !< Field
type(alphatype),intent(inout) :: alpha     !< Subgrid variable

! Local variables
integer :: isb,il1,ic1b,il0
real :: beta(sdatampi%nc1b,sdatampi%nl1),gamma(sdatampi%nc1b,sdatampi%nl1),delta(sdatampi%nc1b,sdatampi%nl0)
type(fldtype) :: fld_tmp

! Allocation
allocate(fld_tmp%vala(sdatampi%nc0a,sdatampi%nl0))

! Normalization
fld_tmp%vala = fld%vala*sdatampi%norm%vala

! Horizontal interpolation
!$omp parallel do private(il0)
do il0=1,sdatampi%nl0
   call apply_linop_ad(sdatampi%h(min(il0,sdatampi%nl0i)),fld_tmp%vala(:,il0),delta(:,il0))
end do
!$omp end parallel do

! Vertical interpolation
!$omp parallel do private(ic1b)
do ic1b=1,sdatampi%nc1b
   call apply_linop_ad(sdatampi%v(sdatampi%vbot(ic1b)),delta(ic1b,:),gamma(ic1b,:))
end do
!$omp end parallel do

! Subsampling horizontal interpolation
!$omp parallel do private(il1)
do il1=1,sdatampi%nl1
   call apply_linop_ad(sdatampi%s(il1),gamma(:,il1),beta(1:sdatampi%nc2b(il1),il1))
end do
!$omp end parallel do

!$omp parallel do private(isb)
do isb=1,sdatampi%nsb
   if (lsqrt) then
      ! Internal normalization
      alpha%valb(isb) = beta(sdatampi%isb_to_ic2b(isb),sdatampi%isb_to_il1(isb))*sdatampi%norm_sqrt%valb(isb)
   else
      ! Copy
      alpha%valb(isb) = beta(sdatampi%isb_to_ic2b(isb),sdatampi%isb_to_il1(isb))
   end if
end do
!$omp end parallel do

end subroutine interp_ad_local

end module module_apply_interp


C
C	$Id: cssmsgs.f,v 1.5 2008-07-27 03:10:08 haley Exp $
C
C                Copyright (C)  2000
C        University Corporation for Atmospheric Research
C                All Rights Reserved
C
C The use of this Software is governed by a License Agreement.
C
      SUBROUTINE CSSMSGS (N,X,Y,Z,U,LIST,LPTR,LEND,IFLGS,
     .                  SIGMA,W,P, NIT,DFMAX,F,GRAD, IER)
      INTEGER N, LIST(*), LPTR(*), LEND(N), IFLGS, NIT, IER
      DOUBLE PRECISION X(N), Y(N), Z(N), U(N), SIGMA(*),
     .                 W(N), P, DFMAX, F(N), GRAD(3,N)
C
C***********************************************************
C
C                                              From SSRFPACK
C                                            Robert J. Renka
C                                  Dept. of Computer Science
C                                       Univ. of North Texas
C                                           renka@cs.unt.edu
C                                                   07/25/96
C
C   This subroutine solves the symmetric positive definite
C linear system associated with minimizing the quadratic
C functional Q(F,FX,FY,FZ) described in Subroutine CSSMSURF.
C Since the gradient at node K lies in the plane tangent to
C the sphere surface at K, it is effectively defined by only
C two components -- its X and Y components in the coordinate
C system obtained by rotating K to the north pole.  Thus,
C the minimization problem corresponds to an order-3N system
C which is solved by the block Gauss-Seidel method with 3 by
C 3 blocks.
C
C On input:
C
C       N,X,Y,Z,U,LIST,LPTR,LEND,IFLGS,SIGMA,W = Parameters
C           as described in Subroutine CSSMSURF.
C
C       P = Positive smoothing parameter defining Q.
C
C The above parameters are not altered by this routine.
C
C       NIT = Maximum number of iterations to be used.  This
C             maximum will likely be achieved if DFMAX is
C             smaller than the machine precision.  NIT .GE.
C             0.
C
C       DFMAX = Nonnegative convergence criterion.  The
C               method is terminated when the maximum
C               change in a solution F-component between
C               iterations is at most DFMAX.  The change in
C               a component is taken to be the absolute
C               difference relative to 1 plus the old value.
C
C       F = Initial estimate of the first N solution compo-
C           nents.
C
C       GRAD = 3 by N array containing initial estimates of
C              the last 3N solution components (the gradi-
C              ent with FX, FY, and FZ in rows 1, 2, and 3,
C              respectively).
C
C On output:
C
C       NIT = Number of Gauss-Seidel iterations employed.
C
C       DFMAX = Maximum relative change in a solution F-
C               component at the last iteration.
C
C       F = First N solution components -- function values
C           at the nodes.
C
C       GRAD = Last 3N solution components -- gradients at
C              the nodes.
C
C       IER = Error indicator:
C             IER = 0 if no errors were encountered and the
C                     convergence criterion was achieved.
C             IER = 1 if no errors were encountered but con-
C                     vergence was not achieved within NIT
C                     iterations.
C             IER = -1 if N, P, NIT, or DFMAX is outside its
C                      valid range on input.  F and GRAD are
C                      not altered in this case.
C             IER = -2 if all nodes are collinear or the
C                      triangulation is invalid.
C             IER = -3 if duplicate nodes were encountered.
C
C SSRFPACK modules required by CSSMSGS:  CSAPLYRT, CSCONSTR,
C                                        CSGRCOEF, CSSNHCSH
C
C Intrinsic functions called by CSSMSGS:  ABS, ATAN, MAX, SQRT
C
C***********************************************************
C
      INTEGER IFL, ITER, ITMAX, J, K, LPJ, LPL, NN
      DOUBLE PRECISION ALFA, ALFSQ, C11, C12, C13, C22, C23,
     .                 C33, CC22, CC23, CC33, CX, CY, DEN1,
     .                 DEN2, DET, DF, DFMX, DGK(3), DGX,
     .                 DGY, FK, G1, G2, G3, GJK, GKJ, PP,
     .                 R1, R2, R3, RR2, RR3, SIG, SINAL, SX,
     .                 SY, T, T1, T2, T3, T4, T5, T6, TOL,
     .                 XJ, XK, XS, YJ, YK, YS, ZJ, ZK
C
      NN = N
      IFL = IFLGS
      PP = P
      ITMAX = NIT
      TOL = DFMAX
C
C Test for errors in input and initialize iteration count,
C   tension factor, and output value of DFMAX.
C
      IF (NN .LT. 3  .OR.  PP .LE. 0.  .OR.  ITMAX .LT. 0
     .    .OR.  TOL .LT. 0.) GO TO 5
      ITER = 0
      SIG = SIGMA(1)
      DFMX = 0.
C
C Top of iteration loop.
C
    1 IF (ITER .EQ. ITMAX) GO TO 4
      DFMX = 0.
C
C   Loop on nodes.
C
      DO 3 K = 1,NN
        XK = X(K)
        YK = Y(K)
        ZK = Z(K)
        FK = F(K)
        G1 = GRAD(1,K)
        G2 = GRAD(2,K)
        G3 = GRAD(3,K)
C
C   Construct the rotation mapping node K to the north pole.
C
        CALL CSCONSTR (XK,YK,ZK, CX,SX,CY,SY)
C
C   Initialize components of the order-3 system for the
C     change (DF,DGX,DGY) in the K-th solution components.
C
        C11 = PP*W(K)
        C12 = 0.
        C13 = 0.
        C22 = 0.
        C23 = 0.
        C33 = 0.
        R1 = C11*(U(K)-FK)
        R2 = 0.
        R3 = 0.
C
C   Loop on neighbors J of node K.
C
        LPL = LEND(K)
        LPJ = LPL
    2   LPJ = LPTR(LPJ)
          J = ABS(LIST(LPJ))
C
C   Compute the coordinates of J in the rotated system.
C
          T = SX*Y(J) + CX*Z(J)
          YJ = CX*Y(J) - SX*Z(J)
          ZJ = SY*X(J) + CY*T
          XJ = CY*X(J) - SY*T
C
C   Compute arc-length ALFA between K and J, ALFSQ = ALFA*
C     ALFA, SINAL = SIN(ALFA), DEN1 = ALFA*SIN(ALFA)**2, and
C     DEN2 = ALFSQ*SINAL.
C
          ALFA = 2.D0*ATAN(SQRT((1.D0-ZJ)/(1.D0+ZJ)))
          ALFSQ = ALFA*ALFA
          XS = XJ*XJ
          YS = YJ*YJ
          SINAL = SQRT(XS+YS)
          DEN1 = ALFA*(XS+YS)
          DEN2 = ALFSQ*SINAL
C
C   Test for coincident nodes and compute functions of SIG:
C     T1 = SIG*SIG*COSHM/E, T2 = SIG*SINHM/E, and T3 = SIG*
C     (SIG*COSHM-SINHM)/E for E = SIG*SINH - 2*COSHM.
C
          IF (DEN1 .EQ. 0.) GO TO 7
          IF (IFL .GE. 1) SIG = SIGMA(LPJ)
          CALL CSGRCOEF (SIG, T3,T2)
          T1 = T2 + T3
C
C   Update system components for node J.
C
          T4 = 2.*T1/(ALFA*ALFSQ)
          T5 = T1/DEN2
          T6 = T3/DEN1
          C11 = C11 + T4
          C12 = C12 + T5*XJ
          C13 = C13 + T5*YJ
          C22 = C22 + T6*XS
          C23 = C23 + T6*XJ*YJ
          C33 = C33 + T6*YS
          GKJ = G1*X(J) + G2*Y(J) + G3*Z(J)
          GJK = GRAD(1,J)*XK + GRAD(2,J)*YK + GRAD(3,J)*ZK
          R1 = R1 + T4*(F(J)-FK) + T5*(GJK-GKJ)
          T = T5*(F(J)-FK) - T6*GKJ + T2*GJK/DEN1
          R2 = R2 + T*XJ
          R3 = R3 + T*YJ
C
C   Bottom of loop on neighbors.
C
          IF (LPJ .NE. LPL) GO TO 2
C
C   Solve the system associated with the K-th block.
C
        CC22 = C11*C22 - C12*C12
        CC23 = C11*C23 - C12*C13
        CC33 = C11*C33 - C13*C13
        RR2 = C11*R2 - C12*R1
        RR3 = C11*R3 - C13*R1
        DET = CC22*CC33 - CC23*CC23
        IF (DET .EQ. 0.  .OR.  CC22 .EQ. 0.  .OR.
     .      C11 .EQ. 0.) GO TO 6
        DGY = (CC22*RR3 - CC23*RR2)/DET
        DGX = (RR2 - CC23*DGY)/CC22
        DF = (R1 - C12*DGX - C13*DGY)/C11
C
C   Rotate (DGX,DGY,0) back to the original coordinate
C     system, and update GRAD( ,K), F(K), and DFMX.
C
        CALL CSAPLYRT (DGX,DGY,CX,SX,CY,SY, DGK)
        GRAD(1,K) = G1 + DGK(1)
        GRAD(2,K) = G2 + DGK(2)
        GRAD(3,K) = G3 + DGK(3)
        F(K) = FK + DF
        DFMX = MAX(DFMX,ABS(DF)/(1.+ABS(FK)))
    3   CONTINUE
C
C   Increment ITER and test for convergence.
C
      ITER = ITER + 1
      IF (DFMX .GT. TOL) GO TO 1
C
C The method converged.
C
      NIT = ITER
      DFMAX = DFMX
      IER = 0
      RETURN
C
C The method failed to converge within NIT iterations.
C
    4 DFMAX = DFMX
      IER = 1
      RETURN
C
C Invalid input parameter.
C
    5 NIT = 0
      DFMAX = 0.
      IER = -1
      RETURN
C
C Node K and its neighbors are collinear.
C
    6 NIT = 0
      DFMAX = DFMX
      IER = -2
      RETURN
C
C Nodes J and K coincide.
C
    7 NIT = 0
      DFMAX = DFMX
      IER = -3
      RETURN
      END


!-----------------------------------------------------------------------
!     Subroutine:    chisq_limiter
!     Authors:       S. Lazerson (lazerson@pppl.gov)
!     Date:          04/25/2013
!     Description:   Calculates the distance from the equilibrium
!                    surface to a limiter surface.  If the equilibrium
!                    falls outside the surface, a large chisq is
!                    returned.  Target essentially sets the point and
!                    slope at which the effect of the limiter is felt.
!-----------------------------------------------------------------------
      SUBROUTINE chisq_limiter(target,sigma,niter,iflag)
!-----------------------------------------------------------------------
!     Libraries
!-----------------------------------------------------------------------
      USE stellopt_runtime
      USE stellopt_targets
      USE equil_utils

!-----------------------------------------------------------------------
!     Input/Output Variables
!
!-----------------------------------------------------------------------
      IMPLICIT NONE
      REAL(rprec), INTENT(in)    ::  target(nu_max,nv_max)
      REAL(rprec), INTENT(in)    ::  sigma(nu_max,nv_max)
      INTEGER,     INTENT(in)    ::  niter
      INTEGER,     INTENT(inout) ::  iflag

!-----------------------------------------------------------------------
!     Local Variables
!
!-----------------------------------------------------------------------
      INTEGER :: u, u2, v, nv2, ier, dex, nu_val
      REAL(rprec) :: s, xu, xv, R1, Z1, R2, Z2, dist, temp, lim_sgn, &
                     RA, ZA, rho_plasma, rho_limiter, RP, ZP, RN, ZN, &
                     RD, ZD, RX, ZX
      REAL(rprec) :: d1(nu_max)
!----------------------------------------------------------------------
!     BEGIN SUBROUTINE
!----------------------------------------------------------------------
      IF (iflag < 0) RETURN
      dex = SUM(COUNT(sigma_limiter < bigno,DIM=1))
      IF (iflag == 1) WRITE(iunit_out,'(A,2(2X,I5.5))') 'LIMITER ',dex,6
      IF (iflag == 1) WRITE(iunit_out,'(A)') 'TARGET  SIGMA  DIST  R  PHI  Z'
      IF (niter >= 0) THEN
         xu = 0.0
         xv = 0.0
         s  = 0.0
         CALL get_equil_RZ(s,xu,xv,RA,ZA,ier)
         R1 = r_limiter(1,1)
         Z1 = z_limiter(1,1)
         R2 = r_limiter(2,1)
         Z2 = z_limiter(2,1)
         xu = ATAN2(Z1-ZA,R1-RA)
         xv = ATAN2(Z2-ZA,R2-RA)
         lim_sgn = 1.0
         IF (xv < xu) lim_sgn = -1.0
         DO v = 1, nv_max
            nu_val = MAXLOC(sigma_limiter(:,v),DIM=1)
            !PRINT *,'nu_val',nu_val
            DO u = 1, nu_val-2
               IF (sigma_limiter(u,v) >= bigno) CYCLE
               s  = 1.0_rprec
               xv = phi_limiter(u,v)
               IF (xv < 0.0) xv = xv + pi2
               xv = MOD(nfp*xv,pi2)
               R1 = r_limiter(u,v)
               Z1 = z_limiter(u,v)
               R2 = r_limiter(u+1,v)
               Z2 = z_limiter(u+1,v)
               RN = lim_sgn*Z2-Z1
               ZN = -lim_sgn*(R2-R1)
               RD = R2-R1
               ZD = Z2-Z1
               temp = SQRT(RD*RD+ZD*ZD)
               RD = RD/temp
               ZD = ZD/temp
               d1 = 0.0
               !PRINT *,'R1,Z1',R1,Z1
               !PRINT *,'R2,Z2',R2,Z2
               !PRINT *,'RD,ZD',RD,ZD
               !PRINT *,'RD,ZD',RN,ZN
               DO u2 = 1, nu_max
                  ier = 0
                  xu = pi2*REAL(u2-1)/REAL(nu_max)
                  CALL get_equil_RZ(s,xu,xv,RP,ZP,ier)
                  ! Now X1 is the first point X2 is the second point an XP is the plasma point
                  dist =   ABS( (R2-R1)*(Z1-ZP)-(R1-RP)*(Z2-Z1) ) &
                         / SQRT( (R2-R1)*(R2-R1)+(Z2-Z1)*(Z2-Z1) )
                  ! Now (RN, ZN) points out of the vessel
                  ! and (RD, ZD) points along the vessel
                  ! THEN X = P - X1 + ((P-X1).D)D points from the line to the point p and is normal to the line segment
                  RX = RP - (R1 + ((RP-R1)*RD+(ZP-Z1)*ZD)*RD)
                  ZX = ZP - (Z1 + ((RP-R1)*RD+(ZP-Z1)*ZD)*ZD)
                  ! So if X.N is positive  the point is outside, if X.N is negative the point is inside
                  temp = RX*RN + ZX*ZN
                  d1(u2) = dist
                  IF (temp > 0) d1(u2) = -dist
                  !PRINT *,'dist',u2,d1(u2)
               END DO
               dist = MINVAL(d1,DIM=1)
               mtargets = mtargets + 1
               targets(mtargets) = target(u,v)
               sigmas(mtargets)  = sigma(u,v)
               IF (dist < target(u,v)) THEN
                  vals(mtargets) = dist
               ELSE
                  vals(mtargets) = target(u,v)
               END IF
               IF (iflag == 1) WRITE(iunit_out,'(6ES22.12E3)') target(u,v),sigma(u,v),vals(mtargets),R1,phi_limiter(u,v),Z1
            END DO
         END DO
      ELSE
         DO v = 1, nv_max
            DO u = 1, nu_max
               IF (sigma_limiter(u,v) >= bigno) CYCLE
               mtargets = mtargets + 1
               IF (niter == -2) target_dex(mtargets)=jtarget_limiter
            END DO
         END DO
      END IF
      RETURN
!----------------------------------------------------------------------
!     END SUBROUTINE
!----------------------------------------------------------------------
      END SUBROUTINE chisq_limiter


      DOUBLE PRECISION FUNCTION CDFWAK(X,PARA)
C***********************************************************************
C*                                                                     *
C*  FORTRAN CODE WRITTEN FOR INCLUSION IN IBM RESEARCH REPORT RC20525, *
C*  'FORTRAN ROUTINES FOR USE WITH THE METHOD OF L-MOMENTS, VERSION 3' *
C*                                                                     *
C*  J. R. M. HOSKING                                                   *
C*  IBM RESEARCH DIVISION                                              *
C*  T. J. WATSON RESEARCH CENTER                                       *
C*  YORKTOWN HEIGHTS                                                   *
C*  NEW YORK 10598, U.S.A.                                             *
C*                                                                     *
C*  VERSION 3     AUGUST 1996                                          *
C*                                                                     *
C***********************************************************************
C
C  CUMULATIVE DISTRIBUTION FUNCTION OF THE WAKEBY DISTRIBUTION
C
C  OTHER ROUTINES USED: QUAWAK
C
C  METHOD: THE EQUATION X=G(Z), WHERE G(Z) IS THE WAKEBY QUANTILE
C  EXPRESSED AS A FUNCTION OF Z=-LOG(1-F), IS SOLVED USING HALLEY'S
C  METHOD (THE 2ND-ORDER ANALOGUE OF NEWTON-RAPHSON ITERATION).
C
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      DOUBLE PRECISION PARA(5)
      DATA ZERO/0D0/,HALF/0.5D0/,ONE/1D0/
      DATA P1/0.1D0/,P7/0.7D0/,P99/0.99D0/
C
C         EPS,MAXIT CONTROL THE TEST FOR CONVERGENCE OF THE ITERATION
C         ZINCMX IS THE LARGEST PERMITTED ITERATIVE STEP
C         ZMULT CONTROLS WHAT HAPPENS WHEN THE ITERATION STEPS BELOW ZERO
C         UFL SHOULD BE CHOSEN SO THAT DEXP(UFL) JUST DOES NOT CAUSE
C           UNDERFLOW
C
      DATA EPS/1D-8/,MAXIT/20/,ZINCMX/3D0/,ZMULT/0.2D0/
      DATA UFL/-170D0/
C
      XI=PARA(1)
      A=PARA(2)
      B=PARA(3)
      C=PARA(4)
      D=PARA(5)
C
C         TEST FOR VALID PARAMETERS
C
      IF(B+D.LE.ZERO.AND.(B.NE.ZERO.OR.C.NE.ZERO.OR.D.NE.ZERO))GOTO 1000
      IF(A.EQ.ZERO.AND.B.NE.ZERO)GOTO 1000
      IF(C.EQ.ZERO.AND.D.NE.ZERO)GOTO 1000
      IF(C.LT.ZERO.OR.A+C.LT.ZERO)GOTO 1000
      IF(A.EQ.ZERO.AND.C.EQ.ZERO)GOTO 1000
C
      CDFWAK=ZERO
      IF(X.LE.XI)RETURN
C
C         TEST FOR SPECIAL CASES
C
      IF(B.EQ.ZERO.AND.C.EQ.ZERO.AND.D.EQ.ZERO)GOTO 100
      IF(C.EQ.ZERO)GOTO 110
      IF(A.EQ.ZERO)GOTO 120
C
C         GENERAL CASE
C
      CDFWAK=ONE
      IF(D.LT.ZERO.AND.X.GE.XI+A/B-C/D)RETURN
C
C         INITIAL VALUES FOR ITERATION:
C         IF X IS IN THE LOWEST DECILE OF THE DISTRIBUTION, START AT Z=0
C           (F=0);
C         IF X IS IN THE HIGHEST PERCENTILE OF THE DISTRIBUTION,
C           STARTING VALUE IS OBTAINED FROM ASYMPTOTIC FORM OF THE
C           DISTRIBUTION FOR LARGE Z (F NEAR 1);
C         OTHERWISE START AT Z=0.7 (CLOSE TO F=0.5).
C
      Z=P7
      IF(X.LT.QUAWAK(P1,PARA))Z=ZERO
      IF(X.LT.QUAWAK(P99,PARA))GOTO 10
      IF(D.LT.ZERO)Z=DLOG((X-XI-A/B)*D/C+ONE)/D
      IF(D.EQ.ZERO)Z=(X-XI-A/B)/C
      IF(D.GT.ZERO)Z=DLOG((X-XI)*D/C+ONE)/D
   10 CONTINUE
C
C         HALLEY'S METHOD, WITH MODIFICATIONS:
C         IF HALLEY ITERATION WOULD MOVE IN WRONG DIRECTION
C           (TEMP.LE.ZERO), USE ORDINARY NEWTON-RAPHSON INSTEAD;
C         IF STEP GOES TOO FAR (ZINC.GT.ZINCMX OR ZNEW.LE.ZERO),
C            LIMIT ITS LENGTH.
C
      DO 30 IT=1,MAXIT
      EB=ZERO
      BZ=-B*Z
      IF(BZ.GE.UFL)EB=DEXP(BZ)
      GB=Z
      IF(DABS(B).GT.EPS)GB=(ONE-EB)/B
      ED=DEXP(D*Z)
      GD=-Z
      IF(DABS(D).GT.EPS)GD=(ONE-ED)/D
      XEST=XI+A*GB-C*GD
      FUNC=X-XEST
      DERIV1=A*EB+C*ED
      DERIV2=-A*B*EB+C*D*ED
      TEMP=DERIV1+HALF*FUNC*DERIV2/DERIV1
      IF(TEMP.LE.ZERO)TEMP=DERIV1
      ZINC=FUNC/TEMP
      IF(ZINC.GT.ZINCMX)ZINC=ZINCMX
      ZNEW=Z+ZINC
      IF(ZNEW.LE.ZERO)GOTO 20
      Z=ZNEW
      IF(DABS(ZINC).LE.EPS)GOTO 200
      GOTO 30
   20 Z=Z*ZMULT
   30 CONTINUE
C
C         NOT CONVERGED
C
      WRITE(6,7010)
      GOTO 200
C
C         SPECIAL CASE B=C=D=0: WAKEBY IS EXPONENTIAL
C
  100 CONTINUE
      Z=(X-XI)/A
      GOTO 200
C
C         SPECIAL CASE C=0: WAKEBY IS GENERALIZED PARETO, BOUNDED ABOVE
C
  110 CONTINUE
      CDFWAK=ONE
      IF(X.GE.XI+A/B)RETURN
      Z=-DLOG(ONE-(X-XI)*B/A)/B
      GOTO 200
C
C         SPECIAL CASE A=0: WAKEBY IS GENERALIZED PARETO, NO UPPER BOUND
C
  120 CONTINUE
      Z=DLOG(ONE+(X-XI)*D/C)/D
      GOTO 200
C
C         CONVERT Z VALUE TO PROBABILITY
C
  200 CDFWAK=ONE
      IF(-Z.LT.UFL)RETURN
      CDFWAK=ONE-DEXP(-Z)
      RETURN
C
 1000 WRITE(6,7000)
      CDFWAK=ZERO
      RETURN
C
 7000 FORMAT(' *** ERROR *** ROUTINE CDFWAK : PARAMETERS INVALID')
 7010 FORMAT(' ** WARNING ** ROUTINE CDFWAK :',
     *  ' ITERATION HAS NOT CONVERGED. RESULT MAY BE UNRELIABLE.')
      END


        subroutine idz_qmatvec(ifadjoint,m,n,a,krank,v)
c
c       applies to a single vector the Q matrix (or its adjoint)
c       which the routine idzp_qrpiv or idzr_qrpiv has stored
c       in a triangle of the matrix it produces (stored, incidentally,
c       as data for applying a bunch of Householder reflections).
c       Use the routine idz_qmatmat to apply the Q matrix
c       (or its adjoint)
c       to a bunch of vectors collected together as a matrix,
c       if you're concerned about efficiency.
c
c       input:
c       ifadjoint -- set to 0 for applying Q;
c                    set to 1 for applying the adjoint of Q
c       m -- first dimension of a and length of v
c       n -- second dimension of a
c       a -- data describing the qr decomposition of a matrix,
c            as produced by idzp_qrpiv or idzr_qrpiv
c       krank -- numerical rank
c       v -- vector to which Q (or its adjoint) is to be applied
c
c       output:
c       v -- vector to which Q (or its adjoint) has been applied
c
        implicit none
        save
        integer m,n,krank,k,ifrescal,mm,ifadjoint
        real*8 scal
        complex*16 a(m,n),v(m)
c
c
        ifrescal = 1
c
c
        if(ifadjoint .eq. 0) then
c
          do k = krank,1,-1
            mm = m-k+1
            if(k .lt. m) call idz_houseapp(mm,a(k+1,k),v(k),
     1                                     ifrescal,scal,v(k))
          enddo ! k
c
        endif
c
c
        if(ifadjoint .eq. 1) then
c
          do k = 1,krank
            mm = m-k+1
            if(k .lt. m) call idz_houseapp(mm,a(k+1,k),v(k),
     1                                     ifrescal,scal,v(k))
          enddo ! k
c
        endif
c
c
        return
        end
c
c
c
c


module multigrid

  implicit none

  private
  public :: mg_interface

  ! will need own private constants and parameters since entirely modular
  integer, parameter :: num=selected_real_kind(p=15)

  integer :: ix, iy

  logical :: eta_present = .false.

  ! data object
  type grid
    integer :: level, nx, ny
    real(num) :: dx, dy
    real(num),dimension(:,:), allocatable :: phi
    real(num),dimension(:,:), allocatable :: f
    real(num),dimension(:,:), allocatable :: residue
    real(num),dimension(:,:), allocatable :: eta, eta_xface, eta_yface
    type(grid), pointer :: next, prev
  end type grid

  type(grid), pointer :: head, tail

  ! public state object - used for better handling of optional inputs than a big list of dummies
  ! in mg_interface

  type, public :: mg_state
    ! required in constructor a=mg_state(nx = ... etc)
    integer :: nx
    integer :: ny
    real(num) :: dx, dy
    real(num),dimension(:,:), allocatable :: phi
    real(num),dimension(:,:), allocatable :: f
    character(len=20) :: bc_xmin, bc_ymin, bc_xmax, bc_ymax
    real(num) :: tol
    ! optional non-allocatable variables just set to a default
    integer :: nlevels = -1 !-1 is "auto"
    logical :: eta_present = .false.
    ! optional allocatables
    real(num),dimension(:,:), allocatable :: eta

  end type mg_state

  type(mg_state) :: module_state

  ! boundary conditions. The program calling mg_interface
  ! is responsible for getting the integers correct
  integer :: bc_xmin , bc_xmax, bc_ymin, bc_ymax
  integer, parameter :: periodic = 0
  integer, parameter :: zero_gradient = 1 ! Neumann
  integer, parameter :: fixed = 2 ! Dirichlet

contains

  subroutine mg_state_constructor(this)
    type(grid) :: this
    print *,'stub'
  end subroutine mg_state_constructor

  subroutine mg_interface(f, phi, eta, tol, &
        &  nx,ny,dx,dy, nlevels, &
        & bc_xmin_in, bc_xmax_in, bc_ymin_in, bc_ymax_in)
    real(num), dimension(:,:), intent(in) :: f
    real(num), dimension(:,:), allocatable, intent(inout) :: phi
    real(num), dimension(:,:), allocatable, optional, intent(in) :: eta
    integer, intent(in) :: nx
    integer, intent(in) :: ny
    integer, intent(inout) :: nlevels
    real(num), intent(in) :: dx, dy
    real(num), intent(in) :: tol
    integer, intent(in) :: bc_xmin_in, bc_xmax_in, bc_ymin_in, bc_ymax_in !***
    type(grid), pointer :: current
    real(num) :: start, finish

    !*** this is a bit annoying but the alternative seems to be passing a single type
    ! as the input

    bc_xmin = bc_xmin_in
    bc_xmax = bc_xmax_in
    bc_ymin = bc_ymin_in
    bc_ymax = bc_ymax_in

    if (present(eta)) then
      eta_present = .true.
      call sanity_checks(f=f, eta=eta, phi=phi, nx=nx,ny=ny,nlevels=nlevels,dx=dx,dy=dy) !*
      call initialise_grids(f=f, eta=eta, nx=nx,ny=ny,nlevels=nlevels,dx=dx,dy=dy) !*
    else
      eta_present = .false.
      call sanity_checks(f=f, phi=phi, nx=nx,ny=ny,nlevels=nlevels,dx=dx,dy=dy) !*
      call initialise_grids(f=f, nx=nx,ny=ny,nlevels=nlevels,dx=dx,dy=dy) !*
    endif

    print *,'*** Multigrid called'
    print *,'****** nx = ',nx
    print *,'****** ny = ',ny
    print *,'****** nlevels ',nlevels
    print *,'****** tolerance = ',tol
    print *,'****** bc_xmin = ', bc_xmin
    print *,'****** bc_xmax = ', bc_xmax
    print *,'****** bc_ymin = ', bc_ymin
    print *,'****** bc_ymax = ', bc_ymax

    call cpu_time(start)
    call mg_solve(tol)
    call cpu_time(finish)
    print '(" ****** cpu_time: ",f20.3," seconds.")',finish-start

    ! set the inout(phi) = phi on finest grid to return to caller
    current => head
    phi = current%phi

    print *,'*** Multigrid finished'

  end subroutine mg_interface

!* in  practice, in CCAPS / multistep hydro codes might want to "first
!  call" this only to keep
!  them all allocated throughout .. not sure yet how that will work so
!  keep this comment till it pans out

  ! main solver

  subroutine mg_solve(tol)
    real(num), intent(in) :: tol
    type(grid), pointer :: current

    real(num) :: L2, L2_old

    integer :: nsteps
    integer :: c
    integer :: num_sweeps_down = 3
    integer :: num_sweeps_up = 3

    L2_old = 1e6_num
    current => head

    nsteps = 0

    mainloop: do
      nsteps = nsteps +1

      downcycle: do
        if (current%level == tail%level) exit downcycle

        if (current%level /= 1) current%phi = 0.0_num ! important

        do c = 1, num_sweeps_down
          call relax(current)
        enddo

       if (current%level == 1) then
          call residual(current)
          L2 = sqrt(sum(abs(current%residue)**2)/real(current%nx*current%ny,num))
          if (abs(L2-L2_old) <= tol) exit mainloop
!          if (isnan(L2)) STOP
          L2_old = L2
        endif
        call residual(current)
        call restrict(current)
        current=>current%next

      enddo downcycle

      bottom_solve: do

        current%phi = 0.0_num

        do c = 1, 50 !**
          call relax(current)
          if (modulo(c-1,5)==0) then
            call residual(current)
            L2 = sqrt(sum(abs(current%residue)**2)/real(current%nx*current%ny,num))
            if (L2 < tol) exit
!          if (isnan(L2)) STOP
          endif
        enddo
        call inject(current)
        current => current%prev
        exit bottom_solve !not really a loop / readability. Possible performance hit?

      enddo bottom_solve

      upcycle: do
        if (current%level == 1) exit upcycle
        do c = 1, num_sweeps_up
          call relax(current)
        enddo
        call inject(current)
        current => current%prev
      enddo upcycle

    enddo mainloop

    print '(" ****** Finished in: ",i3.3," V cycles")',nsteps
    print '(" ****** Fine grid residual: ",e20.8," (L2)")',L2

    !** if not refining to ideal case (nx=ny=1 + ghosts) this wont automagically
    ! be solved exactly (discretely). If so, do an arbitary amount of
    ! relaxations, checking the residual, up to a max of 50. Perhaps in some
    ! problems, if you can't refine very much because of a large Lx/=Ly asoect
    ! ratio, this could cause an issue
  end subroutine mg_solve

  subroutine gs_solve ! for comparison

    type(grid), pointer :: current

    real(num) :: L2, L2_old

    integer :: nsteps

    L2_old = 1e6_num
    current => head
    nsteps = 0
    do
      nsteps = nsteps + 1
      call relax(current)
      call residual(current)

      L2 = sqrt(sum(abs(current%residue)**2)/real(current%nx*current%ny,num))
      if (abs(L2-L2_old) < 1e-12_num) exit ! should replace with user chosen tol eventually
      L2_old = L2
    enddo

    print *,'nsteps',nsteps
  end subroutine gs_solve

  subroutine mg_2level_solve(tol) ! for debugging, dont call with more than 2 levels
    real(num), intent(in) :: tol
    type(grid), pointer :: current

    real(num) :: L2, L2_old

    integer :: nsteps
    integer :: c
    integer :: num_sweeps_down = 3

    L2_old = 1e6_num
    current => head

    nsteps = 0

    mainloop: do
      nsteps = nsteps +1

      downcycle: do
        if (current%level == tail%level) exit

        do c = 1, num_sweeps_down
          call relax(current)
        enddo

        if (current%level == 1) then
          call residual(current)
          L2 = sqrt(sum(abs(current%residue)**2)/real(current%nx*current%ny,num))
          if (abs(L2-L2_old) <= tol) exit mainloop
          L2_old = L2
        endif
        call residual(current)
        call restrict(current)
        current=>current%next

      enddo downcycle

      ! bottom solve
      do c = 1, 50 ! for now only
        call relax(current)
      enddo
      call inject(current)
      current => current%prev

      ! upcycle goes here , currently unnecessary as two level

    enddo mainloop

  print *,'nsteps',nsteps

  end subroutine mg_2level_solve

  ! methods used in the main solver

  subroutine restrict(this)
    ! restrict the residual at level "this" (this%residue)
    ! to be the rhs of the next level (next%f)

    type(grid), pointer :: this
    type(grid), pointer :: next
    integer :: ixc, iyc

    next => this%next

    iy=  1
    do iyc = 1, next%ny
    ix = 1
    do ixc = 1, next%nx
      next%f(ixc,iyc) = 0.25_num * (this%residue(ix,iy) + this%residue(ix+1,iy) &
        & + this%residue(ix,iy+1) + this%residue(ix+1,iy+1) )
      ix = ix + 2
    enddo
    iy = iy + 2
    enddo

  end subroutine restrict

  subroutine inject(this)

    type(grid), pointer :: this
    type(grid), pointer :: prev
    integer :: ixc, iyc
    prev => this%prev

    iy=  1
    do iyc = 1, this%ny
    ix = 1
    do ixc = 1, this%nx
      prev%phi(ix,iy) = prev%phi(ix,iy) - this%phi(ixc,iyc)
      prev%phi(ix+1,iy) = prev%phi(ix+1,iy) - this%phi(ixc,iyc)
      prev%phi(ix,iy+1) = prev%phi(ix,iy+1) - this%phi(ixc,iyc)
      prev%phi(ix+1,iy+1) = prev%phi(ix+1,iy+1) - this%phi(ixc,iyc)
      ix = ix + 2
    enddo
    iy = iy + 2
    enddo

  end subroutine inject

  subroutine relax(this)

    type(grid), pointer :: this
    integer :: odd_then_even
    real(num) :: eta_ip, eta_im, eta_jp, eta_jm

    call bcs(this)

    if (eta_present) then

      do odd_then_even = 1, 0, -1
        do iy = 1, this%ny
        do ix = 1, this%nx
          if (modulo(ix+iy,2) == odd_then_even) then
            eta_ip = 0.5_num * (this%eta(ix,iy)+this%eta(ix+1,iy)) / this%dx**2
            eta_im = 0.5_num * (this%eta(ix,iy)+this%eta(ix-1,iy)) / this%dx**2
            eta_jp = 0.5_num * (this%eta(ix,iy)+this%eta(ix,iy+1)) / this%dy**2
            eta_jm = 0.5_num * (this%eta(ix,iy)+this%eta(ix,iy-1)) / this%dy**2

            this%phi(ix,iy) = eta_ip * this%phi(ix+1,iy) + eta_im*this%phi(ix-1,iy) &
              & + eta_jp * this%phi(ix,iy+1) + eta_jm * this%phi(ix,iy-1) - this%f(ix,iy)
            this%phi(ix,iy) = this%phi(ix,iy) / ( eta_ip + eta_im + eta_jp + eta_jm )
          endif
        end do
        end do
      enddo

    else ! eta not present

      do odd_then_even = 1, 0, -1
        do iy = 1, this%ny
        do ix = 1, this%nx
          if (modulo(ix+iy,2) == odd_then_even) then

            this%phi(ix,iy) = 0.25_num * ( &
              & this%phi(ix+1,iy) + this%phi(ix-1,iy) + this%phi(ix,iy+1) + this%phi(ix,iy-1) &
              - this%dx**2 * this%f(ix,iy) )
          endif
        end do
        end do
      enddo

    endif
  end subroutine relax

  subroutine residual(this)

    type(grid), pointer :: this

    real(num) :: Lap ! 5 point discrete laplacian at a cell center
    real(num) :: eta_ip, eta_im, eta_jp, eta_jm

    call bcs(this)

    if (eta_present) then

      do iy = 1, this%ny
      do ix = 1, this%nx
        eta_ip = 0.5_num * (this%eta(ix,iy)+this%eta(ix+1,iy))! / this%dx**2
        eta_im = 0.5_num * (this%eta(ix,iy)+this%eta(ix-1,iy))! / this%dx**2
        eta_jp = 0.5_num * (this%eta(ix,iy)+this%eta(ix,iy+1))! / this%dy**2
        eta_jm = 0.5_num * (this%eta(ix,iy)+this%eta(ix,iy-1))! / this%dy**2

        Lap = ( eta_ip * (this%phi(ix+1,iy)-this%phi(ix,iy)) &
              &    -  eta_im * (this%phi(ix,iy)-this%phi(ix-1,iy)) ) / this%dx**2 + &
              & ( eta_jp * (this%phi(ix,iy+1)-this%phi(ix,iy)) &
              &    -  eta_jm * (this%phi(ix,iy)-this%phi(ix,iy-1)) ) / this%dy**2

        this%residue(ix,iy) = Lap - this%f(ix,iy)
      enddo
      enddo

    else

      do iy = 1, this%ny
      do ix = 1, this%nx
        Lap = (this%phi(ix+1,iy) - 2.0_num*this%phi(ix,iy) + this%phi(ix-1,iy)) / this%dx**2 + &
            & (this%phi(ix,iy+1) - 2.0_num*this%phi(ix,iy) + this%phi(ix,iy-1)) / this%dy**2
        this%residue(ix,iy) = Lap - this%f(ix,iy)
      enddo
      enddo

    endif

  end subroutine residual

  subroutine eta_initialise
    ! restrict the residual at level "this" (this%residue)
    ! to be the rhs of the next level (next%f)

    type(grid), pointer :: current
    type(grid), pointer :: next
    integer :: ixc, iyc

    current => head
    next => current%next

    ! do the restrictions of cc eta
    do while( associated(next))
      iy=  1
      do iyc = 1, next%ny
      ix = 1
      do ixc = 1, next%nx
        next%eta(ixc,iyc) = 0.25_num * (current%eta(ix,iy) + current%eta(ix+1,iy) &
          & + current%eta(ix,iy+1) + current%eta(ix+1,iy+1) )
        ix = ix + 2
      enddo
      iy = iy + 2
      enddo
      current => current%next
      next => current%next
    enddo

    current => head
    do while( associated(current))
      ! fill the boundaries
      call eta_bcs(current)

! could be some saving by doing this once, at the start. Look into it later
      ! fill the x_face and y_face arrays
!      do iy = 0, current%ny
!      do ix = 0, current%nx
!        ! xface
!        if (iy /= 0) then
!          current%eta_xface(ix,iy) = 0.5_num * (current%eta(ix,iy) + current%eta(ix+1,iy))
!        endif
!        ! yface
!        if (ix /= 0) then
!          current%eta_yface(ix,iy) = 0.5_num * (current%eta(ix,iy) + current%eta(ix,iy+1))
!        endif
!      enddo
!      enddo
!      ! next level
      current => current%next
    enddo

  end subroutine eta_initialise

  subroutine eta_bcs(this)

    type(grid), pointer :: this

    ! some logic for if level 1 vs others for homo vs inhomo bc etc will be needed
    ! eventually

    if (bc_xmin == periodic) then
      this%eta(0,:) = this%eta(this%nx,:)
    endif
    if (bc_xmax == periodic) then
      this%eta(this%nx+1,:) = this%eta(1,:)
    endif
    if (bc_ymin == periodic) then
      this%eta(:,0) = this%eta(:,this%ny)
    endif
    if (bc_ymax == periodic) then
      this%eta(:,this%ny+1) = this%eta(:,1)
    endif

    if (bc_xmin == zero_gradient) then
      this%eta(0,:) = this%eta(1,:)
    endif

    if (bc_xmax == zero_gradient) then
      this%eta(this%nx+1,:) = this%eta(this%nx,:)
    endif
    if (bc_ymin == zero_gradient) then
      this%eta(:,0) = this%eta(:,1)
    endif
    if (bc_ymax == zero_gradient) then
      this%eta(:,this%ny+1) = this%eta(:,this%ny)
    endif

    if (bc_xmin == fixed) then
!      this%eta(0,:) = -this%eta(1,:)
      this%eta(0,:) = this%eta(1,:)
    endif
    if (bc_xmax == fixed) then
!      this%eta(this%nx+1,:) = -this%eta(this%nx,:)
      this%eta(this%nx+1,:) = this%eta(this%nx,:)
    endif
    if (bc_ymin == fixed) then
!      this%eta(:,0) = -this%eta(:,1)
      this%eta(:,0) = this%eta(:,1)
    endif
    if (bc_ymax == fixed) then
!      this%eta(:,this%ny+1) = -this%eta(:,this%ny)
      this%eta(:,this%ny+1) = this%eta(:,this%ny)
    endif

  end subroutine eta_bcs

  subroutine bcs(this)

    type(grid), pointer :: this
!    print *, 'test inside subroutine bcs'
!    print *,'****** bc_xmin = ', bc_xmin
!    print *,'****** bc_xmax = ', bc_xmax
!    print *,'****** bc_ymin = ', bc_ymin
!    print *,'****** bc_ymax = ', bc_ymax
!STOP
    ! some logic for if level 1 vs others for homo vs inhomo bc etc will be needed
    ! eventually

    if (bc_xmin == periodic) then
      this%phi(0,:) = this%phi(this%nx,:)
    endif
    if (bc_xmax == periodic) then
      this%phi(this%nx+1,:) = this%phi(1,:)
    endif
    if (bc_ymin == periodic) then
      this%phi(:,0) = this%phi(:,this%ny)
    endif
    if (bc_ymax == periodic) then
      this%phi(:,this%ny+1) = this%phi(:,1)
    endif

    if (bc_xmin == zero_gradient) then
      this%phi(0,:) = this%phi(1,:)
    endif

    if (bc_xmax == zero_gradient) then
      this%phi(this%nx+1,:) = this%phi(this%nx,:)
    endif
    if (bc_ymin == zero_gradient) then
      this%phi(:,0) = this%phi(:,1)
    endif
    if (bc_ymax == zero_gradient) then
      this%phi(:,this%ny+1) = this%phi(:,this%ny)
    endif

    if (bc_xmin == fixed) then
      this%phi(0,:) = -this%phi(1,:)
    endif
    if (bc_xmax == fixed) then
      this%phi(this%nx+1,:) = -this%phi(this%nx,:)
    endif
    if (bc_ymin == fixed) then
      this%phi(:,0) = -this%phi(:,1)
    endif
    if (bc_ymax == fixed) then
      this%phi(:,this%ny+1) = -this%phi(:,this%ny)
    endif

  end subroutine bcs

  ! check everything passed to the interface is as assumed

  subroutine sanity_checks(f,phi, eta, nx,ny,dx,dy,nlevels)
    integer, intent(in) :: nx
    integer, intent(in) :: ny
    real(num), dimension(:,:), intent(in) :: f
    real(num), dimension(:,:), allocatable, intent(inout) :: phi
    real(num), dimension(:,:), allocatable, optional, intent(in) :: eta
    integer, intent(in) :: nlevels
    real(num), intent(in) :: dx, dy

    if (dx /= dy) then
      print *,'multigrid: dx =/ dy, terminating'
      stop
    endif

    if (size(f,1) /= nx) then
      print *, 'wrong size on f input'
      print *,'size(f,1)=',size(f,1)
      stop
    endif

    if (size(f,2) /= ny) then
      print *, 'wrong size on f input'
      print *,'size(f,2)=',size(f,2)
      stop
    endif

    if (lbound(f,1) /= 1) then
      print *, 'wrong lbound on allocatable f input to MG'
      print *,'lbound(f,1)=',lbound(f,1)
      stop
    endif

    if (lbound(f,2) /= 1) then
      print *, 'wrong lbound on allocatable f input to MG'
      print *,'lbound(f,2)=',lbound(f,2)
      stop
    endif

    if (ubound(f,1) /= nx) then
      print *, 'wrong ubound on allocatable f input to MG'
      print *,'ubound(f,1)=',ubound(f,1)
      stop
    endif

    if (ubound(f,2) /= ny) then
      print *, 'wrong ubound on allocatable f input to MG'
      print *,'ubound(f,2)=',ubound(f,2)
      stop
    endif

    if (lbound(phi,1) /= -1) then
      print *, 'wrong lbound on allocatable phi input to MG'
      print *,'lbound(phi,1)=',lbound(phi,1)
      stop
    endif

    if (lbound(phi,2) /= -1) then
      print *, 'wrong lbound on allocatable phi input to MG'
      print *,'lbound(phi,2)=',lbound(phi,2)
      stop
    endif

    if (ubound(phi,1) /= nx+2) then
      print *, 'wrong ubound on allocatable phi input to MG'
      print *,'ubound(phi,1)=',ubound(phi,1)
      stop
    endif

    if (ubound(phi,2) /= ny+2) then
      print *, 'wrong ubound on allocatable phi input to MG'
      print *,'ubound(phi,2)=',ubound(phi,2)
      stop
    endif

    if (eta_present) then
      if (lbound(eta,1) /= 1 ) then
        print *, 'wrong lbound on allocatable eta input to MG'
        print *,'lbound(eta,1)=',lbound(eta,1)
        stop
      endif

      if (lbound(eta,2) /= 1 ) then
        print *, 'wrong lbound on allocatable eta input to MG'
        print *,'lbound(eta,2)=',lbound(eta,2)
        stop
      endif

      if (ubound(eta,1) /= nx) then
        print *, 'wrong ubound on allocatable eta input to MG'
        print *,'ubound(eta,1)=',ubound(eta,1)
        stop
      endif

      if (ubound(eta,2) /= ny) then
        print *, 'wrong ubound on allocatable eta input to MG'
         print *,'ubound(eta,2)=',ubound(eta,2)
        stop
      endif
    endif
  end subroutine sanity_checks

  ! Methods relating to the grid heirarchy and setup below jere

  subroutine create_grid(new_grid)
    type(grid), pointer :: new_grid
    allocate(new_grid)
    nullify(new_grid%next)
    nullify(new_grid%prev)
    new_grid%level = -1
    new_grid%nx = -1
    new_grid%ny = -1
    new_grid%dx = -1.0_num
    new_grid%dy = -1.0_num
  end subroutine create_grid

  subroutine add_grid(new_grid)
    ! add grid to a list / create a new list if 1st one
    type(grid), pointer :: new_grid
    if (.not. associated(head)) then
      head=>new_grid
      tail=>new_grid
      return
    endif
    tail%next=>new_grid
    new_grid%prev=>tail
    tail=>new_grid
  end subroutine add_grid

  subroutine allocate_arrays(new_grid)
    type(grid), pointer :: new_grid
    allocate(new_grid%phi(0:new_grid%nx+1,0:new_grid%ny+1))
    allocate(new_grid%f(1:new_grid%nx,1:new_grid%ny))
    allocate(new_grid%residue(1:new_grid%nx,1:new_grid%ny))
    new_grid%phi = 0.0_num
    new_grid%f = 0.0_num
    new_grid%residue = 0.0_num

    if (eta_present) then
      allocate(new_grid%eta(0:new_grid%nx+1,0:new_grid%ny+1))
      allocate(new_grid%eta_xface(0:new_grid%nx,1:new_grid%ny))
      allocate(new_grid%eta_yface(1:new_grid%nx,0:new_grid%ny))
    endif

  end subroutine allocate_arrays

  subroutine grid_report(this)
    type(grid), pointer :: this
      print *,'******'
      print *,'level',this%level
      print *,'nx',this%nx
      print *,'ny',this%ny
      print *,'dx',this%dx
      print *,'dy',this%dy
      print *,'lbound phi',lbound(this%phi)
      print *,'ubound phi',ubound(this%phi)
      print *,'lbound f',lbound(this%f)
      print *,'ubound f',ubound(this%f)

      if (eta_present) then
        print *,'lbound eta',lbound(this%eta)
        print *,'ubound eta',ubound(this%eta)
        print *,'lbound eta_xface',lbound(this%eta_xface)
        print *,'ubound eta_xface',ubound(this%eta_xface)
        print *,'lbound eta_yface',lbound(this%eta_yface)
        print *,'ubound eta_yface',ubound(this%eta_yface)
      endif

      print *,'******'
  end subroutine grid_report

  subroutine initialise_grids(f,eta, nx,ny,dx,dy, nlevels)

    real(num), dimension(:,:), intent(in) :: f
    real(num), dimension(:,:), allocatable, optional, intent(in) :: eta
    integer, intent(in) :: nx
    integer, intent(in) :: ny
    integer, intent(inout) :: nlevels
    real(num), intent(in) :: dx, dy
    integer :: lev

    type(grid), pointer :: new
    type(grid), pointer :: current
    nullify(new)
    nullify(current)
    nullify(head)
    nullify(tail)

    if (nlevels == -1) nlevels = set_nlevels(nx,ny)

    ! create a linked list of grids with blank / unallocated data
    do lev = 1, nlevels
      call create_grid(new)
      call add_grid(new)
    enddo

    ! go through the list and set up the arrays
    current =>head
    lev = 0
    do while(associated(current))
      lev = lev + 1
      current%level = lev
      current%nx = nx / (2**(lev-1))
      current%ny = ny / (2**(lev-1))
      current%dx = dx * real(2**(lev-1),num)
      current%dy = dy * real(2**(lev-1),num)
      call allocate_arrays(current)
      current=>current%next
    enddo

    ! set phi and f on the level-1 (finest) grid
    current => head
    current%f = f
    current%phi = 0.0_num
    if (eta_present) then
      current%eta(1:nx,1:ny) = eta
      call eta_initialise
    endif

! add to a debug / verbose option
!    ! cycle through the list for a report to check all is set up good
    current => head
    do while(associated(current))
      call grid_report(current)
      current=>current%next
    enddo
  end subroutine initialise_grids

  integer function set_nlevels(nx,ny)
    integer, intent(in) :: nx, ny

    if (nx <= ny) then
      set_nlevels = log2_int(nx) + 1
    else
      set_nlevels = log2_int(ny) + 1
    endif

  end function set_nlevels

  integer function log2_int(x)
    implicit none
    integer, intent(in) :: x

    log2_int = int( log(real(x,num)) / log(2.0_num))

  end function log2_int

end module multigrid


program test

    use stdlib_io_disp
    use stdlib_string_type
    call disp(string_type("x"))
    call disp([1, 2], "dada")
    block
        real(8) :: r(2, 6), r_2d(50, 20)
        complex :: c(2, 3), c_3d(2, 100, 20)
        integer :: i(2, 3)
        logical :: l(10, 10)
        r = 1.; c = 1.; c_3d = 2.; i = 1; l = .true.
        r(1, 1) = -1.e-11
        r(1, 2) = -1.e10
        c(2, 2) = (-1.e10, -1.e10)
        c_3d(1, 3, 1) = (1000, 0.001)
        c_3d(1, 3, 2) = (1.e4, 100.)
        call random_number(r_2d)
        call disp(string_type('string'), header='disp(string):')
        call disp(string_type('It is a note.'))
        call disp()
        call disp(r, header='disp(r):')
        call disp(r(1, :), header='disp(r(1,:))', brief=.true.)
        call disp(c, header='disp(c):', width=90)
        call disp(i, header='disp(i):')
        call disp(l, header='disp(l):', brief=.true., sep=", ")
        call disp(c_3d(:, 3, :), header='disp(c_3d(:,:,3)):', brief=.true., width=90)
        call disp(100*r_2d, header='disp(r_2d):', brief=.true., width=132, sep=", ")
    end block

    block
        type(string_type) :: s(2)
        s = string_type("abcdefghijk")
        call disp(s, header='disp(s):', width=6)
        call disp(s(1), header='disp(s):', width=6)

        s = string_type(repeat("abcdefghijk", 10))
        call disp(s, header='disp(s):', width=6)
        call disp(s(1), header='disp(s):', width=6)
        s(2) = string_type("abcdefghijk")
        call disp(s, header='disp(s):', width=6)
        call disp("xyz")

    end block

    block

    real    :: r(2, 3)
    complex :: c(2, 3), c_3d(2, 100, 20)
    integer :: i(2, 3)
    logical :: l(10, 10)

    r = 1.; c = 1.; c_3d = 2.; i = 1; l = .true.
    c_3d(1,3,1) = (1000, 0.001)

    call disp('string', header='disp(string):')
    call disp('It is a note.')
    call disp()
    call disp(r, header='disp(r):')
    call disp(r(1,:), header='disp(r(1,:))', format="f6.2")
    call disp(c, header='disp(c):')
    call disp(i, header='disp(i):', sep=",")
    call disp(l, header='disp(l):', brief=.true.)
    call disp(c_3d(:,3,1:10), header='disp(c_3d(:,3,1:10)):', width=100)
    call disp(c_3d(2,:,:), header='disp(c_3d(2,:,:)):', brief=.true.)

    end block

end program test