      subroutine fitp(am,x,px,rx,w,Ntraj,dpx,ddp,dr,ddr,u,ap,ar,err)

      implicit real*8(a-h,o-z)

      integer*4,intent(IN) :: Ntraj
      integer*4,parameter :: nb=3
      real*8 :: px(Ntraj), w(Ntraj),x(Ntraj),rx(ntraj)
      real*8 :: dpx(Ntraj),dr(ntraj),ddr(ntraj)
      real*8 :: f(nb),s(nb,nb),c(nb,2),cr(nb,1),mat(nb,nb),u(ntraj), ap(ntraj),ar(ntraj),ddp(ntraj)
      real*8 :: err(2)

      c = 0d0
      s = 0d0
      cr = 0d0

   do j = 1, nb
      do i=1,Ntraj

!        f=(/x(i),x(i)**2,x(i)**3,1d0/)
        c(j,1) = c(j,1) + px(i) * w(i) * x(i)**(j-1)
        c(j,2) = c(j,2) + rx(i) * w(i) * x(i)**(j-1)
!        c(2,1) = c(2,1)+px(i)*f(2)*w(i)
!        c(3,1) = c(3,1)+f(3)*px(i)*w(i)
!        c(4,1) = c(4,1)+f(4)*px(i)*w(i)
      enddo
	enddo

!      do i=1,Ntraj
!		do j=1,nb
!        f=(/x(i),x(i)**2,x(i)**3,1d0/)
!        cr(j,1) = cr(j,1)+rx(i) * w(i) * x(i)**(j-1)
!        cr(2,1) = cr(2,1)+rx(i)*f(2)*w(i)
!        cr(3,1) = cr(3,1)+f(3)*rx(i)*w(i)
!        cr(4,1) = cr(4,1)+f(4)*rx(i)*w(i)
!      enddo!
!		enddo

      do m=1,nb
        do n=1,nb
          do i=1,Ntraj
            !f=(/x(i),x(i)**2,x(i)**3,1d0/)
            s(m,n) = w(i)* x(i)**(m+n-2) + s(m,n)
          end do
        end do
      end do

! calculate matrix c(t)
        mat = s
        call DPOSV('U',nb,2,mat,nb,c,nb,INFO)
        if(INFO/=0) then
        print *, "info=",info
        print *, 'S = ', s
        print *, 'c = ', c
        print *, "polynomial fitting of p : matrix fails"
        stop
        end if

!        call DPOSV('U',nb,1,s,nb,cr,nb,INFO)
!        if(INFO/=0) then
!        print *, "info=",info
!        print *, "polynomial fitting of r : matrix fails"
!        stop
!        end if

!     check the error of polyfit

! the momentum operator r=cf
	ap = 0d0
	ar = 0d0
	dpx = 0d0
	dr = 0d0
	ddp = 0d0
	ddr = 0d0

      do i=1,Ntraj
		do j=1,nb
        ap(i) = ap(i) + c(j,1)*x(i)**(j-1)
        ar(i) = ar(i) + c(j,2)*x(i)**(j-1) !+cr(2,1)*x(i)**2+cr(3,1)*x(i)**3+cr(4,1)
        enddo

        do j=2,nb
        dpx(i) = dpx(i) + c(j,1) * (j-1) * x(i)**(j-2) !+2d0*c(2,1)*x(i)+c(3,1)*3d0*x(i)**2
        dr(i) = dr(i) + c(j,2) * (j-1) * x(i)**(j-2) ! + 2d0*x(i)*cr(2,1) + cr(3,1)*3d0*x(i)**2
        enddo

        do j=3,nb
        ddp(i) = ddp(i) + c(j,1) * (j-1)*(j-2) * x(i)**(j-3) !2d0*c(2,1) + 6d0*x(i)*c(3,1)
        ddr(i) = ddr(i) + c(j,2) * (j-1)*(j-2) * x(i)**(j-3) !2d0*cr(2,1) + 6d0*x(i)*cr(3,1)
		enddo
	enddo

	do i=1,ntraj
        u(i) = -(ar(i)**2 + dr(i))/(2d0*am)
    enddo

      err = 0d0
      do i=1,ntraj
        err(1) = err(1) + (px(i)-ap(i))**2*w(i)
        err(2) = err(2) + (rx(i)-ar(i))**2*w(i)
      enddo
!c     &             py(i)*(c(1,2)+2d0*c(3,2)*x(i)+c(5,2)*y(i))/am2
!c          dpy(i) = px(i)*(c(2,1)+2d0*c(4,1)*y(i)+c(5,1)*x(i))/am1+
!c     &             py(i)*(c(2,2)+2d0*c(4,2)*y(i)+c(5,2)*x(i))/am2
!! calculate quantum potential
!c          qp(i) = (c(1,1)+2d0*c(2,1)*x(i)+c(3,1)*3d0*x(i)**2)/2d0/am1
!c     &            (c(2,2)+2d0*c(4,2)*y(i)+c(5,2)*x(i))/2d0/am2
!c          qfx(i) = -(2d0*c(2,1)+6d0*x(i)*c(3,1))/2d0/am1
!c     qfy(i) = -(c(5,1)/2d0/am1+c(4,2)/am2)

      return
      end subroutine

!     --------------------------------
!     check the error of poly-fit of p
!     --------------------------------
      subroutine errp(nt,p,x,w,c,error)
      implicit real*8(a-h,o-z)

      integer*4, intent(in)  :: nt
      real*8,    intent(in)  :: w(nt),p(nt),x(nt),c(4,1)
      real*8,    intent(out) :: error

      error = 0d0

      do i=1,nt
        pfit = c(1,1)*x(i)+c(2,1)*x(i)**2+c(3,1)*x(i)**3+c(4,1)
        error = error + (p(i)-pfit)**2*w(i)
      enddo

      return
      end subroutine


module mvalAO

  use readgauss
  implicit none
  public cvalAO

contains

  subroutine cvalAO( P, valAO)
    !************************************************************************
    !
    ! calculates the value of the AO's at point P, where the AO's are
    !  normalized contracted gaussians
    !
    !************************************************************************
    double precision, intent(in)  :: P(3)
    double precision, intent(out) :: valAO(:)

    integer          :: iat, ish, ipr, iam, l, indx
    double precision :: center(3), cexpp, prefacp

    valAO = 0.d0
    indx = 0
!    write(*,*) 'valAO enter'
    do iat =  1, nat

       center = geom(iat)%coord

       do ish = 1, bset(iat)%nshells

          l = bset(iat)%primtvs(ish)%l

          do iam = 1, AngMTab(l)%ncomb

             cexpp = 0.0d0

             do ipr = 1, bset(iat)%primtvs(ish)%nprims

                cexpp = cexpp + bset(iat)%primtvs(ish)%NormCoeff(ipr,iam) * &
                     & dexp( - bset(iat)%primtvs(ish)%exp(ipr) * &
!                     & sum( (center - P) * (center - P)))
                     & ((P(1)-center(1))**2 + (P(2)-center(2))**2 + (P(3)-center(3))**2))
             end do
!    write(*,*) 'valAO mid'

!    write(*,*) 'valAO mid'
             indx = indx + 1
             prefacp = ( P(1) - center(1))**AngMTab(l)%lmn(1,iam) * &
                  & ( P(2) - center(2))**AngMTab(l)%lmn(2,iam) * &
                  & ( P(3) - center(3))**AngMTab(l)%lmn(3,iam)

             valAO(indx) = prefacp * cexpp

!             write(*,*) 'valAO', indx, iat, ish, l, iam, valAO(indx)

          end do

       end do
    end do

    return

  end subroutine cvalAO

end module mvalAO


| The Great Computer Language Shootout
| http://shootout.alioth.debian.org/
|
| contributed by Ian Osgood
| NOTE: must run gforth with flags "-m 8M" for NUM = 9
|
| Modified for Reva by Ron Aaron

| read NUM from last command line argument
: getnum argc argv >single 0if 2drop 10 then ;
getnum constant num

: bufsize 1 swap << 10000 * ;
num bufsize allocate value buf
: sieve ( size -- n )
	buf over 1 fill
	0 over 2 do
		buf i + c@ if
			1+ over buf + buf i 2* + over min ?do
				0 i c!
				j 1- skip
				loop
		then
	loop nip ;

| count primes up to 2^n*10000
: test ( n -- )
  bufsize
  ." Primes up to " dup 8 .r sieve 9 .r cr ;

| run sieve for N, N-1, N-2
  num test  num 1- test  num 2 - test

bye  \ done!


      program center
      implicit none

      integer i, j, imax, jmax
      parameter (imax = 3, jmax = 3)
      logical flag

      real a(0:imax,0:jmax), b(0:imax,0:jmax), x

      do 1 i = 1, (imax-1)
         do 2 j = 1, (jmax-1)
            != stencil readOnce, pointed(dim=1)*centered(depth=1, dim=2) + centered(depth=1, dim=1)*pointed(dim=2) :: a
            b(i,j) = a(i,j) + a(i+1,j) + a(i-1,j) + a(i,j+1) + a(i,j-1)
            select case (flag)
            case (.true.)
               x = a(i,j)
               != stencil readOnce, forward(depth=1, dim=1)*pointed(dim=2) :: a
               b(i,j) = a(i+1,j) + x
            end select
 2       continue
 1    continue

      end


!	Find estimated conditional expectations and conditional covariance matrices for latent vectors.
!	postdensity is the densities.
!	theta is the latent vectors.
!	covtheta is the array of conditional covariance matrices.
!	meantheta is the array of conditional means.
subroutine eap(postdensity,theta,covtheta,meantheta)
implicit none
interface

!	Multinomial covariance matrix.
	function mcovth(density,meanth,quadpoint)
		implicit none
		real(kind=8),intent(in)::density(:),meanth(:),quadpoint(:,:)
		real(kind=8)::mcovth(size(meanth),size(meanth))
	end function mcovth
	!	Multinomial mean.
	function mmeanth(density,quadpoint)
		implicit none
		real(kind=8),intent(in)::density(:),quadpoint(:,:)
		real(kind=8)::mmeanth(size(quadpoint,1))
	end function mmeanth
end interface
real(kind=8),intent(in)::postdensity(:,:),theta(:,:,:)
real(kind=8),intent(out)::covtheta(:,:,:),meantheta(:,:)
!	obs is observation number.
!	quad counts quadrature points.
integer::obs,quad
!	Obtain results for each observation.
do obs=1,size(postdensity,2)

! Observation obs.

!	mean and covariance.
	meantheta(:,obs)=mmeanth(postdensity(:,obs),theta(:,:,obs))
	covtheta(:,:,obs)=mcovth(postdensity(:,obs),meantheta(:,obs),theta(:,:,obs))

end do
return
end subroutine eap


c=======================================================================
c
c     subroutine MSTEP
c
c     M Step - Cluster Analysis
c
c-----------------------------------------------------------------------
      SUBROUTINE mstep ( n, d, k, x, label, proba,
     &                   pk, nk, muk, covk, info )
c-----------------------------------------------------------------------
c
c     INPUT
c       n        : number of points (n>1)                        integer
c       d        : dimension (d>0)                               integer
c       k        : number of class (k>1)                         integer
c       x        : data (n*d)                                     double
c       label    : partition label (n*k)                         integer
c       proba    : posteriori probability (n*k)                   double
c
c     OUTPUT
c       pk       : proportion of each group (k)                   double
c       nk       : number of each group (k)                      integer
c       muk      : mean of each group (d*k)                       double
c       covk     : cov. matrix of each group (d)*(d*k)            double
c       info     : diagnostic argument                           integer
c
c-----------------------------------------------------------------------
c
      IMPLICIT NONE
c
c     arguments i/o
      INTEGER n, d, k, nk(*), label(n,*), info
      DOUBLE PRECISION x(n,*), pk(*), muk(d,*), proba(n,*), covk(d,*)
c
c     local variables
      INTEGER i, j, l, imat
      DOUBLE PRECISION EPS, sumd, invnk, a, b, p
      PARAMETER (EPS = 1.E-30)
c
c-----------------------------------------------------------------------
c
c     initializations
      info = 0
c
c     M step (General Model), Sigma = lambda(k)*A'(k)D(k)A(k)
c     ---------------------
c
c     nk(k) : number of element(s) of the k-th group
      DO l = 1,k
        nk(l) = 0
        DO i = 1,n
            nk(l) = nk(l) + label(i,l)
        ENDDO
      ENDDO
c
c     case nk(l)=0
      DO l = 1,k
        IF (nk(l) .EQ. 0) THEN
             info = 6000
             RETURN
        ENDIF
      ENDDO
c
c     pk(k) : proportion of the k-th group
      DO l = 1,k
        pk(l) = float(nk(l))/float(n)
      ENDDO
c
c     muk(d,k) : centre of the k-th group
      DO l = 1,k
        DO j = 1,d
            sumd = 0.0
            DO i = 1,n
                sumd = sumd + label(i,l)*x(i,j)
            ENDDO
            muk(j,l) = sumd/float(nk(l))
        ENDDO
      ENDDO
c
c     covk(d,d*k) : cov matrix of the k-th group
      DO l = 1,k
c
c       cov(d,d) : cov matrix of the k-th group
        IF (nk(l) .EQ. 1) THEN
        DO i = 1,d
            DO j = 1,d
                covk(i, d*(l-1) + j) = 0.0
            ENDDO
            covk(i, d*(l-1) + i) = 1.E-15
        ENDDO
        ELSE
        invnk = 1.0/float(nk(l))
c
        DO j = 1,d
            DO i = 1,j
                sumd = 0.0
                DO imat = 1,n
                    p = proba(imat,l)
                    IF (p .GT. EPS) THEN
                        a = x(imat,i) - muk(i,l)
                        b = x(imat,j) - muk(j,l)
                        sumd = sumd + p*a*b
                    ENDIF
                ENDDO
                covk(i, d*(l-1) + j) = invnk * sumd
                IF ( i .NE. j ) covk(j, d*(l-1)+i) = covk(i, d*(l-1)+j)
            ENDDO
        ENDDO
        ENDIF
      ENDDO
      RETURN
      END


      SUBROUTINE MB01LD( UPLO, TRANS, M, N, ALPHA, BETA, R, LDR, A, LDA,
     $                   X, LDX, 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 matrix formula
C        _
C        R = alpha*R + beta*op( A )*X*op( A )',
C                                                 _
C     where alpha and beta are scalars, R, X, and R are skew-symmetric
C     matrices, A is a general matrix, and op( A ) is one of
C
C        op( A ) = A   or   op( A ) = A'.
C
C     The result is overwritten on R.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     UPLO    CHARACTER*1
C             Specifies which triangles of the skew-symmetric matrices R
C             and X are given, as follows:
C             = 'U':  the strictly upper triangular part is given;
C             = 'L':  the strictly lower triangular part is given.
C
C     TRANS   CHARACTER*1
C             Specifies the form of op( A ) to be used in the matrix
C             multiplication, as follows:
C             = 'N':  op( A ) = A;
C             = 'T':  op( A ) = A';
C             = 'C':  op( A ) = A'.
C
C     Input/Output Parameters
C
C     M       (input) INTEGER           _
C             The order of the matrices R and R and the number of rows
C             of the matrix op( A ).  M >= 0.
C
C     N       (input) INTEGER
C             The order of the matrix X and the number of columns of the
C             matrix op( A ).  N >= 0.
C
C     ALPHA   (input) DOUBLE PRECISION
C             The scalar alpha. When alpha is zero then R need not be
C             set before entry, except when R is identified with X in
C             the call.
C
C     BETA    (input) DOUBLE PRECISION
C             The scalar beta. When beta is zero or N <= 1, or M <= 1,
C             then A and X are not referenced.
C
C     R       (input/output) DOUBLE PRECISION array, dimension (LDR,M)
C             On entry with UPLO = 'U', the leading M-by-M strictly
C             upper triangular part of this array must contain the
C             strictly upper triangular part of the skew-symmetric
C             matrix R. The lower triangle is not referenced.
C             On entry with UPLO = 'L', the leading M-by-M strictly
C             lower triangular part of this array must contain the
C             strictly lower triangular part of the skew-symmetric
C             matrix R. The upper triangle is not referenced.
C             On exit, the leading M-by-M strictly upper triangular part
C             (if UPLO = 'U'), or strictly lower triangular part
C             (if UPLO = 'L'), of this array contains the corresponding
C                                                             _
C             strictly triangular part of the computed matrix R.
C
C     LDR     INTEGER
C             The leading dimension of the array R.  LDR >= MAX(1,M).
C
C     A       (input) DOUBLE PRECISION array, dimension (LDA,k)
C             where k is N when TRANS = 'N' and is M when TRANS = 'T' or
C             TRANS = 'C'.
C             On entry with TRANS = 'N', the leading M-by-N part of this
C             array must contain the matrix A.
C             On entry with TRANS = 'T' or TRANS = 'C', the leading
C             N-by-M part of this array must contain the matrix A.
C
C     LDA     INTEGER
C             The leading dimension of the array A.  LDA >= MAX(1,k),
C             where k is M when TRANS = 'N' and is N when TRANS = 'T' or
C             TRANS = 'C'.
C
C     X       (input or input/output) DOUBLE PRECISION array, dimension
C             (LDX,K), where K = N, if UPLO = 'U' or  LDWORK >= M*(N-1),
C                  or K = MAX(N,M), if UPLO = 'L' and LDWORK <  M*(N-1).
C             On entry, if UPLO = 'U', the leading N-by-N strictly upper
C             triangular part of this array must contain the strictly
C             upper triangular part of the skew-symmetric matrix X and
C             the lower triangular part of the array is not referenced.
C             On entry, if UPLO = 'L', the leading N-by-N strictly lower
C             triangular part of this array must contain the strictly
C             lower triangular part of the skew-symmetric matrix X and
C             the upper triangular part of the array is not referenced.
C             If LDWORK < M*(N-1), this array is overwritten with the
C             matrix op(A)*X, if UPLO = 'U', or X*op(A)', if UPLO = 'L'.
C
C     LDX     INTEGER
C             The leading dimension of the array X.
C             LDX >= MAX(1,N),   if UPLO = 'L' or  LDWORK >= M*(N-1);
C             LDX >= MAX(1,N,M), if UPLO = 'U' and LDWORK <  M*(N-1).
C
C     Workspace
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             This array is not referenced when beta = 0, or M <= 1, or
C             N <= 1.
C
C     LDWORK  The length of the array DWORK.
C             LDWORK >= N, if  beta <> 0, and M > 0, and N >  1;
C             LDWORK >= 0, if  beta =  0, or  M = 0, or  N <= 1.
C             For optimum performance, LDWORK >= M*(N-1), if  beta <> 0,
C             M > 1, and N > 1.
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -k, the k-th argument had an illegal
C                   value.
C
C     METHOD
C
C     The matrix expression is efficiently evaluated taking the skew-
C     symmetry into account. If LDWORK >= M*(N-1), a BLAS 3 like
C     implementation is used. Specifically, let X = T - T', with T a
C     strictly upper or strictly lower triangular matrix, defined by
C
C        T = striu( X ),  if UPLO = 'U',
C        T = stril( X ),  if UPLO = 'L',
C
C     where striu and stril denote the strictly upper triangular part
C     and strictly lower triangular part of X, respectively. Then,
C
C        A*X*A' = ( A*T )*A' - A*( A*T )',  for TRANS = 'N',
C        A'*X*A = A'*( T*A ) - ( T*A )'*A,  for TRANS = 'T', or 'C',
C
C     which involve BLAS 3 operations DTRMM and the skew-symmetric
C     correspondent of DSYR2K (with a Fortran implementation available
C     in the SLICOT Library routine MB01KD).
C     If LDWORK < M*(N-1), a BLAS 2 implementation is used.
C
C     NUMERICAL ASPECTS
C
C     The algorithm requires approximately
C
C                   2         2
C        3/2 x M x N + 1/2 x M
C
C     operations.
C
C     CONTRIBUTORS
C
C     V. Sima, Research Institute for Informatics, Bucharest, Jan. 2010.
C     Based on the SLICOT Library routine MB01RU and the HAPACK Library
C     routine DSKUPD.
C
C     REVISIONS
C
C     V. Sima, Research Institute for Informatics, Bucharest, Oct. 2010.
C
C     KEYWORDS
C
C     Elementary matrix operations, matrix algebra, matrix operations.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE
      PARAMETER         ( ZERO = 0.0D0, ONE = 1.0D0 )
C     .. Scalar Arguments ..
      CHARACTER         TRANS, UPLO
      INTEGER           INFO, LDA, LDR, LDWORK, LDX, M, N
      DOUBLE PRECISION  ALPHA, BETA
C     .. Array Arguments ..
      DOUBLE PRECISION  A(LDA,*), DWORK(*), R(LDR,*), X(LDX,*)
C     .. Local Scalars ..
      LOGICAL           LTRANS, NOTTRA, UPPER
      INTEGER           I, J, M2
C     .. External Functions ..
      LOGICAL           LSAME
      EXTERNAL          LSAME
C     .. External Subroutines ..
      EXTERNAL          DCOPY, DGEMV, DLACPY, DLASCL, DLASET, DSCAL,
     $                  DTRMM, MB01KD, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         MAX, MIN
C     .. Executable Statements ..
C
C     Test the input scalar arguments.
C
      INFO   = 0
      UPPER  = LSAME( UPLO,  'U' )
      NOTTRA = LSAME( TRANS, 'N' )
      LTRANS = LSAME( TRANS, 'T' ) .OR. LSAME( TRANS, 'C' )
C
      IF(      ( .NOT.UPPER  ).AND.( .NOT.LSAME( UPLO, 'L' ) ) )THEN
         INFO = -1
      ELSE IF( ( .NOT.NOTTRA ).AND.( .NOT.LTRANS ) )THEN
         INFO = -2
      ELSE IF( M.LT.0 ) THEN
         INFO = -3
      ELSE IF( N.LT.0 ) THEN
         INFO = -4
      ELSE IF( LDR.LT.MAX( 1, M ) ) THEN
         INFO = -8
      ELSE IF( LDA.LT.1 .OR. ( LTRANS .AND. LDA.LT.N ) .OR.
     $                       ( NOTTRA .AND. LDA.LT.M ) ) THEN
         INFO = -10
      ELSE IF( LDX.LT.MAX( 1, N ) .OR.
     $       ( LDX.LT.M .AND. UPPER .AND. LDWORK.LT.M*( N - 1 ) ) ) THEN
         INFO = -12
      ELSE IF( LDWORK.LT.0 .OR. ( BETA.NE.ZERO .AND. M.GT.1 .AND. N.GT.1
     $      .AND. LDWORK.LT.N ) ) THEN
         INFO = -14
      END IF
C
      IF ( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'MB01LD', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF ( M.LE.0 )
     $   RETURN
C
      M2 = MIN( 2, M )
      IF ( BETA.EQ.ZERO .OR. N.LE.1 ) THEN
         IF ( UPPER ) THEN
            I = 1
            J = M2
         ELSE
            I = M2
            J = 1
         END IF
C
         IF ( ALPHA.EQ.ZERO ) THEN
C
C           Special case alpha = 0.
C
            CALL DLASET( UPLO, M-1, M-1, ZERO, ZERO, R(I,J), LDR )
         ELSE
C
C           Special case beta = 0 or N <= 1.
C
            IF ( ALPHA.NE.ONE )
     $         CALL DLASCL( UPLO, 0, 0, ONE, ALPHA, M-1, M-1, R(I,J),
     $                      LDR, INFO )
         END IF
         RETURN
      END IF
C
C     General case: beta <> 0.
C
      IF ( LDWORK.GE.M*( N - 1 ) ) THEN
C
C        Use a BLAS 3 like implementation.
C        Compute W = A*T or W = T*A in DWORK, and apply the updating
C        formula (see METHOD section). Note that column 1 (if
C        UPLO = 'U') or column N (if UPLO = 'L') is zero in the first
C        case, and it is not stored; similarly, row N (if UPLO = 'U') or
C        row 1 (if UPLO = 'L') is zero in the second case, and it is not
C        stored.
C        Workspace: need M*(N-1).
C
         IF ( UPPER ) THEN
            I = 1
            J = M2
         ELSE
            I = M2
            J = 1
         END IF
C
         IF( NOTTRA ) THEN
C
            CALL DLACPY( 'Full', M, N-1, A(1,I), LDA, DWORK, M )
            CALL DTRMM(  'Right', UPLO, 'NoTranspose', 'Non-unit', M,
     $                   N-1, ONE, X(I,J), LDX, DWORK, M )
            CALL MB01KD( UPLO, TRANS, M, N-1, BETA, DWORK, M, A(1,J),
     $                   LDA, ALPHA, R, LDR, INFO )
C
         ELSE
C
            CALL DLACPY( 'Full', N-1, M, A(J,1), LDA, DWORK, N-1 )
            CALL DTRMM(  'Left', UPLO, 'NoTranspose', 'Non-unit', N-1,
     $                   M, ONE, X(I,J), LDX, DWORK, N-1 )
            CALL MB01KD( UPLO, TRANS, M, N-1, BETA, A(I,1), LDA, DWORK,
     $                   N-1, ALPHA, R, LDR, INFO )
C
         END IF
C
      ELSE
C
C        Use a BLAS 2 implementation.
C
C
         IF ( NOTTRA ) THEN
C
C           Compute A*X*A'.
C
            IF ( UPPER ) THEN
C
C              Compute A*X in X (M-by-N).
C
               DO 10 J = 1, N-1
                  CALL DCOPY( J-1, X(1,J), 1, DWORK, 1 )
                  DWORK(J) = ZERO
                  CALL DCOPY( N-J, X(J,J+1), LDX, DWORK(J+1), 1 )
                  CALL DSCAL( N-J, -ONE, DWORK(J+1), 1 )
                  CALL DGEMV( TRANS, M, N, ONE, A, LDA, DWORK, 1, ZERO,
     $                        X(1,J), 1 )
   10          CONTINUE
C
               CALL DCOPY( N-1, X(1,N), 1, DWORK, 1 )
               CALL DGEMV( TRANS, M, N-1, ONE, A, LDA, DWORK, 1, ZERO,
     $                     X(1,N), 1 )
C
C              Compute alpha*striu( R ) + beta*striu( X*A' ) in the
C              strictly upper triangular part of R.
C
               DO 20 I = 1, M-1
                  CALL DCOPY( N, X(I,1), LDX, DWORK, 1 )
                  CALL DGEMV( TRANS, M-I, N, BETA, A(I+1,1), LDA, DWORK,
     $                        1, ALPHA, R(I,I+1), LDR )
   20          CONTINUE
C
            ELSE
C
C              Compute X*A' in X (N-by-M).
C
               DO 30 I = 1, N-1
                  CALL DCOPY( I-1, X(I,1), LDX, DWORK, 1 )
                  DWORK(I) = ZERO
                  CALL DCOPY( N-I, X(I+1,I), 1, DWORK(I+1), 1 )
                  CALL DSCAL( N-I, -ONE, DWORK(I+1), 1 )
                  CALL DGEMV( TRANS, M, N, ONE, A, LDA, DWORK, 1, ZERO,
     $                        X(I,1), LDX )
   30          CONTINUE
C
               CALL DCOPY( N-1, X(N,1), LDX, DWORK, 1 )
               CALL DGEMV( TRANS, M, N-1, ONE, A, LDA, DWORK, 1, ZERO,
     $                     X(N,1), LDX )
C
C              Compute alpha*stril( R ) + beta*stril( A*X ) in the
C              strictly lower triangular part of R.
C
               DO 40 J = 1, M-1
                  CALL DCOPY( N, X(1,J), 1, DWORK, 1 )
                  CALL DGEMV( TRANS, M-J, N, BETA, A(J+1,1), LDA, DWORK,
     $                        1, ALPHA, R(J+1,J), 1 )
   40          CONTINUE
C
            END IF
C
         ELSE
C
C           Compute A'*X*A.
C
            IF ( UPPER ) THEN
C
C              Compute A'*X in X (M-by-N).
C
               DO 50 J = 1, N-1
                  CALL DCOPY( J-1, X(1,J), 1, DWORK, 1 )
                  DWORK(J) = ZERO
                  CALL DCOPY( N-J, X(J,J+1), LDX, DWORK(J+1), 1 )
                  CALL DSCAL( N-J, -ONE, DWORK(J+1), 1 )
                  CALL DGEMV( TRANS, N, M, ONE, A, LDA, DWORK, 1, ZERO,
     $                        X(1,J), 1 )
   50          CONTINUE
C
               CALL DCOPY( N-1, X(1,N), 1, DWORK, 1 )
               CALL DGEMV( TRANS, N-1, M, ONE, A, LDA, DWORK, 1, ZERO,
     $                     X(1,N), 1 )
C
C              Compute alpha*striu( R ) + beta*striu( X*A ) in the
C              strictly upper triangular part of R.
C
               DO 60 I = 1, M-1
                  CALL DCOPY( N, X(I,1), LDX, DWORK, 1 )
                  CALL DGEMV( TRANS, N, M-I, BETA, A(1,I+1), LDA, DWORK,
     $                        1, ALPHA, R(I,I+1), LDR )
   60          CONTINUE
C
            ELSE
C
C              Compute X*A in X (N-by-M).
C
               DO 70 I = 1, N-1
                  CALL DCOPY( I-1, X(I,1), LDX, DWORK, 1 )
                  DWORK(I) = ZERO
                  CALL DCOPY( N-I, X(I+1,I), 1, DWORK(I+1), 1 )
                  CALL DSCAL( N-I, -ONE, DWORK(I+1), 1 )
                  CALL DGEMV( TRANS, N, M, ONE, A, LDA, DWORK, 1, ZERO,
     $                        X(I,1), LDX )
   70          CONTINUE
C
               CALL DCOPY( N-1, X(N,1), LDX, DWORK, 1 )
               CALL DGEMV( TRANS, N-1, M, ONE, A, LDA, DWORK, 1, ZERO,
     $                     X(N,1), LDX )
C
C              Compute alpha*stril( R ) + beta*stril( A'*X ) in the
C              strictly lower triangular part of R.
C
               DO 80 J = 1, M-1
                  CALL DCOPY( N, X(1,J), 1, DWORK, 1 )
                  CALL DGEMV( TRANS, N, M-J, BETA, A(1,J+1), LDA, DWORK,
     $                        1, ALPHA, R(J+1,J), 1 )
   80          CONTINUE
C
            END IF
         END IF
      END IF
C
      RETURN
C *** Last line of MB01LD ***
      END


C$Procedure      RMAINI ( Remainder --- integer )

      SUBROUTINE RMAINI ( NUM, DENOM, Q, REM )

C$ Abstract
C
C     Compute the integer quotient and non-negative remainder
C     of NUM and DENOM.
C
C$ Disclaimer
C
C     THIS SOFTWARE AND ANY RELATED MATERIALS WERE CREATED BY THE
C     CALIFORNIA INSTITUTE OF TECHNOLOGY (CALTECH) UNDER A U.S.
C     GOVERNMENT CONTRACT WITH THE NATIONAL AERONAUTICS AND SPACE
C     ADMINISTRATION (NASA). THE SOFTWARE IS TECHNOLOGY AND SOFTWARE
C     PUBLICLY AVAILABLE UNDER U.S. EXPORT LAWS AND IS PROVIDED "AS-IS"
C     TO THE RECIPIENT WITHOUT WARRANTY OF ANY KIND, INCLUDING ANY
C     WARRANTIES OF PERFORMANCE OR MERCHANTABILITY OR FITNESS FOR A
C     PARTICULAR USE OR PURPOSE (AS SET FORTH IN UNITED STATES UCC
C     SECTIONS 2312-2313) OR FOR ANY PURPOSE WHATSOEVER, FOR THE
C     SOFTWARE AND RELATED MATERIALS, HOWEVER USED.
C
C     IN NO EVENT SHALL CALTECH, ITS JET PROPULSION LABORATORY, OR NASA
C     BE LIABLE FOR ANY DAMAGES AND/OR COSTS, INCLUDING, BUT NOT
C     LIMITED TO, INCIDENTAL OR CONSEQUENTIAL DAMAGES OF ANY KIND,
C     INCLUDING ECONOMIC DAMAGE OR INJURY TO PROPERTY AND LOST PROFITS,
C     REGARDLESS OF WHETHER CALTECH, JPL, OR NASA BE ADVISED, HAVE
C     REASON TO KNOW, OR, IN FACT, SHALL KNOW OF THE POSSIBILITY.
C
C     RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF
C     THE SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY
C     CALTECH AND NASA FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE
C     ACTIONS OF RECIPIENT IN THE USE OF THE SOFTWARE.
C
C$ Required_Reading
C
C     None.
C
C$ Keywords
C
C     MATH
C     UTILITY
C
C$ Declarations

      INTEGER               NUM
      INTEGER               DENOM
      INTEGER               Q
      INTEGER               REM

C$ Brief_I/O
C
C     Variable  I/O  Description
C     --------  ---  --------------------------------------------------
C     NUM        I   Numerator used to compute quotient and remainder.
C     DENOM      I   Denominator used to compute quotient and remainder.
C     Q          O   Integer portion of the quotient NUM/DENOM.
C     REM        O   Remainder of the quotient NUM/DENOM.
C
C$ Detailed_Input
C
C     NUM        is the numerator of a quotient
C
C     DENOM      is the denominator of a quotient
C
C$ Detailed_Output
C
C     Q          is the largest integer less than or equal to the
C                quotient NUM/DENOM
C
C     REM        is the remainder of the integer division NUM/DENOM
C
C$ Parameters
C
C     None.
C
C$ Exceptions
C
C     1) If DENOM is zero, the error 'SPICE(DIVIDEBYZERO)' will be
C        signalled.
C
C$ Files
C
C     None.
C
C$ Particulars
C
C     Given the integer inputs NUM and DENOM, this routine
C     finds integers Q and REM that satisfy the following conditions:
C
C         1) NUM = DENOM * Q + REM
C
C         2) REM is a non negative integer less than the absolute
C            value of DENOM.
C
C     This routine serves as a macro.  In this way the code to perform
C     this task can be written and maintained in a single location.
C
C$ Examples
C
C     One frequently needs to compute the  ``360 modulus'' of a
C     number.  For positive numbers the FORTRAN intrinsic mod
C     function works well.  However, for negative numbers the
C     intrinsic will return a negative modulus.  This routine
C     can be used to compute the positive 360 pi modulus (MOD360) for
C     any integer I by the call:
C
C         CALL RMAINI ( I, 360, Q, MOD360 )
C
C$ Restrictions
C
C     None.
C
C$ Literature_References
C
C     None.
C
C$ Author_and_Institution
C
C     W.L. Taber     (JPL)
C
C$ Version
C
C-    SPICELIB Version 1.0.0, 01-DEC-1995 (WLT)
C
C-&

C$ Index_Entries
C
C     Compute the remainder of an integer division
C
C-&

C
C     Take care of the zero-denominator case first
C
      IF ( DENOM .EQ. 0.0D0 ) THEN
         CALL CHKIN  ( 'RMAINI'                                       )
         CALL SETMSG ( 'Attempting to compute a quotient with a '     //
     .                 'divide by zero.'                              )
         CALL SIGERR ( 'SPICE(DIVIDEBYZERO)'                          )
         CALL CHKOUT ( 'RMAINI'                                       )
         RETURN
      END IF

      Q   = NUM / DENOM
      REM = NUM - DENOM*Q

      IF ( REM .LT. 0 ) THEN
         Q   = Q   - 1
         REM = REM + DENOM
      END IF

      RETURN
      END


*DECK TRED3
      SUBROUTINE TRED3 (N, NV, A, D, E, E2)
C***BEGIN PROLOGUE  TRED3
C***PURPOSE  Reduce a real symmetric matrix stored in packed form to
C            symmetric tridiagonal matrix using orthogonal
C            transformations.
C***LIBRARY   SLATEC (EISPACK)
C***CATEGORY  D4C1B1
C***TYPE      SINGLE PRECISION (TRED3-S)
C***KEYWORDS  EIGENVALUES, EIGENVECTORS, EISPACK
C***AUTHOR  Smith, B. T., et al.
C***DESCRIPTION
C
C     This subroutine is a translation of the ALGOL procedure TRED3,
C     NUM. MATH. 11, 181-195(1968) by Martin, Reinsch, and Wilkinson.
C     HANDBOOK FOR AUTO. COMP., VOL.II-LINEAR ALGEBRA, 212-226(1971).
C
C     This subroutine reduces a REAL SYMMETRIC matrix, stored as
C     a one-dimensional array, to a symmetric tridiagonal matrix
C     using orthogonal similarity transformations.
C
C     On Input
C
C        N is the order of the matrix A.  N is an INTEGER variable.
C
C        NV is an INTEGER variable set equal to the dimension of the
C          array A as specified in the calling program.  NV must not
C          be less than  N*(N+1)/2.
C
C        A contains the lower triangle, stored row-wise, of the real
C          symmetric packed matrix.  A is a one-dimensional REAL
C          array, dimensioned A(NV).
C
C     On Output
C
C        A contains information about the orthogonal transformations
C          used in the reduction in its first N*(N+1)/2 positions.
C
C        D contains the diagonal elements of the symmetric tridiagonal
C          matrix.  D is a one-dimensional REAL array, dimensioned D(N).
C
C        E contains the subdiagonal elements of the symmetric
C          tridiagonal matrix in its last N-1 positions.  E(1) is set
C          to zero.  E is a one-dimensional REAL array, dimensioned
C          E(N).
C
C        E2 contains the squares of the corresponding elements of E.
C          E2 may coincide with E if the squares are not needed.
C          E2 is a one-dimensional REAL array, dimensioned E2(N).
C
C     Questions and comments should be directed to B. S. Garbow,
C     APPLIED MATHEMATICS DIVISION, ARGONNE NATIONAL LABORATORY
C     ------------------------------------------------------------------
C
C***REFERENCES  B. T. Smith, J. M. Boyle, J. J. Dongarra, B. S. Garbow,
C                 Y. Ikebe, V. C. Klema and C. B. Moler, Matrix Eigen-
C                 system Routines - EISPACK Guide, Springer-Verlag,
C                 1976.
C***ROUTINES CALLED  (NONE)
C***REVISION HISTORY  (YYMMDD)
C   760101  DATE WRITTEN
C   890831  Modified array declarations.  (WRB)
C   890831  REVISION DATE from Version 3.2
C   891214  Prologue converted to Version 4.0 format.  (BAB)
C   920501  Reformatted the REFERENCES section.  (WRB)
C***END PROLOGUE  TRED3
C
      INTEGER I,J,K,L,N,II,IZ,JK,NV
      REAL A(*),D(*),E(*),E2(*)
      REAL F,G,H,HH,SCALE
C
C     .......... FOR I=N STEP -1 UNTIL 1 DO -- ..........
C***FIRST EXECUTABLE STATEMENT  TRED3
      DO  300 II = 1, N
         I = N + 1 - II
         L = I - 1
         IZ = (I * L) / 2
         H = 0.0E0
         SCALE = 0.0E0
         IF (L .LT. 1) GO TO 130
C     .......... SCALE ROW (ALGOL TOL THEN NOT NEEDED) ..........
         DO 120 K = 1, L
            IZ = IZ + 1
            D(K) = A(IZ)
            SCALE = SCALE + ABS(D(K))
  120    CONTINUE
C
         IF (SCALE .NE. 0.0E0) GO TO 140
  130    E(I) = 0.0E0
         E2(I) = 0.0E0
         GO TO 290
C
  140    DO 150 K = 1, L
            D(K) = D(K) / SCALE
            H = H + D(K) * D(K)
  150    CONTINUE
C
         E2(I) = SCALE * SCALE * H
         F = D(L)
         G = -SIGN(SQRT(H),F)
         E(I) = SCALE * G
         H = H - F * G
         D(L) = F - G
         A(IZ) = SCALE * D(L)
         IF (L .EQ. 1) GO TO 290
         F = 0.0E0
C
         DO 240 J = 1, L
            G = 0.0E0
            JK = (J * (J-1)) / 2
C     .......... FORM ELEMENT OF A*U ..........
            DO 180 K = 1, L
               JK = JK + 1
               IF (K .GT. J) JK = JK + K - 2
               G = G + A(JK) * D(K)
  180       CONTINUE
C     .......... FORM ELEMENT OF P ..........
            E(J) = G / H
            F = F + E(J) * D(J)
  240    CONTINUE
C
         HH = F / (H + H)
         JK = 0
C     .......... FORM REDUCED A ..........
         DO 260 J = 1, L
            F = D(J)
            G = E(J) - HH * F
            E(J) = G
C
            DO 260 K = 1, J
               JK = JK + 1
               A(JK) = A(JK) - F * E(K) - G * D(K)
  260    CONTINUE
C
  290    D(I) = A(IZ+1)
         A(IZ+1) = SCALE * SQRT(H)
  300 CONTINUE
C
      RETURN
      END


!++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++!
!                          Futility Development Group                          !
!                             All rights reserved.                             !
!                                                                              !
! Futility is a jointly-maintained, open-source project between the University !
! of Michigan and Oak Ridge National Laboratory.  The copyright and license    !
! can be found in LICENSE.txt in the head directory of this repository.        !
!++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++!
!> @brief A Fortran 2003 module implementing interpolation routines for input
!>        arrays returning either interpolated values or a flat "extrapolation"
!>        when  requested interpolation is out of range. Right now only 1D, 2D,
!>        and 3D routines are available.
!++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++!
MODULE InterpolatorsModule
#include "Futility_DBC.h"
USE Futility_DBC
USE IntrType
USE ArrayUtils
USE Search

IMPLICIT NONE
PRIVATE

PUBLIC :: Interp

!> @brief Interface to interpolate whatever is handed in.
!>
INTERFACE Interp
  !> @copybrief InterpolatorsModule::Interp_1D
  !> @copydetails InterpolatorsModule::Interp_1D
  MODULE PROCEDURE Interp_1D
  !> @copybrief InterpolatorsModule::Interp_2D
  !> @copydetails InterpolatorsModule::Interp_2D
  MODULE PROCEDURE Interp_2D
  !> @copybrief InterpolatorsModule::Interp_3D
  !> @copydetails InterpolatorsModule::Interp_3D
  MODULE PROCEDURE Interp_3D
ENDINTERFACE Interp
!
!===============================================================================
CONTAINS
!
!-------------------------------------------------------------------------------
!> @brief This routine takes a 1D data "table" and linearly interpolates to find
!>        a desired point.
!> @param labels axis labels at which data points in table are defined
!> @param table contains data to be interpolated between defined at points
!>        given in labels
!> @param point point at which interpolation needs to be done to return a value
!> @returns Interpolant interpolated data found through linear interpolation to the point point
FUNCTION Interp_1D(labels,table,point) RESULT(Interpolant)
  REAL(SRK),INTENT(IN) :: labels(:)
  REAL(SRK),INTENT(IN) :: table(:)
  REAL(SRK),INTENT(IN) :: point
  REAL(SRK) :: Interpolant,f(2)
  INTEGER(SIK) :: i,N_i,i_p

  REQUIRE(SIZE(labels) > 1)
  REQUIRE(SIZE(labels) == SIZE(table))
  REQUIRE(isStrictlyIncDec(labels))

  CALL Get_points_and_weights(labels,point,f,i_p,N_i)

  !Get interpolated value
  Interpolant=0.0_SRK
  DO i=MAX(i_p-1,1),MIN(i_p,N_i)
    Interpolant=Interpolant+f(i-i_p+2)*table(i)
  ENDDO

ENDFUNCTION Interp_1D
!
!-------------------------------------------------------------------------------
!> @brief This routine takes a 2D data table and linearly interpolates to find
!>        a desired point.
!> @param labels1 axis labels at which data points in table are defined for 1st dimension
!> @param labels2 axis labels at which data points in table are defined for 2nd dimension
!> @param table contains data to be interpolated between defined at points
!>        given in labels1 and labels2
!> @param point point at which interpolation needs to be done to return a value
!> @returns Interpolant interpolated data found through linear interpolation to the point point
FUNCTION Interp_2D(labels1,labels2,table,point) RESULT(Interpolant)
  REAL(SRK),INTENT(IN) :: labels1(:)
  REAL(SRK),INTENT(IN) :: labels2(:)
  REAL(SRK),INTENT(IN) :: table(:,:)
  REAL(SRK),INTENT(IN) :: point(:)
  REAL(SRK) :: Interpolant,f1(2),f2(2)
  INTEGER(SIK) :: i,j,N_i,N_j,i_p,j_p

  REQUIRE(SIZE(labels1) > 1)
  REQUIRE(SIZE(labels2) > 1)
  REQUIRE(SIZE(labels1) == SIZE(table(:,1)))
  REQUIRE(SIZE(labels2) == SIZE(table(1,:)))
  REQUIRE(isStrictlyIncDec(labels1))
  REQUIRE(isStrictlyIncDec(labels2))

  CALL Get_points_and_weights(labels1,point(1),f1,i_p,N_i)
  CALL Get_points_and_weights(labels2,point(2),f2,j_p,N_j)

  !Get interpolated value
  Interpolant=0.0_SRK
  DO j=MAX(j_p-1,1),MIN(j_p,N_j)
    DO i=MAX(i_p-1,1),MIN(i_p,N_i)
      Interpolant=Interpolant+f1(i-i_p+2)*f2(j-j_p+2)*table(i,j)
    ENDDO
  ENDDO

ENDFUNCTION Interp_2D
!
!-------------------------------------------------------------------------------
!> @brief This routine takes a 3D data table and linearly interpolates to find
!>        a desired point.
!> @param labels1 axis labels at which data points in table are defined for 1st dimension
!> @param labels2 axis labels at which data points in table are defined for 2nd dimension
!> @param labels3 axis labels at which data points in table are defined for 3rd dimension
!> @param table contains data to be interpolated between defined at points
!>        given in labels1, labels2, and labels 3
!> @param point point at which interpolation needs to be done to return a value
!> @returns Interpolant interpolated data found through linear interpolation to the point point
!>
FUNCTION Interp_3D(labels1,labels2,labels3,table,point) RESULT(Interpolant)
  REAL(SRK),INTENT(IN) :: labels1(:)
  REAL(SRK),INTENT(IN) :: labels2(:)
  REAL(SRK),INTENT(IN) :: labels3(:)
  REAL(SRK),INTENT(IN) :: table(:,:,:)
  REAL(SRK),INTENT(IN) :: point(:)
  REAL(SRK) :: Interpolant,f1(2),f2(2),f3(2)
  INTEGER(SIK) :: i,j,k,N_i,N_j,N_k,i_p,j_p,k_p

  REQUIRE(SIZE(labels1) > 1)
  REQUIRE(SIZE(labels2) > 1)
  REQUIRE(SIZE(labels3) > 1)
  REQUIRE(SIZE(labels1) == SIZE(table(:,1,1)))
  REQUIRE(SIZE(labels2) == SIZE(table(1,:,1)))
  REQUIRE(SIZE(labels3) == SIZE(table(1,1,:)))
  REQUIRE(isStrictlyIncDec(labels1))
  REQUIRE(isStrictlyIncDec(labels2))
  REQUIRE(isStrictlyIncDec(labels3))

  CALL Get_points_and_weights(labels1,point(1),f1,i_p,N_i)
  CALL Get_points_and_weights(labels2,point(2),f2,j_p,N_j)
  CALL Get_points_and_weights(labels3,point(3),f3,k_p,N_k)

  !Get interpolated value
  Interpolant=0.0_SRK
  DO k=MAX(k_p-1,1),MIN(k_p,N_k)
    DO j=MAX(j_p-1,1),MIN(j_p,N_j)
      DO i=MAX(i_p-1,1),MIN(i_p,N_i)
        Interpolant=Interpolant+f1(i-i_p+2)*f2(j-j_p+2)*f3(k-k_p+2)*table(i,j,k)
      ENDDO
    ENDDO
  ENDDO

ENDFUNCTION Interp_3D
!
!-------------------------------------------------------------------------------
!> @brief This routine takes a 1D array of table labels (the points along a single
!>        dimension at which table entries are defined) and the interpolation point
!>        coordinate along that dimension and returns the index of the label entry
!>        directly ahead of that point along with the weights of that point and the
!>        preceeding point.
!> @param labels axis labels at which data points in table are defined
!> @param point point at which interpolation needs to be done to return a value
!> @param f weights associated with the points on either side of point
!> @param i_p index of data point directly ahead of the interpolation point in labels
!> @param N_i number of entries of labels
!>
SUBROUTINE Get_points_and_weights(labels,point,f,i_p,N_i)
  REAL(SRK),INTENT(IN) :: labels(:)
  REAL(SRK),INTENT(IN) :: point
  REAL(SRK),INTENT(OUT) :: f(2)
  INTEGER(SIK),INTENT(OUT) :: i_p,N_i
  N_i=SIZE(labels)

  !Determine labels index(s) and weights
  f(:)=1.0_SRK
  IF(labels(1) < labels(2)) THEN
    !Ascending order
    i_p=getFirstGreater(labels,point)
    IF(i_p > 1 .AND. i_p < N_i+1) THEN
      f(1)=(labels(i_p)-point)/(labels(i_p)-labels(i_p-1))
      f(2)=1.0_SRK-f(1)
    ENDIF
  ELSE
    !Descending order
    i_p=getFirstGreaterEqual(labels,point)
    IF(i_p > 1 .AND. i_p < N_i+1) THEN
      f(1)=(point-labels(i_p))/(labels(i_p-1)-labels(i_p))
      f(2)=1.0_SRK-f(1)
    ENDIF
  ENDIF
ENDSUBROUTINE Get_points_and_weights
!
ENDMODULE InterpolatorsModule


      SUBROUTINE MB03WX( N, P, T, LDT1, LDT2, WR, WI, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To compute the eigenvalues of a product of matrices,
C     T = T_1*T_2*...*T_p, where T_1 is an upper quasi-triangular
C     matrix and T_2, ..., T_p are upper triangular matrices.
C
C     ARGUMENTS
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the matrix T.  N >= 0.
C
C     P       (input) INTEGER
C             The number of matrices in the product T_1*T_2*...*T_p.
C             P >= 1.
C
C     T       (input) DOUBLE PRECISION array, dimension (LDT1,LDT2,P)
C             The leading N-by-N part of T(*,*,1) must contain the upper
C             quasi-triangular matrix T_1 and the leading N-by-N part of
C             T(*,*,j) for j > 1 must contain the upper-triangular
C             matrix T_j, j = 2, ..., p.
C             The elements below the subdiagonal of T(*,*,1) and below
C             the diagonal of T(*,*,j), j = 2, ..., p, are not
C             referenced.
C
C     LDT1    INTEGER
C             The first leading dimension of the array T.
C             LDT1 >= max(1,N).
C
C     LDT2    INTEGER
C             The second leading dimension of the array T.
C             LDT2 >= max(1,N).
C
C     WR, WI  (output) DOUBLE PRECISION arrays, dimension (N)
C             The real and imaginary parts, respectively, of the
C             eigenvalues of T. The eigenvalues are stored in the same
C             order as on the diagonal of T_1. If T(i:i+1,i:i+1,1) is a
C             2-by-2 diagonal block with complex conjugated eigenvalues
C             then WI(i) > 0 and WI(i+1) = -WI(i).
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value.
C
C     CONTRIBUTOR
C
C     V. Sima, Katholieke Univ. Leuven, Belgium, February 1999.
C
C     REVISIONS
C
C     -
C
C     KEYWORDS
C
C     Eigenvalue, eigenvalue decomposition, periodic systems,
C     real Schur form, triangular form.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION ZERO, ONE
      PARAMETER        ( ZERO = 0.0D0, ONE = 1.0D+0 )
C     .. Scalar Arguments ..
      INTEGER          INFO, LDT1, LDT2, N, P
C     .. Array Arguments ..
      DOUBLE PRECISION T( LDT1, LDT2, * ), WI( * ), WR( * )
C     .. Local Scalars ..
      INTEGER          I, I1, INEXT, J
      DOUBLE PRECISION A11, A12, A21, A22, CS, SN, T11, T12, T22
C     .. External Subroutines ..
      EXTERNAL         DLANV2, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC        MAX
C     .. Executable Statements ..
C
      INFO = 0
C
C     Test the input scalar arguments.
C
      IF( N.LT.0 ) THEN
         INFO = -1
      ELSE IF( P.LT.1 ) THEN
         INFO = -2
      ELSE IF( LDT1.LT.MAX( 1, N ) ) THEN
         INFO = -4
      ELSE IF( LDT2.LT.MAX( 1, N ) ) THEN
         INFO = -5
      END IF
C
      IF( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'MB03WX', -INFO )
         RETURN
      END IF
C
      INEXT = 1
      DO 30 I = 1, N
         IF( I.LT.INEXT )
     $      GO TO 30
         IF( I.NE.N ) THEN
            IF( T( I+1, I, 1 ).NE.ZERO ) THEN
C
C              A pair of eigenvalues. First compute the corresponding
C              elements of T(I:I+1,I:I+1).
C
               INEXT = I + 2
               I1  = I + 1
               T11 = ONE
               T12 = ZERO
               T22 = ONE
C
               DO 10 J = 2, P
                  T22 = T22*T( I1, I1, J )
                  T12 = T11*T( I,  I1, J ) + T12*T( I1, I1, J )
                  T11 = T11*T( I,  I,  J )
   10          CONTINUE
C
               A11 = T( I,  I, 1 )*T11
               A12 = T( I,  I, 1 )*T12 + T( I,  I1, 1 )*T22
               A21 = T( I1, I, 1 )*T11
               A22 = T( I1, I, 1 )*T12 + T( I1, I1, 1 )*T22
C
               CALL DLANV2( A11, A12, A21, A22, WR( I ), WI( I ),
     $                      WR( I1 ), WI( I1 ), CS, SN )
               GO TO 30
            END IF
         END IF
C
C        Simple eigenvalue. Compute the corresponding element of T(I,I).
C
         INEXT = I + 1
         T11 = ONE
C
         DO 20 J = 1, P
            T11 = T11*T( I, I, J )
   20    CONTINUE
C
         WR( I ) = T11
         WI( I ) = ZERO
   30 CONTINUE
C
      RETURN
C *** Last line of MB03WX ***
      END


	SUBROUTINE PC_PHGT  ( hght, level1, level2, pres, iret )
C************************************************************************
C* PC_PHGT								*
C*									*
C* This subroutine computes the value of the pressure in a dataset	*
C* for a given value of height.  The computation uses the vertical	*
C* data stored internally by the PC package.				*
C*									*
C* PC_PHGT  ( HGHT, LEVEL1, LEVEL2, PRES, IRET )			*
C*									*
C* Input parameters:							*
C*	HGHT		REAL		Height				*
C*	LEVEL1		INTEGER		Level at or below height	*
C*	LEVEL2		INTEGER	  	Level above height		*
C*									*
C* Output parameters:							*
C*	PRES		REAL		Pressure			*
C*	IRET		INTEGER		Return code			*
C*					  0 = normal return		*
C*					-25 = incorrect levels		*
C*					-26 = vdata not computable	*
C*					-27 = max iterations done	*
C*					-28 = bad or missing data	*
C**									*
C* Log:									*
C* M. desJardins/GSFC	 9/84						*
C* M. desJardins/GSFC	 9/88	GEMPAK4					*
C* T. Lee/GSC		 8/97	Moved p1 outside EXP in PGUESS function	*
C************************************************************************
	INCLUDE		'GEMPRM.PRM'
	INCLUDE		'GMBDTA.CMN'
	INCLUDE    	'pccmn.cmn'
C*
	LOGICAL    	BETWEN
C
C*      Following include is for the missing data statement function.
C*      It must be before the other statement function declarations,
C*      since it is used in these other functions.
C
        INCLUDE    'ERMISS.FNC'
C
C*	Statement function to compute pressure, the logarithm of which
C*	is assumed to vary linearly with height.
C
	PGUESS () = p1 * EXP ( (hght-h1) * ALOG (p2/p1) / (h2-h1) )
C
C*	Statement function to check if x < a < y.
C
	BETWEN (x, y, a) = ( (.not. ERMISS (x)) .and.
     +                       (.not. ERMISS (y)) .and.
     +			      (x .le. a) .and. (a .le. y) )
C------------------------------------------------------------------------
C*	Check that data will be in vertical table.
C
	iret = 0
	pres = RMISSD
	IF  ( ( .not. vcomp (1) ) .or. ( .not. vcomp (3) ) )  THEN
	    iret = -26
	    RETURN
	END IF
C
C*	Find the bounding values of pressure and height.  Use index of
C*	1 for lower value of height and index of 2 for higher value.
C
	IF  ( vdata ( 3, level1 ) .lt. vdata ( 3, level2 ) )  THEN
	    p1 = vdata ( 1, level1 )
	    p2 = vdata ( 1, level2 )
	    h1 = vdata ( 3, level1 )
	    h2 = vdata ( 3, level2 )
	  ELSE
	    p1 = vdata ( 1, level2 )
	    p2 = vdata ( 1, level1 )
	    h1 = vdata ( 3, level2 )
	    h2 = vdata ( 3, level1 )
	END IF
C
C*	Check that this required data is not missing.
C
	IF  ( ERMISS ( p1 ) .or. ERMISS ( p2 ) .or.
     +	      ERMISS ( h1 ) .or. ERMISS ( h2 ) )  THEN
	    iret = -28
	  ELSE IF  ( ( p1 .eq. 0. ) .or. ( h1 .eq. h2 ) .or.
     +		     ( .not. BETWEN ( h1, h2, hght ) ) )  THEN
	    iret = -28
	END IF
	IF  ( iret .ne. 0 )  RETURN
C
C*	Use function PGUESS toc compute pressure.
C
	pres  = PGUESS ()
C*
	RETURN
	END


! --------------------------------------------------------------------------- !
!    UMAT FOR NEO HOOKEAN HYPERELASTICITY
!    CANNOT BE USED FOR PLANE STRESS
!
!    PROPS(1) : Young's modulus
!    PROPS(2) : Poisson's ratio
! --------------------------------------------------------------------------- !
subroutine umat(stress, statev, ddsdde, sse, spd, scd, rpl, &
     ddsddt, drplde, drpldt, stran, dstran, time, dtime, temp, dtemp, &
     predef, dpred, cmname, ndi, nshr, ntens, nstatv, props, nprops, &
     coords, drot, pnewdt, celent, dfgrd0, dfgrd1, noel, npt, layer, &
     kspt, kstep, kinc)

  implicit none
  integer, parameter :: dp=selected_real_kind(14)
  character*8, intent(in) :: cmname
  integer, intent(in) :: ndi, nshr, ntens, nstatv, nprops
  integer, intent(in) :: noel, npt, layer, kspt, kstep, kinc
  real(kind=dp), intent(in) :: sse, spd, scd, rpl, drpldt, time, dtime, temp, dtemp
  real(kind=dp), intent(in) :: pnewdt, celent
  real(kind=dp), intent(inout) :: stress(ntens), statev(nstatv), ddsdde(ntens, ntens)
  real(kind=dp), intent(inout) :: ddsddt(ntens), drplde(ntens)
  real(kind=dp), intent(in) :: stran(ntens), dstran(ntens)
  real(kind=dp), intent(in) :: predef(1), dpred(1), props(nprops), coords(3)
  real(kind=dp), intent(in) :: drot(3, 3), dfgrd0(3, 3), dfgrd1(3, 3)!

  integer :: i, j
  real(kind=dp) :: ee(6), eep(3), bbp(3), bbn(3,3)
  real(kind=dp) :: emod, enu, c10, d1, eg, ek, eg23, pr
  real(kind=dp) :: jac, scale, fb(3,3), bb(6), trbbar
  character*120 :: msg
  character*8 :: charv(1)
  integer :: intv(1)
  real(kind=dp) :: realv(1)
  real(kind=dp), parameter :: I6(6)=[1.,1.,1.,0.,0.,0.]
  ! ------------------------------------------------------------------------- !

  if (ndi /= 3) then
     msg = 'this umat may only be used for elements &
          &with three direct stress components'
     call stdb_abqerr(-3, msg, intv, realv, charv)
  end if

  ! elastic properties
  emod = props(1)
  enu = props(2)
  c10 = emod / (4. * (1. + enu))
  d1 = 6. * (1. - 2. * enu) / emod

  ! jacobian and distortion tensor
  jac = dfgrd1(1,1)*dfgrd1(2,2)*dfgrd1(3,3) - dfgrd1(1,2)*dfgrd1(2,1)*dfgrd1(3,3) &
      + dfgrd1(1,2)*dfgrd1(2,3)*dfgrd1(3,1) + dfgrd1(1,3)*dfgrd1(3,2)*dfgrd1(2,1) &
      - dfgrd1(1,3)*dfgrd1(3,1)*dfgrd1(2,2) - dfgrd1(2,3)*dfgrd1(3,2)*dfgrd1(1,1)
  scale = jac ** (-1. / 3.)
  fb = scale * dfgrd1

  ! deviatoric left cauchy-green deformation tensor
  bb(1) = fb(1,1) * fb(1,1) + fb(1,2) * fb(1,2) + fb(1,3) * fb(1,3)
  bb(2) = fb(2,1) * fb(2,1) + fb(2,2) * fb(2,2) + fb(2,3) * fb(2,3)
  bb(3) = fb(3,1) * fb(3,1) + fb(3,2) * fb(3,2) + fb(3,3) * fb(3,3)
  bb(4) = fb(2,1) * fb(1,1) + fb(2,2) * fb(1,2) + fb(2,3) * fb(1,3)
  bb(5) = fb(3,1) * fb(2,1) + fb(3,2) * fb(2,2) + fb(3,3) * fb(2,3)
  bb(6) = fb(3,1) * fb(1,1) + fb(3,2) * fb(1,2) + fb(3,3) * fb(1,3)
  trbbar = sum(bb(1:3)) / 3.
  eg = 2. * c10 / jac
  ek = 2. / d1 * (2. * jac - 1.)
  pr = 2. / d1 * (jac - 1.)

  ! cauchy stress
  stress = eg * (bb - trbbar * I6) + pr * I6

  ! spatial stiffness
  eg23 = eg * 2. / 3.
  ddsdde(1,1) =  eg23 * (bb(1) + trbbar) + ek
  ddsdde(1,2) = -eg23 * (bb(1) + bb(2)-trbbar) + ek
  ddsdde(1,3) = -eg23 * (bb(1) + bb(3)-trbbar) + ek
  ddsdde(1,4) =  eg23 * bb(4) / 2.
  ddsdde(1,5) = -eg23 * bb(5)
  ddsdde(1,6) =  eg23 * bb(6) / 2.

  ddsdde(2,2) =  eg23 * (bb(2) + trbbar) + ek
  ddsdde(2,3) = -eg23 * (bb(2) + bb(3)-trbbar) + ek
  ddsdde(2,4) =  eg23 * bb(4) / 2.
  ddsdde(2,5) =  eg23 * bb(5) / 2.
  ddsdde(2,6) = -eg23 * bb(6)

  ddsdde(3,3) =  eg23 * (bb(3) + trbbar) + ek
  ddsdde(3,4) = -eg23 * bb(4)
  ddsdde(3,5) =  eg23 * bb(5) / 2.
  ddsdde(3,6) =  eg23 * bb(6) / 2.

  ddsdde(4,4) =  eg * (bb(1) + bb(2)) / 2.
  ddsdde(4,5) =  eg * bb(6) / 2.
  ddsdde(4,6) =  eg * bb(5) / 2.

  ddsdde(5,5) =  eg * (bb(1) + bb(3)) / 2.
  ddsdde(5,6) =  eg * bb(4) / 2.

  ddsdde(6,6) =  eg * (bb(2) + bb(3)) / 2.
  forall(i=1:ntens,j=1:ntens,j<i) ddsdde(i,j) = ddsdde(j,i)

  ! ! logarithmic strains
  ! call sprind(bb, bbp, bbn, 1, 3, 3)
  ! eep(1) = log(sqrt(bbp(1)) / scale)
  ! eep(2) = log(sqrt(bbp(2)) / scale)
  ! eep(3) = log(sqrt(bbp(3)) / scale)
  ! ee(1) = eep(1) * bbn(1,1) ** 2 + eep(2) * bbn(2,1) ** 2 &
  !          + eep(3) * bbn(3,1) ** 2
  ! ee(2) = eep(1) * bbn(1,2) ** 2 + eep(2) * bbn(2,2) ** 2 &
  !          + eep(3) * bbn(3,2) ** 2
  ! ee(3) = eep(1) * bbn(1,3) ** 2 + eep(2) * bbn(2,3) ** 2 &
  !          + eep(3) * bbn(3,3) ** 2
  ! ee(4) = 2. * (eep(1) * bbn(1,1) * bbn(1,2) &
  !          + eep(2) * bbn(2,1) * bbn(2,2) &
  !          + eep(3) * bbn(3,1) * bbn(3,2))
  ! ee(5) = 2. * (eep(1) * bbn(1,1) * bbn(1,3) &
  !          + eep(2) * bbn(2,1) * bbn(2,3) &
  !          + eep(3) * bbn(3,1) * bbn(3,3))
  ! ee(6) = 2. * (eep(1) * bbn(1,2) * bbn(1,3) &
  !          + eep(2) * bbn(2,2) * bbn(2,3) &
  !          + eep(3) * bbn(3,2) * bbn(3,3))
  ! statev(1:ntens) = ee(k1)
  return
end subroutine umat


      SUBROUTINE AB08ND( EQUIL, N, M, P, A, LDA, B, LDB, C, LDC, D, LDD,
     $                   NU, RANK, DINFZ, NKROR, NKROL, INFZ, KRONR,
     $                   KRONL, AF, LDAF, BF, LDBF, TOL, 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 construct for a linear multivariable system described by a
C     state-space model (A,B,C,D) a regular pencil (A - lambda*B ) which
C                                                    f          f
C     has the invariant zeros of the system as generalized eigenvalues.
C     The routine also computes the orders of the infinite zeros and the
C     right and left Kronecker indices of the system (A,B,C,D).
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     EQUIL   CHARACTER*1
C             Specifies whether the user wishes to balance the compound
C             matrix (see METHOD) as follows:
C             = 'S':  Perform balancing (scaling);
C             = 'N':  Do not perform balancing.
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The number of state variables, i.e., the order of the
C             matrix A.  N >= 0.
C
C     M       (input) INTEGER
C             The number of system inputs.  M >= 0.
C
C     P       (input) INTEGER
C             The number of system outputs.  P >= 0.
C
C     A       (input) DOUBLE PRECISION array, dimension (LDA,N)
C             The leading N-by-N part of this array must contain the
C             state dynamics matrix A of the system.
C
C     LDA     INTEGER
C             The leading dimension of array A.  LDA >= MAX(1,N).
C
C     B       (input) DOUBLE PRECISION array, dimension (LDB,M)
C             The leading N-by-M part of this array must contain the
C             input/state matrix B of the system.
C
C     LDB     INTEGER
C             The leading dimension of array B.  LDB >= MAX(1,N).
C
C     C       (input) DOUBLE PRECISION array, dimension (LDC,N)
C             The leading P-by-N part of this array must contain the
C             state/output matrix C of the system.
C
C     LDC     INTEGER
C             The leading dimension of array C.  LDC >= MAX(1,P).
C
C     D       (input) DOUBLE PRECISION array, dimension (LDD,M)
C             The leading P-by-M part of this array must contain the
C             direct transmission matrix D of the system.
C
C     LDD     INTEGER
C             The leading dimension of array D.  LDD >= MAX(1,P).
C
C     NU      (output) INTEGER
C             The number of (finite) invariant zeros.
C
C     RANK    (output) INTEGER
C             The normal rank of the transfer function matrix.
C
C     DINFZ   (output) INTEGER
C             The maximum degree of infinite elementary divisors.
C
C     NKROR   (output) INTEGER
C             The number of right Kronecker indices.
C
C     NKROL   (output) INTEGER
C             The number of left Kronecker indices.
C
C     INFZ    (output) INTEGER array, dimension (N)
C             The leading DINFZ elements of INFZ contain information
C             on the infinite elementary divisors as follows:
C             the system has INFZ(i) infinite elementary divisors
C             of degree i, where i = 1,2,...,DINFZ.
C
C     KRONR   (output) INTEGER array, dimension (MAX(N,M)+1)
C             The leading NKROR elements of this array contain the
C             right Kronecker (column) indices.
C
C     KRONL   (output) INTEGER array, dimension (MAX(N,P)+1)
C             The leading NKROL elements of this array contain the
C             left Kronecker (row) indices.
C
C     AF      (output) DOUBLE PRECISION array, dimension
C             (LDAF,N+MIN(P,M))
C             The leading NU-by-NU part of this array contains the
C             coefficient matrix A  of the reduced pencil. The remainder
C                                 f
C             of the leading (N+M)-by-(N+MIN(P,M)) part is used as
C             internal workspace.
C
C     LDAF    INTEGER
C             The leading dimension of array AF.  LDAF >= MAX(1,N+M).
C
C     BF      (output) DOUBLE PRECISION array, dimension (LDBF,N+M)
C             The leading NU-by-NU part of this array contains the
C             coefficient matrix B  of the reduced pencil. The
C                                 f
C             remainder of the leading (N+P)-by-(N+M) part is used as
C             internal workspace.
C
C     LDBF    INTEGER
C             The leading dimension of array BF.  LDBF >= MAX(1,N+P).
C
C     Tolerances
C
C     TOL     DOUBLE PRECISION
C             A tolerance used in rank decisions to determine the
C             effective rank, which is defined as the order of the
C             largest leading (or trailing) triangular submatrix in the
C             QR (or RQ) factorization with column (or row) pivoting
C             whose estimated condition number is less than 1/TOL.
C             If the user sets TOL to be less than SQRT((N+P)*(N+M))*EPS
C             then the tolerance is taken as SQRT((N+P)*(N+M))*EPS,
C             where EPS is the machine precision (see LAPACK Library
C             Routine DLAMCH).
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (MAX(M,P))
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) returns the optimal value
C             of LDWORK.
C
C     LDWORK  INTEGER
C             The length of the array DWORK.
C             LDWORK >= MAX( 1, MIN(P,M) + MAX(3*M-1,N),
C                               MIN(P,N) + MAX(3*P-1,N+P,N+M),
C                               MIN(M,N) + MAX(3*M-1,N+M) ).
C             An upper bound is MAX(s,N) + MAX(3*s-1,N+s), with
C             s = MAX(M,P).
C             For optimum performance LDWORK should be larger.
C
C             If LDWORK = -1, then a workspace query is assumed;
C             the routine only calculates the optimal size of the
C             DWORK array, returns this value as the first entry of
C             the DWORK array, and no error message related to LDWORK
C             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
C     METHOD
C
C     The routine extracts from the system matrix of a state-space
C     system (A,B,C,D) a regular pencil A - lambda*B  which has the
C                                        f          f
C     invariant zeros of the system as generalized eigenvalues as
C     follows:
C
C        (a) construct the (N+P)-by-(N+M) compound matrix (B  A);
C                                                         (D  C)
C
C        (b) reduce the above system to one with the same invariant
C            zeros and with D of full row rank;
C
C        (c) pertranspose the system;
C
C        (d) reduce the system to one with the same invariant zeros and
C            with D square invertible;
C
C        (e) perform a unitary transformation on the columns of
C            (A - lambda*I  B) in order to reduce it to
C            (      C       D)
C
C            (A  - lambda*B   X)
C            ( f           f   ), with Y and B  square invertible;
C            (      0         Y)              f
C
C        (f) compute the right and left Kronecker indices of the system
C            (A,B,C,D), which together with the orders of the infinite
C            zeros (determined by steps (a) - (e)) constitute the
C            complete set of structural invariants under strict
C            equivalence transformations of a linear system.
C
C     REFERENCES
C
C     [1] Svaricek, F.
C         Computation of the Structural Invariants of Linear
C         Multivariable Systems with an Extended Version of
C         the Program ZEROS.
C         System & Control Letters, 6, pp. 261-266, 1985.
C
C     [2] Emami-Naeini, A. and Van Dooren, P.
C         Computation of Zeros of Linear Multivariable Systems.
C         Automatica, 18, pp. 415-430, 1982.
C
C     NUMERICAL ASPECTS
C
C     The algorithm is backward stable (see [2] and [1]).
C
C     FURTHER COMMENTS
C
C     In order to compute the invariant zeros of the system explicitly,
C     a call to this routine may be followed by a call to the LAPACK
C     Library routine DGGEV with A = A , B = B  and N = NU.
C                                     f       f
C     If RANK = 0, the routine DGEEV can be used (since B = I).
C                                                        f
C     CONTRIBUTOR
C
C     Release 3.0: V. Sima, Katholieke Univ. Leuven, Belgium, Nov. 1996.
C     Supersedes Release 2.0 routine AB08BD by F. Svaricek.
C
C     REVISIONS
C
C     Oct. 1997, Feb. 1998, Dec. 2003, March 2004, Jan. 2009, Mar. 2009,
C     Apr. 2009, Apr. 2011.
C
C     KEYWORDS
C
C     Generalized eigenvalue problem, Kronecker indices, multivariable
C     system, orthogonal transformation, structural invariant.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE
      PARAMETER         ( ZERO = 0.0D0, ONE = 1.0D0 )
C     .. Scalar Arguments ..
      CHARACTER         EQUIL
      INTEGER           DINFZ, INFO, LDA, LDAF, LDB, LDBF, LDC, LDD,
     $                  LDWORK, M, N, NKROL, NKROR, NU, P, RANK
      DOUBLE PRECISION  TOL
C     .. Array Arguments ..
      INTEGER           INFZ(*), IWORK(*), KRONL(*), KRONR(*)
      DOUBLE PRECISION  A(LDA,*), AF(LDAF,*), B(LDB,*), BF(LDBF,*),
     $                  C(LDC,*), D(LDD,*), DWORK(*)
C     .. Local Scalars ..
      LOGICAL           LEQUIL, LQUERY
      INTEGER           I, I1, II, J, MM, MNU, MU, NINFZ, NN, NU1, NUMU,
     $                  NUMU1, PP, RO, SIGMA, WRKOPT
      DOUBLE PRECISION  MAXRED, SVLMAX, THRESH, TOLER
C     .. External Functions ..
      LOGICAL           LSAME
      DOUBLE PRECISION  DLAMCH, DLANGE
      EXTERNAL          DLAMCH, DLANGE, LSAME
C     .. External Subroutines ..
      EXTERNAL          AB08NX, DCOPY, DLACPY, DLASET, DORMRZ, DTZRZF,
     $                  TB01ID, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         DBLE, INT, MAX, MIN, SQRT
C     .. Executable Statements ..
C
      INFO = 0
      LEQUIL = LSAME( EQUIL, 'S' )
      LQUERY = ( LDWORK.EQ.-1 )
C
C     Test the input scalar arguments.
C
      IF( .NOT.LEQUIL .AND. .NOT.LSAME( EQUIL, 'N' ) ) THEN
         INFO = -1
      ELSE IF( N.LT.0 ) THEN
         INFO = -2
      ELSE IF( M.LT.0 ) THEN
         INFO = -3
      ELSE IF( P.LT.0 ) THEN
         INFO = -4
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -6
      ELSE IF( LDB.LT.MAX( 1, N ) ) THEN
         INFO = -8
      ELSE IF( LDC.LT.MAX( 1, P ) ) THEN
         INFO = -10
      ELSE IF( LDD.LT.MAX( 1, P ) ) THEN
         INFO = -12
      ELSE IF( LDAF.LT.MAX( 1, N + M ) ) THEN
         INFO = -22
      ELSE IF( LDBF.LT.MAX( 1, N + P ) ) THEN
         INFO = -24
      ELSE
         II = MIN( P, M )
         I  = MAX(          II + MAX( 3*M - 1, N ),
     $             MIN( P, N ) + MAX( 3*P - 1, N+P, N+M ),
     $             MIN( M, N ) + MAX( 3*M - 1, N+M ), 1 )
         IF( LQUERY ) THEN
            SVLMAX = ZERO
            NINFZ  = 0
            CALL AB08NX( N, M, P, P, 0, SVLMAX, BF, LDBF, NINFZ, INFZ,
     $                   KRONL, MU, NU, NKROL, TOL, IWORK, DWORK, -1,
     $                   INFO )
            WRKOPT = MAX( I, INT( DWORK(1) ) )
            CALL AB08NX( N, II, M, M-II, II, SVLMAX, AF, LDAF, NINFZ,
     $                   INFZ, KRONL, MU, NU, NKROL, TOL, IWORK, DWORK,
     $                   -1, INFO )
            WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) )
            CALL DTZRZF( II, N+II, AF, LDAF, DWORK, DWORK, -1, INFO )
            WRKOPT = MAX( WRKOPT, II + INT( DWORK(1) ) )
            CALL DORMRZ( 'Right', 'Transpose', N, N+II, II, N, AF, LDAF,
     $                   DWORK, AF, LDAF, DWORK, -1, INFO )
            WRKOPT = MAX( WRKOPT, II + INT( DWORK(1) ) )
         ELSE IF( LDWORK.LT.I ) THEN
            INFO = -28
         END IF
      END IF
C
      IF ( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'AB08ND', -INFO )
         RETURN
      ELSE IF( LQUERY ) THEN
         DWORK(1) = WRKOPT
         RETURN
      END IF
C
      DINFZ = 0
      NKROL = 0
      NKROR = 0
C
C     Quick return if possible.
C
      IF ( N.EQ.0 ) THEN
         IF ( MIN( M, P ).EQ.0 ) THEN
            NU   = 0
            RANK = 0
            DWORK(1) = ONE
            RETURN
         END IF
      END IF
C
      MM = M
      NN = N
      PP = P
C
      DO 20 I = 1, N
         INFZ(I) = 0
   20 CONTINUE
C
      IF ( M.GT.0 ) THEN
         DO 40 I = 1, N + 1
            KRONR(I) = 0
   40    CONTINUE
      END IF
C
      IF ( P.GT.0 ) THEN
         DO 60 I = 1, N + 1
            KRONL(I) = 0
   60    CONTINUE
      END IF
C
C     (Note: Comments in the code beginning "Workspace:" describe the
C     minimal amount of real workspace needed at that point in the
C     code, as well as the preferred amount for good performance.)
C
      WRKOPT = 1
C
C     Construct the compound matrix  ( B  A ), dimension (N+P)-by-(M+N).
C                                    ( D  C )
C
      CALL DLACPY( 'Full', NN, MM, B, LDB, BF, LDBF )
      IF ( PP.GT.0 )
     $   CALL DLACPY( 'Full', PP, MM, D, LDD, BF(1+NN,1), LDBF )
      IF ( NN.GT.0 ) THEN
         CALL DLACPY( 'Full', NN, NN, A, LDA, BF(1,1+MM), LDBF )
         IF ( PP.GT.0 )
     $      CALL DLACPY( 'Full', PP, NN, C, LDC, BF(1+NN,1+MM), LDBF )
      END IF
C
C     If required, balance the compound matrix (default MAXRED).
C     Workspace: need   N.
C
      IF ( LEQUIL .AND. NN.GT.0 .AND. PP.GT.0 ) THEN
         MAXRED = ZERO
         CALL TB01ID( 'A', NN, MM, PP, MAXRED, BF(1,1+MM), LDBF, BF,
     $                LDBF, BF(1+NN,1+MM), LDBF, DWORK, INFO )
         WRKOPT = N
      END IF
C
C     If required, set tolerance.
C
      THRESH = SQRT( DBLE( (N + P)*(N + M) ) )*DLAMCH( 'Precision' )
      TOLER = TOL
      IF ( TOLER.LT.THRESH ) TOLER = THRESH
      SVLMAX = DLANGE( 'Frobenius', NN+PP, NN+MM, BF, LDBF, DWORK )
C
C     Reduce this system to one with the same invariant zeros and with
C     D upper triangular of full row rank MU (the normal rank of the
C     original system).
C     Workspace: need   MAX( 1, MIN(P,M) + MAX(3*M-1,N),
C                               MIN(P,N) + MAX(3*P-1,N+P,N+M) );
C                prefer larger.
C
      RO = PP
      SIGMA = 0
      NINFZ = 0
      CALL AB08NX( NN, MM, PP, RO, SIGMA, SVLMAX, BF, LDBF, NINFZ, INFZ,
     $             KRONL, MU, NU, NKROL, TOLER, IWORK, DWORK, LDWORK,
     $             INFO )
      WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) )
      RANK = MU
C
C     Pertranspose the system.
C
      NUMU = NU + MU
      IF ( NUMU.NE.0 ) THEN
         MNU = MM + NU
         NUMU1 = NUMU + 1
C
         DO 80 I = 1, NUMU
            CALL DCOPY( MNU, BF(I,1), LDBF, AF(1,NUMU1-I), -1 )
   80    CONTINUE
C
         IF ( MU.NE.MM ) THEN
C
C           Here MU < MM and MM > 0 (since MM = 0 implies MU = 0 = MM).
C
            PP = MM
            NN = NU
            MM = MU
C
C           Reduce the system to one with the same invariant zeros and
C           with D square invertible.
C           Workspace: need  MAX( 1, MU + MAX(3*MU-1,N),
C                                 MIN(M,N) + MAX(3*M-1,N+M) );
C                prefer larger. Note that MU <= MIN(P,M).
C
            RO = PP - MM
            SIGMA = MM
            CALL AB08NX( NN, MM, PP, RO, SIGMA, SVLMAX, AF, LDAF, NINFZ,
     $                   INFZ, KRONR, MU, NU, NKROR, TOLER, IWORK,
     $                   DWORK, LDWORK, INFO )
            WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) )
         END IF
C
         IF ( NU.NE.0 ) THEN
C
C           Perform a unitary transformation on the columns of
C                     ( B  A-lambda*I )
C                     ( D       C     )
C           in order to reduce it to
C                     ( X  AF-lambda*BF )
C                     ( Y       0       )
C           with Y and BF square invertible.
C
            CALL DLASET( 'Full', NU, MU, ZERO, ZERO, BF, LDBF )
            CALL DLASET( 'Full', NU, NU, ZERO, ONE,  BF(1,MU+1), LDBF )
C
            IF ( RANK.NE.0 ) THEN
               NU1 = NU + 1
               I1  = NU + MU
C
C              Workspace: need   2*MIN(M,P);
C                         prefer MIN(M,P) + MIN(M,P)*NB.
C
               CALL DTZRZF( MU, I1, AF(NU1,1), LDAF, DWORK, DWORK(MU+1),
     $                      LDWORK-MU, INFO )
               WRKOPT = MAX( WRKOPT, INT( DWORK(MU+1) ) + MU )
C
C              Workspace: need   MIN(M,P) + N;
C                         prefer MIN(M,P) + N*NB.
C
               CALL DORMRZ( 'Right', 'Transpose', NU, I1, MU, NU,
     $                      AF(NU1,1), LDAF, DWORK, AF, LDAF,
     $                      DWORK(MU+1), LDWORK-MU, INFO )
               WRKOPT = MAX( WRKOPT, INT( DWORK(MU+1) ) + MU )
C
               CALL DORMRZ( 'Right', 'Transpose', NU, I1, MU, NU,
     $                      AF(NU1,1), LDAF, DWORK, BF, LDBF,
     $                      DWORK(MU+1), LDWORK-MU, INFO )
C
            END IF
C
C           Move AF and BF in the first columns. This assumes that
C           DLACPY moves column by column.
C
            CALL DLACPY( 'Full', NU, NU, AF(1,MU+1), LDAF, AF, LDAF )
            IF ( RANK.NE.0 )
     $         CALL DLACPY( 'Full', NU, NU, BF(1,MU+1), LDBF, BF, LDBF )
C
         END IF
      END IF
C
C     Set right Kronecker indices (column indices).
C
      IF ( NKROR.GT.0 ) THEN
         J = 1
C
         DO 120 I = 1, N + 1
C
            DO 100 II = J, J + KRONR(I) - 1
               IWORK(II) = I - 1
  100       CONTINUE
C
            J = J + KRONR(I)
            KRONR(I) = 0
  120    CONTINUE
C
         NKROR = J - 1
C
         DO 140 I = 1, NKROR
            KRONR(I) = IWORK(I)
  140    CONTINUE
C
      END IF
C
C     Set left Kronecker indices (row indices).
C
      IF ( NKROL.GT.0 ) THEN
         J = 1
C
         DO 180 I = 1, N + 1
C
            DO 160 II = J, J + KRONL(I) - 1
               IWORK(II) = I - 1
  160       CONTINUE
C
            J = J + KRONL(I)
            KRONL(I) = 0
  180    CONTINUE
C
         NKROL = J - 1
C
         DO 200 I = 1, NKROL
            KRONL(I) = IWORK(I)
  200    CONTINUE
C
      END IF
C
      IF ( N.GT.0 ) THEN
         DINFZ = N
C
  220    CONTINUE
         IF ( INFZ(DINFZ).EQ.0 ) THEN
            DINFZ = DINFZ - 1
            IF ( DINFZ.GT.0 )
     $         GO TO 220
         END IF
      END IF
C
      DWORK(1) = WRKOPT
      RETURN
C *** Last line of AB08ND ***
      END


! update for sigma_zz, an sigma_xx
subroutine forward(nz, nx, nstep, &
                   deltaZ, deltaX, dt, &
                   lambda, mu, rho, &
                   hLambdaZ, hLambda2MuZ, hMuZ, hMuX, hRhoZX, &
                   sigmazz, sigmaxx, sigmazx, vz, vx, &
                   vdvzdz, vdvxdx, vdvxdz, vdvzdx, vdsigmazzdz, vdsigmazxdx, vdsigmaxxdx, vdsigmazxdz, &
                   mdvzdz, mdvxdx, mdvxdz, mdvzdx, mdsigmazzdz, mdsigmazxdx, mdsigmaxxdx, mdsigmazxdz, &
                   az, bz, kz, haz, hbz, hkz, &
                   ax, bx, kx, hax, hbx, hkx   )
    implicit none

    integer(kind=8)                         :: nz, nx, nstep
    real(kind=8)                            :: deltaZ, deltaX, dt
    real(kind=8), dimension(nz, nx)         :: lambda, mu, rho
    real(kind=8)                            :: hLambdaZ, hLambda2MuZ, hMuz, hMuX, hRhoZX
    real(kind=8), dimension(0:nz+1, 0:nx+1) :: sigmazz, sigmaxx, sigmazx, vz, vx
    real(kind=8)                            :: vdvzdz, vdvxdx, vdvxdz, vdvzdx, vdsigmazzdz, vdsigmazxdx, vdsigmaxxdx, vdsigmazxdz
    real(kind=8), dimension(nz, nx)         :: mdvzdz, mdvxdx, mdvxdz, mdvzdx, mdsigmazzdz, mdsigmazxdx, mdsigmaxxdx, mdsigmazxdz

    real(kind=8), dimension(nz)             :: az, bz, kz, haz, hbz, hkz
    real(kind=8), dimension(nx)             :: ax, bx, kx, hax, hbx, hkx

    integer(kind=8)                         :: i, j, it
    ! source related terms
    real(kind=8) :: f0, pi, factor, angle, t0, t, a, source, forceZ, forceX

    do it = 1 , nstep

      ! update sigmazz and sigmaxx
      do i = 2 , nx
         do j = 1, nz
            hLambdaZ    = 0.5d0 * (lambda(i,j) + lambda(i+1,j))
            hMuZ        = 0.5d0 * (    mu(i,j) +     mu(i+1,j))
            hLambda2MuZ = hLambdaZ + 2.d0 * hmuz
            ! partial derivative
            vdvzdz = (27.d0*vz(i+1,j)-27.d0*vz(i,j  ) - vz(i+2,j) + vz(i-1,j)) / (24.d0*deltaZ)
            vdvxdx = (27.d0*vx(i  ,j)-27.d0*vx(i,j-1) - vx(i,j+1) + vx(i,j-2)) / (24.d0*deltaX)
            ! update memory variable
            mdvzdz(i,j) = hbz(i)*mdvzdz(i,j) + haz(i) * vdvzdz
            mdvxdx(i,j) =  bx(j)*mdvxdx(i,j) +  ax(j) * vdvxdx
            ! apply dampping
            vdvzdz = vdvzdz / hkz(i) + mdvzdz(i,j)
            vdvxdx = vdvxdx /  kx(j) + mdvxdx(i,j)
            ! update state variable
            sigmazz(i,j) = sigmazz(i,j) + (hLambda2MuZ * vdvzdz + hLambdaZ * vdvxdx) * dt
            sigmaxx(i,j) = sigmaxx(i,j) + (hLambda2MuZ * vdvxdx + hLambdaZ * vdvzdz) * dt
         enddo
      enddo

      ! update sigmazx
      do j = 1 , nx-1
         do i = 2 , nz
            hMuX = 0.5d0 * (mu(i,j+1) + mu(i,j))

            vdvxdz = (27.d0*vx(i,j  ) - 27.d0*vx(i-1,j) - vx(i+1,j  ) + vx(i-2,j  )) / (24.d0*deltaZ)
            vdvzdx = (27.d0*vz(i,j+1) - 27.d0*vz(i  ,j) - vz(i  ,j+2) + vz(i  ,j-1)) / (24.d0*deltaX)
            mdvxdz(i,j) =  bz(i) * mdvxdz(i,j) +  az(i) * vdvxdz
            mdvzdx(i,j) = hbx(j) * mdvzdx(i,j) + hax(j) * vdvzdx

            vdvxdz = vdvxdz /  kz(i) + mdvxdz(i,j)
            vdvzdx = vdvzdx / hkx(j) + mdvzdx(i,j)  !!double check the damping parameter!!

            sigmazx(i,j) = sigmazx(i,j) + hMuX * (vdvxdz + vdvzdx) * dt
         enddo
      enddo

      ! update vz
      do j = 2 , nx
         do i = 2 , nz
            vdsigmazzdz = (27.d0*sigmazz(i,j) - 27.d0*sigmazz(i-1,j) - sigmazz(i+1,j) + sigmazz(i-2,j)) / (24.d0*deltaZ)
            vdsigmazxdx = (27.d0*sigmazx(i,j) - 27.d0*sigmazx(i,j-1) - sigmazx(i,j+1) + sigmazx(i,j-2)) / (24.d0*deltaX)

            mdsigmazzdz(i,j) = bz(i) * mdsigmazzdz(i,j) + az(i) * vdsigmazzdz
            mdsigmazxdx(i,j) = bx(j) * mdsigmazxdx(i,j) + ax(j) * vdsigmazxdx

            vdsigmazzdz = vdsigmazzdz / kz(i) + mdsigmazzdz(i,j)
            vdsigmazxdx = vdsigmazxdx / kx(j) + mdsigmazxdx(i,j)

            vz(i,j) = vz(i,j) + (vdsigmazzdz + vdsigmazxdx) * dt / rho(i,j)
         enddo
      enddo

      ! update vx
      do j = 1 , nx-1
         do i = 1 , nz-1
            hRhoZX = 0.25d0 * (rho(i,j) + rho(i+1,j) + rho(i,j+1) + rho(i+1,j+1))

            vdsigmazxdz = (27.d0*sigmazx(i+1,j) - 27.d0*sigmazx(i,j) - sigmazx(i+2,j) + sigmazx(i-1,j)) / (24.d0*deltaZ)
            vdsigmaxxdx = (27.d0*sigmaxx(i,j+1) - 27.d0*sigmaxx(i,j) - sigmaxx(i,j+2) + sigmazx(i,j-1)) / (24.d0*deltaX)

            mdsigmazxdz(i,j) = hbz(i) * mdsigmazxdz(i,j) + haz(i) * vdsigmazxdz
            mdsigmaxxdx(i,j) = hbx(j) * mdsigmaxxdx(i,j) + hax(j) * vdsigmaxxdx

            vdsigmazxdz = vdsigmazxdz / hkz(i) + mdsigmazxdz(i,j)
            vdsigmaxxdx = vdsigmaxxdx / hkx(j) + mdsigmaxxdx(i,j)
         enddo
      enddo

      ! add source to wave field
      f0 = 7.0; pi = 3.1415926d0; factor = 1.d7; angle = 2.3561d0
      t0 = 1.2d0 / f0
      a = pi * pi * f0 * f0
      t = (it-1)  * dt
      source = - factor * 2.d0 * a * (t-t0) * exp(-a*(t-t0)**2)
      forceZ = sin(angle) * source
      forceX = cos(angle) * source

      i = 20; j = 320;
      hRhoZX = 0.25d0 * (rho(i,j) + rho(i+1,j) + rho(i,j+1) + rho(i+1,j+1))

      vz(i,j) = vz(i,j) + forceZ * dt / rho(i,j)
      vx(i,j) = vx(i,j) + forceX * dt / hRhoZX
    enddo

end subroutine forward

subroutine assign(A, m, n)
    implicit none
    integer(kind=8), intent(in) :: m, n
    integer(kind=8), intent(inout), dimension(0:m+1, 0:n+1) :: A

    integer(kind=8) :: i, j, c=1
    real(kind=8) :: v

    do j = 1, n
       do i = 1, m
          A(i,j) = c
          c = c + 1
       end do
    end do

    A(1,1) = 1.d7

end subroutine assign


Module LTE
  Use Param_structure
  Use Model_structure
  Use Line_data_structure
  Use Atomic_data

Contains

Function Saha(npoints, T, Ne, U1, U2, E_ioniz)
!
! The Saha equation. This function returns the ratio of the atoms in the
! ionization stage 1 to those in the next ionization stage (e.g., neutrals
! over ions)
!
  Implicit none
  Integer :: npoints
  Real, dimension (npoints) :: T, Ne, U1, U2, Saha
  Double Precision, dimension (npoints) :: Sahad
  Real :: E_ioniz
!
  Sahad(1:npoints)=Ne(1:npoints)*2.07D-16*U1(1:npoints)/U2(1:npoints)* &
       (T(1:npoints)**(-1.5))*exp(E_ioniz/bk/T(1:npoints)*1.d0)
  Where(Sahad .gt. 1.e30)
     Sahad=1.e30
  End Where

  Saha=Sahad
  Return
End Function Saha

Subroutine Saha123(npoints,iel,T,Ne,n0overn,n1overn,n2overn)
! Solve (using safety checks) the ionization equilibrium for the three lowest ionization
! stages of atomic species of atomic number iel
! Exception: If iel .eq. 1 (H atom), then the third ionization stage actually refers to
! the H- ion
!
  Use Atomic_data
  Implicit none
  Integer :: iel, ip, npoints
  Real :: Ioniz
  Real, Parameter :: BK= 1.38066D-16, eV_to_cgs=1.602189D-12
  Real, dimension (npoints) :: T, Ne, U1, U2, U3, D1, D2, D3, n1overn, n2overn,  &
       n0overn,n3overn,nminusovern0,n1overn0,n2overn1
!
  Do ip=1, npoints
     Call Partition_f(iel, T(ip), U1(ip), U2(ip), U3(ip), D1(ip), D2(ip), D3(ip))
  End do
  Ioniz=At_ioniz1(iel)*eV_to_cgs
  n1overn0=1./saha(npoints, T, Ne, U1, U2, Ioniz)
  If (iel .ge. 2) then ! generic atom (say Fe)
     Ioniz=At_ioniz2(iel)*eV_to_cgs
     n2overn1=1./saha(npoints, T, Ne, U2, U3, Ioniz)
  Else ! H-
     U2(:)=1.0
     Ioniz=0.754*eV_to_cgs ! ionization energy for Hminus
     nminusovern0=saha(npoints, T, Ne, U2, U1, Ioniz) ! For Hminus
  End if
  Do ip=1, npoints
     If (iel .eq. 1) then ! H, H+ and H-
        If (nminusovern0(ip) .gt. 1e20) then ! It's all H-
           n2overn(ip)=1.
           n1overn(ip)=0.
           n0overn(ip)=0.
        Else if (n1overn0(ip) .gt. 1e20) then ! H is negligible (and so is H-), it's all H+
           n1overn(ip)=1.
           n2overn(ip)=0.
           n0overn(ip)=0.
        Else ! Consider them all
           n0overn(ip)=1./(1+nminusovern0(ip)+n1overn0(ip))
           n1overn(ip)=n1overn0(ip)*n0overn(ip)
           n2overn(ip)=nminusovern0(ip)*n0overn(ip)
        End if
     Else ! generic atom (say Fe)
        If (n2overn1(ip) .gt. 1e20) then ! It's all Fe++
           n2overn(ip)=1.
           n1overn(ip)=0.
           n0overn(ip)=0.
        Else if (n1overn0(ip) .gt. 1e20) then ! Fe is negligible, it's all Fe+ and Fe++
           n0overn(ip)=0.
           n2overn(ip)=1./(1+n2overn1(ip))
           n1overn(ip)=1.-n2overn(ip)
        Else ! Consider them all
           n0overn(ip)=1./(1.+n2overn1(ip)*n1overn0(ip)+n1overn0(ip))
           n1overn(ip)=n0overn(ip)*n1overn0(ip)
           n2overn(ip)=n1overn(ip)*n2overn1(ip)
        End if
     End if
  End do

End Subroutine Saha123

!****************************************************
!
      FUNCTION PLANCK(U,T)

!
!  CALCULATES PLANCK FUNCTION BNY AT FREQUENCY U, TEMP T
!
      DATA  EM/9.109534E-28/,UU/1.6605655E-24/
!
      X=HH*U/BK/T
!      PRINT *,'BK=',BK

!      PRINT *,'T=',T
!      PRINT *,'U=',U
!      PRINT *,'X=',X

      IF(X.LT.80.) THEN
        PLANCK=2.0*HH*U/CC*U/CC*U/(EXP(X)-1.0)
      ELSE
        PLANCK=2.0*HH*U/CC*U/CC*U*EXP(-X)
      ENDIF
!
      RETURN
    END FUNCTION PLANCK

Subroutine LTE_pop(Params, Line, Atmo, ng_i, ng_j)
!
  Implicit None
  Type (Parameters) :: Params
  Type (Model) :: Atmo
  Type (Line_data) :: Line
  Real, dimension (Params%n_points) :: N_e, U0, U1, U2, U_ion
  Real, dimension (Params%n_points) :: N_ion, ng_i, ng_j, N0_N1, N1_N2, N_tot
  Integer :: index, npoints, idepth, iline, Atomic_number
  Real :: du0, du1, du2, E_ioniz1, E_ioniz2, E_exc_i, E_exc_j, Abund_mass
  Real :: nu
  Logical, Save :: Warning=.FALSE.
!
  npoints=Params%n_points
  Atomic_number=Line%Atomic_number
  Abund_mass=At_weight(Atomic_number)*(10.**(At_abund(Atomic_number)-12.))/ &
       Sum(At_weight(1:N_elements)*(10.**(At_abund(1:N_elements)-12.)))
  N_tot(1:npoints)=Abund_mass*Atmo%rho(1:npoints)/At_weight(Atomic_number)/mass_pr

  N_e(1:npoints)=Atmo%El_p(1:npoints)/bk/Atmo%Temp(1:npoints)*1.d0
  Do idepth=1, npoints
     Call Partition_f(Atomic_number, Atmo%Temp(idepth), &
          U0(idepth), U1(idepth), U2(idepth), du0, du1, du2)
  End do
  E_ioniz1=At_ioniz1(Atomic_number)*eV_to_cgs
  E_ioniz2=At_ioniz2(Atomic_number)*eV_to_cgs
!
! Saha ionization equations
!
  N0_N1(1:npoints)=Saha(npoints, Atmo%Temp, N_e, U0, U1, E_ioniz1)
  N1_N2(1:npoints)=Saha(npoints, Atmo%Temp, N_e, U1, U2, E_ioniz2)
  N_ion(1:npoints)=N_tot(1:npoints)/  &
       (1.+N0_N1(1:npoints)+1./N1_N2(1:npoints)) ! This is N_1
  U_ion(1:npoints)=U1(1:npoints)
  If (Line%Ion_stage .eq. 1) then ! N_0
     N_ion(1:npoints)=N_ion(1:npoints)*N0_N1(1:npoints)
     U_ion(1:npoints)=U0(1:npoints)
  Else If (Line%Ion_stage .ge. 3) then ! N_2
     N_ion(1:npoints)=N_ion(1:npoints)/N1_N2(1:npoints)
     U_ion(1:npoints)=U2(1:npoints)
  End if
  If (Line%Ion_stage .gt. 3 .and. .not. Warning) then
     Warning=.TRUE.
     If (Params%Printout .ge. 1) Print *,'Warning. Ionization stage gt 3. Assuming 3 in LTE pop'
  End if
!
! Boltzmann excitation equations
!
  nu=cc/(Line%Wlength*1e-8) ! s^-1
  E_exc_i=Line%Energy_low*eV_to_cgs
  E_exc_j=E_exc_i+hh*nu
  ng_i(1:npoints)=N_ion(1:npoints)*exp(-E_exc_i/bk/Atmo%Temp(1:npoints))/ &
       U_ion(1:npoints)
  ng_j(1:npoints)=N_ion(1:npoints)*exp(-E_exc_j/bk/Atmo%Temp(1:npoints))/ &
       U_ion(1:npoints)

  Return
End Subroutine LTE_pop
!

End Module LTE


	FUNCTION PS_CINS ( datain, nparm, tvflag )
C************************************************************************
C* PS_CINS								*
C*									*
C* This function computes the Convective Inhibition (CINS) from top of	*
C* the mixing layer to the level of free convection (LFC):		*
C*									*
C*	CINS = GRAVTY * SUMN ( DELZ * ( THP - TH ) / TH )		*
C*									*
C*	     SUMN = sum of negative arguments				*
C*	     THP  = potential temperature of a parcel from the lowest	*
C*		    500 m of the atmosphere, raised dry adiabatically	*
C*		    to the lcl and moist adiabatically thereafter	*
C*	     TH   = potential temperature of the environment		*
C*									*
C* Integration is terminated at the equilibrium level.  TVFLAG		*
C* determines if temperature or virtual temperature is used in the	*
C* calculation.								*
C*									*
C* REAL PS_CINS ( DATAIN, NPARM, TVFLAG )				*
C*									*
C* Input parameters:							*
C*	DATAIN(NPARM,*)	REAL		Station data			*
C*	NPARM		INTEGER		Number of dataset parameters	*
C*	TVFLAG		LOGICAL		Temperature flag		*
C*									*
C* Output parameters:							*
C*	PS_CINS		REAL		Convective Inhibition		*
C**									*
C* Log:									*
C* G. Huffman/USRA	 8/89	Adapted from PS_CAPE			*
C* M. desJardins/GSFC	 7/90	Reorganized for GEMPAK 5		*
C* T. Lee/GSC		 8/97	Cleaned up; Added a temperature flag	*
C* M. Linda/GSC		10/97	Corrected the prologue format		*
C************************************************************************
	INCLUDE		'GEMPRM.PRM'
C*
	REAL		datain ( nparm, * )
C*
	REAL		stndl (10), stndb (10), stndt (10)
	LOGICAL		tvflag
C*
	INCLUDE		'ERMISS.FNC'
C------------------------------------------------------------------------
	PS_CINS = RMISSD
	dfdpth  = 500.
	idfcrd  = 3
C
C*	Get the layer averages in the surface parcel.
C
	CALL PS_PRCL  ( datain, nparm, dfdpth, idfcrd, 1, depth,
     +			idcord, pavg, tavg, tdavg, uavg, vavg, zavg,
     +			thavg, rmxavg, ier )
	IF  ( ( ier .ne. 0 ) .or. ERMISS (pavg) .or. ERMISS (tavg) .or.
     +	        ERMISS (tdavg) )  RETURN
C
C*	Calculate LCL; convert LCL in Kelvin to Celcius, compute THTA,
C*	THTE, and MIXR at the LCL; get the height at the LCL.
C
	tlcl  = PR_TLCL ( tavg, tdavg )
	plcl  = PR_PLCL ( tavg, pavg, tlcl )
	tlclc = PR_TMKC ( tlcl )
	thta  = PR_THTA ( tlclc, plcl )
	thte  = PR_THTE ( plcl,  tlclc, tlclc )
	rmix  = PR_MIXR ( tdavg, pavg )
	CALL PC_CMDT  ( 5, 6, 7, plcl, 1, datain, stndt, ier )
	htlcl = stndt (6)
C
C*	Find the level of free convection (LFC) and compute the height.
C*	Return if LFC is missing.
C
	plfc  = PS_LFCV  ( datain, nparm, thte, tvflag )
	IF  ( ERMISS (plfc) )  THEN
	    PS_CINS = 0.
	    RETURN
	END IF
	CALL PC_CMDT  ( 5, 6, 7, plfc, 1, datain, stndt, ier )
	htlfc  = stndt (6)
C
C*	Get the height of top of the mixing layer. Compute the
C*	environmental and parcel temperatures.
C
	CALL PC_DPTH  ( datain, nparm, 0., 1, dfdpth, idfcrd, 1,
     +			depth, idcord, stndl, stndb, stndt, ier )
	IF  ( idcord .eq. 1 )  THEN
	    ztop = stndt (1)
	  ELSE IF  ( idcord .eq. 2 )  THEN
	    ztop = PR_THTA ( stndt (2), stndt (1) )
	  ELSE
	    ztop = stndt (6)
	END IF
	CALL PC_CMDT  ( 5, 6, 7, ztop, idcord, datain, stndt, ier )
	IF  ( tvflag )  THEN
	    tetop = PR_TMCK ( stndt (7) )
	  ELSE
	    tetop = PR_TMCK ( stndt (2) )
	END IF
C
C*	If the parcel is below the LCL, get T, Td along the THTA,
C*	RMIX.  If the parcel is at or above the LCL (parcel is
C*	saturated), find T along the moist adiabat.
C
	IF  ( ztop .lt. htlcl )  THEN
	    ttop  = PR_TMPK ( stndt (1), thta )
	    IF  ( tvflag )  THEN
		ttopc = PR_TMKC ( ttop )
		tdc   = PR_DWPT ( rmix, stndt (1) )
		ttop = PR_TVRK ( ttopc, tdc, stndt (1) )
	    END IF
	  ELSE
	    ttop  = PR_TMST ( thte, stndt (1), 0. )
	    IF  ( tvflag )  THEN
		ttopc = PR_TMKC ( ttop )
		ttop = PR_TVRK ( ttopc, ttopc, stndt (1) )
	    END IF
	END IF
C
C*	Set increment, initial values, accumulation register.  Take
C*	trapezoidal integration in the negative areas.  Proceed until
C*	HTLFC is reached.
C
	zinc  = 100.
	tcum = 0.
C
	DO  WHILE ( ztop .lt. htlfc )
	    zbot  = ztop
	    tbot  = ttop
	    tebot = tetop
	    ztop  = zbot + zinc
	    CALL PC_CMDT  ( 5, 6, 7, ztop, 3, datain, stndt, ier )
C
C*	    Missing data ends the integration.  We assume that we're
C*	    done if the last difference was positive, and return a
C*	    value; otherwise, we are in the middle of negative area
C*	    and have to terminate with an error.
C
	    IF  ( ERMISS ( stndt (1) ) .or.
     +		  ERMISS ( stndt (2) ) )  THEN
		IF  ( ( tbot - tebot ) .ge. 0. )  THEN
		    PS_CINS = GRAVTY * tcum
		  ELSE
		    PS_CINS = RMISSD
		END IF
		RETURN
	    END IF
C
	    IF  ( tvflag )  THEN
		tetop = PR_TMCK ( stndt (7) )
	      ELSE
		tetop = PR_TMCK ( stndt (2) )
	    END IF
	    IF  ( ztop .lt. htlcl )  THEN
		ttop  = PR_TMPK ( stndt (1), thta )
		IF  ( tvflag )  THEN
		    ttopc = PR_TMKC ( ttop )
		    tdc   = PR_DWPT ( rmix, stndt (1) )
		    ttop = PR_TVRK ( ttopc, tdc, stndt (1) )
		END IF
	      ELSE
		ttop  = PR_TMST ( thte, stndt (1), 0. )
		IF  ( tvflag )  THEN
		    ttopc = PR_TMKC ( ttop )
		    ttop = PR_TVRK ( ttopc, ttopc, stndt (1) )
		END IF
	    END IF
	    dtbot = tbot - tebot
	    dttop = ttop - tetop
C
C*	    CASE I . . . bottom and top negative - add in contribution.
C
	    IF  ( dtbot .le. 0. )  THEN
		IF  ( dttop .le. 0. )  THEN
		    tcum = tcum + ( dtbot + dttop ) * zinc /
     +				  ( tebot + tetop )
C
C*		  CASE II . . . bottom negative, top positive - hit a
C*		  level of free convection (not necessarily HTLFC);
C*		  take partial layer.
C
		  ELSE
		    frac = dtbot / ( dtbot - dttop )
		    zfc  = zbot  + ( zinc * frac )
		    tfc  = tebot + ( ( tetop - tebot ) * frac )
		    tcum = tcum  + dtbot * ( zfc - zbot ) /
     +				   ( tebot + tfc )
		END IF
C
C*	      CASE III . . . bottom positive, top negative - hit an
C*	      equilibrium level (not necessarily HTEQ); take a
C*	      partial layer.
C
	      ELSE
		IF  ( dttop .le. 0. )  THEN
		    frac = dttop / ( dttop - dtbot )
		    zeq  = ztop  - ( zinc * frac )
		    teq  = tetop - ( ( tetop - tebot ) * frac )
		    tcum = tcum  + dttop * ( ztop - zeq ) /
     +			           ( teq + tetop )
C
C*		CASE IV . . . bottom and top positive - still in CAPE;
C*		ignore.
C
		END IF
	    END IF
	END DO
C
	PS_CINS = GRAVTY * tcum
C*
	RETURN
	END


C++***************************************************************
C Program to compute atmospheric refraction versus wavelength
C From Simon, W. 1966, A.J. 71,190
C
C
C JLP
C Version of 10-09-91
C--***************************************************************
	PROGRAM ATM_REFRAC
	PARAMETER (IDIM=1024)
	REAL*4 CC(15),QQ,LAMBDA(IDIM),AD(IDIM)
	CHARACTER OUTFILE*40,BUFFER*80
	DATA CC/3.45020E-03,-3.34591E-04,-1.60149E-05,-1.54316,
     1	2.27095E-01,3.14759E-03,2.87409E-03,-2.92730E-04,
     1	-1.56673E-05,1.41299E-03,-2.22307E-04,-1.66610E-06,
     1	-3.34814E-05,5.33352E-06,3.52107E-08/
	DATA QQ/0.740568/

	WRITE(6,35)
35	FORMAT(' Program ATM_REFRAC to compute atmospheric'
     1	' refraction',/,
     1	' From Simon, W. 1966, A.J. 71,190')

	WRITE(6,36)
36	FORMAT(' Enter P (mm Hg, atm. pressure), F (mm, water vapor),',
     1	/,' T (degree C, temperature), Zenith angle (degrees) :')
	READ(5,*) PP,FF,TT,ZD

	WRITE(6,37)
37	FORMAT(' Output file:')
	READ(5,10) OUTFILE
10	FORMAT(A)

	ZZ=TAND(ZD)
	Z2=ZZ*ZZ
	Z3=Z2*ZZ
	Z4=Z2*Z2

	SS=PP-0.148238*FF+(1.049-0.0157*TT)*PP*PP*1.E-06
	SS=SS/(720.883*(1.+0.003661*TT))
	SQ=SS/QQ

	NPTS=66
	STEP=(1-0.35)/FLOAT(NPTS-1)
	DO I=1,NPTS
C First Lambda in microns (for the formulae..)
	  LAMBDA(I)=0.35+STEP*FLOAT(I-1)
	  YY=1./(LAMBDA(I)*LAMBDA(I))
	  Y2=YY*YY
C Atmospheric dispersion in arcsec
C (From Simon, W. 1966, A.J. 71,190)
	  AD(I)=CC(1)+CC(2)*YY+CC(3)*Y2+CC(4)*ZZ+CC(5)*ZZ*YY
     1	+CC(6)*ZZ*Y2+CC(7)*Z2+CC(8)*Z2*YY+CC(9)*Z2*Y2
     1	+CC(10)*Z3+CC(11)*Z3*YY+CC(12)*Z3*Y2+CC(13)*Z4
     1	+CC(14)*Z4*YY+CC(15)*Z4*Y2
	  AD(I)=AD(I)*SQ
C Conversion to nm:
	  LAMBDA(I)=LAMBDA(I)*1000.
	END DO

C Output of the data:
	OPEN(1,FILE=OUTFILE,STATUS='UNKNOWN',ACCESS='SEQUENTIAL')
	WRITE(BUFFER,41) LAMBDA(1),AD(1),PP,FF,TT,ZD
41	FORMAT(F10.4,X,G13.6,5X,4(F10.4,X))
	WRITE(1,10) BUFFER
	DO I=2,NPTS
	  WRITE(1,42) LAMBDA(I),AD(I)
42	  FORMAT(F10.4,X,G13.6)
	END DO
	CLOSE(1)

	STOP
	END


 SUBROUTINE build_coeff_3D (ETA, beta, N, Hs, Tp, DELT, SEED, seed2, beta0,gamma)
!
! This subroutine returns Z(w) and kvec, the Fourier transform of
! the wave elevation, zeta(t) and a direction beta(w) for each frequency.
! The coefficients are based on a JONSWAP spectrum with a normal spreading
! around the heading angle beta0.
!
   USE Error_Function
   IMPLICIT none
   integer, parameter :: long=selected_real_kind(12,99)
   INTEGER SEED, seed2, n
   REAL(kind=long) :: Hs, Tp, delt, beta0,gamma
   REAL(kind=long) :: ETA(n), beta(n)
   ! Locals
   Integer i
   REAL gasdev, ran1_b
   REAL(kind=long) ::  pi,  df, cst, freq, spec,                       &
        jonswap_spectrum, zero=0._long,                                &
        one=1._long, two=2._long, four=4._long, half=.5_long,          &
        twopi, ran1, psi, mag, deg2rad, fp, sigma, erf1, erf2,         &
        rad2deg
   COMPLEX(kind=long) :: phase
   EXTERNAL gasdev, ran1_b, jonswap_spectrum
!
! Open the file to write the spectral coeeficients out to
!
   open(unit=78,file='spectrum',status='unknown')
   write(78,79)
79 format('# Spectrum for the directional random wave wavemaker signal. f, T, S(f), beta(f)')

   pi = acos(-one)
   twopi = two * pi
   deg2rad=pi/180._long; rad2deg=1/deg2rad
   df = one / (n * delt)
   cst = sqrt(df)
   fp=1/Tp

! Initialize
   beta(:)=beta0

! Take care of zero and Nyquist frequencies.
   eta (1) = zero;
   eta (2) = zero

! Build the Fourier coefficients using the chosen Spectrum and a random
! phase.  Each frequency has one heading angle which is also based on this
! random number and the accumulated PDF of the spectrum.

   DO  i = 2, n / 2
      freq = (i - 1) * df
      psi=ran1_b(seed2)
!
! Choose here whether the Fourier amplitudes should be exactly on the spectrum
! or Gaussianly distributed around the spectral value.
!
      mag=one
!      mag=gasdev(seed)  ! This will give a Guassian distribution of magnitude.
!
      phase=mag*cmplx(cos(twopi*psi),sin(twopi*psi))

! Get the wave spectrum value and the direction at this frequency.

      spec= jonswap_spectrum(freq, Hs, Tp, gamma)

      sigma=0.2_long*(freq/fp)**2.5_long
      erf1=Erf(pi/(two*sqrt(two*sigma)))
      erf2=InvErf((two*psi-one)*erf1)

      beta(i)=beta0+rad2deg*sqrt(2*sigma)*erf2

      write(78,*)freq,1/freq,spec,beta(i)
      !
      ! Load the Fourier coefficients for this frequency
      !
      ! Real part
      eta (2*i-1) = REAL(sqrt (two * spec) * cst * phase)
      ! Imaginary part
      eta (2*i)   = AIMAG(sqrt (two * spec) * cst * phase)

   END DO
   close(78)

   RETURN
 END SUBROUTINE BUILD_COEFF_3D