c     prog6
c     c.l.lawson and r.j.hanson, jet propulsion laboratory, 1973 jun 15
c     to appear in 'solving least squares problems', prentice-hall, 1974
c	   demonstrate the use of the subroutine   ldp	for least
c     distance programming by solving the constrained line data fitting
c     problem of chapter 23.
c
      dimension e(4,2), f(4), g(3,2), h(3), g2(3,2), h2(3), x(2), z(2),
     1w(4)
      dimension wldp(21), s(6), t(4)
      integer index(3)
c
      write (6,110)
      mde=4
      mdgh=3
c
      me=4
      mg=3
      n=2
c		 define the least squares and constraint matrices.
      t(1)=0.25
      t(2)=0.5
      t(3)=0.5
      t(4)=0.8
c
      w(1)=0.5
      w(2)=0.6
      w(3)=0.7
      w(4)=1.2
c
	  do 10 i=1,me
	  e(i,1)=t(i)
	  e(i,2)=1.
   10	  f(i)=w(i)
c
      g(1,1)=1.
      g(1,2)=0.
      g(2,1)=0.
      g(2,2)=1.
      g(3,1)=-1.
      g(3,2)=-1.
c
      h(1)=0.
      h(2)=0.
      h(3)=-1.
c
c     compute the singular value decomposition of the matrix, e.
c
      call svdrs (e,mde,me,n,f,1,1,s)
c
      write (6,120) ((e(i,j),j=1,n),i=1,n)
      write (6,130) f,(s(j),j=1,n)
c
c     generally rank determination and levenberg-marquardt
c     stabilization could be inserted here.
c
c	 define the constraint matrix for the z coordinate system.
	  do 30 i=1,mg
	      do 30 j=1,n
	      sm=0.
		  do 20 l=1,n
   20		  sm=sm+g(i,l)*e(l,j)
   30	      g2(i,j)=sm/s(j)
c	  define constraint rt side for the z coordinate system.
	  do 50 i=1,mg
	  sm=0.
	      do 40 j=1,n
   40	      sm=sm+g2(i,j)*f(j)
   50	  h2(i)=h(i)-sm
c
      write (6,140) ((g2(i,j),j=1,n),i=1,mg)
      write (6,150) h2
c
c			 solve the constrained problem in z-coordinates.
c
      call ldp (g2,mdgh,mg,n,h2,z,znorm,wldp,index,mode)
c
      write (6,200) mode,znorm
      write (6,160) z
c
c		     transform back from z-coordinates to x-coordinates.
	  do 60 j=1,n
   60	  z(j)=(z(j)+f(j))/s(j)
	  do 80 i=1,n
	  sm=0.
	      do 70 j=1,n
   70	      sm=sm+e(i,j)*z(j)
   80	  x(i)=sm
      res=znorm**2
      np1=n+1
	  do 90 i=np1,me
   90	  res=res+f(i)**2
      res=sqrt(res)
c				     compute the residuals.
	  do 100 i=1,me
  100	  f(i)=w(i)-x(1)*t(i)-x(2)
      write (6,170) (x(j),j=1,n)
      write (6,180) (i,f(i),i=1,me)
      write (6,190) res
      stop
c
  110 format (46h0prog6..  example of constrained curve fitting,26h usin
     1g the subroutine ldp.,/43h0related intermediate quantities are giv
     2en.)
  120 format (10h0v =	   ,2f10.5/(10x,2f10.5))
  130 format (10h0f tilda =,4f10.5/10h0s =	,2f10.5)
  140 format (10h0g tilda =,2f10.5/(10x,2f10.5))
  150 format (10h0h tilda =,3f10.5)
  160 format (10h0z =	   ,2f10.5)
  170 format (52h0the coeficients of the fitted line f(t)=x(1)*t+x(2),12
     1h are x(1) = ,f10.5,14h	and x(2) = ,f10.5)
  180 format (30h0the consecutive residuals are/1x,4(i10,f10.5))
  190 format (23h0the residuals norm is ,f10.5)
  200 format (18h0mode (from ldp) =,i3,2x,7hznorm =,f10.5)
c
      end


      subroutine gaussj(a,n,np,b,m,mp)
c
c ****************************************************************
c *                                                              *
c *    Gauss-Jordan routine used by pseud* to find polynominal   *
c *  constants.  Taken from Numerical Recipes, page 28.          *
c *  Version dated May 7, 1991                                   *
c *  njtj                                                        *
c *                                                              *
c ****************************************************************
c
      implicit real*8 (a-h,o-z)
c
      parameter (nmax=50,zero=0.0d0,one=1.0d0)
c
      dimension a(np,np),b(np,mp),ipiv(nmax),indxr(nmax),indxc(nmax)
c
      do 11 j=1,n
        ipiv(j)=0
11    continue
      do 22 i=1,n
        big=zero
        do 13 j=1,n
          if(ipiv(j).ne.1)then
            do 12 k=1,n
              if (ipiv(k).eq.0) then
                if (abs(a(j,k)).ge.big)then
                  big=abs(a(j,k))
                  irow=j
                  icol=k
                endif
              else if (ipiv(k).gt.1) then
                write(6,100)
                stop 'gaussj one'
              endif
12          continue
          endif
13      continue
        ipiv(icol)=ipiv(icol)+1
        if (irow.ne.icol) then
          do 14 l=1,n
            dum=a(irow,l)
            a(irow,l)=a(icol,l)
            a(icol,l)=dum
14        continue
          do 15 l=1,m
            dum=b(irow,l)
            b(irow,l)=b(icol,l)
            b(icol,l)=dum
15        continue
        endif
        indxr(i)=irow
        indxc(i)=icol
        if (a(icol,icol).eq.zero) then
          write(6,100)
          stop 'gaussj two'
        endif
        pivinv=one/a(icol,icol)
        a(icol,icol)=one
        do 16 l=1,n
          a(icol,l)=a(icol,l)*pivinv
16      continue
        do 17 l=1,m
          b(icol,l)=b(icol,l)*pivinv
17      continue
        do 21 ll=1,n
          if(ll.ne.icol)then
            dum=a(ll,icol)
            a(ll,icol)=zero
            do 18 l=1,n
              a(ll,l)=a(ll,l)-a(icol,l)*dum
18          continue
            do 19 l=1,m
              b(ll,l)=b(ll,l)-b(icol,l)*dum
19          continue
          endif
21      continue
22    continue
      do 24 l=n,1,-1
        if(indxr(l).ne.indxc(l))then
          do 23 k=1,n
            dum=a(k,indxr(l))
            a(k,indxr(l))=a(k,indxc(l))
            a(k,indxc(l))=dum
23        continue
        endif
24    continue
      return
 100  format(//,'Singular matrix, stopped in gaussj')
      end


      SUBROUTINE H12 (MODE,LPIVOT,L1,M,U,IUE,UP,C,ICE,ICV,NCV)

C     C.L.LAWSON AND R.J.HANSON, JET PROPULSION LABORATORY, 1973 JUN 12
C     TO APPEAR IN 'SOLVING LEAST SQUARES PROBLEMS', PRENTICE-HALL, 1974

C     CONSTRUCTION AND/OR APPLICATION OF A SINGLE
C     HOUSEHOLDER TRANSFORMATION  Q = I + U*(U**T)/B

C     MODE    = 1 OR 2   TO SELECT ALGORITHM  H1  OR  H2 .
C     LPIVOT IS THE INDEX OF THE PIVOT ELEMENT.
C     L1,M   IF L1 <= M   THE TRANSFORMATION WILL BE CONSTRUCTED TO
C            ZERO ELEMENTS INDEXED FROM L1 THROUGH M.
C            IF L1 > M THE SUBROUTINE DOES AN IDENTITY TRANSFORMATION.
C     U(),IUE,UP
C            ON ENTRY TO H1 U() STORES THE PIVOT VECTOR.
C            IUE IS THE STORAGE INCREMENT BETWEEN ELEMENTS.
C            ON EXIT FROM H1 U() AND UP STORE QUANTITIES DEFINING
C            THE VECTOR U OF THE HOUSEHOLDER TRANSFORMATION.
C            ON ENTRY TO H2 U() AND UP
C            SHOULD STORE QUANTITIES PREVIOUSLY COMPUTED BY H1.
C            THESE WILL NOT BE MODIFIED BY H2.
C     C()    ON ENTRY TO H1 OR H2 C() STORES A MATRIX WHICH WILL BE
C            REGARDED AS A SET OF VECTORS TO WHICH THE HOUSEHOLDER
C            TRANSFORMATION IS TO BE APPLIED.
C            ON EXIT C() STORES THE SET OF TRANSFORMED VECTORS.
C     ICE    STORAGE INCREMENT BETWEEN ELEMENTS OF VECTORS IN C().
C     ICV    STORAGE INCREMENT BETWEEN VECTORS IN C().
C     NCV    NUMBER OF VECTORS IN C() TO BE TRANSFORMED.
C            IF NCV <= 0 NO OPERATIONS WILL BE DONE ON C().

      INTEGER          INCR, ICE, ICV, IUE, LPIVOT, L1, MODE, NCV
      INTEGER          I, I2, I3, I4, J, M
      DOUBLE PRECISION U,UP,C,CL,CLINV,B,SM,ONE,ZERO
      DIMENSION        U(IUE,*), C(*)
      DATA             ONE/1.0D+00/, ZERO/0.0D+00/

      IF (0.GE.LPIVOT.OR.LPIVOT.GE.L1.OR.L1.GT.M) GOTO 80
      CL=ABS(U(1,LPIVOT))
      IF (MODE.EQ.2)                              GOTO 30

C     ****** CONSTRUCT THE TRANSFORMATION ******

          DO 10 J=L1,M
             SM=ABS(U(1,J))
   10     CL=MAX(SM,CL)
      IF (CL.LE.ZERO)                             GOTO 80
      CLINV=ONE/CL
      SM=(U(1,LPIVOT)*CLINV)**2
          DO 20 J=L1,M
   20     SM=SM+(U(1,J)*CLINV)**2
      CL=CL*SQRT(SM)
      IF (U(1,LPIVOT).GT.ZERO) CL=-CL
      UP=U(1,LPIVOT)-CL
      U(1,LPIVOT)=CL
                                                  GOTO 40
C     ****** APPLY THE TRANSFORMATION  I+U*(U**T)/B  TO C ******

   30 IF (CL.LE.ZERO)                             GOTO 80
   40 IF (NCV.LE.0)                               GOTO 80
      B=UP*U(1,LPIVOT)
      IF (B.GE.ZERO)                              GOTO 80
      B=ONE/B
      I2=1-ICV+ICE*(LPIVOT-1)
      INCR=ICE*(L1-LPIVOT)
          DO 70 J=1,NCV
          I2=I2+ICV
          I3=I2+INCR
          I4=I3
          SM=C(I2)*UP
              DO 50 I=L1,M
              SM=SM+C(I3)*U(1,I)
   50         I3=I3+ICE
          IF (SM.EQ.ZERO)                         GOTO 70
          SM=SM*B
          C(I2)=C(I2)+SM*UP
              DO 60 I=L1,M
              C(I4)=C(I4)+SM*U(1,I)
   60         I4=I4+ICE
   70     CONTINUE
   80 END


module velocityverlet

  contains
  function positions(x,y,z,vx,vy,vz,a,t)
    implicit none
    real(16),intent(in) :: x,y,z,vx,vy,vz,a(3),t
    real(16) :: x1,y1,z1,positions(3)
    x1=x+vx*t+0.5*a(1)*t**2
    y1=y+vy*t+0.5*a(2)*t**2
    z1=z+vz*t+0.5*a(3)*t**2
    positions=[x1,y1,z1]
  end function

  function velocities(vx,vy,vz,a,a1,t)
    implicit none
    real(16),intent(in) :: vx,vy,vz,a(3),a1(3),t
    real(16) :: vx1,vy1,vz1,velocities(3)
    vx1=vx+0.5*(a(1)+a1(1))*t
    vy1=vy+0.5*(a(2)+a1(2))*t
    vz1=vz+0.5*(a(3)+a1(3))*t
    velocities=[vx1,vy1,vz1]
  end function

end module


c****************************************************************

	subroutine schbasis(ptm,r_sch,r_xyzschmat,r_schxyzmat)

c****************************************************************
c**
c**	FILE NAME: schbasis.f
c**
c**     DATE WRITTEN: 10/01/97
c**
c**     PROGRAMMER: Scott Hensley
c**
c** 	FUNCTIONAL DESCRIPTION: This routine computes the transformation
c**     matrix from xyz to a local sch frame.
c**
c**     ROUTINES CALLED:
c**
c**     NOTES: none
c**
c**     UPDATE LOG:
c**
c*****************************************************************

       	implicit none

c	INPUT VARIABLES:

c	structure /pegtrans/              !peg transformation parameters
c	   real*8 r_mat(3,3)
c	   real*8 r_matinv(3,3)
c	   real*8 r_ov(3)
c	   real*8 r_radcur
c	end structure
c	record /pegtrans/ ptm

	type pegtrans
          sequence
	  real (8) r_mat(3,3)
	  real (8) r_matinv(3,3)
	  real (8) r_ov(3)
	  real (8) r_radcur
        end type pegtrans

	type (pegtrans) ptm

	real*8 r_sch(3)                  !SCH position

c   	OUTPUT VARIABLES:

	real*8 r_xyzschmat(3,3)
	real*8 r_schxyzmat(3,3)

c	LOCAL VARIABLES:

        real*8  r_coss,r_cosc,r_sins,r_sinc
	real*8 r_xyzv(3),r_llh(3),r_schhdg
	real*8 r_matschxyzp(3,3)

c	DATA STATEMENTS: none

C	FUNCTION STATEMENTS:

c  	PROCESSING STEPS:

c       compute transformation from a sch local basis to X'Y'Z' basis

	r_coss = cos(r_sch(1)/ptm%r_radcur)
	r_sins = sin(r_sch(1)/ptm%r_radcur)

	r_cosc = cos(r_sch(2)/ptm%r_radcur)
	r_sinc = sin(r_sch(2)/ptm%r_radcur)

	r_matschxyzp(1,1) = -r_sins
	r_matschxyzp(1,2) = -r_sinc*r_coss
	r_matschxyzp(1,3) = r_coss*r_cosc
	r_matschxyzp(2,1) = r_coss
	r_matschxyzp(2,2) = -r_sinc*r_sins
	r_matschxyzp(2,3) = r_sins*r_cosc
	r_matschxyzp(3,1) = 0.0
	r_matschxyzp(3,2) = r_cosc
	r_matschxyzp(3,3) = r_sinc

c       compute sch to xyz matrix

        call matmat(ptm%r_mat,r_matschxyzp,r_schxyzmat)

c       get the inverse

	call tranmat(r_schxyzmat,r_xyzschmat)

        end


! =============================================================================
!                           Test FFT module
!
!                      Tests the indexing in FFTs.
!                    (start index 0 vs start index 1)
! =============================================================================
program test_fft_1
    use unit_test

    !Import FFT library:
    use stafft, only : initfft, dct, forfft
    use deriv1d
    use constants

    implicit none

    !Parameters and constants:
    !---------------------------------------------------------------------
    !Number of x & z grid divisions:
    integer,parameter:: nx=40, nz=20

    !Maximum number of parcels:
    integer,parameter:: nm=64*nx*nz

    !Box width and height:
    double precision,parameter:: ellx=4.d0, ellz=2.d0

     !Domain half widths and edge values:
    double precision,parameter:: hlx=ellx/two

    !Grid lengths and their inverses:
    double precision,parameter:: dx=ellx/dble(nx)
    double precision,parameter:: dz=ellz/dble(nz)

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

    double precision:: f0(nx,0:nz),f1(nx,0:nz)
    double precision:: f2(0:nx-1,0:nz)
    double precision:: s1(0:nz,nx),s2(0:nz,0:nx-1)

    double precision:: hrkx(nx),rkx(nx),rkz(nz)

    double precision:: xtrig(2*nx),ztrig(2*nz)
    integer:: xfactors(5),zfactors(5)

    double precision:: xg,zg,emax
    integer:: ix,iz,kx,kz

    !---------------------------------------------------------------------
    ! Set up FFTs:
    call initfft(nx,xfactors,xtrig)
    call initfft(nz,zfactors,ztrig)

    ! Define x wavenumbers:
    call init_deriv(nx,ellx,hrkx)
    rkx(1)=zero
    do kx=1,nx/2-1
        rkx(kx+1)   =hrkx(2*kx)
        rkx(nx+1-kx)=hrkx(2*kx)
    enddo
    rkx(nx/2+1)=hrkx(nx)

    ! Define z wavenumbers:
    call init_deriv(nz,ellz,rkz)

    ! For z derivatives of a cosine in z function:
    rkz(nz)=zero

    !-----------------------------------------------------------------
    ! Define a field:
    do iz=0,nz
        zg=dz*dble(iz)
        do ix=1,nx
            xg=dx*dble(ix-1)-hlx
            f0(ix,iz)=(zg*(ellz-zg))**2/(one-f12*cos(xg+one))
        enddo
    enddo
    f1=f0
    f2=f0

    !-----------------------------------------
    ! Forward z cosine FFT:
    call dct(nx,nz,f1,ztrig,zfactors)
    ! Transpose array:
    do ix=1,nx
        do kz=0,nz
            s1(kz,ix)=f1(ix,kz)
        enddo
    enddo
    ! Forward x FFT:
    call forfft(nz+1,nx,s1,xtrig,xfactors)

    !-----------------------------------------
    ! Forward z cosine FFT:
    call dct(nx,nz,f2,ztrig,zfactors)
    ! Transpose array:
    do ix=0,nx-1
        do kz=0,nz
            s2(kz,ix)=f2(ix,kz)
        enddo
    enddo
    ! Forward x FFT:
    call forfft(nz+1,nx,s2,xtrig,xfactors)

    !-----------------------------------------
    ! Check difference:
    emax=zero
    do kx=1,nx
        do kz=0,nz
            emax=max(emax,abs(s1(kz,kx)-s2(kz,kx-1)))
        enddo
    enddo

    ! final check
    call print_result_dp('Test FFT indexing', emax)

end program test_fft_1


program example

  implicit none

  integer :: array(8) = (/ 2, 8, 6, 1, 3, 5, 4, 7 /)

  call Gnomesort(array)
  write(*,*) array

contains

subroutine Gnomesort(a)

  integer, intent(in out) :: a(:)
  integer :: i, j, temp

  i = 2
  j = 3
  do while (i <= size(a))
    if (a(i-1) <= a(i)) then
      i = j
      j = j + 1
    else
      temp = a(i-1)
      a(i-1) = a(i)
      a(i) = temp
      i = i -  1
      if (i == 1) then
        i = j
        j = j + 1
      end if
    end if
  end do

end subroutine Gnomesort

end program example


program EthiopicMult
  implicit none

  print *, ethiopic(17, 34, .true.)

contains

  subroutine halve(v)
    integer, intent(inout) :: v
    v = int(v / 2)
  end subroutine halve

  subroutine doublit(v)
    integer, intent(inout) :: v
    v = v * 2
  end subroutine doublit

  function iseven(x)
    logical :: iseven
    integer, intent(in) :: x
    iseven = mod(x, 2) == 0
  end function iseven

  function ethiopic(multiplier, multiplicand, tutorialized) result(r)
    integer :: r
    integer, intent(in) :: multiplier, multiplicand
    logical, intent(in), optional :: tutorialized

    integer :: plier, plicand
    logical :: tutor

    plier = multiplier
    plicand = multiplicand

    if ( .not. present(tutorialized) ) then
       tutor = .false.
    else
       tutor = tutorialized
    endif

    r = 0

    if ( tutor ) write(*, '(A, I0, A, I0)') "ethiopian multiplication of ", plier, " by ", plicand

    do while(plier >= 1)
       if ( iseven(plier) ) then
          if (tutor) write(*, '(I4, " ", I6, A)') plier, plicand, " struck"
       else
          if (tutor) write(*, '(I4, " ", I6, A)') plier, plicand, " kept"
          r = r + plicand
       endif
       call halve(plier)
       call doublit(plicand)
    end do

  end function ethiopic

end program EthiopicMult


!    Program to fit a given set of data in terms of a given polynomial using
!    Singular Value Decomposition, see Golub and Van Loan, Matrix Computations
!    Numerical Recipes, chapter 15.
!    This explict example fits a polynomial to a given degree based. The example
!    input should look like
!     8 2    #  number of data entries and order of polynomial
!     0.001 	-2.89017 	0.00073621
!     0.002 	-2.88946 	0.00052732
!     0.005	-2.89067  	0.00055038
!     0.010 	-2.89091 	0.00040973
!     0.015 	-2.89084 	0.00034278
!     0.02  	-2.89086 	0.00029315
!     0.025 	-2.89059	0.00034278
!     0.03 	-2.89077 	0.00025017
!    The present input file  stems from a variational Monte Carlo calculation of the ground
!    state energy (second column). It contains also the standard deviation (3rd column)
!    The first column is the time step used in importance sampling. Extrapolating to zero
!    should give the best estimate for the VMC energy.
!
!    Author: Morten Hjorth-Jensen, Department of Physics, University of Oslo,
!    POB 1048 Blindern, N-0316 Oslo, Norway. email: morten.hjorth-jensen@fys.uio.no
!    Latest update: October 1999.
!    Run in old fashioned way, executable < inputfile  > outputfile

!
!     Main program starts here
!
      PROGRAM  fitting
      IMPLICIT NONE
      CALL vmcfit

      END  PROGRAM fitting
!
!     This subroutine fits a data set with  in terms
!     of a polynomial expansion. The number of terms in the polynomial
!     expansion is given by the variable number_terms
!     The number of data ( delta  and corresponding energy
!     and the standard error) are defined by the variable number_of_data
!
      SUBROUTINE  vmcfit
      IMPLICIT NONE
      INTEGER :: i, number_of_data, number_terms
      DOUBLE PRECISION, ALLOCATABLE, DIMENSION(:) :: w, n, e,sig,polynom_terms, variance
      DOUBLE PRECISION, ALLOCATABLE, DIMENSION(:,:) :: cvm, v, u
      DOUBLE PRECISION :: chisq, kappa, v_over_c, adiabatic
      EXTERNAL eos_param

      READ (5,*) number_of_data, number_terms
      ALLOCATE (cvm(number_terms,number_terms), &
                v(number_terms,number_terms) )
      ALLOCATE ( u(number_of_data,number_terms) )
      ALLOCATE ( w(number_terms) )
      ALLOCATE ( variance(number_terms) )
      ALLOCATE ( polynom_terms(number_terms) )
      ALLOCATE (n(number_of_data), e(number_of_data), sig(number_of_data) )
      DO i=1,number_of_data
         READ (5,*)n(i) , e(i), sig(i)
      ENDDO
      CALL svdfit(n,e,sig,number_of_data,polynom_terms, &
                  number_terms, &
                  u,v,w,number_of_data,number_terms,chisq)
      CALL svdvar(v,number_terms,number_terms,w,cvm,number_terms)
!     Compute now the variance for each coefficient using Eq. 15.4.19
!     of numerical recipes
      variance = 0.0; w = 1.0/(w*w)
      DO i = 1, number_terms
         variance(i) = SUM(v(i,:)*v(i,:)*w(:))
      ENDDO
      WRITE(*,*) ' Quality of fit:'
      WRITE(*,*) ' Number of terms in polynomial expansion:',  number_terms
      WRITE (*,'(''CHISQ = '',F12.5)') chisq
      WRITE(*,*) 'Extrapolated energy with error'
      WRITE(*,*) polynom_terms(1), SQRT(variance(1))
      DEALLOCATE (cvm, v)
      DEALLOCATE ( u )
      DEALLOCATE ( w,polynom_terms, variance )
      DEALLOCATE (n, e, sig)

      END SUBROUTINE vmcfit

!     This function encodes the actual functional form of the polynomial.
!     If you need to change the functional form, this is the function to adjust.
!     This specific version is a polynomial in x^i, with i >=0 and being an integer

      SUBROUTINE eos_param(x,afunc,ma)
      IMPLICIT NONE
      INTEGER :: ma, i
      DOUBLE PRECISION :: afunc, x
      DIMENSION afunc(ma)
      afunc(1)=1.D0
      DO i=2,ma
         afunc(i)=afunc(i-1)*x
      ENDDO

      END SUBROUTINE eos_param

      SUBROUTINE svdvar(v,ma,np,w,cvm,ncvm)
      IMPLICIT NONE
      INTEGER :: ma,ncvm,np,MMAX
      DOUBLE PRECISION ::  cvm(ncvm,ncvm),v(np,np),w(np)
      PARAMETER (MMAX=100)
      INTEGER :: i,j,k
      DOUBLE PRECISION :: sum,wti(MMAX)
      DO i=1,ma
        wti(i)=0.
        IF (w(i) /= 0.) wti(i)=1./(w(i)*w(i))
      ENDDO
      DO i=1,ma
         DO j=1,i
            sum=0.
            DO k=1,ma
               sum=sum+v(i,k)*v(j,k)*wti(k)
            ENDDO
            cvm(i,j)=sum
            cvm(j,i)=sum
         ENDDO
      ENDDO

      END SUBROUTINE svdvar

      SUBROUTINE svbksb(u,w,v,m,n,mp,np,b,x)
      IMPLICIT NONE
      INTEGER :: m,mp,n,np,NMAX
      DOUBLE PRECISION :: b(mp),u(mp,np),v(np,np),w(np),x(np)
      PARAMETER (NMAX=5000)
      INTEGER :: i,j,jj
      DOUBLE PRECISION :: s,tmp(NMAX)
      DO j=1,n
         s=0.
         IF (w(j) /= 0.) THEN
            DO i=1,m
               s=s+u(i,j)*b(i)
            ENDDO
            s=s/w(j)
         ENDIF
         tmp(j)=s
      ENDDO
      DO j=1,n
         s=0.
         DO jj=1,n
            s=s+v(j,jj)*tmp(jj)
         ENDDO
         x(j)=s
      ENDDO

      END SUBROUTINE svbksb

      SUBROUTINE svdfit(x,y,sig,ndata,a,ma,u,v,w,mp,np,chisq)
      IMPLICIT NONE
      INTEGER :: ma,mp,ndata,np,NMAX,MMAX
      DOUBLE PRECISION :: chisq,a(ma),sig(ndata),u(mp,np),v(np,np),w(np), &
                          x(ndata),  y(ndata),TOL
      PARAMETER (NMAX=5000,MMAX=100,TOL=1.e-5)
      INTEGER :: i,j
      DOUBLE PRECISION :: sum,thresh,tmp,wmax,afunc(MMAX), &
                          b(NMAX)

      DO i=1,ndata
         CALL  eos_param(x(i),afunc,ma)
         tmp=1./sig(i)
         DO j=1,ma
            u(i,j)=afunc(j)*tmp
         ENDDO
         b(i)=y(i)*tmp
      ENDDO
      CALL svdcmp(u,ndata,ma,mp,np,w,v)
      wmax=0.
      DO j=1,ma
         IF (w(j) > wmax)wmax=w(j)
      ENDDO
      thresh=TOL*wmax
      DO j=1,ma
         IF (w(j) < thresh)  w(j)=0.
      ENDDO
      CALL svbksb(u,w,v,ndata,ma,mp,np,b,a)
      chisq=0.
      DO i=1,ndata
         CALL eos_param(x(i),afunc,ma)
         sum=0.
         DO j=1,ma
            sum=sum+a(j)*afunc(j)
         ENDDO
         chisq=chisq+((y(i)-sum)/sig(i))**2
      ENDDO

      END SUBROUTINE svdfit

      FUNCTION pythag(a,b)
      IMPLICIT NONE
      DOUBLE PRECISION ::  a, b, pythag
      DOUBLE PRECISION :: absa, absb
      absa=ABS(a)
      absb=ABS(b)
      IF (absa > absb) THEN
         pythag=absa*SQRT(1.+(absb/absa)**2)
      ELSE
         IF(absb == 0.) THEN
            pythag=0.
         ELSE
            pythag=absb*SQRT(1.+(absa/absb)**2)
         ENDIF
      ENDIF

      END FUNCTION pythag

      SUBROUTINE svdcmp(a,m,n,mp,np,w,v)
      IMPLICIT NONE
      INTEGER :: m,mp,n,np,NMAX
      DOUBLE PRECISION :: a(mp,np),v(np,np),w(np)
      PARAMETER (NMAX=5000)
      INTEGER :: i,its,j,jj,k,l,nm
      DOUBLE PRECISION :: anorm,c,f,g,h,s,scale,x,y,z, &
                          rv1(NMAX),pythag
      g=0.0
      scale=0.0
      anorm=0.0
      DO i=1,n
         l=i+1
         rv1(i)=scale*g
         g=0.0
         s=0.0
         scale=0.0
         IF(i <= m)THEN
            DO k=i,m
               scale=scale+ABS(a(k,i))
            ENDDO
            IF(scale /= 0.0)THEN
               DO k=i,m
                  a(k,i)=a(k,i)/scale
                  s=s+a(k,i)*a(k,i)
               ENDDO
               f=a(i,i)
               g=-sign(SQRT(s),f)
               h=f*g-s
               a(i,i)=f-g
               DO j=l,n
                  s=0.0
                  DO k=i,m
                     s=s+a(k,i)*a(k,j)
                  ENDDO
                  f=s/h
                  DO k=i,m
                     a(k,j)=a(k,j)+f*a(k,i)
                  ENDDO
               ENDDO
               DO k=i,m
                  a(k,i)=scale*a(k,i)
               ENDDO
            ENDIF
         ENDIF
         w(i)=scale *g
         g=0.0
         s=0.0
         scale=0.0
         IF((i <= m).and.(i /= n))THEN
            DO k=l,n
               scale=scale+ABS(a(i,k))
            ENDDO
            IF(scale /= 0.0)THEN
               DO k=l,n
                  a(i,k)=a(i,k)/scale
                  s=s+a(i,k)*a(i,k)
               ENDDO
               f=a(i,l)
               g=-sign(SQRT(s),f)
               h=f*g-s
               a(i,l)=f-g
               DO k=l,n
                  rv1(k)=a(i,k)/h
               ENDDO
               DO j=l,m
                  s=0.0
                  DO k=l,n
                     s=s+a(j,k)*a(i,k)
                  ENDDO
                  DO k=l,n
                     a(j,k)=a(j,k)+s*rv1(k)
                  ENDDO
               ENDDO
               DO k=l,n
                  a(i,k)=scale*a(i,k)
               ENDDO
            ENDIF
         ENDIF
         anorm=max(anorm,(ABS(w(i))+ABS(rv1(i))))
      ENDDO
      DO i=n,1,-1
         IF(i < n)THEN
            IF(g /= 0.0)THEN
               DO j=l,n
                  v(j,i)=(a(i,j)/a(i,l))/g
               ENDDO
               DO j=l,n
                  s=0.0
                  DO k=l,n
                     s=s+a(i,k)*v(k,j)
                  ENDDO
                  DO k=l,n
                     v(k,j)=v(k,j)+s*v(k,i)
                  ENDDO
               ENDDO
            ENDIF
            DO j=l,n
               v(i,j)=0.0
               v(j,i)=0.0
            ENDDO
         ENDIF
         v(i,i)=1.0
         g=rv1(i)
         l=i
      ENDDO
      DO i=min(m,n),1,-1
         l=i+1
         g=w(i)
         DO j=l,n
            a(i,j)=0.0
         ENDDO
         IF(g /= 0.0)THEN
            g=1.0/g
            DO j=l,n
               s=0.0
               DO k=l,m
                  s=s+a(k,i)*a(k,j)
               ENDDO
               f=(s/a(i,i))*g
               DO k=i,m
                  a(k,j)=a(k,j)+f*a(k,i)
               ENDDO
            ENDDO
            DO j=i,m
               a(j,i)=a(j,i)*g
            ENDDO
         ELSE
            DO j= i,m
               a(j,i)=0.0
            ENDDO
         ENDIF
         a(i,i)=a(i,i)+1.0
      ENDDO
      DO k=n,1,-1
         DO its=1,30
            DO l=k,1,-1
               nm=l-1
               IF((ABS(rv1(l))+anorm) == anorm)  goto 2
               IF((ABS(w(nm))+anorm) == anorm)  goto 1
            ENDDO
1           c=0.0
            s=1.0
            DO i=l,k
               f=s*rv1(i)
               rv1(i)=c*rv1(i)
               IF((ABS(f)+anorm) == anorm) goto 2
               g=w(i)
               h=pythag(f,g)
               w(i)=h
               h=1.0/h
               c= (g*h)
               s=-(f*h)
               DO j=1,m
                  y=a(j,nm)
                  z=a(j,i)
                  a(j,nm)=(y*c)+(z*s)
                  a(j,i)=-(y*s)+(z*c)
               ENDDO
            ENDDO
2           z=w(k)
            IF(l == k)THEN
               IF(z < 0.0)THEN
                  w(k)=-z
                  DO j=1,n
                     v(j,k)=-v(j,k)
                  ENDDO
               ENDIF
               goto 3
            ENDIF
            IF(its == 30) THEN
               WRITE(*,*) 'no convergence in svdcmp'; STOP
            ENDIF
            x=w(l)
            nm=k-1
            y=w(nm)
            g=rv1(nm)
            h=rv1(k)
            f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y)
            g=pythag(f,1.D0)
            f=((x-z)*(x+z)+h*((y/(f+sign(g,f)))-h))/x
            c=1.0
            s=1.0
            DO j=l,nm
               i=j+1
               g=rv1(i)
               y=w(i)
               h=s*g
               g=c*g
               z=pythag(f,h)
               rv1(j)=z
               c=f/z
               s=h/z
               f= (x*c)+(g*s)
               g=-(x*s)+(g*c)
               h=y*s
               y=y*c
               DO jj=1,n
                  x=v(jj,j)
                  z=v(jj,i)
                  v(jj,j)= (x*c)+(z*s)
                  v(jj,i)=-(x*s)+(z*c)
               ENDDO
               z=pythag(f,h)
               w(j)=z
               IF(z /= 0.0)THEN
                  z=1.0/z
                  c=f*z
                  s=h*z
               ENDIF
               f= (c*g)+(s*y)
               x=-(s*g)+(c*y)
               DO jj=1,m
                  y=a(jj,j)
                  z=a(jj,i)
                  a(jj,j)= (y*c)+(z*s)
                  a(jj,i)=-(y*s)+(z*c)
               ENDDO
            ENDDO
            rv1(l)=0.0
            rv1(k)=f
            w(k)=x
         ENDDO
3        CONTINUE
      ENDDO

      END SUBROUTINE svdcmp


! Routine that computes fitness of each individual,
! and sorts them by decreasing fitness.
! Igor Lopes, February 2015
SUBROUTINE FITNESS(XMIN,XMAX,XVEC,POP,VFIT,OBJF,NBITS,NDIM,NPOP,NTBIT)
    IMPLICIT NONE
    ! Parameters
    REAL(8) R1 /1.0D0/
    REAL(8) R2 /2.0D0/
    ! Argument
    INTEGER :: NDIM, NPOP, NTBIT
    REAL(8) :: FMAX, FMIN
    INTEGER,DIMENSION(NDIM) :: NBITS
    REAL(8),DIMENSION(NPOP) :: VFIT, OBJF
    REAL(8),DIMENSION(NDIM) :: XMIN, XMAX
    REAL(8),DIMENSION(NDIM,NPOP)    :: XVEC
    INTEGER,DIMENSION(NPOP,NTBIT)   :: POP
    ! Locals
    INTEGER     :: IBIT, IDIM, IPOP, IPOS, ISUB, NPOS
    REAL(8)     :: AUX, BIN2REAL, EVALFUNC
    REAL(8),DIMENSION(NPOP) :: VFITO, OBJFO
    INTEGER,DIMENSION(NPOP,NTBIT)   :: POPO
    REAL(8),DIMENSION(NDIM,NPOP)    :: XVECO
    ! Algorithm
    ! Decode from binary to real
    DO IPOP=1,NPOP
        IPOS=0
        DO IDIM=1,NDIM
            AUX=BIN2REAL(POP(IPOP,IPOS+1:IPOS+NBITS(IDIM)),NBITS(IDIM))
            XVEC(IDIM,IPOP)=XMIN(IDIM)+AUX*(XMAX(IDIM)-XMIN(IDIM))/(R2**NBITS(IDIM)-R1)
            IPOS=IPOS+NBITS(IDIM)
        ENDDO
        OBJF(IPOP)=EVALFUNC(XVEC(:,IPOP),NDIM)
    ENDDO
    ! Minimization of function -> higher fitness for lower function value
    FMAX=MAXVAL(OBJF)
    FMIN=MINVAL(OBJF)
    DO IPOP=1,NPOP
        VFIT(IPOP)=FMAX-OBJF(IPOP)+0.01*(FMAX-FMIN)
        !artifitial increase of fitness in 1% of the best fitness
    ENDDO
    ! Sort by decreasing fitness
    POPO=POP
    VFITO=VFIT
    XVECO=XVEC
    OBJFO=OBJF
    POP=0
    VFIT=MINVAL(VFIT)
    POP(1,:)=POPO(1,:)
    VFIT(1)=VFITO(1)
    XVEC(:,1)=XVECO(:,1)
    OBJF(1)=OBJFO(1)
    DO IPOP=2,NPOP
        DO ISUB=1,IPOP-1
            IF(VFITO(IPOP).GE.VFIT(ISUB))THEN
                DO IPOS=IPOP,ISUB+1,-1
                    VFIT(IPOS)=VFIT(IPOS-1)
                    POP(IPOS,:)=POP(IPOS-1,:)
                    OBJF(IPOS)=OBJF(IPOS-1)
                    XVEC(:,IPOS)=XVEC(:,IPOS-1)
                ENDDO
                VFIT(ISUB)=VFITO(IPOP)
                POP(ISUB,:)=POPO(IPOP,:)
                OBJF(ISUB)=OBJFO(IPOP)
                XVEC(:,ISUB)=XVECO(:,IPOP)
                GOTO 10
            ENDIF
        ENDDO
        VFIT(IPOP)=VFITO(IPOP)
        POP(IPOP,:)=POPO(IPOP,:)
        OBJF(IPOP)=OBJFO(IPOP)
        XVEC(:,IPOP)=XVECO(:,IPOP)
10      CONTINUE
    ENDDO
END SUBROUTINE


      program bestfit

c***********************************************************************
c
c     Least squares fitting of arbitary functions to data
c     uses Gram-Schmidt Orthogonalisation procedure.
c
c     copyright daresbury laboratory 1993
c     author t. forester may 1993.
c
c***********************************************************************

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

c     maximum number of functions to fit

      Parameter (M1 = 20)

c     maximum number of data points (> M)

      Parameter (N1 = 1000)

      Dimension A(m1,n1), B(m1,m1),C(m1,n1),x(n1),y(n1),d(m1)

      write(*,*) 'Gram Schmidt Orthogonalisation with a maximum of '
     x     ,m1,' functions and ',n1, ' data points'

      write(*,'(//,a30)')'number of functions to use ?'
      read(*,*) m
      if(m.gt.m1) call error(1)

      write(*,*)'enter points x, y {type" -999 -999" to terminate list}'

      n = 0
   10 read(*,*) x1,y1
      if (x1.eq.-999.d0.and.y1.eq.-999.d0) goto 20
      if(n.ge.n1) call error(2)
      n = n+1
      x(n) = x1
      y(n) = y1
      goto 10

   20 continue

      if(n.lt.m) call error(4)
c
c     create a matrix - change function definitions here - only the first m
c     functions are used.

      do i = 1,n
c
c     define functions as power series
c     but in general may be any function you like

         if(m.ge.1) a(1,i) = 1
         if(m.ge.2) a(2,i) = x(i)
         if(m.ge.3) a(3,i) = x(i)*a(2,i)
         if(m.ge.4) a(4,i) = x(i)*a(3,i)
         if(m.ge.5) a(5,i) = x(i)*a(4,i)
         if(m.ge.6) a(6,i) = x(i)*a(5,i)
         if(m.ge.7) a(7,i) = x(i)*a(6,i)
         if(m.ge.8) a(8,i) = x(i)*a(7,i)
         if(m.ge.9) a(9,i) = x(i)*a(8,i)
         if(m.ge.10) a(10,i) = x(i)*a(9,i)
         if(m.ge.11) a(11,i) = x(i)*a(10,i)
         if(m.ge.12) a(12,i) = x(i)*a(11,i)
         if(m.ge.13) a(13,i) = x(i)*a(12,i)
         if(m.ge.14) a(14,i) = x(i)*a(13,i)
         if(m.ge.15) a(15,i) = x(i)*a(14,i)
         if(m.ge.16) a(16,i) = x(i)*a(15,i)
         if(m.ge.17) a(17,i) = x(i)*a(16,i)
         if(m.ge.18) a(18,i) = x(i)*a(17,i)
         if(m.ge.19) a(19,i) = x(i)*a(18,i)
         if(m.ge.20) a(20,i) = x(i)*a(19,i)

      enddo

c
c     construct b = a.(a transpose)  (m x m)

      do i = 1,m

         do j = i,m

            b(i,j) = 0.d0

            do k = 1,n

               b(i,j) = b(i,j) + a(i,k)*a(j,k)

            enddo

            b(j,i) = b(i,j)

         enddo

      enddo

c
c     find inverse of b

      call invert(m,m1,b,b)

c
c     construct (b^-1)a  (m x n)

      do i = 1,m

         do j = 1,n

            c(i,j) = 0.0d0

            do k = 1,m

               c(i,j) = c(i,j) + b(i,k)*a(k,j)

            enddo

         enddo

      enddo

c
c     find coefficients

      do i = 1,m

         d(i) = 0.d0

         do j = 1,n

            d(i) = d(i) + c(i,j)*y(j)

         enddo

         write(*,198) i,d(i)
  198    format (' coefficient',i5,4x,1p,e16.8)

      enddo

c
c     calculate best fit of data points
      write(*,*)
      write(*,'(3(5x,a5,5x))') 'x','y','fit'
      do i = 1,n

         ax = 0.d0

         do j = 1,m

            ax = ax + a(j,i)*d(j)

         enddo

         write(*,95) x(i), y(i), ax

   95    format(1p,3e16.8)

      enddo

      end
      subroutine error(i)

    1 format(/,' error - too many functions specified')
    2 format(/,' error - too many data points specified')
    3 format(/,' error - matrix too big for "invert"')
    4 format(/,' error - too few data points specified')

      if(i.eq.1) write(*,1)
      if(i.eq.2) write(*,2)
      if(i.eq.3) write(*,3)
      if(i.eq.4) write(*,4)

      call exit()
      return
      end

      subroutine invert(nnn,nmax,aaa,bbb)
c
c***********************************************************************
c
c     routine to invert a real symmetric matrix (reference:
c     computing methods v.ii, i.s. berezin and n.p.zhidkov,
c     pergamon press 1965). note that the matrices are
c     packed in the minimum storage mode, (i.e. a(k)=a(i,j),
c     where i>j and k=(i*(i-1))/2+j ).
c     the matrices aaa and bbb may be equivalenced though
c     this will destroy the contents of the original
c     array.
c
c     general version for all real symmetric matrices
c
c     copyright daresbury laboratory 1993
c     author w.smith (added to dl_poly july 1993)
c
c***********************************************************************
c
      implicit real*8(a-h,o-z)
c
c     maximum matrix (mbig*mbig) to invert

      parameter (mbig =100, bmax = mbig*(mbig+1)/2)
      dimension aaa(nmax*nmax),bbb(nmax*nmax),bbi(bmax)

      ind(i,j)=(max0(i,j)*(max0(i,j)-1))/2+min0(i,j)

      if(nnn.gt.mbig) call error(3)
c
c     pack matrix aaa into a triangular form

      do i = 1,nnn
         do j = 1,i
            k = ind(i,j)
            k1 = (i-1)*nmax + j
            bbi(k) = aaa(k1)
         enddo
      enddo
      do i = 1,nnn
         do j = 1,i
            k = ind(i,j)
            aaa(k) = bbi(k)
         enddo
      enddo

c
c     factorize matrix aaa into upper and lower forms
      do l=1,nnn
         do m=1,l
            k=ind(l,m)
            bbi(k)=0.d0
            bbb(k)=aaa(k)
            if(m.ne.1)then
               lm=m-1
               do n=1,lm
                  ln=ind(l,n)
                  mn=ind(m,n)
                  temv  =bbb(k)-bbb(ln)*bbb(mn)+bbi(ln)*bbi(mn)
                  bbi(k)=bbi(k)-bbi(ln)*bbb(mn)-bbb(ln)*bbi(mn)
                  bbb(k)=temv
               enddo
            endif
            if(l.eq.m)then
               if(bbb(k).lt.0.d0)then
                  bbi(k)=sqrt(abs(bbb(k)))
                  bbb(k)=0.d0
               else
                  bbb(k)=sqrt(abs(bbb(k)))
                  bbi(k)=0.d0
               endif
            else
               mm=ind(m,m)
               den=1.d0/(bbb(mm)**2+bbi(mm)**2)
               temv  =den*(bbb(k)*bbb(mm)+bbi(k)*bbi(mm))
               bbi(k)=den*(bbi(k)*bbb(mm)-bbb(k)*bbi(mm))
               bbb(k)=temv
            endif
         enddo
      enddo
c
c     invert lower triangular matrix
      do l=1,nnn
         n=ind(l,l)
         den=1.d0/(bbb(n)*bbb(n)+bbi(n)*bbi(n))
         do m=1,l
            k=ind(l,m)
            if(l.eq.m)then
               bbb(k)= den*bbb(k)
               bbi(k)=-den*bbi(k)
            else
               lm=l-1
               suma=0.d0
               sumb=0.d0
               do j=m,lm
                  lj=ind(l,j)
                  mj=ind(m,j)
                  suma=suma-bbb(lj)*bbb(mj)+bbi(lj)*bbi(mj)
                  sumb=sumb-bbb(lj)*bbi(mj)-bbi(lj)*bbb(mj)
               enddo
               temv  =den*(suma*bbb(n)+sumb*bbi(n))
               bbi(k)=den*(sumb*bbb(n)-suma*bbi(n))
               bbb(k)=temv
            endif
         enddo
      enddo
c
c     form product of upper and lower inverse triangular matrices
      do l=1,nnn
         do m=1,l
            sum=0.d0
            k=ind(l,m)
            do j=l,nnn
               lj=ind(l,j)
               mj=ind(m,j)
               sum=sum+bbb(lj)*bbb(mj)-bbi(lj)*bbi(mj)
            enddo
            bbb(k)=sum
         enddo
      enddo

c
c     unpack inverse matrix bbb

      do i = 1,nnn
         do j = 1,i
            k=ind(i,j)
            bbi(k) = bbb(k)
         enddo
      enddo

      do i = 1,nnn
         do j = 1,nnn
            k1 = (i-1)*nmax + j
            k = ind(i,j)
            bbb(k1) = bbi(k)
         enddo
      enddo

      return
      end


SUBROUTINE poly_interp(x1a,x2a,ya,x1,x2,y)
!------------------------------------------------------------------------
!
! Description:
!
!   Returns an interpolated value using polynomial interpolation (2D). If
!   input function is [2x2], bilinear interpolation is performed
!
!
! Dependencies:
!
!   Subroutine polint
!
! References:
!
!   Numerical Recipes
!
! Author: W. Imperatori
!
! Modified: January 2009 (v1.3)
!

use def_kind; use interfaces, only: polint

implicit none

! arrays for x,y coordinates
real(kind=r_single),dimension(:),intent(in)  :: x1a,x2a
! values of function at (x,y)
real(kind=r_single),dimension(:,:),intent(in):: ya
! coordinates of interpolated point
real(kind=r_single),intent(in)               :: x1,x2
! interpolated point
real(kind=r_single),intent(out)              :: y
! counter, vector length
integer(kind=i_single)                       :: i,m
! temporary arrays
real(kind=r_single),dimension(size(x1a))     :: ymtmp
real(kind=r_single),dimension(size(x2a))     :: yntmp

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

! check for data
if ( size(x1a) /= size(ya,1) ) call error_handling(7,'null','POLY_INTERP (interpolation.f90)')
if ( size(x2a) /= size(ya,2) ) call error_handling(7,'null','POLY_INTERP (interpolation.f90)')

m=size(x1a)

do i=1,m
   yntmp = ya(i,:)
   call polint(x2a,yntmp,x2,ymtmp(i))
enddo

call polint(x1a,ymtmp,x1,y)

END SUBROUTINE poly_interp

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

SUBROUTINE polint(xa,ya,x,y)
!----------------------------------------------------------------
!
! Description:
!
!   Returns value through polynomial interpolation (1D), that means
!   the returned value is y=P(x), where P is polynomial of degree
!   N-1 (N is the number of points in xa)
!
! Dependencies:
!
!   None
!
! References:
!
!   Numerical recipes
!
! Author: W. Imperatori
!
! Modified: January 2009 (v1.3)
!

use def_kind

implicit none

! arrays for x coordinate and y values
real(kind=r_single),dimension(:),intent(in):: xa,ya
! coordinate of interpolated value
real(kind=r_single),intent(in)             :: x
! interpolated value and error estimation
real(kind=r_single),intent(out)            :: y
! counter and dummies (locals)
real(kind=r_single)                        :: dy
integer(kind=i_single)                     :: m,n,ns
real(kind=r_single), dimension(size(xa))   :: c,d,den,ho
integer(kind=i_single),dimension(1)        :: imin

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

! check for data consistency
if ( size(xa) /= size(ya) ) call error_handling(7,'null','POLINT (interpolation.f90)')

n=size(xa)

! initialize tableau
c=ya
d=ya

ho=xa-x

! find index closest to table entry
imin=minloc(abs(x-xa))
ns=imin(1)

! initial approximation
y=ya(ns)
ns=ns-1

do m=1,n-1

   den(1:n-m)=ho(1:n-m)-ho(1+m:n)

   ! abort if two xa are identical
   if (any(den(1:n-m) == 0.0))  call error_handling(4,'null','POLINT (interpolation.f90)')

   den(1:n-m)=(c(2:n-m+1)-d(1:n-m))/den(1:n-m)
   d(1:n-m)=ho(1+m:n)*den(1:n-m)
   c(1:n-m)=ho(1:n-m)*den(1:n-m)

   if (2*ns < n-m) then
      dy=c(ns+1)
   else
          dy=d(ns)
          ns=ns-1
   endif

   y=y+dy

enddo

END SUBROUTINE polint

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

SUBROUTINE sampling_check
!----------------------------------------------------------------------
!
! Description:
!
!   Compute number of points (npts) for the interpolated time-series.
!   This value is to guarantee a Nyquist frequency >= the maximum
!   frequency used during coda wave computation AND a npts that is a
!   2**N value (necessary for FFT/IFFT subroutines)
!
! Dependencies:
!
!   None
!
! Notes:
!
!   For the "LF-HF merging" modality: to assure same time-series length
!   (in time), interpolation is performed here.
!
! Author: W. Imperatori
!
! Modified: January 2009 (v1.3)
!

use def_kind; use flags; use scattering, only: fmax,npts
use source_receiver, only: n_stat; use waveform

implicit none

! local varaibles
real(kind=r_single):: dt,exponent

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

if (modality_flag == 0) then

   ! compute HF time-sampling
   dt = hf_len / (hf_npts - 1)

   ! recompute number of points to closest 2**N number
   npts = hf_npts   !related number of points

   ! check if this is a 2**N value (if not, adjust npts)
   exponent = log(real(npts)) / log(2.0)

   if (exponent /= nint(exponent))  npts = 2**(ceiling(exponent))

   if (lf_len /= hf_len .or. hf_npts /= lf_npts)  then
      call error_handling(4,'null','SAMPLING_CHECK (interpolation.f90)')
   endif

else if (modality_flag /= 0) then

   ! assume maximum frequency for coda waves as Nyquist frequency
   dt = 1 / (2 * fmax)   !new time-step

   npts = nint(lf_len / dt) + 1   !related number of points

   ! check if this is a 2**N value (if not, adjust npts)
   exponent = log(real(npts)) / log(2.0)

   if (exponent /= nint(exponent))  npts = 2**(ceiling(exponent))
endif

END SUBROUTINE sampling_check

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

SUBROUTINE spline_interp(in_seis,time_len,interp_npts,orig_npts,out_seis)
!-------------------------------------------------------------------------------------
!
! Description:
!
! Performs spline interpolation on given time-series using a different sampling rate.
! Time series is assumed to be given in an 1 x npts array.
!
! Dependencies:
!
!   Subroutines spline, splint
!
! References:
!
!   Numerical Recipes
!
! Authors: W. Imperatori, M. Mai
!
! Modified: January 2009 (v1.3)
!
! Updated: February 2019 (v2.0)
!   Change dt_interp and dt_orig computations.
!
use def_kind
use tmp_para;
use waveform, only: lf_dt,v_npts

implicit none

! number of points of the new (interpolated) and original time-series
integer(kind=i_single),intent(in)           :: interp_npts,orig_npts
! time-length of the time-series
real(kind=r_single),intent(in)              :: time_len
! input time-series
real(kind=r_single),dimension(:),intent(in) :: in_seis       !assumed-shape vec
! output time-series
real(kind=r_single),dimension(:),intent(out):: out_seis     !assumed-shape vec
! local arrays for time vector and second derivative
real(kind=r_single),allocatable,dimension(:):: t_vec,sec_der
! time-vector, time-steps of interpolated and original time-series
real(kind=r_single)                         :: t_interp,dt_interp,dt_orig
! counter
integer(kind=i_single)                      :: j,n
! parameters used for spline interpolation
real(kind=r_single),parameter               :: yp1=2.0e20, ypn=2.0e20

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

! time-step of interpolated time-series
!dt_interp = time_len / (interp_npts-1)
n=ceiling(time_len/tmp_lf_len)
dt_interp = tmp_lf_len*n / (v_npts - 1)

! time-step of original time-series
!dt_orig = time_len / (orig_npts-1)
! dt_orig is from the actual dt from LF = lf_dt
dt_orig = lf_dt

if(.not.allocated(t_vec)) allocate(t_vec(orig_npts),sec_der(orig_npts))

!create original time vector
do j = 1,orig_npts
   t_vec(j) = dt_orig*(j-1)
enddo

! returns second derivative terms
call spline(t_vec,in_seis,orig_npts,yp1,ypn,sec_der)

! loop over number of points for interpolated time-series
do j = 1,interp_npts

   t_interp = dt_interp*(j-1)     !new time vector

   ! spline interpolation
   call splint(t_vec,in_seis,sec_der,orig_npts,t_interp,out_seis(j))

enddo

! deallocate temporary arrays
if(allocated(t_vec)) deallocate(t_vec,sec_der)

!<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>-

CONTAINS

   SUBROUTINE spline(x,y,n,yp1,ypn,y2)
   !---------------------------------------------------------------
   !
   ! Description:
   !
   !   Computes second derivative terms
   !
   ! Dependencies:
   !
   !   None
   !
   ! References:
   !
   !   Numerical Receipes
   !
   ! Authors: W. Imperatori, M. Mai
   !
   ! Modified: January 2009 (v1.3)
   !

   implicit none

   ! x-values and y-values of input time-series
   real(kind=r_single),dimension(:),intent(in):: x,y
   ! second derivative terms
   real(kind=r_single),dimension(:),intent(out):: y2
   ! original time-series number or points
   integer(kind=i_single),intent(in):: n
   ! parameters for interpolation
   real(kind=r_single),intent(in):: yp1,ypn
   ! local counters, dummies and indexes
   real(kind=r_single),dimension(30000):: u
   real(kind=r_single):: p,qn,sig,un
   integer(kind=i_single):: i,k

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

   if (yp1.gt.1e20) then
      y2(1)=0.
      u(1)=0.
   else
      y2(1)=-0.5
      u(1)=(3./(x(2)-x(1)))*((y(2)-y(1))/(x(2)-x(1))-yp1)
   endif

   do i=2,n-1
      sig=(x(i)-x(i-1))/(x(i+1)-x(i-1))
      p=sig*y2(i-1)+2.
      y2(i)=(sig-1.)/p
      u(i)=(6.*((y(i+1)-y(i))/(x(i+1)-x(i))-(y(i)-y(i-1))    &
           /(x(i)-x(i-1)))/(x(i+1)-x(i-1))-sig*u(i-1))/p
   enddo

   if (ypn.gt.1e20) then
      qn=0.
      un=0.
   else
      qn=0.5
      un=(3./(x(n)-x(n-1)))*(ypn-(y(n)-y(n-1))/(x(n)-x(n-1)))
   endif

   y2(n)=(un-qn*u(n-1))/(qn*y2(n-1)+1.)

   do k=n-1,1,-1
      y2(k)=y2(k)*y2(k+1)+u(k)
   enddo

   END SUBROUTINE spline

!<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>--<>-

   SUBROUTINE splint(xa,ya,y2a,n,x,y)
   !------------------------------------------------------------
   !
   ! Description:
   !
   !   Performs interpolation using spline
   !
   ! Dependencies:
   !
   !   None
   !
   ! References:
   !
   !   Numerical Receipes
   !
   ! Authors: W. Imperatori, M. Mai
   !
   ! Modified: January 2009 (v1.3)
   !

   implicit none

   ! x-values and y-values of input time-series, second derivative terms
   real(kind=r_single),dimension(:),intent(in):: xa,ya,y2a
   ! original time-series number of points
   integer(kind=i_single),intent(in):: n
   ! x-value of new (interpolated) time-series
   real(kind=r_single),intent(in):: x
   ! y-value of new (interpolated) time-series
   real(kind=r_single),intent(out):: y
   ! local counter and dummies
   integer(kind=i_single):: k,khi,klo
   real(kind=r_single):: a,b,h

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

   klo=1
   khi=n

1  if (khi-klo.gt.1) then
      k=(khi+klo)/2

      if (xa(k).gt.x) then
         khi=k
      else
         klo=k
      endif
      goto 1
   endif

   h=xa(khi)-xa(klo)

   ! aborting execution
   if (h.eq.0.) call error_handling(4,'null','SPLINT (interpolation.f90)')

   a=(xa(khi)-x)/h
   b=(x-xa(klo))/h
   y=a*ya(klo)+b*ya(khi)+((a**3-a) * y2a(klo)+(b**3-b)*y2a(khi))*(h**2)/6.

   END SUBROUTINE splint

END SUBROUTINE spline_interp

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


      FUNCTION BESSK0(X)
      REAL*8 Y,P1,P2,P3,P4,P5,P6,P7,
     *    Q1,Q2,Q3,Q4,Q5,Q6,Q7
      DATA P1,P2,P3,P4,P5,P6,P7/-0.57721566D0,0.42278420D0,0.23069756D0,
     *    0.3488590D-1,0.262698D-2,0.10750D-3,0.74D-5/
      DATA Q1,Q2,Q3,Q4,Q5,Q6,Q7/1.25331414D0,-0.7832358D-1,0.2189568D-1,
     *    -0.1062446D-1,0.587872D-2,-0.251540D-2,0.53208D-3/
      IF (X.LE.2.0) THEN
        Y=X*X/4.0
        BESSK0=(-LOG(X/2.0)*BESSI0(X))+(P1+Y*(P2+Y*(P3+
     *        Y*(P4+Y*(P5+Y*(P6+Y*P7))))))
      ELSE
        Y=(2.0/X)
        BESSK0=(EXP(-X)/SQRT(X))*(Q1+Y*(Q2+Y*(Q3+
     *        Y*(Q4+Y*(Q5+Y*(Q6+Y*Q7))))))
      ENDIF
      RETURN
      END


      SUBROUTINE DTRNS (NNH,SCOEF,LDIAG,TRIGS,ALP,RLAT,PHI)
C
C THIS SUBROUTINE PERFORMS INVERSE SPHERICAL HARMONIC TRANSFORMS
C INTO FOURIER SPACE USING THE ASSOCIATED LEGENDRE POLYNOMIALS.
C IT IS A REDUCED VERSION OF THE SHTRNS ROUTINE
C
C CALLED BY: EVAL
C CALLS: DFT991
C
C REVISIONS:
C 7-10-92 CHANGE TO CCM CODING CONVENTIONS (R. JAKOB)
C
C---- Model Parameters -------------------------------------------------
C
      INCLUDE 'params.i'
C
C
C------------ Arguments ------------------------------------------------
C
C     Input
C
C SPECTRAL TRUNCATION PARAMETER
      INTEGER NNH
C SPHERICAL HARMONIC COEFFICIENT ARRAY
      COMPLEX SCOEF(NALPH)
C INDEX INTO SPECTRAL ARRAY
      INTEGER LDIAG(0:MAXH,2)
C COEFFICIENTS FOR DISCRETE FOURIER TRANSFORM
      REAL TRIGS(MAXH+1,2)
C ASSOC. LEGENDRE POLYNOMIALS
      REAL ALP(NALPH)
C LATITUDE
      REAL RLAT
C
C     Output
C
C COMPUTED FUNCTION VALUE AT GRIDPOINT
      REAL PHI
C
C------ Local Variables ------------------------------------------------
C
      COMPLEX   CEVEN(MAXH+1), CODD(MAXH+1)
C
C     TEMPORARIES
C
      INTEGER JM,JN,IS
C
C     FOURIER COEFFICIENTS
C
      COMPLEX X(MAXH+1)
C
C----- Executable Statements -------------------------------------------
C
C     DETERMINE FOURIER COEFFICIENTS BY INVERSE LEGENDRE TRANSFORM.
C     VARY M AND N SO PROCEDURE MOVES ALONG DIAGONALS DENOTED BY
C     INDEX JN.  M IS GIVEN BY (JM-1) WHILE N IS GIVEN BY (JN+M).
C     FIRST ACCUMULATE EVEN WAVENUMBER CONTRIBUTION
C
      DO 180 JN=0,NNH,2
         IS = LDIAG(JN,2)
C
C        THIS DETOUR HELPS AVOID THE NEED TO SEPARATELY ZERO CEVEN
C
         IF (JN .EQ. 0) THEN
            DO 150 JM=1,LDIAG(0,1)
               CEVEN(JM) = SCOEF(IS+JM)*ALP(IS+JM)
  150       CONTINUE
C
         ELSE
C
            DO 170 JM=1,LDIAG(JN,1)
               CEVEN(JM) = CEVEN(JM) + SCOEF(IS+JM)*ALP(IS+JM)
  170       CONTINUE
C
         ENDIF
  180 CONTINUE
C
C     NEXT ACCUMULATE ODD WAVENUMBER CONTRIBUTION
C
      DO 215 JN=1,NNH,2
         IS = LDIAG(JN,2)
C
C        THIS DETOUR HELPS AVOID THE NEED TO SEPARATELY ZERO CODD
C
         IF (JN .EQ. 1) THEN
            DO 190 JM=1,LDIAG(1,1)
                  CODD (JM) = SCOEF(IS+JM)*ALP(IS+JM)
  190       CONTINUE
C
C        ACCOUNT FOR THE FACT THAT THE FIRST ODD DIAGONAL MAY BE
C        SHORTER THAN THE FIRST EVEN DIAGONAL (PART OF THE GAME
C        TO AVOID EXPLICITLY ZEROING THE ENTIRE CODD ARRAY)
C
            IF (LDIAG(1,1) .LT. LDIAG(0,1)) THEN
               DO 200 JM=LDIAG(1,1)+1, LDIAG(0,1)
                     CODD (JM) =  (0.0,0.0)
  200          CONTINUE
C
            ENDIF
         ELSE
C
            DO 210 JM=1,LDIAG(JN,1)
               CODD (JM) = CODD (JM) + SCOEF(IS+JM)*ALP(IS+JM)
  210       CONTINUE
C
         ENDIF
  215 CONTINUE
C
C     COMBINE CONTRIBUTIONS OF EVEN AND ODD WAVENUMBERS TO OBTAIN
C     COMPLEX FOURIER COEFFICIENTS, FOLLOWED BY INVERSE FFT
C
      DO 225 JM=1,LDIAG(0,1)
         IF (RLAT .GE. 0.0) THEN
            X(JM) = CEVEN(JM) + CODD(JM)
         ELSE
            X(JM) = CEVEN(JM) - CODD(JM)
         ENDIF
  225 CONTINUE
C
C     INVERSE FOURIER TRANSFORMATION
C
      CALL DFT991(X,TRIGS,MAXH,LDIAG(0,1),PHI)
C
      RETURN
C
      END


! subroutine geod2geo
SUBROUTINE geod2geo(yin,orb_elements,spheroid,phip,istat)
!   This routine converts a scalar from geodetic latitude
!   in degrees to geocentric in radians

!   * Re-written as an independent subroutine by JS, Aug 2014

!   Inputs:
!       yin
!       orb_elements
!           1. Orbital inclination (degrees)
!           2. Semi_major radius (m)
!           3. Angular velocity (rad sec-1)
!       spheroid
!           1. Spheroid major axis
!           2. Inverse flattening
!           3. Eccentricity squared
!           4. Earth rotational angular velocity rad/sec

!   Outputs:
!       phip
!       istat

    use sys_variables, only : pi, d2r, r2d

    implicit none

    double precision yin, phip
    double precision orb_elements(3), spheroid(4)
    double precision orad
    double precision asph, e2
    double precision rn, temp
    integer istat

!   Satellite orbital paramaters
!   semi_major radius (m)
    orad = orb_elements(2)

!   Spheroid paramaters
!   Spheroid major axis
!   Eccentricity squared
    asph = spheroid(1)
    e2 = spheroid(3)

!   Initialise the return status
    istat = 0

    temp = dble(yin)*d2r
    RN = asph/sqrt(1.0d0-e2*sin(temp)**2)
    phip = temp-asin(RN*e2*sin(temp)*cos(temp)/orad)

    return

END SUBROUTINE geod2geo


PROGRAM polyplot
  IMPLICIT NONE

  REAL :: x,y,f
  INTEGER :: i,j

  OPEN(unit=20,file='mydata.dat')

  ! write out function f(x,y) = exp(-x^2 + y^2))sin(xy) for -2 < x < 2 and -2 < y < 2

  DO i=-20,20,1
    x=i*0.1
    Do j=-20,20,1
      y=j*0.1
      f=exp(-(x**2 + y**2))*sin(x*y)
      WRITE(20,*) x, y, f
  END DO
      WRITE(20,*) " "
  END DO
END PROGRAM polyplot


module fcn_cylindricalduct_eigenfunction
    use mod_kinds,          only: rk, ik
    use mod_constants,      only: HALF, ZERO
    use type_point,         only: point_t
    use type_function,      only: function_t
    implicit none

    !>  Function for computing the eigenfunction for cylindrical duct modes.
    !!  This could be passed to a root-finding routine to find the zeros,
    !!  which are the cylindrical duct mode eigenvalues.
    !!
    !!  \f$    f(alpha_{mn}) = J'_m (alpha_{mn})    \f$
    !!
    !!  @author Nathan A. Wukie
    !!  @date   4/14/2016
    !!
    !!
    !------------------------------------------------------------------------
    type, extends(function_t) :: cylindricalduct_eigenfunction_f

    contains
        procedure :: init
        procedure :: compute
    end type cylindricalduct_eigenfunction_f
    !************************************************************************

contains

    !>  Initialization for eigenfunction class
    !!
    !!  @author Nathan A. Wukie
    !!  @date   4/14/2016
    !!
    !-------------------------------------------------------------------------
    subroutine init(self)
        class(cylindricalduct_eigenfunction_f),  intent(inout)   :: self

        ! Set function name
        call self%add_name("cylindricalduct_eigenfunction")

        ! Add option
        !
        ! The option 'm' here corresponds to m the azimuthal mode order
        !
        call self%add_option('m', ZERO)

    end subroutine init
    !*************************************************************************

    !>  Function implementation for cylindrical duct eigenfunction
    !!
    !!  \f$ f(alpha_{mn}) = J'_m (alpha_{mn})    \f$
    !!
    !!  @author Nathan A. Wukie
    !!  @date   4/14/2016
    !!
    !--------------------------------------------------------------------------
    impure elemental function compute(self,time,coord) result(val)
        class(cylindricalduct_eigenfunction_f), intent(inout)   :: self
        real(rk),                               intent(in)      :: time
        type(point_t),                          intent(in)      :: coord

        real(rk)    :: m
        real(rk)    :: x
        real(rk)    :: val

        ! Get x
        x = coord%c1_

        ! Get m
        m = self%get_option_value('m')

        ! We need the (derivative of the bessel function Jm) = Jm'
        !
        !   Jm'(x) = -J1(x)                        for   m=0
        !   Jm'(x) = 1/2( Jm-1(x)  -  Jm+1(x) )    for m/=0
        !
        if ( int(m) == 0 ) then
            val = -bessel_j1(x)
        else
            val = HALF*( bessel_jn(int(m-1),x) - bessel_jn(int(m+1),x) )
        end if

    end function compute
    !**************************************************************************

end module fcn_cylindricalduct_eigenfunction


      Program Biorthog
!
!     This program perfoms a borthogonalization between alpha and beta
!     orbitals obtained from an unrestricted Hartree-Fock or Density
!     Functional Theory Calculation.
!
      Use MQC_Algebra
      Use MQC_Gaussian
!
!
!     Variable Declarations...
!
      Implicit None
      Character(Len=256)::FileName
      Integer::IOut=6,NBasis=0,NElectrons=0,Multiplicity=0,NAlpha=0, &
        NBeta=0,NOccA=0,NOccB=0,NVirtA=0,NVirtB=0,ND=0,NSingA=0,NSingB=0, &
        I=0,Ind=0,NVirtP=0,IPrint=1
      Real::Thresh=0.1d0
      Type(MQC_Molecule_Data)::MoleculeInfo
      Type(MQC_MO_Coefficients)::MO_Coeff,Biorthog_Orbs
      Type(MQC_Fock_Matrix)::Fock_Matrix
      Type(MQC_Core_Hamiltonian)::Core_Hamiltonian
      Integer,Dimension(:),Allocatable::Basis2AtomMap
      Real,Dimension(:),Allocatable::A_SVals,Biorthog_Overlap
      Real,Dimension(:,:),Allocatable::Overlap_Matrix,A_UVecs,A_VVecs, &
        MO_Overlap
!
!
!     Get the user-defined filename from the command line and then call the
!     routine that reads the Gaussian matrix file.
!
      Call Get_Command_Argument(1,FileName)
      Call MQC_Gaussian_Read_Matrix_File(.True.,FileName,MoleculeInfo,NBasis, &
        NElectrons,Multiplicity,Basis2AtomMap,Overlap_Matrix,MO_Coeff,Core_Hamiltonian, &
        Fock_Matrix)

      If(IPrint.ge.1) then
        Write(*,*)' Back from Read_Gaussian_Matrix_File'
        Write(*,*)' NAtoms       = ',MoleculeInfo%NAtoms
        Write(*,*)' NBasis       = ',NBasis
        Write(*,*)' NElectrons   = ',NElectrons
        Write(*,*)' Multiplicity = ',Multiplicity
      EndIf

      NAlpha = (NElectrons + (Multiplicity-1))/2
      NBeta = NElectrons-NAlpha
      NOccA = NAlpha
      NOccB = NBeta
      NVirtA = NBasis-NAlpha
      NVirtB = NBasis-NBeta

      If(IPrint.ge.1) then
        Write(*,*)' NAlpha = ',NAlpha
        Write(*,*)' NBeta  = ',NBeta
        Write(*,*)' NOccA  = ',NOccA
        Write(*,*)' NOccB  = ',NOccB
        Write(*,*)' NVirtA = ',NVirtA
        Write(*,*)' NVirtB = ',NVirtB
        Call Print_Vector(IOut,MoleculeInfo%Atomic_Numbers,  &
          'Atomic Numbers')
        Call Print_Vector(IOut,MoleculeInfo%Atomic_Masses,  &
          'Atomic Masses')
        Call Print_Vector(IOut,MoleculeInfo%Atomic_Charges,  &
          'Atomic Charges')
        Call Print_Matrix(IOut,MoleculeInfo%Cartesian_Coordinates,  &
          'Cartesian Coordinates')
        Call Print_Vector(IOut,Basis2AtomMap,'Basis-Atom Mapping')
        Call Print_Matrix(IOut,Overlap_Matrix,'Overlap Matrix')
        Call Print_Matrix(IOut,Core_Hamiltonian%Alpha,'Core Hamiltonian Alpha')
        Call Print_Matrix(IOut,Core_Hamiltonian%Beta,'Core Hamiltonian Beta')
        Call Print_Matrix(IOut,Fock_Matrix%Alpha,'Alpha Fock Matrix')
        Call Print_Matrix(IOut,Fock_Matrix%Beta,'Beta Fock Matrix')
        Call Print_Matrix(IOut,MO_Coeff%Alpha,'Alpha MO Coefficient Matrix')
        Call Print_Matrix(IOut,MO_Coeff%Beta,'Beta MO Coefficient Matrix')
      EndIf
!
!     Allocate arrays in which to collect results
!
      Allocate(Biorthog_Overlap(NBasis))
      Call MQC_Gaussian_Fill_MO_Coefficients(NBasis,MO_Coeff%Alpha, &
        MO_Coeff%Beta,Biorthog_Orbs)
!
!     Do SVD on occupied alpha and beta orbitals and then rotate MO coefficients
!
      Write(*,*) 'Occupied Alpha - Occupied Beta'
      Allocate(MO_Overlap(NOccA,NOccB))
      MO_Overlap = MatMul(MatMul(Transpose(Biorthog_Orbs%Alpha(:,1:NOccA)),Overlap_Matrix), &
        Biorthog_Orbs%Beta(:,1:NOccB))
      If(IPrint.ge.1) Call Print_Matrix(IOut,MO_Overlap,' MO Overlap')
      Call MQC_Matrix_SVD(MO_Overlap,A_UVecs,A_VVecs,A_SVals)
      If(IPrint.ge.1) then
        Call Print_Matrix(IOut,A_UVecs,'U Vectors')
        Call Print_Matrix(IOut,A_VVecs,'V Vectors')
      EndIf
      Call Print_Vector(IOut,A_SVals,'Singular Values')
      Biorthog_Orbs%Alpha(:,1:NOccA) = MatMul(Biorthog_Orbs%Alpha(:,1:NOccA),A_UVecs)
      Biorthog_Orbs%Beta(:,1:NOccB) = MatMul(Biorthog_Orbs%Beta(:,1:NOccB),Transpose(A_VVecs))
      Biorthog_Overlap(1:Size(A_SVals,1)) = A_SVals
      If(IPrint.ge.1) then
        Call Print_Matrix(IOut,Biorthog_Orbs%Alpha(:,1:NOccA),'Biorthogonalized Alpha Orbitals')
        Call Print_Matrix(IOut,Biorthog_Orbs%Beta(:,1:NOccB),'Biorthogonalized Beta Orbitals')
      EndIf
      Do I = 1, Min(NOccA,NOccB)
        If(A_SVals(I).ge.Thresh) ND = ND + 1
      EndDo
!     Gaussian code has this always set so we never have singly occupied beta
      ND = NBeta
      NSingA = NAlpha - ND
      NSingB = NBeta - ND
!
!     Do SVD on left-over alpha occupieds and beta virtals and then rotate MO coefficients
!
      If(NSingA.gt.0) then
        Write(*,*) 'Singly Occupied Alpha - Virtual Beta'
        Deallocate(MO_Overlap,A_SVals,A_VVecs,A_UVecs)
        Allocate(MO_Overlap(NSingA,NVirtB))
        MO_Overlap = MatMul(MatMul(Transpose(Biorthog_Orbs%Alpha(:,ND+1:ND+NSingA)),Overlap_Matrix), &
          Biorthog_Orbs%Beta(:,NOccB+1:NBasis))
        If(IPrint.ge.1) Call Print_Matrix(IOut,MO_Overlap,' MO Overlap')
        Call MQC_Matrix_SVD(MO_Overlap,A_UVecs,A_VVecs,A_SVals)
        If(IPrint.ge.1) then
          Call Print_Matrix(IOut,A_UVecs,'U Vectors')
          Call Print_Matrix(IOut,A_VVecs,'V Vectors')
        EndIf
        Call Print_Vector(IOut,A_SVals,'Singular Values')
        Biorthog_Orbs%Alpha(:,ND+1:ND+NSingA) = MatMul(Biorthog_Orbs%Alpha(:,ND+1:ND+NSingA),A_UVecs)
        Biorthog_Orbs%Beta(:,NOccB+1:NOccB+NVirtB) = MatMul(Biorthog_Orbs%Beta(:,NOccB+1:NOccB+NVirtB),Transpose(A_VVecs))
        Biorthog_Overlap(ND+1:ND+NSingA) = A_SVals
        If(IPrint.ge.1) then
          Call Print_Matrix(IOut,Biorthog_Orbs%Alpha(:,ND+1:ND+NSingA),'Biorthogonalized Alpha Orbitals')
          Call Print_Matrix(IOut,Biorthog_Orbs%Beta(:,NOccB+1:NOccB+NVirtB),'Biorthogonalized Beta Orbitals')
        EndIf
      EndIf
!
!     Do SVD on left-over beta occupieds and alpha virtals and then rotate MO coefficients
!
      If(NSingB.gt.0) then
        Write(*,*) 'Singly Occupied Beta - Virtual Alpha'
        Deallocate(MO_Overlap,A_SVals,A_VVecs,A_UVecs)
        Allocate(MO_Overlap(NVirtA,NSingB))
        MO_Overlap = MatMul(MatMul(Transpose(Biorthog_Orbs%Alpha(:,NOccA+1:NBasis)),Overlap_Matrix), &
          Biorthog_Orbs%Beta(:,ND+1:ND+NSingB))
        If(IPrint.ge.1) Call Print_Matrix(IOut,MO_Overlap,' MO Overlap')
        Call MQC_Matrix_SVD(MO_Overlap,A_UVecs,A_VVecs,A_SVals)
        If(IPrint.ge.1) then
          Call Print_Matrix(IOut,A_UVecs,'U Vectors')
          Call Print_Matrix(IOut,A_VVecs,'V Vectors')
        EndIf
        Call Print_Vector(IOut,A_SVals,'Singular Values')
        Biorthog_Orbs%Alpha(:,NOccA+1:NOccA+NVirtA) = MatMul(Biorthog_Orbs%Alpha(:,NOccA+1:NOccA+NVirtA),A_UVecs)
        Biorthog_Orbs%Beta(:,ND+1:ND+NSingB) = MatMul(Biorthog_Orbs%Beta(:,ND+1:ND+NSingB),Transpose(A_VVecs))
        Biorthog_Overlap(ND+NSingA+1:ND+NSingA+NSingB) = A_SVals
        If(IPrint.ge.1) then
          Call Print_Matrix(IOut,Biorthog_Orbs%Alpha(:,ND+1:ND+NSingA),'Biorthogonalized Alpha Orbitals')
          Call Print_Matrix(IOut,Biorthog_Orbs%Beta(:,NOccB+1:NOccB+NVirtB),'Biorthogonalized Beta Orbitals')
        EndIf
      EndIf
!
!     Do SVD on alpha and beta virtal orbitals and then rotate MO coefficients
!
      Ind = ND + NSingA + NSingB
      NVirtP = NBasis - Ind
      If(NVirtP.gt.0) then
        Write(*,*) 'Virtual Alpha - Virtual Beta'
        Deallocate(MO_Overlap,A_SVals,A_VVecs,A_UVecs)
        Allocate(MO_Overlap(NVirtP,NVirtP))
        MO_Overlap = MatMul(MatMul(Transpose(Biorthog_Orbs%Alpha(:,Ind+1:Ind+NVirtP)),Overlap_Matrix), &
          Biorthog_Orbs%Beta(:,Ind+1:Ind+NVirtP))
        If(IPrint.ge.1) Call Print_Matrix(IOut,MO_Overlap,' MO Overlap')
        Call MQC_Matrix_SVD(MO_Overlap,A_UVecs,A_VVecs,A_SVals)
        If(IPrint.ge.1) then
          Call Print_Matrix(IOut,A_UVecs,'U Vectors')
          Call Print_Matrix(IOut,A_VVecs,'V Vectors')
        EndIf
        Call Print_Vector(IOut,A_SVals,'Singular Values')
        Biorthog_Orbs%Alpha(:,NOccA+1:NOccA+NVirtA) = MatMul(Biorthog_Orbs%Alpha(:,Ind+1:Ind+NVirtP),A_UVecs)
        Biorthog_Orbs%Beta(:,Ind+1:Ind+NVirtP) = MatMul(Biorthog_Orbs%Beta(:,Ind+1:Ind+NVirtP),Transpose(A_VVecs))
        Biorthog_Overlap(Ind+1:Ind+NVirtP) = A_SVals
        If(IPrint.ge.1) then
          Call Print_Matrix(IOut,Biorthog_Orbs%Alpha(:,Ind+1:Ind+NVirtP),'Biorthogonalized Alpha Orbitals')
          Call Print_Matrix(IOut,Biorthog_Orbs%Beta(:,Ind+1:Ind+NVirtP),'Biorthogonalized Beta Orbitals')
        EndIf
      EndIf
!
        Call Print_Matrix(IOut,Biorthog_Orbs%Alpha,'Biorthogonalized Alpha Orbitals')
        Call Print_Matrix(IOut,Biorthog_Orbs%Beta,'Biorthogonalized Beta Orbitals')
        Call Print_Vector(IOut,Biorthog_Overlap,'Biorthogonal Overlap')
!
 999  End Program Biorthog


      FUNCTION bessk1(x)
      REAL bessk1,x
CU    USES bessi1
      REAL bessi1
      DOUBLE PRECISION p1,p2,p3,p4,p5,p6,p7,q1,q2,q3,q4,q5,q6,q7,y
      SAVE p1,p2,p3,p4,p5,p6,p7,q1,q2,q3,q4,q5,q6,q7
      DATA p1,p2,p3,p4,p5,p6,p7/1.0d0,0.15443144d0,-0.67278579d0,
     *-0.18156897d0,-0.1919402d-1,-0.110404d-2,-0.4686d-4/
      DATA q1,q2,q3,q4,q5,q6,q7/1.25331414d0,0.23498619d0,-0.3655620d-1,
     *0.1504268d-1,-0.780353d-2,0.325614d-2,-0.68245d-3/
      if (x.le.2.0) then
        y=x*x/4.0
        bessk1=(log(x/2.0)*bessi1(x))+(1.0/x)*(p1+y*(p2+y*(p3+y*(p4+y*
     *(p5+y*(p6+y*p7))))))
      else
        y=2.0/x
        bessk1=(exp(-x)/sqrt(x))*(q1+y*(q2+y*(q3+y*(q4+y*(q5+y*(q6+y*
     *q7))))))
      endif
      return
      END

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