!> Subroutines for SOR iteration for the Crank--Nicholson step.
!!
!! These subroutines use flux-conservative differencing, and the cell
!! -averaged viscosities are copied from the master subroutine.
!!
!! @author Prashant Valluri, Lennon O Naraigh, Iain Bethunex
!! Toni Collis, David Scott, Peter Spelt.
!! @copyright (c) 2013-2015, Prashant Valluri, Lennon O Naraigh,
!! Iain Bethune, Toni Collis, David Scott, Peter Spelt.
!! This program is distributed under the BSD License See LICENSE.txt
!! for details.

module sor_iteration_allflux

  implicit none

contains

  subroutine do_sor_u(u3,RHS,viscosity,dx,dy,dz,dt,&
      ex,ey,sx,sy,maxn,ex_max,flg,iteration_time)

    implicit none

    ! Arguments
    integer,intent(in) :: sx,sy,ex,ey,flg
    integer,intent(in) :: maxn,iteration_time,ex_max

    double precision,intent(inout) :: u3(sx-1:ex+1,sy-1:ey+1,0:maxn-2)
    double precision,intent(in) :: RHS(sx-1:ex+1,sy-1:ey+1,0:maxn-2)
    double precision,intent(in) :: viscosity(sx-2:ex+2,sy-2:ey+2,0:maxn)
    double precision,intent(in) :: dx,dy,dz,dt

    ! Local variables
    integer :: i,j,k,ex_loc

    double precision :: diag_val,relax,residual,ax,ay,az
    double precision :: mu_plushalf_x_val,mu_minushalf_x_val
    double precision :: mu_plushalf_y_val,mu_minushalf_y_val
    double precision :: mu_plushalf_z_val,mu_minushalf_z_val
    double precision :: u_minusz,u_plusz

    relax=1.2d0

    ax=dt/(dx*dx)
    ay=dt/(dy*dy)
    az=dt/(dz*dz)

    if(ex==ex_max)then
       ex_loc=ex-1
    else
       ex_loc=ex
    end if

    if(mod(iteration_time,2).eq.0)then

       !$omp parallel default(shared), private(i,j,k,mu_plushalf_x_val,mu_minushalf_x_val, &
       !$omp& mu_plushalf_y_val,mu_minushalf_y_val,mu_plushalf_z_val,mu_minushalf_z_val,   &
       !$omp& diag_val,u_minusz,u_plusz,residual)
       !$omp do
       do k=0,maxn-2
          do j=sy,ey
             do i=mod(k+j+flg,2)+sx,ex_loc,2

                if(k.eq.0)then
                   u_minusz=-2.d0*u3(i,j,0)+u3(i,j,1)/3.d0
                else
                   u_minusz=u3(i,j,k-1)
                end if

                if(k.eq.(maxn-2))then
                   u_plusz=-2.d0*u3(i,j,maxn-2)+u3(i,j,maxn-3)/3.d0
                else
                   u_plusz=u3(i,j,k+1)
                end if

                mu_plushalf_x_val =viscosity(i+1,j+1,k+1)
                mu_minushalf_x_val=viscosity(i,  j+1,k+1)

                mu_plushalf_y_val =(viscosity(i,j+1,k+1)+viscosity(i+1,j+1,k+1)+viscosity(i,j+2,k+1)+viscosity(i+1,j+2,k+1))/4.d0
                mu_minushalf_y_val=(viscosity(i,j+1,k+1)+viscosity(i+1,j+1,k+1)+viscosity(i,j  ,k+1)+viscosity(i+1,j  ,k+1))/4.d0

                mu_plushalf_z_val =(viscosity(i,j+1,k+1)+viscosity(i+1,j+1,k+1)+viscosity(i,j+1,k+2)+viscosity(i+1,j+1,k+2))/4.d0
                mu_minushalf_z_val=(viscosity(i,j+1,k+1)+viscosity(i+1,j+1,k+1)+viscosity(i,j+1,k  )+viscosity(i+1,j+1,k  ))/4.d0

                diag_val=1.d0+(ax/2.d0)*(mu_plushalf_x_val+mu_minushalf_x_val)+&
                     (ay/2.d0)*(mu_plushalf_y_val+mu_minushalf_y_val)+&
                     (az/2.d0)*(mu_plushalf_z_val+mu_minushalf_z_val)

                residual = (1.d0/diag_val) * ( (ax/2.d0)*(mu_plushalf_x_val*u3(i+1,j,k)+mu_minushalf_x_val*u3(i-1,j,k)) + &
                     (ay/2.d0)*(mu_plushalf_y_val*u3(i,j+1,k)+mu_minushalf_y_val*u3(i,j-1,k)) + &
                     (az/2.d0)*(mu_plushalf_z_val*u_plusz    +mu_minushalf_z_val*u_minusz   ) + RHS(i,j,k))

                u3(i,j,k) = u3(i,j,k)+relax*(residual-u3(i,j,k))

             end do
          end do
       end do
       !$omp end do
       !$omp end parallel
    else

       !$omp parallel default(shared), private(i,j,k,mu_plushalf_x_val,mu_minushalf_x_val, &
       !$omp& mu_plushalf_y_val,mu_minushalf_y_val,mu_plushalf_z_val,mu_minushalf_z_val,   &
       !$omp& diag_val,u_minusz,u_plusz,residual)
       !$omp do
       do k=0,maxn-2
          do j=sy,ey
             do i=mod(j+flg,2)+sx,ex_loc,2

                if(k.eq.0)then
                   u_minusz=-2.d0*u3(i,j,0)+u3(i,j,1)/3.d0
                else
                   u_minusz=u3(i,j,k-1)
                end if

                if(k.eq.(maxn-2))then
                   u_plusz=-2.d0*u3(i,j,maxn-2)+u3(i,j,maxn-3)/3.d0
                else
                   u_plusz=u3(i,j,k+1)
                end if

                mu_plushalf_x_val =viscosity(i+1,j+1,k+1)
                mu_minushalf_x_val=viscosity(i,  j+1,k+1)

                mu_plushalf_y_val =(viscosity(i,j+1,k+1)+viscosity(i+1,j+1,k+1)+viscosity(i,j+2,k+1)+viscosity(i+1,j+2,k+1))/4.d0
                mu_minushalf_y_val=(viscosity(i,j+1,k+1)+viscosity(i+1,j+1,k+1)+viscosity(i,j  ,k+1)+viscosity(i+1,j  ,k+1))/4.d0

                mu_plushalf_z_val =(viscosity(i,j+1,k+1)+viscosity(i+1,j+1,k+1)+viscosity(i,j+1,k+2)+viscosity(i+1,j+1,k+2))/4.d0
                mu_minushalf_z_val=(viscosity(i,j+1,k+1)+viscosity(i+1,j+1,k+1)+viscosity(i,j+1,k  )+viscosity(i+1,j+1,k  ))/4.d0

                diag_val=1.d0+(ax/2.d0)*(mu_plushalf_x_val+mu_minushalf_x_val)+&
                     (ay/2.d0)*(mu_plushalf_y_val+mu_minushalf_y_val)+&
                     (az/2.d0)*(mu_plushalf_z_val+mu_minushalf_z_val)

                residual = (1.d0/diag_val) * ( (ax/2.d0)*(mu_plushalf_x_val*u3(i+1,j,k)+mu_minushalf_x_val*u3(i-1,j,k)) + &
                     (ay/2.d0)*(mu_plushalf_y_val*u3(i,j+1,k)+mu_minushalf_y_val*u3(i,j-1,k)) + &
                     (az/2.d0)*(mu_plushalf_z_val*u_plusz    +mu_minushalf_z_val*u_minusz   ) + RHS(i,j,k))

                u3(i,j,k) = u3(i,j,k)+relax*(residual-u3(i,j,k))

             end do
          end do
       end do
       !$omp end do
       !$omp end parallel
    end if

    return
  end subroutine do_sor_u

  subroutine do_sor_v(v3,RHS,viscosity,dx,dy,dz,dt, &
      ex,ey,sx,sy,maxn,flg,iteration_time)

    implicit none

    ! Arguments
    integer,intent(in) :: sx,sy,ex,ey,flg
    integer,intent(in) :: maxn,iteration_time

    double precision,intent(inout) :: v3(sx-1:ex+1,sy-1:ey+1,0:maxn-2)
    double precision,intent(in) :: RHS(sx-1:ex+1,sy-1:ey+1,0:maxn-2)
    double precision,intent(in) :: viscosity(sx-2:ex+2,sy-2:ey+2,0:maxn)
    double precision,intent(in) :: dx,dy,dz,dt

    ! Local variables
    integer :: i,j,k

    double precision :: diag_val,relax,residual,ax,ay,az
    double precision :: mu_plushalf_x_val,mu_minushalf_x_val
    double precision :: mu_plushalf_y_val,mu_minushalf_y_val
    double precision :: mu_plushalf_z_val,mu_minushalf_z_val
    double precision :: v_minusz,v_plusz

    relax=1.2d0
    ax=dt/(dx*dx)
    ay=dt/(dy*dy)
    az=dt/(dz*dz)

    if(mod(iteration_time,2).eq.0)then

       !$omp parallel default(shared), private(i,j,k,mu_plushalf_x_val,mu_minushalf_x_val, &
       !$omp& mu_plushalf_y_val,mu_minushalf_y_val,mu_plushalf_z_val,mu_minushalf_z_val,   &
       !$omp& diag_val,v_minusz,v_plusz,residual)
       !$omp do
       do k=0,maxn-2
          do j=sy,ey
             do i=mod(k+j+flg,2)+sx,ex,2

                if(k.eq.0)then
                   v_minusz=-2.d0*v3(i,j,0)+v3(i,j,1)/3.d0
                else
                   v_minusz=v3(i,j,k-1)
                end if

                if(k.eq.(maxn-2))then
                   v_plusz=-2.d0*v3(i,j,maxn-2)+v3(i,j,maxn-3)/3.d0
                else
                   v_plusz=v3(i,j,k+1)
                end if

                mu_plushalf_x_val= (viscosity(i,j+1,k+1)+viscosity(i,j,k+1)+viscosity(i+1,j+1,k+1)+viscosity(i+1,j,k+1))/4.d0
                mu_minushalf_x_val=(viscosity(i,j+1,k+1)+viscosity(i,j,k+1)+viscosity(i-1,j+1,k+1)+viscosity(i-1,j,k+1))/4.d0

                mu_plushalf_y_val= viscosity(i,j+1,k+1)
                mu_minushalf_y_val=viscosity(i,j,  k+1)

                mu_plushalf_z_val= (viscosity(i,j+1,k+1)+viscosity(i,j,k+1)+viscosity(i,j+1,k+2)+viscosity(i,j,k+2))/4.d0
                mu_minushalf_z_val=(viscosity(i,j+1,k+1)+viscosity(i,j,k+1)+viscosity(i,j+1,k  )+viscosity(i,j,k  ))/4.d0

                diag_val=1.d0+(ax/2.d0)*(mu_plushalf_x_val+mu_minushalf_x_val)+&
                     (ay/2.d0)*(mu_plushalf_y_val+mu_minushalf_y_val)+&
                     (az/2.d0)*(mu_plushalf_z_val+mu_minushalf_z_val)

                residual = (1.d0/diag_val) * ( (ax/2.d0)*(mu_plushalf_x_val*v3(i+1,j,k)+mu_minushalf_x_val*v3(i-1,j,k)) + &
                     (ay/2.d0)*(mu_plushalf_y_val*v3(i,j+1,k)+mu_minushalf_y_val*v3(i,j-1,k)) + &
                     (az/2.d0)*(mu_plushalf_z_val*v_plusz    +mu_minushalf_z_val*v_minusz   ) + RHS(i,j,k))

                v3(i,j,k) = v3(i,j,k)+relax*(residual-v3(i,j,k))

             end do
          end do
       end do
       !$omp end do
       !$omp end parallel
    else

       !$omp parallel default(shared), private(i,j,k,mu_plushalf_x_val,mu_minushalf_x_val, &
       !$omp& mu_plushalf_y_val,mu_minushalf_y_val,mu_plushalf_z_val,mu_minushalf_z_val,   &
       !$omp& diag_val,v_minusz,v_plusz,residual)
       !$omp do
       do k=0,maxn-2
          do j=sy,ey
             do i=mod(j+flg,2)+sx,ex,2

                if(k.eq.0)then
                   v_minusz=-2.d0*v3(i,j,0)+v3(i,j,1)/3.d0
                else
                   v_minusz=v3(i,j,k-1)
                end if

                if(k.eq.(maxn-2))then
                   v_plusz=-2.d0*v3(i,j,maxn-2)+v3(i,j,maxn-3)/3.d0
                else
                   v_plusz=v3(i,j,k+1)
                end if

                mu_plushalf_x_val= (viscosity(i,j+1,k+1)+viscosity(i,j,k+1)+viscosity(i+1,j+1,k+1)+viscosity(i+1,j,k+1))/4.d0
                mu_minushalf_x_val=(viscosity(i,j+1,k+1)+viscosity(i,j,k+1)+viscosity(i-1,j+1,k+1)+viscosity(i-1,j,k+1))/4.d0

                mu_plushalf_y_val= viscosity(i,j+1,k+1)
                mu_minushalf_y_val=viscosity(i,j,  k+1)

                mu_plushalf_z_val= (viscosity(i,j+1,k+1)+viscosity(i,j,k+1)+viscosity(i,j+1,k+2)+viscosity(i,j,k+2))/4.d0
                mu_minushalf_z_val=(viscosity(i,j+1,k+1)+viscosity(i,j,k+1)+viscosity(i,j+1,k  )+viscosity(i,j,k  ))/4.d0

                diag_val=1.d0+(ax/2.d0)*(mu_plushalf_x_val+mu_minushalf_x_val)+&
                     (ay/2.d0)*(mu_plushalf_y_val+mu_minushalf_y_val)+&
                     (az/2.d0)*(mu_plushalf_z_val+mu_minushalf_z_val)

                residual = (1.d0/diag_val) * ( (ax/2.d0)*(mu_plushalf_x_val*v3(i+1,j,k)+mu_minushalf_x_val*v3(i-1,j,k)) + &
                     (ay/2.d0)*(mu_plushalf_y_val*v3(i,j+1,k)+mu_minushalf_y_val*v3(i,j-1,k)) + &
                     (az/2.d0)*(mu_plushalf_z_val*v_plusz    +mu_minushalf_z_val*v_minusz   ) + RHS(i,j,k))

                v3(i,j,k) = v3(i,j,k)+relax*(residual-v3(i,j,k))

             end do
          end do
       end do
       !$omp end do
       !$omp end parallel
    end if

    return
  end subroutine do_sor_v

  subroutine do_sor_w(w3,RHS,viscosity,dx,dy,dz,dt, &
      ex,ey,sx,sy,maxn,flg,iteration_time)

    implicit none

    ! Arguments
    integer,intent(in) :: sx,sy,ex,ey,flg
    integer,intent(in) :: maxn,iteration_time

    double precision,intent(inout) :: w3(sx-1:ex+1,sy-1:ey+1,0:maxn-1)
    double precision,intent(in) :: RHS(sx-1:ex+1,sy-1:ey+1,0:maxn-1)
    double precision,intent(in) :: viscosity(sx-2:ex+2,sy-2:ey+2,0:maxn)
    double precision,intent(in) :: dx,dy,dz,dt

    ! Local variables
    integer :: i,j,k

    double precision :: diag_val,relax,residual,ax,ay,az
    double precision :: mu_plushalf_x_val,mu_minushalf_x_val
    double precision :: mu_plushalf_y_val,mu_minushalf_y_val
    double precision :: mu_plushalf_z_val,mu_minushalf_z_val

    relax=1.2d0
    ax=dt/(dx*dx)
    ay=dt/(dy*dy)
    az=dt/(dz*dz)

    if(mod(iteration_time,2).eq.0)then

       !$omp parallel default(shared), private(i,j,k,mu_plushalf_x_val,mu_minushalf_x_val, &
       !$omp& mu_plushalf_y_val,mu_minushalf_y_val,mu_plushalf_z_val,mu_minushalf_z_val,   &
       !$omp& diag_val, residual)
       !$omp do
       do k=1,maxn-2
          do j=sy,ey
             do i=mod(k+j+flg,2)+sx,ex,2

                mu_plushalf_x_val= (viscosity(i,j+1,k)+viscosity(i,j+1,k+1)+viscosity(i+1,j+1,k)+viscosity(i+1,j+1,k+1))/4.d0
                mu_minushalf_x_val=(viscosity(i,j+1,k)+viscosity(i,j+1,k+1)+viscosity(i-1,j+1,k)+viscosity(i-1,j+1,k+1))/4.d0

                mu_plushalf_y_val= (viscosity(i,j+1,k)+viscosity(i,j+1,k+1)+viscosity(i,j+2,k)+viscosity(i,j+2,k+1))/4.d0
                mu_minushalf_y_val=(viscosity(i,j+1,k)+viscosity(i,j+1,k+1)+viscosity(i,j  ,k)+viscosity(i,j  ,k+1))/4.d0

                mu_plushalf_z_val=  viscosity(i,j+1,k+1)
                mu_minushalf_z_val= viscosity(i,j+1,k  )

                diag_val=1.d0+(ax/2.d0)*(mu_plushalf_x_val+mu_minushalf_x_val)+&
                     (ay/2.d0)*(mu_plushalf_y_val+mu_minushalf_y_val)+&
                     (az/2.d0)*(mu_plushalf_z_val+mu_minushalf_z_val)

                residual = (1.d0/diag_val) * ( (ax/2.d0)*(mu_plushalf_x_val*w3(i+1,j,k)+mu_minushalf_x_val*w3(i-1,j,k)) + &
                     (ay/2.d0)*(mu_plushalf_y_val*w3(i,j+1,k)+mu_minushalf_y_val*w3(i,j-1,k)) + &
                     (az/2.d0)*(mu_plushalf_z_val*w3(i,j,k+1)+mu_minushalf_z_val*w3(i,j,k-1)) + RHS(i,j,k))

                w3(i,j,k) = w3(i,j,k)+relax*(residual-w3(i,j,k))
             end do
          end do
       end do
       !$omp end do
       !$omp end parallel

    else

       !$omp parallel default(shared), private(i,j,k,mu_plushalf_x_val,mu_minushalf_x_val, &
       !$omp& mu_plushalf_y_val,mu_minushalf_y_val,mu_plushalf_z_val,mu_minushalf_z_val,   &
       !$omp& diag_val,residual)
       !$omp do
       do k=1,maxn-2
          do j=sy,ey
             do i=mod(j+flg,2)+sx,ex,2

                mu_plushalf_x_val= (viscosity(i,j+1,k)+viscosity(i,j+1,k+1)+viscosity(i+1,j+1,k)+viscosity(i+1,j+1,k+1))/4.d0
                mu_minushalf_x_val=(viscosity(i,j+1,k)+viscosity(i,j+1,k+1)+viscosity(i-1,j+1,k)+viscosity(i-1,j+1,k+1))/4.d0

                mu_plushalf_y_val= (viscosity(i,j+1,k)+viscosity(i,j+1,k+1)+viscosity(i,j+2,k)+viscosity(i,j+2,k+1))/4.d0
                mu_minushalf_y_val=(viscosity(i,j+1,k)+viscosity(i,j+1,k+1)+viscosity(i,j  ,k)+viscosity(i,j  ,k+1))/4.d0

                mu_plushalf_z_val=  viscosity(i,j+1,k+1)
                mu_minushalf_z_val= viscosity(i,j+1,k  )

                diag_val=1.d0+(ax/2.d0)*(mu_plushalf_x_val+mu_minushalf_x_val)+&
                     (ay/2.d0)*(mu_plushalf_y_val+mu_minushalf_y_val)+&
                     (az/2.d0)*(mu_plushalf_z_val+mu_minushalf_z_val)

                residual = (1.d0/diag_val) * ( (ax/2.d0)*(mu_plushalf_x_val*w3(i+1,j,k)+mu_minushalf_x_val*w3(i-1,j,k)) + &
                     (ay/2.d0)*(mu_plushalf_y_val*w3(i,j+1,k)+mu_minushalf_y_val*w3(i,j-1,k)) + &
                     (az/2.d0)*(mu_plushalf_z_val*w3(i,j,k+1)+mu_minushalf_z_val*w3(i,j,k-1)) + RHS(i,j,k))

                w3(i,j,k) = w3(i,j,k)+relax*(residual-w3(i,j,k))
             end do
          end do
       end do
       !$omp end do
       !$omp end parallel
    end if

    return
  end subroutine do_sor_w

end module sor_iteration_allflux


*DECK DLNGAM
      DOUBLE PRECISION FUNCTION DLNGAM (X)
C***BEGIN PROLOGUE  DLNGAM
C***PURPOSE  Compute the logarithm of the absolute value of the Gamma
C            function.
C***LIBRARY   SLATEC (FNLIB)
C***CATEGORY  C7A
C***TYPE      DOUBLE PRECISION (ALNGAM-S, DLNGAM-D, CLNGAM-C)
C***KEYWORDS  ABSOLUTE VALUE, COMPLETE GAMMA FUNCTION, FNLIB, LOGARITHM,
C             SPECIAL FUNCTIONS
C***AUTHOR  Fullerton, W., (LANL)
C***DESCRIPTION
C
C DLNGAM(X) calculates the double precision logarithm of the
C absolute value of the Gamma function for double precision
C argument X.
C
C***REFERENCES  (NONE)
C***ROUTINES CALLED  D1MACH, D9LGMC, DGAMMA, XERMSG
C***REVISION HISTORY  (YYMMDD)
C   770601  DATE WRITTEN
C   890531  Changed all specific intrinsics to generic.  (WRB)
C   890531  REVISION DATE from Version 3.2
C   891214  Prologue converted to Version 4.0 format.  (BAB)
C   900315  CALLs to XERROR changed to CALLs to XERMSG.  (THJ)
C   900727  Added EXTERNAL statement.  (WRB)
C***END PROLOGUE  DLNGAM
      DOUBLE PRECISION X, DXREL, PI, SINPIY, SQPI2L, SQ2PIL, XMAX,
     1  Y, DGAMMA, D9LGMC, D1MACH, TEMP
      LOGICAL FIRST
      EXTERNAL DGAMMA
      SAVE SQ2PIL, SQPI2L, PI, XMAX, DXREL, FIRST
      DATA SQ2PIL / 0.9189385332 0467274178 0329736405 62 D0 /
      DATA SQPI2L / +.2257913526 4472743236 3097614947 441 D+0    /
      DATA PI / 3.1415926535 8979323846 2643383279 50 D0 /
      DATA FIRST /.TRUE./
C***FIRST EXECUTABLE STATEMENT  DLNGAM
      IF (FIRST) THEN
         TEMP = 1.D0/LOG(D1MACH(2))
         XMAX = TEMP*D1MACH(2)
         DXREL = SQRT(D1MACH(4))
      ENDIF
      FIRST = .FALSE.
C
      Y = ABS (X)
      IF (Y.GT.10.D0) GO TO 20
C
C LOG (ABS (DGAMMA(X)) ) FOR ABS(X) .LE. 10.0
C
      DLNGAM = LOG (ABS (DGAMMA(X)) )
      RETURN
C
C LOG ( ABS (DGAMMA(X)) ) FOR ABS(X) .GT. 10.0
C
 20   IF (Y .GT. XMAX) CALL XERMSG ('SLATEC', 'DLNGAM',
     +   'ABS(X) SO BIG DLNGAM OVERFLOWS', 2, 2)
C
      IF (X.GT.0.D0) DLNGAM = SQ2PIL + (X-0.5D0)*LOG(X) - X + D9LGMC(Y)
      IF (X.GT.0.D0) RETURN
C
      SINPIY = ABS (SIN(PI*Y))
      IF (SINPIY .EQ. 0.D0) CALL XERMSG ('SLATEC', 'DLNGAM',
     +   'X IS A NEGATIVE INTEGER', 3, 2)
C
      IF (ABS((X-AINT(X-0.5D0))/X) .LT. DXREL) CALL XERMSG ('SLATEC',
     +   'DLNGAM',
     +   'ANSWER LT HALF PRECISION BECAUSE X TOO NEAR NEGATIVE INTEGER',
     +   1, 1)
C
      DLNGAM = SQPI2L + (X-0.5D0)*LOG(Y) - X - LOG(SINPIY) - D9LGMC(Y)
      RETURN
C
      END


c
c
c=========================================================
      subroutine qinit(meqn,mbc,mx,xlower,dx,q,maux,aux)
c=========================================================
c
c     # Set initial conditions for q.
c     # Pulse in pressure, zero velocity
c
c
      implicit none

      integer, intent(in) :: meqn, mbc, mx, maux
      double precision, intent(in) :: xlower, dx, aux
      double precision, intent(out) :: q
      dimension q(meqn, 1-mbc:mx+mbc)
      dimension aux(maux, 1-mbc:mx+mbc)

      common /cqinit/ beta
      double precision beta

      integer i
      double precision xcell, pi, a
      pi = 4.d0*datan(1.d0)
      a = dsqrt(beta/pi)
c
c
      do 150 i=1,mx
         xcell = xlower + (i-0.5d0)*dx

         if (xcell.gt.0d0 .and. xcell.lt.9d0) then
             q(1,i) = a*dexp(-beta*(xcell-4.5d0)**2)
         else
             q(1,i) = 0.d0
         endif

         q(2,i) = 0.d0
 150  continue
c
      return
      end


!
!   FnVFN.f90
!
!
!   Created by Kurt Mitman on 10/10/14.
!
!

DOUBLE PRECISION FUNCTION FnVFN(bsav)

USE Params
USE Globals
USE funcs
USE Procedures

IMPLICIT NONE

DOUBLE PRECISION, INTENT(IN) :: bsav
DOUBLE PRECISION             :: cons,qltemp,qltemp2
DOUBLE PRECISION             :: vals(2)
INTEGER                      :: inds(2)

call basefun(GridB,nb,bsav,vals,inds)

call BiLinInterp1(nb,GridB,ngpPh,PhGrid,ql(:,gil,ginds(1),gih,giExo,:,gij+1),bsav,gPh,qltemp)
call BiLinInterp1(nb,GridB,ngpPh,PhGrid,ql(:,gil,ginds(2),gih,giExo,:,gij+1),bsav,gPh,qltemp2)

cons=gliq+(gvals(1)*qltemp+gvals(2)*qltemp2)*gloc*ghouse-FnQb(bsav,gPh*ghouse-gmort)*bsav

if(cons .GT. cmin) then

FnVFN = -1.0d0*(uown(cons,ghouse,gij)+bet*(vals(1)*sum(gvals*EVown(inds(1),gil,ginds,gih,gij))+vals(2)*sum(gvals*EVown(inds(2),gil,ginds,gih,gij))))

else

FnVFN = 1.0d10+10.0d0*(cmin-cons)

endif

END FUNCTION FnVFN


      subroutine spherv(r,nfi,nteta,dx,dy,dz,nlines,nbeg)
           dimension dx(2,nlines),dy(2,nlines),dz(2,nlines)
C  ***************************************************
C    "nfi" semi-circles per "nteta" lines.
C    the total number of lines is "nfi*nteta".
C  ***************************************************
      k=nbeg
      dteta=3.14159265/nteta
      dfi=2.*3.14159265/nfi
C
      fi=0.
      do 4 i=1,nfi
      teta=0.
      do 3 j=1,nteta
      dz(1,k)=r*sin(teta)*cos(fi)
      dx(1,k)=r*sin(teta)*sin(fi)
      dy(1,k)=r*cos(teta)
      teta=teta+dteta
      dz(2,k)=r*sin(teta)*cos(fi)
      dx(2,k)=r*sin(teta)*sin(fi)
      dy(2,k)=r*cos(teta)
      k=k+1
3      continue
      fi=fi+dfi
4      continue
      return
      end


c                    ALL RIGHTS RESERVED!
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
c        program FDM_RRT_1D_FinalVersion_CJH
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
c     1. NOTE:    USE OR DISTRIBUTE WITH PERMISSION ONLY.
c
c     2. NAME:    fd_rrt1d.f
c
c     3. AUTHORS: Jianhan Chen (jianhanc@uci.edu)
c                 V. A. Mandelshtam (mandelsh@uci.edu)
c
c     4. DATE:    10-10-2001
c
c     5. PURPOSE: 1D FDM/RRT/DFT
c
C              FDM = Filter Diagonalization Method
c              RRT = Regularized Resolvent Transform
c              DFT = Discrete Fourier Transform
C
c     6. GENERAL DESCRIPTION:
c
c      The program is based on the FDM and RRT algorithms for high resolution
c      spectral analysis of time domain signals. See references for details.
c      Given a signal c(n) = C(t0+tau*n) n=0, 1, ..., Nsig, calculate the
c      line list {w_k, d_k} and/or the complex spectrum F(w) by fitting c(n)
c      to the Auto-Regressive (AR) model,
c
c                 c(n) = sum_k d_k exp(-i n w_k)        (1)
c
c      where, w_k is the complex frequency (line position & width)
c            d_k is the complex amplitude 30482(phase & intregral)
c
c      1) in FDM, the complex spectrum is computed using the formula,
c
c                 F(w) = sum_k d_k/(w-w_k+i*Gamm)       (2)
c
c         in RRT, it is estimated as,
c
c                 F(w) ~ Cv 1/R(w) Cv'                    (3)
c
c      2). QZ-algorithm is applied to solve the generalized eigenvalue
c      problem. ZGESV routine from LAPACK is used to invert matrix R(w).
c
c     7. INPUTS:
C
C      IT IS IMPORTANT THAT YOU UNDERSTAND SOME BASIC IDEAS OF FDM/RRT
C      IN ORDER TO UNDERSTAND THE MEANING OF SOME INPUTS SUCH AS BASIS
C      SIZE, BASIS DENSITY. IN SUMMARY, FDM/RRT PROCESS THE TIME SIGNAL
C      BY DIVIDING THE WHOLE NYQUIST RANGE INTO ONE OR SEVERAL LOCAL
C      WINDOWS IN THE FREQUENCY DOMAIN.
c
c      signal: input data file with appropriate first line. The data
c              file is assumed to contain an ASCII formatted list of
c              c(n\tau). The first line must be as following:
c
c               dim { NSigmax tau }_[1,..,dim] {idat}_[1,...,dim]
c
c              where dim    : the dimension of the signal (dim=1, 2, ...)
c                    Nsigmax: the maximum signal length
c                    tau    : time step (acquistion inteval)
c                    idat   : data format along each dimension
c               e.g/ 1D example: 1 1024 0.001 1
c                    2D example: 2 2024 0.001 128 0.0001 1 1
c
c              Note: 'idat' implies the data tormat. In 1D, it is defined
c              as following:
c                     idat>0 read real signal, idat<0 read complex signal
c                     |idat| = 1 read c_n
c                     |idat| = 2 read t_n,c_n
c
c      t0    : 1st order (i.e., linear) phase correction (in seconds)
c              NOTE: t0 is not used in this code.
c      theta : zero order (overall) phase correction
c      method: method used to compute the spectra: FDM, RRT, or DFT
c      Nsig  : number of data points to be used in computing the spectra.
c              It will be automatically reduced to Nsigmax (maximum length)
c              if specified Nsig is greater than Nsigmax.
c      wmino : (see wmaxo)
c      wmaxo : specify the frequency range of the spectra to be computed.
c              It might be adjusted to fit harmonically with the basis size
c              specified (see Nb0, rho) and to fit in the Nyquist Range.
c
c      threshhold: see par below.
c      par   : file where the linelist will be written (FDM only).
c              Only those with |d(k)| > threshhold will be output.
c      ReSp  : file where Re[F(w)] will be written
c      ImSp  : file where Im[F(w)] will be written
c      AbsSp : file where Abs[F(w)] will be written
c              NOTE: if file name is specified as 'none' or 'None', no
c                    corresponding output will be written.

c      rho   : basis density (default = 1.1). rho > 1.0 means putting
c              more basis functions than default. For example, rho=1.1
c              means putting 10% more basis functions then default.
c              rho should not be less than 1.0. rho = 1.5 should be
c              considered as the maximum. For typical data, rho = 1.1-1.2
c              will usually work the best. if rho < 0, default is used.
c      Nb0   : number of narrow band Fourier basis functions used per
c              window (see references). Typical value is about 50-300.
c              Large Nb0 means bigger spectral range per window. The result
c              might be more stable but the calculation will be slower for
c              larger Nb0. Except for very long signal (say, Nsig > 10,000),
c              Nb0 = 150 will be a good choice.
c      Nbc   : number of broad band Fourier basis functions (coarse basis).
c              See second reference for more details. Nbc <= 0 means no
c              coarse basis. Adding coarse basis (Nbc>0) will usually give
c              stabler results and more accurate linelist, especially when
c              the signal is noisy or contains non-localized features such
c              as broad lines, backgrounds. Typical Nbc = 10 ~ 50. A good
c              choice could be Nbc = 20.
c
c      Nsp   : number of points used in plotting the spectra. Defines the
c              digital resolution of output spectra.
c      Gamm  : smoothing parameter. Defines the smallest possible linewidth.
c              Default Gamm = 0.2 * 1/(N*tau). FDM/RRT might give some very
c              narrow lines (spikes) due to noise. A non-zero Gamm will
c              improve the looking of resulted spectra. if Gamm < 0, default
c              will be used.
c      cheat : multiply all widths by cheat (FDM only).
c              NOTE: be very careful to use cheat < 1.0. It might lead to
c                    very misleading results. Use cheat=1.0 only.
c      cheatmore: if is .true. F(w) is computed with Im d_k (FDM only).
c              NOTE: be very careful again. Default: cheatmore = .false.
c
c      ros   : regularization parameter (RRT only). If ros < = 0, no
c              regularization in RRT. See 3rd reference for more details.
c              In general, bigger ros means more 'regularization', and will
c              lead to more stable result but will lower resolution.
c              Qualitively, ros is related to the amount of noise present
c              in the signal. Noisy data requires bigger regularization.
c              For typical NMR signals with fine S/N (say, 500M magnet,
c              1 mM sample, normal probe, 4-16 scans), ros = 1d-6 should
c              a good guess. If you see artifacts (very easy to tell if
c              you know what a typical NMR peak will look like), increase
c              ros (say, double the value) and run again. If the spectrum
c              look very clean but the resolution is very poor, it may
c              indicate too big regularization. Decrease ros and run again.
c              NOTE: Choosing an optimal value for ros is a try-and-error
c                    game. Typically, there will be a flat regime where
c                    the spectra remains similar even changing ros by order
c                    of a magnitude. Try following:
c
c                              too many artifacts
c                  ros=1d-6---------------  ros = ros*10 = 1d-5 --> cont.
c                            | clean, but low res.
c                            -------------  ros = ros/10 = 1d-7 --> cont.
c                            | GREAT!
c                            --------- done!
c
c      *** SAMPLE INPUT FILE: TO PROCESS A SIGNAL STORED IN 'signal.txt'
c
c       'signal.txt'        	                /signal
c       1.57                     		/theta
c       1                               	/ispec
c       1000	                            	/Nsig
c       -10 10 	                  		/wmin wmax
c       'par'	 	                      	/parameters file
c       'ft','none','none' 	          	/ReSp,ImSp,AbsSp files
c       1., 200, -20                          	/rho, Nb0, Nbc
c       3000, 1d-2				/Nsp, Gamm
c       1 F                             	/cheat, cheatmore
c       5d-7                             	/ros
c
c     8. OUTPUTS: can be found in the spectra and parameter files specified
c        in the input file. When multi-scale is used, the position of basis
c        functions can be found in file 'fort.11'.
c
c     9. REFERENCES
c
c      FDM/RRT:
c       H. Hu et. al., J. Magn. Reson. 134, 76-87 (1998).
c       J. Chen and V. A. Mandelshtam, J. Chem. Phys. 112, 4429-4437 (2000).
c       J. Chen, et.al., J. Magn. Reson. 147, 129-137 (2000).
c       V. A. Mandelshtam, Prog. Nuc. Magn. Reson. Spect. 38, 159-196 (2001).
c      QZ: http://www.netlib.org/toms/535
c      ZGESV/LAPACK: http://www.netlib.org/lapack/index.html
c
c     Questions and comments should be directed to V. A. Mandelshtam.
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
c Following soubroutine was originally the calling main() fortran program. Trying
c to convert to a subr and then f2py it and then call from python, passing it the
c input params via arguments.
c calling python program handles params, cmdline arg, database calls, etc.

      subroutine fd_rrt1d(signal,theta,method,Nsig,wmino,wmaxo,par,
     &     threshhold,ReSp,ImSp,AbsSp,rho,Nb0,Nbc,Nsp,Gamm,cheat,
     &     cheatmore,ros)
      implicit none
      integer Nb,Nb0,Nbc,M,Nsig,idat,Nsig1max,NWin,ispec,Dim,Nz,Nsp,
     &     Nbmax,Nsigmax,Npowermax,iminl,imaxl,imeanl,nw,N0,
     &     i, j, k, n, di,subcnt
      parameter (Nsigmax=99999,Npowermax=99999,Nbmax=1500)
      real*8 theta,cheat,pi,Omega,Gamm,delt,wmin,wmax,
     &     wmino,wmaxo,rho,dOmega,dW,wminl,wmaxl,wmeanl,
     &     tt,r1,r2,r3,ros, threshhold
      complex*16 wk(Nbmax),dk(Nbmax),phase_corr,Spec(0:Npowermax),
     &     cc,dd,uu,Z1,xi,coef(0:Nsigmax),Spec_win(0:Npowermax)
      complex*16 gsave(Nbmax), U(Nbmax*Nbmax*3), heap(4*Nbmax**2)
      character*30 par,signal,AbsSp,ReSp,ImSp,method
      logical cheatmore,checkfile

      write(*,001)
 001  format(/,'  1D FDM/RRT/DFT with Multi-Scale option, Beta version',
     &     /,  '       J. Chen and V. A. Mandelshtam, UC-Irvine',/)
c
c     READ IN THE PARAMTERS
c
c      read(5,*) signal           ! signal file
c      read(5,*) theta            ! zero, order phase correction
c      read(5,*) method            ! FDM, RRT or DFT
c      read(5,*) Nsig             ! signal length to be used
c      read(5,*) wmino,wmaxo      ! window size
c      read(5,*) par, threshhold  ! linelist output file
c      read(5,*) ReSp,ImSp,AbsSp  ! spectra output files
c      read(5,*) rho, Nb0, Nbc    ! basis density, basis size, coarse basis
c      read(5,*) Nsp, Gamm        ! plotting pnts, smoothing
c      read(5,*) cheat, cheatmore ! used in FDM, see discriptions.
c      read(5,*) ros              ! RRT regularization parameter
c
c     CHECK THE PARAMETERS
c
      write(*,*) 'signal = ', signal
      write(*,*) 'theta  = ', theta
      write(*,*) 'method  = ', method
      write(*,*) 'Nsig  = ', Nsig
      write(*,*) 'wmino, wmaxo  = ', wmino, wmaxo
      write(*,*) 'par, threshhold    = ', par, threshhold
      write(*,*) 'ReSp,ImSp,AbsSp  = ', ReSp,ImSp,AbsSp
      write(*,*) 'rho, Nb0, Nbc,  = ', rho, Nb0, Nbc
      write(*,*) 'Nsp, Gamm = ', Nsp, Gamm
      write(*,*) 'cheat, cheatmore, ros = ', cheat, cheatmore, ros

      xi=(0d0,1d0)
      pi=dacos(-1d0)
      open(15,file=signal,status='old')
      write(6,*) 'Read the signal from: ', signal
      read(15,*) Dim, Nsig1max, delt, (i, r1, k=1,Dim-1), idat
      if(Dim.le.0.or.delt.eq.0.or.Nsig1max.le.0)
     &     stop 'Error: invalid entries in 1st line of the signal!'
      if(Nsig.gt.Nsig1max-1) Nsig=Nsig1max-1
      if(Dim.ne.1) idat = 0     ! multi-D data, idat = 0
      if(Nsig.gt.Nsigmax) stop 'Error: Nsig > Maximum allowed!'
      r2=0d0
      r3=0d0                    ! r3 = sum_n |c(n)|^2
      phase_corr=cdexp(dcmplx(0d0,theta))
      subcnt=0
      do n=0,Nsig
         if(idat.eq.-1) then
            read(15,*,end=100) r1,r2
         else if(idat.eq.-2) then
            read(15,*,end=100) tt,r1,r2
         else if(idat.eq.1) then
            read(15,*,end=100) r1
         else if(idat.eq.2) then
            read(15,*,end=100) tt,r1
         else
            read(15,*,end=100) r1,r2 ! default format
         endif
         coef(n)=dcmplx(r1,r2)*phase_corr
         r3=r3+cdabs(coef(n))**2
      enddo
      goto 101
 100  Nsig=n-1
 101  M=(Nsig-2)/2
      Nsig=2*M+2
      write(6,9991) 2*M+3
 9991 format(' ', I5, ' c_n are read and to be used')
      ros=ros*r3*(M+1d0)**2     ! actual regularization parameter
c
      wmin=wmino*delt
      wmax=wmaxo*delt
      if(wmin.gt.wmax) then
         r1 = wmin
         wmin = wmax
         wmax = r1
      endif
      if(wmin.gt.pi.or.wmax.lt.-pi)
     &     stop 'Error: invalid spectral range, abort!'
      if(wmin.lt.-pi) wmin=-pi
      if(wmax.gt.pi) wmax=pi
      wmino=wmin/delt
      wmaxo=wmax/delt
      write(6,9992) wmino,wmaxo
 9992 format(' Spectral range to be processed: [',F10.3,', ',F10.3,']')
c      if(Gamm.lt.0) write(6,*) 'Warning: Gamm < 0,',
c     &     ' which means line narrowing instead of smoothing!'
      if(Gamm.le.0) Gamm = 0.4*pi/(Nsig*delt)
      write(*,*) 'Smoothing factor Gamm = ', Gamm
      di = 10*Nsig*(wmax-wmin)/(2*pi)
      Nsp = max(Nsp, di)
      if(Nsp>Npowermax) then
         write(6,*) 'Warning: Nsp is truncated to Npowermax (99999).'
         Nsp=Npowermax
      endif
      dOmega=(wmaxo-wmino)/Nsp
      do j=0,Nsp
         Spec(j)=(0d0,0)
      enddo
c
      write(6,*)
      if(method.eq.'DFT') then
         ispec = -1
         write(6,*) 'Discrete Fourier Transform'
         goto 246               ! DFT
      else if(method.eq.'NDFT') then
         ispec = -2
         write(6,*) 'Discrete Fourier Transform w/o apodization'
         goto 246               ! DFT
      else if(method.eq.'RRT') then
         ispec = 0
         write(6,*) 'Regularized Resolvent Transform (RRT)'
      else
         ispec = 1
         write(6,*) 'Filter Diagonalization Method (FDM)'
      endif
c
      if(rho.le.0) rho=1.1      ! default basis density
      if(Nb0.lt.2) Nb0=2
      write(6,*) 'Use the frequency grid with rho = ',rho
      Nz=((wmax-wmin)/(2*pi))*(M+1)*rho
      if(Nb0.gt.Nz) Nb0=Nz
      NWin = (2*Nz)/Nb0         ! number of windows
      dW = (wmaxo-wmino)/NWin   ! window size
      Nb0 = dW*delt/pi * (M+1)*rho ! adjusted size of window basis
      Nb0=Nb0+mod(Nb0,2)
      write(6,9993) NWin-1, Nb0
 9993 format(' Divide the spectral range into ',I4,' windows with Nb0 ='
     &     , I4)
      if(Nbc.le.0) then
         Nbc=0
         write(6,*) 'No coarse basis used'
      else
         Nbc=Nbc+mod(Nbc,2)
         write(6,*) 'Add coarse basis with size Nbc = ',Nbc
      endif
      Nb=Nbc+Nb0
      write(6,9994) Nb
 9994 format(' The actual basis size per window is Nb = ',I4)
      if(Nb.gt.Nbmax) stop 'Error: Nb > Nbmax, abort! Reduce the size'
c
      if(ispec.eq.0) then
         write(6,*) 'Regularization paramter q^2 = ', ros
      else if(ispec.gt.0) then
         if(cheat.ne.1d0) write(6,*) 'using cheat = ', cheat
         if(cheatmore) write(6,*) 'cheatmore = .true.,',
     &        ' |dk| is used to construct F(w)'
      endif
c_______________________________________________________________
c
      if(checkfile(par).and.ispec.gt.0) then
         threshhold = max(0d0, threshhold)
         open(7,file=par)
         write(7,8880) threshhold
 8880    format('# Parameter files generated by fd_rrt1d.f',/,
     &        '# Only lines with  |d(k)| > ',F10.7, ' are recorded',/
     &        '# w: complex frequencies, d: complex amplitudes',/,/,
     &        '#',7x,'Re w',18x,'Im w',15x,'Re d',14x,'Im d')
         write(6,8881) threshhold, par
 8881    format(' {wk,dk} for lines with |d(k)| > ',F8.6,
     &        ' will be writen to : ',A14)
      endif
      write(6,*)
      wmeanl=wmino
      do nw=1, NWin-1
         wminl=wmeanl
         wmeanl=wminl+dW
         wmaxl=wmeanl+dW
         write(6,9995) nw, wminl, wmaxl
 9995    format('  window No.',I4,': [wmin,wmax]=[',F10.3,',',F10.3,']')
         iminl=(wminl-wmino)/dOmega
         imeanl=(wmeanl-wmino)/dOmega
         imaxl=(wmaxl-wmino)/dOmega
         do i=iminl, imaxl
            Spec_win(i) = dcmplx(0,0)
         enddo
c
         call UMatrix1D_MS0(coef, Nsig, delt, Nb0, Nbc, Nb,
     &        wminl, wmaxl, gsave, U)
c         call Umatrix1D_dMS0(coef,Nsig, 0, delt, Nb0, Nbc, Nb,
c     &        wminl, wmaxl, gsave, U)
c         call Umatrix1D_dMS1(coef,Nsig, 0, delt, Nb0, Nbc, Nb,
c     &        wminl, wmaxl, gsave, U)
c
         if(ispec.eq.0) then    ! RRT
            call ROS2K_1D(Nb,U,imaxl-iminl+1,wmino+iminl*dOmega,
     &         dOmega,delt,gsave,ros,heap,Spec_win(iminl),Gamm)
         else                   ! FDM
         subcnt=subcnt+1
         write(6,*) '*'
            call FD_1D(Nb, U, gsave, delt, wk, dk)
            do k=1,Nb           ! F(w) ~ sum_k {...}
               dd=dk(k)
               cc=wk(k)
               if(dabs(dimag(cc)).lt.Gamm) then
                  cc=dcmplx(dreal(cc),-Gamm)
               else
                  cc=dcmplx(dreal(cc),cheat*dimag(cc))
               endif
               do i=iminl, imaxl
                  Omega=wmino+i*dOmega
                  if(cheatmore) then
                     Spec_win(i)=Spec_win(i)-xi*delt*dd*
     &                    dreal(1/(1-cdexp(xi*delt*(Omega-cc))))
                  else
                     Spec_win(i)=Spec_win(i)-xi*delt*dd/
     &                    (1-cdexp(xi*delt*(Omega-cc)))
                  endif
               enddo
               if(checkfile(par)) then
                  if(dreal(wk(k)).ge.wminl.and.dreal(wk(k)).le.wmaxl
     &                 .and.cdabs(dk(k)).ge.threshhold)
     &                 write(7,13) dreal(wk(k)),dimag(wk(k)),
     &                 dreal(dk(k)),dimag(dk(k))
               endif
 13            format(E22.16,3E18.10)
            enddo
         endif
c
c     add up the indivual contribution of each window
c
         if(iminl.eq.0) then
            do i=iminl, imeanl
               Spec(i)=Spec(i)+Spec_win(i)
            enddo
         else
            di=imeanl-iminl
            do i=iminl, imeanl
               Spec(i)=Spec(i)+Spec_win(i)*
     &              dsin(0.5*pi*dfloat(i-iminl)/float(di))**2
            enddo
         endif
         if(imaxl.ge.Nsp-1) then
            do i=imeanl+1, imaxl
               Spec(i)=Spec(i)+Spec_win(i)
            enddo
         else
            di=imaxl-imeanl
            do i=imeanl+1, imaxl
               Spec(i)=Spec(i)+Spec_win(i)*
     &              dsin(0.5*pi*dfloat(imaxl-i)/dfloat(di))**2
            enddo
         endif
         if(checkfile(par).and.ispec.gt.0) write(7,*)
c 177     continue
      enddo
c
 246  if(ispec.lt.0) then       ! DFT
         if(ispec.eq.-1) then
            N0=Nsig/10
            do n=0,Nsig
               if (n.gt.N0) then
                  r1 = dfloat(n-N0)/dfloat(Nsig+1-N0)
                  coef(n)=coef(n)*dexp(r1)*(1D0-r1)
               endif
            enddo
         endif
         do i=0,Nsp
            Omega=wmino+i*dOmega
            uu=cdexp((0,1d0)*Omega*delt)
            Z1=-delt*xi         !*cdexp(dcmplx(0d0,t0*Omega))
            Spec(i)=Spec(i)+coef(0)*Z1/2
            do n=1,Nsig
               Z1=Z1*uu
               Spec(i)=Spec(i)+coef(n)*Z1
            enddo
         enddo
      endif
c
      if(ispec.ne.-1) then      ! first point correction
         cc=-coef(0)*delt*xi/2.0
         do i=0, Nsp
            Spec(i)=Spec(i)-cc
         enddo
      endif
c
      write(6,*)                ! output spectra
      if(checkfile(ReSp)) then
         write(6,*) 'write Re[I(w)] to file: ', ReSp
         open(14,file=ReSp)
         do i=0,Nsp
            write(14,986) wmino+i*dOmega,dreal(Spec(i))
         enddo
         close(14)
      endif
      if(checkfile(ImSp)) then
         write(6,*) 'write Im[I(w)] to file: ', ImSp
         open(14,file=ImSp)
         do i=0,Nsp
            write(14,986) wmino+i*dOmega,dimag(Spec(i))
         enddo
         close(14)
      endif
      if(checkfile(AbsSp)) then
         write(6,*) 'write Abs[I(w)] to file: ', AbsSp
         open(14,file=AbsSp)
         do i=0,Nsp
            write(14,986) wmino+i*dOmega,cdabs(Spec(i))
         enddo
         close(14)
      endif
 986  format(E14.7,E12.4)
      write(6,*) 'Number of calls to FD_1D: ', subcnt
      write(6,*)
      write(6,*) 'End of program: successful excution'
c      stop
      end
c-----------------------------------------------------------------------------
c-------------------- Formal End of Program, beginning of functions ----------
c-----------------------------------------------------------------------------
      logical function checkfile(file)
      character*30 file
      checkfile=.true.
      if(file.eq.'none'.or.file.eq.'NONE') checkfile=.false.
      return
      end
c-----------------------------------------------------------------------------
c     Beginning of subroutine declarations
c
c     FDM subroutine: give U and gsave, calculate dk, wk
c-----------------------------------------------------------------------------
      subroutine FD_1D(Nb,U,gsave,delt,wk,dk)
      implicit none
      integer Nb, k, j
      real*8 delt
      complex*16 U(Nb,Nb,0:1), gsave(Nb), dk(Nb), wk(Nb), ZZ(Nb,Nb)
c
      call CQZ(Nb,U(1,1,1),U,ZZ,wk) ! solve GEP
      do k=1,Nb                 ! get the frequency in the correct units
         wk(k)=(0,1d0)*cdlog(wk(k))/delt
      enddo
      do k=1,Nb                 ! compute the amplitudes
         dk(k)=(0,0d0)
         do j=1,Nb
            dk(k)=dk(k) + ZZ(j,k)*gsave(j)
         enddo
         dk(k) = dk(k)**2
      enddo
      call cpiksrt(Nb,wk,dk)    ! sort according to fequency
      return
      end
c
c     The frequencies are sorted in increasing order.
c
      subroutine cpiksrt(Nb,wr,wi)
      implicit real*8(a-h,o-z)
      complex*16 wr(Nb),wi(Nb),war,wai
      do j=2,Nb
         war=wr(j)
         wai=wi(j)
         do i=j-1,1,-1
            if(dreal(wr(i)).le.dreal(war))go to 10
            wr(i+1)=wr(i)
            wi(i+1)=wi(i)
         enddo
         i=0
 10      continue
         wr(i+1)=war
         wi(i+1)=wai
      enddo
      return
      end
c
c     calculate spectra according to Regularized Resolvent Transform
c     NOTE: first point correction is done in the main program
c
      subroutine ROS2K_1D(Nb, U, Nsp, wmin, dOmega, delt,
     &     gsave, ros, Ull, spec, gamm)
      implicit none
      integer Nb, Nsp, i, j, n1, IPIV(Nb), INFO
      real*8 wmin, delt, ros, dOmega, Omega, gamm
      complex*16 U(Nb,Nb,0:1),gsave(Nb),B(Nb,Nb),C(Nb),A(Nb,Nb),
     &     ALPHA,BETA,cc,zz,xi,Ull(Nb,Nb,4),spec(Nsp),tmp(Nb)
c
      xi=(0d0,1d0)
      ALPHA=dcmplx(1d0,0d0)     ! needed in calling BLAS routines
      BETA=dcmplx(0d0,0d0)      ! needed in calling BLAS routines
c
c     calculate U00 = U0 * U0+,  ZGEMM is a BLAS level 3 routine
c
      call ZGEMM ( 'N', 'C', Nb, Nb, Nb, ALPHA, U(1,1,0), Nb,
     &     U(1,1,0), Nb, BETA, Ull(1,1,1), Nb )
c
      if(ros.lt.0) then         ! just in case ...
         write(6,001)
 001     format('WARNING: ros = ',E9.3,' should be non-negative, ',
     &        'thus set to zero')
         ros=0d0
      endif
c
c     calculate U0l=U0*Ul+, Ul0=U1*U0+, Ull=Ul*Ul+
c
      call ZGEMM ( 'N', 'C', Nb, Nb, Nb, ALPHA, U, Nb, U(1,1,1), Nb,
     &     BETA, Ull(1,1,2), Nb )
      call ZGEMM ( 'N', 'C', Nb, Nb, Nb, ALPHA, U(1,1,1), Nb, U, Nb,
     &     BETA, Ull(1,1,3), Nb )
      call ZGEMM ( 'N', 'C', Nb, Nb, Nb, ALPHA, U(1,1,1), Nb,
     &     U(1,1,1), Nb, BETA, Ull(1,1,4), Nb )
c
      do n1=1, Nsp
         Omega=wmin+(n1-1)*dOmega
         zz=cdexp(-xi*(Omega+xi*gamm)*delt)
c
c     A = U - z*S ( where S is the overlap matrix )
c
         do i=1, Nb
            C(i)=gsave(i)
            do j=1, Nb
               A(i,j)=zz*U(i,j,0)-U(i,j,1)
            enddo
         enddo
c
c     calculate B = (U-z*S) * (U-z*S)+ + ros, ros = q**2
c
         cc=dcmplx(dreal(zz),-dimag(zz))
         do i=1, Nb
            do j=1, Nb
               B(i,j) = Ull(i,j,1)*zz*cc
     &              - Ull(i,j,2)*zz - Ull(i,j,3)*cc + Ull(i,j,4)
            enddo
         enddo
         if(ros.gt.0) then
            do i=1, Nb
               B(i,i) = B(i,i) + ros
            enddo
         endif
c
c     solve B * X = C, with result X stored in C;
c                      Using ZGESV or ZSYSV Lapack routines
c
         call ZGESV( Nb, 1, B, Nb, IPIV, C, Nb, INFO )
         if(INFO.ne.0) then
            write(6,*) 'WARNING: ZGESV::INFO = ',INFO
            write(6,*) 'Error: ZGESV failed, unable to continue'
            stop
         endif
c
c     calculate I(z) = C' * (U-z*S)+ * X ( X is in C here)
c
         call ZGEMV ( 'C', Nb, Nb, ALPHA, A, Nb, C, 1,
     &        BETA, tmp, 1 )    !  ZGEMV is a BLAS level 2 routine
         cc=(0,0d0)
         do i=1, Nb
            cc=cc+gsave(i)*tmp(i)
         enddo
         spec(n1) = -cc*delt*zz*xi
      enddo
      return
      end


      SUBROUTINE fmn_to_uv(nu,nv,nuvh,fuv,mf,nf,mnf,fmn,ms,ns,sincos)
      USE stel_kinds
      IMPLICIT NONE
C-----------------------------------------------
C   D u m m y   A r g u m e n t s
C-----------------------------------------------
      INTEGER nu, nv, nuvh, mf, nf, mnf, ms, ns
      REAL(rprec), DIMENSION(nuvh) :: fuv
      REAL(rprec), DIMENSION(mnf) :: fmn
      REAL(rprec), DIMENSION(nuvh,0:ms,-ns:ns) :: sincos
C-----------------------------------------------
C   L o c a l   P a r a m e t e r s
C-----------------------------------------------
      REAL(rprec), PARAMETER :: zero = 0
C-----------------------------------------------
C   L o c a l   V a r i a b l e s
C-----------------------------------------------
      INTEGER :: i, kv, ku, k, m
C-----------------------------------------------
c      Calculate inverse SIN/COS transform of fmn on mn space, i.e.,
c      fuv(u,v)=Sum_m_n[ fmn * SIN(2*pi*(m*u+n*v)) ]
c
c      Inputs:
c      mf,nf,mnf: MAX m, n anf dimensions of arrays
c      DIMENSION mnf must be = nf+mf*(2*nf+1))
c      fmn(mnf) : Fourier transform of fuv
c
c      Outputs:
c      fuv(nuvh), nuvh: (fuv(nuvh) is defined on half field period)
c      DIMENSION nuvh must be = nu*nv/2+nu
c
      fuv = zero
      IF (nf .lt. 0) nf = -nf

c     Calculate fuv at each point on half+ period on uv surface
      i = 0                                !i is REAL space infex 1,nuvh
      DO kv = 1, 1 + nv/2              !See surfacep.f, this is the same
         DO ku = 1, nu
            i = i + 1                 !i goes from 1 to nuvh=nu*(1+nv/2)
c        Find fuv(i) at this uv point
c        Go over m=0,mf; n=-nf,nf
            k = 0                      !k is fourier space INDEX 1,mnfim
c                                                !m=0 CASE, -nf<n<-1
            fuv(i) = fuv(i) + SUM(fmn(k+1:nf+1+k)*sinCOS(i,0,0:nf))
            k = nf + 1 + k
c
            DO m = 1, mf                         !for m>0, n=-nf:nf
               fuv(i) = fuv(i) + SUM(fmn(k+1:nf*2+1+k)*
     1                  SINCOS(i,m,-nf:nf))
               k = nf*2 + 1 + k
            END DO
         END DO                                  !ku loop
      END DO                                     !kv loop

      END SUBROUTINE fmn_to_uv


       subroutine mcohc(yy,xx,xy,b,iq,ip,coh)
c
c       computes multivariate covariance for the multivariate
c         complex linear model
c               Y   =    X     B
c             n x q    n x p  p x q
c
c            input: xx is x*x, yy is y*y (q times q and p times p
c                hermitian matrices in full storage mode, xy
c                is x*y, B is estimated prediction coefficents,
c            output:  coh is overall coherence of predicted and
c                 observed data (Y)
c            note that this will give an answer for any estimate
c            B, not just the ls estimate; the result returned is the
c             real part of the complex coherence between observed
c             and predicted

      complex xx(ip,ip),yy(iq,iq),xy(ip,iq),b(ip,iq)
      real coh,bxxb,yxb,yt

      yt = 0.
      bxxb = 0.
      do 10 i = 1,iq
         do 5 j = 1,ip
         do 5  k = 1,ip
5        bxxb = bxxb + real(conjg(b(j,i))*xx(j,k)*b(k,i))
      yt = yt + real(yy(i,i))
10    continue

      yxb = 0.
      do 20 i = 1,iq
         do 15 j = 1,ip
         yxb = yxb + conjg(xy(j,i))*b(j,i)
15       continue
20    continue
      coh = yxb*yxb/(yt*bxxb)

      if(coh.gt.1) then
         print*,'coh',coh
         print*,'xx',xx
         print*,'yy',yy
         print*,'xy',xy
         print*,'b',b
         print*,'yt,bxxb,yxb',yt,bxxb,yxb
      end if
      return
      end


      function toric(wip,se)
!     to obtain throat radius of curvature from velocity gradient
      use kinddefine
      use fg, only:gc,gd,ge,gf
      use gg, only:gam,g7,qt
      implicit none
      real(kind=K8) :: ie,ff,fiv,fp,fw,one,thr,tk,toric,tr2,trr
      real(kind=K8),intent(in) :: se,wip
      data one/1.d+0/,thr/3.d+0/,fiv/5.d+0/
      ie=one/qt-one
      fw=wip*se*dsqrt(qt*(gam+one))
      trr=fw*(one+(gc+(thr*gc**2-gd)*fw**2)*fw**2)
1     tr2=trr**2
      tk=(one-g7*(one+(ge+gf*tr2)*tr2)*tr2**2/(45.d+0+3*ie))**qt
      ff=fw/tk-trr*(one-tr2*(gc-gd*tr2))
      fp=one-tr2*(thr*gc-fiv*gd*tr2)
      trr=trr+ff/fp
      if(dabs(ff).gt.1.d-1) go to 1
      toric=one/trr**2
      end function toric


      SUBROUTINE SB02MT( JOBG, JOBL, FACT, UPLO, N, M, A, LDA, B, LDB,
     $                   Q, LDQ, R, LDR, L, LDL, IPIV, OUFACT, G, LDG,
     $                   IWORK, DWORK, LDWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To compute the following matrices
C
C                -1
C         G = B*R  *B',
C
C         -          -1
C         A = A - B*R  *L',
C
C         -          -1
C         Q = Q - L*R  *L',
C
C     where A, B, Q, R, L, and G are N-by-N, N-by-M, N-by-N, M-by-M,
C     N-by-M, and N-by-N matrices, respectively, with Q, R and G
C     symmetric matrices.
C
C     When R is well-conditioned with respect to inversion, standard
C     algorithms for solving linear-quadratic optimization problems will
C     then also solve optimization problems with coupling weighting
C     matrix L. Moreover, a gain in efficiency is possible using matrix
C     G in the deflating subspace algorithms (see SLICOT Library routine
C     SB02OD) or in the Newton's algorithms (see SLICOT Library routine
C     SG02CD).
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     JOBG    CHARACTER*1
C             Specifies whether or not the matrix G is to be computed,
C             as follows:
C             = 'G':  Compute G;
C             = 'N':  Do not compute G.
C
C     JOBL    CHARACTER*1
C             Specifies whether or not the matrix L is zero, as follows:
C             = 'Z':  L is zero;
C             = 'N':  L is nonzero.
C
C     FACT    CHARACTER*1
C             Specifies how the matrix R is given (factored or not), as
C             follows:
C             = 'N':  Array R contains the matrix R;
C             = 'C':  Array R contains the Cholesky factor of R;
C             = 'U':  Array R contains the factors of the symmetric
C                     indefinite UdU' or LdL' factorization of R.
C
C     UPLO    CHARACTER*1
C             Specifies which triangle of the matrices R, Q (if
C             JOBL = 'N'), and G (if JOBG = 'G') is stored, as follows:
C             = 'U':  Upper triangle is stored;
C             = 'L':  Lower triangle is stored.
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the matrices A, Q, and G, and the number of
C             rows of the matrices B and L.  N >= 0.
C
C     M       (input) INTEGER
C             The order of the matrix R, and the number of columns of
C             the matrices B and L.  M >= 0.
C
C     A       (input/output) DOUBLE PRECISION array, dimension (LDA,N)
C             On entry, if JOBL = 'N', the leading N-by-N part of this
C             array must contain the matrix A.
C             On exit, if JOBL = 'N', and INFO = 0, the leading N-by-N
C                                                    -          -1
C             part of this array contains the matrix A = A - B*R  L'.
C             If JOBL = 'Z', this array is not referenced.
C
C     LDA     INTEGER
C             The leading dimension of array A.
C             LDA >= MAX(1,N) if JOBL = 'N';
C             LDA >= 1        if JOBL = 'Z'.
C
C     B       (input/output) DOUBLE PRECISION array, dimension (LDB,M)
C             On entry, the leading N-by-M part of this array must
C             contain the matrix B.
C             On exit, if OUFACT = 1, and INFO = 0, the leading N-by-M
C                                                             -1
C             part of this array contains the matrix B*chol(R)  .
C             On exit, B is unchanged if OUFACT <> 1 (hence also when
C             FACT = 'U').
C
C     LDB     INTEGER
C             The leading dimension of array B.  LDB >= MAX(1,N).
C
C     Q       (input/output) DOUBLE PRECISION array, dimension (LDQ,N)
C             On entry, if JOBL = 'N', the leading N-by-N upper
C             triangular part (if UPLO = 'U') or lower triangular part
C             (if UPLO = 'L') of this array must contain the upper
C             triangular part or lower triangular part, respectively, of
C             the symmetric matrix Q. The strictly lower triangular part
C             (if UPLO = 'U') or strictly upper triangular part (if
C             UPLO = 'L') is not referenced.
C             On exit, if JOBL = 'N' and INFO = 0, the leading N-by-N
C             upper triangular part (if UPLO = 'U') or lower triangular
C             part (if UPLO = 'L') of this array contains the upper
C             triangular part or lower triangular part, respectively, of
C                                  -          -1
C             the symmetric matrix Q = Q - L*R  *L'.
C             If JOBL = 'Z', this array is not referenced.
C
C     LDQ     INTEGER
C             The leading dimension of array Q.
C             LDQ >= MAX(1,N) if JOBL = 'N';
C             LDQ >= 1        if JOBL = 'Z'.
C
C     R       (input/output) DOUBLE PRECISION array, dimension (LDR,M)
C             On entry, if FACT = 'N', the leading M-by-M upper
C             triangular part (if UPLO = 'U') or lower triangular part
C             (if UPLO = 'L') of this array must contain the upper
C             triangular part or lower triangular part, respectively,
C             of the symmetric input weighting matrix R.
C             On entry, if FACT = 'C', the leading M-by-M upper
C             triangular part (if UPLO = 'U') or lower triangular part
C             (if UPLO = 'L') of this array must contain the Cholesky
C             factor of the positive definite input weighting matrix R
C             (as produced by LAPACK routine DPOTRF).
C             On entry, if FACT = 'U', the leading M-by-M upper
C             triangular part (if UPLO = 'U') or lower triangular part
C             (if UPLO = 'L') of this array must contain the factors of
C             the UdU' or LdL' factorization, respectively, of the
C             symmetric indefinite input weighting matrix R (as produced
C             by LAPACK routine DSYTRF).
C             If FACT = 'N', the strictly lower triangular part (if UPLO
C             = 'U') or strictly upper triangular part (if UPLO = 'L')
C             of this array is used as workspace (filled in by
C             symmetry).
C             On exit, if OUFACT = 1, and INFO = 0 (or INFO = M+1),
C             the leading M-by-M upper triangular part (if UPLO = 'U')
C             or lower triangular part (if UPLO = 'L') of this array
C             contains the Cholesky factor of the given input weighting
C             matrix.
C             On exit, if OUFACT = 2, and INFO = 0 (or INFO = M+1),
C             the leading M-by-M upper triangular part (if UPLO = 'U')
C             or lower triangular part (if UPLO = 'L') of this array
C             contains the factors of the UdU' or LdL' factorization,
C             respectively, of the given input weighting matrix.
C             On exit R is unchanged if FACT = 'C' or 'U'.
C
C     LDR     INTEGER
C             The leading dimension of array R.  LDR >= MAX(1,M).
C
C     L       (input/output) DOUBLE PRECISION array, dimension (LDL,M)
C             On entry, if JOBL = 'N', the leading N-by-M part of this
C             array must contain the matrix L.
C             On exit, if JOBL = 'N', OUFACT = 1, and INFO = 0, the
C             leading N-by-M part of this array contains the matrix
C                      -1
C             L*chol(R)  .
C             On exit, L is unchanged if OUFACT <> 1 (hence also when
C             FACT = 'U').
C             L is not referenced if JOBL = 'Z'.
C
C     LDL     INTEGER
C             The leading dimension of array L.
C             LDL >= MAX(1,N) if JOBL = 'N';
C             LDL >= 1        if JOBL = 'Z'.
C
C     IPIV    (input/output) INTEGER array, dimension (M)
C             On entry, if FACT = 'U', this array must contain details
C             of the interchanges performed and the block structure of
C             the d factor in the UdU' or LdL' factorization of matrix R
C             (as produced by LAPACK routine DSYTRF).
C             On exit, if OUFACT = 2, this array contains details of
C             the interchanges performed and the block structure of the
C             d factor in the UdU' or LdL' factorization of matrix R,
C             as produced by LAPACK routine DSYTRF.
C             This array is not referenced if FACT = 'C'.
C
C     OUFACT  (output) INTEGER
C             Information about the factorization finally used.
C             OUFACT = 0:  no factorization of R has been used (M = 0);
C             OUFACT = 1:  Cholesky factorization of R has been used;
C             OUFACT = 2:  UdU' (if UPLO = 'U') or LdL' (if UPLO = 'L')
C                          factorization of R has been used.
C
C     G       (output) DOUBLE PRECISION array, dimension (LDG,N)
C             If JOBG = 'G', and INFO = 0, the leading N-by-N upper
C             triangular part (if UPLO = 'U') or lower triangular part
C             (if UPLO = 'L') of this array contains the upper
C             triangular part (if UPLO = 'U') or lower triangular part
C                                                                 -1
C             (if UPLO = 'L'), respectively, of the matrix G = B*R  B'.
C             If JOBG = 'N', this array is not referenced.
C
C     LDG     INTEGER
C             The leading dimension of array G.
C             LDG >= MAX(1,N) if JOBG = 'G';
C             LDG >= 1        if JOBG = 'N'.
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (M)
C             If FACT = 'C' or FACT = 'U', this array is not referenced.
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0 or LDWORK = -1, DWORK(1) returns the
C             optimal value of LDWORK; if FACT = 'N' and LDWORK is set
C             as specified below, DWORK(2) contains the reciprocal
C             condition number of the given matrix R.
C             On exit, if LDWORK = -2 on input or INFO = -23, then
C             DWORK(1) returns the minimal value of LDWORK.
C
C     LDWORK  INTEGER
C             The length of the array DWORK.
C             LDWORK >= 1              if FACT = 'C' or  (FACT = 'U' and
C                                         JOBG = 'N' and  JOBL = 'Z');
C             LDWORK >= MAX(2,3*M)     if FACT = 'N' and  JOBG = 'N' and
C                                                         JOBL = 'Z';
C             LDWORK >= MAX(2,3*M,N*M) if FACT = 'N' and (JOBG = 'G' or
C                                                         JOBL = 'N');
C             LDWORK >= MAX(1,N*M)     if FACT = 'U' and (JOBG = 'G' or
C                                                         JOBL = 'N').
C             For optimum performance LDWORK should be larger than 3*M,
C             if FACT = 'N'.
C
C             If LDWORK = -1, an optimal workspace query is assumed; the
C             routine only calculates the optimal size of the DWORK
C             array, returns this value as the first entry of the DWORK
C             array, and no error message is issued by XERBLA.
C
C             If LDWORK = -2, a minimal workspace query is assumed; the
C             routine only calculates the minimal size of the DWORK
C             array, returns this value as the first entry of the DWORK
C             array, and no error message is issued by XERBLA.
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value;
C             = i:  if the i-th element (1 <= i <= M) of the d factor is
C                   exactly zero; the UdU' (or LdL') factorization has
C                   been completed, but the block diagonal matrix d is
C                   exactly singular;
C             = M+1:  if the matrix R is numerically singular.
C
C     METHOD
C                            -     -
C     The matrices G, and/or A and Q are evaluated using the given or
C     computed symmetric factorization of R.
C
C     NUMERICAL ASPECTS
C
C     The routine should not be used when R is ill-conditioned.
C
C     CONTRIBUTOR
C
C     Release 3.0: V. Sima, Katholieke Univ. Leuven, Belgium, Sep. 1997.
C
C     REVISIONS
C
C     V. Sima, Research Institute for Informatics, Bucharest, Dec. 2013,
C     Feb. 2014, Mar. 2014, May 2014.
C
C     KEYWORDS
C
C     Algebraic Riccati equation, closed loop system, continuous-time
C     system, discrete-time system, optimal regulator, Schur form.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE
      PARAMETER         ( ZERO = 0.0D0, ONE = 1.0D0 )
C     .. Scalar Arguments ..
      CHARACTER         FACT, JOBG, JOBL, UPLO
      INTEGER           INFO, LDA, LDB, LDG, LDL, LDQ, LDR, LDWORK, M,
     $                  N, OUFACT
C     .. Array Arguments ..
      INTEGER           IPIV(*), IWORK(*)
      DOUBLE PRECISION  A(LDA,*), B(LDB,*), DWORK(*), G(LDG,*),
     $                  L(LDL,*), Q(LDQ,*), R(LDR,*)
C     .. Local Scalars ..
      LOGICAL           LFACTC, LFACTU, LJOBG, LJOBL, LNFACT, LUPLOU
      CHARACTER         NT, TR, TRANS
      INTEGER           J, WRKMIN, WRKOPT
      DOUBLE PRECISION  EPS, RCOND, RNORM
C     .. External Functions ..
      LOGICAL           LSAME
      DOUBLE PRECISION  DLAMCH, DLANSY
      EXTERNAL          DLAMCH, DLANSY, LSAME
C     .. External Subroutines ..
      EXTERNAL          DCOPY, DGEMM, DLASET, DPOCON, DPOTRF, DSYCON,
     $                  DSYRK, DSYTRF, DSYTRS, DTRSM, MA02ED, MB01RB,
     $                  XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         INT, MAX
C     .. Executable Statements ..
C
      INFO   = 0
      LJOBG  = LSAME( JOBG, 'G' )
      LJOBL  = LSAME( JOBL, 'N' )
      LFACTC = LSAME( FACT, 'C' )
      LFACTU = LSAME( FACT, 'U' )
      LUPLOU = LSAME( UPLO, 'U' )
      LNFACT = .NOT.( LFACTC .OR. LFACTU )
C
C     Test the input scalar arguments.
C
      IF(      .NOT.LJOBG  .AND. .NOT.LSAME( JOBG, 'N' ) ) THEN
         INFO = -1
      ELSE IF( .NOT.LJOBL  .AND. .NOT.LSAME( JOBL, 'Z' ) ) THEN
         INFO = -2
      ELSE IF(     LNFACT  .AND. .NOT.LSAME( FACT, 'N' ) ) THEN
         INFO = -3
      ELSE IF( .NOT.LUPLOU .AND. .NOT.LSAME( UPLO, 'L' ) ) THEN
         INFO = -4
      ELSE IF( N.LT.0 ) THEN
         INFO = -5
      ELSE IF( M.LT.0 ) THEN
         INFO = -6
      ELSE IF( LDA.LT.1 .OR. ( LJOBL .AND. LDA.LT.N ) ) THEN
         INFO = -8
      ELSE IF( LDB.LT.MAX( 1, N ) ) THEN
         INFO = -10
      ELSE IF( LDQ.LT.1 .OR. ( LJOBL .AND. LDQ.LT.N ) ) THEN
         INFO = -12
      ELSE IF( LDR.LT.MAX( 1, M ) ) THEN
         INFO = -14
      ELSE IF( LDL.LT.1 .OR. ( LJOBL .AND. LDL.LT.N ) ) THEN
         INFO = -16
      ELSE IF( LDG.LT.1 .OR. ( LJOBG .AND. LDG.LT.N ) ) THEN
         INFO = -20
      ELSE
         IF( LFACTC ) THEN
            WRKMIN = 1
         ELSE IF( LFACTU ) THEN
            IF( LJOBG .OR. LJOBL ) THEN
               WRKMIN = MAX( 1, N*M )
            ELSE
               WRKMIN = 1
            END IF
         ELSE
            IF( LJOBG .OR. LJOBL ) THEN
               WRKMIN = MAX( 2, 3*M, N*M )
            ELSE
               WRKMIN = MAX( 2, 3*M )
            END IF
         END IF
         IF( LDWORK.EQ.-1 ) THEN
            IF( LNFACT ) THEN
               CALL DSYTRF( UPLO, M, R, LDR, IPIV, DWORK, -1, INFO )
               WRKOPT = MAX( WRKMIN, INT( DWORK(1) ) )
            ELSE
               WRKOPT = WRKMIN
            END IF
            DWORK(1) = WRKOPT
            RETURN
         ELSE IF( LDWORK.EQ.-2 ) THEN
            DWORK(1) = WRKMIN
            RETURN
         ELSE IF( LDWORK.LT.WRKMIN ) THEN
            INFO = -23
            DWORK(1) = WRKMIN
         END IF
      END IF
C
      IF( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'SB02MT', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF( M.EQ.0 ) THEN
         IF( LJOBG )
     $      CALL DLASET( UPLO, N, N, ZERO, ZERO, G, LDG )
         OUFACT   = 0
         DWORK(1) = WRKMIN
         IF( LNFACT )
     $      DWORK(2) = ZERO
         RETURN
      END IF
C
C     (Note: Comments in the code beginning "Workspace:" describe the
C     minimal amount of workspace needed at that point in the code,
C     as well as the preferred amount for good performance.
C     NB refers to the optimal block size for the immediately
C     following subroutine, as returned by ILAENV.)
C
      WRKOPT = 1
C
      IF( LNFACT ) THEN
C
C        Set relative machine precision.
C
         EPS = DLAMCH( 'Precision' )
C
C        Compute the norm of the matrix R, which is not factored.
C        Then save the given triangle of R in the other strict triangle
C        and the diagonal in the workspace, and try Cholesky
C        factorization.
C        Workspace: need M.
C
         RNORM = DLANSY( '1-norm', UPLO, M, R, LDR, DWORK )
         CALL DCOPY(  M, R, LDR+1, DWORK, 1 )
         CALL MA02ED( UPLO, M, R, LDR )
         CALL DPOTRF( UPLO, M, R, LDR, INFO )
         IF( INFO.EQ.0 ) THEN
C
C           Compute the reciprocal of the condition number of R.
C           Workspace: need 3*M.
C
            CALL DPOCON( UPLO, M, R, LDR, RNORM, RCOND, DWORK, IWORK,
     $                   INFO )
C
C           Return if the matrix is singular to working precision.
C
            OUFACT = 1
            IF( RCOND.LT.EPS ) THEN
               INFO = M + 1
               DWORK(2) = RCOND
               RETURN
            END IF
            WRKOPT = MAX( WRKOPT, 3*M )
         ELSE
C
C           Use UdU' or LdL' factorization, first restoring the saved
C           triangle.
C
            CALL DCOPY( M, DWORK, 1, R, LDR+1 )
            IF( LUPLOU ) THEN
               CALL MA02ED( 'Lower', M, R, LDR )
            ELSE
               CALL MA02ED( 'Upper', M, R, LDR )
            END IF
C
C           Compute the UdU' or LdL' factorization.
C           Workspace: need   1,
C                      prefer M*NB.
C
            CALL DSYTRF( UPLO, M, R, LDR, IPIV, DWORK, LDWORK, INFO )
            OUFACT = 2
            IF( INFO.GT.0 ) THEN
               DWORK(2) = ZERO
               RETURN
            END IF
            WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) )
C
C           Compute the reciprocal of the condition number of R.
C           Workspace: need 2*M.
C
            CALL DSYCON( UPLO, M, R, LDR, IPIV, RNORM, RCOND, DWORK,
     $                   IWORK, INFO )
C
C           Return if the matrix is singular to working precision.
C
            IF( RCOND.LT.EPS ) THEN
               INFO = M + 1
               DWORK(2) = RCOND
               RETURN
            END IF
         END IF
      ELSE IF( LFACTC ) THEN
         OUFACT = 1
      ELSE
         OUFACT = 2
      END IF
C
      IF( N.GT.0 .AND. ( LJOBG .OR. LJOBL ) ) THEN
         NT = 'No transpose'
         TR = 'Transpose'
C
         IF( OUFACT.EQ.1 ) THEN
C
C           Solve positive definite linear system(s).
C
            IF( LUPLOU ) THEN
               TRANS = NT
            ELSE
               TRANS = TR
            END IF
C
C           Solve the system X*U = B, overwriting B with X.
C
            CALL DTRSM( 'Right', UPLO, TRANS, 'Non-unit', N, M, ONE, R,
     $                  LDR, B, LDB )
C
            IF( LJOBG ) THEN
C                                         -1
C              Compute the matrix  G = B*R  *B', multiplying X*X' in G.
C
               CALL DSYRK( UPLO, NT, N, M, ONE, B, LDB, ZERO, G, LDG )
            END IF
C
            IF( LJOBL ) THEN
C
C              Update matrices A and Q.
C
C              Solve the system Y*U = L, overwriting L with Y.
C
               CALL DTRSM( 'Right', UPLO, TRANS, 'Non-unit', N, M, ONE,
     $                     R, LDR, L, LDL )
C
C              Compute A <- A - X*Y'.
C
               CALL DGEMM( NT, TR, N, N, M, -ONE, B, LDB, L, LDL, ONE,
     $                     A, LDA )
C
C              Compute Q <- Q - Y*Y'.
C
               CALL DSYRK( UPLO, NT, N, M, -ONE, L, LDL, ONE, Q, LDQ )
            END IF
         ELSE
C
C           Solve indefinite linear system(s).
C
            IF( LJOBG ) THEN
C
C              Solve the system UdU'*X = B' (or LdL'*X = B').
C              Workspace: need N*M.
C
               DO 10 J = 1, M
                  CALL DCOPY( N, B(1,J), 1, DWORK(J), M )
   10          CONTINUE
C
               CALL DSYTRS( UPLO, M, N, R, LDR, IPIV, DWORK, M, INFO )
C                                                       -1
C              Compute a triangle of the matrix  G = B*R  *B' = B*X.
C
               CALL MB01RB( 'Left', UPLO, NT, N, M, ZERO, ONE, G, LDG,
     $                      B, LDB, DWORK, M, INFO )
            END IF
C
            IF( LJOBL ) THEN
C
C              Update matrices A and Q.
C
C              Solve the system UdU'*Y = L' (or LdL'*Y = L').
C
               DO 20 J = 1, M
                  CALL DCOPY( N, L(1,J), 1, DWORK(J), M )
   20          CONTINUE
C
               CALL DSYTRS( UPLO, M, N, R, LDR, IPIV, DWORK, M, INFO )
C
C              A <- A - B*Y.
C
               CALL DGEMM( NT, NT, N, N, M, -ONE, B, LDB, DWORK, M, ONE,
     $                     A, LDA )
C                                               -          -1
C              Compute a triangle of the matrix Q = Q - L*R  *L'
C                                                 = Q - L*Y.
C
               CALL MB01RB( 'Left', UPLO, NT, N, M, ONE, -ONE, Q, LDQ,
     $                      L, LDL, DWORK, M, INFO )
            END IF
         END IF
      END IF
C
      DWORK(1) = WRKOPT
      IF( LNFACT )
     $   DWORK(2) = RCOND
C
C *** Last line of SB02MT ***
      RETURN
      END


!==========================================================================
elemental function gsw_thermobaric (sa, ct, p)
!==========================================================================
!
!  Calculates the thermobaric coefficient of seawater with respect to
!  Conservative Temperature.  This routine is based on the
!  computationally-efficient expression for specific volume in terms of
!  SA, CT and p (Roquet et al., 2014).
!
!  SA  =  Absolute Salinity                                        [ g/kg ]
!  CT  =  Conservative Temperature (ITS-90)                       [ deg C ]
!  p   =  sea pressure                                             [ dbar ]
!         ( i.e. absolute pressure - 10.1325 dbar )
!
!  thermobaric  =  thermobaric coefficient with                [ 1/(K Pa) ]
!                  respect to Conservative Temperature.
!  Note. The pressure derivative is taken with respect to
!    pressure in Pa not dbar.
!--------------------------------------------------------------------------

use gsw_mod_toolbox, only : gsw_rho, gsw_specvol_first_derivatives
use gsw_mod_toolbox, only : gsw_specvol_second_derivatives

use gsw_mod_kinds

implicit none

real (r8), intent(in) :: sa, ct, p

real (r8) :: gsw_thermobaric

real (r8) :: v_ct, v_ct_p, v_sa, v_sa_p

call gsw_specvol_first_derivatives(sa,ct,p,v_sa=v_sa,v_ct=v_ct)

call gsw_specvol_second_derivatives(sa,ct,p,v_sa_p=v_sa_p,v_ct_p=v_ct_p)

gsw_thermobaric = gsw_rho(sa,ct,p)*(v_ct_p - (v_ct/v_sa)*v_sa_p)

return
end function

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


program test_sort
use types, only: dp
use utils, only: assert
use sorting, only: sort, sortpairs
implicit none

integer :: a(5)
real(dp) :: b(5), c(5), vec(2, 5)
a = [4, 3, 2, 1, 5]
call sort(a)
call assert(all(a == [1, 2, 3, 4, 5]))
a = [5, 4, 3, 2, 1]
call sort(a)
call assert(all(a == [1, 2, 3, 4, 5]))

b = [5, 4, 3, 2, 1]
call sort(b)
call assert(all(abs(b - [1, 2, 3, 4, 5]) < epsilon(1._dp)))

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

a = [5, 3, 4, 2, 1]
vec = reshape([1, 2, 3, 4, 5, 6, 7, 8, 9, 10], [2, 5])
call sortpairs(a, vec)
call assert(all(a == [1, 2, 3, 4, 5]))
call assert(all(abs(vec - reshape([9, 10, 7, 8, 3, 4, 5, 6, 1, 2], [2, 5])) &
    < epsilon(1._dp)))

b = [5, 3, 4, 2, 1]
vec = reshape([1, 2, 3, 4, 5, 6, 7, 8, 9, 10], [2, 5])
call sortpairs(b, vec)
call assert(all(abs(b - [1, 2, 3, 4, 5]) < epsilon(1._dp)))
call assert(all(abs(vec - reshape([9, 10, 7, 8, 3, 4, 5, 6, 1, 2], [2, 5])) &
    < epsilon(1._dp)))

b = [5, 3, 4, 2, 1]
c = [1, 2, 3, 4, 5]
call sortpairs(b, c)
call assert(all(abs(b - [1, 2, 3, 4, 5]) < epsilon(1._dp)))
call assert(all(abs(c - [5, 4, 2, 3, 1]) < epsilon(1._dp)))

end program


      SUBROUTINE quadvl(x,y,fa,fb,fc,fd)
      REAL fa,fb,fc,fd,x,y
      REAL qa,qb,qc,qd
      qa=min(2.,max(0.,1.-x))
      qb=min(2.,max(0.,1.-y))
      qc=min(2.,max(0.,x+1.))
      qd=min(2.,max(0.,y+1.))
      fa=0.25*qa*qb
      fb=0.25*qb*qc
      fc=0.25*qc*qd
      fd=0.25*qd*qa
      return
      END

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


  function totalLinearMomentum(residual) result(totMomentum)

! ====================================================================
!
! This procedure returns the total linear momentum of a residual nucleus
!
!
! Written by CMJ, XCP-3 (03/2019)
!
! ====================================================================

    use, intrinsic:: iso_fortran_env, only: real64

    implicit none
    class(GSMResidual), intent(in   ) :: residual
    real(real64)                      :: totMomentum

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

    totMomentum = sqrt( residual%linearMom(1)**2 + &
         & residual%linearMom(2)**2 + residual%linearMom(3)**2 )

    return
! ====================================================================
  end function totalLinearMomentum

  function totalAngularMomentum(residual) result(totMomentum)

! ====================================================================
!
! This procedure returns the total angular momentum of a residual nucleus
!
!
! Written by CMJ, XCP-3 (03/2019)
!
! ====================================================================

    use, intrinsic:: iso_fortran_env, only: real64

    implicit none
    class(GSMResidual), intent(in   ) :: residual
    real(real64)                      :: totMomentum

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

    totMomentum = sqrt( residual%angularMom(1)**2 + &
         & residual%angularMom(2)**2 + residual%angularMom(3)**2 )

    return
! ====================================================================
  end function totalAngularMomentum


      SUBROUTINE PARLIN (RCX,RCY, PARITY, PRVC1X,PRVC1Y, PRVC2X,PRVC2Y,
     1                   XA,YA, A,B,C, STEP)

C  PARLIN accepts a reference point and 2 vectors and computes a
C  side of the smallest parallelogram containing a specified
C  rectangle.  The side is found in the direction specified by
C  parity.

C  Input parameters
C    RCX,RCY       : reference point within rectangle
C    PARITY        : +1 to search in direction of 1st vector, -1 to
C                    search in opposite direction
C    PRVC1X,PRVC1Y : first vector: used to increment/decrement reference point
C    PRVC2X,PRVC2Y : second vector: indicate direction (slope) of side
C    XA,YA         : 4 corners of rectangle to be contained
C  Output parameters
C    A,B,C         : coefficients of side
C    STEP          : number of steps taken from reference point

      INTEGER  PARITY, IX, STEP

      REAL     RCX,RCY, PRVC1X,PRVC1Y, PRVC2X,PRVC2Y, XA(4),YA(4), A,B,C
      REAL     CURX,CURY, CHK

      CURX = RCX
      CURY = RCY
      STEP = 0

100   CONTINUE
      STEP = STEP+1
      CURX = CURX + PARITY*PRVC1X
      CURY = CURY + PARITY*PRVC1Y
C  candidate line coefficients
      CALL PARCOF (CURX,CURY, PRVC2X,PRVC2Y, A,B,C)
      CHK = A*XA(1) + B*YA(1) + C
C  check if all on same side
      DO 200 IX = 2,4
C  if not on same side, go back and take another step
         IF (CHK * (A*XA(IX) + B*YA(IX) + C) .LE. 0) GOTO 100
200   CONTINUE

      END


!======================
! 1D euler eq.
!=====================
program Euler

    implicit none

    real(8) :: CFL, time_max, eosGamma, eosCp, len, dx, &
     time, charVel, wL(3), wR(3), vecSf, dPN, avgDp, underRelaxFactP, &
     timestep, timestepMax, resiP, fVol, gP, gF, resiU, &
     maxP,minP,maxU,minU
    real(8), dimension(:), allocatable :: cX, cRho, cU, cP, cT, &
     cVol, cDt, area, fMdot, fU, fP, fT, cAcRU, cAfRU, cBcRU, &
     cAcUU, cAfUU, cAfUP, cBcUU, cDrDp, cHt, &
     fDrDp, fRho, fFirPE, fAcUU, fSecPE, fDp_fS, &
     cAcPDp, cAfPR, cAfPDp, cBcPDp, cDp, &
     fHt, cAcHH, cAfHU, cBcHH, fThrPE, fDpDn, fDp, cDU, &
     fUx
    integer :: nCell, nCellStr, nCellEnd, nTCellStr, nTCellEnd, &
     nFace, nFaceStr, nFaceEnd, i, nstep, lFace, rFace, iterSIMPLE, &
     iterMaxSIMPLE, lCell, rCell, iterPISO, iterMaxPISO

    CFL = 0.1
    nCell = 21
    time_max = 0.012 !sec
    eosGamma = 1.4 ! air
    eosCp = 1006.d0
    len = 1.0 ! m, x-length
    iterMaxSIMPLE = 10
    iterMaxPISO = 100
    underRelaxFactP = 0.1d0
    timestepMax = 1.d-2

    nCellStr = 1
    nCellEnd = nCell

    nTCellStr = 0
    nTCellEnd = nCell + 1
    allocate( cX(nTCellStr:nTCellEnd), &
              cRho(nTCellStr:nTCellEnd), &
              cU(nTCellStr:nTCellEnd), &
              cP(nTCellStr:nTCellEnd), &
              cT(nTCellStr:nTCellEnd), &
              cDt(nTCellStr:nTCellEnd), &
              cVol(nTCellStr:nTCellEnd), &
              cAcRU(nTCellStr:nTCellEnd), &
              cAfRU(nTCellStr:nTCellEnd), &
              cBcRU(nTCellStr:nTCellEnd), &
              cAcUU(nTCellStr:nTCellEnd), &
              cAfUU(nTCellStr:nTCellEnd), &
              cAfUP(nTCellStr:nTCellEnd), &
              cBcUU(nTCellStr:nTCellEnd), &
              cDrDp(nTCellStr:nTCellEnd), &
              cHt(nTCellStr:nTCellEnd), &
              cAcPDp(nTCellStr:nTCellEnd), &
              cAfPR(nTCellStr:nTCellEnd), &
              cAfPDp(nTCellStr:nTCellEnd), &
              cBcPDp(nTCellStr:nTCellEnd), &
              cDp(nTCellStr:nTCellEnd), &
              cAcHH(nTCellStr:nTCellEnd), &
              cAfHU(nTCellStr:nTCellEnd), &
              cBcHH(nTCellStr:nTCellEnd), &
              cDu(nTCellStr:nTCellEnd) )
    cDP(:) = 0.d0

    nFace = nCell+1
    nFaceStr = 0
    nFaceEnd = nCell
    allocate( fP(nFaceStr:nFaceEnd), &
              fU(nFaceStr:nFaceEnd), &
              fT(nFaceStr:nFaceEnd), &
              area(nFaceStr:nFaceEnd), &
              fMdot(nFaceStr:nFaceEnd), &
              fDrDp(nFaceStr:nFaceEnd), &
              fRho(nFaceStr:nFaceEnd), &
              fFirPE(nFaceStr:nFaceEnd), &
              fAcUU(nFaceStr:nFaceEnd), &
              fSecPE(nFaceStr:nFaceEnd), &
              fDp_fS(nFaceStr:nFaceEnd), &
              fHt(nFaceStr:nFaceEnd), &
              fDpDn(nFaceStr:nFaceEnd), &
              fThrPE(nFaceStr:nFaceEnd), &
              fDp(nFaceStr:nFaceEnd), &
              fUx(nFaceStr:nFaceEnd) )

    !-------- one-dimensional Mesh
    dx = len/real(ncell,8)
    do i=nTCellStr,nTCellEnd
      ! x value of cell center point
      cX(i) = 0.5d0*dx + (real(i,8)-1.d0)*dx
      cVol(i) = dx*1.d0*1.d0
    enddo
    do i=nFaceStr,nFaceEnd
      area(i) = 1.d0
    enddo

    !-------- initial condition
    ! w=(rho,u,p), primitive variables
    do i=nTCellStr,nTCellEnd
      !> sod shock condition
      if( cX(i) < 0.8d0 ) then
          cRho(i) = 1.000d0
          cU(i)   = 0.000d0
          cP(i)   = 1.000d0
      else
          cRho(i) = 0.125d0
          cU(i)   = 0.000d0
          cP(i)   = 0.100d0
      endif
    enddo

    time = 0.d0
    nstep=0

    do while( time <= time_max )

        nstep=nstep+1

        !---- calculation time-step
        do i=nCellStr,nCellEnd
            lFace = i-1
            rFace = i
            charVel = dabs(cU(i))*dmax1(area(lFace),area(rFace))
            cDt(i) = dmin1( timestepMax, CFL*cVol(i)/charVel ) ! sec
        enddo
        timestep = minval(cDt(nCellStr:nCellEnd))
        cDt(:) = timestep

        SIMPLE: do iterSIMPLE = 1,iterMaxSIMPLE

            !!> Variables initialization AUSM+
            !!> mass flow rate, velocity, pressure of cell interface
            !do i=nFaceStr,nFaceEnd
            !    lCell = i
            !    rCell = i+1
            !    wL(:) = (/cRho(lCell),cU(lCell),cP(lCell)/)
            !    wR(:) = (/cRho(rCell),cU(rCell),cP(rCell)/)
            !    CALL calfaceValAUSM(wL,wR,eosGamma, fMdot(i),fU(i),fP(i))
            !    fMdot(i) = fMdot(i)*area(i)
            !    gP = 0.5d0*(1.d0+dsign(1.d0,fU(i)))
            !    gF = 0.5d0*(1.d0-dsign(1.d0,fU(i)))
            !    fUx(i) = gP*cU(lCell)  + gF*cU(rCell)
            !enddo
            !
            !!=======================================================
            !!> Density prediction (explicit)
            !!> Continuity equation
            !do i=nCellStr,nCellEnd
            !    lFace = i-1
            !    rFace = i
            !
            !    cAcRU(i) = 1.d0 *cVol(i)/cDt(i)
            !    cAfRU(i) = fMdot(rFace) &
            !             - fMdot(lFace)
            !    cBcRU(i) = cRho(i) *cVol(i)/cDt(i)
            !
            !    cRho(i) = ( cBcRU(i) - cAfRU(i)) / cAcRU(i)
            !enddo
            !!> B.C.
            !cRho(0) = cRho(1)
            !cRho(ncell+1) = cRho(ncell)
            !!=======================================================
            !
            !!=======================================================
            !!> Velocity prediction (implicit)
            !!> Momentum equation
            !do i=nCellStr,nCellEnd
            !    lFace = i-1
            !    rFace = i
            !
            !    cAcUU(i) = cRho(i) *cVol(i)/cDt(i)
            !    cAfUU(i) = fMdot(rFace)*fUx(rFace) &
            !             - fMdot(lFace)*fUx(lFace)
            !    cAfUP(i) = fP(rFace)*area(rFace) &
            !             - fP(lFace)*area(lFace)
            !    cBcUU(i) = cRho(i)*cU(i) *cVol(i)/cDt(i)
            !
            !    cU(i) = ( cBcUU(i) - cAfUU(i) - cAfUP(i) ) / cAcUU(i)
            !
            !enddo
            !!> B.C.
            !cU(0) = cU(1)
            !cU(ncell+1) = cU(ncell)
            !cAcUU(0) = cAcUU(1)
            !cAcUU(ncell+1) = cAcUU(ncell)
            !!=======================================================

            !> PISO iteration start
            PISO: do iterPISO = 1,iterMaxPISO

            !> Variables initialization AUSM+
            !> mass flow rate, velocity, pressure of cell interface
            do i=nFaceStr,nFaceEnd
                lCell = i
                rCell = i+1
                wL(:) = (/cRho(lCell),cU(lCell),cP(lCell)/)
                wR(:) = (/cRho(rCell),cU(rCell),cP(rCell)/)
                CALL calfaceValAUSM(wL,wR,eosGamma, fMdot(i),fU(i),fP(i))
                fMdot(i) = fMdot(i)*area(i)
                gP = 0.5d0*(1.d0+dsign(1.d0,fU(i)))
                gF = 0.5d0*(1.d0-dsign(1.d0,fU(i)))
                fUx(i) = gP*cU(lCell)  + gF*cU(rCell)
            enddo

            !=======================================================
            !> Density prediction (explicit)
            !> Continuity equation
            do i=nCellStr,nCellEnd
                lFace = i-1
                rFace = i

                cAcRU(i) = 1.d0 *cVol(i)/cDt(i)
                cAfRU(i) = fMdot(rFace) &
                         - fMdot(lFace)
                cBcRU(i) = cRho(i) *cVol(i)/cDt(i)

                cRho(i) = ( cBcRU(i) - cAfRU(i)) / cAcRU(i)
            enddo
            !> B.C.
            cRho(0) = cRho(1)
            cRho(ncell+1) = cRho(ncell)
            !=======================================================

            !=======================================================
            !> Velocity prediction (implicit)
            !> Momentum equation
            do i=nCellStr,nCellEnd
                lFace = i-1
                rFace = i

                cAcUU(i) = cRho(i) *cVol(i)/cDt(i)
                cAfUU(i) = fMdot(rFace)*fUx(rFace) &
                         - fMdot(lFace)*fUx(lFace)
                cAfUP(i) = fP(rFace)*area(rFace) &
                         - fP(lFace)*area(lFace)
                cBcUU(i) = cRho(i)*cU(i) *cVol(i)/cDt(i)

                cU(i) = ( cBcUU(i) - cAfUU(i) - cAfUP(i) ) / cAcUU(i)

            enddo
            !> B.C.
            cU(0) = cU(1)
            cU(ncell+1) = cU(ncell)
            cAcUU(0) = cAcUU(1)
            cAcUU(ncell+1) = cAcUU(ncell)
            !=======================================================

                !!> Velocity prediction (implicit)
                !!> Momentum equation, neglecting the gradient of pressure
                !if( iterPISO /= 1 ) then
                !    !> Variables initialization AUSM+
                !    !> mass flow rate, velocity, pressure of cell interface
                !    do i=nFaceStr,nFaceEnd
                !        lCell = i
                !        rCell = i+1
                !        wL(:) = (/cRho(lCell),cU(lCell),cP(lCell)/)
                !        wR(:) = (/cRho(rCell),cU(rCell),cP(rCell)/)
                !        CALL calfaceValAUSM(wL,wR,eosGamma, fMdot(i),fU(i),fP(i))
                !    enddo
                !
                !    do i=nCellStr,nCellEnd
                !        lFace = i-1
                !        rFace = i
                !
                !        cAcUU(i) = cRho(i) *cVol(i)/cDt(i)
                !        cAfUU(i) = fMdot(rFace)*fU(rFace)*area(rFace) &
                !                 - fMdot(lFace)*fU(lFace)*area(lFace)
                !        cBcUU(i) = cRho(i)*cU(i) *cVol(i)/cDt(i)
                !
                !        cU(i) =  ( cBcUU(i) - cAfUU(i) ) / cAcUU(i)
                !    enddo
                !    !> B.C.
                !    cU(0) = cU(1)
                !    cU(ncell+1) = cU(ncell)
                !    cAcUU(0) = cAcUU(1)
                !    cAcUU(ncell+1) = cAcUU(ncell)
                !endif
                !

                !=======================================================
                !> Variables initialization AUSM+
                !> mass flow rate, velocity, pressure of cell interface
                do i=nFaceStr,nFaceEnd
                    lCell = i
                    rCell = i+1
                    wL(:) = (/cRho(lCell),cU(lCell),cP(lCell)/)
                    wR(:) = (/cRho(rCell),cU(rCell),cP(rCell)/)
                    CALL calfaceValAUSM(wL,wR,eosGamma, fMdot(i),fU(i),fP(i))
                    fMdot(i) = fMdot(i)*area(i)
                enddo
                !=======================================================

                !=======================================================
                !> Calc. Compressible Coefficient from EOS
                !> Calc. Total Enthalpy from EOS
                TCellEOS : do i=nTCellStr,nTCellEnd
                    cT(i) = eosGamma/(eosGamma-1.d0)/eosCp/cRho(i)*cP(i)
                    cDrDp(i) = eosGamma/(eosGamma-1.d0)/eosCp/cT(i)
                    cHt(i) = eosCp*cT(i) + 0.5d0*(cU(i)**2.d0)
                enddo TCellEOS
                !=======================================================

                !=======================================================
                !> Pressure Correction (implicit)
                !> Pressure equation : Continuity + Momentum equations
                FacePress : do i=nFaceStr,nFaceEnd
                    lCell = i
                    rCell = i+1

                    gP = 0.5d0*(1.d0+dsign(1.d0,fU(i)))
                    gF = 0.5d0*(1.d0-dsign(1.d0,fU(i)))
                    fDrDp(i) = gP*cDrDp(lCell)  + gF*cDrDp(rCell)
                    fRho(i)  = gP*cRho(lCell)   + gF*cRho(rCell)
                    fAcUU(i) = gP*cAcUU(lCell)  + gF*cAcUU(rCell)
                    fDp(i)   = gP*cDp(lCell)    + gF*cDp(rCell)
                    fUx(i)   = gP*cU(lCell)  + gF*cU(rCell)

                    fVol = 0.5d0*( cVol(lCell) + cVol(rCell) )

                    fDpDn(i) = ( cDp(rCell) - cDp(lCell) ) / ( cX(rCell) - cX(lCell) )

                    fFirPE(i) = fRho(i)*fUx(i)*area(i)
                    fSecPE(i) = fDrDp(i)*fDp(i)*fUx(i)*area(i) &
                              - fRho(i)*fVol*fDpDn(i)/fAcUU(i)*area(i)
                enddo FacePress
                CellPress: do i=nCellStr,nCellEnd
                    lFace = i-1
                    rFace = i

                    !cAcPDp(i) = cDrDp(i) *cVol(i)/cDt(i)
                    !cAfPR(i)  = fFirPE(rFace)*area(rFace) - fFirPE(lFace)*area(lFace)
                    !cAfPDp(i) = fSecPE(rFace)*fDp_fS(rFace) - fSecPE(lFace)*fDp_fS(lFace)
                    !cBcPDp(i) = 0.d0
                    !
                    !cDp(i) = ( cBcPDp(i) - cAfPR(i) - cAfPDp(i) ) / cAcPDp(i)
                    !
                    !cDp(i) = underRelaxFactP*cDp(i)
                    !cP(i) = cP(i) + cDp(i)

                    cAcPDp(i) = cDrDp(i) *cVol(i)/cDt(i)
                    cAfPR(i)  = fFirPE(rFace) - fFirPE(lFace)
                    cAfPDp(i) = fSecPE(rFace) - fSecPE(lFace)
                    cBcPDp(i) = 0.d0

                    cDp(i) = ( cBcPDp(i) - cAfPR(i) - cAfPDp(i) ) / cAcPDp(i)

                    cDp(i) = underRelaxFactP*cDp(i)
                    cP(i) = cP(i) + cDp(i)

                enddo CellPress
                !> B.C.
                cP(0) = cP(1)
                cP(ncell+1) = cP(ncell)
                !=======================================================

                !=======================================================
                !> Velocity Correction (explicit)
                !> Momentum equation, neglecting the pressure term
                do i=nFaceStr,nFaceEnd
                    lCell = i
                    rCell = i+1
                    gP = 0.5d0*(1.d0+dsign(1.d0,fU(i)))
                    gF = 0.5d0*(1.d0-dsign(1.d0,fU(i)))
                    fDp(i) = gP*cDp(lCell) + gF*cDp(rCell)
                enddo
                do i=nCellStr,nCellEnd
                    lFace = i-1
                    rFace = i
                    cDU(i) = -cVol(i)/cAcUU(i) &
                        * ( fDp(lFace)*area(lFace) &
                           -fDp(rFace)*area(rFace) )/cVol(i)
                    cU(i) = cU(i) + cDU(i)
                enddo
                !> B.C.
                cU(0) = cU(1)
                cU(ncell+1) = cU(ncell)
                !=======================================================

                !=======================================================
                !> Mass flux Correction (explicit)
                do i=nFaceStr,nFaceEnd
                    lCell = i
                    rCell = i+1
                    gP = 0.5d0*(1.d0+dsign(1.d0,fU(i)))
                    gF = 0.5d0*(1.d0-dsign(1.d0,fU(i)))
                    fDrDp(i) = gP*cDrDp(lCell)  + gF*cDrDp(rCell)
                    fRho(i)  = gP*cRho(lCell)   + gF*cRho(rCell)
                    fAcUU(i) = gP*cAcUU(lCell)  + gF*cAcUU(rCell)
                    fDp(i)   = gP*cDp(lCell)    + gF*cDp(rCell)
                    fUx(i)   = gP*cU(lCell)  + gF*cU(rCell)

                    fSecPE(i) = fDrDp(i)*fDp(i)*fUx(i)*area(i) &
                              - fRho(i)*fVol*fDpDn(i)/fAcUU(i)*area(i)

                    fMdot(i) = fMdot(i) - fSecPE(i)
                enddo
                !=======================================================

                !> Pressure Residual
                resiP = 0.d0
                maxP = -1.d8
                minP = 1.d8
                do i=nCellStr,nCellEnd
                    resiP = resiP + cDp(i)**2.d0
                    maxP = dmax1(maxP,cP(i))
                    minP = dmin1(minP,cP(i))
                enddo
                resiP = dsqrt(resiP) / (dmax1(maxP,0.d0)-dmin1(minP,0.d0)+1.d-15)

                !> Velocity Residual
                resiU = 0.d0
                maxU = -1.d8
                minU = 1.d8
                do i=nCellStr,nCellEnd
                    resiU = resiU + cDU(i)**2.d0
                    maxU = dmax1(maxU,cU(i))
                    minU = dmin1(minU,cU(i))
                enddo
                resiU = dsqrt(resiU) / (dmax1(maxU,0.d0)-dmin1(minU,0.d0)+1.d-15)

                write(*,100)iterSIMPLE,iterPISO,resiP,resiU
                if(iterPISO==99) stop

            enddo PISO !> End iteration PISO

            stop

            !> Temperature Correction (explicit)
            !> Energy equation
            do i=nFaceStr,nFaceEnd
                lCell = i
                rCell = i+1

                fHt(i) = 0.5d0*( (1.d0+dsign(1.d0,fU(i)))*cHt(lCell) &
                                +(1.d0-dsign(1.d0,fU(i)))*cHt(rCell) )
            enddo
            do i=nCellStr,nCellEnd
                lFace = i-1
                rFace = i

                !> Calc. T to Ht from the EOS

                cAcHH(i) = cRho(i) *cVol(i)/cDt(i)
                cAfHU(i) = fMdot(rFace)*fHt(rFace)*area(rFace) &
                            - fMdot(lFace)*fHt(lFace)*area(lFace)
                cBcHH(i) = cRho(i)*cHt(i) *cVol(i)/cDt(i) &
                            + cDp(i) *cVol(i)/cDt(i)

                cHt(i) = ( cBcHH(i) - cAfHU(i) ) / cAcHH(i)

                !> Calc. Ht to T from the EOS
                cT(i) = ( cHt(i) - 0.5d0*(cU(i)**2.d0) ) / eosCp
            enddo
            !> B.C.
            cT(0) = cT(1)
            cT(ncell+1) = cT(ncell)

        enddo SIMPLE !> End iteration SIMPLE

        time = time + timestep

    !/////////////////////////////////
        print*,nstep,time
    !/////////////////////////////////

    enddo

    !/////////////////////////////////
    open(20,file='a.dat')!,status='old',position='append')
    do i=nCellStr,nCellEnd
        WRITE( 20, '(20(F20.9, 1X))' ) cX(i),cRho(i),cU(i),cP(i)
    enddo
    close(20)
    !/////////////////////////////////

100 format(i7,1x,i7,1x,6(1pe10.2))

end program Euler

subroutine calfaceValAUSM(wL,wR,gamma, mLR,uLR,pLR)

    implicit none

    real(8), dimension(3), intent(in) :: wL, wR
    real(8), intent(in) :: gamma
    real(8), intent(out):: mLR,uLR,pLR
    real(8) :: rho_L,rho_R,u_L,u_R,p_L,p_R,c_L,c_R, &
    H_L,H_R,chat,M_L,M_R,MLP,preP,MRM,preM,mdot

    ! AUSM+ scheme
    ! wL : left value of cell interface (face)
    ! wR : right
    rho_L = wL(1); rho_R = wR(1)
    u_L = wL(2); u_R = wR(2)
    p_L = wL(3); p_R = wR(3)

    c_L = sqrt(gamma*p_L/rho_L); c_R = sqrt(gamma*p_R/rho_R)
    H_L = gamma/(gamma-1.d0)*p_L/rho_L+0.5d0*u_L**2.d0
    H_R = gamma/(gamma-1.d0)*p_R/rho_R+0.5d0*u_R**2.d0
    !----------------------------------------------------

    chat = 0.5d0*(c_L+c_R)
    M_L = u_L/chat; M_R = u_R/chat

    !------ numerical mass flux & pressure flux in AUSM+ scheme ------
    if( abs(M_L) > 1.0 ) then
        MLP = 0.5*(M_L+abs(M_L))
        preP = 0.5*(1.0+sign(1.0,M_L))
    else
        MLP = 0.25*(M_L+1.0)**2.0
        preP = 0.25*(M_L+1.0)**2.0*(2.0-M_L)
    endif

    if( abs(M_R) > 1.0 ) then
        MRM = 0.5*(M_R-abs(M_R))
        preM = 0.5*(1.0-sign(1.0,M_R))
    else
        MRM = -0.25*(M_R-1.0)**2.0
        preM = 0.25*(M_R-1.0)**2.0*(2.0+M_R)
    endif

    uLR = (MLP + MRM)*chat

    if( uLR>=0.d0 ) then
        mLR = rho_L*uLR
    else
        mLR = rho_R*uLR
    endif

    pLR = preP*p_L+preM*p_R

endsubroutine