      subroutine reorganz(coefs, mbuse, nbuse, factor, a1,
     1   ntheta, nzeta)
C-----------------------------------------------
C   M o d u l e s
C-----------------------------------------------
      use vmec0
      implicit none
C-----------------------------------------------
C   D u m m y   A r g u m e n t s
C-----------------------------------------------
      integer mbuse, nbuse, ntheta, nzeta
      real(rprec) factor
      real(rprec), dimension(ntheta + 2,nzeta) :: a1
      complex(rprec), dimension(-mbuse:mbuse,0:nbuse) :: coefs
C-----------------------------------------------
C   L o c a l   V a r i a b l e s
C-----------------------------------------------
      integer :: j, m, i, n, ia
C-----------------------------------------------
c--
c  Version reorganz creates a COMPLEX coefs array.
c  Subroutine to reorganize and scale the output from fft991 and cft99 into complex
c  coefficients with index (m,n).  The coefficients are scaled by the
c  factor FACTOR.  This factor should be 1/(number of points in the
c  forward complex transform direction).  The original coefficients appear
c  in array a1.  The scaled coefficients will be
c  copied to array COEFS in the calling list.  Here, m is the poloidal
c  mode number and n is the toroidal mode number/periods. Because we use two
c  FORWARD transforms, we must flip the sign of m to get the desired nu
c  argument (u=theta, v=zeta)
c--
c
c- Because of (anti)symmetry, only nonnegative values of n are needed.  We also
c  only fill to m = mbuse and n = nbuse since this is all that will be
c  used in the sums over m and n.
c  Therefore, only i = 1 to mbuse+1 (for m = 0 to mbuse) and i = NTH+1-mbuse
c  to nth (for m = -mbuse to -1) and only j = 1 to nbuse+1 (for n = 0 to nbuse)
c  are needed.
c
      do j = 1, nbuse + 1
         n = j - 1
         do i = 1, mbuse + 1
            m = i - 1
            ia = 2*i - 1
            coefs(-m,n) = factor*cmplx(a1(ia,j),a1(ia+1,j))
         end do
      end do

      do j = nzeta + 1 - nbuse, nzeta
         n = -(nzeta + 1) + j
         do i = 1, mbuse + 1
            m = i - 1
            ia = 2*i - 1
            coefs(m,-n) = factor*cmplx(a1(ia,j),(-a1(ia+1,j)))
         end do
      end do

      coefs(1:mbuse,0) = coefs(-1:-mbuse:-1,0)

      end subroutine reorganz


      FUNCTION ERFCC(X)
      Z=ABS(X)
      T=1./(1.+0.5*Z)
      ERFCC=T*EXP(-Z*Z-1.26551223+T*(1.00002368+T*(.37409196+
     *    T*(.09678418+T*(-.18628806+T*(.27886807+T*(-1.13520398+
     *    T*(1.48851587+T*(-.82215223+T*.17087277)))))))))
      IF (X.LT.0.) ERFCC=2.-ERFCC
      RETURN
      END


C      +----------------------+
C      !     LITT splines     !
C      +----------------------+
C                      13.03.95

C      Partie 1: Queue, corps
C

       integer n,I,m
       parameter (n=10)
       parameter (m=200)
       double precision x(m+1),y(m+1),XN(n),YN(n),DN(n),W(3*n),x1,y1

C      ------------------------
C               Data
C      ------------------------

       data YN/0.0 , 0.3 , 0.35 , 0.4 , 0.6 , 1.2 , 1.55, 1.5 ,
     *         1.1 , 0.6/

       data XN/0.0 , 0.5 ,1.9, 2.0 , 2.1 , 2.5 ,
     *         3.5 , 4.0 , 5.0 , 5.9/

C      ------------------------
C        Interpolation spline
C      ------------------------
       call TB04AD(n,XN,YN,DN,W)

       x1=XN(1)
       do 10 I=1,m+1
          y1=TG01BD(1,n,XN,YN,DN,x1)
          x(I)=x1
          y(I)=y1
          x1=x1+(XN(n)-XN(1))/m
10     continue

       call tomat(n,m,XN,YN,x,y)

       end

C      ------------------------
C        Graphique -> Matlab
C      ------------------------

       subroutine tomat(n,m,XX,YY,X,Y)

       integer m,n,I
       double precision X(m),Y(m),XX(n),YY(n)

       open(UNIT = 2, FILE = 'snoo1.m')
          do 100 I=1,m+1
             write(2,*)'x1(',I,')=',X(I),';'
             write(2,*)'y1(',I,')=',Y(I),';'
100       continue

          do 200 I=1,n
             write(2,*)'xn1(',I,')=',XX(I),';'
             write(2,*)'yn1(',I,')=',YY(I),';'
200       continue

          write(2,*)'plot(xn1,yn1,''w+'',x1,y1,''w-'')'
          write(2,*)'axis(''equal'');grid'
       close (UNIT = 2)

       return
       end


      subroutine transport46_setaux_manifold(blockno, mx,my,mbc,
     &      xlower,ylower,dx,dy, area, edgelengths, aux, maux)
      implicit none

      integer mbc, mx,my, maux
      integer blockno
      double precision dx,dy, xlower, ylower
      double precision aux(1-mbc:mx+mbc,1-mbc:my+mbc, maux)

      double precision  edgelengths(-mbc:mx+mbc+2,-mbc:my+mbc+2,2)
      double precision        area(-mbc:mx+mbc+1,-mbc:my+mbc+1)

      integer i,j
      double precision dxdy

c     # ----------------------------------------------------------------
c     # Color equation (edge velocities)
c     # 1      capacity
c     # 2-3    Edge velocities at left/right x faces
c     # 4-5    Edge velocities at top/bottom y faces
c     # 6-7    Edge lengths (x-faces, y-faces)
c     3 8-9    Spherical coordinates
c     # ----------------------------------------------------------------

      dxdy = dx*dy

c     # Capacity : entry (1)
      do i = 1-mbc,mx+mbc
         do j = 1-mbc,my+mbc
            aux(i,j,1) = area(i,j)/dxdy
         enddo
      enddo

c     # Needed to scale speeds in Riemann solver when using
c     # cell-centered velocities
      do i = 1-mbc,mx+mbc
         do j = 1-mbc,my+mbc
c           # x-face and y-face edge lengths (6,7)
            aux(i,j,6) = edgelengths(i,j,1)/dy
            aux(i,j,7) = edgelengths(i,j,2)/dx
         enddo
      enddo

      return
      end


!*==cgtrfs.f90  processed by SPAG 7.51RB at 20:08 on  3 Mar 2022
!> \brief \b CGTRFS
!
!  =========== DOCUMENTATION ===========
!
! Online html documentation available at
!            http://www.netlib.org/lapack/explore-html/
!
!> \htmlonly
!> Download CGTRFS + dependencies
!>
!> [TGZ]
!>
!> [ZIP]
!>
!> [TXT]
!> \endhtmlonly
!
!  Definition:
!  ===========
!
!       SUBROUTINE CGTRFS( TRANS, N, NRHS, DL, D, DU, DLF, DF, DUF, DU2,
!                          IPIV, B, LDB, X, LDX, FERR, BERR, WORK, RWORK,
!                          INFO )
!
!       .. Scalar Arguments ..
!       CHARACTER          TRANS
!       INTEGER            INFO, LDB, LDX, N, NRHS
!       ..
!       .. Array Arguments ..
!       INTEGER            IPIV( * )
!       REAL               BERR( * ), FERR( * ), RWORK( * )
!       COMPLEX            B( LDB, * ), D( * ), DF( * ), DL( * ),
!      $                   DLF( * ), DU( * ), DU2( * ), DUF( * ),
!      $                   WORK( * ), X( LDX, * )
!       ..
!
!
!> \par Purpose:
!  =============
!>
!> \verbatim
!>
!> CGTRFS improves the computed solution to a system of linear
!> equations when the coefficient matrix is tridiagonal, and provides
!> error bounds and backward error estimates for the solution.
!> \endverbatim
!
!  Arguments:
!  ==========
!
!> \param[in] TRANS
!> \verbatim
!>          TRANS is CHARACTER*1
!>          Specifies the form of the system of equations:
!>          = 'N':  A * X = B     (No transpose)
!>          = 'T':  A**T * X = B  (Transpose)
!>          = 'C':  A**H * X = B  (Conjugate transpose)
!> \endverbatim
!>
!> \param[in] N
!> \verbatim
!>          N is INTEGER
!>          The order of the matrix A.  N >= 0.
!> \endverbatim
!>
!> \param[in] NRHS
!> \verbatim
!>          NRHS is INTEGER
!>          The number of right hand sides, i.e., the number of columns
!>          of the matrix B.  NRHS >= 0.
!> \endverbatim
!>
!> \param[in] DL
!> \verbatim
!>          DL is COMPLEX array, dimension (N-1)
!>          The (n-1) subdiagonal elements of A.
!> \endverbatim
!>
!> \param[in] D
!> \verbatim
!>          D is COMPLEX array, dimension (N)
!>          The diagonal elements of A.
!> \endverbatim
!>
!> \param[in] DU
!> \verbatim
!>          DU is COMPLEX array, dimension (N-1)
!>          The (n-1) superdiagonal elements of A.
!> \endverbatim
!>
!> \param[in] DLF
!> \verbatim
!>          DLF is COMPLEX array, dimension (N-1)
!>          The (n-1) multipliers that define the matrix L from the
!>          LU factorization of A as computed by CGTTRF.
!> \endverbatim
!>
!> \param[in] DF
!> \verbatim
!>          DF is COMPLEX array, dimension (N)
!>          The n diagonal elements of the upper triangular matrix U from
!>          the LU factorization of A.
!> \endverbatim
!>
!> \param[in] DUF
!> \verbatim
!>          DUF is COMPLEX array, dimension (N-1)
!>          The (n-1) elements of the first superdiagonal of U.
!> \endverbatim
!>
!> \param[in] DU2
!> \verbatim
!>          DU2 is COMPLEX array, dimension (N-2)
!>          The (n-2) elements of the second superdiagonal of U.
!> \endverbatim
!>
!> \param[in] IPIV
!> \verbatim
!>          IPIV is INTEGER array, dimension (N)
!>          The pivot indices; for 1 <= i <= n, row i of the matrix was
!>          interchanged with row IPIV(i).  IPIV(i) will always be either
!>          i or i+1; IPIV(i) = i indicates a row interchange was not
!>          required.
!> \endverbatim
!>
!> \param[in] B
!> \verbatim
!>          B is COMPLEX array, dimension (LDB,NRHS)
!>          The right hand side matrix B.
!> \endverbatim
!>
!> \param[in] LDB
!> \verbatim
!>          LDB is INTEGER
!>          The leading dimension of the array B.  LDB >= max(1,N).
!> \endverbatim
!>
!> \param[in,out] X
!> \verbatim
!>          X is COMPLEX array, dimension (LDX,NRHS)
!>          On entry, the solution matrix X, as computed by CGTTRS.
!>          On exit, the improved solution matrix X.
!> \endverbatim
!>
!> \param[in] LDX
!> \verbatim
!>          LDX is INTEGER
!>          The leading dimension of the array X.  LDX >= max(1,N).
!> \endverbatim
!>
!> \param[out] FERR
!> \verbatim
!>          FERR is REAL array, dimension (NRHS)
!>          The estimated forward error bound for each solution vector
!>          X(j) (the j-th column of the solution matrix X).
!>          If XTRUE is the true solution corresponding to X(j), FERR(j)
!>          is an estimated upper bound for the magnitude of the largest
!>          element in (X(j) - XTRUE) divided by the magnitude of the
!>          largest element in X(j).  The estimate is as reliable as
!>          the estimate for RCOND, and is almost always a slight
!>          overestimate of the true error.
!> \endverbatim
!>
!> \param[out] BERR
!> \verbatim
!>          BERR is REAL array, dimension (NRHS)
!>          The componentwise relative backward error of each solution
!>          vector X(j) (i.e., the smallest relative change in
!>          any element of A or B that makes X(j) an exact solution).
!> \endverbatim
!>
!> \param[out] WORK
!> \verbatim
!>          WORK is COMPLEX array, dimension (2*N)
!> \endverbatim
!>
!> \param[out] RWORK
!> \verbatim
!>          RWORK is REAL array, dimension (N)
!> \endverbatim
!>
!> \param[out] INFO
!> \verbatim
!>          INFO is INTEGER
!>          = 0:  successful exit
!>          < 0:  if INFO = -i, the i-th argument had an illegal value
!> \endverbatim
!
!> \par Internal Parameters:
!  =========================
!>
!> \verbatim
!>  ITMAX is the maximum number of steps of iterative refinement.
!> \endverbatim
!
!  Authors:
!  ========
!
!> \author Univ. of Tennessee
!> \author Univ. of California Berkeley
!> \author Univ. of Colorado Denver
!> \author NAG Ltd.
!
!> \date December 2016
!
!> \ingroup complexGTcomputational
!
!  =====================================================================
      SUBROUTINE CGTRFS(Trans,N,Nrhs,Dl,D,Du,Dlf,Df,Duf,Du2,Ipiv,B,Ldb, &
     &                  X,Ldx,Ferr,Berr,Work,Rwork,Info)
      IMPLICIT NONE
!*--CGTRFS213
!
!  -- LAPACK computational routine (version 3.7.0) --
!  -- LAPACK is a software package provided by Univ. of Tennessee,    --
!  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
!     December 2016
!
!     .. Scalar Arguments ..
      CHARACTER Trans
      INTEGER Info , Ldb , Ldx , N , Nrhs
!     ..
!     .. Array Arguments ..
      INTEGER Ipiv(*)
      REAL Berr(*) , Ferr(*) , Rwork(*)
      COMPLEX B(Ldb,*) , D(*) , Df(*) , Dl(*) , Dlf(*) , Du(*) , Du2(*) &
     &        , Duf(*) , Work(*) , X(Ldx,*)
!     ..
!
!  =====================================================================
!
!     .. Parameters ..
      INTEGER ITMAX
      PARAMETER (ITMAX=5)
      REAL ZERO , ONE
      PARAMETER (ZERO=0.0E+0,ONE=1.0E+0)
      REAL TWO
      PARAMETER (TWO=2.0E+0)
      REAL THREE
      PARAMETER (THREE=3.0E+0)
!     ..
!     .. Local Scalars ..
      LOGICAL notran
      CHARACTER transn , transt
      INTEGER count , i , j , kase , nz
      REAL eps , lstres , s , safe1 , safe2 , safmin
      COMPLEX zdum
!     ..
!     .. Local Arrays ..
      INTEGER isave(3)
!     ..
!     .. External Subroutines ..
      EXTERNAL CAXPY , CCOPY , CGTTRS , CLACN2 , CLAGTM , XERBLA
!     ..
!     .. Intrinsic Functions ..
      INTRINSIC ABS , AIMAG , CMPLX , MAX , REAL
!     ..
!     .. External Functions ..
      LOGICAL LSAME
      REAL SLAMCH
      EXTERNAL LSAME , SLAMCH
!     ..
!     .. Statement Functions ..
      REAL CABS1
!     ..
!     .. Statement Function definitions ..
      CABS1(zdum) = ABS(REAL(zdum)) + ABS(AIMAG(zdum))
!     ..
!     .. Executable Statements ..
!
!     Test the input parameters.
!
      Info = 0
      notran = LSAME(Trans,'N')
      IF ( .NOT.notran .AND. .NOT.LSAME(Trans,'T') .AND.                &
     &     .NOT.LSAME(Trans,'C') ) THEN
         Info = -1
      ELSEIF ( N<0 ) THEN
         Info = -2
      ELSEIF ( Nrhs<0 ) THEN
         Info = -3
      ELSEIF ( Ldb<MAX(1,N) ) THEN
         Info = -13
      ELSEIF ( Ldx<MAX(1,N) ) THEN
         Info = -15
      ENDIF
      IF ( Info/=0 ) THEN
         CALL XERBLA('CGTRFS',-Info)
         RETURN
      ENDIF
!
!     Quick return if possible
!
      IF ( N==0 .OR. Nrhs==0 ) THEN
         DO j = 1 , Nrhs
            Ferr(j) = ZERO
            Berr(j) = ZERO
         ENDDO
         RETURN
      ENDIF
!
      IF ( notran ) THEN
         transn = 'N'
         transt = 'C'
      ELSE
         transn = 'C'
         transt = 'N'
      ENDIF
!
!     NZ = maximum number of nonzero elements in each row of A, plus 1
!
      nz = 4
      eps = SLAMCH('Epsilon')
      safmin = SLAMCH('Safe minimum')
      safe1 = nz*safmin
      safe2 = safe1/eps
!
!     Do for each right hand side
!
      DO j = 1 , Nrhs
!
         count = 1
         lstres = THREE
         DO
!
!        Loop until stopping criterion is satisfied.
!
!        Compute residual R = B - op(A) * X,
!        where op(A) = A, A**T, or A**H, depending on TRANS.
!
            CALL CCOPY(N,B(1,j),1,Work,1)
            CALL CLAGTM(Trans,N,1,-ONE,Dl,D,Du,X(1,j),Ldx,ONE,Work,N)
!
!        Compute abs(op(A))*abs(x) + abs(b) for use in the backward
!        error bound.
!
            IF ( notran ) THEN
               IF ( N==1 ) THEN
                  Rwork(1) = CABS1(B(1,j)) + CABS1(D(1))*CABS1(X(1,j))
               ELSE
                  Rwork(1) = CABS1(B(1,j)) + CABS1(D(1))*CABS1(X(1,j))  &
     &                       + CABS1(Du(1))*CABS1(X(2,j))
                  DO i = 2 , N - 1
                     Rwork(i) = CABS1(B(i,j)) + CABS1(Dl(i-1))          &
     &                          *CABS1(X(i-1,j)) + CABS1(D(i))          &
     &                          *CABS1(X(i,j)) + CABS1(Du(i))           &
     &                          *CABS1(X(i+1,j))
                  ENDDO
                  Rwork(N) = CABS1(B(N,j)) + CABS1(Dl(N-1))             &
     &                       *CABS1(X(N-1,j)) + CABS1(D(N))             &
     &                       *CABS1(X(N,j))
               ENDIF
            ELSEIF ( N==1 ) THEN
               Rwork(1) = CABS1(B(1,j)) + CABS1(D(1))*CABS1(X(1,j))
            ELSE
               Rwork(1) = CABS1(B(1,j)) + CABS1(D(1))*CABS1(X(1,j))     &
     &                    + CABS1(Dl(1))*CABS1(X(2,j))
               DO i = 2 , N - 1
                  Rwork(i) = CABS1(B(i,j)) + CABS1(Du(i-1))             &
     &                       *CABS1(X(i-1,j)) + CABS1(D(i))             &
     &                       *CABS1(X(i,j)) + CABS1(Dl(i))              &
     &                       *CABS1(X(i+1,j))
               ENDDO
               Rwork(N) = CABS1(B(N,j)) + CABS1(Du(N-1))*CABS1(X(N-1,j))&
     &                    + CABS1(D(N))*CABS1(X(N,j))
            ENDIF
!
!        Compute componentwise relative backward error from formula
!
!        max(i) ( abs(R(i)) / ( abs(op(A))*abs(X) + abs(B) )(i) )
!
!        where abs(Z) is the componentwise absolute value of the matrix
!        or vector Z.  If the i-th component of the denominator is less
!        than SAFE2, then SAFE1 is added to the i-th components of the
!        numerator and denominator before dividing.
!
            s = ZERO
            DO i = 1 , N
               IF ( Rwork(i)>safe2 ) THEN
                  s = MAX(s,CABS1(Work(i))/Rwork(i))
               ELSE
                  s = MAX(s,(CABS1(Work(i))+safe1)/(Rwork(i)+safe1))
               ENDIF
            ENDDO
            Berr(j) = s
!
!        Test stopping criterion. Continue iterating if
!           1) The residual BERR(J) is larger than machine epsilon, and
!           2) BERR(J) decreased by at least a factor of 2 during the
!              last iteration, and
!           3) At most ITMAX iterations tried.
!
            IF ( Berr(j)>eps .AND. TWO*Berr(j)<=lstres .AND.            &
     &           count<=ITMAX ) THEN
!
!           Update solution and try again.
!
               CALL CGTTRS(Trans,N,1,Dlf,Df,Duf,Du2,Ipiv,Work,N,Info)
               CALL CAXPY(N,CMPLX(ONE),Work,1,X(1,j),1)
               lstres = Berr(j)
               count = count + 1
               CYCLE
            ENDIF
!
!        Bound error from formula
!
!        norm(X - XTRUE) / norm(X) .le. FERR =
!        norm( abs(inv(op(A)))*
!           ( abs(R) + NZ*EPS*( abs(op(A))*abs(X)+abs(B) ))) / norm(X)
!
!        where
!          norm(Z) is the magnitude of the largest component of Z
!          inv(op(A)) is the inverse of op(A)
!          abs(Z) is the componentwise absolute value of the matrix or
!             vector Z
!          NZ is the maximum number of nonzeros in any row of A, plus 1
!          EPS is machine epsilon
!
!        The i-th component of abs(R)+NZ*EPS*(abs(op(A))*abs(X)+abs(B))
!        is incremented by SAFE1 if the i-th component of
!        abs(op(A))*abs(X) + abs(B) is less than SAFE2.
!
!        Use CLACN2 to estimate the infinity-norm of the matrix
!           inv(op(A)) * diag(W),
!        where W = abs(R) + NZ*EPS*( abs(op(A))*abs(X)+abs(B) )))
!
            DO i = 1 , N
               IF ( Rwork(i)>safe2 ) THEN
                  Rwork(i) = CABS1(Work(i)) + nz*eps*Rwork(i)
               ELSE
                  Rwork(i) = CABS1(Work(i)) + nz*eps*Rwork(i) + safe1
               ENDIF
            ENDDO
!
            kase = 0
            EXIT
         ENDDO
         DO
            CALL CLACN2(N,Work(N+1),Work,Ferr(j),kase,isave)
            IF ( kase/=0 ) THEN
               IF ( kase==1 ) THEN
!
!              Multiply by diag(W)*inv(op(A)**H).
!
                  CALL CGTTRS(transt,N,1,Dlf,Df,Duf,Du2,Ipiv,Work,N,    &
     &                        Info)
                  DO i = 1 , N
                     Work(i) = Rwork(i)*Work(i)
                  ENDDO
               ELSE
!
!              Multiply by inv(op(A))*diag(W).
!
                  DO i = 1 , N
                     Work(i) = Rwork(i)*Work(i)
                  ENDDO
                  CALL CGTTRS(transn,N,1,Dlf,Df,Duf,Du2,Ipiv,Work,N,    &
     &                        Info)
               ENDIF
               CYCLE
            ENDIF
!
!        Normalize error.
!
            lstres = ZERO
            DO i = 1 , N
               lstres = MAX(lstres,CABS1(X(i,j)))
            ENDDO
            IF ( lstres/=ZERO ) Ferr(j) = Ferr(j)/lstres
            EXIT
         ENDDO
!
      ENDDO
!
!
!     End of CGTRFS
!
      END SUBROUTINE CGTRFS


module mixing_r

use types
use dft_data
use mixings
implicit none
private
public R1, R2, R3

contains

function R1(x, i, tmp)
real(dp), intent(in) :: x(:)
integer, intent(in) :: i
type(dft_data_t), intent(inout) :: tmp
real(dp) :: R1(size(x))

real(dp) :: d(size(x)), c
print *, i, associated(tmp%ks_energies)

d = (/ 3.0_dp, 2.0_dp, 1.5_dp, 1.0_dp, 0.5_dp /)
c = 0.01_dp
R1 = -d*x - c*sum(x*x)*x
end function

function R2(x, i, tmp)
real(dp), intent(in) :: x(:)
integer, intent(in) :: i
type(dft_data_t), intent(inout) :: tmp
real(dp) :: R2(size(x))
print *, i, associated(tmp%ks_energies)

R2 = cos(x) - x
end function

function R3(x, i, tmp)
real(dp), intent(in) :: x(:)
integer, intent(in) :: i
type(dft_data_t), intent(inout) :: tmp
real(dp) :: R3(size(x))

real(dp) :: a, b
real(dp), parameter :: c = 2.0_dp
print *, i, associated(tmp%ks_energies)
a = x(1)
b = x(2)
R3 = a**2 + b**2 - c**2
end function

end module

program mixings_test
! Tests all mixing schemes, that they work as excpected.
use types
use mixings
use dft_data
use mixing_r
use utils
implicit none

type(dft_data_t) :: d
real(dp) :: x0(5), x(5), y(50), y0(50), z(2), z0(2)
real(dp), target :: tmp1(5), tmp2(50), tmp3(2)
integer :: i

x0(:) = 1.0_dp

! For anderson mixing:
tmp1(:) = 1.0_dp
d%R => tmp1

x = mixing_linear(R1, x0, 60, 0.5_dp, d)
call assert(sqrt(sum(x**2)) < 1e-7_dp)
call assert(sqrt(sum(R1(x, 1, d)**2)) < 1e-7_dp)

x = mixing_linear_adapt(R1, x0, 20, 0.5_dp, d)
call assert(sqrt(sum(x**2)) < 1e-5_dp)
call assert(sqrt(sum(R1(x, 1, d)**2)) < 1e-5_dp)

x = mixing_anderson(R1, x0, 35, .false., d, alpha=0.5_dp, eps=0.0_dp)
call assert(sqrt(sum(x**2)) < 1e-5_dp)
call assert(sqrt(sum(R1(x, 1, d)**2)) < 1e-5_dp)

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

do i = 1, size(y)
    y0(i) = i
end do

! For anderson mixing:
tmp2(:) = 1.0_dp
d%R => tmp2

y = mixing_linear(R2, y0, 50, 1.0_dp, d)
call assert(sum((y-0.73908513321516067)**2) < 1e-10_dp)

y = mixing_linear_adapt(R2, y0, 20, 0.1_dp, d)
call assert(sum((y-0.73908513321516067)**2) < 1e-10_dp)

y = mixing_anderson(R2, y0, 9, .false., d, alpha=0.5_dp, eps=0.0_dp)
call assert(sum((y-0.73908513321516067)**2) < 1e-10_dp)

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

z0 = (/ 0.5, 0.5 /)

! For anderson mixing:
tmp3(:) = 1.0_dp
d%R => tmp3

z = mixing_linear(R3, z0, 40, 0.1_dp, d)
call assert(abs(z(1)**2 + z(2)**2 - 2.0_dp**2) < 1e-10_dp)

z = mixing_linear_adapt(R3, z0, 30, 0.1_dp, d)
call assert(abs(z(1)**2 + z(2)**2 - 2.0_dp**2) < 1e-10_dp)

z = mixing_anderson(R3, z0, 8, .false., d, alpha=0.5_dp, eps=0.0_dp)
call assert(abs(z(1)**2 + z(2)**2 - 2.0_dp**2) < 1e-10_dp)

end program


*     DG01MD EXAMPLE PROGRAM TEXT
*     Copyright (c) 2002-2020 NICONET e.V.
*
*     .. Parameters ..
      INTEGER          NIN, NOUT
      PARAMETER        ( NIN = 5, NOUT = 6 )
      INTEGER          NMAX
      PARAMETER        ( NMAX = 128 )
*     .. Local Scalars ..
      INTEGER          I, INFO, N
      CHARACTER*1      INDI
*     .. Local Arrays ..
      DOUBLE PRECISION XI(NMAX), XR(NMAX)
*     .. External Subroutines ..
      EXTERNAL         DG01MD
*     .. Executable Statements ..
*
      WRITE ( NOUT, FMT = 99999 )
*     Skip the heading in the data file and read the data.
      READ ( NIN, FMT = '()' )
      READ ( NIN, FMT = * ) N, INDI
      IF ( N.LE.0 .OR. N.GT.NMAX ) THEN
         WRITE ( NOUT, FMT = 99995 ) N
      ELSE
         READ ( NIN, FMT = * ) ( XR(I), XI(I), I = 1,N )
*        Find the Fourier transform of the given complex signal.
         CALL DG01MD( INDI, N, XR, XI, INFO )
*
         IF ( INFO.NE.0 ) THEN
            WRITE ( NOUT, FMT = 99998 ) INFO
         ELSE
            WRITE ( NOUT, FMT = 99997 )
            DO 20 I = 1, N
               WRITE ( NOUT, FMT = 99996 ) I, XR(I), XI(I)
   20       CONTINUE
         END IF
      END IF
      STOP
*
99999 FORMAT (' DG01MD EXAMPLE PROGRAM RESULTS',/1X)
99998 FORMAT (' INFO on exit from DG01MD = ',I2)
99997 FORMAT (' Components of Fourier transform are',//'   i',6X,
     $       'XR(i)',6X,'XI(i)',/)
99996 FORMAT (I4,3X,F8.4,3X,F8.4)
99995 FORMAT (/' N is out of range.',/' N = ',I5)
      END


MODULE m_supercheck
!*********************************************************************
!     checks whether this is a supercell and determines the
!     translations that take the crystal in to itself
!*********************************************************************
CONTAINS
  SUBROUTINE super_check(nat,pos,ity,ntypm, ns,trs)

    USE m_constants

    IMPLICIT NONE

    !==> Arguments
    INTEGER, INTENT (IN)  :: nat,ntypm,ity(nat)
    REAL,    INTENT (IN)  :: pos(3,nat)
    INTEGER, INTENT (OUT) :: ns       ! size of supercell
    REAL,    INTENT (OUT) :: trs(3,*) ! translations
    !==> Locals
    INTEGER i,j,n,ntysmin
    INTEGER ntys(ntypm)
    REAL    tr(3)
    LOGICAL l_f

    REAL, PARAMETER :: eps=1.e-7

    ns = 1
    trs(1:3,ns) = (/ 0.000 , 0.000 , 0.000 /)

    !---> check if possible to be a supercell
    ntys(1:ntypm) = 0
    DO n = 1, nat
       ntys( ity(n) ) = ntys( ity(n) ) + 1
    ENDDO
    ntysmin = MINVAL( ntys )

    IF ( ntysmin == 1 ) RETURN  ! not a supercell if one atom different

    DO i = ntysmin,2,-1                 ! check ratios of atoms
       IF ( MOD(ntysmin,i).NE.0 ) CYCLE ! only factors of ntysmin allowed
       l_f = .TRUE.
       DO n = 1, ntypm
          IF ( MOD( ntys(n), i ) .NE. 0 ) THEN
             l_f = .FALSE.
             EXIT
          ENDIF
       ENDDO
       IF (l_f) THEN
          ns = i    ! possible value
          EXIT
       ENDIF
    ENDDO
    IF (ns == 1) RETURN

    !---> based on number of atoms (and type), possibly a supercell;
    !---> now need to check by doing translations

    ns = 1
    !---> get possible shifts
    DO j = 1, nat-1
       shift_i:  DO i = j+1, nat  ! -ve shifts will come through naturally

          tr(:) = pos(:,i)-pos(:,j) - ANINT( pos(:,i)-pos(:,j) - eps )

          !--->       check if already done
          DO n=1,ns
             IF ( ALL( ABS( tr(:)-trs(:,n) ) < eps ) ) CYCLE shift_i
          ENDDO

          IF ( l_shiftm(tr,pos,nat) ) THEN
             ns = ns + 1
             trs(:,ns) = tr(:)
          ENDIF

       ENDDO shift_i
    ENDDO

    IF ( ns > 1 ) THEN
       WRITE(oUnit,'(/," The system appears to be a supercell"," containing",i4," primitive cells:")') ns
       DO n = 1, ns
          WRITE (oUnit,'(i8,3f12.6)') n,trs(1:3,n)
       ENDDO
    ENDIF

  CONTAINS ! internal function

    LOGICAL FUNCTION l_shiftm(tr,pos,nat)
      !********************************************************************
      !     determines whether the vector tr is a translation of the
      !     crystal (non-primitive for supercell)
      !********************************************************************
      IMPLICIT NONE

      INTEGER, INTENT(IN) :: nat
      REAL,    INTENT(IN) :: tr(3)
      REAL,    INTENT(IN) :: pos(3,nat)

      REAL    rp(3)
      INTEGER i,j,in

      l_shiftm = .FALSE.

      DO i = 1, nat
         !--->    rotated and shifted atom, reduced to (-1/2,1/2]
         rp(:) = pos(:,i) + tr(:) - ANINT( pos(:,i) + tr(:) - eps )
         !--->    find which atom, if any, this matches
         in = 0
         DO j = 1, nat
            IF ( ity(i).NE.ity(j) ) CYCLE
            !            if( all( abs(pos(:,j)-rp(:) ) < eps ) ) then
            ! causes problem with intel compiler ifc Version 5.0.1 (gs2001-11-07)
            IF ( ABS(pos(1,j)-rp(1) ) < eps  .AND. ABS(pos(2,j)-rp(2) ) < eps  .AND.&
                 ABS(pos(3,j)-rp(3) ) < eps  ) THEN
               in = j
               EXIT
            ENDIF
         ENDDO
         IF (in == 0 ) RETURN
      ENDDO

      l_shiftm = .TRUE.  !  only if everything matches

    END FUNCTION l_shiftm

  END SUBROUTINE super_check
END MODULE m_supercheck


      SUBROUTINE FODE(S,Y,YP,YPOLD,A,QR,ALPHA,TZ,PIVOT,NFE,N,IFLAG,
     $                PAR,IPAR)
C
C SUBROUTINE  FODE  IS USED BY SUBROUTINE  STEPS  TO SPECIFY THE
C ORDINARY DIFFERENTIAL EQUATION  DY/DS = G(S,Y) , WHOSE SOLUTION
C IS THE ZERO CURVE OF THE HOMOTOPY MAP.  S = ARC LENGTH,
C YP = DY/DS, AND  Y(S) = (LAMBDA(S), X(S)) .
C
C PAR(1:*) AND IPAR(1:*) ARE ARRAYS FOR (OPTIONAL) USER PARAMETERS,
C    WHICH ARE SIMPLY PASSED THROUGH TO THE USER WRITTEN SUBROUTINES
C    RHOA, RHOJAC.
C
      DOUBLE PRECISION DDOT,DNRM2,S,SUM,YPNORM
      INTEGER I,IERR,IFLAG,IK,J,K,KP1,KPIV,LW,N,NFE,NP1
C
C *****  ARRAY DECLARATIONS.  *****
C
      DOUBLE PRECISION Y(N+1),YP(N+1),YPOLD(N+1),A(N),PAR(1)
      INTEGER IPAR(1)
C
C ARRAYS FOR COMPUTING THE JACOBIAN MATRIX AND ITS KERNEL.
      DOUBLE PRECISION QR(N,N+1),ALPHA(N),TZ(N+1)
      INTEGER PIVOT(N+1)
C
C *****  END OF DIMENSIONAL INFORMATION.  *****
C
C
      NP1=N+1
      NFE=NFE+1
C NFE CONTAINS THE NUMBER OF JACOBIAN EVALUATIONS.
C   *   *   *   *   *   *   *   *   *   *   *   *   *   *   *   *   *
C
C COMPUTE THE JACOBIAN MATRIX, STORE IT IN QR.
C
      IF (IFLAG .EQ. -2) THEN
C
C  QR = ( D RHO(A,LAMBDA,X)/D LAMBDA , D RHO(A,LAMBDA,X)/DX )  .
C
        DO 30 K=1,NP1
          CALL RHOJAC(A,Y(1),Y(2),QR(1,K),K,PAR,IPAR)
30      CONTINUE
      ELSE
        CALL F(Y(2),TZ)
        IF (IFLAG .EQ. 0) THEN
C
C      QR = ( A - F(X), I - LAMBDA*DF(X) )  .
C
          DO 100 J=1,N
100       QR(J,1)=A(J)-TZ(J)
          DO 120 K=1,N
            CALL FJAC(Y(2),TZ,K)
            KP1=K+1
            DO 110 J=1,N
110         QR(J,KP1)=-Y(1)*TZ(J)
120       QR(K,KP1)=1.0+QR(K,KP1)
        ELSE
C
C   QR = ( F(X) - X + A, LAMBDA*DF(X) + (1 - LAMBDA)*I ) .
C
140       DO 150 J=1,N
150       QR(J,1)=TZ(J)-Y(J+1)+A(J)
          DO 170 K=1,N
            CALL FJAC(Y(2),TZ,K)
            KP1=K+1
            DO 160 J=1,N
160         QR(J,KP1)=Y(1)*TZ(J)
170       QR(K,KP1)=1.0-Y(1)+QR(K,KP1)
        ENDIF
      ENDIF
C
C   *   *   *   *   *   *   *   *   *   *   *   *   *   *   *   *   *
C REDUCE THE JACOBIAN MATRIX TO UPPER TRIANGULAR FORM.
210   CALL DCPOSE(N,N,QR,ALPHA,PIVOT,IERR,TZ,YP)
      IF (IERR .EQ. 0) GO TO 220
      IFLAG=4
      RETURN
C COMPUTE KERNEL OF JACOBIAN, WHICH SPECIFIES YP=DY/DS.
220   TZ(NP1)=1.0
      DO 240 LW=1,N
        I=NP1-LW
        IK=I+1
        SUM=0.0
        DO 230 J=IK,NP1
230     SUM=SUM+QR(I,J)*TZ(J)
240   TZ(I)=-SUM/ALPHA(I)
      YPNORM=DNRM2(NP1,TZ,1)
      DO 260 K=1,NP1
        KPIV=PIVOT(K)
260   YP(KPIV)=TZ(K)/YPNORM
      IF (DDOT(NP1,YP,1,YPOLD,1) .GE. 0.0) GO TO 280
      DO 270 I=1,NP1
270   YP(I)=-YP(I)
C
C SAVE CURRENT DERIVATIVE (= TANGENT VECTOR) IN  YPOLD .
280   DO 290 I=1,NP1
290   YPOLD(I)=YP(I)
      RETURN
      END


module quarter_cosine_transform_routines

    use auxiliary_routines

    use fftpack_precision, only: &
        wp, & ! working precision
        ip ! integer precision

    use real_transform_routines

    ! Explicit typing only
    implicit none

    ! Everything is private unless stated otherwise
    private
    public :: cosq1i, cosq1b, cosq1f, cosqmi, cosqmb, cosqmf

    !---------------------------------------------------------------------------------
    ! Variables confined to the module
    !---------------------------------------------------------------------------------
    real (wp), parameter :: ZERO = 0.0_wp
    real (wp), parameter :: HALF = 0.5_wp
    real (wp), parameter :: ONE = 1.0_wp
    real (wp), parameter :: TWO = 2.0_wp
    real (wp), parameter :: THREE = 3.0_wp
    real (wp), parameter :: FOUR = 4.0_wp
    real (wp), parameter :: FIVE = 5.0_wp
    !---------------------------------------------------------------------------------

contains

    subroutine cosq1b(n, inc, x, lenx, wsave, lensav, work, lenwrk, ier)
        !
        ! cosq1b: real backward cosine quarter wave transform, 1d.
        !
        !  Purpose:
        !
        !  cosq1b computes the one-dimensional fourier transform of a sequence
        !  which is a cosine series with odd wave numbers.  this transform is
        !  referred to as the backward transform or fourier synthesis, transforming
        !  the sequence from spectral to physical space.
        !
        !  this transform is normalized since a call to cosq1b followed
        !  by a call to cosq1f (or vice-versa) reproduces the original
        !  array  within roundoff error.
        !
        !  integer n, the number of elements to be transformed
        !  in the sequence.  the transform is most efficient when n is a
        !  product of small primes.
        !
        !  integer inc, the increment between the locations,
        !  in array r, of two consecutive elements within the sequence.
        !
        !  input/real r(lenr); on input, containing the sequence
        !  to be transformed, and on output, containing the transformed sequence.
        !
        !  integer lenr, the dimension of the r array.
        !  lenr must be at least inc*(n-1)+ 1.
        !
        !  input, real (wp) wsave(lensav).  wsave's contents must be
        !  initialized with a call to cosq1i before the first call to routine cosq1f
        !  or cosq1b for a given transform length n.  wsave's contents may be
        !  re-used for subsequent calls to cosq1f and cosq1b with the same n.
        !
        !  integer lensav, the dimension of the wsave array.
        !  lensav must be at least 2*n + int(log(real(n))) + 4.
        !
        !  workspace, real (wp) work(lenwrk).
        !
        !  integer lenwrk, the dimension of the work array.
        !  lenwrk must be at least n.
        !
        !  integer ier, error_flag.
        !  0, successful exit;
        !  1, input parameter lenr not big enough;
        !  2, input parameter lensav not big enough;
        !  3, input parameter lenwrk not big enough;
        !  20, input error returned by lower level routine.
        !
        integer (ip) inc
        integer (ip) lensav
        integer (ip) lenwrk

        integer (ip) ier
        integer (ip) local_error_flag
        integer (ip) lenx
        integer (ip) n
        real (wp) work(lenwrk)
        real (wp) wsave(lensav)
        real (wp) x(inc,*)
        real (wp) x1

        !
        !==> Check validity of input arguments
        !
        if (lenx < inc*(n-1) + 1) then
            ier = 1
            call fft_error_handler('cosq1b', 6)
            return
        else if (lensav < get_cost_1d_saved_workspace_length(n) ) then
            ier = 2
            call fft_error_handler('cosq1b', 8)
            return
        else if (lenwrk < get_cost_1d_workspace_length(n)) then
            ier = 3
            call fft_error_handler('cosq1b', 10)
            return
        else
            ier = 0
        end if

        !
        !==> Perform transform
        !
        if (n < 2) then
            return
        end if

        select case (n)
            case (2)
                x1 = x(1,1)+x(1,2)
                x(1,2) = (x(1,1)-x(1,2))/sqrt(TWO)
                x(1,1) = x1
            case default

                call cosqb1(n,inc,x,wsave,work,local_error_flag)

                ! check error_flag
                if (local_error_flag /= 0) then
                    ier = 20
                    call fft_error_handler('cosq1b',-5)
                end if

        end select

    end subroutine cosq1b

    subroutine cosq1f(n, inc, x, lenx, wsave, lensav, work, lenwrk, ier)
        !
        ! cosq1f: real forward cosine quarter wave transform, 1d.
        !
        !  Purpose:
        !
        !  cosq1f computes the one-dimensional fourier transform of a sequence
        !  which is a cosine series with odd wave numbers.  this transform is
        !  referred to as the forward transform or fourier analysis, transforming
        !  the sequence from physical to spectral space.
        !
        !  this transform is normalized since a call to cosq1f followed
        !  by a call to cosq1b (or vice-versa) reproduces the original
        !  array  within roundoff error.
        !
        !  parameters:
        !
        !  integer n, the number of elements to be transformed
        !  in the sequence.  the transform is most efficient when n is a
        !  product of small primes.
        !
        !  integer inc, the increment between the locations,
        !  in array r, of two consecutive elements within the sequence.
        !
        !  input/real r(lenr); on input, containing the sequence
        !  to be transformed, and on output, containing the transformed sequence.
        !
        !  integer lenr, the dimension of the r array.
        !  lenr must be at least inc*(n-1)+ 1.
        !
        !  input, real (wp) wsave(lensav).  wsave's contents must be
        !  initialized with a call to cosq1i before the first call to routine cosq1f
        !  or cosq1b for a given transform length n.  wsave's contents may be
        !  re-used for subsequent calls to cosq1f and cosq1b with the same n.
        !
        !  integer lensav, the dimension of the wsave array.
        !  lensav must be at least 2*n + int(log(real(n))) + 4.
        !
        !  workspace, real (wp) work(lenwrk).
        !
        !  integer lenwrk, the dimension of the work array.
        !  lenwrk must be at least n.
        !
        !  integer ier, error_flag.
        !  0, successful exit;
        !  1, input parameter lenr not big enough;
        !  2, input parameter lensav not big enough;
        !  3, input parameter lenwrk not big enough;
        !  20, input error returned by lower level routine.
        !
        integer (ip) inc
        integer (ip) lensav
        integer (ip) lenwrk

        integer (ip) ier
        integer (ip) local_error_flag
        integer (ip) n
        integer (ip) lenx
        real (wp) tsqx
        real (wp) work(lenwrk)
        real (wp) wsave(lensav)
        real (wp) x(inc,*)

        !
        !==> Check validity of calling arguments
        !
        if (lenx < inc*(n-1) + 1) then
            ier = 1
            call fft_error_handler('cosq1f', 6)
            return
        else if (lensav < get_cost_1d_saved_workspace_length(n) ) then
            ier = 2
            call fft_error_handler('cosq1f', 8)
            return
        else if (lenwrk < get_cost_1d_workspace_length(n)) then
            ier = 3
            call fft_error_handler('cosq1f', 10)
            return
        else
            ier = 0
        end if

        !
        !==> Perform transform
        !
        if (n < 2) then
            return
        else if (n == 2) then
            tsqx = x(1,2)/sqrt(TWO)
            x(1,2) = HALF *x(1,1)-tsqx
            x(1,1) = HALF *x(1,1)+tsqx
        else
            ! Peform cosine transform
            call cosqf1(n,inc,x,wsave,work,local_error_flag)

            ! Check error_flag
            if (local_error_flag /= 0) then
                ier = 20
                call fft_error_handler('cosq1f',-5)
            end if
        end if

    end subroutine cosq1f

    subroutine cosq1i(n, wsave, lensav, ier)
        !
        ! cosq1i: initialization for cosq1b and cosq1f.
        !
        !  Purpose:
        !
        !  cosq1i initializes array wsave for use in its companion routines
        !  cosq1f and cosq1b.  the prime factorization of n together with a
        !  tabulation of the trigonometric functions are computed and stored
        !  in array wsave.  separate wsave arrays are required for different
        !  values of n.
        !
        !  parameters:
        !
        !  integer n, the length of the sequence to be
        !  transformed.  the transform is most efficient when n is a product
        !  of small primes.
        !
        !  integer lensav, the dimension of the wsave array.
        !  lensav must be at least 2*n + int(log(real(n))) + 4.
        !
        !  real wsave(lensav), containing the prime factors of n
        !  and also containing certain trigonometric values which will be used
        !  in routines cosq1b or cosq1f.
        !
        !  integer ier, error_flag.
        !  0, successful exit;
        !  2, input parameter lensav not big enough;
        !  20, input error returned by lower level routine.
        !
        integer (ip) lensav
        real (wp) dt
        real (wp) fk
        integer (ip) ier
        integer (ip) local_error_flag
        integer (ip) k
        integer (ip) n
        real (wp), parameter :: HALF_PI = acos(-ONE)/2
        real (wp) wsave(lensav)

        !
        !==> Check validity of input arguments
        !
        if (lensav < get_cost_1d_saved_workspace_length(n)) then
            ier = 2
            call fft_error_handler('cosq1i', 3)
            return
        else
            ier = 0
        end if

        !
        !==> Perform transform
        !
        dt = HALF_PI /n
        fk = ZERO

        do k=1,n
            fk = fk + ONE
            wsave(k) = cos(fk*dt)
        end do

        associate( lnsv => n+int(log(real(n, kind=wp))/log(TWO))+4 )

            call rfft1i(n, wsave(n+1), lnsv, local_error_flag)

        end associate

        if (local_error_flag /= 0) then
            ier = 20
            call fft_error_handler('cosq1i',-5)
        end if

    end subroutine cosq1i

    subroutine cosqb1(n, inc, x, wsave, work, ier)

        integer (ip) inc

        integer (ip) i
        integer (ip) ier
        integer (ip) local_error_flag
        integer (ip) k
        integer (ip) kc
        integer (ip) modn
        integer (ip) n
        integer (ip) np2
        integer (ip) ns2
        real (wp) work(*)
        real (wp) wsave(*)
        real (wp) x(inc,*)
        real (wp) xim1

        ier = 0
        ns2 = (n+1)/2
        np2 = n+2

        do i=3,n,2
            xim1 = x(1,i-1)+x(1,i)
            x(1,i) = HALF * (x(1,i-1)-x(1,i))
            x(1,i-1) = HALF * xim1
        end do

        x(1,1) = HALF * x(1,1)
        modn = mod(n,2)

        if (modn == 0) then
            x(1,n) = HALF * x(1,n)
        end if

        associate( &
            lenx => inc*(n-1)  + 1, &
            lnsv => n + int(log(real(n, kind=wp) )/log(TWO)) + 4, &
            lnwk => n &
            )

            call rfft1b(n,inc,x,lenx,wsave(n+1),lnsv,work,lnwk,local_error_flag)

        end associate

        if (local_error_flag /= 0) then
            ier = 20
            call fft_error_handler('cosqb1',-5)
            return
        end if

        do k=2,ns2
            kc = np2-k
            work(k) = wsave(k-1)*x(1,kc)+wsave(kc-1)*x(1,k)
            work(kc) = wsave(k-1)*x(1,k)-wsave(kc-1)*x(1,kc)
        end do

        if (modn == 0) then
            x(1,ns2+1) = TWO * wsave(ns2) * x(1,ns2+1)
        end if

        do k=2,ns2
            kc = np2-k
            x(1,k) = work(k)+work(kc)
            x(1,kc) = work(k)-work(kc)
        end do

        x(1,1) = TWO * x(1,1)

    end subroutine cosqb1

    subroutine cosqf1(n, inc, x, wsave, work, ier)

        integer (ip) inc

        integer (ip) i
        integer (ip) ier
        integer (ip) local_error_flag
        integer (ip) k
        integer (ip) kc
        integer (ip) modn
        integer (ip) n
        integer (ip) np2
        integer (ip) ns2
        real (wp) work(*)
        real (wp) wsave(*)
        real (wp) x(inc,*)
        real (wp) xim1

        ier = 0
        ns2 = (n+1)/2
        np2 = n+2

        do k=2,ns2
            kc = np2-k
            work(k)  = x(1,k)+x(1,kc)
            work(kc) = x(1,k)-x(1,kc)
        end do

        modn = mod(n,2)

        if (modn == 0) then
            work(ns2+1) = TWO * x(1,ns2+1)
        end if

        do k=2,ns2
            kc = np2-k
            x(1,k)  = wsave(k-1)*work(kc)+wsave(kc-1)*work(k)
            x(1,kc) = wsave(k-1)*work(k) -wsave(kc-1)*work(kc)
        end do

        if (modn == 0) then
            x(1,ns2+1) = wsave(ns2)*work(ns2+1)
        end if

        associate( &
            lenx => inc*(n-1)  + 1, &
            lnsv => n + int(log(real(n, kind=wp) )/log(TWO)) + 4, &
            lnwk => n &
            )

            call rfft1f(n,inc,x,lenx,wsave(n+1),lnsv,work,lnwk,local_error_flag)

        end associate

        if (local_error_flag /= 0) then
            ier = 20
            call fft_error_handler('cosqf1',-5)
            return
        end if

        do i=3,n,2
            xim1 = HALF * (x(1,i-1)+x(1,i))
            x(1,i) = HALF * (x(1,i-1)-x(1,i))
            x(1,i-1) = xim1
        end do

    end subroutine cosqf1

    subroutine cosqmb(lot, jump, n, inc, x, lenx, wsave, lensav, work, lenwrk, ier)
        !
        ! cosqmb: real backward cosine quarter wave, multiple vectors.
        !
        !  Purpose:
        !
        !  cosqmb computes the one-dimensional fourier transform of multiple
        !  sequences, each of which is a cosine series with odd wave numbers.
        !  this transform is referred to as the backward transform or fourier
        !  synthesis, transforming the sequences from spectral to physical space.
        !
        !  this transform is normalized since a call to cosqmb followed
        !  by a call to cosqmf (or vice-versa) reproduces the original
        !  array within roundoff error.
        !
        !  integer lot, the number of sequences to be transformed
        !  within array r.
        !
        !  integer jump, the increment between the locations,
        !  in array r, of the first elements of two consecutive sequences to be
        !  transformed.
        !
        !  integer n, the length of each sequence to be
        !  transformed.  the transform is most efficient when n is a product of
        !  small primes.
        !
        !  integer inc, the increment between the locations,
        !  in array r, of two consecutive elements within the same sequence.
        !
        !  input/real r(lenr), array containing lot sequences,
        !  each having length n.  r can have any number of dimensions, but the total
        !  number of locations must be at least lenr.  on input, r contains the data
        !  to be transformed, and on output, the transformed data.
        !
        !  integer lenr, the dimension of the r array.
        !  lenr must be at least (lot-1)*jump + inc*(n-1)+ 1.
        !
        !  input, real (wp) wsave(lensav).  wsave's contents must be
        !  initialized with a call to cosqmi before the first call to routine cosqmf
        !  or cosqmb for a given transform length n.  wsave's contents may be re-used
        !  for subsequent calls to cosqmf and cosqmb with the same n.
        !
        !  integer lensav, the dimension of the wsave array.
        !  lensav must be at least 2*n + int(log(real(n))) + 4.
        !
        !  workspace, real (wp) work(lenwrk).
        !
        !  integer lenwrk, the dimension of the work array.
        !  lenwrk must be at least lot*n.
        !
        !  integer ier, error_flag.
        !  0, successful exit;
        !  1, input parameter lenr not big enough;
        !  2, input parameter lensav not big enough;
        !  3, input parameter lenwrk not big enough;
        !  4, input parameters inc,jump,n,lot are not consistent;
        !  20, input error returned by lower level routine.
        !
        integer (ip) inc
        integer (ip) lensav
        integer (ip) lenwrk
        integer (ip) ier
        integer (ip) local_error_flag
        integer (ip) jump
        integer (ip) lenx
        integer (ip) lj
        integer (ip) lot
        integer (ip) m
        integer (ip) n
        real (wp) work(lenwrk)
        real (wp) wsave(lensav)
        real (wp) x(inc,*)
        real (wp) x1

        !
        !==> Check validity of calling arguments
        !
        if (lenx < (lot-1)*jump + inc*(n-1) + 1) then
            ier = 1
            call fft_error_handler('cosqmb', 6)
            return
        else if (lensav < get_1d_saved_workspace_length(n)) then
            ier = 2
            call fft_error_handler('cosqmb', 8)
            return
        else if (lenwrk < lot*n) then
            ier = 3
            call fft_error_handler('cosqmb', 10)
            return
        else if (.not. fft_consistent(inc,jump,n,lot)) then
            ier = 4
            call fft_error_handler('cosqmb', -1)
            return
        else
            ier = 0
        end if

        !
        !==> Perform transform
        !
        lj = (lot-1)*jump+1
        if (n < 2) then
            do m=1,lj,jump
                x(m,1) = x(m,1)
            end do
        else if (n == 2) then
            do m=1,lj,jump
                x1 = x(m,1)+x(m,2)
                x(m,2) = (x(m,1)-x(m,2))/sqrt(TWO)
                x(m,1) = x1
            end do
        else
            call mcsqb1(lot,jump,n,inc,x,wsave,work,local_error_flag)

            ! Check error_flag
            if (local_error_flag /= 0) then
                ier = 20
                call fft_error_handler('cosqmb',-5)
            end if
        end if

    end subroutine cosqmb

    subroutine cosqmf(lot, jump, n, inc, x, lenx, wsave, lensav, work, lenwrk, ier)
        !
        ! cosqmf: real forward cosine quarter wave, multiple vectors.
        !
        !  purpose:
        !
        !  cosqmf computes the one-dimensional fourier transform of multiple
        !  sequences within a real array, where each of the sequences is a
        !  cosine series with odd wave numbers.  this transform is referred to
        !  as the forward transform or fourier synthesis, transforming the
        !  sequences from spectral to physical space.
        !
        !  this transform is normalized since a call to cosqmf followed
        !  by a call to cosqmb (or vice-versa) reproduces the original
        !  array within roundoff error.
        !
        !  parameters:
        !
        !  integer lot, the number of sequences to be transformed
        !  within array r.
        !
        !  integer jump, the increment between the locations, in
        !  array r, of the first elements of two consecutive sequences to be
        !  transformed.
        !
        !  integer n, the length of each sequence to be
        !  transformed.  the transform is most efficient when n is a product of
        !  small primes.
        !
        !  integer inc, the increment between the locations,
        !  in array r, of two consecutive elements within the same sequence.
        !
        !  input/real r(lenr), array containing lot sequences,
        !  each having length n.  r can have any number of dimensions, but the total
        !  number of locations must be at least lenr.  on input, r contains the data
        !  to be transformed, and on output, the transformed data.
        !
        !  integer lenr, the dimension of the r array.
        !  lenr must be at least (lot-1)*jump + inc*(n-1)+ 1.
        !
        !  input, real (wp) wsave(lensav).  wsave's contents must be
        !  initialized with a call to cosqmi before the first call to routine cosqmf
        !  or cosqmb for a given transform length n.  wsave's contents may be re-used
        !  for subsequent calls to cosqmf and cosqmb with the same n.
        !
        !  integer lensav, the dimension of the wsave array.
        !  lensav must be at least 2*n + int(log(real(n))) + 4.
        !
        !  workspace, real (wp) work(lenwrk).
        !
        !  integer lenwrk, the dimension of the work array.
        !  lenwrk must be at least lot*n.
        !
        !  integer ier, error_flag.
        !  0, successful exit;
        !  1, input parameter lenr not big enough;
        !  2, input parameter lensav not big enough;
        !  3, input parameter lenwrk not big enough;
        !  4, input parameters inc,jump,n,lot are not consistent;
        !  20, input error returned by lower level routine.
        !
        integer (ip) inc
        integer (ip) lensav
        integer (ip) lenwrk
        integer (ip) ier
        integer (ip) local_error_flag
        integer (ip) jump
        integer (ip) lenx
        integer (ip) lj
        integer (ip) lot
        integer (ip) m
        integer (ip) n
        real (wp) tsqx
        real (wp) work(lenwrk)
        real (wp) wsave(lensav)
        real (wp) x(inc,*)

        !
        !==> Validity of input arguments
        !
        if (lenx < (lot-1)*jump + inc*(n-1) + 1) then
            ier = 1
            call fft_error_handler('cosqmf', 6)
            return
        else if (lensav < get_1d_saved_workspace_length(n)) then
            ier = 2
            call fft_error_handler('cosqmf', 8)
            return
        else if (lenwrk < lot*n) then
            ier = 3
            call fft_error_handler('cosqmf', 10)
            return
        else if (.not. fft_consistent(inc,jump,n,lot)) then
            ier = 4
            call fft_error_handler('cosqmf', -1)
            return
        else
            ier = 0
        end if

        !
        !==> Perform transform
        !
        lj = (lot-1)*jump+1

        if (n < 2) then
            return
        else if (n == 2) then
            do m=1,lj,jump
                tsqx = x(m,2)/sqrt(TWO)
                x(m,2) = HALF * x(m,1)-tsqx
                x(m,1) = HALF * x(m,1)+tsqx
            end do
        else
            call mcsqf1(lot,jump,n,inc,x,wsave,work,local_error_flag)

            ! Check error_flag
            if (local_error_flag /= 0) then
                ier = 20
                call fft_error_handler('cosqmf',-5)
            end if
        end if

    end subroutine cosqmf

    subroutine cosqmi(n, wsave, lensav, ier)
        !
        ! cosqmi: initialization for cosqmb and cosqmf.
        !
        !  purpose:
        !
        !  cosqmi initializes array wsave for use in its companion routines
        !  cosqmf and cosqmb.  the prime factorization of n together with a
        !  tabulation of the trigonometric functions are computed and stored
        !  in array wsave.  separate wsave arrays are required for different
        !  values of n.
        !
        !  parameters:
        !
        !  integer n, the length of each sequence to be
        !  transformed.  the transform is most efficient when n is a product of
        !  small primes.
        !
        !  integer lensav, the dimension of the wsave array.
        !  lensav must be at least 2*n + int(log(real(n))) + 4.
        !
        !  real wsave(lensav), containing the prime factors of
        !  n and also containing certain trigonometric values which will be used
        !  in routines cosqmb or cosqmf.
        !
        !  integer ier, error_flag.
        !  0, successful exit;
        !  2, input parameter lensav not big enough;
        !  20, input error returned by lower level routine.
        !
        integer (ip) lensav
        real (wp) dt
        real (wp) fk
        integer (ip) ier
        integer (ip) local_error_flag
        integer (ip) k
        integer (ip) lnsv
        integer (ip) n
        real (wp), parameter ::  HALF_PI = acos(-ONE)/2
        real (wp) wsave(lensav)

        if (lensav < get_1d_saved_workspace_length(n)) then
            ier = 2
            call fft_error_handler('cosqmi', 3)
            return
        else
            ier = 0
        end if

        dt = HALF_PI/n
        fk = ZERO

        do k=1,n
            fk = fk + ONE
            wsave(k) = cos(fk*dt)
        end do

        ! Set workspace index pointer
        lnsv = get_1d_saved_workspace_length(n) - n

        call rfftmi(n, wsave(n+1), lnsv, local_error_flag)

        ! Check error_flag
        if (local_error_flag /= 0) then
            ier = 20
            call fft_error_handler('cosqmi',-5)
        end if

    end subroutine cosqmi

    subroutine mcsqb1(lot,jump,n,inc,x,wsave,work,ier)
        integer (ip) inc
        integer (ip) lot
        integer (ip) i
        integer (ip) ier
        integer (ip) local_error_flag
        integer (ip) jump
        integer (ip) k
        integer (ip) kc
        integer (ip) lenx
        integer (ip) lj
        integer (ip) lnsv
        integer (ip) lnwk
        integer (ip) m
        integer (ip) m1
        integer (ip) modn
        integer (ip) n
        integer (ip) np2
        integer (ip) ns2
        real (wp) work(lot,*)
        real (wp) wsave(*)
        real (wp) x(inc,*)
        real (wp) xim1

        ier = 0
        lj = (lot-1)*jump+1
        ns2 = (n+1)/2
        np2 = n+2

        do i=3,n,2
            do m=1,lj,jump
                xim1 = x(m,i-1)+x(m,i)
                x(m,i) = HALF * (x(m,i-1)-x(m,i))
                x(m,i-1) = HALF * xim1
            end do
        end do

        do m=1,lj,jump
            x(m,1) = HALF * x(m,1)
        end do

        modn = mod(n,2)
        if (modn == 0) then
            do m=1,lj,jump
                x(m,n) = HALF * x(m,n)
            end do
        end if

        lenx = (lot-1)*jump + inc*(n-1)  + 1
        lnsv = n + int(log(real(n, kind=wp) )/log(TWO)) + 4
        lnwk = lot*n

        call rfftmb(lot,jump,n,inc,x,lenx,wsave(n+1),lnsv,work,lnwk,local_error_flag)

        if (local_error_flag /= 0) then
            ier = 20
            call fft_error_handler('mcsqb1',-5)
            return
        end if

        do k=2,ns2
            kc = np2-k
            m1 = 0
            do m=1,lj,jump
                m1 = m1 + 1
                work(m1,k) = wsave(k-1)*x(m,kc)+wsave(kc-1)*x(m,k)
                work(m1,kc) = wsave(k-1)*x(m,k)-wsave(kc-1)*x(m,kc)
            end do
        end do

        if (modn == 0) then
            do m=1,lj,jump
                x(m,ns2+1) = wsave(ns2)*(x(m,ns2+1)+x(m,ns2+1))
            end do
        end if

        do k=2,ns2
            kc = np2-k
            m1 = 0
            do m=1,lj,jump
                m1 = m1 + 1
                x(m,k) = work(m1,k)+work(m1,kc)
                x(m,kc) = work(m1,k)-work(m1,kc)
            end do
        end do

        do m=1,lj,jump
            x(m,1) = x(m,1)+x(m,1)
        end do

    end subroutine mcsqb1

    subroutine mcsqf1(lot,jump,n,inc,x,wsave,work,ier)

        integer (ip) inc
        integer (ip) lot
        integer (ip) i
        integer (ip) ier
        integer (ip) local_error_flag
        integer (ip) jump
        integer (ip) k
        integer (ip) kc
        integer (ip) lenx
        integer (ip) lnsv
        integer (ip) lnwk
        integer (ip) lj
        integer (ip) m
        integer (ip) m1
        integer (ip) modn
        integer (ip) n
        integer (ip) np2
        integer (ip) ns2
        real (wp) work(lot,*)
        real (wp) wsave(*)
        real (wp) x(inc,*)
        real (wp) xim1

        ier = 0

        lj = (lot-1)*jump+1
        ns2 = (n+1)/2
        np2 = n+2

        do k=2,ns2
            kc = np2-k
            m1 = 0
            do m=1,lj,jump
                m1 = m1 + 1
                work(m1,k) = x(m,k)+x(m,kc)
                work(m1,kc) = x(m,k)-x(m,kc)
            end do
        end do

        modn = mod(n,2)

        if (modn == 0) then
            m1 = 0
            do m=1,lj,jump
                m1 = m1 + 1
                work(m1,ns2+1) = x(m,ns2+1)+x(m,ns2+1)
            end do
        end if

        do k=2,ns2
            kc = np2-k
            m1 = 0
            do m=1,lj,jump
                m1 = m1 + 1
                x(m,k)  = wsave(k-1)*work(m1,kc)+wsave(kc-1)*work(m1,k)
                x(m,kc) = wsave(k-1)*work(m1,k) -wsave(kc-1)*work(m1,kc)
            end do
        end do

        if (modn == 0) then
            m1 = 0
            do m=1,lj,jump
                m1 = m1 + 1
                x(m,ns2+1) = wsave(ns2)*work(m1,ns2+1)
            end do
        end if

        lenx = (lot-1)*jump + inc*(n-1)  + 1
        lnsv = n + int(log(real(n, kind=wp) )/log(TWO)) + 4
        lnwk = lot*n

        call rfftmf(lot,jump,n,inc,x,lenx,wsave(n+1),lnsv,work,lnwk,local_error_flag)

        if (local_error_flag /= 0) then
            ier = 20
            call fft_error_handler('mcsqf1',-5)
            return
        end if

        do i=3,n,2
            do m=1,lj,jump
                xim1 = HALF * (x(m,i-1)+x(m,i))
                x(m,i) = HALF * (x(m,i-1)-x(m,i))
                x(m,i-1) = xim1
            end do
        end do

    end subroutine mcsqf1

end module quarter_cosine_transform_routines


! ********************************************************
! Program: ExactTS
! Author: Wenbin, FAN (fanwenbin@shu.edu.cn, langzihuigu@qq.com)
!
! Release: Dec. 7, 2019
! Update: Jul. 19, 2020
!
! Hartree and Angstrom are used here!
! ********************************************************

program main
!use lapack95
!use fakeG
implicit none
integer Natoms
real(8), allocatable :: coord(:,:), coord1D(:) ! coordinates 3D and 1D
real(8), allocatable :: grad(:,:), grad1D(:)
real(8), allocatable :: hessian(:,:), hessianT(:,:) ! Hessian
integer, allocatable :: ipiv(:) ! for matrix inversion
real(8), allocatable :: coord1DNew(:)
character(4), allocatable :: eleName(:) ! element names
real(8), allocatable :: eleMass(:)
real(8) rand
real(8) energy
!real(8) error ! difference between current and the last Cartesian geometries.
integer step, convIF
integer i, j, k, pinvInfo
real(8) rr, mc(2), R, x1, y1, x2, y2, gam, rmgh1, rmgh2 ! output for bonds

! PES initialization
call TSinit

! read coordinates from TS.xyz ! angstrom
open(999, file='TS.xyz', status='old', action='read') ! xyz -> 999
read(999, *) Natoms
allocate(coord(3, Natoms), eleName(Natoms))
read(999, *)
do i = 1, Natoms
read(999, *) eleName(i), coord(:, i)
!# write(*,'(3F11.6)') coord(:,i)
end do
close(999)

! output file ! out -> 688, ene -> 363
open(688, file='opt.xyz', status='unknown', action='write')
open(363, file='energy.txt', status='unknown', action='write')
open(373, file='freq_out.txt', action='write') ! fre -> 373
open(566, file='bond_out.txt', action='write') ! bon -> 566

! allocate
allocate(eleMass(Natoms))
allocate(coord1D(3 * Natoms))
coord1D = reshape(coord, (/Natoms * 3/)) ! reshape coordinates to 1D array
allocate(grad(3, Natoms), grad1D(3 * Natoms))
allocate(hessian(Natoms * 3, Natoms * 3), &
         hessianT(Natoms * 3, Natoms * 3), &
         ipiv(Natoms * 3))
allocate(coord1DNew(3 * Natoms))

! Get mass
call eleName2Mass(Natoms, eleName, eleMass)

!error = 1.d0
step = 1
convIF = 0
do while (convIF .lt. 3.5d0)
!do while (step .lt. 3)
write(*,'(A12,I5)') 'Steps: ', step
coord = reshape(coord1D, (/3, Natoms/))
call TSenergy(Natoms, coord, energy)
write(363,*) energy * 627.503
write(*,'(A12,F11.6,A)') 'Energy: ', energy * 627.503, ' kcal/mol'

! 1st derivation
call gradCalc(Natoms, coord, grad)
grad1D = reshape(grad, (/Natoms * 3/))

! 2nd derivation / Hessian matrix
call hessianCalc(Natoms, coord1D, hessian)
call vibAna(Natoms, eleMass, hessian, 373)
! Inversion of Hessian matrix
!hessianT = hessian
!call getrf(hessianT, ipiv)
!call getri(hessianT, ipiv)
!#if (maxval(hessianT - hessian) .lt. 1E-9) then
!#write(*,*) 'Inversion of Hessian matrix failed! '
!#!!stop
!#end if
call pinv(Natoms * 3, hessian, hessianT, pinvInfo)
if (pinvInfo .ne. 0) stop
!# write(*,*) matmul(hessianT, hessian) ! Check the identity

! Evolution: x' = x - dx * Hessian^T
coord1DNew = coord1D - matmul(hessianT, grad1D) *0.01d0!* (rand() * 0.2d0 + 0.1d0)
call convCrit(Natoms, coord1D, coord1DNew, grad1D, convIF)
!# write(*,*) shape(coord1D)
!# write(*,*) shape(hessianT)
! Export results
call cleanGeom(Natoms, eleMass, coord1DNew)
call writeXYZ(Natoms, eleName, reshape(coord1DNew, (/3, Natoms/)), 688)

! write R, r, gamma
rr = dsqrt((coord1D(4) - coord1D(1)) ** 2d0 + (coord1D(5) - coord1D(2)) ** 2d0)
mc(1) = (coord1D(4) * 24.305 + coord1D(1) * 1.008) / (24.305 + 1.008)
mc(2) = (coord1D(5) * 24.305 + coord1D(2) * 1.008) / (24.305 + 1.008)
R = dsqrt((coord1D(7) - mc(1)) ** 2d0 + (coord1D(8) - mc(2)) ** 2d0)
x1 = coord1D(1) - coord1D(4)
y1 = coord1D(2) - coord1D(5)
x2 = coord1D(7) - mc(1)
y2 = coord1D(8) - mc(2)
gam = dacos((x1 * x2 + y1 * y2)/dsqrt((x1*x1 + y1*y1) * (x2*x2 + y2*y2))) * 180d0 / dacos(-1d0)
rmgh1 = dsqrt((coord1D(4)-coord1D(1))**2d0 + (coord1D(5)-coord1D(2))**2d0 + (coord1D(6)-coord1D(3))**2d0)
rmgh2 = dsqrt((coord1D(4)-coord1D(7))**2d0 + (coord1D(5)-coord1D(8))**2d0 + (coord1D(6)-coord1D(9))**2d0)
write(566, '(5F15.6)') R/0.529177210903d0, rr/0.529177210903d0, gam, rmgh1/0.529177210903d0, rmgh2/0.529177210903d0

! loop
coord1D = coord1DNew
step = step + 1
write(*,*) ''
!error = 1d-7
end do ! while loop

call fakeG(Natoms, eleName, coord)

end program

subroutine convCrit(Natoms, coord1D, coord1DNew, grad1D, convIF)
implicit none
integer Natoms, i, j, k
real(8) coord1D(Natoms * 3), coord1DNew(Natoms * 3)
real(8) grad1D(Natoms * 3)
real(8) maxF, rmsF, maxR, rmsR
integer :: convIF

convIF = 0
maxF = 0d0
rmsF = 0d0
maxR = 0d0
rmsR = 0d0

! max force
maxF = maxval(abs(grad1D))
if (maxF .lt. 2d-6) convIF = convIF + 1

! RMS force
do i = 1, Natoms * 3
rmsF = rmsF + grad1D(i) ** 2d0
end do
rmsF = dsqrt(rmsF) / (Natoms * 3d0)
if (rmsF .lt. 1d-6) convIF = convIF + 1

! max displacement
maxR = maxval(abs(coord1D - coord1DNew))
if (maxR .lt. 6d-6) convIF = convIF + 1

! RMS displacement
do i = 1, Natoms * 3
rmsR = rmsR + (coord1D(i) - coord1DNew(i)) ** 2d0
end do
rmsR = dsqrt(rmsR) / (Natoms * 3d0)
if (rmsR .lt. 4d-6) convIF = convIF + 1

! ouput
write(*,'(2(A12,E11.2,A4))') 'Max force: ', maxF, '', 'RMS force: ', rmsF
write(*,'(2(A12,E11.2,A4))') 'Max disp.: ', maxR, '', 'RMS disp.: ', rmsR

end subroutine

! Vibrational analysis
subroutine vibAna(Natoms, eleMass, hessian, fileID)
use lapack95
implicit none
integer Natoms
real(8) hessian(3 * Natoms, 3 * Natoms), hessianMW(3 * Natoms, 3 * Natoms) ! mass-weighted
real(8) eleMass(Natoms), eigenvalue(3 * Natoms), freq(3 * Natoms)!, wavenumber(3 * Natoms)
real(8) tmp ! temp value for sorting freq
integer i, j, m, n
integer fileID

call convMWhessian(Natoms, eleMass, hessian, hessianMW)

! Eigenvalue
call syev(hessianMW, eigenvalue)
!# write(*,'(6F11.6)') eigenvalue

call calcFreq(Natoms, eigenvalue, freq)

write(fileID, '(300F11.2)') freq

! sort modes by absolute value
do i = 1, Natoms * 3
do j = i + 1, Natoms * 3
if (abs(freq(i))>abs(freq(j))) then
tmp=freq(i)
freq(i)=freq(j)
freq(j)=tmp
end if
end do
end do

! output freq
write(*,'(A)') 'Freq(cm-1): '
write(*,'(6F12.2)') freq(7:)

end subroutine

subroutine convMWhessian(Natoms, eleMass, hessian, hessianMW)
implicit none
integer :: i,j,k,l,m,n,Natoms
real(8) hessian(3 * Natoms, 3 * Natoms), hessianMW(3 * Natoms, 3 * Natoms)
real(8) eleMass(Natoms)

hessianMW = 0d0
! Convert to mass-weighted Hessian
do j = 0, Natoms - 1
do i = 0, Natoms - 1
! 9 elements for each atom ! 3 * 3 matrix, ie. xx, xy, xz, yx, yy, yz, zx, zy, zz
do n = 1, 3
do m = 1, 3
hessianMW(i*3+m, j*3+n) = hessian(i*3+m, j*3+n) / dsqrt(eleMass(i+1) * eleMass(j+1))
end do
end do
end do
end do

end subroutine

subroutine calcFreq(Natoms, eigenvalue, freq)
implicit none
integer :: Natoms, i, j, k
real(8) eigenvalue(3 * Natoms), freq(3 * Natoms)
real(8) amu2kg, a2m, au2J
real(8), parameter :: pi = 4.d0*atan(1.d0)

! To frequency ! Below 13 lines were referred Sobereva's `Hess2freq`.
amu2kg=1.66053878D-27
a2m=1D-10
au2J=4.35974434D-18
eigenvalue=eigenvalue*au2J/a2m**2/amu2kg !convert force constant from a.u. to SI
do i = 1, Natoms * 3
if (eigenvalue(i)<0) then
freq(i) = -dsqrt(abs(eigenvalue(i))) / (2 * pi)
else
freq(i) = dsqrt(eigenvalue(i)) / (2 * pi)
end if
end do

freq = freq/2.99792458D10

end subroutine

! Shift centroid to origin
subroutine cleanGeom(Natoms, eleMass, coord)
integer Natoms
real(8) coord(3 * Natoms), coord3(3, Natoms), cm(3) ! center of mass
real(8) eleMass(Natoms), sumMass(3), totalMass

coord3 = reshape(coord, (/3, Natoms/))
!# coord3(1,:) = coord3(1,:) + 1d0
!# write(*,*) 'Coordintaes'
!# write(*,'(3F11.6)') coord3

! Find center of mass
sumMass = 0d0
do j = 1, Natoms
do i = 1, 3
sumMass(i) = sumMass(i) + coord3(i, j) * eleMass(j)
!# write(*,'(3F11.6)') coord3(i, j) * eleMass(j)
end do
end do
!# write(*,*) 'Sum of mass * r'
!# write(*,'(3F11.6)') sumMass

! total mass
totalMass = sum(eleMass)
!# write(*,*) 'Total mass: '
!# write(*,'(3F11.6)') totalMass

! center of mass
cm = sumMass / totalMass
!# write(*,*) 'Center of mass: '
!# write(*,'(3F11.6)') cm

! shift CM to origin
do j = 1, Natoms
coord3(:, j) = coord3(:, j) - cm
end do
!# write(*,*) 'Coordintaes shifted '
!# write(*,'(3F11.6)') coord3

coord = reshape(coord3, (/Natoms/))

end subroutine

! Get each mass from element name
subroutine eleName2Mass(Natoms, eleName, eleMass)
implicit none
integer Natoms
character(4) :: allName(22) = &
(/'H', 'He',   'Li',   'Be',   'B', &
 'C',  'N',    'O',    'F',    'Ne', &
 'Na', 'Mg',   'Al',   'Si',   'P', &
 'S',  'Cl',   'Ar',   'K',    'Ca', &
 'D', 'C13'/)
real(8) :: allMass(22) = &
(/1.008d0,      4.002602d0, 6.94d0,       9.0121831d0,    10.81d0, &
 12.000d0,      14.007d0,   15.999d0,     18.998403163d0, 20.1797d0, &
 22.98976928d0, 24.305d0,   26.9815385d0, 28.085d0,       30.973761998d0, &
 32.06d0,       35.45d0,    39.948d0,     39.0983d0,      40.078d0, &
 2.0141d0,      13.00335483521d0/)
character(4) eleName(Natoms)
real(8), intent(out) :: eleMass(Natoms)
integer i, j

do i = 1, Natoms
do j = 1, 22
!# write(*,*) i, j, eleName(i), allName(j)
if (trim(eleName(i)) == trim(allName(j))) then
eleMass(i) = allMass(j)
exit
!else
!write(*, *) '[ERROR] Element not supported: ', eleName(i)
end if
end do
end do
!# write(*,*) 'Element mass: '
!# write(*,*) eleMass
end subroutine

! Export coordinates to .xyz file
subroutine writeXYZ(Natoms, eleName, coord, fileID)
implicit none
integer Natoms, fileID, i
character(4) eleName(Natoms)
character(64) info
real(8) coord(3, Natoms)

write(fileID,'(I4)') Natoms
write(fileID,'(A)') 'TEST'
do i = 1, Natoms
write(fileID,'(A,3F18.10)') eleName(i), coord(:,i)
end do
end subroutine

! Calculate the Hessian matrix
subroutine hessianCalc(Natoms, coord1D, hessian)
implicit none
integer Natoms
real(8) coord1D(3 * Natoms)
real(8) hessian(Natoms * 3, Natoms * 3)
real(8) energy2nd

integer i, j

do j = 1, Natoms * 3
do i = 1, Natoms * 3
call der2nd(Natoms, coord1D, i, j, energy2nd)
hessian(i, j) = energy2nd
!# write(*,*) hessian(i,j)
end do
end do

!# ! check Hessian: $H_{i,j} = H_{j,i}$ ! check passed!
!# do j = 1, Natoms * 3
!# do i = 1, Natoms * 3
!# write(*,'(3F11.6)') hessian(i, j), hessian(j, i), hessian(i, j) - hessian(j, i)
!# end do
!# end do

end subroutine

! Calculate the 2nd derivation
subroutine der2nd(Natoms, coord1D, i, j, energy)
implicit none
integer Natoms, i, j
real(8) coord1D(3 * Natoms), energy
real(8) :: delta = 1d-5, delta4squared = 4d-10
real(8) e1, e2, e3, e4 ! See ref: William, Fortran Reciept, Page 182
real(8) bkup1, bkup2 ! backup energy

! e1: x + delta, y + delta
bkup1 = coord1D(i)
bkup2 = coord1D(j)
coord1D(i) = coord1D(i) + delta
coord1D(j) = coord1D(j) + delta
call TSenergy(Natoms, reshape(coord1D, (/3, Natoms/)), e1)
coord1D(i) = bkup1
coord1D(j) = bkup2

! e2: x + delta, y - delta
bkup1 = coord1D(i)
bkup2 = coord1D(j)
coord1D(i) = coord1D(i) + delta
coord1D(j) = coord1D(j) - delta
call TSenergy(Natoms, reshape(coord1D, (/3, Natoms/)), e2)
coord1D(i) = bkup1
coord1D(j) = bkup2

! e3: x - delta, y + delta
bkup1 = coord1D(i)
bkup2 = coord1D(j)
coord1D(i) = coord1D(i) - delta
coord1D(j) = coord1D(j) + delta
call TSenergy(Natoms, reshape(coord1D, (/3, Natoms/)), e3)
coord1D(i) = bkup1
coord1D(j) = bkup2

! e4: x - delta, y - delta
bkup1 = coord1D(i)
bkup2 = coord1D(j)
coord1D(i) = coord1D(i) - delta
coord1D(j) = coord1D(j) - delta
call TSenergy(Natoms, reshape(coord1D, (/3, Natoms/)), e4)
coord1D(i) = bkup1
coord1D(j) = bkup2

energy = (e1 - e2 - e3 + e4) / delta4squared

end subroutine

! Calculate the gradient
subroutine gradCalc(Natoms, coord, grad)
implicit none
integer Natoms
real(8) coord(3, Natoms)
real(8) grad(3, Natoms)
real(8) eneFor, eneBak, coordBackup ! forward, backward, backup
real(8) :: delta = 1d-5 ! displacement
integer i, j

do j = 1, Natoms
do i = 1, 3 ! x,y,z
! finite displacement
coordBackup = coord(i,j)
coord(i,j) = coord(i,j) + delta
call TSenergy(Natoms, coord, eneFor)
coord(i,j) = coord(i,j) - 2d0 * delta
call TSenergy(Natoms, coord, eneBak)
coord(i,j) = coordBackup
! calculate gradient
grad(i,j) = (eneFor - eneBak) / (2d0 * delta)
end do
!# write(*,'(3F11.6)') grad(:,j)
end do
end subroutine

subroutine pinv(N, A, pinvA, info)
use lapack95, only: gesvd
implicit none
integer N
real(8), intent(in) :: A(N,N)
real(8), intent(out) :: pinvA(N,N)
real(8) U(N,N), S(N), VT(N,N)!, WW(3)
real(8) S1(N,N), V(N,N), VS1(N,N)
real(8) work(N*N), lwork
integer, intent(out) :: info
integer i

call dgesvd('A', 'A', N, N, A, N, S, U, N, VT, N, work, N*N, info)

!#write(*,*) 'A'
!#write(*,'(6F11.4)') A

! pseudo inversion for S
S1 = 0d0
do i = 1, N
if (S(i) .gt. 1d-10) S1(i,i) = 1/S(i)
end do

! A-1 = V * S-1 * U^T
VS1 = matmul(transpose(VT), S1)
pinvA = matmul(VS1, transpose(U))

end subroutine


      SUBROUTINE FD4n(X, Y)
      USE global
      IMPLICIT NONE
      REAL*8 :: Y(1,NXP4), X(1,NXP4)
      INTEGER ::  IC
!----------------------------------------------------------------------
!     Derivative by 4 th order central central finite difference scheam
!     X = Matric of which derivative is wanted to find
!     Y = Derivative of matirx
!----------------------------------------------------------------------
      DO 111 IC = 3,NXP2
         Y(1,IC) = (8*X(1,IC+1) - 8*X(1,IC-1) - X(1,IC+2) &
     &   + X(1,IC-2))/(12*DX)
  111 CONTINUE

      RETURN
      END


! Use a Neural network to obtain Pg from (T,Pe)
!
Subroutine ann_pgfrompe(T, Pe, Metalicity, Pg)
  Use ann_pgfrompe_data
  Use Debug_module
  Real :: T, T2, Pe, Pg, Metalicity, Met2
  Real :: LogPe, LogPg

  LogPe=Log10(Pe)
  If (LogPe .gt. 5) then
     Debug_warningflags(flag_computepg)=1
     Call Debug_Log('Log10 (Pe) .gt. 5. Clipping it',2)
     LogPe=5
  End if
  If (LogPe .lt. -3) then
     Debug_warningflags(flag_computepg)=1
     Call Debug_Log('Log10 (Pe) .lt. -3. Clipping it',2)
     LogPe=-3
  End if
  T2=T
  If (T .gt. 1e4) then
     Debug_warningflags(flag_computepg)=1
     Call Debug_Log('T .gt. 1E4. Clipping it',2)
     T2=1e4
  End if
  If (T .lt. 1500) then
     Debug_warningflags(flag_computepg)=1
     Call Debug_Log('T .lt. 1500. Clipping it',2)
     T2=1500
  End if
  Met2=Metalicity
  If (Met2 .gt. .5) then
     Debug_warningflags(flag_computepg)=1
     Call Debug_Log('Metalicity .gt. 0.5. Clipping it',2)
     Met2=.5
  End if
  If (Met2 .lt. -1.5) then
     Debug_warningflags(flag_computepg)=1
     Call Debug_Log('Metalicity .lt. -1.5. Clipping it',2)
     Met2=-1.5
  End if
  inputs(1)=(T2-xmean(1))/xnorm(1)
  inputs(2)=(LogPe-xmean(2))/xnorm(2)
  inputs(3)=(Met2-xmean(3))/xnorm(3)

  Call ANN_Forward(W, Beta, Nonlin, inputs, outputs, nlayers, &
       nmaxperlayer, nperlayer, ninputs, noutputs, y)

  LogPg=outputs(1)*ynorm(1)+ymean(1)
  Pg=10.**(LogPg)

End Subroutine ann_pgfrompe