program hmipt_2dsquare
  !########################################################
  !     Program  : HMIPT
  !     TYPE     : Main program
  !     PURPOSE  : Solve the Hubbard model using DMFT-IPT
  !     AUTHORS  : Adriano Amaricci
  !########################################################
  !LOCAL:
  USE DMFT_IPT
  USE SQUARE_LATTICE
  USE IOTOOLS
  implicit none

  logical                :: converged
  real(8)                :: n,Wbath,Vbath
  integer                :: i,Lk
  complex(8)             :: zeta
  complex(8),allocatable :: sigma(:),fg(:),fg0(:)
  real(8),allocatable    :: wr(:),wt(:),epsik(:),nk(:)
  !
  real(8),allocatable    :: bath_dens(:)
  complex(8),allocatable :: bath_sigma(:)

  call read_input("inputIPT.in")

  wbath=20.d0
  Vbath=0.d0
  do i=1,command_argument_count()
     nml_var=get_cmd_variable(i)
     if(nml_var%name=="VBATH")read(nml_var%value,*)vbath
     if(nml_var%name=="WBATH")read(nml_var%value,*)wbath
  end do

  !allocate functions:
  allocate(fg(L))
  allocate(sigma(L))
  allocate(fg0(L))
  allocate(wr(L))
  allocate(bath_dens(L),bath_sigma(L))

  !build freq. array
  wr = linspace(-wmax,wmax,L,mesh=fmesh)

  !build square lattice structure:
  Lk   = square_lattice_dimension(Nx)
  allocate(wt(Lk),epsik(Lk),nk(Lk))
  wt   = square_lattice_structure(Lk,Nx)
  epsik= square_lattice_dispersion_array(Lk,ts)

  !build bath
  call get_bath

  !dmft loop:
  sigma=zero ; iloop=0 ; converged=.false.
  do while (.not.converged)
     iloop=iloop+1
     write(*,"(A,i5)",advance="no")"DMFT-loop",iloop
     do i=1,L
        zeta = cmplx(wr(i),eps) - sigma(i) - bath_sigma(i)
        fg(i) = sum_overk_zeta(zeta,epsik,wt)
     enddo
     n   = sum(aimag(fg)*fermi(wr,beta))/sum(aimag(fg))
     fg0 = one/(one/fg + sigma)
     sigma= solve_ipt_sopt(fg0,wr)
     converged=check_convergence(sigma,eps_error,nsuccess,nloop)
     call splot("nVSiloop.ipt",iloop,n,append=TT)
  enddo
  call close_file("nVSiloop.ipt")
  call splot("DOS.ipt",wr,-aimag(fg)/pi,append=printf)
  call splot("G_realw.ipt",wr,fg,append=printf)
  call splot("G0_realw.ipt",wr,fg0,append=printf)
  call splot("Sigma_realw.ipt",wr,sigma,append=printf)
  nk = square_lattice_momentum_distribution(Lk)
  call splot("nkVSepsk.ipt",epsik,nk,append=printf)
contains

  function square_lattice_momentum_distribution(Lk) result(nk)
    integer            :: Lk
    integer,parameter  :: M=2048
    integer            :: ik,i
    type(matsubara_gf) :: gm,sm
    real(8)            :: nk(Lk),wm(M),w,Epot,docc,nd
    call allocate_gf(gm,M)
    call allocate_gf(sm,M)
    wm   = pi/beta*real(2*arange(1,M)-1,8)
    call get_matsubara_gf_from_DOS(wr,sigma,sm%iw,beta)
    do ik=1,Lk
       gm%iw=one/(xi*wm - epsik(ik) - sm%iw)
       call fftgf_iw2tau(gm%iw,gm%tau,beta)
       nk(ik)=-gm%tau(M)
    enddo
    call get_matsubara_gf_from_DOS(wr,fg,gm%iw,beta)
    call splot("Sigma_iw.ipt",wm,sm%iw)
    call splot("G_iw.ipt",wm,gm%iw)

    Epot=0.d0
    do i=1,M
       Epot=Epot+sm%iw(i)*gm%iw(i)
       !Epot=dot_product(conjg(sm%iw(:)),gm%iw(:))*2.d0/beta
    enddo
    Epot=Epot/beta*2.d0

    docc=0.25d0
    if(u>=1.d-3)docc = Epot/u + 0.25d0
    call splot("docc.ipt",temp,u,vbath**2/2.d0/wbath,epot,docc,append=printf)
  end function square_lattice_momentum_distribution

  subroutine get_bath
    complex(8) :: peso,bath_gf
    real(8)    :: w,wini,wfin,dw,de,nless,ngtr,en,epsimu(Lk),wfreq(L)
    integer    :: i,iw,im

    ! dw=2.d0*wbath/dble(Lk)
    ! do i=1,Lk
    !    epsimu(i)=-wbath+dble(i)*dw
    !    print*,epsimu(i)
    ! enddo
    epsimu = linspace(-2.d0*wbath,2.d0*wbath,Lk,mesh=de)
    wfreq  = linspace(-2.d0*wbath,2.d0*wbath,L,mesh=dw)

    bath_sigma=zero
    do i=1,L
       bath_gf=zero
       do im=1,Lk
          bath_gf=bath_gf+1.d0/(wr(i)+xi*eps-epsimu(im))/dble(Lk)
       enddo
       bath_sigma=vbath**2*bath_gf
       bath_dens=-aimag(bath_gf)/pi
    enddo

    ! do i=1,L
    !    w=wr(i)
    !    bath_dens(i) = heaviside(Wbath-abs(w))/(2.d0*Wbath)
    !    bath_gf      = log(abs((Wbath+w)/(Wbath-w)))-xi*pi*bath_dens(i)
    !    bath_sigma(i)= Vbath**2*bath_gf
    ! enddo
    call splot("bathDens.ipt",wfreq,bath_dens)
    call splot("bathSigma.ipt",wfreq,bath_sigma)
  end subroutine get_bath

end program hmipt_2dsquare


      subroutine compute_emph(n_phi,x,Nx,myzero,phys_bdy,res)
      implicit none
      integer i
      integer Nx
      real*8 myzero
      real*8 n_phi(Nx)
      real*8 x(Nx)
      integer phys_bdy(2)
      real*8 res(Nx)
      real*8 qb
      if (phys_bdy(1) .eq. 1) then
      do i=1, 1, 1
      qb = myzero * x(i) + 0.1D1
      res(i)=qb
      end do
      endif
      do i=2, Nx-1, 1
      qb = 0.1D1 / exp(n_phi(i)) ** 4
      res(i)=qb
      end do
      if (phys_bdy(2) .eq. 1) then
      do i=Nx, Nx, 1
      qb = myzero * x(i) + 0.1D1
      res(i)=qb
      end do
      endif
      END


MODULE ErrorAnalysisModule

  USE KindModule, ONLY: &
    DP, Zero, Third, Half, One, Three, &
    Pi, TwoPi, FourPi
  USE ProgramHeaderModule, ONLY: &
    ProgramName, &
    nDOF, nE, nX, nNodes
  USE ReferenceElementModule, ONLY: &
    NodeNumberTable1D3D
  USE MeshModule, ONLY: &
    MeshX, &
    NodeCoordinate
  USE RadiationFieldsModule, ONLY: &
    WeightsR, nSpecies, &
    uCR, nCR, iCR_N, iCR_G1, iCR_G2, iCR_G3

  IMPLICIT NONE
  PRIVATE

  REAL(DP), DIMENSION(:),             ALLOCATABLE :: One_Error
  REAL(DP), DIMENSION(:),             ALLOCATABLE :: Inf_Error
  REAL(DP), DIMENSION(:,:,:,:,:,:,:), ALLOCATABLE :: uCR_Exact

  PUBLIC :: InitializeErrorAnalysis
  PUBLIC :: FinalizeErrorAnalysis

CONTAINS

  SUBROUTINE InitializeErrorAnalysis( Time )

    REAL(DP), INTENT(in) :: Time

    ALLOCATE( One_Error(nCR) )
    ALLOCATE( Inf_Error(nCR) )
    ALLOCATE( uCR_Exact(nDOF,nE,nX(1),nX(2),nX(3),nCR,nSpecies) )

    SELECT CASE ( TRIM( ProgramName ) )

      CASE( 'SineWaveDiffusion1D' )

        CALL ComputeExactSolution_SineWaveDiffusion1D( Time )

      CASE( 'SineWaveDamping1D' )

        CALL ComputeExactSolution_SineWaveDamping1D( Time )

    END SELECT

  END SUBROUTINE InitializeErrorAnalysis

  SUBROUTINE FinalizeErrorAnalysis

    INTEGER :: iE, iX1, iX2, iX3, iCR, iS
    REAL(DP), DIMENSION(nDOF) :: Error

    One_Error = 0.0_DP
    Inf_Error = 0.0_DP

    DO iS = 1, nSpecies
      DO iCR = 1, nCR
        DO iX3 = 1, nX(3)
          DO iX2 = 1, nX(2)
            DO iX1 = 1, nX(1)
              DO iE = 1, nE

                Error(:) &
                  = ABS( uCR(:,iE,iX1,iX2,iX3,iCR,iS) &
                         - uCR_Exact(:,iE,iX1,iX2,iX3,iCR,iS) )

                One_Error(iCR) &
                  = One_Error(iCR) &
                      + DOT_PRODUCT( WeightsR, Error )

                Inf_Error(iCR) &
                  = MAX( Inf_Error(iCR), MAXVAL( Error ) )

              END DO
            END DO
          END DO
        END DO
      END DO
    END DO

    One_Error &
      = One_Error / REAL( nDOF * nE * PRODUCT( nX ) * nSpecies )

    WRITE(*,*)
    WRITE(*,'(A4,A)') '', '--- Error Analysis ---'
    WRITE(*,*)
    WRITE(*,'(A4,A10,3I5.4)') '', 'nX = ', nX
    WRITE(*,'(A4,A10,1I5.4)') '', 'nE = ', nE
    WRITE(*,'(A4,A10,1I5.4)') '', 'nNodes = ', nNodes
    WRITE(*,*)
    WRITE(*,'(A4,A12,A4,A12)') '', 'L_1', '', 'L_Inf'
    WRITE(*,*)
    WRITE(*,'(A4,A4,ES12.6E2,A4,ES12.6E2)') &
      '', 'N: ',  One_Error(iCR_N ), '', Inf_Error(iCR_N )
    WRITE(*,'(A4,A4,ES12.6E2,A4,ES12.6E2)') &
      '', 'G1: ', One_Error(iCR_G1), '', Inf_Error(iCR_G1)
    WRITE(*,'(A4,A4,ES12.6E2,A4,ES12.6E2)') &
      '', 'G2: ', One_Error(iCR_G2), '', Inf_Error(iCR_G2)
    WRITE(*,'(A4,A4,ES12.6E2,A4,ES12.6E2)') &
      '', 'G3: ', One_Error(iCR_G3), '', Inf_Error(iCR_G3)
    WRITE(*,*)

    DEALLOCATE( One_Error, Inf_Error, uCR_Exact )

  END SUBROUTINE FinalizeErrorAnalysis

  SUBROUTINE ComputeExactSolution_SineWaveDiffusion1D( Time )

    REAL(DP), INTENT(in) :: Time

    INTEGER  :: iS, iX1, iX2, iX3, iE
    INTEGER  :: iNodeX1, iNode
    REAL(DP) :: Kappa, X1

    Kappa = 1.0d+2

    DO iS = 1, nSpecies

      DO iX3 = 1, nX(3)
        DO iX2 = 1, nX(2)
          DO iX1 = 1, nX(1)
            DO iE = 1, nE

              DO iNode = 1, nDOF

                iNodeX1 = NodeNumberTable1D3D(2,iNode)
                X1      = NodeCoordinate( MeshX(1), iX1, iNodeX1 )

                uCR_Exact(iNode,iE,iX1,iX2,iX3,iCR_N,iS) &
                  = Three * SQRT( FourPi ) &
                    * ( One + EXP( - Pi**2 * Time / ( 27.0_DP * Kappa ) ) &
                              * SIN( Third * Pi * X1 ) )

                uCR_Exact(iNode,iE,iX1,iX2,iX3,iCR_G1,iS) &
                  = - ( Pi * SQRT( FourPi ) / ( Three * Kappa ) ) &
                    * EXP( - Pi**2 * Time / ( 27.0_DP * Kappa ) ) &
                    * COS( Third * Pi * X1 )

                uCR_Exact(iNode,iE,iX1,iX2,iX3,iCR_G2,iS) &
                  = Zero

                uCR_Exact(iNode,iE,iX1,iX2,iX3,iCR_G3,iS) &
                  = Zero

              END DO

            END DO
          END DO
        END DO
      END DO

    END DO

  END SUBROUTINE ComputeExactSolution_SineWaveDiffusion1D

  SUBROUTINE ComputeExactSolution_SineWaveDamping1D( Time )

    REAL(DP), INTENT(in) :: Time

    INTEGER  :: iS, iX1, iX2, iX3, iE
    INTEGER  :: iNodeX1, iNode
    REAL(DP) :: Kappa, X1

    Kappa = 1.0d+1

    DO iS = 1, nSpecies

      DO iX3 = 1, nX(3)
        DO iX2 = 1, nX(2)
          DO iX1 = 1, nX(1)
            DO iE = 1, nE

              DO iNode = 1, nDOF

                iNodeX1 = NodeNumberTable1D3D(2,iNode)
                X1      = NodeCoordinate( MeshX(1), iX1, iNodeX1 )

                uCR_Exact(iNode,iE,iX1,iX2,iX3,iCR_N,iS) &
                  = ( One + Half * SIN( TwoPi * ( X1 - Time ) ) ) &
                      * EXP( - Kappa * Time )

                uCR_Exact(iNode,iE,iX1,iX2,iX3,iCR_G1,iS) &
                  = uCR_Exact(iNode,iE,iX1,iX2,iX3,iCR_N,iS)

                uCR_Exact(iNode,iE,iX1,iX2,iX3,iCR_G2,iS) &
                  = Zero

                uCR_Exact(iNode,iE,iX1,iX2,iX3,iCR_G3,iS) &
                  = Zero

              END DO

            END DO
          END DO
        END DO
      END DO

    END DO

  END SUBROUTINE ComputeExactSolution_SineWaveDamping1D

END MODULE ErrorAnalysisModule


      call test
      end

      subroutine test
!  ********************************************************************
!  ********************************************************************
      use ddmodule
      use avh_olo_forIREGI
      implicit none
      integer :: nn
      character(4) :: next
      real(kind(1d0)) :: mu
!
      call ltini !|LTVSNge26=yesyes
!#      call ffini !|LTVSNge26=yesno
      call olo_onshell( 1d-6 )
!      call olo_unit( 6 ,'printall' )
      call olo_precision(34)
      write(*,*) 'dp precision:',olo_dp_precision()
      write(*,*) 'dd precision:',olo_dd_precision()
!
      next = 'next'
      do while (next.eq.'next')
        read(5,*) nn ,mu
        call olo_scale( mu )
        call setmudim( mu*mu ) !|LT=yes
        if     (nn.eq.2) then
          call test_2
        elseif (nn.eq.3) then
          call test_3
        else!if(nn.eq.4) then
          call test_4
        endif
        read(5,*) next
      enddo
      end

      subroutine test_4
!  ********************************************************************
!  ********************************************************************
      use ddmodule
      use avh_olo_forIREGI
      implicit none
      complex(kind(1d0)) :: p1,p2,p3,p4,p12,p23,m1,m2,m3,m4
      complex(kind(1d0)) :: zLT,D0C,zdp(0:2)
      type(dd_complex) :: p1_dd,p2_dd,p3_dd,p4_dd,p12_dd,p23_dd
      type(dd_complex) :: m1_dd,m2_dd,m3_dd,m4_dd,zdd(0:2)
!
      read(5,*) p1
      read(5,*) p2
      read(5,*) p3
      read(5,*) p4
      read(5,*) p12
      read(5,*) p23
      read(5,*) m1
      read(5,*) m2
      read(5,*) m3
      read(5,*) m4
      write(6,101) 'p1 : (',real(p1),',',aimag(p1),')'
      write(6,101) 'p2 : (',real(p2),',',aimag(p2),')'
      write(6,101) 'p3 : (',real(p3),',',aimag(p3),')'
      write(6,101) 'p4 : (',real(p4),',',aimag(p4),')'
      write(6,101) 'p12: (',real(p12),',',aimag(p12),')'
      write(6,101) 'p23: (',real(p23),',',aimag(p23),')'
      write(6,101) 'm1 : (',real(m1),',',aimag(m1),')'
      write(6,101) 'm2 : (',real(m2),',',aimag(m2),')'
      write(6,101) 'm3 : (',real(m3),',',aimag(m3),')'
      write(6,101) 'm4 : (',real(m4),',',aimag(m4),')'
  101 format(a6,d31.24,a1,d31.24,a1)
!
      p1_dd =p1
      p2_dd =p2
      p3_dd =p3
      p4_dd =p4
      p12_dd=p12
      p23_dd=p23
      m1_dd =m1
      m2_dd =m2
      m3_dd =m3
      m4_dd =m4
      call olo( zdp ,p1,p2,p3,p4,p12,p23 ,m1,m2,m3,m4 )
      call olo( zdd ,p1_dd,p2_dd,p3_dd,p4_dd,p12_dd,p23_dd
     &              ,m1_dd,m2_dd,m3_dd,m4_dd )
!
      zLT = dcmplx(0d0)
      if (zdd(1).eq.dcmplx(0d0))                      !|LT=yes
     &  zLT = D0C( p1,p2,p3,p4,p12,p23 ,m1,m2,m3,m4 ) !|LT=yes
!
      write(6,'(a10)') ' eps^( 0):'
      if (zLT.ne.dcmplx(0d0))
     &write(6,'(a8,2d24.16)') '     LT:',zLT
      write(6,'(a8,2d24.16)') '    olo:',zdp(0)
      call ddwrite(6,zdd(0))
      write(6,'(a10)') ' eps^(-1):'
      write(6,'(a8,2d24.16)') '    olo:',zdp(1)
      call ddwrite(6,zdd(1))
      write(6,'(a10)') ' eps^(-2):'
      write(6,'(a8,2d24.16)') '    olo:',zdp(2)
      call ddwrite(6,zdd(2))
      write(6,*)
!
      end

      subroutine test_3
!  ********************************************************************
!  ********************************************************************
      use ddmodule
      use avh_olo_forIREGI
      implicit none
      complex(kind(1d0)) :: p1,p2,p3,m1,m2,m3 ,zLT,C0C,zdp(0:2)
      type(dd_complex) :: p1_dd,p2_dd,p3_dd
      type(dd_complex) :: m1_dd,m2_dd,m3_dd,zdd(0:2)
!
      read(5,*) p1
      read(5,*) p2
      read(5,*) p3
      read(5,*) m1
      read(5,*) m2
      read(5,*) m3
      write(6,101) 'p1 : (',real(p1),',',aimag(p1),')'
      write(6,101) 'p2 : (',real(p2),',',aimag(p2),')'
      write(6,101) 'p3 : (',real(p3),',',aimag(p3),')'
      write(6,101) 'm1 : (',real(m1),',',aimag(m1),')'
      write(6,101) 'm2 : (',real(m2),',',aimag(m2),')'
      write(6,101) 'm3 : (',real(m3),',',aimag(m3),')'
  101 format(a6,d31.24,a1,d31.24,a1)
!
      p1_dd=p1
      p2_dd=p2
      p3_dd=p3
      m1_dd=m1
      m2_dd=m2
      m3_dd=m3
      call olo( zdp ,p1,p2,p3 ,m1,m2,m3 )
      call olo( zdd ,p1_dd,p2_dd,p3_dd ,m1_dd,m2_dd,m3_dd )
!
      zLT = dcmplx(0d0)
      if (zdd(1).eq.dcmplx(0d0))        !|LT=yes
     &  zLT = C0C( p1,p2,p3 ,m1,m2,m3 ) !|LT=yes
!
      write(6,'(a10)') ' eps^( 0):'
      if (zLT.ne.dcmplx(0d0))
     &write(6,'(a8,2d24.16)') '     LT:',zLT
      write(6,'(a8,2d24.16)') '    olo:',zdp(0)
      call ddwrite(6,zdd(0))
      write(6,'(a10)') ' eps^(-1):'
      write(6,'(a8,2d24.16)') '    olo:',zdp(1)
      call ddwrite(6,zdd(1))
      write(6,'(a10)') ' eps^(-2):'
      write(6,'(a8,2d24.16)') '    olo:',zdp(2)
      call ddwrite(6,zdd(2))
      write(6,*)
!
      end

      subroutine test_2
!  ********************************************************************
!  ********************************************************************
      use ddmodule
      use avh_olo_forIREGI
      implicit none
      complex(kind(1d0)) :: p1,m1,m2
      complex(kind(1d0)) :: b0LT,b1LT,b00LT,b11LT,b0C,b1C,b00C,b11C
      type(dd_complex) :: m1_dd,m2_dd,p1_dd
      type(dd_complex) :: rslt(0:2),b0(0:2),b1(0:2),b00(0:2),b11(0:2)
!
      read(5,*) p1
      read(5,*) m1
      read(5,*) m2
      write(6,101) 'p1 : (',real(p1),',',aimag(p1),')'
      write(6,101) 'm1 : (',real(m1),',',aimag(m1),')'
      write(6,101) 'm2 : (',real(m2),',',aimag(m2),')'
  101 format(a6,d31.24,a1,d31.24,a1)
!
      p1_dd = p1
      m1_dd = m1
      m2_dd = m2
      call olo_b11( b11,b00,b1,b0 ,p1_dd ,m1_dd,m2_dd )
      call olo_b0( rslt ,p1_dd ,m1_dd,m2_dd )
      b0LT  = B0C(  p1 ,m1,m2 ) !|LT=yes
      b1LT  = B1C(  p1 ,m1,m2 ) !|LT=yes
      b00LT = B00C( p1 ,m1,m2 ) !|LT=yes
      b11LT = B11C( p1 ,m1,m2 ) !|LT=yes
!
      write(6,'(a10)') ' eps^( 0):'
      write(6,'(a8,2d24.16)') 'b0   LT:',b0LT !|LT=yes
      write(6,'(a8,2d24.16)') '    olo:'
      call ddwrite(6,rslt(0))
      write(6,'(a8,2d24.16)') '    olo:'
      call ddwrite(6,b0(0))
      write(6,'(a8,2d24.16)') 'b1   LT:',b1LT !|LT=yes
      write(6,'(a8,2d24.16)') '    olo:'
      call ddwrite(6,b1(0))
      write(6,'(a8,2d24.16)') 'b00  LT:',b00LT !|LT=yes
      write(6,'(a8,2d24.16)') '    olo:'
      call ddwrite(6,b00(0))
      write(6,'(a8,2d24.16)') 'b11  LT:',b11LT !|LT=yes
      write(6,'(a8,2d24.16)') '    olo:'
      call ddwrite(6,b11(0))
      write(6,'(a10)') ' eps^(-1):'
      write(6,'(a8,2d24.16)') '    olo:'
      call ddwrite(6,b0(1))
      write(6,'(a10)') ' eps^(-2):'
      write(6,'(a8,2d24.16)') '    olo:'
      call ddwrite(6,b0(2))
      write(6,*)
!

!
      end


	SUBROUTINE MT_PRRM ( stprrm, itrace, precrm, iret )
C************************************************************************
C* MT_PRRM							        *
C*								        *
C* This subroutine will decode precipitation amount in hundredths of	*
C* inches.								*
C*								        *
C* MT_PRRM ( STPRRM, ITRACE, PRECRM, IRET )    	     		        *
C*								        *
C* Input parameters:						        *
C*	STPRRM		CHAR*		Precip string			*
C*	ITRACE		LOGICAL		Flag for trace precip check     *
C*									*
C* Output parameters:						        *
C*	PRECRM		REAL		Precip amount		        *
C*	IRET		INTEGER		Return code		        *
C*					   0 = normal return 	        *
C*					  -2 = decode error 	        *
C*								        *
C**								        *
C* Log:								        *
C* D. Kidwell/NCEP	11/95	Original author		                *
C* D. Kidwell/NCEP	 9/96	Modified for indeterminate & trace amts *
C* K. Tyle/GSC	 	 1/97	Modify for trace amounts; reorganize	*
C*				header and comments			*
C* D. Kidwell/NCEP       6/97   Replaced ST_CRNM with ST_INTG           *
C* D. Kidwell/NCEP	 4/98   Changed error handling; set flag value  *
C*				for indeterminate amt; reinstated trace *
C************************************************************************
	INCLUDE 	'GEMPRM.PRM'
	INCLUDE 	'mtcmn.cmn'
C*
	CHARACTER*(*) 	stprrm
	LOGICAL		itrace
C------------------------------------------------------------------------
	iret = 0
C
C*      Check for indeterminate amount.
C
	IF ( stprrm ( 2:5 ) .eq. '////' ) THEN
	    precrm = -99.
	  ELSE
C
C*	    Decode precip amount.
C
	    CALL ST_INTG ( stprrm ( 2:5 ), iprec, iret )
	    IF ( iret. eq. 0 ) THEN
		precrm = FLOAT ( iprec ) * .01
C
C*		Interface value for trace precip is -1.
C
		IF ( itrace ) THEN
		    IF ( precrm .eq. 0. ) precrm = -1.
		END IF
	      ELSE
		precrm = RMISSD
	    END IF
	END IF
C*
	RETURN
	END


!==============================================================================!
  subroutine Calculate_Mass_Flow_Rate(grid)
!------------------------------------------------------------------------------!
!   Calculate mass flow rate at cell faces based on velocities only.           !
!----------------------------------[Modules]-----------------------------------!
  use Flow_Mod
  use Grid_Mod
!------------------------------------------------------------------------------!
  implicit none
!---------------------------------[Arguments]----------------------------------!
  type(Grid_Type) :: grid
!-----------------------------------[Locals]-----------------------------------!
  integer           :: s, c1, c2
  real              :: fs
!==============================================================================!

  !-------------------------------------------------!
  !   Calculate the mass fluxes on the cell faces   !
  !-------------------------------------------------!
  do s=1, grid % n_faces
    c1 = grid % faces_c(1,s)
    c2 = grid % faces_c(2,s)
    fs = grid % f(s)

    ! Face is inside the domain
    if( c2 > 0 ) then

      ! Extract the "centred" pressure terms from cell velocities
      flux(s) = density *                                                 &
              (   (fs * u % n(c1) + (1.0-fs) * u % n(c2)) * grid % sx(s)  &
                + (fs * v % n(c1) + (1.0-fs) * v % n(c2)) * grid % sy(s)  &
                + (fs * w % n(c1) + (1.0-fs) * w % n(c2)) * grid % sz(s) )

    ! Face is on the boundary
    else
      flux(s) = density *                   &
              (   u % n(c1) * grid % sx(s)  &
                + v % n(c1) * grid % sy(s)  &
                + w % n(c1) * grid % sz(s) )
    end if

  end do

  end subroutine


!c***************************************************************

      subroutine offsetpoly()

!c***************************************************************
!c*
!c*  Estimates the offset polynomial to be used for resampling
!c*
!c*
!c****************************************************************

      use offsetpolyState
      use fortranUtils
      implicit none

!c    PARAMETER STATEMENTS:

      integer    NPP, MP
      parameter (NPP=10)

!c    LOCAL VARIABLES:

      real*8, allocatable ::r_ranpos(:),r_azpos(:),r_sig(:),r_off(:)
      real*8, allocatable :: r_coef(:), r_w(:)
      real*8, allocatable :: r_u(:,:), r_v(:,:)
      real*8 r_chisq, r_ro, rmean, rsq
      integer i,j, i_numpnts

      real*4 t0, t1

!c    COMMON BLOCKS:

      integer i_fitparam(NPP),i_coef(NPP)
      external poly_funcs
      common /fred/ i_fitparam,i_coef

      t0 = secnds(0.0)

!c    ARRAY ALLOCATIONS:
      MP = numOffsets

      allocate(r_ranpos(MP))
      allocate(r_azpos(MP))
      allocate(r_sig(MP))
      allocate(r_off(MP))
      allocate(r_coef(NPP))
      allocate(r_u(MP,NPP))
      allocate(r_v(NPP,NPP))
      allocate(r_w(NPP))

!c    reading offsets data file (note NS*NPM is maximal number of pixels)

      i_numpnts = numOffsets
      ! also convert the snr to the format used here. there my be division by zero that i guess fortran can handle (gives  +Infinity)
      do j=1,i_numpnts           !read the offset data file
            r_ranpos(j) = r_ranposV(j)
            r_azpos(j) = r_azposV(j)
            r_off(j) = r_offV(j)
            r_sig(j) = 1.0 + 1.d0/r_sigV(j)
      end do

!c    make two two dimensional quadratic fits for the offset fields
!c    one of the azimuth offsets and the other for the range offsets

      do i = 1 , NPP
         r_coef(i) = 0.
         i_coef(i) = 0
      end do

      do i=1,i_ma
         i_coef(i) = i
      enddo

!c    azimuth offsets as a function range and azimuth
!     do i=1,i_numpnts
!        print *,r_ranpos(i),r_azpos(i),r_sig(i), r_off(i)
!     end do
!     print *, 'Fit: ', i_fitparam
!     print *, 'Coef: ', i_coef

      call svdfit(r_ranpos,r_azpos,r_off,r_sig,i_numpnts, &
          r_coef,i_ma,r_u,r_v,r_w,MP,NPP,r_chisq)

      print *, 'Fit sigma = ',sqrt(r_chisq/i_numpnts)

      rmean= 0.
      rsq= 0.
      do i=1,i_numpnts
         r_ro = r_coef(1) + r_azpos(i)*(r_coef(3) + &
             r_azpos(i)*(r_coef(6) + r_azpos(i)*r_coef(10))) + &
             r_ranpos(i)*(r_coef(2) + r_ranpos(i)*(r_coef(5) + &
             r_ranpos(i)*r_coef(9))) + &
             r_ranpos(i)*r_azpos(i)*(r_coef(4) + r_azpos(i)*r_coef(7) + &
             r_ranpos(i)*r_coef(8))
         rmean = rmean + (r_off(i)-r_ro)
         rsq = rsq + (r_off(i)-r_ro)**2
      enddo

      rmean = rmean / i_numpnts
      rsq = sqrt(rsq/i_numpnts - rmean**2)
      print *,'mean, sigma offset residual (pixels): ',rmean, rsq

      print *, 'Constant term =            ',r_coef(1)
      print *, 'Range Slope term =         ',r_coef(2)
      print *, 'Azimuth Slope =            ',r_coef(3)
      print *, 'Range/Azimuth cross term = ',r_coef(4)
      print *, 'Range quadratic term =     ',r_coef(5)
      print *, 'Azimuth quadratic term =   ',r_coef(6)
      print *, 'Range/Azimuth^2   term =   ',r_coef(7)
      print *, 'Azimuth/Range^2 =          ',r_coef(8)
      print *, 'Range cubic term =         ',r_coef(9)
      print *, 'Azimuth cubic term =       ',r_coef(10)

      t1 = secnds(t0)
      print *,  'XXX time: ', t1-t0

      do i=1,i_ma
        r_polyV(i) = r_coef(i)
      end do

      deallocate(r_ranpos)
      deallocate(r_azpos)
      deallocate(r_sig)
      deallocate(r_off)
      deallocate(r_coef)
      deallocate(r_u)
      deallocate(r_v)
      deallocate(r_w)
      end


***
      SUBROUTINE BSETID(kw,mass,mc,menv,r,rc,renv,lum,ospin,k2,q,
     &                  sep,ecc,oorb,delet1,dspin,eqspin,djt)
*
      implicit none
      integer kw
      real*8 mass,mc,menv,r,rc,renv,lum,ospin,k2,q
      real*8 sep,ecc,oorb,delet1,dspin,eqspin,djt
      real*8 renvk,rg2,k3
      PARAMETER(K3=0.21D0)
      real*8 ecc2,omecc2,sqome2,sqome3,raa2,raa6,twopi
      real*8 f1,f2,f3,f4,f5,tc,fc,tcqr,ttid
*
      ecc2 = ecc*ecc
      omecc2 = 1.d0 - ecc2
      sqome2 = SQRT(omecc2)
      sqome3 = sqome2**3
*
      raa2 = (r/sep)**2
      raa6 = raa2**3
      twopi = 2.d0*ACOS(-1.d0)
*
      f5 = 1.d0+ecc2*(3.d0+ecc2*0.375d0)
      f4 = 1.d0+ecc2*(1.5d0+ecc2*0.125d0)
      f3 = 1.d0+ecc2*(3.75d0+ecc2*(1.875d0+ecc2*7.8125d-02))
      f2 = 1.d0+ecc2*(7.5d0+ecc2*(5.625d0+ecc2*0.3125d0))
      f1 = 1.d0+ecc2*(15.5d0+ecc2*(31.875d0+ecc2*(11.5625d0
     &     +ecc2*0.390625d0)))
*
      if((kw.eq.1.and.mass.ge.1.25).or.kw.eq.4.or.kw.eq.7)then
         tc = 1.592d-09*(mass**2.84)
         fc = 1.9782d+04*SQRT((mass*r*r)/sep**5)
     &        *tc*(1.d0+q)**(5.d0/6.d0)
         tcqr = fc*q*raa6
         rg2 = k2
      elseif(kw.le.9)then
         renvk = MIN(renv,r-rc)
         renvk = MAX(renvk,1.0d-10)
         tc = (menv*renvk*(r-0.5d0*renvk)/
     &        (3.d0*lum))**(1.d0/3.d0)
         tc = 0.4311D0*tc
         ttid = twopi/(1.0d-10 + ABS(oorb - ospin))
         fc = MIN(1.d0,(ttid/(2.d0*tc)**2))
         tcqr = 2.d0*fc*q*raa6*menv/(21.d0*tc*mass)
         rg2 = (k2*(mass-mc))/mass
      else
         fc = 7.33d-09*(lum/mass)**(5.d0/7.d0)
         tcqr = fc*q*q*raa2*raa2/(1.d0+q)
         rg2 = k3
      endif
*
      delet1 = 27.d0*tcqr*(1.d0+q)*raa2*(ecc/sqome2**13)*
     &         (f3 - (11.d0/18.d0)*sqome3*f4*ospin/oorb)
      dspin = (3.d0*q*tcqr/(rg2*omecc2**6))*
     &        (f2*oorb - sqome3*f5*ospin)
      eqspin = oorb*f2/(sqome3*f5)
      djt = (k2*(mass-mc)*r*r + k3*rc*rc*mc)*dspin
*
      RETURN
      END
***


      subroutine kostia(
     i                  rwdth)
c
c     + + + PURPOSE + + +
c     This subprogram calibrates modified Kostiakov infiltration
c     equation constants for a series of time-infiltration rate pairs
c     where infiltration rate is calculated using the Green and Ampt
c     infiltration function.
c
c     Called from FURROW
c     Author(s): E. R. Kottwitz
c     Reference in User Guide: Chapter 12
c
c     Changes:
c
c     Version: This module recoded from WEPP Version 93.06.
c     Date recoded: 07/07/93.
c     Recoded by: Charles R. Meyer.
c
c     + + + PARAMETERS + + +
      include 'pmxcrp.inc'
      include 'pmxhil.inc'
      include 'pmxnsl.inc'
      include 'pmxpln.inc'
      include 'pmxpnd.inc'
      include 'pmxres.inc'
      include 'pmxsrg.inc'
      include 'pmxtil.inc'
      include 'pmxtls.inc'
      include 'pntype.inc'
      include 'pxstep.inc'
c
c     + + + ARGUMENT DECLARATIONS + + +
      real      rwdth
c
c     + + + ARGUMENT DEFINITIONS + + +
c     rwdth  - assumed row width of current overland flow element (m)
c
c     + + + COMMON BLOCKS + + +
      include 'cavepar.inc'
c     modify: aveks(mxplan),avesm(mxplan)
c
      include 'ccrpout.inc'
c       read: rh(mxplan)
c
      include 'cdist2.inc'
c       read: slplen(mxplan)
c
      include 'ciraflo.inc'
c       read: aqcnst,aqexp,botwid,sidslp
c
      include 'cirfur2.inc'
c       read: dtheta(mxplan),qspply(mxplan),surge,timtot
c     modify: srg
c
      include 'cirinfl.inc'
c     modify: kostf
c      write: kosta,kostk
c
      include 'cirriga.inc'
c       read: irofe,noirr
c
      include 'cparame.inc'
c       read: ks(mxplan),sm(mxplan)
c
      include 'cstruc.inc'
c       read: iplane
c
c     + + + LOCAL VARIABLES + + +
      integer   furint,i,nsteps
      real      avgdp2,deltai,denom,diydt,dlower,dupper,horcon,hormax,
     1          infadj,infil2,infilh(50),infilt,infilv(0:50),infold,
     1          inftmx,irsum1,irsum2,irsum3,irsum4,irsum5,kostfd,natlni,
     1          natlnt,piovr2,qmaxl,qmaxu,testf,time(50),timhmx,vertmx
      real      tmpvr1,tmpvr2,tmpvr3,tmpvr4
c
c     + + + LOCAL DEFINITIONS + + +
c     furint - irrigated furrow interval (1: all furrows irrigated, 2:
c              every other furrow irrigated)
c     i      - do loop counter
c     nsteps - number of infiltration increments
c     avgdp2 - two times the average of dlower and dupper (m)
c     deltai - 1-D vertical distance to the wetting front increment (m)
c     denom  - coefficient used as a denominator in calculating kosta
c              kostk (m**4)
c     diydt  - time derivative if vertical advance distance (m/s)
c     dlower - flow depth at lower end of current OFE for maximum of
c              furrow supply rates assuming steady state infiltration
c              (m)
c     dupper - flow depth at upper end of current OFE for maximum of
c              furrow supply rates (m)
c     horcon - horizontal infiltration constant equal to
c              (2*Ke*Ns)**.5/(change in soil moisture content) (m/s**.5)
c     hormax - maximum horizontal distance to the wetting front (m)
c     infadj - 2-D cumulative infiltration minus Ks*(infiltration
c              opportunity time)*(row spacing) (m**2)
c     infil2 - infilt*infilt (m**4)
c     infilh - 1-D horizontal distance to the wetting front (m)
c     infilt - 2-D cumulative infiltration (volume of water infiltrated
c              per unit furrow length) (m**2)
c     infilv - 1-D vertical distance to the wetting front as determined
c              by SR NEWTON (m)
c     infold - estimate of infiltration sent to SR NEWTON (m)
c     inftmx - cumulative 2-D infiltration at t=timhmx (m**2)
c     irsum1 - summation of infil2 (m**4)
c     irsum2 - summation of infil2*natlnt (m**4)
c     irsum3 - summation of infil2*natlnt**2 (m**4)
c     irsum4 - summation of infil2*natlni (m**4)
c     irsum5 - summation of infil2*natlnt*natlni (m**4)
c     kostfd - estimate of kostf using derivative of 2-D cumulative
c              infiltration function (m**2/s)
c     natlni - natural logarithm of infilt
c     natlnt - natural logarithm of time
c     piovr2 - one-half the ratio of circle area to square of its radius
c     qmaxl  - maximum flow rate at the lower end of the OFE (m**3/s)
c     qmaxu  - maximum furrow supply rate (m**3/s)
c     testf  - estimate of new value of kostf if old value was too large
c              (m**2/s)
c     time   - infiltration opportunity time (s)
c     timhmx - time when horizontal wetting fronts meet (s)
c     vertmx - vertical distance to the wetting front at t=timhmx (m)
c     tmpvr1 - variable to reduce number of calculations (m/s)
c     tmpvr2 - variable to reduce number of calculations (m)
c     tmpvr3 - variable to reduce number of calculations (m)
c
c     + + + SUBROUTINES CALLED + + +
c     newton
c
c     + + + DATA INITIALIZATIONS + + +
      data      nsteps/50/,piovr2/1.57080/
c
c     + + + OUTPUT FORMATS + + +
 2001 format(' *** WARNING ***',/,
     1       'Furrow irrigation flow depth (',f5.1,' mm) exceeds',
     1       ' ridge height (',f5.1,' mm).',/,
     1       ' *** WARNING ***',/)
c
c     + + + END SPECIFICATIONS + + +
c
c     Assume furint = 1.  User will eventually be required to provide
c     this information.
c
      furint = 1
c
c     Initialize variables
c
      infilv(0)=0.0
      inftmx = 0.0
      vertmx = 0.0
      kostfd = 1.0
      aveks(iplane) = ks(iplane)
      avesm(iplane) = sm(iplane)
      tmpvr1 = aveks(iplane)/dtheta(iplane)
      tmpvr2 = avesm(iplane)/dtheta(iplane)
      tmpvr3 = dtheta(iplane)*float(furint)*rwdth
c
c     Determine maximum furrow inflow rate
c
      qmaxu = qspply(1)
      if(noirr.eq.2 .and. surge.ge.2)then
        do 100  srg = 2, surge
          qmaxu = max(qmaxu,qspply(srg))
  100   continue
      endif
c
c     Determine flow depth at the upper end of the OFE
c
      if(sidslp.gt.0.001)then
        dupper = (sqrt(botwid**2+4.0*sidslp*(qmaxu/aqcnst)**(1.0/aqexp))
     1          -botwid)/2.0/sidslp
      else
        dupper = (qmaxu/aqcnst)**(1.0/aqexp)/botwid
      endif
c
c     Print warning to user if dupper exceeds ridge height
c
      if(iplane.eq.irofe .and. dupper.gt.rh(iplane))
     1       write (6,2001)dupper*1000.0,rh(iplane)*1000.0
c
c     Determine horizontal infiltration constant
c
c      ---- The value in parenthesis is from that portion of WEPP
c           Equation 12.3 also in parenthesis.  HORCON is the square
c           root of it.
c           Note that avesm(iplane) = (total head loss)*dtheta(iplane).
c
      horcon = sqrt(2.0*tmpvr1*tmpvr2)
c
c     Determine maximum horizontal wetting distance and time when
c     horizontal wetting fronts meet
c
c      ---- (WEPP Equation 12.5, with W = float(furint)*rwdth)
      hormax = (float(furint)*rwdth-botwid)*0.5
      timhmx = (hormax/horcon)**2
c
c     Determine vertical advance distance at t=timhmx
c
      infold = timhmx*aveks(iplane)
      call newton(timhmx,infold,aveks(iplane),avesm(iplane),vertmx)
      vertmx = vertmx/dtheta(iplane)
c
c     Determine 1-D vert. distance to the wetting front for t=timtot
c
      infold = timtot*aveks(iplane)
      call newton(timtot,infold,aveks(iplane),avesm(iplane),
     1            infilv(nsteps))
      infilv(nsteps) = infilv(nsteps)/dtheta(iplane)
c
c     Determine deltai
c
      deltai = infilv(nsteps)/float(nsteps)
c
c     Determine 1-D horizontal distance to the wetting front for
c     t=timtot
c
      if(timtot.lt.timhmx)then
c
c       ---- (WEPP Equation 12.3)
        infilh(nsteps) = horcon*sqrt(timtot)
      else
        infilh(nsteps) = hormax
      endif
c
c     Determine infiltration rate at t=timtot using 2-D infiltration
c     function
c
c      ---- (WEPP Equation 4.2.3) divided by change in soil water
c           content
      diydt = tmpvr1*(1.0+tmpvr2/infilv(nsteps))
c
c     If the following is true then horizontal wetting fronts have not
c     met.
c
      if(timtot.le.timhmx)then
c
c        ---- Time derivative of WEPP Equation 12.1
        kostfd = (infilh(nsteps)*(dupper/timtot+piovr2*infilv(nsteps)/
     1           (2.0*timtot)+piovr2*diydt)+botwid*diydt)*dtheta(iplane)
      else
        kostfd = diydt*tmpvr3
      endif
c
c     Calculate values for elements of infilh, infilv, and time arrays.
c     Also determine the minimum infiltration rate.
c
      do 400  i = 1, nsteps-1
c
c       Determine 1-D vertical distance to the wetting front
c
        infilv(i) = infilv(i-1)+deltai
c
c       Calculate time required for infiltration depth infilv(i)
c
c        ---- (WEPP Equation 12.2, solved for time (tau in 12.2))
        time(i) = infilv(i)/tmpvr1-avesm(iplane)/aveks(iplane)*
     1            log(1.0+infilv(i)/tmpvr2)
c
c       Determine 1-D horizontal advance distance to the wetting front
c
        if(time(i).lt.timhmx)then
c
c         ---- (WEPP Equation 12.3)
          infilh(i) = horcon*sqrt(time(i))
        else
          infilh(i) = hormax
        endif
c
c       Determine kostf using derivative of 2-D cumulative infiltration
c       function
c
c        ---- (WEPP Equation 4.2.3) divided by change in soil water
c             content
        diydt = tmpvr1*(1.0+tmpvr2/infilv(i))
        if(timtot.le.timhmx)then
c
c          ---- Time derivative of WEPP Equation 12.1
          kostf = (infilh(i)*(dupper/time(i)+piovr2*infilv(i)/(2.0*
     1            time(i))+piovr2*diydt)+botwid*diydt)*dtheta(iplane)
        else
          kostf = diydt*tmpvr3
        endif
        kostfd = min(kostf,kostfd)
  400 continue
c
c     Determine kostf using wetted perimeter at the upper end of the
c     furrow
c
      kostf = aveks(iplane)*(botwid+2.0*dupper*sqrt(1.0+sidslp**2))
c
c     Set kostf to minimum of possible values
c
      kostf = min(kostf,kostfd)
c
c     Determine maximum flow rate at the lower end of the OFE
c
   10 qmaxl = qmaxu-slplen(iplane)*kostf
c
c     Determine 2*(average depth of flow) for steady state conditions
c
      if(qmaxl.gt.0.0)then
        if(sidslp.gt.0.001)then
          dlower = (sqrt(botwid**2+4.0*sidslp*(qmaxl/aqcnst)**(1.0/
     1             aqexp))-botwid)/2.0/sidslp
        else
          dlower = (qmaxl/aqcnst)**(1.0/aqexp)/botwid
        endif
        avgdp2 = dupper+dlower
      else
        avgdp2 = dupper
      endif
c
c     Determine 2-D cumulative infiltration for t=timtot
c
      if(timtot.le.timhmx)then
c
c        ---- (WEPP Equation 12.1)
        infilt = (avgdp2*infilh(nsteps)+botwid*infilv(nsteps)+
     1           piovr2*infilh(nsteps)*infilv(nsteps))*dtheta(iplane)
      else
        inftmx = (avgdp2*hormax+botwid*vertmx+piovr2*hormax*vertmx)*
     1           dtheta(iplane)
        infilt = inftmx+(infilv(nsteps)-vertmx)*float(furint)*rwdth*
     1           dtheta(iplane)
      endif
c
c     Adjust 2-D cumulative infiltration to eliminate steady state
c     infiltration rate
c
      infadj = infilt-kostf*timtot
c
c     Adjust kostf if infadj is less than 0.000001 m**2
c
      if(infadj.lt.0.000001)then
        infadj = 0.000001
        testf = (infilt-infadj)/timtot
        if(testf.lt.0.999*kostf)then
          kostf = testf
        else
          kostf = 0.999*kostf
        endif
        goto 10
      endif
c
c     Determine values for functions of cumulative infiltration and time
c
      natlni = log(infadj)
      infil2 = infadj**2
      natlnt = log(timtot)
c
c     Calculate summations
c
      irsum1 = infil2
      irsum2 = infil2*natlnt
      irsum3 = irsum2*natlnt
      irsum4 = infil2*natlni
      irsum5 = irsum2*natlni
c
c     Initialize variables for calculation loop
c
      i = 0
c
c     Calculation loop
c
  200 i = i+1
c
c       Determine 2-D cumulative infiltration
c
        if(time(i).le.timhmx)then
c
c          ---- (WEPP Equation 12.1)
          infilt = (avgdp2*infilh(i)+botwid*infilv(i)+piovr2*
     1             infilh(i)*infilv(i))*dtheta(iplane)
        else
          infilt = inftmx+(infilv(i)-vertmx)*float(furint)*rwdth*
     1             dtheta(iplane)
        endif
c
c       Adjust 2-D cumulative infiltration to eliminate steady state
c       infiltration rate
c
        infadj = infilt-kostf*time(i)
c
c       Adjust kostf if infadj is less than 0.0001 m**2
c
        if(infadj.lt.0.000001)then
          testf = (infilt-infadj)/time(i)
          if(testf.lt.0.999*kostf)then
            kostf = testf
          else
            kostf = 0.999*kostf
          endif
          goto 10
        endif
c
c       Determine values for functions of infadj and time
c
        natlni = log(infadj)
        infil2 = infadj*infadj
        natlnt = log(time(i))
        tmpvr4 = infil2*natlnt
c
c       Perform summations
c
        irsum1 = irsum1+infil2
        irsum2 = irsum2+tmpvr4
        irsum3 = irsum3+tmpvr4*natlnt
        irsum4 = irsum4+infil2*natlni
        irsum5 = irsum5+tmpvr4*natlni
c
c     Check for completion of calculation loop
c
      if(i.lt.nsteps-1)goto 200
c
c     Determine Kostiakov-Lewis infiltration equation constants
c
      denom = irsum2*irsum2-irsum1*irsum3
      kosta = (irsum2*irsum4-irsum1*irsum5)/denom
      if(kosta.gt.1.0)then
        kosta = 1.0
        kostk = infadj/timtot
      else
        kostk = exp((irsum2*irsum5-irsum3*irsum4)/denom)
      endif
c
      return
      end


!==========================================================================
elemental function gsw_pt0_from_t (sa, t, p)
!==========================================================================
!
! Calculates potential temperature with reference pressure, p_ref = 0 dbar.
!
! sa     : Absolute Salinity                               [g/kg]
! t      : in-situ temperature                             [deg C]
! p      : sea pressure                                    [dbar]
!
! gsw_pt0_from_t : potential temperature, p_ref = 0        [deg C]
!--------------------------------------------------------------------------

use gsw_mod_toolbox, only : gsw_entropy_part, gsw_entropy_part_zerop
use gsw_mod_toolbox, only : gsw_gibbs_pt0_pt0

use gsw_mod_teos10_constants, only : gsw_cp0, gsw_sso, gsw_t0, gsw_ups

use gsw_mod_kinds

implicit none

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

real (r8) :: gsw_pt0_from_t

integer n, no_iter
real (r8) :: s1, true_entropy_part, pt0m
real (r8) :: pt0, pt0_old, de_dt, dentropy, dentropy_dt

s1 = sa/gsw_ups

pt0 = t + p*( 8.65483913395442e-6_r8  - &
        s1 *  1.41636299744881e-6_r8  - &
         p *  7.38286467135737e-9_r8  + &
         t *(-8.38241357039698e-6_r8  + &
        s1 *  2.83933368585534e-8_r8  + &
         t *  1.77803965218656e-8_r8  + &
         p *  1.71155619208233e-10_r8))

dentropy_dt = gsw_cp0/((gsw_t0 + pt0)*(1.0_r8 - 0.05_r8*(1.0_r8 - sa/gsw_sso)))

true_entropy_part = gsw_entropy_part(sa,t,p)

do no_iter = 1, 2
    pt0_old = pt0
    dentropy = gsw_entropy_part_zerop(sa,pt0_old) - true_entropy_part
    pt0 = pt0_old - dentropy/dentropy_dt
    pt0m = 0.5_r8*(pt0 + pt0_old)
    dentropy_dt = -gsw_gibbs_pt0_pt0(sa,pt0m)
    pt0 = pt0_old - dentropy/dentropy_dt
end do

gsw_pt0_from_t = pt0

return
end function

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


       PROGRAM  ccomp
C#######################################################################
C  NAME OF ROUTINE
C      ccomp (Convert Complex)
C
C  PURPOSE
C      THIS IS THE STANDARD MAIN PROGRAM USED FOR TAE/VICAR PROGRAMS.
C      THIS MODULE CALLS SUBROUTINE MAIN44 TO ENTER INTO THE BODY OF THE
C      PROGRAM.
C      ccmp is a VICAR applications program which performs conversions
C      between comples pixel format and two real format images.  Two
C      types of transformations are possible (amplitude and phase) or
C      (real and imaginary).  The transformations may be done in either
C      direction:  a complex image to two real images, or two real
C      images to a complex image.
C
C  PR[CEPARED FOR USE ON MIPL SYSTEM BY
C      FRANK EVANS
C  ORIGINAL CCOMP PROGRAM BY
C      FRANK EVANS
C
C  ENVIRONMENT
C      VAX 11/780    VMS  with TAE/VICAR2 EXECUTIVE       FORTRAN-77
C
C  REVISION HISTORY
C     4-94  CRI  MSTP (S/W CONVERSION) VICAR PORTING
C
C    CALLING SEQUENCE (TAE COMMAND LINE)
C      The following command line formats show the major allowable forms:
C
C      ccomp INP=a OUT=(b,c) optional parameters
C        or
C      ccomp INP=(b,c) OUT=a optional parameters
C
C      ccomp a (b,c) optional parameters
C        or
C      ccomp (b,c) a optional parameters
C
C       Here 'a' represents the input or output complex image file name,
C       'b' represents the input or output image real or amplitude file name.
C       'c' represents the input or output image imaginary or phase file name.
C         When (b,c) are inputs or outputs they are paired as:
C              (real,imaginary) or (amplitude,phase)
C
C  INPUT PARAMETERS (listed by keyword)
C      INP    - Input file name(s).
C      OUT    - Output file name(s).
C      POLAR  - for amplitude and phase (default).
C      RECTANG- for real and imaginary.
C      FORWARD- for complex input (default).
C      INVERSE- for complex output.
C  OUTPUT PARAMETERS
C      The output image produced is written to the output file(s).
C  PROGRAM LIMITATIONS
C      1. The input image must be COMP data for keyword FORWARD.
C  SUBROUTINES CALLED
C      MAIN44
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
C
      INCLUDE 'VICMAIN_FOR'
C
      SUBROUTINE MAIN44
C
C#######################################################################
C  NAME OF ROUTINE
C     MAIN44 (name for top level subroutine by VICAR convention)
C
C  PURPOSE
C      MAIN44 processes parameters entered by user to perform translation.
C#######################################################################

	IMPLICIT  NONE
	INTEGER*4 IN1UNIT, IN2UNIT, OUT1UNIT, OUT2UNIT, STATUS
	INTEGER*4 SL, SS, NL, NS, NLI, NSI, LINE, SAMP
	COMPLEX*8 COMPBUF(4096)
	REAL*4	  REAL1BUF(4096), REAL2BUF(4096)
	REAL*4	  AMP
	CHARACTER*8  INFORMAT
	LOGICAL*4 PHASEAMP
        LOGICAL*4 XVPTST

        CALL IFMESSAGE ('CCOMP version 18 Dec 2012 (64-bit) - rjb')

        CALL XVEACTION ('SA',' ')

	PHASEAMP = XVPTST('POLAR')

	IF (XVPTST('INVERSE')) GOTO 2000

	CALL XVUNIT (IN1UNIT,'INP',1,STATUS,' ')
	CALL XVOPEN (IN1UNIT,STATUS,
     +		'OP','READ', 'U_FORMAT', 'COMP',' ')		!formerly COMPLEX
	CALL XVSIZE (SL, SS, NL, NS, NLI, NSI)
	CALL XVGET (IN1UNIT, STATUS, 'FORMAT', INFORMAT, ' ')
	IF (INFORMAT(1:4) .NE. 'COMP') THEN
	    CALL MABEND (' Input must be complex ')
	ENDIF

	CALL XVUNIT (OUT1UNIT,'OUT',1,STATUS,' ')
	CALL XVOPEN (OUT1UNIT,STATUS,
     +		'OP','WRITE',  'U_FORMAT','REAL', 'O_FORMAT','REAL',
     +		'U_NL',NL, 'U_NS',NS, ' ')
	CALL XVUNIT (OUT2UNIT,'OUT',2,STATUS,' ')
	CALL XVOPEN (OUT2UNIT,STATUS,
     +		'OP','WRITE',  'U_FORMAT','REAL', 'O_FORMAT','REAL',
     +		'U_NL',NL, 'U_NS',NS, ' ')

	DO LINE = SL, NL+SL-1
	    CALL XVREAD (IN1UNIT, COMPBUF, STATUS, 'LINE',LINE,
     +				'SAMP',SS, 'NSAMPS',NS, ' ')
	    IF (PHASEAMP) THEN
		DO SAMP = 1, NS
		    AMP = CABS(COMPBUF(SAMP))
		    REAL1BUF(SAMP) = AMP
		    IF (AMP .EQ. 0) THEN
			REAL2BUF(SAMP) = 0.0
		    ELSE
			REAL2BUF(SAMP) = ATAN2( AIMAG(COMPBUF(SAMP)),
     +					   REAL(COMPBUF(SAMP))    )
		    ENDIF
		ENDDO
	    ELSE
		DO SAMP = 1, NS
		    REAL1BUF(SAMP) = REAL(COMPBUF(SAMP))
		    REAL2BUF(SAMP) = AIMAG(COMPBUF(SAMP))
		ENDDO
	    ENDIF
	    CALL XVWRIT (OUT1UNIT, REAL1BUF, STATUS, ' ')
	    CALL XVWRIT (OUT2UNIT, REAL2BUF, STATUS, ' ')
	ENDDO

	CALL XVCLOSE (IN1UNIT,STATUS,' ')
	CALL XVCLOSE (OUT1UNIT,STATUS,' ')
	CALL XVCLOSE (OUT2UNIT,STATUS,' ')

	RETURN

2000	CONTINUE

	CALL XVUNIT(IN1UNIT,'INP',1,STATUS,' ')
	CALL XVOPEN(IN1UNIT,STATUS,
     +		'OP','READ', 'U_FORMAT', 'REAL', ' ')
	CALL XVSIZE (SL, SS, NL, NS, NLI, NSI)

	CALL XVUNIT(IN2UNIT,'INP',2,STATUS,' ')
	CALL XVOPEN(IN2UNIT,STATUS,
     +		'OP','READ', 'U_FORMAT', 'REAL', ' ')

	CALL XVUNIT (OUT1UNIT,'OUT',1,STATUS,' ')
	CALL XVOPEN (OUT1UNIT,STATUS,
     +		'OP','WRITE',  'U_FORMAT','COMP', 'O_FORMAT','COMP',		!formerly COMPLEX
     +		'U_NL',NL, 'U_NS',NS, ' ')

	DO LINE = SL, NL+SL-1
	    CALL XVREAD (IN1UNIT, REAL1BUF, STATUS, 'LINE',LINE,
     +				'SAMP',SS, 'NSAMPS',NS, ' ')
	    CALL XVREAD (IN2UNIT, REAL2BUF, STATUS, 'LINE',LINE,
     +				'SAMP',SS, 'NSAMPS',NS, ' ')
	    IF (PHASEAMP) THEN
		DO SAMP = 1, NS
		    COMPBUF(SAMP) = REAL1BUF(SAMP)*EXP((0,1)*REAL2BUF(SAMP))
		ENDDO
	    ELSE
		DO SAMP = 1, NS
                    COMPBUF(SAMP) = CMPLX(REAL1BUF(SAMP),REAL2BUF(SAMP))
		ENDDO
	    ENDIF
	    CALL XVWRIT (OUT1UNIT, COMPBUF, STATUS, ' ')
	ENDDO

	CALL XVCLOSE (IN1UNIT,STATUS,' ')
	CALL XVCLOSE (IN2UNIT,STATUS,' ')
	CALL XVCLOSE (OUT1UNIT,STATUS,' ')

	RETURN
	END


      PROGRAM ALGTST
C------------------------------------------------------------------
C FORTRAN 77 program to test ALGAMA or DLGAMA
C
C   Method:
C
C      Accuracy tests compare function values against values
C      generated with the duplication formula.
C
C   Data required
C
C      None
C
C   Subprograms required from this package
C
C      MACHAR - An environmental inquiry program providing
C               information on the floating-point arithmetic
C               system.  Note that the call to MACHAR can
C               be deleted provided the following five
C               parameters are assigned the values indicated
C
C               IBETA  - The radix of the floating-point system
C               IT     - The number of base-ibeta digits in the
C                        significant of a floating-point number
C               EPS    - The smallest positive floating-point
C                        number such that 1.0+EPS .NE. 1.0
C               XMIN   - The smallest non-vanishing floating-point
C                        integral power of the radix
C               XMAX   - The largest finite floating-point number
C
C      REN(K) - A function subprogram returning random real
C               numbers uniformly distributed over (0,1)
C
C
C    Intrinsic functions required are:
C
C         ABS, ANINT, DBLE, LOG, MAX, REAL, SQRT
C
C  Reference: "Performance evaluation of programs related
C              to the real gamma function", W. J. Cody,
C              submitted for publication.
C
C  Latest modification: March 9, 1992
C
C  Authors: W. J. Cody and L. Stoltz
C           Mathematics and Computer Science Division
C           Argonne National Laboratory
C           Argonne, IL 60439
C
C--------------------------------------------------------------------
      INTEGER I,IBETA,IEXP,IOUT,IRND,IT,J,JT,K1,K2,K3,
     1        MACHEP,MAXEXP,MINEXP,N,NEGEP,NGRD
CS    REAL  ALGAMA,
      DOUBLE PRECISION DLGAMA,
     1    A,AIT,ALBETA,ALL9,B,BETA,CL,CONV,C1,C2,C3,DEL,EPS,EPSNEG,
     2    FUNC,HALF,ONE,P875,P3125,P625,P6875,REN,R6,R7,SIXTEN,TEN,
     3    TWO,U,V,W,X,XC,XL,XMAX,XMIN,XN,XP99,Y,Z,ZERO,ZZ
C------------------------------------------------------------------
C   C1 = 0.5 - LN(SQRT(PI))
C   C2 = LN(2)
C   C3 = LN(2) - 11/16
C------------------------------------------------------------------
CS    DATA C1,C2/-7.2364942924700087072E-2,6.9314718055994530942E-1/,
CS   1     C3,ZERO,HALF/5.6471805599453094172E-3,0.0E0,0.5E0/,
CS   2     ONE,TWO,TEN,SIXTEN/1.0E0,2.0E0,10.0E0,16.0E0/,
CS   3     P6875,P875,P3125,P625/0.6875E0,0.875E0,1.3125E0,1.625E0/,
CS   4     ALL9,XP99/-999.0E0,0.99E0/
      DATA C1,C2/-7.2364942924700087072D-2,6.9314718055994530942D-1/,
     1     C3,ZERO,HALF/5.6471805599453094172D-3,0.0D0,0.5D0/,
     2     ONE,TWO,TEN,SIXTEN/1.0D0,2.0D0,10.0D0,16.0D0/,
     3     P6875,P875,P3125,P625/0.6875D0,0.875D0,1.3125D0,1.625D0/,
     4     ALL9,XP99/-999.0D0,0.99D0/
      DATA IOUT/6/
C------------------------------------------------------------------
C  Statement functions for conversion
C------------------------------------------------------------------
CS    CONV(I) = REAL(I)
      CONV(I) = DBLE(I)
CS    FUNC(X) = ALGAMA(X)
      FUNC(X) = DLGAMA(X)
C------------------------------------------------------------------
C  Determine machine parameters and set constants
C------------------------------------------------------------------
      CALL MACHAR(IBETA,IT,IRND,NGRD,MACHEP,NEGEP,IEXP,MINEXP,
     1            MAXEXP,EPS,EPSNEG,XMIN,XMAX)
      BETA = CONV(IBETA)
      ALBETA = LOG(BETA)
      AIT = CONV(IT)
      A = ZERO
      B = P875
      N = 2000
      XN = CONV(N)
      JT = 0
C-----------------------------------------------------------------
C  Determine largest argument for DLGAMA by iteration
C-----------------------------------------------------------------
      CL = XP99 * XMAX
      Z = -CL / ALL9
   80 ZZ = CL / (LOG(Z)-ONE)
      IF (ABS(ZZ/Z-ONE) .GT. (TWO*BETA*EPS)) THEN
         Z = ZZ
         GO TO 80
      END IF
      CL = ZZ
C-----------------------------------------------------------------
C  Random argument accuracy tests
C-----------------------------------------------------------------
      DO 300 J = 1, 3
         K1 = 0
         K3 = 0
         XC = ZERO
         R6 = ZERO
         R7 = ZERO
         DEL = (B - A) / XN
         XL = A
         DO 200 I = 1, N
            X = DEL * REN(JT) + XL
C-----------------------------------------------------------------
C  Use duplication formula
C-----------------------------------------------------------------
            IF (J .NE. 3) THEN
                  IF (J .EQ. 1) THEN
                        Z = X + HALF
                        X = Z - HALF
                        Y = X + X
                     ELSE
                        X = X + X
                        X = X * HALF
                        Y = (X + X) - ONE
                        Z = X - HALF
                  END IF
                  U = FUNC(X)
                  W = (Y-HALF)-HALF
                  ZZ = ANINT(W*SIXTEN)/SIXTEN
                  W = W - ZZ
                  V = (((HALF-ZZ*P6875) - C1) - W*P6875)-C3*(W+ZZ)
                  V = ((V + FUNC(Y)) - FUNC(Z))
               ELSE
                  Z = X * HALF + HALF
                  Y = Z - HALF
                  X = Y + Y
                  U = FUNC(X)
                  V = (C1 + ((X-HALF)-HALF)*C2)+FUNC(Y)+FUNC(Z)-HALF
            END IF
C--------------------------------------------------------------------
C  Accumulate results
C--------------------------------------------------------------------
            W = (U - V) / U
            IF (W .GT. ZERO) THEN
                  K1 = K1 + 1
               ELSE IF (W .LT. ZERO) THEN
                  K3 = K3 + 1
            END IF
            W = ABS(W)
            IF (W .GT. R6) THEN
                  R6 = W
                  XC = X
            END IF
            R7 = R7 + W * W
            XL = XL + DEL
  200    CONTINUE
C------------------------------------------------------------------
C  Gather and print statistics for test
C------------------------------------------------------------------
         K2 = N - K3 - K1
         R7 = SQRT(R7/XN)
         IF (J .EQ. 1) THEN
               WRITE (IOUT,1000)
            ELSE IF (J .EQ. 2) THEN
               WRITE (IOUT,1001)
            ELSE
               WRITE (IOUT,1002)
         END IF
         WRITE (IOUT,1010) N,A,B
         WRITE (IOUT,1011) K1,K2,K3
         WRITE (IOUT,1020) IT,IBETA
         IF (R6 .NE. ZERO) THEN
               W = LOG(ABS(R6))/ALBETA
            ELSE
               W = ALL9
         END IF
         WRITE (IOUT,1021) R6,IBETA,W,XC
         W = MAX(AIT+W,ZERO)
         WRITE (IOUT,1022) IBETA,W
         IF (R7 .NE. ZERO) THEN
               W = LOG(ABS(R7))/ALBETA
            ELSE
               W = ALL9
         END IF
         WRITE (IOUT,1023) R7,IBETA,W
         W = MAX(AIT+W,ZERO)
         WRITE (IOUT,1022) IBETA,W
C------------------------------------------------------------------
C  Initialize for next test
C------------------------------------------------------------------
         A = P3125
         B = P625
         IF (J .EQ. 2) THEN
               A = TWO + TWO
               B = TEN + TEN
         END IF
  300 CONTINUE
C-----------------------------------------------------------------
C  Special tests
C  First test with special arguments
C-----------------------------------------------------------------
      WRITE (IOUT,1025)
      WRITE (IOUT,1040)
      Z = EPS
      ZZ = FUNC(Z)
      WRITE (IOUT,1041) Z,ZZ
      Z = HALF
      ZZ = FUNC(Z)
      WRITE (IOUT,1041) Z,ZZ
      Z = ONE
      ZZ = FUNC(Z)
      WRITE (IOUT,1041) Z,ZZ
      Z = TWO
      ZZ = FUNC(Z)
      WRITE (IOUT,1041) Z,ZZ
C-----------------------------------------------------------------
C  Test of error returns
C-----------------------------------------------------------------
      WRITE (IOUT,1050)
      Z = XMIN
      WRITE (IOUT,1053) Z
      ZZ = FUNC(Z)
      WRITE (IOUT,1061) ZZ
      Z = CL
      WRITE (IOUT,1053) Z
      ZZ = FUNC(Z)
      WRITE (IOUT,1061) ZZ
      Z = -ONE
      WRITE (IOUT,1052) Z
      ZZ = FUNC(Z)
      WRITE (IOUT,1061) ZZ
      Z = ZERO
      WRITE (IOUT,1052) Z
      ZZ = FUNC(Z)
      WRITE (IOUT,1061) ZZ
      Z = XP99 * XMAX
      WRITE (IOUT,1052) Z
      ZZ = FUNC(Z)
      WRITE (IOUT,1061) ZZ
      WRITE (IOUT,1100)
      STOP
C-----------------------------------------------------------------
 1000 FORMAT('1Test of LGAMA(X) vs LN(2*SQRT(PI))-2X*LN(2)+',
     1    'LGAMA(2X)-LGAMA(X+1/2)'//)
 1001 FORMAT('1Test of LGAMA(X) vs LN(2*SQRT(PI))-(2X-1)*LN(2)+',
     1    'LGAMA(X-1/2)-LGAMA(2X-1)'//)
 1002 FORMAT('1Test of LGAMA(X) vs -LN(2*SQRT(PI))+X*LN(2)+',
     1    'LGAMA(X/2)+LGAMA(X/2+1/2)'//)
 1010 FORMAT(I7,' Random arguments were tested from the interval (',
     1    F5.1,',',F5.1,')'//)
 1011 FORMAT('  LGAMA(X) was larger',I6,' times,'/
     1    14X,' agreed',I6,' times, and'/
     2    10X,'was smaller',I6,' times.'//)
 1020 FORMAT(' There are',I4,' base',I4,
     1    ' significant digits in a floating-point number'//)
 1021 FORMAT(' The maximum relative error of',E15.4E3,' = ',I4,' **',
     1    F7.2/4X,'occurred for X =',E13.6E3)
 1022 FORMAT(' The estimated loss of base',I4,
     1    ' significant digits is',F7.2//)
 1023 FORMAT(' The root mean square relative error was',E15.4E3,
     1    ' = ',I4,' **',F7.2)
 1025 FORMAT('1Special Tests'//)
 1040 FORMAT(//' Test of special arguments'//)
 1041 FORMAT(' LGAMA (',E13.6E3,') = ',E13.6E3//)
 1050 FORMAT('1Test of Error Returns'///)
 1052 FORMAT(' LGAMA will be called with the argument',E13.6E3,/
     1    ' This should trigger an error message'//)
 1053 FORMAT(' LGAMA will be called with the argument',E13.6E3,/
     1    ' This should not trigger an error message'//)
 1061 FORMAT(' LGAMA returned the value',E13.6E3///)
 1100 FORMAT(' This concludes the tests')
C---------- Last line of ALGAMA/DLGAMA test program ----------
      END


module ang_mom_defs_test
    use fruit
    use global_defs
    use ang_mom_defs
    implicit none

contains

    subroutine test_spherical_to_cartesian_gaussian
        real(dp), parameter :: tol = 1.e-12_dp
        real(dp), parameter :: s3o4 = sqrt(3.0_dp/4.0_dp)
        real(dp), parameter :: s3o8 = sqrt(3.0_dp/8.0_dp)
        real(dp), parameter :: s3o40 = sqrt(3.0_dp/40.0_dp)
        real(dp), parameter :: s5o8 = sqrt(5.0_dp/8.0_dp)
        real(dp), parameter :: s6o5 = sqrt(6.0_dp/5.0_dp)
        real(dp), parameter :: s9o8 = sqrt(9.0_dp/8.0_dp)
        real(dp), parameter :: s9o20 = sqrt(9.0_dp/20.0_dp)
        real(dp), parameter :: p_trans(3,3) = reshape( [ &
        &    num1, num0, num0, &
        &    num0, num1, num0, &
        &    num0, num0, num1  &
        &    ], shape(p_trans))
        real(dp), parameter :: d_trans(6,5) = reshape( [ &
        &   -f1o2,-f1o2, num1, num0, num0, num0, &
        &    num0, num0, num0, num0, num1, num0, &
        &    num0, num0, num0, num0, num0, num1, &
        &    s3o4,-s3o4, num0, num0, num0, num0, &
        &    num0, num0, num0, num1, num0, num0  &
        &    ], shape(d_trans))
        real(dp), parameter :: f_trans(10, 7) = reshape( [ &
        &     num0, num0, num1,  num0,-s9o20,  num0,-s9o20, num0, num0, num0, &
        &    -s3o8, num0, num0,  num0,  num0,-s3o40,  num0, s6o5, num0, num0, &
        &     num0,-s3o8, num0,-s3o40,  num0,  num0,  num0, num0, s6o5, num0, &
        &     num0, num0, num0,  num0,  s3o4,  num0, -s3o4, num0, num0, num0, &
        &     num0, num0, num0,  num0,  num0,  num0,  num0, num0, num0, num1, &
        &     s5o8, num0, num0,  num0,  num0, -s9o8,  num0, num0, num0, num0, &
        &     num0,-s5o8, num0,  s9o8,  num0,  num0,  num0, num0, num0, num0  &
        &     ], shape(f_trans))

        call amp%init(4)
        ! S
        call assert_equals(num1, amp%sphe_cart_trans(0)%c(1, 1), tol, 'S orbital transformation')
        ! P
        call assert_equals(p_trans, amp%sphe_cart_trans(1)%c, 3, 3, tol, 'P orbital transformation')
        ! D
        call assert_equals(d_trans, amp%sphe_cart_trans(2)%c, 5, 6, tol, 'D orbital transformation')
        ! F
        call assert_equals(f_trans, amp%sphe_cart_trans(3)%c, 10, 7, tol, 'F orbital transformation')
    end subroutine test_spherical_to_cartesian_gaussian

end module ang_mom_defs_test


! ##################################################################################################################################
! Begin MIT license text.
! _______________________________________________________________________________________________________

! Copyright 2019 Dr William R Case, Jr (dbcase29@gmail.com)

! Permission is hereby granted, free of charge, to any person obtaining a copy of this software and
! associated documentation files (the "Software"), to deal in the Software without restriction, including
! without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
! copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to
! the following conditions:

! The above copyright notice and this permission notice shall be included in all copies or substantial
! portions of the Software and documentation.

! THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
! OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
! FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
! AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
! LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
! OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
! THE SOFTWARE.
! _______________________________________________________________________________________________________

! End MIT license text.

      SUBROUTINE PENTA ( OPT, INT_ELEM_ID, IORD_IJ, IORD_K, RED_INT_SHEAR, WRITE_WARN )

! Isoparametric pentahedron solid element (6 or 15 nodes and with full or reduced gaussian integration)

! Subroutine calculates:

!  1) ME        = element mass matrix                  , if OPT(1) = 'Y'
!  2) PTE       = element thermal load vectors         , if OPT(2) = 'Y'
!  3) SEi, STEi = element stress data recovery matrices, if OPT(3) = 'Y'
!  4) KE        = element linea stiffness matrix       , if OPT(4) = 'Y'
!  5) KED       = element differen stiff matrix calc   , if OPT(6) = 'Y' = 'Y'

      USE PENTIUM_II_KIND, ONLY       :  BYTE, LONG, DOUBLE
      USE IOUNT1, ONLY                :  WRT_ERR, WRT_LOG, ERR, F04, F06
      USE SCONTR, ONLY                :  BLNK_SUB_NAM, FATAL_ERR, MAX_ORDER_TRIA, MAX_ORDER_GAUSS, NTSUB
      USE TIMDAT, ONLY                :  TSEC
      USE CONSTANTS_1, ONLY           :  HALF, THIRD, ZERO, SIX
      USE DEBUG_PARAMETERS, ONLY      :  DEBUG
      USE SUBR_BEGEND_LEVELS, ONLY    :  PENTA_BEGEND
      USE PARAMS, ONLY                :  EPSIL
      USE NONLINEAR_PARAMS, ONLY      :  LOAD_ISTEP
      USE MODEL_STUF, ONLY            :  ALPVEC, BE1, BE2, DT, EID, ELGP, NUM_EMG_FATAL_ERRS, ES, KE, KED, ME, PTE, RHO,           &
                                         SE1, SE2, STE1, STRESS, TREF, TYPE

      USE PENTA_USE_IFs

      IMPLICIT NONE

      CHARACTER(LEN=LEN(BLNK_SUB_NAM)):: SUBR_NAME = 'PENTA'
      CHARACTER( 1*BYTE), INTENT(IN)  :: RED_INT_SHEAR           ! If 'Y', use Gaussian weighted avg of B matrices for shear terms
      CHARACTER( 1*BYTE), INTENT(IN)  :: OPT(6)                  ! 'Y'/'N' flags for whether to calc certain elem matrices
      CHARACTER(LEN=*), INTENT(IN)    :: WRITE_WARN              ! If 'Y" write warning messages, otherwise do not
      CHARACTER(46*BYTE)              :: IORD_MSG                ! Character name of an integration order (used for debug output)

      INTEGER(LONG), INTENT(IN)       :: INT_ELEM_ID             ! Internal element ID
      INTEGER(LONG), INTENT(IN)       :: IORD_IJ                 ! Integration order in the triangular plane
      INTEGER(LONG), INTENT(IN)       :: IORD_K                  ! Integration order in Z direction
      INTEGER(LONG)                   :: IORD_IJ_SH              ! Integration order in the triang plane for red integr for shear
      INTEGER(LONG)                   :: IORD_K_SH               ! Integration order in Z direction for red integr for shear
      INTEGER(LONG)                   :: GAUSS_PT                ! Gauss point number (used for DEBUG output in subr SHP3DP
      INTEGER(LONG)                   :: I,J,IJ,K,L,M,N          ! DO loop indices
      INTEGER(LONG)                   :: II,JJ                   ! Counters
      INTEGER(LONG)                   :: ID(3*ELGP)              ! Array which shows equivalence of DOF's in virgin element with the
   !                                                             6 DOF/grid of the final element stiffness matrix
      INTEGER(LONG), PARAMETER        :: SUBR_BEGEND = PENTA_BEGEND

      REAL(DOUBLE)                    :: ALP(6)                  ! First col of ALPVEC
                                                                 ! Strain-displ matrix for this element for all Gauss points
      REAL(DOUBLE)                    :: B(6,3*ELGP,IORD_IJ*IORD_K)

      REAL(DOUBLE)                    :: BI(6,3*ELGP)            ! Strain-displ matrix for this element for one Gauss point
      REAL(DOUBLE)                    :: CBAR(3,3*ELGP)          ! Derivatives of shape fcns wrt x,y,z used in diff stiff matrix
!                                                                  (contains terms from DPSHX matrices for each grid of the PENTA)

      REAL(DOUBLE)                    :: DETJ(IORD_IJ*IORD_K)    ! Determinant of JAC for all Gauss points
      REAL(DOUBLE)                    :: DPSHG(3,ELGP)           ! Output from subr SHP3DP. Derivs of PSH wrt elem isopar coords.
      REAL(DOUBLE)                    :: DPSHX(3,ELGP)           ! Derivatives of PSH wrt elem x, y coords.
      REAL(DOUBLE)                    :: DUM0(3*ELGP)            ! Intermediate matrix used in solving for elem matrices
      REAL(DOUBLE)                    :: DUM1(3*ELGP)            ! Intermediate matrix used in solving for elem matrices
      REAL(DOUBLE)                    :: DUM2(6,3*ELGP)          ! Intermediate matrix used in solving for elem matrices
      REAL(DOUBLE)                    :: DUM3(3*ELGP,3*ELGP)     ! Intermediate matrix used in solving for elem matrices
      REAL(DOUBLE)                    :: DUM4(6,3*ELGP)          ! Intermediate matrix used in solving for elem matrices
      REAL(DOUBLE)                    :: DUM5(3*ELGP,3*ELGP)     ! Intermediate matrix used in solving for KE elem matrices
      REAL(DOUBLE)                    :: DUM6(3,3*ELGP)          ! Intermediate matrix used in solving for elem matrices
      REAL(DOUBLE)                    :: EALP(6)                 ! Interm var used in calc PTE therm loads & STEi stress coeffs
      REAL(DOUBLE)                    :: EPS1                    ! A small number to compare to real zero

                                                                 ! Array of all DT values at the grids GRID_DT_ARRAY(i,j) = DT(i,j)
      REAL(DOUBLE)                    :: GRID_DT_ARRAY(ELGP,NTSUB)

      REAL(DOUBLE)                    :: HH_IJ(MAX_ORDER_TRIA)   ! An output from subr ORDER_TRIA, called herein.  Gauss weights.
      REAL(DOUBLE)                    :: HH_K(MAX_ORDER_GAUSS)   ! An output from subr ORDER_TRIA, called herein.  Gauss weights.
      REAL(DOUBLE)                    :: HH_IJ_SH(MAX_ORDER_TRIA)! An output from subr ORDER_TRIA, called herein.  Gauss weights.
      REAL(DOUBLE)                    :: HH_K_SH(MAX_ORDER_GAUSS)! An output from subr ORDER_TRIA, called herein.  Gauss weights.
      REAL(DOUBLE)                    :: INTFAC                  ! An integration factor (constant multiplier for the Gauss integ)
      REAL(DOUBLE)                    :: JAC(3,3)                ! An output from subr JAC3D, called herein. 3 x 3 Jacobian matrix.
      REAL(DOUBLE)                    :: JACI(3,3)               ! An output from subr JAC3D, called herein. 3 x 3 Jacobian inverse.
      REAL(DOUBLE)                    :: KWW(3,3)                ! Portion of differential stiffness matrix
      REAL(DOUBLE)                    :: M0                      ! An intermediate variable used in calc elem mass, ME
      REAL(DOUBLE)                    :: PSH(ELGP)               ! Output from subr SHP3DP. Shape fcn at Gauss pts SSI, SSJ
      REAL(DOUBLE)                    :: SIGxx                   ! Normal stress in the elem x  direction
      REAL(DOUBLE)                    :: SIGyy                   ! Normal stress in the elem y  direction
      REAL(DOUBLE)                    :: SIGzz                   ! Normal stress in the elem z  direction
      REAL(DOUBLE)                    :: SIGxy                   ! Shear  stress in the elem xy direction
      REAL(DOUBLE)                    :: SIGyz                   ! Shear  stress in the elem yz direction
      REAL(DOUBLE)                    :: SIGzx                   ! Shear  stress in the elem zx direction
      REAL(DOUBLE)                    :: SS_I(MAX_ORDER_TRIA)    ! An output from subr ORDER_TRIA, called herein. Gauss abscissa's
      REAL(DOUBLE)                    :: SS_J(MAX_ORDER_TRIA)    ! An output from subr ORDER_TRIA, called herein. Gauss abscissa's
      REAL(DOUBLE)                    :: SS_K(MAX_ORDER_GAUSS)   ! An output from subr ORDER_GAUSS, called herein. Gauss abscissa's
      REAL(DOUBLE)                    :: SS_I_SH(MAX_ORDER_TRIA) ! An output from subr ORDER_TRIA, called herein. Gauss abscissa's
      REAL(DOUBLE)                    :: SS_J_SH(MAX_ORDER_TRIA) ! An output from subr ORDER_TRIA, called herein. Gauss abscissa's
      REAL(DOUBLE)                    :: SS_K_SH(MAX_ORDER_GAUSS)! An output from subr ORDER_GAUSS, called herein. Gauss abscissa's
      REAL(DOUBLE)                    :: SUMB                    ! An intermediate variable used in calc B matrix for reduced integ
      REAL(DOUBLE)                    :: SUMD                    ! An intermediate variable used in calc B matrix for reduced integ
      REAL(DOUBLE)                    :: TBAR(NTSUB)             ! Average elem temperature for each subcase
      REAL(DOUBLE)                    :: TEMP                    ! Temperature to use in PTE calc
      REAL(DOUBLE)                    :: TREF1                   ! TREF(1)
      REAL(DOUBLE)                    :: TGAUSS(1,NTSUB)         ! Temp at a Gauss point for a theral subcase
      REAL(DOUBLE)                    :: VOLUME                  ! 3D element volume

! **********************************************************************************************************************************
      IF (WRT_LOG >= SUBR_BEGEND) THEN
         CALL OURTIM
         WRITE(F04,9001) SUBR_NAME,TSEC
 9001    FORMAT(1X,A,' BEGN ',F10.3)
      ENDIF

! **********************************************************************************************************************************
      EPS1 = EPSIL(1)

! Calculate volume by Gaussian integration

      CALL ORDER_TRIA  ( IORD_IJ, SS_I, SS_J, HH_IJ )
      CALL ORDER_GAUSS ( IORD_K , SS_K , HH_K  )

! Calculate volume by Gaussian integration

      IORD_MSG = 'for triangular-cross section x z direction  = '
      VOLUME = ZERO
      GAUSS_PT = 0
      DO K=1,IORD_K
         DO IJ=1,IORD_IJ
            GAUSS_PT = GAUSS_PT + 1
            CALL SHP3DP ( I, J, K, ELGP, SUBR_NAME, IORD_MSG, IORD_IJ, IORD_K, SS_I(IJ), SS_J(IJ), SS_K(K), 'Y', PSH, DPSHG )
            CALL JAC3D ( SS_I(IJ), SS_J(IJ), SS_K(K), DPSHG, 'Y', JAC, JACI, DETJ(GAUSS_PT) )
            VOLUME = VOLUME + HH_IJ(IJ)*HH_K(K)*DETJ(GAUSS_PT)
         ENDDO
      ENDDO

      IF (VOLUME < EPS1) THEN                              ! If VOLUME <= 0, write error and return
         WRITE(ERR,1925) EID, TYPE, 'VOLUME', VOLUME
         WRITE(F06,1925) EID, TYPE, 'VOLUME', VOLUME
         NUM_EMG_FATAL_ERRS = NUM_EMG_FATAL_ERRS + 1
         FATAL_ERR = FATAL_ERR + 1
         RETURN
      ENDIF

! Calculate ID array

      JJ = 0
      DO I=1,ELGP
         II = 6*(I - 1)
         DO J=1,3
            II = II + 1
            JJ = JJ + 1
            ID(JJ) = II
         ENDDO
      ENDDO

! ALP is first col of ALPVEC

      ALP(1) = ALPVEC(1,1)
      ALP(2) = ALPVEC(2,1)
      ALP(3) = ALPVEC(3,1)
      ALP(4) = ZERO
      ALP(5) = ZERO
      ALP(6) = ZERO

      TREF1 = TREF(1)

! EALP is needed to calculate both PTE and STE2

      CALL MATMULT_FFF ( ES, ALP, 6, 6, 1, EALP )

! Calc TBAR (used for PTE, STEi)

      IF ((OPT(2) == 'Y') .OR. (OPT(3) == 'Y')) THEN
         DO J=1,NTSUB
            TBAR(J) = ZERO
            DO K=1,ELGP
               TBAR(J) = TBAR(J) + DT(K,J)
            ENDDO
            TBAR(J) = TBAR(J)/ELGP - TREF1
         ENDDO
      ENDIF

! **********************************************************************************************************************************
! Generate the mass matrix for this element.

      IF (OPT(1) == 'Y') THEN

         M0 = (RHO(1))*VOLUME/ELGP

         DO I=1,ELGP
            DO J=1,3
               K = 6*(I-1) + J
               ME(K,K) = M0
            ENDDO
         ENDDO

      ENDIF

! **********************************************************************************************************************************
! Generate B matrices. Dimensions 1 and 2 of B store a element B matrix for 1 Gauss point. The 3rd dimension has B for all other
! Gauss points (denoted by IGAUSS index)

opt234:IF ((OPT(2) == 'Y') .OR. (OPT(3) == 'Y') .OR. (OPT(4) == 'Y') .OR. (OPT(6) == 'Y')) THEN

         DO I=1,6
            DO J=1,3*ELGP
               DO K=1,IORD_IJ*IORD_K
                  B(I,J,K) = ZERO
               ENDDO
            ENDDO
         ENDDO

         IORD_MSG = 'for triangular-cross section x z direction  = '
         GAUSS_PT = 0
         DO K=1,IORD_K
            DO IJ=1,IORD_IJ
               GAUSS_PT = GAUSS_PT + 1
               CALL SHP3DP ( IJ, IJ, K, ELGP, SUBR_NAME, IORD_MSG, IORD_IJ, IORD_K, SS_I(IJ), SS_J(IJ), SS_K(K), 'N', PSH, DPSHG )
               CALL JAC3D ( SS_I(IJ), SS_J(IJ), SS_K(K), DPSHG, 'N', JAC, JACI, DETJ(GAUSS_PT) )
               CALL MATMULT_FFF ( JACI, DPSHG, 3, 3, ELGP, DPSHX )
               CALL B3D_ISOPARAMETRIC ( DPSHX, GAUSS_PT, IJ, IJ, K, 'direct strains', 'Y', BI )
               DO L=1,6
                  DO M=1,3*ELGP
                     B(L,M,GAUSS_PT) = BI(L,M)
                  ENDDO
               ENDDO
            ENDDO
         ENDDO

         IF (RED_INT_SHEAR == 'Y') THEN

            IF (IORD_IJ == 3) THEN                            ! Full integ is 2x3 so calc B using selective substit for shear terms
!                                                               -------------------------------------------------------------------

               DO L=4,6                                       ! NOTE: the following numbering for the Ki is not the CW or CCW num
!                                                               of element nodes. It is the numbering of Gauss points consistent w/
!                                                               the DO loops (K,J,I) which use Gauss coords SSS(I), SSS(J), SSS(K).
                  IF      (L == 4) THEN                       ! Numbering for selecive  substitution for Gamma-xy

                     SUMD = DETJ(1) + DETJ(2) + DETJ(3)
                     DO J=1,3*ELGP                            !      Jacobian weighted average for xy shear terms on one z face
                        SUMB = DETJ(1)*B(L,J,1) + DETJ(2)*B(L,J,2) + DETJ(3)*B(L,J,3)
                        B(L,J,1) = SUMB/SUMD
                        B(L,J,2) = B(L,J,1)
                        B(L,J,3) = B(L,J,1)
                     ENDDO

                     SUMD = DETJ(4) + DETJ(5) + DETJ(6)
                     DO J=1,3*ELGP                            !      Jacobian weighted average for xy shear terms on other z face
                        SUMB = DETJ(4)*B(L,J,4) + DETJ(5)*B(L,J,5) + DETJ(6)*B(L,J,6)
                        B(L,J,4) = SUMB/SUMD
                        B(L,J,5) = B(L,J,4)
                        B(L,J,6) = B(L,J,4)
                     ENDDO

                  ELSE IF (L == 5) THEN                       ! Numbering for selecive  substitution for Gamma-yz

                     SUMD = DETJ(1) + DETJ(2) + DETJ(4) + DETJ(5)
                     DO J=1,3*ELGP                            !      Jacobian weighted average for yz shear terms
                        SUMB = DETJ(1)*B(L,J,1) + DETJ(2)*B(L,J,2) + DETJ(4)*B(L,J,4) + DETJ(5)*B(L,J,5)
                        B(L,J,1) = SUMB/SUMD
                        B(L,J,2) = B(L,J,1)
                        B(L,J,4) = B(L,J,1)
                        B(L,J,5) = B(L,J,1)
                     ENDDO

                     SUMD = DETJ(3) + DETJ(6)
                     DO J=1,3*ELGP                            !      Jacobian weighted average for yz shear terms
                        SUMB = DETJ(3)*B(L,J,3) + DETJ(6)*B(L,J,6)
                        B(L,J,3) = SUMB/SUMD
                        B(L,J,6) = B(L,J,3)
                     ENDDO

                  ELSE IF (L == 6) THEN                       ! Numbering for selecive  substitution for Gamma-zx

                     SUMD = DETJ(2) + DETJ(3) + DETJ(5) + DETJ(6)
                     DO J=1,3*ELGP                            !      Jacobian weighted average for xz shear terms
                        SUMB = DETJ(2)*B(L,J,2) + DETJ(3)*B(L,J,3) + DETJ(5)*B(L,J,5) + DETJ(6)*B(L,J,6)
                        B(L,J,2) = SUMB/SUMD
                        B(L,J,3) = B(L,J,2)
                        B(L,J,5) = B(L,J,2)
                        B(L,J,6) = B(L,J,2)
                     ENDDO

                     SUMD = DETJ(1) + DETJ(4)
                     DO J=1,3*ELGP                            !      Jacobian weighted average for yz shear terms
                        SUMB = DETJ(1)*B(L,J,1) + DETJ(4)*B(L,J,4)
                        B(L,J,1) = SUMB/SUMD
                        B(L,J,4) = B(L,J,1)
                     ENDDO

                  ENDIF

               ENDDO

            ELSE                                              ! IORD_IJ = 7, so calc B using 2x3 integration for shear terms
!                                                               ------------------------------------------------------------
               IORD_IJ_SH = 3
               IORD_K_SH  = 2
               CALL ORDER_TRIA  ( IORD_IJ_SH, SS_I_SH, SS_J_SH, HH_IJ_SH )
               CALL ORDER_GAUSS ( IORD_K_SH , SS_K_SH , HH_K_SH  )

               GAUSS_PT = 0
               IORD_MSG = 'reduced: tria-cross-section x z direction   = '
               DO K=1,IORD_K_SH
                  DO IJ=1,IORD_IJ_SH
                     GAUSS_PT = GAUSS_PT + 1
                     CALL SHP3DP ( IJ, IJ, K, ELGP, SUBR_NAME, IORD_MSG, IORD_IJ_SH, IORD_K_SH,                                    &
                                   SS_I_SH(IJ), SS_J_SH(IJ), SS_K_SH(K), 'N', PSH, DPSHG )
                     CALL JAC3D ( SS_I_SH(IJ), SS_J_SH(IJ), SS_K_SH(K), DPSHG, 'N', JAC, JACI, DETJ(GAUSS_PT) )
                     CALL MATMULT_FFF ( JACI, DPSHG, 3, 3, ELGP, DPSHX )
                     CALL B3D_ISOPARAMETRIC ( DPSHX, GAUSS_PT, IJ, IJ, K, 'direct strains', 'Y', BI )
                     DO L=4,6
                        DO M=1,3*ELGP
                           B(L,M,GAUSS_PT) = BI(L,M)
                        ENDDO
                     ENDDO
                  ENDDO
               ENDDO

            ENDIF

         ENDIF

      ENDIF opt234

! **********************************************************************************************************************************
! Calculate element thermal loads.

      IF (OPT(2) == 'Y') THEN

         DO N=1,NTSUB
            DO L=1,ELGP
               GRID_DT_ARRAY(L,N) = DT(L,N)
            ENDDO
         ENDDO

         DO N=1,NTSUB

            DO L=1,3*ELGP
               DUM0(L) = ZERO
               DUM1(L) = ZERO
            ENDDO

            IORD_MSG = ' '
            GAUSS_PT = 0
            IORD_MSG = 'for triangular-cross section x z direction  = '
            DO K=1,IORD_K
               DO IJ=1,IORD_IJ
                  GAUSS_PT = GAUSS_PT + 1
                  DO L=1,6
                     DO M=1,3*ELGP
                        BI(L,M) = B(L,M,GAUSS_PT)
                     ENDDO
                  ENDDO
                  CALL MATMULT_FFF_T ( BI, EALP, 6, 3*ELGP, 1, DUM0 )
                  INTFAC = DETJ(GAUSS_PT)*HH_IJ(IJ)*HH_K(K)
                  IF (DEBUG(192) == 0) THEN                ! Use temperatures at Gauss points for PTE
                     CALL SHP3DP ( IJ, IJ, K, ELGP, SUBR_NAME, IORD_MSG, IORD_IJ, IORD_K, SS_I(IJ), SS_J(IJ), SS_K(K),             &
                                   'N', PSH, DPSHG )
                     CALL MATMULT_FFF ( PSH, GRID_DT_ARRAY, 1, ELGP, NTSUB, TGAUSS )
                     TEMP = TGAUSS(1,N) - TREF1
                  ELSE                                     ! Use avg element temperature for PTE
                     TEMP = TBAR(N)
                  ENDIF
                  DO L=1,3*ELGP
                     DUM1(L) = DUM1(L) + DUM0(L)*TEMP*INTFAC
                  ENDDO
               ENDDO
            ENDDO

            DO L=1,3*ELGP
               PTE(ID(L),N) = DUM1(L)
            ENDDO

         ENDDO

      ENDIF

! **********************************************************************************************************************************
! Calculate SE1, SE2 matrices for stress data recovery. All stresses calculated at center of element

      IF ((OPT(3) == 'Y') .OR. (OPT(6) == 'Y')) THEN

         DO K=1,6
            DO L=1,3*ELGP
               DUM2(K,L) = ZERO
            ENDDO
         ENDDO

         GAUSS_PT = 1                                      ! Calc SE1,2,3
         IORD_MSG = 'for triangular-cross section x z direction  = '
         CALL SHP3DP ( 1, 1, 1, ELGP, SUBR_NAME, IORD_MSG, 1, 1, THIRD, THIRD, ZERO, 'N', PSH, DPSHG )
         CALL JAC3D ( THIRD, THIRD, THIRD, DPSHG, 'N', JAC, JACI, DETJ(GAUSS_PT) )
         CALL MATMULT_FFF ( JACI, DPSHG, 3, 3, ELGP, DPSHX )
         CALL B3D_ISOPARAMETRIC ( DPSHX, GAUSS_PT, 1, 1, 1, 'all strains', 'N', BI )
         CALL MATMULT_FFF ( ES, BI, 6, 6, 3*ELGP, DUM2 )

         DO I=1,3
            DO J=1,3*ELGP
               SE1(I,ID(J),1) = DUM2(I  ,J)
               SE2(I,ID(J),1) = DUM2(I+3,J)
            ENDDO
         ENDDO

         DO J=1,NTSUB                                      ! STE thermal stress terms
            STE1(1,J,1) = EALP(1)*TBAR(J)
            STE1(2,J,1) = EALP(2)*TBAR(J)
            STE1(3,J,1) = EALP(3)*TBAR(J)
         ENDDO

         DO I=1,3                                          ! Strain-displ matrix
            DO J=1,3*ELGP
               BE1(I,ID(J),1) = BI(I  ,J)
               BE2(I,ID(J),1) = BI(I+3,J)
            ENDDO
         ENDDO

      ENDIF

! **********************************************************************************************************************************
! Calculate element stiffness matrix KE.

      IF (OPT(4) == 'Y') THEN

         DO I=1,3*ELGP
            DO J=1,3*ELGP
               DUM3(I,J) = ZERO
            ENDDO
         ENDDO

         IORD_MSG = ' '
         GAUSS_PT = 0
         DO K=1,IORD_K
            DO IJ=1,IORD_IJ
               GAUSS_PT = GAUSS_PT + 1
               DO L=1,6
                  DO M=1,3*ELGP
                     BI(L,M) = B(L,M,GAUSS_PT)
                  ENDDO
               ENDDO
               CALL MATMULT_FFF ( ES, BI, 6, 6, 3*ELGP, DUM4 )
               CALL MATMULT_FFF_T ( BI, DUM4, 6, 3*ELGP, 3*ELGP, DUM5 )
               INTFAC = DETJ(GAUSS_PT)*HH_IJ(IJ)*HH_K(K)
               DO L=1,3*ELGP
                  DO M=1,3*ELGP
                     DUM3(L,M) = DUM3(L,M) + DUM5(L,M)*INTFAC
                  ENDDO
               ENDDO
            ENDDO
         ENDDO

         DO I=1,3*ELGP
            DO J=1,3*ELGP
               KE(ID(I),ID(J)) = DUM3(I,J)
            ENDDO
         ENDDO

! Set lower triangular portion of KE equal to upper portion

         DO I=2,6*ELGP
            DO J=1,I-1
               KE(I,J) = KE(J,I)
            ENDDO
         ENDDO

      ENDIF

! **********************************************************************************************************************************
! Calculate linear differential stiffness matrix

      IF ((OPT(6) == 'Y') .AND. (LOAD_ISTEP > 1)) THEN

         CALL ELMDIS
         CALL ELEM_STRE_STRN_ARRAYS ( 1 )

         SIGxx = STRESS(1)
         SIGyy = STRESS(2)
         SIGzz = STRESS(3)
         SIGxy = STRESS(4)
         SIGyz = STRESS(5)
         SIGzx = STRESS(6)

         KWW(1,1) = SIGyy + SIGzz   ;   KWW(1,2) = -SIGxy   ;   KWW(1,3) = -SIGzx
         KWW(2,2) = SIGxx + SIGzz   ;   KWW(2,3) = -SIGyz
         KWW(3,3) = SIGxx + SIGyy
         KWW(2,1) = KWW(1,2)
         KWW(3,1) = KWW(1,3)
         KWW(3,2) = KWW(2,3)
         DO I=1,ELGP
            CBAR(1,3*(I-1)+1) =  ZERO             ;  CBAR(1,3*(I-1)+2) = -HALF*DPSHX(3,I)  ;  CBAR(1,3*(I-1)+3)=  HALF*DPSHX(2,I)
            CBAR(2,3*(I-1)+1) =  HALF*DPSHX(3,I)  ;  CBAR(2,3*(I-1)+2) =  ZERO             ;  CBAR(2,3*(I-1)+3)= -HALF*DPSHX(1,I)
            CBAR(3,3*(I-1)+1) = -HALF*DPSHX(2,I)  ;  CBAR(3,3*(I-1)+2) =  HALF*DPSHX(1,I)  ;  CBAR(3,3*(I-1)+3)=  ZERO
         ENDDO

! DPSHG(3,ELGP), DPSHX(3,ELGP)
! DUM3(3*ELGP,3*ELGP)
! DUM5(3*ELGP,3*ELGP)
! DUM6(3,3*ELGP)

         DO I=1,3*ELGP
            DO J=1,3*ELGP
               DUM3(I,J) = ZERO
            ENDDO
         ENDDO

         IORD_MSG = ' '
         GAUSS_PT = 0
         DO K=1,IORD_K
            DO IJ=1,IORD_IJ
               GAUSS_PT = GAUSS_PT + 1
               CALL MATMULT_FFF ( KWW, CBAR, 3, 3, 3*ELGP, DUM6 )
               CALL MATMULT_FFF_T ( CBAR, DUM6, 3, 3*ELGP, 3*ELGP, DUM5 )
               INTFAC = DETJ(GAUSS_PT)*HH_IJ(IJ)*HH_K(K)
               DO L=1,3*ELGP
                  DO M=1,3*ELGP
                     DUM3(L,M) = DUM3(L,M) + DUM5(L,M)*INTFAC
                  ENDDO
               ENDDO
            ENDDO
         ENDDO

         DO I=1,3*ELGP
            DO J=1,3*ELGP
               KED(ID(I),ID(J)) = DUM3(I,J)
            ENDDO
         ENDDO

         DO I=2,6*ELGP                                     ! Set lower triangular portion of KE equal to upper portion
            DO J=1,I-1
               KED(I,J) = KED(J,I)
            ENDDO
         ENDDO

         IF (DEBUG(178) >= 1) THEN
            WRITE(F06,2001) TRIM(SUBR_NAME), OPT(6), LOAD_ISTEP, TYPE, EID
            K = 0
            DO I=1,ELGP
               K = 6*(I-1) + 1
               WRITE(F06,2101) (KED(K  ,J),J=1,3*ELGP)
               WRITE(F06,2101) (KED(K+1,J),J=1,3*ELGP)
               WRITE(F06,2101) (KED(K+2,J),J=1,3*ELGP)
               WRITE(F06,*)
               WRITE(F06,2101) (KED(K+3,J),J=1,3*ELGP)
               WRITE(F06,2101) (KED(K+4,J),J=1,3*ELGP)
               WRITE(F06,2101) (KED(K+5,J),J=1,3*ELGP)
               WRITE(F06,*)
            ENDDO
            WRITE(F06,*)
         ENDIF

      ENDIF

! **********************************************************************************************************************************
      IF (WRT_LOG >= SUBR_BEGEND) THEN
         CALL OURTIM
         WRITE(F04,9002) SUBR_NAME,TSEC
 9002    FORMAT(1X,A,' END  ',F10.3)
      ENDIF

      RETURN

! **********************************************************************************************************************************
 1925 FORMAT(' *ERROR  1925: ELEMENT ',I8,', TYPE ',A,', HAS ZERO OR NEGATIVE ',A,' = ',1ES9.1)

 1935 FORMAT(' *ERROR  1935: PROGRAMMING ERROR IN SUBROUTINE ',A                                                                   &
                    ,/,14X,' ELEMENT ',I8,', TYPE ',A,' HAS MATERIAL COUPLING BETWEEN DIRECT AND SHEAR STRESSES. THIS SUBR IS NOT',&
                           ' PROGRAMMED FOR THIS')

 1941 FORMAT(' *ERROR  1941: PROGRAMMING ERROR IN SUBROUTINE ',A                                                                   &
                    ,/,14X,' ELEMENT ',I8,', TYPE ',A,' HAS AN INVALID NUMBER OF NODES = ',I8)

 2001 FORMAT(' In ', A, ' with OPT(6) = ', A, ', and LOAD_ISTEP = ', I8, ': KED Differential Stiffness Matrix for ', A,            &
             ' element number ', I8)

 2101 FORMAT(1000(1ES14.6))

! **********************************************************************************************************************************

      END SUBROUTINE PENTA


*> @file geodinverse.for
*! @brief A test program for invers()

*> A simple program to solve the inverse geodesic problem.
*!
*! This program reads in lines with lat1, lon1, lon2, lat2 and prints
*! out lines with azi1, azi2, s12 (for the WGS84 ellipsoid).

      program geodinverse
      implicit none

      include 'geodesic.inc'

      double precision a, f, lat1, lon1, azi1, lat2, lon2, azi2, s12,
     +    dummy1, dummy2, dummy3, dummy4, dummy5
      integer omask

* WGS84 values
      a = 6378137d0
      f = 1/298.257223563d0

      omask = 0

 10   continue
      read(*, *, end=90, err=90) lat1, lon1, lat2, lon2
      call invers(a, f, lat1, lon1, lat2, lon2,
     +    s12, azi1, azi2, omask,
     +    dummy1, dummy2, dummy3, dummy4, dummy5)
      print 20, azi1, azi2, s12
 20   format(1x, f20.15, 1x, f20.15, 1x, f19.10)
      go to 10
 90   continue

      stop
      end


      SUBROUTINE FRBK2 (V1,V2,V3,VB)
C
C     LAST REVISED BY G.CHAN/UNISYS  11/1991
C     . ELIMINATE UN-NECCESSARY REWIND AND SKIP AFTER FIRST CALL TO THIS
C       ROUTINE (NASTRAN ORIGINAL METHOD)
C     . ADDITION OF A NEW BACKWARD-FORWARD SUBSTITUTION METHOD WHICH IS
C       MORE EFFICIENT, AND IS ALREADY GOOD FOR VECTORIZATION
C
CDB   LOGICAL          DEBUG
      INTEGER          BASE    ,BUF(6) ,LJJ(2)  ,IBLK(15)
      DOUBLE PRECISION V1(1)   ,V2(1)  ,V3(1)   ,VB(1)  ,XL(1)  ,XLJJ ,
     1                 ZERO    ,V3J    ,SUM
      COMMON  /OPINV / MCBLT(7),MCBSMA(7)
      COMMON  /SYSTEM/ KSYSTM  ,IO
      COMMON  /FEERXX/ DUMM(18),NZVB
      COMMON  /ZZZZZZ/ IZ(1)
      EQUIVALENCE      (XL(1),IZ(1))
      EQUIVALENCE      (LJJ(1) ,XLJJ)  ,(L16,DUMM(6))
      DATA     BUF   / 4HFRBK  ,4H2    ,2*4HBEGN ,4HEND  ,4HBGIN /
      DATA     ZERO  / 0.0D+0       /
CDB   DATA     DEBUG , ITER    ,MAX /  .FALSE.   ,0      ,3      /
C
CDB   IF (.NOT.DEBUG) GO TO 20
C     ITER = ITER + 1
C     IF (ITER .GT. MAX) DEBUG = .FALSE.
C     WRITE  (IO,10) NZVB,ITER
C  10 FORMAT ('  .... IN FRBK2.  NZVB =',I8,',  ITER =',I3)
C  20 CONTINUE
      NROW = MCBLT(2)
      DO 30 I = 1,NROW
   30 V2(I) = V1(I)
C
C     SELECTION OF ORIGINAL OR NEW FBS METHOD
C
      J = NROW
      IF (MCBLT(7) .LT. 0) GO TO 200
C
C     NASTRAN ORIGIANL METHOD
C
      IBLK( 1) = MCBLT(1)
      IBLK( 9) = 1
      IBLK(10) = 1
C
C     BACKWARD SUBSTITUTION
C
      IF (BUF(3) .EQ. BUF(5)) GO TO 40
C     BUF(3) = BUF(4)
      IF (L16 .NE. 0) CALL CONMSG (BUF,3,0)
C
C     REWIND AND SKIP TO COLUMN N
C
      CALL REWIND (MCBLT)
      CALL SKPREC (MCBLT,NROW+1)
      GO TO 50
C
C     ALREADY AT END, NO SKIP NEEDED
C
   40 BUF(3) = BUF(6)
      IF (L16 .NE. 0) CALL CONMSG (BUF,3,0)
C
   50 IBLK(8) = -1
   60 CALL GETSTB (*100,IBLK(1))
      NTMS = IBLK(6)
      JI   = IBLK(5)
      IK   = IBLK(4)
      IF (IK-NTMS+1 .NE. J) GO TO 70
      NTMS = NTMS - 1
      XLJJ = XL(JI-NTMS)
      IF (NTMS .EQ. 0) GO TO 90
   70 SUM  = ZERO
      DO 80 II = 1,NTMS
      SUM  = SUM + XL(JI)*V2(IK)
      JI   = JI - 1
      IK   = IK - 1
   80 CONTINUE
      V2(J)= V2(J) + SUM
   90 CALL ENDGTB (IBLK(1))
      GO TO 60
  100 V2(J)= V2(J)/XLJJ
      IF (J .EQ. 1) GO TO 110
      J    = J - 1
      GO TO 50
  110 CALL FRMLTD (MCBSMA(1),V2(1),V3(1),VB(1))
C
C     FORWARD SWEEP DIRECTLY ON V3
C
      DO 160 J = 1,NROW
      IBLK(8) = -1
  120 CALL GETSTR (*160,IBLK(1))
      JI   = IBLK(5)
      NTMS = IBLK(6)
      IK   = IBLK(4)
      IF (IK .NE. J) GO TO 130
      NTMS = NTMS - 1
      V3(J)= V3(J)/XL(JI)
      JI   = JI + 1
      IK   = IK + 1
  130 IF (NTMS .EQ. 0) GO TO 150
      V3J  = V3(J)
      IF (V3J .EQ. ZERO) GO TO 150
      DO 140 II = 1,NTMS
      V3(IK) = V3(IK) + XL(JI)*V3J
      IK   = IK + 1
      JI   = JI + 1
  140 CONTINUE
  150 CALL ENDGET (IBLK(1))
      GO TO 120
  160 CONTINUE
      GO TO 500
C
C     NEW METHOD
C
C     MATRIX MCBLT HAS BEEN RE-WRITTEN TO MCBLTX BY UNPSCR/FEER3. NO
C     STRING OPERATIONS HERE.
C
  200 IF (BUF(3) .EQ. BUF(5)) BUF(3) = BUF(6)
      IF (L16 .NE. 0) CALL CONMSG (BUF,3,0)
      MCBLTX = -MCBLT(7)
      IF (MOD(MCBLT(4),10) .NE. 3) GO TO 440
      NREC = 0
      CALL REWIND (MCBLTX)
      CALL FWDREC (*400,MCBLTX)
      NWDS = MCBLT(5)
C
C     IZ(1)                                                      GINO
C      / V1   V2    V3           VB (OPEN CORE LENGTH = NZVB)   BUFFERS
C     +-----+-----+-----+-----+-------------------------------+---------
C                          OPEN  CORE
C
C
C     BACKWARD SUBSTITUTION
C
C
      LL2  = 0
      BASE = 1
      IFB  = -350
      DO 280 IK = 1,NROW
      IF (BASE .LT. LL2) GO TO 240
      NREC = NREC + 1
CDB   IF (DEBUG) WRITE (IO,210) NREC,IFB
C 210 FORMAT ('  ...READING RECORD',I5,'.   IFB =',I5)
      CALL READ (*400,*220,MCBLTX,VB,NZVB,1,LL)
      CALL MESAGE (-8,0,NAM)
  220 LL2  = LL/NWDS
CDB   LL3  = LL2/30
C     LL4  = LL2 - LL3
C     IF (DEBUG) WRITE (IO,230) LL,NREC,LL2
C 230 FORMAT (1X,I10,' WORDS READ FROM RECORD NO.',I5,'.   LL2 =',I10)
      BASE = 1
  240 XLJJ = VB(BASE)
      II   = LJJ(1)
      JJ   = LJJ(2)
CDB   IF (DEBUG .AND. (BASE.LT.LL3 .OR. BASE.GT.LL4))
C    1    WRITE (IO,250) J,BASE,II,JJ,IFB
C 250 FORMAT (11X,'J,BASE,II,JJ,IFB =',5I8)
      IF (II .NE. J) GO TO 420
      NTMS = JJ - II + 1
      IB   = BASE + 2
      IE   = BASE + NTMS
      BASE = IE + 1
      IF (NTMS .LE. 1) GO TO 270
      SUM  = ZERO
      DO 260 I = IB,IE
      II   = II + 1
  260 SUM  = SUM + VB(I)*V2(II)
      V2(J)= V2(J) + SUM
  270 V2(J)= V2(J)/VB(IB-1)
  280 J    = J - 1
      CALL FRMLTD (MCBSMA(1),V2(1),V3(1),VB(1))
C
C     FORWARD SWEEP DIRECTLY ON V3
C
      IF (NROW .EQ. 1) GO TO 500
      NREC = 0
      LL2  = 0
      BASE = 1
      IFB  = +390
      DO 320 J = 1,NROW
      IF (BASE .LT. LL2) GO TO 300
      NREC = NREC + 1
CDB   IF (DEBUG) WRITE (IO,210) NREC,IFB
      CALL READ (*400,*290,MCBLTX,VB,NZVB,1,LL)
      CALL MESAGE (-8,0,NAM)
  290 LL2  = LL/NWDS
CDB   LL3  = LL2/30
C     LL4  = LL2 - LL3
C     IF (DEBUG) WRITE (IO,230) LL,NREC,LL2
      BASE = 1
  300 XLJJ = VB(BASE)
      II   = LJJ(1)
      JJ   = LJJ(2)
CDB   IF (DEBUG .AND. (BASE.LT.LL3 .OR. BASE.GT.LL4))
C    1    WRITE (IO,250) J,BASE,II,JJ,IFB
      IF (II .NE. J) GO TO 420
      NTMS = JJ - II + 1
      V3(J)= V3(J)/VB(BASE+1)
      IF (NTMS .LE. 1) GO TO 320
      V3J  = V3(J)
      IF (V3J .EQ. ZERO) GO TO 320
      IB   = BASE + 2
      IE   = BASE + NTMS
      DO 310 I = IB,IE
      II   = II + 1
  310 V3(II) = V3(II) + VB(I)*V3J
  320 BASE = BASE + NTMS + 1
      GO TO 500
C
  400 I = MCBLT(4)/10
      WRITE  (IO,410) NREC,J,I,IFB
  410 FORMAT ('0*** TRY TO READ RECORD',I5,'.  J,MCBLT(4),IFB =',I7,2I5)
      CALL MESAGE (-3,MCBLTX,NAM)
  420 WRITE  (IO,430) J,II,IFB
  430 FORMAT ('0*** ROW MISMATCH.  J,II,(IFB =',I7,I12,3H  (,I4)
      GO TO 460
  440 J = MOD(MCBLT(4),10)
      WRITE  (IO,450) J
  450 FORMAT ('0*** MCBLT MATRIX IN WRONG FORM.   UNPSCR FLAG =',2I3)
  460 CALL MESAGE (-37,0,BUF(1))
C
  500 BUF(3) = BUF(5)
      IF (L16 .NE. 0) CALL CONMSG (BUF,3,0)
      RETURN
      END