      subroutine pest_decay

!!    ~ ~ ~ PURPOSE ~ ~ ~
!!    this subroutine calculates degradation of pesticide in the soil and on
!!    the plants

!!    ~ ~ ~ INCOMING VARIABLES ~ ~ ~
!!    name          |units         |definition
!!    ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~
!!    decay_f(:)    |none          |exponential of the rate constant for
!!                                 |degradation of the pesticide on foliage
!!    decay_s(:)    |none          |exponential of the rate constant for
!!                                 |degradation of the pesticide in soil
!!    ihru          |none          |HRU number

      use pesticide_data_module
      use hru_module, only : hru, ihru
      use constituent_mass_module
      use soil_module
      use plant_module
      use output_ls_pesticide_module

      implicit none

      integer :: j               !none     |hru number
      integer :: k               !none     |seqential pesticide number being simulated
      integer :: ipest_db        !none     |pesticide number from pesticide data base
      integer :: l               !none     |layer number
      real :: pest_init          !kg/ha    |amount of pesticide present at beginning of day
      real :: pest_end           !kg/ha    |amount of pesticide present at end of day
      real :: pst_decay_s        !kg/ha    |amount of pesticide decay in soil during day

      j = ihru

      if (cs_db%num_pests == 0) return

      do k = 1, cs_db%num_pests
        hpestb_d(j)%pest(k)%decay_s = 0.
        hpestb_d(j)%pest(k)%decay_f = 0.
        ipest_db = cs_db%pest_num(k)
        if (ipest_db > 0) then
          pst_decay_s = 0.
          !! calculate degradation in soil
          do l = 1, soil(j)%nly
            pest_init = cs_soil(j)%ly(l)%pest(k)
            if (pest_init > 1.e-12) then
              pest_end = pest_init * pestcp(ipest_db)%decay_s
              cs_soil(j)%ly(l)%pest(k) = pest_end
              pst_decay_s = pst_decay_s + (pest_init - pest_end)
            end if
          end do
          hpestb_d(j)%pest(k)%decay_s = pst_decay_s

          !! calculate degradation on plant foliage
          pest_init = cs_pl(j)%pest(k)
          if (pest_init > 1.e-12) then
            pest_end = pest_init * pestcp(ipest_db)%decay_f
            cs_pl(j)%pest(k) = pest_end
            hpestb_d(j)%pest(k)%decay_f = pest_init - pest_end
          end if
        end if
      end do

      return
      end subroutine pest_decay


      SUBROUTINE DSICO(A,LDA,N,KPVT,RCOND,Z)
      INTEGER LDA,N,KPVT(1)
      DOUBLE PRECISION A(LDA,1),Z(1)
      DOUBLE PRECISION RCOND
C
C     DSICO FACTORS A DOUBLE PRECISION SYMMETRIC MATRIX BY ELIMINATION
C     WITH SYMMETRIC PIVOTING AND ESTIMATES THE CONDITION OF THE
C     MATRIX.
C
C     IF  RCOND  IS NOT NEEDED, DSIFA IS SLIGHTLY FASTER.
C     TO SOLVE  A*X = B , FOLLOW DSICO BY DSISL.
C     TO COMPUTE  INVERSE(A)*C , FOLLOW DSICO BY DSISL.
C     TO COMPUTE  INVERSE(A) , FOLLOW DSICO BY DSIDI.
C     TO COMPUTE  DETERMINANT(A) , FOLLOW DSICO BY DSIDI.
C     TO COMPUTE  INERTIA(A), FOLLOW DSICO BY DSIDI.
C
C     ON ENTRY
C
C        A       DOUBLE PRECISION(LDA, N)
C                THE SYMMETRIC MATRIX TO BE FACTORED.
C                ONLY THE DIAGONAL AND UPPER TRIANGLE ARE USED.
C
C        LDA     INTEGER
C                THE LEADING DIMENSION OF THE ARRAY  A .
C
C        N       INTEGER
C                THE ORDER OF THE MATRIX  A .
C
C     OUTPUT
C
C        A       A BLOCK DIAGONAL MATRIX AND THE MULTIPLIERS WHICH
C                WERE USED TO OBTAIN IT.
C                THE FACTORIZATION CAN BE WRITTEN  A = U*D*TRANS(U)
C                WHERE  U  IS A PRODUCT OF PERMUTATION AND UNIT
C                UPPER TRIANGULAR MATRICES , TRANS(U) IS THE
C                TRANSPOSE OF  U , AND  D  IS BLOCK DIAGONAL
C                WITH 1 BY 1 AND 2 BY 2 BLOCKS.
C
C        KPVT    INTEGER(N)
C                AN INTEGER VECTOR OF PIVOT INDICES.
C
C        RCOND   DOUBLE PRECISION
C                AN ESTIMATE OF THE RECIPROCAL CONDITION OF  A .
C                FOR THE SYSTEM  A*X = B , RELATIVE PERTURBATIONS
C                IN  A  AND  B  OF SIZE  EPSILON  MAY CAUSE
C                RELATIVE PERTURBATIONS IN  X  OF SIZE  EPSILON/RCOND .
C                IF  RCOND  IS SO SMALL THAT THE LOGICAL EXPRESSION
C                           1.0 + RCOND .EQ. 1.0
C                IS TRUE, THEN  A  MAY BE SINGULAR TO WORKING
C                PRECISION.  IN PARTICULAR,  RCOND  IS ZERO  IF
C                EXACT SINGULARITY IS DETECTED OR THE ESTIMATE
C                UNDERFLOWS.
C
C        Z       DOUBLE PRECISION(N)
C                A WORK VECTOR WHOSE CONTENTS ARE USUALLY UNIMPORTANT.
C                IF  A  IS CLOSE TO A SINGULAR MATRIX, THEN  Z  IS
C                AN APPROXIMATE NULL VECTOR IN THE SENSE THAT
C                NORM(A*Z) = RCOND*NORM(A)*NORM(Z) .
C
C     LINPACK. THIS VERSION DATED 08/14/78 .
C     CLEVE MOLER, UNIVERSITY OF NEW MEXICO, ARGONNE NATIONAL LAB.
C
C     SUBROUTINES AND FUNCTIONS
C
C     LINPACK DSIFA
C     BLAS DAXPY,DDOT,DSCAL,DASUM
C     FORTRAN DABS,DMAX1,IABS,DSIGN
C
C     INTERNAL VARIABLES
C
      DOUBLE PRECISION AK,AKM1,BK,BKM1,DDOT,DENOM,EK,T
      DOUBLE PRECISION ANORM,S,DASUM,YNORM
      INTEGER I,INFO,J,JM1,K,KP,KPS,KS
C
C
C     FIND NORM OF A USING ONLY UPPER HALF
C
      DO 30 J = 1, N
         Z(J) = DASUM(J,A(1,J),1)
         JM1 = J - 1
         IF (JM1 .LT. 1) GO TO 20
         DO 10 I = 1, JM1
            Z(I) = Z(I) + DABS(A(I,J))
   10    CONTINUE
   20    CONTINUE
   30 CONTINUE
      ANORM = 0.0D0
      DO 40 J = 1, N
         ANORM = DMAX1(ANORM,Z(J))
   40 CONTINUE
C
C     FACTOR
C
      CALL DSIFA(A,LDA,N,KPVT,INFO)
C
C     RCOND = 1/(NORM(A)*(ESTIMATE OF NORM(INVERSE(A)))) .
C     ESTIMATE = NORM(Z)/NORM(Y) WHERE  A*Z = Y  AND  A*Y = E .
C     THE COMPONENTS OF  E  ARE CHOSEN TO CAUSE MAXIMUM LOCAL
C     GROWTH IN THE ELEMENTS OF W  WHERE  U*D*W = E .
C     THE VECTORS ARE FREQUENTLY RESCALED TO AVOID OVERFLOW.
C
C     SOLVE U*D*W = E
C
      EK = 1.0D0
      DO 50 J = 1, N
         Z(J) = 0.0D0
   50 CONTINUE
      K = N
   60 IF (K .EQ. 0) GO TO 120
         KS = 1
         IF (KPVT(K) .LT. 0) KS = 2
         KP = IABS(KPVT(K))
         KPS = K + 1 - KS
         IF (KP .EQ. KPS) GO TO 70
            T = Z(KPS)
            Z(KPS) = Z(KP)
            Z(KP) = T
   70    CONTINUE
         IF (Z(K) .NE. 0.0D0) EK = DSIGN(EK,Z(K))
         Z(K) = Z(K) + EK
         CALL DAXPY(K-KS,Z(K),A(1,K),1,Z(1),1)
         IF (KS .EQ. 1) GO TO 80
            IF (Z(K-1) .NE. 0.0D0) EK = DSIGN(EK,Z(K-1))
            Z(K-1) = Z(K-1) + EK
            CALL DAXPY(K-KS,Z(K-1),A(1,K-1),1,Z(1),1)
   80    CONTINUE
         IF (KS .EQ. 2) GO TO 100
            IF (DABS(Z(K)) .LE. DABS(A(K,K))) GO TO 90
               S = DABS(A(K,K))/DABS(Z(K))
               CALL DSCAL(N,S,Z,1)
               EK = S*EK
   90       CONTINUE
            IF (A(K,K) .NE. 0.0D0) Z(K) = Z(K)/A(K,K)
            IF (A(K,K) .EQ. 0.0D0) Z(K) = 1.0D0
         GO TO 110
  100    CONTINUE
            AK = A(K,K)/A(K-1,K)
            AKM1 = A(K-1,K-1)/A(K-1,K)
            BK = Z(K)/A(K-1,K)
            BKM1 = Z(K-1)/A(K-1,K)
            DENOM = AK*AKM1 - 1.0D0
            Z(K) = (AKM1*BK - BKM1)/DENOM
            Z(K-1) = (AK*BKM1 - BK)/DENOM
  110    CONTINUE
         K = K - KS
      GO TO 60
  120 CONTINUE
      S = 1.0D0/DASUM(N,Z,1)
      CALL DSCAL(N,S,Z,1)
C
C     SOLVE TRANS(U)*Y = W
C
      K = 1
  130 IF (K .GT. N) GO TO 160
         KS = 1
         IF (KPVT(K) .LT. 0) KS = 2
         IF (K .EQ. 1) GO TO 150
            Z(K) = Z(K) + DDOT(K-1,A(1,K),1,Z(1),1)
            IF (KS .EQ. 2)
     *         Z(K+1) = Z(K+1) + DDOT(K-1,A(1,K+1),1,Z(1),1)
            KP = IABS(KPVT(K))
            IF (KP .EQ. K) GO TO 140
               T = Z(K)
               Z(K) = Z(KP)
               Z(KP) = T
  140       CONTINUE
  150    CONTINUE
         K = K + KS
      GO TO 130
  160 CONTINUE
      S = 1.0D0/DASUM(N,Z,1)
      CALL DSCAL(N,S,Z,1)
C
      YNORM = 1.0D0
C
C     SOLVE U*D*V = Y
C
      K = N
  170 IF (K .EQ. 0) GO TO 230
         KS = 1
         IF (KPVT(K) .LT. 0) KS = 2
         IF (K .EQ. KS) GO TO 190
            KP = IABS(KPVT(K))
            KPS = K + 1 - KS
            IF (KP .EQ. KPS) GO TO 180
               T = Z(KPS)
               Z(KPS) = Z(KP)
               Z(KP) = T
  180       CONTINUE
            CALL DAXPY(K-KS,Z(K),A(1,K),1,Z(1),1)
            IF (KS .EQ. 2) CALL DAXPY(K-KS,Z(K-1),A(1,K-1),1,Z(1),1)
  190    CONTINUE
         IF (KS .EQ. 2) GO TO 210
            IF (DABS(Z(K)) .LE. DABS(A(K,K))) GO TO 200
               S = DABS(A(K,K))/DABS(Z(K))
               CALL DSCAL(N,S,Z,1)
               YNORM = S*YNORM
  200       CONTINUE
            IF (A(K,K) .NE. 0.0D0) Z(K) = Z(K)/A(K,K)
            IF (A(K,K) .EQ. 0.0D0) Z(K) = 1.0D0
         GO TO 220
  210    CONTINUE
            AK = A(K,K)/A(K-1,K)
            AKM1 = A(K-1,K-1)/A(K-1,K)
            BK = Z(K)/A(K-1,K)
            BKM1 = Z(K-1)/A(K-1,K)
            DENOM = AK*AKM1 - 1.0D0
            Z(K) = (AKM1*BK - BKM1)/DENOM
            Z(K-1) = (AK*BKM1 - BK)/DENOM
  220    CONTINUE
         K = K - KS
      GO TO 170
  230 CONTINUE
      S = 1.0D0/DASUM(N,Z,1)
      CALL DSCAL(N,S,Z,1)
      YNORM = S*YNORM
C
C     SOLVE TRANS(U)*Z = V
C
      K = 1
  240 IF (K .GT. N) GO TO 270
         KS = 1
         IF (KPVT(K) .LT. 0) KS = 2
         IF (K .EQ. 1) GO TO 260
            Z(K) = Z(K) + DDOT(K-1,A(1,K),1,Z(1),1)
            IF (KS .EQ. 2)
     *         Z(K+1) = Z(K+1) + DDOT(K-1,A(1,K+1),1,Z(1),1)
            KP = IABS(KPVT(K))
            IF (KP .EQ. K) GO TO 250
               T = Z(K)
               Z(K) = Z(KP)
               Z(KP) = T
  250       CONTINUE
  260    CONTINUE
         K = K + KS
      GO TO 240
  270 CONTINUE
C     MAKE ZNORM = 1.0
      S = 1.0D0/DASUM(N,Z,1)
      CALL DSCAL(N,S,Z,1)
      YNORM = S*YNORM
C
      IF (ANORM .NE. 0.0D0) RCOND = YNORM/ANORM
      IF (ANORM .EQ. 0.0D0) RCOND = 0.0D0
      RETURN
      END


      double complex function A43ggpppp(e1,p2,p3,e4,za,zb,zab,zba)
      implicit none
C-----Authors: John Campbell and Keith Ellis, November 2011
C---- taken form arXiv:1101.5947 [hep-ph], Eq. (5,52)
      include 'constants.f'
      include 'masses.f'
      include 'zprods_decl.f'
      include 'zabprods_decl.f'
      include 'sprods_com.f'
      include 'epinv.f'
      include 'scale.f'
      double precision mt2,beta,xlog
      double complex BSYALggpppphp,BSYARggpppphp,BSYALslggpppp
      double complex BSYA0ggpppp,lnrat,Vslc
      integer e1,p2,p3,e4

      mt2=mt**2
      beta=sqrt(1d0-4d0*mt2/s(p2,p3))
      xlog=log((1d0-beta)/(1d0+beta))/beta
C---- arXiv:1101.5947 [hep-ph], Eq. (52)
c----   with an extra minus sign to account for A4;3 -> -Vslc, c.f. Eq. (53))
      Vslc=-dcmplx(epinv)*
     & (+s(1,p2)/s(p2,p3)*(lnrat(musq,-s(1,p2))+lnrat(mt2,-s(1,p2)))
     &  +s(1,p3)/s(p2,p3)*(lnrat(musq,-s(1,p3))+lnrat(mt2,-s(1,p3)))
     & +lnrat(musq,-s(p2,p3))+(1d0-2d0*mt2/s(p2,p3))*xlog)

      A43ggpppp=(
     &  Vslc*(BSYA0ggpppp(e1,p2,p3,e4,za,zb,zab,zba)
     &       +BSYA0ggpppp(e1,p3,p2,e4,za,zb,zab,zba))
     & +BSYALggpppphp(e1,p2,p3,e4,za,zb,zab,zba)
     & +BSYALggpppphp(e1,p3,p2,e4,za,zb,zab,zba)
     & +BSYALslggpppp(e1,p2,e4,p3,za,zb,zab,zba)
     & +BSYALslggpppp(e1,p3,e4,p2,za,zb,zab,zba)
     & +BSYARggpppphp(e1,p2,p3,e4,za,zb,zab,zba)
     & +BSYARggpppphp(e1,p3,p2,e4,za,zb,zab,zba))

      return
      end


      subroutine geneq_stress_fem_2D_XY()
!***********************************************************************
! Copyright 2011 Los Alamos National Security, LLC  All rights reserved
! Unless otherwise indicated,  this information has been authored by an
! employee or employees of the Los Alamos National Security, LLC (LANS),
! operator of the  Los  Alamos National  Laboratory  under Contract  No.
! DE-AC52-06NA25396  with  the U. S. Department  of  Energy.  The  U. S.
! Government   has   rights  to  use,  reproduce,  and  distribute  this
! information.  The  public may copy  and  use this  information without
! charge, provided that this  Notice and any statement of authorship are
! reproduced on all copies.  Neither  the  Government nor LANS makes any
! warranty,   express   or   implied,   or   assumes  any  liability  or
! responsibility for the use of this information.
!***********************************************************************
!
! Assembles the mechanical equations for 'fem' computations in two dimensions
!
! Author : Sai Rapaka
!

      use comai, only: nei, neq, ns, igrav, grav
      use combi, only: nelm, nelmdg
      use comdi, only: denr
      use comei, only: a
      use comgi, only: bp
      use comsi, only: e1, e2, e3, iPlastic, ibodyforce
      use comfem
      use davidi,only: nmat, nrhs

      implicit none

      integer                      :: i,j,k,li,m,ii1,ii2
      integer                      :: nodei, nodej, jmia, iau
      integer, dimension(4)        :: node
      real*8,  dimension(3, 8)     :: B
      real*8,  dimension(3, 3)     :: D
      real*8,  dimension(3, 8)     :: DB
      real*8,  dimension(8, 8)     :: BtDB
      real*8,  dimension(2, 2)     :: nodalK
      real*8,  dimension(3)        :: gp_stress
      real*8,  dimension(8)        :: Btsigma
      real*8,  dimension(4)        :: bforcex, bforcey, bforcez

      real*8                       :: e1bar, e2bar, e3bar
      real*8                       :: fac

      do i=1,nei
        do k=1,ns
          node(k) = elnode(i,k)
        enddo

        do j=1,numgausspoints

          call fem_update_stress_2D(i, j)

          B = 0.0d0
          do k=1,ns
            B(1,2*(k-1) + 1) = dPsidX(i, j, k)
            B(2,2*(k-1) + 2) = dPsidY(i, j, k)
            B(3,2*(k-1) + 1) = dPsidY(i, j, k)
            B(3,2*(k-1) + 2) = dPsidX(i, j, k)
          enddo

          !!! Get D matrix from material module
          if(iPlastic.eq.1) then
            call fem_material_stiffness_2D(i, j, D)
          else
            e1bar = 0.0d0
            e2bar = 0.0d0
            e3bar = 0.0d0

            do k=1,ns
              e1bar = e1bar + Psi(i, j, k)*e1(node(k))
              e2bar = e2bar + Psi(i, j, k)*e2(node(k))
              e3bar = e3bar + Psi(i, j, k)*e3(node(k))
            enddo

            D = 0.0d0
            D(1,1) = e1bar
            D(1,2) = e2bar
            D(2,1) = e2bar
            D(2,2) = e1bar
            D(3,3) = e3bar
          endif

          DB = matmul(D, B)
          BtDB = matmul(transpose(B), DB)

          fac = detJ(i,j)*gpweight(j)

          do k=1,ns
            nodei = node(k)
            do li=1,ns
              if(li.ne.k) then
                nodej = node(li)
                ! diagonal entry
                jmia = nelmdg(nodei) - (neq + 1)
                ! (i,j) entry
                ii1 = nelm(nodei) + 1
                ii2 = nelm(nodei + 1)
                iau = 0
                do m=ii1,ii2
                  if(nelm(m) .eq. nodej) then
                    iau  = m - (neq + 1)
                  endif
                enddo

                nodalK = BtDB(2*k-1:2*k, 2*li-1:2*li)
                a(iau + nmat(1)) = a(iau + nmat(1)) + nodalK(1,1)*fac
                a(iau + nmat(2)) = a(iau + nmat(2)) + nodalK(1,2)*fac
                a(iau + nmat(3)) = a(iau + nmat(3)) + nodalK(2,1)*fac
                a(iau + nmat(4)) = a(iau + nmat(4)) + nodalK(2,2)*fac

                a(jmia + nmat(1)) = a(jmia + nmat(1)) - nodalK(1,1)*fac
                a(jmia + nmat(2)) = a(jmia + nmat(2)) - nodalK(1,2)*fac
                a(jmia + nmat(3)) = a(jmia + nmat(3)) - nodalK(2,1)*fac
                a(jmia + nmat(4)) = a(jmia + nmat(4)) - nodalK(2,2)*fac
              endif
            enddo ! inner loop over neighboring node
          enddo   ! loop over node i

          !body forces
          bforcex = 0.0d0
          bforcey = 0.0d0

          if(ibodyforce.ne.0) then
            do k=1,ns
              if(igrav.eq.1) then
                bforcex(k) = -denr(k)*grav*Psi(i,j,k)*fac
              elseif(igrav.eq.2) then
                bforcey(k) = -denr(k)*grav*Psi(i,j,k)*fac
              endif
              bp(node(k) + nrhs(1)) = bp(node(k) + nrhs(1)) + bforcex(k)
              bp(node(k) + nrhs(2)) = bp(node(k) + nrhs(2)) + bforcey(k)
            enddo
          endif

          gp_stress = fem_stress(i,j,:)
          Btsigma = matmul(transpose(B), gp_stress)

          do k=1,ns
            bp(node(k) + nrhs(1)) = bp(node(k) + nrhs(1)) +
     &         Btsigma(2*k-1)*fac
            bp(node(k) + nrhs(2)) = bp(node(k) + nrhs(2)) +
     &         Btsigma(2*k  )*fac
          enddo

        enddo     ! loop over gausspoints
      enddo       ! loop over elements

      end subroutine geneq_stress_fem_2D_XY


subroutine matrixp()

    use constants_module
    use arrays_module
    use matrix_module
    use var_module
    use arrays_section_module

    implicit none

    ! Local
    integer :: i
    real(kind=4) :: e(2, 2), f_mat(2, 2), a(2, 2), d(2), st(2, 2), g(2, 2)
    real(kind=4) :: cour, crmax, crmin, di, dim1, dip1, dkda, ei, ei1, eia, ter

    do i=1,ncomp
        u(i)=oldQ(i)/area(i)

        ! Nazmul: TODO, check if this equation is good for natural channel
        c(i)=sqrt(grav*area(i)/bo(i))

        !print*, sqrt(grav*area(i)/bo(i)), sqrt(grav*(depth(i)))

        ! This is the matrix L (left eigenvector matrix - eq 13)
        e(1,1)=1.0
        if(abs(u(i) - c(i)) < TOLERANCE) then
            c(i)=c(i)+0.00001
        end if
        e(1,2)=-1.0/(u(i)-c(i))
        e(2,1)=1.0
        e(2,2)=-1.0/(u(i)+c(i))

        ! L^{-1} (inverse of Left eigenvector matrix)
        f_mat(1,1)=-(u(i)-c(i))/(2.0*c(i))
        f_mat(1,2)=(u(i)+c(i))/(2.0*c(i))
        f_mat(2,1)=-(u(i)*u(i)-c(i)*c(i))/(2.0*c(i))
        f_mat(2,2)=(u(i)*u(i)-c(i)*c(i))/(2.0*c(i))

        ! Diagonal wave matrix D (eq 12)
        d(1)=abs(u(i)+c(i))
        d(2)=abs(u(i)-c(i))

        ! Equation 11 (L^{-1} D L)
        a(1,1)=e(1,1)*f_mat(1,1)*d(1)+e(2,1)*f_mat(1,2)*d(2)
        a(1,2)=e(1,2)*f_mat(1,1)*d(1)+e(2,2)*f_mat(1,2)*d(2)
        a(2,1)=e(1,1)*f_mat(2,1)*d(1)+e(2,1)*f_mat(2,2)*d(2)
        a(2,2)=e(1,2)*f_mat(2,1)*d(1)+e(2,2)*f_mat(2,2)*d(2)

        if(ots == 1) then
           dt(i)=cfl*dx(i-1)/max(d(1),d(2))
        else
           dt(i)=dtini
        endif

        ! Calculating dK/dA (eq 15)
        !ter=bo(i)+2.0*area(i)/bo(i)
        !Nazmul
        ter=pere(i)

        !Nazmul: dkda is corrected according to October 1, 2019 meeting at NWC
        dkda=sk(i)*((5.0/3.0*area(i)**(2.0/3.0)*ter)     &
                   -(2.0/3.0*area(i)**(5.0/3.0)*dpda(i)))/(ter**(5.0/3.0))

        ! Matrix S (eq 14)
        st(1, 1)=0.0
        st(1, 2)=0.0 !CHANGE
        !st(1, 2)=1.0
        st(2, 1)=grav*area(i)/bo(i)/bo(i)*dbdx(i)+gso(i)      &
                +f*2.0*grav*area(i)*oldQ(i)*abs(oldQ(i))/co(i)**3.0*dkda
                !+f*2.0*grav*area(i)*q(n,i)*abs(q(n,i))/co(i)**3.0*dkda

        !Nazmul: st(2,2) term is multiplied with a gravity
        !st(2,2)=-2*f*q(n,i)*grav*area(i)/co(i)/co(i)
        st(2,2)=-2*f*oldQ(i)*grav*area(i)/co(i)/co(i)

        if(dx(i) < TOLERANCE) then
            cour=dt(i)
        else
            cour=dt(i)/dx(i)
            crmax=max(crmax,cour*max(d(1),d(2)))
            crmin=min(crmin,cour*max(d(1),d(2)))
        endif

        ! LHS of eq 7 - this is done after the next check in matrixc
        b11(i)=0.5-phi-theta*cour*a(1,1)-0.5*thes*st(1,1)*dt(i)
        b12(i)=-theta*cour*a(1,2)-0.5*thes*st(1,2)*dt(i)
        b21(i)=-theta*cour*a(2,1)-0.5*thes*st(2,1)*dt(i)
        b22(i)=0.5-phi-theta*cour*a(2,2)-0.5*thes*st(2,2)*dt(i)

        ! This is switched with the dx check about in matrixc...
        if(i == 1) then
            cour=dt(i)
        else if(dx(i-1) < TOLERANCE) then
            cour=dt(i)
        else
            cour=dt(i)/dx(i-1)
        endif

        g(1,1)=0.5+phi+theta*cour*a(1,1)-0.5*thes*st(1,1)*dt(i)
        g(1,2)=theta*cour*a(1,2)-0.5*thes*st(1,2)*dt(i)
        g(2,1)=theta*cour*a(2,1)-0.5*thes*st(2,1)*dt(i)
        g(2,2)=0.5+phi+theta*cour*a(2,2)-0.5*thes*st(2,2)*dt(i)

        g11inv(i)= g(2,2)/(g(1,1)*g(2,2)-g(1,2)*g(2,1))
        g12inv(i)=-g(1,2)/(g(1,1)*g(2,2)-g(1,2)*g(2,1))
        g21inv(i)=-g(2,1)/(g(1,1)*g(2,2)-g(1,2)*g(2,1))
        g22inv(i)= g(1,1)/(g(1,1)*g(2,2)-g(1,2)*g(2,1))

        !f1(i)=q(n,i)
        f1(i)=oldQ(i)
        !print *, ncomp, i, area(i)
        !f2(i)=q(n,i)*q(n,i)/area(i)+grav*ci1(i)
        f2(i)=oldQ(i)*oldQ(i)/area(i)+grav*ci1(i)

        if(i >= 2 .and. i < ncomp) then
            dip1=area(i+1)/bo(i+1)
!           Nazmul: WHAT IS di? Need to check how to change it for natural channel
            di=2*area(i)/bo(i)
            dim1=area(i-1)/bo(i-1)
            ! to do: CHECK LOOP
            if (abs(dip1 + di + dim1) < TOLERANCE) then
                eps2(i) = 0.0
            else
                eps2(i)=alfa2*abs(dip1-di+dim1)/(dip1+di+dim1)
            endif
        endif

        !if(i >= 2 .and. i < ncomp) then
        !    dip1=area(i+1)/bo(i+1)
        !    di=2*area(i)/bo(i)
        !    dim1=area(i-1)/bo(i-1)
        !    eps2(i)=alfa2*abs(dip1-di+dim1)/(dip1+di+dim1)
            ! print *, i,eps2(i),area(i),area(i+1),area(i-1)
        !endif
    end do

    eps2(1)=eps2(2)
    eps2(ncomp)=eps2(ncomp-1)

    do i=2,ncomp-1
        if(ityp(i).ne.1) then
            eps2(i)=eps2(i-1)
            eps2(i+1)=eps2(i+2)
        endif
    end do

    do i=2,ncomp-1
        eps2(i)=max(eps2(i+1),eps2(i))
        ! u(i+1)=(u(i+1)+u(i))/2.0
        ! c(i+1)=(c(i+1)+c(i))/2.0
        eps4(i)=max(0.,alfa4-eps2(i)/(u(i)+c(i)))
        ! print *, 'corr',i,eps2(i)
    end do
    d1(1)=0.0
    d2(1)=0.0
    d1(ncomp)=0.0
    d2(ncomp)=0.0

    do i=2,ncomp-1
        d(1)=abs(u(i)+c(i))
        d(2)=abs(u(i)-c(i))
        ei=max(d(1),d(2))
        d(1)=abs(u(i+1)+c(i+1))
        d(2)=abs(u(i+1)-c(i+1))
        ei1=max(d(1),d(2))
        eia=(ei+ei1)/2.0
        if(ityp(i) /= 1) then
            d1(i)=0.0
            d2(i)=0.0
        elseif(i.eq.2.or.i.eq.(ncomp-1)) then
            d1(i)=eps2(i)*eia*(area(i+1)-area(i))
            !d2(i)=eps2(i)*eia*(q(n,i+1)-q(n,i))
            d2(i)=eps2(i)*eia*(oldQ(i+1)-oldQ(i))
            ! print *, i,d1(i),d2(i),eps2(i),area(i+1),area(i)
        else
            d1(i)=eps2(i)*eia*(area(i+1)-area(i))-eps4(i)*(area(i+2)-3*area(i+1)+3*area(i)-area(i-1))
            !d2(i)=eps2(i)*eia*(q(n,i+1)-q(n,i))-eps4(i)*(q(n,i+2)-3*q(n,i+1)+3*q(n,i)-q(n,i-1))
            d2(i)=eps2(i)*eia*(oldQ(i+1)-oldQ(i))-eps4(i)*(oldQ(i+2)-3*oldQ(i+1)+3*oldQ(i)-oldQ(i-1))
        endif
        ! print *, i,d1(i),d2(i),eps2(i),area(i+1),area(i)
    end do
    !print*, 'eps2_pred', (eps2(i), i=1, ncomp)

end subroutine matrixp


MODULE aa_module

  use lattice_module, only: lattice
  use io_tools_module

  implicit none

  PRIVATE
  PUBLIC :: init_aa, read_aa
  PUBLIC :: construct_aa
  PUBLIC :: get_org_aa
  PUBLIC :: set_org_aa

  real(8),PUBLIC :: ax
  real(8),PUBLIC :: Va
  real(8),PUBLIC :: aa(3,3)

  real(8) :: ax_org
  real(8) :: aa_org(3,3)

CONTAINS

  SUBROUTINE init_aa( aa_obj )
    implicit none
    type(lattice),intent(INOUT) :: aa_obj
    ax = aa_obj%LatticeConstant
    aa = aa_obj%LatticeVector/ax
    call set_org_aa( ax, aa )
    call construct_aa
  END SUBROUTINE init_aa

  SUBROUTINE read_aa
    implicit none
    ax=0.0d0
    aa(:,:)=0.0d0
    aa(1,1)=1.0d0
    aa(2,2)=1.0d0
    aa(3,3)=1.0d0
    call IOTools_readReal8Keyword( "AX", ax )
    call IOTools_readReal8Keyword( "A1", aa(:,1) )
    call IOTools_readReal8Keyword( "A2", aa(:,2) )
    call IOTools_readReal8Keyword( "A3", aa(:,3) )
  END SUBROUTINE read_aa

  SUBROUTINE construct_aa
    implicit none
    aa(:,:)=ax*aa(:,:)
    Va = aa(1,1)*aa(2,2)*aa(3,3)+aa(1,2)*aa(2,3)*aa(3,1) &
        +aa(1,3)*aa(2,1)*aa(3,2)-aa(1,3)*aa(2,2)*aa(3,1) &
        -aa(1,2)*aa(2,1)*aa(3,3)-aa(1,1)*aa(2,3)*aa(3,2)
  END SUBROUTINE construct_aa

  SUBROUTINE set_org_aa(ax_in,aa_in)
    implicit none
    real(8),intent(IN) :: ax_in,aa_in(3,3)
    ax_org=ax_in
    aa_org(:,:)=aa_in(:,:)
  END SUBROUTINE set_org_aa

  SUBROUTINE get_org_aa(ax_out,aa_out)
    implicit none
    real(8),intent(OUT) :: ax_out,aa_out(3,3)
    ax_out=ax_org
    aa_out(:,:)=aa_org(:,:)
  END SUBROUTINE get_org_aa

END MODULE aa_module


!======================================================================
! The subroutines in this file are for generating the slip rate
! functions for each point source, and sum them together to get
! spectra of larget event
!
! Written by Pengcheng Liu
! Copyright (c) 2005 by Pengcheng Liu
!
!======================================================================
subroutine sum_point_svf(svf)
 use time_freq
 use sp_sub_f
 implicit NONE
 integer:: i,j,j0,j1,j2,jmm
 real:: fcor_s
 real, dimension(ntime):: svf,svf_s
 real, dimension(2*ntime):: mr
 do i=1,2*ntime
    mr(i)=0.0
 enddo
 do i=1,ntime
    svf(i)=0.0
 enddo

 do i=1,nsum
   fcor_s=1./rstm(i)
   call slip_rate(id_sf_type,fcor_s,svf_s)
   j0=int(rptm(i)/dt+0.5)
   j1=j0+1
   j2=int(rstm(i)/dt)+j1
   do j=j1,j2
     mr(j)=mr(j)+slip(i)*svf_s(j-j0)
   enddo
 enddo
 do i=1,ntime
    svf(i)=mr(i)
 enddo
end subroutine sum_point_svf
!
!======================================================================
!
subroutine slip_rate(id_sf_type,f_cor,svf)
 use time_freq
 implicit NONE
 integer:: id_sf_type
 real:: f_cor,rise_time
 real, dimension(ntime):: svf

 if(id_sf_type == 1) then
   rise_time=1.00/f_cor
   call svf_time_1(ntime,dt,rise_time,svf)
   f_cor=3.*rise_time
 else if(id_sf_type == 2) then
   rise_time=1.0/f_cor
   call svf_time_2(ntime,dt,rise_time,svf)
   f_cor=rise_time
 else if(id_sf_type == 3) then
   rise_time=1.0/f_cor
   call svf_time_3(ntime,dt,rise_time,cft1,svf)
   f_cor=rise_time
 else if(id_sf_type == 4) then
   rise_time=1.00/f_cor
   call svf_time_4(ntime,dt,rise_time,cft1,svf)
   f_cor=rise_time
 else if(id_sf_type == 5) then
   rise_time=1.00/f_cor
   call svf_time_5(ntime,dt,rise_time,cft1,svf)
   f_cor=rise_time
 else if(id_sf_type == 6) then
   rise_time=1.0/f_cor
   call svf_time_6(ntime,dt,rise_time,cft1,svf)
   f_cor=rise_time
 else if(id_sf_type == 7) then
   rise_time=1.7/f_cor
   call svf_time_7(ntime,dt,rise_time,svf)
   f_cor=rise_time
 else if(id_sf_type == 8) then
   rise_time=1.0/f_cor
   call svf_time_8(ntime,dt,rise_time,cft1,svf)
   f_cor=rise_time
 endif

end subroutine slip_rate
!
!======================================================================
!
subroutine peak_slip_rate(pksr)
 use time_freq
 use sp_sub_f
 implicit NONE
 real, dimension(nsum):: pksr
 integer:: i, ni
 real:: f_cor,rt,pi2,tt1,tt2,tm,amp
 pi2=8.*atan(1.0)
 if(id_sf_type == 1) then
   do i=1, nsum
     pksr(i)=slip(i)*exp(-1.0)*pi2/rstm(i)
   enddo
 else if(id_sf_type == 2) then
   do i=1, nsum
     tm=1.0/5.0
     pksr(i)=slip(i)*30.0*tm*(1.-tm)**4/rstm(i)
   enddo
 else if(id_sf_type == 3) then
   do i=1, nsum
     tt1=cft1*rstm(i)
     tt2=rstm(i)-tt1
     pksr(i)=slip(i)*pi2/(4.*tt1+0.5*pi2*tt2)
   enddo
 else if(id_sf_type == 4) then
   do i=1, nsum
     tt1=cft1*rstm(i)
     tt2=rstm(i)-tt1
     pksr(i)=slip(i)*pi2/(0.5*pi2*tt1+0.5*(4.*tt1+0.5*pi2*tt2))
   enddo
 else if(id_sf_type == 5) then
   do i=1, nsum
     tt1=cft1*rstm(i)
     tt2=rstm(i)-tt1
     pksr(i)=slip(i)/(4.*tt1/pi2+0.25*rstm(i))
   enddo
 else if(id_sf_type == 6) then
   do i=1, nsum
     tt1=cft1
     tt2=rstm(i)
     pksr(i)=slip(i)/(4.*tt1/pi2+0.5*tt2)
   enddo
 else
   do i=1, nsum
     pksr(i)=2.*slip(i)/rstm(i)
   enddo
 endif

end subroutine peak_slip_rate
!
!======================================================================
!
subroutine svf_time_1(nt,dt,rise_time,svf)
 implicit NONE
 real, parameter:: pi=3.1415926, pi2=2.*pi
 integer:: nt,np1,i
 real:: wc,ts,sum1,svi,dt,rise_time
 real,dimension(nt):: svf

 svf=0.0
 wc=pi2/rise_time
 np1=3*int(rise_time/dt+1.0)
 sum1=0.0
 do i=1,np1
   ts=(i-1)*dt
   svi=wc*wc*ts*exp(-wc*ts)
   svf(i)=svi
   sum1=sum1+svi
 enddo
 sum1=sum1*dt
 do i=1,np1
   svf(i)=svf(i)/sum1
 enddo

end subroutine svf_time_1
!
!======================================================================
!
subroutine svf_time_2(nt,dt,rise_time,svf)
 implicit none
 integer:: nd,nt,np,i
 real:: dt,rise_time,ts,cof
 real,dimension(nt):: svf

 nd=4
 np=int(rise_time/dt)+1
 cof=(nd+1.0)*(nd+2.0)
 do i=1,np
   ts=(i-1.)*dt/rise_time
   svf(i)=cof*ts*(1.-ts)**nd/rise_time
 enddo
 svf(np+1)=0.0
 return
end subroutine svf_time_2
!
!======================================================================
!
subroutine svf_time_3(nt,dt,rise_time,cft1,svf)
 implicit NONE
 real, parameter:: pi=3.1415926,pi2=2.*pi
 integer:: nt,n1,i
 real:: cft1,dt,rise_time,t21,t22,coef_norm,cf1,cf2,ts
 real,dimension(nt):: svf

 t21=cft1*rise_time
 t22=rise_time-t21
 coef_norm=pi2/(4.*t21+pi*t22)
 n1=int(rise_time/dt)+1
 do i=1,n1
   ts=(i-1.)*dt
   if(ts < t21) then
     svf(i)=coef_norm*sin(0.5*pi*ts/t21)
   else
     svf(i)=coef_norm*0.5*(1.+cos(pi*(ts-t21)/t22))
   endif
 enddo
 svf(n1+1)=0.0

end subroutine svf_time_3
!
!======================================================================
!
subroutine svf_time_4(nt,dt,rise_time,cft1,svf)
 implicit NONE
 real, parameter:: cs1=0.50,cs2=1.-cs1
 real, parameter:: pi=3.1415926, pi2=2.*pi
 integer:: nt,n1,i
 real:: cft1,dt,rise_time,tt1,tt2,coef_norm,cf1,cf2,ts
 real,dimension(nt):: svf
 tt1=cft1*rise_time
 tt2=rise_time-tt1
 coef_norm=pi2/(cs1*2.*pi*tt1+cs2*4.*tt1+cs2*pi*tt2)
 cf2=cs2*coef_norm
 cf1=cs1*coef_norm
 n1=int(rise_time/dt)+1
 do i=1,n1
   ts=(i-1)*dt
   if(ts < tt1) then
     svf(i)=cf2*sin(0.5*pi*ts/tt1)
   else
     svf(i)=cf2*0.5*(1+cos(pi*(ts-tt1)/tt2))
   endif
   if(ts < 2*tt1) then
     svf(i)=svf(i)+cf1*0.5*(1.-cos(pi*ts/tt1))
   endif
 enddo
 svf(n1+1)=0.0

end subroutine svf_time_4
!
!======================================================================
!
subroutine svf_time_5(nt,dt,rise_time,cft1,svf)
 implicit NONE
 real, parameter:: cs1=0.50,cs2=1.-cs1
 real, parameter:: pi=3.1415926,pi2=2.*pi,pid2=0.5*pi
 integer:: nt,n1,i
 real:: cft1,dt,rise_time,t11,t12,t21,t22,coef_norm,cf1,cf2,ts
 real,dimension(nt):: svf

 t11=cft1*rise_time
 t12=1.0*t11
 t21=1.0*t11
 t22=rise_time-t21
 coef_norm=pi2/(cs1*(4.*t11+pi*t12)+cs2*(4.*t21+pi*t22))
 cf1=cs1*coef_norm
 cf2=cs2*coef_norm
 n1=int(rise_time/dt)+1
 do i=1,n1
   ts=(i-1.)*dt
   if(ts < t11) then
     svf(i)=cf1*sin(pid2*ts/t11)
   elseif(ts < t11+t12) then
     svf(i)=cf1*0.5*(1.+cos(pi*(ts-t11)/t12))
   else
     svf(i)=0.0
   endif
   if(ts < t21) then
     svf(i)=svf(i)+cf2*sin(0.5*pi*ts/t21)
   else
     svf(i)=svf(i)+cf2*0.5*(1.+cos(pi*(ts-t21)/t22))
   endif
 enddo
 svf(n1+1)=0.0

end subroutine svf_time_5
!
!======================================================================
!
subroutine svf_time_6(nt,dt,rise_time,cft1,svf)
 implicit NONE
 real, parameter:: pi=3.1415926,pi2=2.*pi
 integer:: nt,n1,i
 real:: cft1,dt,rise_time,cs1,cs2,pid2,t21,t22,coef_norm,cf1,cf2,ts
 real,dimension(nt):: svf

 t21=cft1*rise_time
 t22=rise_time-t21
 coef_norm=pi2/(4.*t21+pi*t22)
 n1=int(rise_time/dt)+1
 do i=1,n1
   ts=(i-1.)*dt
   if(ts < t21) then
     svf(i)=coef_norm*sin(0.5*pi*ts/t21)
   else
     svf(i)=coef_norm*0.5*(1.+cos(pi*(ts-t21)/t22))
   endif
 enddo
 svf(n1+1)=0.0

end subroutine svf_time_6
!
!======================================================================
!
subroutine svf_time_7(nt,dt,rise_time,svf)
 implicit NONE
 integer, parameter:: Nv=4
 real, parameter::  ra=1.0
 integer:: nt,np,n2,i,j,k
 real:: dt,rise_time,aa,ai,tk,t0,t1,tt,cof
 real, dimension(nt):: svf

 np=int(rise_time/dt)+2
 do i=1,np
   svf(i)=0.0
 enddo
 aa=0.0
 do i=1,Nv
   aa=aa+ra**(i-1)
 enddo

 t1= rise_time/2**(Nv-1)
 do k=1,Nv
   ai=ra**(k-1)
   tk=t1*2**(k-1)
   t0=0.5*tk
   n2=int(tk/dt)+1
   cof=4./tk/tk/aa*ai
   do j=1,n2
     tt=(j-1.)*dt
     if(tt < t0) then
       svf(j)=svf(j)+cof*tt
     else
       svf(j)=svf(j)+cof*(tk-tt)
     endif
   enddo
 enddo

end subroutine svf_time_7
!
!======================================================================
!
subroutine svf_time_8(nt,dt,rise_time,cft1,svf)
 implicit NONE
 real, parameter:: pi=3.1415926, pi2=2.*pi
 integer:: nt,np0,np1,np2,i
 real:: cft1,ts,sum1,svi,dt,rise_time,Tp,Te,Tr,psv
 real,dimension(nt):: svf
 integer a_seed
 !Tp=cft1*rise_time
 call random_seed(a_seed)
 CALL random_number(Tp)
 Tp=0.1*Tp+0.15
 Te=0.8*rise_time
 Tr=rise_time
 if(Tr<Tp) Tp=cft1*Tr
 !svf=0.0
 np0=int(Tp/dt+1.0)
 np1=int(Te/dt+1.0)
 np2=int(Tr/dt+1.0)

 psv=sqrt(1.+100./(np0*dt))

 sum1=0.0
 do i=1,np0
    ts=(i-1)*dt
    svi=ts*psv/Tp*sin(0.5*pi/Tp*ts)
    svf(i)=svi
    sum1=sum1+svi
 enddo
 do i=np0+1,np1
    ts=(i-1)*dt
    svi=sqrt(1.+100./ts)
    svf(i)=svi
    sum1=sum1+svi
 enddo
 do i=np1+1,np2
    ts=(i-1)*dt
    svi=sqrt(1.+100./ts)*sin((np2-i)*dt*pi*0.5/(Tr-Te))
    svf(i)=svi
    sum1=sum1+svi
 enddo
 sum1=sum1*dt
 do i=1,np2
   svf(i)=svf(i)/sum1
 enddo
end subroutine svf_time_8


!----------------------Function----------------------------------------!
!
      SUBROUTINE SPLINX( ITS,ITE )
!
!-------------------------Disclaimer-----------------------------------!
!
!     This material was prepared as an account of work sponsored by
!     an agency of the United States Government. Neither the
!     United States Government nor the United States Department of
!     Energy, nor Battelle, nor any of their employees, makes any
!     warranty, express or implied, or assumes any legal liability or
!     responsibility for the accuracy, completeness, or usefulness
!     of any information, apparatus, product, software or process
!     disclosed, or represents that its use would not infringe
!     privately owned rights.
!
!----------------------Acknowledgement---------------------------------!
!
!     This software and its documentation were produced with Government
!     support under Contract Number DE-AC06-76RLO-1830 awarded by the
!     United Department of Energy. The Government retains a paid-up
!     non-exclusive, irrevocable worldwide license to reproduce,
!     prepare derivative works, perform publicly and display publicly
!     by or for the Government, including the right to distribute to
!     other Government contractors.
!
!---------------------Copyright Notices--------------------------------!
!
!            Copyright Battelle Memorial Institute, 1996
!                    All Rights Reserved.
!
!----------------------Description-------------------------------------!
!
!     Given arrays TBLY and TBLX containing a tabulated function
!     (i.e., x(n) = f(y(n))), with y(1) < y(2) < ... < y(n), this
!     routine returns an array TBLDDX which contains the second
!     derivatives of the interpolating function at the tabulated
!     points y(i).  The routine is structured to set the boundary
!     conditions at function end points to a natural spline, with zero
!     second derivatives on that boundary.
!
!----------------------Authors-----------------------------------------!
!
!     Written by MD White, Battelle, PNL, September, 1994.
!     Last Modified by MD White, Battelle, PNL, September 9, 1994.
!     $Id: splinx.F 1080 2017-03-14 16:22:02Z d3c002 $
!

!----------------------Fortran 90 Modules------------------------------!
!
      USE GLB_PAR
      USE TABL
      USE SOLTN
!

!----------------------Implicit Double Precision-----------------------!
!
      IMPLICIT REAL*8 (A-H,O-Z)
      IMPLICIT INTEGER (I-N)
!
!----------------------Parameter Statements----------------------------!
!

!
!----------------------Common Blocks-----------------------------------!
!

!
!----------------------Type Declarations-------------------------------!
!
      REAL*8 TBLU(LTBL)
!
!----------------------Executable Lines--------------------------------!
!
      ISUB_LOG = ISUB_LOG+1
      SUB_LOG(ISUB_LOG) = '/SPLINX'
      IF( INDEX(SVN_ID(198)(1:1),'$').EQ.0 ) SVN_ID(198) =
     & '$Id: splinx.F 1080 2017-03-14 16:22:02Z d3c002 $'
!
!---  Ascending table order  ---
!
      IF( TBLY(ITE).GT.TBLY(ITS) ) THEN
        TBLD = (TBLX(ITS+1)-TBLX(ITS))/(TBLY(ITS+1)-TBLY(ITS))
!
!---  The lower boundary condition is set either to be natural or
!     else to have a specified first derivative.  ---
!
!        IF( TBLD.GT.0.99D+20 ) THEN
          TBLDDX(ITS) = 0.D+0
          TBLU(ITS) = 0.D+0
!        ELSE
!          TBLDDX(ITS) = -0.5D+0
!          TBLU(ITS) = (3.D+0/(TBLY(ITS+1)-TBLY(ITS)))*
!     &      ((TBLX(ITS+1)-TBLX(ITS))/(TBLY(ITS+1)-TBLY(ITS))-TBLD)
!        ENDIF
!
!---  This is the decomposition loop of the tridiagonal algorithm.
!     TBLDDX and TBLU are used as temporary storage of the decomposed
!     factors.  ---
!
        DO 11 I = ITS+1,ITE-1
          SIG = (TBLY(I)-TBLY(I-1))/(TBLY(I+1)-TBLY(I-1))
          P = SIG*TBLDDX(I-1)+2.D+0
          TBLDDX(I) = (SIG-1.D+0)/P
          TBLU(I) = (6.D+0*((TBLX(I+1)-TBLX(I))/(TBLY(I+1)-TBLY(I)) -
     &      (TBLX(I)-TBLX(I-1))/(TBLY(I)-TBLY(I-1)))/
     &      (TBLY(I+1)-TBLY(I-1)) - SIG*TBLU(I-1))/P
   11   CONTINUE
        TBLD = (TBLX(ITE)-TBLX(ITE-1))/(TBLY(ITE)-TBLY(ITE-1))
!
!---  The upper boundary condition is set either to be natural or
!     else to have a specified first derivative.  ---
!
!        IF( TBLD.GT.0.99D+20 ) THEN
          TBLDDX(ITE) = 0.D+0
          TBLU(ITE) = 0.D+0
!        ELSE
!          TBLDDX(ITE) = 0.5D+0
!          TBLU(ITE) = (3.D+0/(TBLY(ITE)-TBLY(ITE-1)))*
!     &      (TBLD-(TBLX(ITE)-TBLX(ITE-1))/(TBLY(ITE)-TBLY(ITE-1)))
!        ENDIF
!
!---  This is the backsubstitution loop of the tridiagonal algorthim
!
        TBLDDX(ITE) = (TBLU(ITE)-TBLDDX(ITE)*TBLU(ITE-1))/
     &    (TBLDDX(ITE)*TBLDDX(ITE-1)+1.D+0)
        DO 12 K = ITE-1,ITS,-1
          TBLDDX(K) = TBLDDX(K)*TBLDDX(K+1)+TBLU(K)
   12   CONTINUE
!
!---  Descending table order  ---
!
      ELSEIF( TBLY(ITE).LT.TBLY(ITS) ) THEN
        TBLD = (TBLX(ITE-1)-TBLX(ITE))/(TBLY(ITE-1)-TBLY(ITE))
!
!---  The lower boundary condition is set either to be natural or
!     else to have a specified first derivative.  ---
!
!        IF( TBLD.GT.0.99D+20 ) THEN
          TBLDDX(ITE) = 0.D+0
          TBLU(ITE) = 0.D+0
!        ELSE
!          TBLDDX(ITE) = -0.5D+0
!          TBLU(ITE) = (3.D+0/(TBLY(ITE-1)-TBLY(ITE)))*
!     &      ((TBLX(ITE-1)-TBLX(ITE))/(TBLY(ITE-1)-TBLY(ITE))-TBLD)
!        ENDIF
!
!---  This is the decomposition loop of the tridiagonal algorithm.
!     TBLDDX and TBLU are used as temporary storage of the decomposed
!     factors.  ---
!
        DO 21 I = ITE-1,ITS+1,-1
          SIG = (TBLY(I)-TBLY(I+1))/(TBLY(I-1)-TBLY(I+1))
          P = SIG*TBLDDX(I+1)+2.D+0
          TBLDDX(I) = (SIG-1.D+0)/P
          TBLU(I) = (6.D+0*((TBLX(I-1)-TBLX(I))/(TBLY(I-1)-TBLY(I)) -
     &      (TBLX(I)-TBLX(I+1))/(TBLY(I)-TBLY(I+1)))/
     &      (TBLY(I-1)-TBLY(I+1)) - SIG*TBLU(I+1))/P
   21   CONTINUE
        TBLD = (TBLX(ITS)-TBLX(ITS+1))/(TBLY(ITS)-TBLY(ITS+1))
!
!---  The upper boundary condition is set either to be natural or
!     else to have a specified first derivative.  ---
!
!        IF( TBLD.GT.0.99D+20 ) THEN
          TBLDDX(ITS) = 0.D+0
          TBLU(ITS) = 0.D+0
!        ELSE
!          TBLDDX(ITS) = 0.5D+0
!          TBLU(ITS) = (3.D+0/(TBLY(ITS)-TBLY(ITS+1)))*
!     &      (TBLD-(TBLX(ITS)-TBLX(ITS+1))/(TBLY(ITS)-TBLY(ITS+1)))
!        ENDIF
!
!---  This is the backsubstitution loop of the tridiagonal algorthim
!
        TBLDDX(ITS) = (TBLU(ITS)-TBLDDX(ITS)*TBLU(ITS+1))/
     &    (TBLDDX(ITS)*TBLDDX(ITS+1)+1.D+0)
        DO 22 K = ITS+1,ITE
          TBLDDX(K) = TBLDDX(K)*TBLDDX(K-1)+TBLU(K)
   22   CONTINUE
      ELSE
        INDX = 3
        CHMSG = 'Invalid Table'
        CALL WRMSGS( INDX )
      ENDIF
      ISUB_LOG = ISUB_LOG-1
!
!---  End of SPLINX group
!
      RETURN
      END


program filter_input_trace

! This program is used to filter signals with a Butterworth bandpass filter.
! It has been initially written by Vadim Monteiller and has been adapted by Alexis Bottero.
! To compile: ifort filter.f90 -o filter
!
! usage: filter [-h] [-v] [--norder FILTER_ORDER] [--output PATH_TO_FILTERED_SIGNAL]
!               PATH_TO_SIGNALS -f1 F1 -f2 F2
!
! Positional arguments :
!   PATH_TO_SIGNALS                  Paths to signal to filter (two columns file time,amplitude)
!   -f1 F1                           Inferior cutoff frequency
!   -f2 F2                           Superior cutoff frequency
!
! Optional arguments :
!   -h, --help                       Show this help message and exit
!   --norder FILTER_ORDER            Order of the filter (default 4)
!   --output PATH_TO_FILTERED_SIGNAL If one signal given: path where we store the filtered
!                                    signal (default PATH_TO_SIGNAL+Filt)
!   -v, --verbose                    Verbose mode

  implicit none

  !======= Variables for argument parsing ======!
  integer :: narg,cptArg ! number of arguments given by user & counter of arguments
  character(len=100) :: name,f1str,f2str,norderstr ! To store argument as character strings
  logical :: lookForF1 = .false.
  logical :: lookForF2 = .false.
  logical :: lookForNorder = .false.
  logical :: lookForOutput = .false.
  logical :: fileExist
  logical :: verbose = .false.

  !======= Local variables ======!
  character(len=100), dimension(:), allocatable :: filesToFilter,output_files
  real(kind=4), dimension(:), allocatable :: input_tr1,input_tr2, output_tr1,output_tr2

  !=============== Default parameters ===============!
  character(len=100) :: output_file_name = "default"
  real(kind=4) :: f1 = -9999
  real(kind=4) :: f2 = -9999
  real(kind=4) :: dt = -9999
  integer      :: norder = 4
  integer      :: nFiles = 0

  !======= Local variables ======!
  integer      :: nstep ! To store the number of lines in file
  integer      :: i = 1
  integer      :: ifile = 1
  integer      :: irek
  real(kind=4) :: x

  !=============== Help message ===============!
  200 format(/5x,'This program is used to filter signals with a Butterworth bandpass filter.',/5x, &
       'It has been initially written by Vadim Monteiller and has been adapted by Alexis Bottero.',/5x, &
       '',/5x, &
       'usage: filter [-h] [-v] [--norder FILTER_ORDER] [--output PATH_TO_FILTERED_SIGNAL]',/5x, &
       '              PATH_TO_SIGNALS -f1 F1 -f2 F2',/5x, &
       '',/5x, &
       'Positional arguments :',/5x, &
       '  PATH_TO_SIGNALS                  Paths to signal to filter (two columns file time,amplitude)',/5x, &
       '  -f1 F1                           Inferior cutoff frequency',/5x, &
       '  -f2 F2                           Superior cutoff frequency',/5x, &
       '',/5x, &
       'Optional arguments :',/5x, &
       '  -h, --help                       Show this help message and exit',/5x, &
       '  --norder FILTER_ORDER            Order of the filter (default 4)',/5x, &
       '  --output PATH_TO_FILTERED_SIGNAL If one signal given: path where we store the filtered signal',/5x, &
       '                                   (default PATH_TO_SIGNAL+Filt)',/5x, &
       '  -v, --verbose                    Verbose mode')

  !=============== Parse arguments ===============!
  ! Check if any arguments are found
  narg = command_argument_count()
  ! Loop over the arguments
  if (narg > 0) then
    ! Loop across options
    do cptArg = 1,narg
      call get_command_argument(cptArg,name)
      select case(adjustl(name))
        case("--help","-h")
          write(*,200)
          stop
        case("-f1")
          lookForF1 = .true.
        case("-f2")
          lookForF2 = .true.
        case("--norder")
          lookForNorder = .true.
        case("--output")
          lookForOutput = .true.
        case("--verbose","-v")
          verbose = .true.
        case default
          if (lookForF1) then
            f1str = adjustl(name)
            read(f1str,*) f1
            lookForF1 = .false.
          else if (lookForF2) then
            f2str = adjustl(name)
            read(f2str,*) f2
            lookForF2 = .false.
          else if (lookForNorder) then
            norderstr = adjustl(name)
            read(norderstr,*) norder
            lookForNorder = .false.
          else if (lookForOutput) then
            output_file_name = adjustl(name)
            lookForOutput = .false.
          else
            nFiles = nFiles + 1
          endif
      end select
    enddo

    if (nFiles > 0) then
      ! Allocate the array to store file names
      allocate(filesToFilter(nFiles),output_files(nFiles))
    else
      write(*,*) "At least one file must be given... Nothing has been done!"
      write(*,200)
      stop
    endif

    ! Second loop across options to store files
    do cptArg = 1,narg
      call get_command_argument(cptArg,name)
      select case(adjustl(name))
        case("-f1")
          lookForF1 = .true.
        case("-f2")
          lookForF2 = .true.
        case("--norder")
          lookForNorder = .true.
        case("--output")
          lookForOutput = .true.
        case("--verbose","-v")
          verbose = .true.
        case default
          if (lookForF1) then
            lookForF1 = .false.
          else if (lookForF2) then
            lookForF2 = .false.
          else if (lookForNorder) then
            lookForNorder = .false.
          else if (lookForOutput) then
            lookForOutput = .false.
          else
            filesToFilter(i) = adjustl(trim(name)) ! assign a value to filesToFilter(i)
            inquire(file=filesToFilter(i),exist=fileExist) ! check if the file exists
            if (.not. fileExist) then
              write(*,*)'File ',adjustl(trim(filesToFilter(i))),' not found.'
              write(*,*) "Nothing has been done!"
              stop
            endif
            i = i + 1
          endif
      end select
    enddo
  endif

  if (f1 == -9999 .or. f2 == -9999) then
    write(*,*) "Options -f1 and -f2 must be given... Nothing has been done!"
    write(*,200)
    stop
  endif

  if (output_file_name == "default") then
    do ifile = 1,nFiles
      output_files(ifile) = adjustl(trim(filesToFilter(ifile))) // "Filt"
    enddo
  else if (nFiles == 1) then
    output_files(1) = adjustl(trim(output_file_name))
  else
    write(*,*) "Option --output can only be used if just one file is suppose to be filtered"
    write(*,*) "Nothing has been done!"
    stop
  endif

  if (verbose) then
    if (nFiles == 1) then
      write(*,"(A,A)") "Name of the file containing the trace to filter: ",adjustl(trim(filesToFilter(1)))
      write(*,"(A,A)") "The filtered signal will be written in : ",adjustl(trim(output_files(1)))
    else
      write(*,"(A)") "Name of the files containing the trace to filter:"
      do ifile = 1,nFiles
        write(*,"(A,A)") "  ",adjustl(trim(filesToFilter(ifile)))
      enddo
      write(*,*)
      write(*,"(A)") "The filtered signals will be written in :"
      do ifile = 1,nFiles
        write(*,"(A,A)") "  ",adjustl(trim(output_files(ifile)))
      enddo
    endif
    write(*,*)
    write(*,"(A,F10.2,A)") "f1 : ",f1," Hz"
    write(*,"(A,F10.2,A)") "f2 : ",f2," Hz"
    write(*,"(A,I3)") "Filter order : ",norder
    write(*,*)
  endif

  do ifile = 1,nFiles
    ! Open file containing signal to filter
    open(10,file=filesToFilter(ifile))

    ! Count the number of values in file
    nstep = 0
    do
      read(10,*,end=99) x
      nstep = nstep + 1
    enddo
99  close(10)

    allocate(input_tr1(nstep),output_tr1(nstep),input_tr2(nstep),output_tr2(nstep))
    output_tr2(:) = 0.

    ! Read input trace
    open(10,file=filesToFilter(ifile))
    do i = 1,nstep
      read(10,*) input_tr1(i),input_tr2(i)
    enddo
    close(10)

    dt = input_tr1(2) - input_tr1(1)
    !if (verbose) then
    !  write(*,"(A,F5.2)") "dt :",dt
    !endif
    output_tr1(:) = input_tr1(:)

    irek = 1
    call bwfilt (input_tr2, output_tr2, dt, nstep, irek, norder, f1, f2)

    open(10,file=output_files(ifile))

    do i = 1,nstep
      write(10,*) output_tr1(i),output_tr2(i)
    enddo
    close(10)

    write(*,"(A,A,A)") "File ",adjustl(trim(output_files(ifile)))," has been written."
    deallocate(input_tr1,output_tr1,input_tr2,output_tr2)
  enddo

  deallocate(filesToFilter,output_files)

end program filter_input_trace

!#############################################################
subroutine bwfilt (x, y, dt, n, irek, norder, f1, f2)

  ! recursive filtering of data with butterworth filter
  ! x: input array
  ! y: output array
  ! dt: time increment
  ! n: number of data points

  ! irek=0: forward filtering only
  ! irek=1: forward and backward filtering

  ! norder: order of butterworth filter
  ! norder=0: only filtering, no determination of coefficients
  ! norder < 0: no starplots of transfer function and impulse response

  ! f1: low cutoff frequency (Hz)
  ! f1=0: low pass filter

  ! f2: high cutoff frequency (Hz)
  ! f2>0.5/dt: high pass filter

  implicit none

  real(kind=4), dimension(1)::x,y
  real(kind=4), dimension (10) ::  a, b1, b2
  real(kind=4) :: dt,f1,f2
  integer :: iunit, npoles,norder,irek,n,lx
  !real(kind(0d0)) :: x(n),y(n)

   iunit = 3

   if (norder /= 0) then
      npoles=iabs(norder)
      !determination of filter coefficients
      call bpcoeff(f1,f2,npoles, dt, a,b1, b2)
      if (norder >= 0) then
         !plot of transfer function and impuulse response
         lx = 100
         !filtering
      endif
   endif

   if (n /= 0) then
      call rekurs(x,y,n,a,b1,b2,npoles,irek)
   endif
   return
 end subroutine bwfilt

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

subroutine rekurs(x,y,ndat,a,b1,b2,npoles,iflag)
  ! performs recursive filtering of data in array x of length ndat
  ! filtered output in y
  ! a, b1, b2 are the filtercoefficients previously determined in bwcoef
  ! npoles is the number of poles
  ! iflag=0: forward filtering only
  ! iflag /= 0: forward and backward filtering

  implicit none

  real(kind=4), dimension(10) :: z,z1,z2 ,a,b1,b2
  real(kind=4) ::  x1,x2
  integer :: ndat, npoles, iflag, n,i
  real(kind=4) :: x(ndat), y(ndat)

  !forward

  x1 = 0.d0
  x2 = 0.d0

  do i = 1, npoles
     z1(i) = 0.d0
     z2(i) = 0.d0
  enddo

  do n = 1, ndat
     z(1) = a(1)*(x(n)-x2) -b1(1)*z1(1) -b2(1)*z2(1)
     do i = 2, npoles
        z(i) = a(i)*(z(i-1)-z2(i-1))-b1(i)*z1(i)-b2(i)*z2(i)
     enddo
     x2=x1
     x1=x(n)
     do i = 1, npoles
        z2(i) =z1(i)
        z1(i) =z(i)
     enddo
     y(n) = z(npoles)
  enddo

  if (iflag == 0) then
     return
  endif

  !backward

  x1 =0.d0
  x2 =0.d0

  do i = 1, npoles
     z1(i) = 0.d0
     z2(i) = 0.d0
  enddo

  do n = ndat, 1, -1
     z(1) = a(1)*(y(n)-x2)-b1(1)*z1(1)-b2(1)*z2(1)
     do i =2, npoles
        z(i) = a(i)*(z(i-1)-z2(i-1))-b1(i)*z1(i)-b2(i)*z2(i)
     enddo
     x2=x1
     x1=y(n)
     do i = 1,npoles
        z2(i)=z1(i)
        z1(i)=z(i)
     enddo
     y(n) = z(npoles)
  enddo
  return
end subroutine rekurs

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

subroutine bpcoeff(f1,f2,npoles,dt,a,b1,b2)
  !determines filtercoefficients for recursive bandpassfilter

  real(kind=4),dimension(10) :: a,b1,b2
  complex(kind=4) :: s(20), t1,t2,p
  real(kind=4), parameter :: pi = 3.141592653589793d0
  real(kind=4) :: f1,f2,dt,d2,w0,w1,w2,ssum, sprod,fact1,fact2,fact3
  integer :: i,npol2,n,npoles

  if (npoles > 10) then
     stop ' npoles greater than 10: STOP '
  endif

  d2= 2.d0/dt
  w1=d2*tan(2.d0*pi*f1/d2)
  w2=d2*tan(2.d0*pi*f2/d2)
  w0=0.5*(w2-w1)

  i=1
  npol2=npoles/2+1
  do n =1,npoles
     p = cexp(cmplx(0.d0,dble(2*n-1+npoles)*pi/dble(2*npoles)))
     t1 = p*cmplx(w0,0.d0)
     t2 = sqrt(t1*t1-cmplx(w1*w2,0.d0))
     s(i)=t1+t2
     s(i+1)=t1-t2
     i=i+2
  enddo

  do n=1,npoles
     ssum=2*real(s(n))
     sprod=dble(s(n)*conjg(s(n)))
     fact1=d2*d2-d2*ssum+sprod
     fact2=2.d0*(sprod-d2*d2)
     fact3=d2*d2+d2*ssum+sprod
     a(n)=2.d0*d2*w0/fact1
     b1(n)=fact2/fact1
     b2(n)=fact3/fact1
  enddo
  return
end subroutine bpcoeff

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


! ##################################################################################################################################

      MODULE LAPACK_SYM_MAT_INV

      USE PENTIUM_II_KIND, ONLY          :  BYTE, LONG, DOUBLE
      USE IOUNT1, ONLY                   :  F04, WRT_LOG
      USE SCONTR, ONLY                   :  BLNK_SUB_NAM
      USE TIMDAT, ONLY                   :  HOUR, MINUTE, SEC,
     &                                      TSEC
      USE SUBR_BEGEND_LEVELS, ONLY       :  LAPACK_BEGEND

      USE LAPACK_BLAS_AUX
      USE LAPACK_LIN_EQN_DPB                               ! Subr DPOTF2

      USE OURTIM_Interface

      INTEGER(LONG), PARAMETER, PRIVATE :: SUBR_BEGEND = LAPACK_BEGEND

! This is a set of LAPACK routines that are used in inverting symmetric matrices (not band matrices)

      CONTAINS

! ##################################################################################################################################
! 001 LAPACK_SYM_MAT_INV

      SUBROUTINE DPOTRF( UPLO, N, A, LDA, INFO )
*
*  -- LAPACK routine (version 3.0) --
*     Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,
*     Courant Institute, Argonne National Lab, and Rice University
*     March 31, 1993
*
*     .. Scalar Arguments ..
      CHARACTER          UPLO
      INTEGER            INFO, LDA, N
*     ..
*     .. Array Arguments ..
      DOUBLE PRECISION   A( LDA, * )
*     ..
*
*  Purpose
*  =======
*
*  DPOTRF computes the Cholesky factorization of a real symmetric
*  positive definite matrix A.
*
*  The factorization has the form
*     A = U**T * U,  if UPLO = 'U', or
*     A = L  * L**T,  if UPLO = 'L',
*  where U is an upper triangular matrix and L is lower triangular.
*
*  This is the block version of the algorithm, calling Level 3 BLAS.
*
*  Arguments
*  =========
*
*  UPLO    (input) CHARACTER*1
*          = 'U':  Upper triangle of A is stored;
*          = 'L':  Lower triangle of A is stored.
*
*  N       (input) INTEGER
*          The order of the matrix A.  N >= 0.
*
*  A       (input/output) DOUBLE PRECISION array, dimension (LDA,N)
*          On entry, the symmetric matrix A.  If UPLO = 'U', the leading
*          N-by-N upper triangular part of A contains the upper
*          triangular part of the matrix A, and the strictly lower
*          triangular part of A is not referenced.  If UPLO = 'L', the
*          leading N-by-N lower triangular part of A contains the lower
*          triangular part of the matrix A, and the strictly upper
*          triangular part of A is not referenced.
*
*          On exit, if INFO = 0, the factor U or L from the Cholesky
*          factorization A = U**T*U or A = L*L**T.
*
*  LDA     (input) INTEGER
*          The leading dimension of the array A.  LDA >= max(1,N).
*
*  INFO    (output) INTEGER
*          = 0:  successful exit
*          < 0:  if INFO = -i, the i-th argument had an illegal value
*          > 0:  if INFO = i, the leading minor of order i is not
*                positive definite, and the factorization could not be
*                completed.
*
*  =====================================================================
*
*     .. Parameters ..
      DOUBLE PRECISION   ONE
      PARAMETER          ( ONE = 1.0D+0 )
*     ..
*     .. Local Scalars ..
      LOGICAL            UPPER
      INTEGER            J, JB, NB
*     ..
*     .. External Functions ..
      LOGICAL            LSAME
      EXTERNAL           LSAME

      INTEGER            ILAENV
      EXTERNAL           ILAENV

*     ..
*     .. External Subroutines ..
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          MAX, MIN
*     ..
*     .. Executable Statements ..
*
*     Test the input parameters.
*
      INFO = 0
      UPPER = LSAME( UPLO, 'U' )
      IF( .NOT.UPPER .AND. .NOT.LSAME( UPLO, 'L' ) ) THEN
         INFO = -1
      ELSE IF( N.LT.0 ) THEN
         INFO = -2
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -4
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'DPOTRF', -INFO )
         RETURN
      END IF
*
*     Quick return if possible
*
      IF( N.EQ.0 )
     $   RETURN
*
*     Determine the block size for this environment.
*
      NB = ILAENV( 1, 'DPOTRF', UPLO, N, -1, -1, -1 )
      IF( NB.LE.1 .OR. NB.GE.N ) THEN
*
*        Use unblocked code.
*
         CALL DPOTF2( UPLO, N, A, LDA, INFO )
      ELSE
*
*        Use blocked code.
*
         IF( UPPER ) THEN
*
*           Compute the Cholesky factorization A = U'*U.
*
            DO 10 J = 1, N, NB
*
*              Update and factorize the current diagonal block and test
*              for non-positive-definiteness.
*
               JB = MIN( NB, N-J+1 )
               CALL DSYRK( 'Upper', 'Transpose', JB, J-1, -ONE,
     $                     A( 1, J ), LDA, ONE, A( J, J ), LDA )
               CALL DPOTF2( 'Upper', JB, A( J, J ), LDA, INFO )
               IF( INFO.NE.0 )
     $            GO TO 30
               IF( J+JB.LE.N ) THEN
*
*                 Compute the current block row.
*
                  CALL DGEMM( 'Transpose', 'No transpose', JB, N-J-JB+1,
     $                        J-1, -ONE, A( 1, J ), LDA, A( 1, J+JB ),
     $                        LDA, ONE, A( J, J+JB ), LDA )
                  CALL DTRSM( 'Left', 'Upper', 'Transpose', 'Non-unit',
     $                        JB, N-J-JB+1, ONE, A( J, J ), LDA,
     $                        A( J, J+JB ), LDA )
               END IF
   10       CONTINUE
*
         ELSE
*
*           Compute the Cholesky factorization A = L*L'.
*
            DO 20 J = 1, N, NB
*
*              Update and factorize the current diagonal block and test
*              for non-positive-definiteness.
*
               JB = MIN( NB, N-J+1 )
               CALL DSYRK( 'Lower', 'No transpose', JB, J-1, -ONE,
     $                     A( J, 1 ), LDA, ONE, A( J, J ), LDA )
               CALL DPOTF2( 'Lower', JB, A( J, J ), LDA, INFO )
               IF( INFO.NE.0 )
     $            GO TO 30
               IF( J+JB.LE.N ) THEN
*
*                 Compute the current block column.
*
                  CALL DGEMM( 'No transpose', 'Transpose', N-J-JB+1, JB,
     $                        J-1, -ONE, A( J+JB, 1 ), LDA, A( J, 1 ),
     $                        LDA, ONE, A( J+JB, J ), LDA )
                  CALL DTRSM( 'Right', 'Lower', 'Transpose', 'Non-unit',
     $                        N-J-JB+1, JB, ONE, A( J, J ), LDA,
     $                        A( J+JB, J ), LDA )
               END IF
   20       CONTINUE
         END IF
      END IF
      GO TO 40
*
   30 CONTINUE
      INFO = INFO + J - 1
*
   40 CONTINUE
      RETURN
*
*     End of DPOTRF
*
      END SUBROUTINE DPOTRF

! ##################################################################################################################################
! 002 LAPACK_SYM_MAT_INV

      SUBROUTINE DPOTRI( UPLO, N, A, LDA, INFO )
*
*  -- LAPACK routine (version 3.0) --
*     Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,
*     Courant Institute, Argonne National Lab, and Rice University
*     March 31, 1993
*
*     .. Scalar Arguments ..
      CHARACTER          UPLO
      INTEGER            INFO, LDA, N
*     ..
*     .. Array Arguments ..
      DOUBLE PRECISION   A( LDA, * )
*     ..
*
*  Purpose
*  =======
*
*  DPOTRI computes the inverse of a real symmetric positive definite
*  matrix A using the Cholesky factorization A = U**T*U or A = L*L**T
*  computed by DPOTRF.
*
*  Arguments
*  =========
*
*  UPLO    (input) CHARACTER*1
*          = 'U':  Upper triangle of A is stored;
*          = 'L':  Lower triangle of A is stored.
*
*  N       (input) INTEGER
*          The order of the matrix A.  N >= 0.
*
*  A       (input/output) DOUBLE PRECISION array, dimension (LDA,N)
*          On entry, the triangular factor U or L from the Cholesky
*          factorization A = U**T*U or A = L*L**T, as computed by
*          DPOTRF.
*          On exit, the upper or lower triangle of the (symmetric)
*          inverse of A, overwriting the input factor U or L.
*
*  LDA     (input) INTEGER
*          The leading dimension of the array A.  LDA >= max(1,N).
*
*  INFO    (output) INTEGER
*          = 0:  successful exit
*          < 0:  if INFO = -i, the i-th argument had an illegal value
*          > 0:  if INFO = i, the (i,i) element of the factor U or L is
*                zero, and the inverse could not be computed.
*
*  =====================================================================
*
*     .. External Functions ..
      LOGICAL            LSAME
      EXTERNAL           LSAME
*     ..
*     .. External Subroutines ..
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          MAX
*     ..
*     .. Executable Statements ..
*
*     Test the input parameters.
*
      INFO = 0
      IF( .NOT.LSAME( UPLO, 'U' ) .AND. .NOT.LSAME( UPLO, 'L' ) ) THEN
         INFO = -1
      ELSE IF( N.LT.0 ) THEN
         INFO = -2
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -4
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'DPOTRI', -INFO )
         RETURN
      END IF
*
*     Quick return if possible
*
      IF( N.EQ.0 )
     $   RETURN
*
*     Invert the triangular Cholesky factor U or L.
*
      CALL DTRTRI( UPLO, 'Non-unit', N, A, LDA, INFO )
      IF( INFO.GT.0 )
     $   RETURN
*
*     Form inv(U)*inv(U)' or inv(L)'*inv(L).
*
      CALL DLAUUM( UPLO, N, A, LDA, INFO )
*
      RETURN
*
*     End of DPOTRI
*
      END SUBROUTINE DPOTRI

! ##################################################################################################################################
! 003 LAPACK_SYM_MAT_INV

      SUBROUTINE DLAUUM( UPLO, N, A, LDA, INFO )
*
*  -- LAPACK auxiliary routine (version 3.0) --
*     Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,
*     Courant Institute, Argonne National Lab, and Rice University
*     February 29, 1992
*
*     .. Scalar Arguments ..
      CHARACTER          UPLO
      INTEGER            INFO, LDA, N
*     ..
*     .. Array Arguments ..
      DOUBLE PRECISION   A( LDA, * )
*     ..
*
*  Purpose
*  =======
*
*  DLAUUM computes the product U * U' or L' * L, where the triangular
*  factor U or L is stored in the upper or lower triangular part of
*  the array A.
*
*  If UPLO = 'U' or 'u' then the upper triangle of the result is stored,
*  overwriting the factor U in A.
*  If UPLO = 'L' or 'l' then the lower triangle of the result is stored,
*  overwriting the factor L in A.
*
*  This is the blocked form of the algorithm, calling Level 3 BLAS.
*
*  Arguments
*  =========
*
*  UPLO    (input) CHARACTER*1
*          Specifies whether the triangular factor stored in the array A
*          is upper or lower triangular:
*          = 'U':  Upper triangular
*          = 'L':  Lower triangular
*
*  N       (input) INTEGER
*          The order of the triangular factor U or L.  N >= 0.
*
*  A       (input/output) DOUBLE PRECISION array, dimension (LDA,N)
*          On entry, the triangular factor U or L.
*          On exit, if UPLO = 'U', the upper triangle of A is
*          overwritten with the upper triangle of the product U * U';
*          if UPLO = 'L', the lower triangle of A is overwritten with
*          the lower triangle of the product L' * L.
*
*  LDA     (input) INTEGER
*          The leading dimension of the array A.  LDA >= max(1,N).
*
*  INFO    (output) INTEGER
*          = 0: successful exit
*          < 0: if INFO = -k, the k-th argument had an illegal value
*
*  =====================================================================
*
*     .. Parameters ..
      DOUBLE PRECISION   ONE
      PARAMETER          ( ONE = 1.0D+0 )
*     ..
*     .. Local Scalars ..
      LOGICAL            UPPER
      INTEGER            I, IB, NB
*     ..
*     .. External Functions ..
      LOGICAL            LSAME
      EXTERNAL           LSAME

      INTEGER            ILAENV
      EXTERNAL           ILAENV

*     ..
*     .. External Subroutines ..
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          MAX, MIN
*     ..
*     .. Executable Statements ..
*
*     Test the input parameters.
*
      INFO = 0
      UPPER = LSAME( UPLO, 'U' )
      IF( .NOT.UPPER .AND. .NOT.LSAME( UPLO, 'L' ) ) THEN
         INFO = -1
      ELSE IF( N.LT.0 ) THEN
         INFO = -2
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -4
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'DLAUUM', -INFO )
         RETURN
      END IF
*
*     Quick return if possible
*
      IF( N.EQ.0 )
     $   RETURN
*
*     Determine the block size for this environment.
*
      NB = ILAENV( 1, 'DLAUUM', UPLO, N, -1, -1, -1 )
*
      IF( NB.LE.1 .OR. NB.GE.N ) THEN
*
*        Use unblocked code
*
         CALL DLAUU2( UPLO, N, A, LDA, INFO )
      ELSE
*
*        Use blocked code
*
         IF( UPPER ) THEN
*
*           Compute the product U * U'.
*
            DO 10 I = 1, N, NB
               IB = MIN( NB, N-I+1 )
               CALL DTRMM( 'Right', 'Upper', 'Transpose', 'Non-unit',
     $                     I-1, IB, ONE, A( I, I ), LDA, A( 1, I ),
     $                     LDA )
               CALL DLAUU2( 'Upper', IB, A( I, I ), LDA, INFO )
               IF( I+IB.LE.N ) THEN
                  CALL DGEMM( 'No transpose', 'Transpose', I-1, IB,
     $                        N-I-IB+1, ONE, A( 1, I+IB ), LDA,
     $                        A( I, I+IB ), LDA, ONE, A( 1, I ), LDA )
                  CALL DSYRK( 'Upper', 'No transpose', IB, N-I-IB+1,
     $                        ONE, A( I, I+IB ), LDA, ONE, A( I, I ),
     $                        LDA )
               END IF
   10       CONTINUE
         ELSE
*
*           Compute the product L' * L.
*
            DO 20 I = 1, N, NB
               IB = MIN( NB, N-I+1 )
               CALL DTRMM( 'Left', 'Lower', 'Transpose', 'Non-unit', IB,
     $                     I-1, ONE, A( I, I ), LDA, A( I, 1 ), LDA )
               CALL DLAUU2( 'Lower', IB, A( I, I ), LDA, INFO )
               IF( I+IB.LE.N ) THEN
                  CALL DGEMM( 'Transpose', 'No transpose', IB, I-1,
     $                        N-I-IB+1, ONE, A( I+IB, I ), LDA,
     $                        A( I+IB, 1 ), LDA, ONE, A( I, 1 ), LDA )
                  CALL DSYRK( 'Lower', 'Transpose', IB, N-I-IB+1, ONE,
     $                        A( I+IB, I ), LDA, ONE, A( I, I ), LDA )
               END IF
   20       CONTINUE
         END IF
      END IF
*
      RETURN
*
*     End of DLAUUM
*
      END SUBROUTINE DLAUUM

! ##################################################################################################################################
! 005 LAPACK_SYM_MAT_INV

      SUBROUTINE DLAUU2( UPLO, N, A, LDA, INFO )
*
*  -- LAPACK auxiliary routine (version 3.0) --
*     Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,
*     Courant Institute, Argonne National Lab, and Rice University
*     February 29, 1992
*
*     .. Scalar Arguments ..
      CHARACTER          UPLO
      INTEGER            INFO, LDA, N
*     ..
*     .. Array Arguments ..
      DOUBLE PRECISION   A( LDA, * )
*     ..
*
*  Purpose
*  =======
*
*  DLAUU2 computes the product U * U' or L' * L, where the triangular
*  factor U or L is stored in the upper or lower triangular part of
*  the array A.
*
*  If UPLO = 'U' or 'u' then the upper triangle of the result is stored,
*  overwriting the factor U in A.
*  If UPLO = 'L' or 'l' then the lower triangle of the result is stored,
*  overwriting the factor L in A.
*
*  This is the unblocked form of the algorithm, calling Level 2 BLAS.
*
*  Arguments
*  =========
*
*  UPLO    (input) CHARACTER*1
*          Specifies whether the triangular factor stored in the array A
*          is upper or lower triangular:
*          = 'U':  Upper triangular
*          = 'L':  Lower triangular
*
*  N       (input) INTEGER
*          The order of the triangular factor U or L.  N >= 0.
*
*  A       (input/output) DOUBLE PRECISION array, dimension (LDA,N)
*          On entry, the triangular factor U or L.
*          On exit, if UPLO = 'U', the upper triangle of A is
*          overwritten with the upper triangle of the product U * U';
*          if UPLO = 'L', the lower triangle of A is overwritten with
*          the lower triangle of the product L' * L.
*
*  LDA     (input) INTEGER
*          The leading dimension of the array A.  LDA >= max(1,N).
*
*  INFO    (output) INTEGER
*          = 0: successful exit
*          < 0: if INFO = -k, the k-th argument had an illegal value
*
*  =====================================================================
*
*     .. Parameters ..
      DOUBLE PRECISION   ONE
      PARAMETER          ( ONE = 1.0D+0 )
*     ..
*     .. Local Scalars ..
      LOGICAL            UPPER
      INTEGER            I
      DOUBLE PRECISION   AII
*     ..
*     .. External Functions ..

      LOGICAL            LSAME
      EXTERNAL           LSAME

*     ..
*     .. External Subroutines ..
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          MAX
*     ..
*     .. Executable Statements ..
*
*     Test the input parameters.
*
      INFO = 0
      UPPER = LSAME( UPLO, 'U' )
      IF( .NOT.UPPER .AND. .NOT.LSAME( UPLO, 'L' ) ) THEN
         INFO = -1
      ELSE IF( N.LT.0 ) THEN
         INFO = -2
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -4
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'DLAUU2', -INFO )
         RETURN
      END IF
*
*     Quick return if possible
*
      IF( N.EQ.0 )
     $   RETURN
*
      IF( UPPER ) THEN
*
*        Compute the product U * U'.
*
         DO 10 I = 1, N
            AII = A( I, I )
            IF( I.LT.N ) THEN
               A( I, I ) = DDOT( N-I+1, A( I, I ), LDA, A( I, I ), LDA )
               CALL DGEMV( 'No transpose', I-1, N-I, ONE, A( 1, I+1 ),
     $                     LDA, A( I, I+1 ), LDA, AII, A( 1, I ), 1 )
            ELSE
               CALL DSCAL( I, AII, A( 1, I ), 1 )
            END IF
   10    CONTINUE
*
      ELSE
*
*        Compute the product L' * L.
*
         DO 20 I = 1, N
            AII = A( I, I )
            IF( I.LT.N ) THEN
               A( I, I ) = DDOT( N-I+1, A( I, I ), 1, A( I, I ), 1 )
               CALL DGEMV( 'Transpose', N-I, I-1, ONE, A( I+1, 1 ), LDA,
     $                     A( I+1, I ), 1, AII, A( I, 1 ), LDA )
            ELSE
               CALL DSCAL( I, AII, A( I, 1 ), LDA )
            END IF
   20    CONTINUE
      END IF
*
      RETURN
*
*     End of DLAUU2
*
      END SUBROUTINE DLAUU2

! ##################################################################################################################################
! 006 LAPACK_SYM_MAT_INV

      SUBROUTINE DTRTI2( UPLO, DIAG, N, A, LDA, INFO )
*
*  -- LAPACK routine (version 3.0) --
*     Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,
*     Courant Institute, Argonne National Lab, and Rice University
*     February 29, 1992
*
*     .. Scalar Arguments ..
      CHARACTER          DIAG, UPLO
      INTEGER            INFO, LDA, N
*     ..
*     .. Array Arguments ..
      DOUBLE PRECISION   A( LDA, * )
*     ..
*
*  Purpose
*  =======
*
*  DTRTI2 computes the inverse of a real upper or lower triangular
*  matrix.
*
*  This is the Level 2 BLAS version of the algorithm.
*
*  Arguments
*  =========
*
*  UPLO    (input) CHARACTER*1
*          Specifies whether the matrix A is upper or lower triangular.
*          = 'U':  Upper triangular
*          = 'L':  Lower triangular
*
*  DIAG    (input) CHARACTER*1
*          Specifies whether or not the matrix A is unit triangular.
*          = 'N':  Non-unit triangular
*          = 'U':  Unit triangular
*
*  N       (input) INTEGER
*          The order of the matrix A.  N >= 0.
*
*  A       (input/output) DOUBLE PRECISION array, dimension (LDA,N)
*          On entry, the triangular matrix A.  If UPLO = 'U', the
*          leading n by n upper triangular part of the array A contains
*          the upper triangular matrix, and the strictly lower
*          triangular part of A is not referenced.  If UPLO = 'L', the
*          leading n by n lower triangular part of the array A contains
*          the lower triangular matrix, and the strictly upper
*          triangular part of A is not referenced.  If DIAG = 'U', the
*          diagonal elements of A are also not referenced and are
*          assumed to be 1.
*
*          On exit, the (triangular) inverse of the original matrix, in
*          the same storage format.
*
*  LDA     (input) INTEGER
*          The leading dimension of the array A.  LDA >= max(1,N).
*
*  INFO    (output) INTEGER
*          = 0: successful exit
*          < 0: if INFO = -k, the k-th argument had an illegal value
*
*  =====================================================================
*
*     .. Parameters ..
      DOUBLE PRECISION   ONE
      PARAMETER          ( ONE = 1.0D+0 )
*     ..
*     .. Local Scalars ..
      LOGICAL            NOUNIT, UPPER
      INTEGER            J
      DOUBLE PRECISION   AJJ
*     ..
*     .. External Functions ..
      LOGICAL            LSAME
      EXTERNAL           LSAME
*     ..
*     .. External Subroutines ..
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          MAX
*     ..
*     .. Executable Statements ..
*
*     Test the input parameters.
*
      INFO = 0
      UPPER = LSAME( UPLO, 'U' )
      NOUNIT = LSAME( DIAG, 'N' )
      IF( .NOT.UPPER .AND. .NOT.LSAME( UPLO, 'L' ) ) THEN
         INFO = -1
      ELSE IF( .NOT.NOUNIT .AND. .NOT.LSAME( DIAG, 'U' ) ) THEN
         INFO = -2
      ELSE IF( N.LT.0 ) THEN
         INFO = -3
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -5
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'DTRTI2', -INFO )
         RETURN
      END IF
*
      IF( UPPER ) THEN
*
*        Compute inverse of upper triangular matrix.
*
         DO 10 J = 1, N
            IF( NOUNIT ) THEN
               A( J, J ) = ONE / A( J, J )
               AJJ = -A( J, J )
            ELSE
               AJJ = -ONE
            END IF
*
*           Compute elements 1:j-1 of j-th column.
*
            CALL DTRMV( 'Upper', 'No transpose', DIAG, J-1, A, LDA,
     $                  A( 1, J ), 1 )
            CALL DSCAL( J-1, AJJ, A( 1, J ), 1 )
   10    CONTINUE
      ELSE
*
*        Compute inverse of lower triangular matrix.
*
         DO 20 J = N, 1, -1
            IF( NOUNIT ) THEN
               A( J, J ) = ONE / A( J, J )
               AJJ = -A( J, J )
            ELSE
               AJJ = -ONE
            END IF
            IF( J.LT.N ) THEN
*
*              Compute elements j+1:n of j-th column.
*
               CALL DTRMV( 'Lower', 'No transpose', DIAG, N-J,
     $                     A( J+1, J+1 ), LDA, A( J+1, J ), 1 )
               CALL DSCAL( N-J, AJJ, A( J+1, J ), 1 )
            END IF
   20    CONTINUE
      END IF
*
      RETURN
*
*     End of DTRTI2
*
      END SUBROUTINE DTRTI2

      END MODULE LAPACK_SYM_MAT_INV


      recursive
     *subroutine hybrj(fcn,n,x,fvec,fjac,ldfjac,xtol,maxfev,diag,mode,
     *                 factor,nprint,info,nfev,njev,r,lr,qtf,wa1,wa2,
     *                 wa3,wa4)
      integer n,ldfjac,maxfev,mode,nprint,info,nfev,njev,lr
      double precision xtol,factor
      double precision x(n),fvec(n),fjac(ldfjac,n),diag(n),r(lr),
     *                 qtf(n),wa1(n),wa2(n),wa3(n),wa4(n)
c     **********
c
c     subroutine hybrj
c
c     the purpose of hybrj is to find a zero of a system of
c     n nonlinear functions in n variables by a modification
c     of the powell hybrid method. the user must provide a
c     subroutine which calculates the functions and the jacobian.
c
c     the subroutine statement is
c
c       subroutine hybrj(fcn,n,x,fvec,fjac,ldfjac,xtol,maxfev,diag,
c                        mode,factor,nprint,info,nfev,njev,r,lr,qtf,
c                        wa1,wa2,wa3,wa4)
c
c     where
c
c       fcn is the name of the user-supplied subroutine which
c         calculates the functions and the jacobian. fcn must
c         be declared in an external statement in the user
c         calling program, and should be written as follows.
c
c         subroutine fcn(n,x,fvec,fjac,ldfjac,iflag)
c         integer n,ldfjac,iflag
c         double precision x(n),fvec(n),fjac(ldfjac,n)
c         ----------
c         if iflag = 1 calculate the functions at x and
c         return this vector in fvec. do not alter fjac.
c         if iflag = 2 calculate the jacobian at x and
c         return this matrix in fjac. do not alter fvec.
c         ---------
c         return
c         end
c
c         the value of iflag should not be changed by fcn unless
c         the user wants to terminate execution of hybrj.
c         in this case set iflag to a negative integer.
c
c       n is a positive integer input variable set to the number
c         of functions and variables.
c
c       x is an array of length n. on input x must contain
c         an initial estimate of the solution vector. on output x
c         contains the final estimate of the solution vector.
c
c       fvec is an output array of length n which contains
c         the functions evaluated at the output x.
c
c       fjac is an output n by n array which contains the
c         orthogonal matrix q produced by the qr factorization
c         of the final approximate jacobian.
c
c       ldfjac is a positive integer input variable not less than n
c         which specifies the leading dimension of the array fjac.
c
c       xtol is a nonnegative input variable. termination
c         occurs when the relative error between two consecutive
c         iterates is at most xtol.
c
c       maxfev is a positive integer input variable. termination
c         occurs when the number of calls to fcn with iflag = 1
c         has reached maxfev.
c
c       diag is an array of length n. if mode = 1 (see
c         below), diag is internally set. if mode = 2, diag
c         must contain positive entries that serve as
c         multiplicative scale factors for the variables.
c
c       mode is an integer input variable. if mode = 1, the
c         variables will be scaled internally. if mode = 2,
c         the scaling is specified by the input diag. other
c         values of mode are equivalent to mode = 1.
c
c       factor is a positive input variable used in determining the
c         initial step bound. this bound is set to the product of
c         factor and the euclidean norm of diag*x if nonzero, or else
c         to factor itself. in most cases factor should lie in the
c         interval (.1,100.). 100. is a generally recommended value.
c
c       nprint is an integer input variable that enables controlled
c         printing of iterates if it is positive. in this case,
c         fcn is called with iflag = 0 at the beginning of the first
c         iteration and every nprint iterations thereafter and
c         immediately prior to return, with x and fvec available
c         for printing. fvec and fjac should not be altered.
c         if nprint is not positive, no special calls of fcn
c         with iflag = 0 are made.
c
c       info is an integer output variable. if the user has
c         terminated execution, info is set to the (negative)
c         value of iflag. see description of fcn. otherwise,
c         info is set as follows.
c
c         info = 0   improper input parameters.
c
c         info = 1   relative error between two consecutive iterates
c                    is at most xtol.
c
c         info = 2   number of calls to fcn with iflag = 1 has
c                    reached maxfev.
c
c         info = 3   xtol is too small. no further improvement in
c                    the approximate solution x is possible.
c
c         info = 4   iteration is not making good progress, as
c                    measured by the improvement from the last
c                    five jacobian evaluations.
c
c         info = 5   iteration is not making good progress, as
c                    measured by the improvement from the last
c                    ten iterations.
c
c       nfev is an integer output variable set to the number of
c         calls to fcn with iflag = 1.
c
c       njev is an integer output variable set to the number of
c         calls to fcn with iflag = 2.
c
c       r is an output array of length lr which contains the
c         upper triangular matrix produced by the qr factorization
c         of the final approximate jacobian, stored rowwise.
c
c       lr is a positive integer input variable not less than
c         (n*(n+1))/2.
c
c       qtf is an output array of length n which contains
c         the vector (q transpose)*fvec.
c
c       wa1, wa2, wa3, and wa4 are work arrays of length n.
c
c     subprograms called
c
c       user-supplied ...... fcn
c
c       minpack-supplied ... dogleg,dpmpar,enorm,
c                            qform,qrfac,r1mpyq,r1updt
c
c       fortran-supplied ... dabs,dmax1,dmin1,mod
c
c     argonne national laboratory. minpack project. march 1980.
c     burton s. garbow, kenneth e. hillstrom, jorge j. more
c
c     **********
      integer i,iflag,iter,j,jm1,l,ncfail,ncsuc,nslow1,nslow2
      integer iwa(1)
      logical jeval,sing
      double precision actred,delta,epsmch,fnorm,fnorm1,one,pnorm,
     *                 prered,p1,p5,p001,p0001,ratio,sum,temp,xnorm,
     *                 zero
      double precision dpmpar,enorm
      data one,p1,p5,p001,p0001,zero
     *     /1.0d0,1.0d-1,5.0d-1,1.0d-3,1.0d-4,0.0d0/
c
c     epsmch is the machine precision.
c
      epsmch = dpmpar(1)
c
      info = 0
      iflag = 0
      nfev = 0
      njev = 0
c
c     check the input parameters for errors.
c
      if (n .le. 0 .or. ldfjac .lt. n .or. xtol .lt. zero
     *    .or. maxfev .le. 0 .or. factor .le. zero
     *    .or. lr .lt. (n*(n + 1))/2) go to 300
      if (mode .ne. 2) go to 20
      do 10 j = 1, n
         if (diag(j) .le. zero) go to 300
   10    continue
   20 continue
c
c     evaluate the function at the starting point
c     and calculate its norm.
c
      iflag = 1
      call fcn(n,x,fvec,fjac,ldfjac,iflag)
      nfev = 1
      if (iflag .lt. 0) go to 300
      fnorm = enorm(n,fvec)
c
c     initialize iteration counter and monitors.
c
      iter = 1
      ncsuc = 0
      ncfail = 0
      nslow1 = 0
      nslow2 = 0
c
c     beginning of the outer loop.
c
   30 continue
         jeval = .true.
c
c        calculate the jacobian matrix.
c
         iflag = 2
         call fcn(n,x,fvec,fjac,ldfjac,iflag)
         njev = njev + 1
         if (iflag .lt. 0) go to 300
c
c        compute the qr factorization of the jacobian.
c
         call qrfac(n,n,fjac,ldfjac,.false.,iwa,1,wa1,wa2,wa3)
c
c        on the first iteration and if mode is 1, scale according
c        to the norms of the columns of the initial jacobian.
c
         if (iter .ne. 1) go to 70
         if (mode .eq. 2) go to 50
         do 40 j = 1, n
            diag(j) = wa2(j)
            if (wa2(j) .eq. zero) diag(j) = one
   40       continue
   50    continue
c
c        on the first iteration, calculate the norm of the scaled x
c        and initialize the step bound delta.
c
         do 60 j = 1, n
            wa3(j) = diag(j)*x(j)
   60       continue
         xnorm = enorm(n,wa3)
         delta = factor*xnorm
         if (delta .eq. zero) delta = factor
   70    continue
c
c        form (q transpose)*fvec and store in qtf.
c
         do 80 i = 1, n
            qtf(i) = fvec(i)
   80       continue
         do 120 j = 1, n
            if (fjac(j,j) .eq. zero) go to 110
            sum = zero
            do 90 i = j, n
               sum = sum + fjac(i,j)*qtf(i)
   90          continue
            temp = -sum/fjac(j,j)
            do 100 i = j, n
               qtf(i) = qtf(i) + fjac(i,j)*temp
  100          continue
  110       continue
  120       continue
c
c        copy the triangular factor of the qr factorization into r.
c
         sing = .false.
         do 150 j = 1, n
            l = j
            jm1 = j - 1
            if (jm1 .lt. 1) go to 140
            do 130 i = 1, jm1
               r(l) = fjac(i,j)
               l = l + n - i
  130          continue
  140       continue
            r(l) = wa1(j)
            if (wa1(j) .eq. zero) sing = .true.
  150       continue
c
c        accumulate the orthogonal factor in fjac.
c
         call qform(n,n,fjac,ldfjac,wa1)
c
c        rescale if necessary.
c
         if (mode .eq. 2) go to 170
         do 160 j = 1, n
            diag(j) = dmax1(diag(j),wa2(j))
  160       continue
  170    continue
c
c        beginning of the inner loop.
c
  180    continue
c
c           if requested, call fcn to enable printing of iterates.
c
            if (nprint .le. 0) go to 190
            iflag = 0
            if (mod(iter-1,nprint) .eq. 0)
     *         call fcn(n,x,fvec,fjac,ldfjac,iflag)
            if (iflag .lt. 0) go to 300
  190       continue
c
c           determine the direction p.
c
            call dogleg(n,r,lr,diag,qtf,delta,wa1,wa2,wa3)
c
c           store the direction p and x + p. calculate the norm of p.
c
            do 200 j = 1, n
               wa1(j) = -wa1(j)
               wa2(j) = x(j) + wa1(j)
               wa3(j) = diag(j)*wa1(j)
  200          continue
            pnorm = enorm(n,wa3)
c
c           on the first iteration, adjust the initial step bound.
c
            if (iter .eq. 1) delta = dmin1(delta,pnorm)
c
c           evaluate the function at x + p and calculate its norm.
c
            iflag = 1
            call fcn(n,wa2,wa4,fjac,ldfjac,iflag)
            nfev = nfev + 1
            if (iflag .lt. 0) go to 300
            fnorm1 = enorm(n,wa4)
c
c           compute the scaled actual reduction.
c
            actred = -one
            if (fnorm1 .lt. fnorm) actred = one - (fnorm1/fnorm)**2
c
c           compute the scaled predicted reduction.
c
            l = 1
            do 220 i = 1, n
               sum = zero
               do 210 j = i, n
                  sum = sum + r(l)*wa1(j)
                  l = l + 1
  210             continue
               wa3(i) = qtf(i) + sum
  220          continue
            temp = enorm(n,wa3)
            prered = zero
            if (temp .lt. fnorm) prered = one - (temp/fnorm)**2
c
c           compute the ratio of the actual to the predicted
c           reduction.
c
            ratio = zero
            if (prered .gt. zero) ratio = actred/prered
c
c           update the step bound.
c
            if (ratio .ge. p1) go to 230
               ncsuc = 0
               ncfail = ncfail + 1
               delta = p5*delta
               go to 240
  230       continue
               ncfail = 0
               ncsuc = ncsuc + 1
               if (ratio .ge. p5 .or. ncsuc .gt. 1)
     *            delta = dmax1(delta,pnorm/p5)
               if (dabs(ratio-one) .le. p1) delta = pnorm/p5
  240       continue
c
c           test for successful iteration.
c
            if (ratio .lt. p0001) go to 260
c
c           successful iteration. update x, fvec, and their norms.
c
            do 250 j = 1, n
               x(j) = wa2(j)
               wa2(j) = diag(j)*x(j)
               fvec(j) = wa4(j)
  250          continue
            xnorm = enorm(n,wa2)
            fnorm = fnorm1
            iter = iter + 1
  260       continue
c
c           determine the progress of the iteration.
c
            nslow1 = nslow1 + 1
            if (actred .ge. p001) nslow1 = 0
            if (jeval) nslow2 = nslow2 + 1
            if (actred .ge. p1) nslow2 = 0
c
c           test for convergence.
c
            if (delta .le. xtol*xnorm .or. fnorm .eq. zero) info = 1
            if (info .ne. 0) go to 300
c
c           tests for termination and stringent tolerances.
c
            if (nfev .ge. maxfev) info = 2
            if (p1*dmax1(p1*delta,pnorm) .le. epsmch*xnorm) info = 3
            if (nslow2 .eq. 5) info = 4
            if (nslow1 .eq. 10) info = 5
            if (info .ne. 0) go to 300
c
c           criterion for recalculating jacobian.
c
            if (ncfail .eq. 2) go to 290
c
c           calculate the rank one modification to the jacobian
c           and update qtf if necessary.
c
            do 280 j = 1, n
               sum = zero
               do 270 i = 1, n
                  sum = sum + fjac(i,j)*wa4(i)
  270             continue
               wa2(j) = (sum - wa3(j))/pnorm
               wa1(j) = diag(j)*((diag(j)*wa1(j))/pnorm)
               if (ratio .ge. p0001) qtf(j) = sum
  280          continue
c
c           compute the qr factorization of the updated jacobian.
c
            call r1updt(n,n,r,lr,wa1,wa2,wa3,sing)
            call r1mpyq(n,n,fjac,ldfjac,wa2,wa3)
            call r1mpyq(1,n,qtf,1,wa2,wa3)
c
c           end of the inner loop.
c
            jeval = .false.
            go to 180
  290    continue
c
c        end of the outer loop.
c
         go to 30
  300 continue
c
c     termination, either normal or user imposed.
c
      if (iflag .lt. 0) info = iflag
      iflag = 0
      if (nprint .gt. 0) call fcn(n,x,fvec,fjac,ldfjac,iflag)
      return
c
c     last card of subroutine hybrj.
c
      end


      SUBROUTINE VISCD
C
C     THIS SUBROUTINE COMPUTES THE 12X12 MATRIX BGG FOR A VISCOUS
C     (DASHPOT) ELEMENT
C
C     DOUBLE PRECISION VERSION
C
C     THE ECPT ENTRIES FOR THE VISC ELEMENT ARE
C
C     ECPT
C     ECPT( 1)   ELEMENT ID
C     ECPT( 2)   SIL NUMBER FOR GRID POINT A
C     ECPT( 3)   SIL NUMBER FOR GRID POINT B
C     ECPT( 4)   EXTENSIONAL DAMPING CONSTANT  - C1
C     ECPT( 5)   TORSIONAL DAMPING COEFFICIENT - C2
C     ECPT( 6)   COORD. SYSTEM ID FOR POINT A
C     ECPT( 7)   X1
C     ECPT( 8)   Y1
C     ECPT( 9)   Z1
C     ECPT(10)   COORD. SYSTEM ID FOR POINT B
C     ECPT(11)   X2
C     ECPT(12)   Y2
C     ECPT(13)   Z2
C     ECPT(14)   ELEMENT TEMPERATURE (NOT USED)
C
C
      LOGICAL          NOGO,IDBUG
      INTEGER          IECPT(14),ELID,ESTID,DICT(7),INDX(4),KX(4),KBX(4)
      DOUBLE PRECISION FL,C1,C2,VEC(3),D(64),B(144),TA(9),TB(9)
      CHARACTER        UFM*23
      COMMON /XMSSG /  UFM
      COMMON /SYSTEM/  SKP,IOUTPT,KSYSTM(53),IHEAT
      COMMON /EMGEST/  ECPT(14)
      COMMON /EMGPRM/  IXTRA,JCORE,NCORE,DUM(12),ISTIF,IMASS,IDAMP,
     1                 IPREC,NOGO,HEAT,ICMBAR,LCSTM,LMAT,LHMAT
      COMMON /ZZZZZZ/  XX(1)
      COMMON /EMGDIC/  IDM,LDICT,NGRIDS,ELID,ESTID
      EQUIVALENCE      (ECPT(1),IECPT(1),IELID), (DICT(5),DICT5),
     1                 (INDX(1),IA), (INDX(2),IAB), (INDX(3),IBA),
     2                 (INDX(4),IB)
      DATA    KX    /  1 ,7 ,73 ,79 /
      DATA    KBX   /  40,46,112,118/
C
C     INITIALIZE EMGOUT PARAMETERS
C
      IDBUG   = .TRUE.
      NGRIDS  = 2
      LDICT   = 5 + NGRIDS
      DICT(1) = ESTID
      DICT(2) = 1
      DICT(3) = 12
      DICT(4) = 63
      DICT5   = 0.
      IFILE   = 3
      IP      = IPREC
C
C     NOW COMPUTE THE LENGTH OF THE ROD AND NORMALIZE
C
      FL = 0.
      DO 20 I = 1,3
      VEC(I) = ECPT(I+6) - ECPT(I+10)
   20 FL = FL + VEC(I)**2
      FL = DSQRT(FL)
C
      IF (FL .LE. 0) GO TO 7770
      DO 30 I = 1,3
   30 VEC(I) = VEC(I)/FL
C
C     SET UP THE N MATRIX
C
      DO 40 I = 1,3
      DO 40 J = 1,3
      IX = (I-1)*3 + J
   40 D(IX) = VEC(I)*VEC(J)
C
C     INITIALIZE THE B MATRIX
C
      DO 50 I = 1,144
   50 B(I) = 0.D0
C
C     SWAP INDICES A AND B IF NECESSARY SO MATRIX WILL BE ORDERED
C     BY INCREASING SIL VALUE
C
      IPA = 6
      IPB = 10
      IF (IECPT(2) .LT. IECPT(3)) GO TO 60
      IX  = IPA
      IPA = IPB
      IPB = IPA
C
C     CONVERT GRID POINTS TO BASIC COORDINATES IF NECESSARY
C
   60 IA   = 1
      IAB  = 1
      IF (IECPT(IPA) .EQ. 0) GO TO 70
      IA   = 19
      IAB  = 10
      CALL TRANSD (ECPT(IPA),TA(1))
      CALL GMMATD (TA(1),3,3,1, D(1), 3,3,0, D(10))
      CALL GMMATD (D(10),3,3,0, TA(1),3,3,0, D(19))
C
   70 IB  = 1
      IBA = 1
      IF (IECPT(IPB) .EQ. 0) GO TO 80
      IB  = 28
      IBA = 37
      CALL TRANSD (ECPT(IPB),TB(1))
      CALL GMMATD (TB(1),3,3,1, D(1), 3,3,0, D(37))
      CALL GMMATD (D(37),3,3,0, TB(1),3,3,0, D(28))
C
      IAB = 46
C
   80 IF (IECPT(IPA) .EQ. 0) GO TO 90
      CALL GMMATD (D(IAB),3,3,0, TB(1),3,3,0, D(46))
      IBA = 55
      CALL GMMATD (D(IBA),3,3,0, TA(1),3,3,0, D(55))
C
C     CALCULATE THE DAMPING MATRIX B
C
C                       ****                    ****
C                       *      /     /      /      *
C                       * C D  /   0 /-C D  /  0   *
C                       *  1 AA/     /  1 AB/      *
C                       *--------------------------*
C                       *  0   /C D  /   0  /-C D  *
C                       *      / 2 AA/      /  2 AB*
C         B    =        *--------------------------*
C                       *-C D  /   0 / C D  /  0   *
C                       *  1 BA/     /  1 BB/      *
C                       *------------/-------------*
C                       *  0   /-C D /   0  / C D  *
C                       *      /  2 BA      /  2 BB*
C                       *      /     /      /      *
C                       ****                    ****
C
   90 C1 = ECPT (4)
      C2 = ECPT (5)
C
      DO 120 JTJ = 1,4
      KB  = KX(JTJ)
      KBB = KBX(JTJ)
      J   = 0
      I1  = INDX(JTJ)
      I2  = I1 + 8
      IF (MOD(JTJ,2) .NE. 0) GO TO 100
      C1  = -C1
      C2  = -C2
C
  100 DO 110 I = I1,I2
      B(KB)  = C1*D(I)
      B(KBB) = C2*D(I)
      IF (MOD(I,3) .EQ. 0) J = 9
      KB  = KB  + 1 + J
      KBB = KBB + 1 + J
      J   = 0
  110 CONTINUE
C
  120 CONTINUE
C
C
C     OUTPUT THE MATRIX
C
      CALL EMGOUT (B,B,144,1,DICT,IFILE,IP)
      RETURN
C
C     ERROR EXITS
C
 7770 WRITE  (IOUTPT,7775) UFM,IELID
 7775 FORMAT (A23,' 31XX, ILLEGAL GEOMETRY OR CONNEC TIONS FOR VISC ',
     1        'ELEMENT',I10)
      NOGO = .TRUE.
      RETURN
      END


!===============================================================================
program CIJ_Hudson_prog
!===============================================================================
!  Wrapper script for the CIJ_hudson subroutine.
!  Calculate effective elastic constants for isotropic matrix with isotropic
!  ellipsoidal inclusions
!
!  Usage:  CIJ_Hudson vp vs rho a phi vpi vsi rhoi
!     or
!          CIJ_Hudson < [list of vp vs rho a phi vpi vsi rhoi]

   use anisotropy_ajn
   implicit none

   real(8) :: vp,vs,rho,a,phi,vpi,vsi,rhoi,e
   real(8) :: C(6,6),rho_eff
   character(len=80) :: arg
   integer :: i,j,iostatus
   logical :: read_stdin = .false., use_e = .false.
   real(8), parameter :: pi = 4._8*atan2(1._8, 1._8)

   ! Determine run options
   call get_args

   ! Called in one-shot mode
   if (.not.read_stdin) then
      if (use_e) call e2a_phi ! Get a and phi from crack density
      call CIJ_hudson(vp,vs,rho,a,phi,vpi,vsi,rhoi,C,rho_eff)
      call write_C
   ! Taking a list of parameters from stdin
   else
      iostatus = 0
      do while (iostatus == 0)
         if (use_e) then
            read(*,*,iostat=iostatus) vp,vs,rho,e,vpi,vsi,rhoi
            call e2a_phi
         else
            read(*,*,iostat=iostatus) vp,vs,rho,a,phi,vpi,vsi,rhoi
         endif
         if (iostatus < 0) exit  ! EOF
         if (iostatus > 0) then
            write(0,'(a)') 'CIJ_Hudson: Problem reading parameters from stdin.'
            stop
         endif
         call CIJ_hudson(vp,vs,rho,a,phi,vpi,vsi,rhoi,C,rho_eff)
         call write_C
      enddo
   endif

contains

   subroutine usage
      write(0,'(a)') 'Usage:  CIJ_Hudson vp vs rho a phi vpi vsi rhoi', &
                     '        CIJ_Hudson vp vs rho e vpi vsi rhoi', &
                     '        CIJ_Hudson < [list of vp vs rho a phi vpi vsi rhoi]',&
                     '        CIJ_Hudson -e < [list of vp vs rho e vpi vsi rhoi]',&
                     'Inputs: vp,vs,rho:    matrix isotropic parameters',&
                     '        vpi,vsi,rhoi: inclusion  "         "',&
                     '    AND:',&
                     '        a:            aspect ratio of inclusions',&
                     '        phi:          volume fraction of inclusions',&
                     '    OR:',&
                     '        e:            crack number density',&
                     '        Theory valid where phi > 0.4*a OR e < 0.1.',&
                     '        Writes 36 ecs and rho to stdout.'
      stop
   end subroutine usage

   subroutine get_args
     integer :: narg
     narg = command_argument_count()

     select case (narg)
        case (0)
           read_stdin = .true.
        case (1)
           call get_command_argument(1, arg)
           if (arg /= '-e') call usage
           read_stdin = .true.
           use_e = .true.
        case (7)
           use_e = .true.
           call get_command_argument(1,arg);  read(arg,*) vp
           call get_command_argument(2,arg);  read(arg,*) vs
           call get_command_argument(3,arg);  read(arg,*) rho
           call get_command_argument(4,arg);  read(arg,*) e
           call get_command_argument(5,arg);  read(arg,*) vpi
           call get_command_argument(6,arg);  read(arg,*) vsi
           call get_command_argument(7,arg);  read(arg,*) rhoi
        case (8)
           call get_command_argument(1,arg);  read(arg,*) vp
           call get_command_argument(2,arg);  read(arg,*) vs
           call get_command_argument(3,arg);  read(arg,*) rho
           call get_command_argument(4,arg);  read(arg,*) a
           call get_command_argument(5,arg);  read(arg,*) phi
           call get_command_argument(6,arg);  read(arg,*) vpi
           call get_command_argument(7,arg);  read(arg,*) vsi
           call get_command_argument(8,arg);  read(arg,*) rhoi
        case default
           call usage
     end select
   end subroutine get_args

   subroutine write_C
      write(*,*) ((C(i,j),j=1,6),i=1,6),rho_eff
   end subroutine write_C

   subroutine e2a_phi
      a = 0.01_8
      phi = 4._8*pi*a*e/3._8
   end subroutine
end program
!-------------------------------------------------------------------------------


program CAMtst
    use temp_like
    integer::i,j
    real*8,dimension(0:2500)::cltt
    real*8::dum,A_ps_100,  A_ps_143, A_ps_217, A_cib_143, A_cib_217, A_sz,  &
       r_ps, r_cib, xi, A_ksz, cal0, cal1, cal2,z,rz
    real*8,dimension(4,5)::bmf
    character*100 like_file, sz143_file, ksz_file, tszxcib_file, beam_file

    like_file = "like_v6.1F"
    tszxcib_file = "sz_x_cib_template.dat"
    sz143_file = "tsz_143_eps0.50.dat"
    ksz_file = "cl_ksz_148_trac.dat"

    beam_file = "beamfile_v61f.dat"

    call like_init(like_file, sz143_file, tszxcib_file, ksz_file, beam_file)

    print *,1
    open(50,file = "ns61F.1.dat",form='formatted',status='unknown')
    print *,2
    do i=0,2500
        read(50,*) dum
        cltt(i) = dum/i/(i+1.)
    enddo
    read(50,*) A_ps_100
    read(50,*) A_ps_143
    read(50,*) A_ps_217
    read(50,*) A_cib_143
    read(50,*) A_cib_217
    read(50,*) A_sz
    read(50,*) r_ps
    read(50,*) r_cib
    read(50,*) xi
    read(50,*) A_ksz
    read(50,*) cal0
    read(50,*) cal1
    read(50,*) cal2
    do i=1,4
        do j=1,5
            read(50,*) bmf(i,j)
        enddo
    enddo
    read(50,*) z

    !A_ps_100 = 0
    !A_ps_143 = 0
    !A_ps_217 = 0
    !A_cib_143 = 0
    !A_cib_217 = 0
    !A_sz = 0
    !r_ps = 0
    !r_cib = 0
    !xi = 0
    !A_ksz = 0
    !cal0 = 1
    !cal1 = 1
    !cal2 = 1
    !bmf = 0
    call calc_like(rz,  cltt, A_ps_100,  A_ps_143, A_ps_217, A_cib_143, A_cib_217, A_sz,  &
       r_ps, r_cib, xi, A_ksz, cal0, cal1, cal2, bmf)

    print *,z,rz,z-rz

end program