module parallel_loop
    ! Cyclic Technique
    ! or "Round-Robin"

    ! Modules
    use mpi

    ! Variables
    implicit none

    private
    public :: threaded_sum, shrinking_arrays

contains

subroutine shrinking_arrays(n)
    implicit none
    integer, intent(in) :: n
    integer :: i, ista, iend, total, total_sum
    integer :: rank, mpi_size, ierror!, tag, status(MPI_STATUS_SIZE)
    integer, allocatable :: a(:)

    ! Initialize MPI
    call MPI_INIT(ierror)
    call MPI_COMM_SIZE(MPI_COMM_WORLD, mpi_size, ierror)
    call MPI_COMM_RANK(MPI_COMM_WORLD, rank, ierror)

    call para_range(1, n, mpi_size, rank, ista, iend)

    if ( .not. allocated(a) ) allocate( a(ista:iend) )
    do i = ista, iend
        a(i) = i
    end do

    total = 0

    do i = ista, iend
        total = total + a(i)
    end do

    call MPI_REDUCE(total, total_sum, 1, MPI_INTEGER,MPI_SUM, 0,MPI_COMM_WORLD, ierror)

    if(rank == 0) write(*,*) "parallel total =",total_sum

    if ( allocated(a) ) deallocate(a)
    ! End MPI
    call MPI_FINALIZE(ierror)

    write(*,*) rank, ista, iend, total

end subroutine

subroutine threaded_sum(n,a)
    implicit none
    integer, intent(in) :: n
    integer, dimension(:) :: a
    integer :: i, total, total_sum

    integer :: rank, mpi_size, ierror!, tag, status(MPI_STATUS_SIZE)
    integer :: ista, iend ! For para_range

    ! Initialize MPI
    call MPI_INIT(ierror)
    call MPI_COMM_SIZE(MPI_COMM_WORLD, mpi_size, ierror)
    call MPI_COMM_RANK(MPI_COMM_WORLD, rank, ierror)

    call para_range(1, n, mpi_size, rank, ista, iend)

    do i = ista, iend
        a(i) = i
    end do

    total = 0

    do i = ista, iend
        total = total + a(i)
    end do

    call MPI_REDUCE(total, total_sum, 1, MPI_INTEGER,MPI_SUM, 0,MPI_COMM_WORLD, ierror)

    if(rank == 0) write(*,*) "parallel total =",total_sum

    ! End MPI
    call MPI_FINALIZE(ierror)

    write(*,*) rank, ista, iend, total

end subroutine threaded_sum

subroutine para_range( n1, n2, nprocs, irank, ista, iend)
!    integer, intent(in) :: n1, n2, nprocs, irank
!    integer, intent(in out) :: ista, iend

    integer :: n1 ! Lowest value of iteration variable
    integer :: n2 ! Highest value of iteration variable
    integer, intent(in) :: nprocs ! # cores
    integer, intent(in) :: irank ! Iproc (rank)
    integer :: ista ! Start of iterations for rank iproc
    integer :: iend ! End of iterations for rank iproc
    integer :: iwork1, iwork2

    iwork1 = ( n2 - n1 + 1 ) / nprocs
    iwork2 = MOD(n2 - n1 + 1, nprocs)
    ista = irank * iwork1 + n1 + MIN(irank, iwork2)
    iend = ista + iwork1 -1
    if ( iwork2 > irank ) iend = iend + 1
return
end subroutine para_range

end module parallel_loop


module mod_cte

   use iso_fortran_env, only: INT32, INT64, REAL32, REAL64

   implicit none

real :: START, FINISH, ACC_TIME=0.e0

   !---------------------!
   ! Constant parameters !
   !---------------------!

   integer, parameter     :: si = INT32  ! simple integer
   integer, parameter     :: di = INT64  ! double integer
   integer, parameter     :: sp = REAL32 ! simple precision
   integer, parameter     :: dp = REAL64 ! double precison
   integer, parameter     :: sc = 100
   integer, parameter     :: dc = 200
   integer, parameter     :: qc = 255

   integer, parameter     :: kd = dp

   real(kd), parameter    :: zero  = 0.0_kd
   real(kd), parameter    :: one   = 1.0_kd
   real(kd), parameter    :: two   = 2.0_kd
   real(kd), parameter    :: three = 3.0_kd
   real(kd), parameter    :: four  = 4.0_kd
   real(kd), parameter    :: five  = 5.0_kd
   real(kd), parameter    :: half  = one/two
   real(kd), parameter    :: pi    = four*atan( one )
   real(kd), parameter    :: euler = exp( one )

   complex(kd), parameter :: ci    = complex(zero,one)
   complex(kd), parameter :: cone  = complex(one,zero)
   complex(kd), parameter :: czero = complex(zero,zero)

   character*9, parameter  :: fmt_sp  = '(f16.8)'
   character*9, parameter  :: fmt_dp  = '(f24.16)'
   character*9, parameter  :: fmt_qp  = '(f40.32)'
   character*9, parameter  :: fmt_si  = '(I12)'
   character*9, parameter  :: fmt_exp = '(e24.16)'
   character*9, parameter  :: fmt_sci = '(es24.16)'
   character*30, parameter :: fmt_zp  = '(f23.16,SP,f23.16,"I")'
   character*30, parameter :: fmt_zp2 = '("(",f23.16,",",f23.16,") ")'
   character*30, parameter :: fmt_cp  = '(f15.8,SP,f15.8,"I")'
   character*30, parameter :: fmt_cp2 = '("(",f15.8,",",f15.8,") ")'

   logical, parameter     :: true = .true., false = .false.

   !--------------------!
   ! Phisical constants !
   !--------------------!
   real(kd), parameter    :: c_si = 299792458._kd*1d+0               ! speed of light
   real(kd), parameter    :: q_si = 1.602176634_kd*1d-19             ! electron charge
   real(kd), parameter    :: mp_si = 1.67262192369_kd*1d-27          ! proton mass
   real(kd), parameter    :: mn_si = 1.67492749804_kd*1d-27          ! neutron mass
   real(kd), parameter    :: me_si = 9.10938188_kd*1d-31             ! electron mass
   real(kd), parameter    :: avogadro_si = 6.02214076_kd*1d+23       ! Avogadro constant
   real(kd), parameter    :: planck_si = 6.62607015_kd*1d-34         ! Planck's constant
   real(kd), parameter    :: hbar_si = planck_si*half/pi             ! hbar
   real(kd), parameter    :: mu_0_si = 1.25663706212_kd*1d-6         ! mu_0
   real(kd), parameter    :: molar_gas_si = 8.314462618_kd*1d+0      ! molar gas constant
   real(kd), parameter    :: kB_si = molar_gas_si/avogadro_si        ! Boltzmann's const
   real(kd), parameter    :: epsilon_0_si = one/(mu_0_si*c_si**2)    ! epsilon_0
   real(kd), parameter    :: coulomb_si = one/(four*pi*epsilon_0_si) ! Coulomb's constant
   real(kd), parameter    :: alpha = 7.2973525693_kd*1d-3            ! Fine-structure constant
   real(kd), parameter    :: hartree_si = me_si*(alpha*c_si)**2      ! Hartree energy
   real(kd), parameter    :: a0_si = hbar_si/(c_si*me_si*alpha)      ! Bohr radius
   real(kd), parameter    :: u_si = 1d-3/avogadro_si                 ! atomic mass unit
   ! Some convertions of Joule to eV
   real(kd), parameter    :: hartree_ev = hartree_si/q_si
   real(kd), parameter    :: hbar_ev = hbar_si/q_si
   real(kd), parameter    :: kB_ev = kB_si/q_si
!  real(kd), parameter    :: J_to_ev = one/q_si

   !--------------------------!
   ! Computational parameters !
   !--------------------------!

   real(kd), parameter    :: min_exp = one*1d-100,     max_exp = one*1d+100
   real(kd), parameter    :: ethrs15 = 0.111_kd*1d-15, ethrs31 = 0.123_kd*1d-31
   real(kd), parameter    :: eps_kd  = epsilon(one)

end module mod_cte


      module eigen
      real*8 , allocatable::vx(:)

      contains

      subroutine tbal(w,a,n,ndim)
      implicit none
c   radix is the machine radix of floating.
      real*8 , parameter ::radix=2.d0,sqrdx=radix**2,rth=0.95d0
      integer*4 ,intent(in):: n,ndim
      integer*4 i,j
      real*8 ,intent(inout):: w(ndim,n)
      real*8 ,intent(out):: a(n,n)
      real*8 c,r,g,f,s,u
      logical*4 last
      a=w(1:n,:)
      w(1:n,:)=0.d0
      vx=1.d0
      do i=1,n
        w(i,i)=1.d0
      enddo
      last=.true.
      do while(last)
        last=.false.
        do i=1,n
          c=0.d0
          r=0.d0
          do j=1,n
            if(j .ne. i)then
              u=vx(j)/vx(i)
              c=c+abs(a(j,i)*u)
              r=r+abs(a(i,j)/u)
            endif
          enddo
          if(c .ne. 0.d0 .and. r .ne. 0.d0)then
            g=r/radix
            f=1.d0
            s=c+r
            do while(c .lt. g)
              f=f*radix
              c=c*sqrdx
            enddo
            g=r*radix
            do while(c .gt. g)
              f=f/radix
              c=c/sqrdx
            enddo
            if((c+r)/f .lt. rth*s)then
              last=.true.
              vx(i)=vx(i)/f
            endif
          endif
        enddo
      enddo
      return
      end subroutine

      subroutine thess(a,w,n,ndim)
      implicit none
      integer*4 n,ndim,i,i1,j
      real*8 a(n,n),w(ndim,n),aa(n)
      real*8 h1,p,q
      vx(2:n)=vx(2:n)**2
      do 10 i=1,n-2
        i1=i+1
        do 20 j=i1+1,n
          if(a(j,i) .ne. 0.d0)then
            if(abs(a(i1,i)) .gt. abs(a(j,i)))then
              p=a(j,i)/a(i1,i)
              h1=vx(i1)+vx(j)*p**2
              q=vx(j)*p/h1
              vx(j)=vx(i1)*vx(j)/h1
              vx(i1)=h1
              a(j,i)=0.d0
              a(j ,i1:n)=a(j ,i1:n)-p*a(i1,i1:n)
              a(i1,i1:n)=a(i1,i1:n)+q*a(j ,i1:n)
              a(:,i1)=a(:,i1)+p*a(:,j )
              a(:,j )=a(:,j )-q*a(:,i1)
              w(1:n,i1)=w(1:n,i1)+p*w(1:n,j )
              w(1:n,j )=w(1:n,j )-q*w(1:n,i1)
            else
              p=a(i1,i)/a(j,i)
              h1=vx(j)+vx(i1)*p**2
              q=vx(i1)*p/h1
              vx(j)=vx(i1)*vx(j)/h1
              vx(i1)=h1
              aa(i1:n)=a(j,i1:n)
              a(j ,i1:n)=p*aa(i1:n)-a(i1,i1:n)
              a(i1,i1:n)=aa(i1:n)-q*a(j ,i1:n)
              a(i1,i)=a(j,i)
              a(j ,i)=0.d0
              aa=a(:,i1)
              a(:,i1)=p*aa+a(:,j )
              a(:,j )=q*a(:,i1)-aa
              aa=w(1:n,i1)
              w(1:n,i1)=p*aa+w(1:n,j )
              w(1:n,j )=q*w(1:n,i1)-aa
            endif
          endif
 20     continue
 10   continue
      vx(2:n)=sqrt(vx(2:n))
      return
      end subroutine

      subroutine tqr(a,w,eig,n,ndim)
      use tfstk, only: ktfenanq
      implicit none
      integer*4 , parameter ::itmax=100
      real*8 ,parameter ::vmax=1.d10,vmin=1.d0/vmax,
     $     decth=1.d-16,threj=1.d-8,alpha=1.d-6,
     $     amth=1.d-30
      integer*4 , intent(in)::n,ndim
      integer*4 ibp,ib,ie,i,j,ie1,iter,is,itm,
     $     is1,is2,i1,i2,i3,jm,j1,ii
      integer*4 ibtab(n)
      real*8 , intent(inout)::a(n,n)
      real*8 , intent(out):: w(ndim,n),eig(2,n)
      real*8 aa(n),aa1(n)
      real*8 px(2),py(2)
      real*8 anorm,c,s,p,q,v1,v2,am,am1,w1,w2,w3,
     $     dec,sqrd,r,da,pk,pl,u,v,pm,pn,a1,a2,x,y,ee,sa,
     $     r1,r2,s1
      logical*4 paired,jordan
      complex*16 ca,cc,cu,cr,ck,cm
      equivalence (ck,px),(cm,py)
      equivalence (px,pk),(px(2),pl),(py,pm),(py(2),pn)
c     begin initialize for preventing compiler warning
      is2=0
      w1=0.d0
      w2=0.d0
      w3=0.d0
c     end   initialize for preventing compiler warning
      jordan=.false.
      ibp=1
      ibtab(1)=1
      ib=1
      ie=n
      do i=1,ie-2
        a(i+2,i)=0.d0
        a(min(i+3,ie),i)=0.d0
      enddo
      anorm=0.d0
      do i=1,n
        do j=max(1,i-1),n
          anorm=anorm+abs(vx(i)/vx(j)*a(i,j))
        enddo
      enddo
      anorm=anorm*2/n**2
      do1:do
        if(ie .le. ib+1)then
          if(ib .eq. 1)then
            exit
          else
            ie=ib-1
            ibp=ibp-1
            ib=ibtab(ibp)
            cycle do1
          endif
        endif
        ie1=ie-1
        iter=0
        itm=min(itmax,10*max(ie-ib+1,2))
        do2:do
c     write(*,'(1P4G15.7)')((vx(i)/vx(j)*a(i,j),j=1,4),i=1,4)
          do i=ie-1,ib,-1
            i1=i+1
            s=max(anorm,abs(a(i1,i1))+abs(a(i,i)))
            if(abs(vx(i1)/vx(i)*a(i1,i))+s .eq. s)then
              a(i1,i)=0.d0
              if(i1 .eq. ie)then
                ie=ie-1
                cycle do1
              endif
              ibp=ibp+1
              ibtab(ibp)=ib
              ib=i1
              cycle do1
            endif
          enddo
          do i=ib,ie
            if(abs(vx(i)) .lt. vmin  .or. abs(vx(i)) .gt. vmax)then
              j1=max(i-1,ib)
              a(i,j1:n)=a(i,j1:n)*vx(i)
              j1=min(ie,i+1)
              a(1:j1,i)=a(1:j1,i)/vx(i)
              w(1:n,i)=w(1:n,i)/vx(i)
              vx(i)=1.d0
            endif
          enddo
          do is=ie-2,ib,-1
            is1=is+1
            is2=is1+1
            a1=a(ie,ie)-a(is,is)
            a2=a(ie1,ie1)-a(is,is)
            w1=((a1*a2-a(ie1,ie)*a(ie,ie1))/a(is1,is)+a(is,is1))
     1           *vx(is)/vx(is1)
            if(w1 .eq. 0.d0)then
              w1=-alpha*a(ie1,ie)*a(ie,ie1)/a(is1,is)*vx(is)/vx(is1)
            endif
            w2=a(is1,is1)-a(is,is)-a1-a2
            w3=a(is2,is1)*vx(is2)/vx(is1)
            if(is .eq. ib)then
              exit
            endif
            u=abs(a(is,is-1)*vx(is)/vx(is-1))*(abs(w2)+abs(w3))
            v=abs(w1)*(abs(a(is-1,is-1))+abs(a(is,is))+abs(a(is1,is1)))
            if(u+v .eq. v)then
              a(is1,is-1)=0.d0
              a(is2,is-1)=0.d0
              exit
            endif
          enddo
          a(is2,is)=0.d0
          a(min(is2+1,ie),is)=0.d0
          am =hypot(w1,w2)
          am1=hypot(am,w3)
          if(is .gt. ib)then
            if(am1 .ne. 0.d0)then
              a(is,is-1)=a(is,is-1)*w1/am1*vx(is)
            endif
          endif
          do i=is-1,ie-2
            i1=i+1
            i2=i1+1
            i3=i2+1
            if(i1 .gt. is)then
              w1=vx(i1)*a(i1,i)
              w2=vx(i2)*a(i2,i)
              a(i2,i)=0.d0
              am =hypot(w1,w2)
              if(i3 .le. ie)then
                w3=vx(i3)*a(i3,i)
                a(i3,i)=0.d0
                am1=hypot(am,w3)
              else
                w3=0.d0
                am1=0.d0
              endif
            endif
            if(am .ne. 0.d0)then
              c=w1/am
              s=w2/am
              if(abs(c) .gt. abs(s))then
                v1=vx(i1)/c
                v2=c*vx(i2)
                p=s*vx(i1)/v2
                q=s*vx(i2)/v1
                vx(i1)=v1
                if(abs(v2) .gt. vmin .and. abs(v2) .lt. vmax)then
                  vx(i2)=v2
                  a(i2,i1:n)=a(i2,i1:n)-p*a(i1,i1:n)
                  a(i1,i1:n)=a(i1,i1:n)+q*a(i2,i1:n)
                  j1=min(ie,i3)
                  a(1:j1,i1)=a(1:j1,i1)+p*a(1:j1,i2)
                  a(1:j1,i2)=a(1:j1,i2)-q*a(1:j1,i1)
                  w(1:n,i1)=w(1:n,i1)+p*w(1:n,i2)
                  w(1:n,i2)=w(1:n,i2)-q*w(1:n,i1)
                else
                  vx(i2)=1.d0
                  aa(i1:n)=a(i2,i1:n)-p*a(i1,i1:n)
                  a(i1,i1:n)=a(i1,i1:n)+q*aa(i1:n)
                  a(i2,i1:n)=v2*aa(i1:n)
                  j1=min(ie,i3)
                  a(1:j1,i1)=a(1:j1,i1)+p*a(1:j1,i2)
                  a(1:j1,i2)=(a(1:j1,i2)-q*a(1:j1,i1))/v2
                  w(1:n,i1)=w(1:n,i1)+p*w(1:n,i2)
                  w(1:n,i2)=(w(1:n,i2)-q*w(1:n,i1))/v2
                endif
              else
                v1=vx(i2)/s
                v2=vx(i1)*s
                if(i1 .gt. is)then
                  a(i1,i)=w2/vx(i2)
                endif
                p=c*vx(i2)/v2
                q=c*vx(i1)/v1
                vx(i1)=v1
                if(abs(v2) .gt. vmin .and. abs(v2) .lt. vmax)then
                  vx(i2)=v2
                  aa(i1:n)=a(i2,i1:n)
                  a(i2,i1:n)=p*aa(i1:n)-a(i1,i1:n)
                  a(i1,i1:n)=aa(i1:n)-q*a(i2,i1:n)
                  j1=min(ie,i3)
                  aa(1:j1)=a(1:j1,i1)
                  a(1:j1,i1)= p*aa(1:j1)+a(1:j1,i2)
                  a(1:j1,i2)= q*a(1:j1,i1)-aa(1:j1)
                  aa=w(1:n,i1)
                  w(1:n,i1)= p*aa+w(1:n,i2)
                  w(1:n,i2)= q*w(1:n,i1)-aa
                else
                  vx(i2)=1.d0
                  aa(i1:n)=a(i2,i1:n)
                  aa1(i1:n)=p*aa(i1:n)-a(i1,i1:n)
                  a(i1,i1:n)=aa(i1:n)-q*aa1(i1:n)
                  a(i2,i1:n)=aa1(i1:n)*v2
                  j1=min(ie,i3)
                  aa(1:j1)=a(1:j1,i1)
                  a(1:j1,i1)= p*aa(1:j1)+a(1:j1,i2)
                  a(1:j1,i2)= (q*a(1:j1,i1)-aa(1:j1))/v2
                  aa=w(1:n,i1)
                  w(1:n,i1)= p*aa+w(1:n,i2)
                  w(1:n,i2)= (q*w(1:n,i1)-aa)/v2
                endif
              endif
            endif
            if(am1 .ne. 0.d0)then
              c=am/am1
              s=w3/am1
              if(abs(c) .gt. abs(s))then
                v1=vx(i1)/c
                v2=c*vx(i3)
                p=s*vx(i1)/v2
                q=s*vx(i3)/v1
                vx(i1)=v1
                vx(i3)=v2
                a(i3,i1:n)=a(i3,i1:n)-p*a(i1,i1:n)
                a(i1,i1:n)=a(i1,i1:n)+q*a(i3,i1:n)
                j1=min(ie,i3+1)
                a(1:j1,i1)=a(1:j1,i1)+p*a(1:j1,i3)
                a(1:j1,i3)=a(1:j1,i3)-q*a(1:j1,i1)
                w(1:n,i1)=w(1:n,i1)+p*w(1:n,i3)
                w(1:n,i3)=w(1:n,i3)-q*w(1:n,i1)
              else
                v1=vx(i3)/s
                v2=vx(i1)*s
                if(i1 .gt. is)then
                  a(i1,i)=w3/vx(i3)
                endif
                p=c*vx(i3)/v2
                q=c*vx(i1)/v1
                vx(i1)=v1
                vx(i3)=v2
                aa(i1:n)=a(i3,i1:n)
                a(i3,i1:n)=p*aa(i1:n)-a(i1,i1:n)
                a(i1,i1:n)=aa(i1:n)-q*a(i3,i1:n)
                j1=min(ie,i3+1)
                aa(1:j1)=a(1:j1,i1)
                a(1:j1,i1)= p*aa(1:j1)+a(1:j1,i3)
                a(1:j1,i3)= q*a(1:j1,i1)-aa(1:j1)
                aa=w(1:n,i1)
                w(1:n,i1)= p*aa+w(1:n,i3)
                w(1:n,i3)= q*w(1:n,i1)-aa
              endif
c     if(abs(vx(i1)) .gt. vmax .or. abs(vx(i1)) .lt. vmin .or.
c     $         abs(vx(i2)) .gt. vmax .or. abs(vx(i2)) .lt. vmin)then
c     write(*,*)'tqs-10 ',i1,vx(i1),vx(i2),v1,v2,c,s,
c     $           vx(8437),a(8437,8435)
c     endif
            endif
c     write(*,'(1P4G15.7)')((vx(ii)/vx(j)*a(ii,j),j=1,4),ii=1,4)
          enddo
          if(is .gt. ib)then
            a(is,is-1)=a(is,is-1)/vx(is)
          endif
          iter=iter+1
          if(iter .gt. itm)then
            write(*,*)' TEIGEN convergence failed. Range =',ib,ie
            write(*,*)
     1           '        Lower right corner =',
     1           a(ie-1,ie-1),vx(ie)/vx(ie-1)*a(ie,ie-1),a(ie,ie)
            a(ie,ie-1)=0.d0
            if(ktfenanq(a(ie,ie)))then
              a(ie,ie)=0.d0
              a(ie-1,ie-1)=0.d0
            endif
            if(ktfenanq(vx(ie)))then
              vx(ie)=1.d0
            endif
            if(ktfenanq(vx(ie-1)))then
              vx(ie-1)=1.d0
            endif
            iter=0
            ie=ie-1
            cycle do1
          endif
        enddo do2
      enddo do1
      do i=1,n
        j1=max(i-1,1)
        a(i,j1:n)=a(i,j1:n)*vx(i)
        j1=min(n,i+1)
        a(1:j1,i)=a(1:j1,i)/vx(i)
        w(1:n,i)=w(1:n,i)/vx(i)
      enddo
      paired=.false.
      do i=1,n-1
        if(paired)then
          paired=.false.
          cycle
        endif
        i1=i+1
        if(a(i1,i) .ne. 0.d0)then
          if(abs(a(i1,i)) .gt. abs(a(i,i1)))then
            aa(i:n)=a(i,i:n)
            a(i,i:n)=a(i1,i:n)
            a(i1,i:n)=aa(i:n)
            aa(1:i1)=a(1:i1,i)
            a(1:i1,i)=a(1:i1,i1)
            a(1:i1,i1)=aa(1:i1)
            aa=w(1:n,i)
            w(1:n,i)=w(1:n,i1)
            w(1:n,i1)=aa
          endif
          if(a(i1,i) .eq. 0.d0)then
            eig(1,i)=a(i,i)
            eig(2,i)=0.d0
            cycle
          endif
          s=a(i,i)-a(i1,i1)
          dec=s**2+4.d0*a(i1,i)*a(i,i1)
          s1=a(i,i)**2+a(i1,i1)**2
          if(dec .ge. -decth*s1)then
            sqrd=sqrt(abs(dec))
            p=2.d0*a(i1,i)/(s+sign(sqrd,s))
            a(i1,i:n)=a(i1,i:n)-p*a(i,i:n)
            a(1:i1,i)=a(1:i1,i)+p*a(1:i1,i1)
            w(1:n,i)=w(1:n,i)+p*w(1:n,i1)
            eig(1,i)=a(i,i)
            eig(1,i1)=a(i1,i1)
            eig(2,i)=0.d0
            eig(2,i1)=0.d0
            a(i1,i)=0.d0
          else
            p=-s/a(i,i1)*.5d0
            r=sign(
     1           sqrt(abs((a(i1,i)-p*(a(i,i)-a(i1,i1)+p*a(i,i1)))
     $           /a(i,i1))),a(i,i1))
            a(i1,i:n)=a(i1,i:n)-p*a(i,i:n)
            a(i ,i:n)=a(i ,i:n)*r
            a(1:i1,i)=(a(1:i1,i)+p*a(1:i1,i1))/r
            w(1:n,i)=(w(1:n,i)+p*w(1:n,i1))/r
            eig(1,i )=a(i ,i )
            eig(1,i1)=a(i1,i1)
            eig(2,i )=a(i ,i1)
            eig(2,i1)=a(i1,i )
          endif
          paired=.true.
        else
          eig(1,i)=a(i,i)
          eig(2,i)=0.d0
        endif
      enddo
      if(.not. paired)then
        eig(1,n)=a(n,n)
        eig(2,n)=0.d0
      endif
      do i=2,n
        if(a(i,i-1) .ne. 0.d0)then
          cycle
        endif
        i1=i+1
        if(eig(2,i) .eq. 0.d0)then
          jm=0
          do  j=i-1,1,-1
            if(a(j+1,j) .ne. 0.d0)then
              cycle
            endif
            if(eig(2,j) .eq. 0.d0)then
              da=a(j,j)-a(i,i)
              sa=abs(a(j,j))+abs(a(i,i))
              if(abs(da) .gt. threj*sa)then
                p=a(j,i)/da
                a(j,i:n)=a(j,i:n)+p*a(i,i:n)
                a(1:j,i)=a(1:j,i)-p*a(1:j,j)
                w(1:n,i)=w(1:n,i)-p*w(1:n,j)
              else
                jordan=.true.
              endif
            else
              j1=j-1
              u=a(i,i)-a(j1,j1)
              v=a(j1,j)
              r=u**2+v**2
              pk=(a(j1,i)*u+a(j ,i)*v)/r
              pl=(a(j ,i)*u-a(j1,i)*v)/r
              a(j1,i:n)=a(j1,i:n)-pk*a(i,i:n)
              a(j ,i:n)=a(j ,i:n)-pl*a(i,i:n)
              a(1:j,i)=a(1:j,i)+pk*a(1:j,j1)+pl*a(1:j,j)
              w(1:n,i)=w(1:n,i)+pk*w(1:n,j1)+pl*w(1:n,j)
            endif
          enddo
        else
          do j=i-1,1,-1
            if(a(j+1,j) .ne. 0.d0)then
              cycle
            endif
            if(eig(2,j) .eq. 0.d0)then
              x=a(i,i)-a(j,j)
              y=a(i,i1)
              r=x**2+y**2
              pk=(a(j,i )*x+a(j,i1)*y)/r
              pl=(a(j,i1)*x-a(j,i )*y)/r
              a(j,i:n)=a(j,i:n)-pk*a(i,i:n)-pl*a(i1,i:n)
              a(1:j,i )=a(1:j,i )+pk*a(1:j,j)
              a(1:j,i1)=a(1:j,i1)+pl*a(1:j,j)
              w(1:n,i )=w(1:n,i )+pk*w(1:n,j)
              w(1:n,i1)=w(1:n,i1)+pl*w(1:n,j)
            else
              j1=j-1
              cu=dcmplx(a(j1,j1)-a(i,i),-a(i,i1))
              v =a(j1,j)
              cr=cu**2+v**2
              if(cr .ne. (0.d0,0.d0))then
                ca=dcmplx(a(j1,i),a(j1,i1))
                cc=dcmplx(a(j ,i),a(j ,i1))
                ck=(cu*ca-v*cc)/cr
                cm=(cu*cc+v*ca)/cr
                a(j1,i:n)=a(j1,i:n)+pk*a(i,i:n)+pl*a(i1,i:n)
                a(j,i:n )=a(j ,i:n)+pm*a(i,i:n)+pn*a(i1,i:n)
                a(1:j,i )=a(1:j,i )-pk*a(1:j,j1)-pm*a(1:j,j)
                a(1:j,i1)=a(1:j,i1)-pl*a(1:j,j1)-pn*a(1:j,j)
                w(1:n,i )=w(1:n,i )-pk*w(1:n,j1)-pm*w(1:n,j)
                w(1:n,i1)=w(1:n,i1)-pl*w(1:n,j1)-pn*w(1:n,j)
              endif
            endif
          enddo
        endif
      enddo
      if(jordan)then
        do i=n,2,-1
          if(eig(2,i) .eq. 0.d0)then
            jm=0
            do j=i-1,1,-1
              if(eig(1,j) .eq. eig(1,i) .and. eig(2,j) .eq. 0.d0)then
                r=a(j,i)
                if(abs(r) .gt. amth)then
                  if(jm .eq. 0)then
                    r1=sqrt(abs(r))
                    r2=r/r1
                    a(j,j:n)=a(j,j:n)/r1
                    a(1:j,j)=a(1:j,j)*r1
                    w(1:n,j)=w(1:n,j)*r1
                    a(1:i,i)=a(1:i,i)/r2
                    a(i,i:n)=a(i,i:n)*r2
                    w(1:n,i)=w(1:n,i)/r2
                    jm=j
                  else
                    a(j,jm:n)=a(j,jm:n)-r*a(jm,jm:n)
                    a(1:j,jm)=a(1:j,jm)+r*a(1:j,j)
                    w(1:n,jm)=w(1:n,jm)+r*w(1:n,j)
                  endif
                else
                  a(j,i)=0.d0
                endif
              endif
            enddo
          endif
        enddo
      endif
c     do 1000 k=1,n
c       write(*,'(1X,:1P10G13.6)')(a(k,j),j=1,n)
c1000  continue
c     do 1010 k=1,n
c       write(*,'(1X,:1P10G13.6)')(w(k,j),j=1,n)
c1010  continue
c     write(*,*)
      if(mod(n,2) .eq. 0)then
        ii=0
        do i=1,n
          if(eig(2,i) .gt. 0.d0 .and. ii .ne. 0)then
            aa=w(1:n,i-1)
            w(1:n,i-1)=w(1:n,i  )
            w(1:n,i  )=w(1:n,i+1)
            w(1:n,i+1)=aa
            ee=eig(1,i-1)
            eig(1,i-1)=eig(1,i  )
            eig(1,i  )=eig(1,i+1)
            eig(1,i+1)=ee
            ee=eig(2,i-1)
            eig(2,i-1)=eig(2,i  )
            eig(2,i  )=eig(2,i+1)
            eig(2,i+1)=ee
          endif
          ii=1-ii
        enddo
      endif
      return
      end subroutine

      subroutine teigenc(a,w,ceig,n,ndim)
      use iso_c_binding
      implicit none
      integer*4 ,intent(in):: n,ndim
      real*8 ,intent(inout):: a(ndim,n),w(n,n)
      real*8 ,pointer :: eig(:,:)
      complex*16 ,target ::ceig(n)
      call c_f_pointer(c_loc(ceig),eig,[2,n])
      call teigen(a,w,eig,n,ndim)
      return
      end subroutine

      end module

      subroutine teigen(a,w,eig,n,ndim)
c
c   Subroutine to obtain eigen values and eigen vectors of a real
c   matrix a.
c                                7-Dec-1987   K. Oide
c   Modified                    27-Oct-1989   K. Oide
c   Modified                    17-Nov-1989   K. Oide
c   Added Balancing              1-Nov-1990   K. Oide
c   Modified Convergence        20-Nov-1990   K. Oide
c   Modified Shifting            4-Dec-1990   K. Oide
c   Fast Givens' Transformation 21-Jan-1992   K. Oide
c   Maximum size 14,000         29-May-2010   K. Oide
c   Maximum size 32768           5-Oct-2010   K. Oide
c
c   Usage:       call teigen(a,w,eig,n,ndim)
c
c                where
c
c                a     is the input n*n matrix, and the eigen vectors
c                      are returned here.
c                w     is a work area of n**2 words.
c                eig   is a (2,n) real array where the eigen values
c                      are returned.   eig(1,i) and eig(2,i) contain
c                      the real and imaginary part of the i-th eigen-
c                      valeu respectively.   When the eigenvalue is
c                      a complex, the output are stored as
c
c                      eig(1,i  ) =      er
c                      eig(2,i  ) =     !ei!
c                      eig(1,i+1) =      er
c                      eig(2,i+1) =    -!ei!
c
c                      where er and ei are the real and imaginary
c                      part of the i-th eigenvalue respectively.
c                n     is the matrix dimension.
c                ndim  is the size of the first dimension of a.
c
c   Restriction: When the matrix cannot be diagonalized (Jordan
c                standard form), the returned eigen vectors are not
c                correct.
c
      use eigen
      implicit none
      integer*4 ,intent(in):: n,ndim
      integer*4 ,parameter ::nmax=32768
      real*8 ,intent(inout):: a(ndim,n)
      real*8 ,intent(out):: w(n,n),eig(2,n)
      if(n .gt. nmax)then
        write(*,*)'TEIGEN-Too large matrix.',n
      else
        allocate (vx(n))
        call tbal(a,w,n,ndim)
        call thess(w,a,n,ndim)
        call tqr(w,a,eig,n,ndim)
        deallocate (vx)
      endif
      return
      end subroutine


module checkcsfs
       use prep, only: i16b, ARRAY_START_LENGTH, ARRAY_SHORT_LENGTH, DET_MAX_LENGTH, equals0, COEFTOL
       use, intrinsic :: iso_fortran_env, only: rk => real64
       use projection, only : Lplus_multiple, Lminus_multiple, LzLzplus1_append, &
           &Splus_multiple, Sminus_multiple, SzSzplus1_append

       implicit none

       contains

       subroutine checkcsfLsq(inbasislist, incoeflist, L, nbasis, ncsf)
           integer(i16b), intent(in) :: inbasislist(:, :)
           real(rk), intent(in) :: incoeflist(:)
           integer, intent(in) :: nbasis, ncsf, L
           integer :: i, j, k
 !          do i = 1, ncsf
 !              do j = 1, nbasis
 !                 call initlists(inbasislist(j, 1:2), baisslist1, coeflist1, eposes1,       iszeros1,
       endsubroutine checkcsfLsq

       subroutine Lsq_multiple(inbasis, incoefs, inepos, iniszeros, innum, &
               & basislist, coeflist, eposlist, iszerolist, num)
           integer(i16b), intent(in) :: inbasis(:, :)
           real(rk), intent(in) :: incoefs(:)
           integer, intent(in) :: inepos(:, :, :)
           logical, intent(in) :: iniszeros(:)
           integer, intent(in) :: innum
           integer(i16b), allocatable :: basislist(:, :), basislist1(:, :)
           real(rk), allocatable :: coeflist(:), coeflist1(:)
           integer, allocatable :: eposlist(:, :, :), eposlist1(:, :, :)
           logical, allocatable :: iszerolist(:), iszerolist1(:)
           integer :: num, num1

           ! write(*, *) ' '
           ! write(*, *) '>>>>>>>>>>>>>>>>>>>>>>> Lsq_multiple >>>>>>>>>>>>>>>>>>>>>>>>>>> '

           !-------set intermediates (basislist1...) and output ( basislist ... ) empty
           num1 = 0
           num = 0
           !----------L^2 = L-L+ + Lz^2 + Lz
           call Lplus_multiple(inbasis, incoefs, inepos, iniszeros, innum, &
                   & basislist1, coeflist1, eposlist1, iszerolist1, num1)
           call Lminus_multiple(basislist1, coeflist1, eposlist1, iszerolist1, num1, &
                   & basislist, coeflist, eposlist, iszerolist, num)
           call LzLzplus1_append(inbasis, incoefs, inepos, iniszeros, innum, &
                   & basislist, coeflist, eposlist, iszerolist, num)
       end subroutine Lsq_multiple

       subroutine Ssq_multiple(inbasis, incoefs, inepos, iniszeros, innum, &
               & basislist, coeflist, eposlist, iszerolist, num)
            integer(i16b), intent(in) :: inbasis(:, :)
            real(rk), intent(in) :: incoefs(:)
            integer, intent(in) :: inepos(:, :, :)
            logical, intent(in) :: iniszeros(:)
            integer, intent(in) :: innum
            integer(i16b), allocatable :: basislist(:, :), basislist1(:, :)
            real(rk), allocatable :: coeflist(:), coeflist1(:)
            integer, allocatable :: eposlist(:, :, :), eposlist1(:, :, :)
            logical, allocatable :: iszerolist(:), iszerolist1(:)
            integer :: num, num1

            ! write(*, *) '>>>>>>>>>>>>>>>>>>>>>>> Lsq_multiple '

            !-------set intermediates (basislist1...) and output ( basislist ... ) empty
            num1 = 0
            num = 0
            !----------S^2 = S-S+ + Sz^2 + Sz
            call Splus_multiple(inbasis, incoefs, inepos, iniszeros, innum, &
                    & basislist1, coeflist1, eposlist1, iszerolist1, num1)
            call Sminus_multiple(basislist1, coeflist1, eposlist1, iszerolist1, num1, &
                    & basislist, coeflist, eposlist, iszerolist, num)
            call SzSzplus1_append(inbasis, incoefs, inepos, iniszeros, innum, &
                    & basislist, coeflist, eposlist, iszerolist, num)
       end subroutine Ssq_multiple

       !subroutine sortBasisCoefTable(inbasislist, incoeftable, nbasis, ncsf)
       !    integer(i16b) :: inbasislist(:, :)
       !    integer(i16b), allocatable:: tpbasis(:, :)
       !    real(rk) :: incoeftable(:, :)
       !    real(rk), allocatable :: tpcoefs(:, :)
       !    integer, intent(in) :: nbasis, ncsf
       !    integer :: i, j, nordered, k
       !    integer, allocatable :: order(:) !indices of dets, from small to large
       !    allocate(tpbasis(nbasis, 2))
       !    allocate(tpcoefs(nbasis, ncsf))
       !    allocate(order(nbasis))
       !    do i = 1, nbasis
       !        tpbasis(i, 1:2) = inbasislist(i, 1:2)
       !        tpcoefs(i, 1:ncsf) = incoeftable(i, 1:ncsf)
       !    enddo
       !    order(1) = 1 !
       !    nordered = 1 ! number of ordered dets
       !    do i = 2, nbasis
       !         do j = 1, nordered

!                    if(tpbasis(i, 1).lt.tpbasis(order(j), 1)) then  !1<
 !                       do k = nordered, j, -1
  !                          order(k + 1) = order(k)
  !                      enddo
  !                      order(j) = i
  !                      nordered = nordered + 1
  !                      exit
!
!                    else if(tpbasis(i, 1).eq.tpbasis(order(j), 1)) then !1=
!                        if(tpbasis(i, 2).lt.tpbasis(order(j), 2)) then  !2<
!                            do k = nordered, j, -1
!                                order(k + 1) = order(k)
!                            enddo
!                            order(j) = i
!                            nordered = nordered + 1
!                            exit
!                        elseif(tpbasis(i, 2).gt.tpbasis(order(j), 2)) then !2>
!                            !i has same updet as order(j) but bigger dndet
!                            !should be placed behind order(j)
!                            if(nordered.eq.j) then   !3
!                                order(j+1) = i
!                                nordered = nordered + 1
!                                exit
!                            else
!                                do k = nordered, j+1, -1
!                                    order(k + 1) = order(k)
!                                enddo
!                                order(j + 1) = i
!                                nordered = nordered + 1
!                                exit
 !                           endif        !3
 !                       endif        !2
!
!                    else!1 basis i, 1 > basis order(j)
!                        if(nordered.eq.j) then
!                            order(j+1) = i
!                            nordered = nordered + 1
!                            exit
!                        endif
!                    endif !1
!
!                enddo !j
!            enddo !i

            ! write(*, *) '----------------------- Sorting csf table by dets----------------'

 !           do i = 1, nbasis
                ! write(*, *) order(i)
 !           enddo

  !          do i = 1, nbasis
  !              inbasislist(i, 1:2) = tpbasis(order(i), 1:2)
  !              incoeftable(i, 1:ncsf) = tpcoefs(order(i), 1:ncsf)
  !          enddo
  !          deallocate(tpbasis)
  !          deallocate(tpcoefs)
  !          deallocate(order)
!
!            do i = 1, nbasis
                ! write(*, '(2B16)') inbasislist(i, 1:2)
                ! write(*, *) incoeftable(i, 1:ncsf)
!            end do
!       endsubroutine sortBasisCoefTable

       logical function all0(coefs, ncsf, tol)
           real(rk), intent(in) :: coefs(:), tol
           integer, intent(in) :: ncsf
           integer:: i

           all0 = .true.
           do i = 1, ncsf
               !all0 = all0.AND.equals0(coefs(i))
               all0 = all0.AND.(abs(coefs(i)).lt.tol)
           enddo
       end function all0

       subroutine sortBasisCoefTable_removeDups(inbasislist, incoeftable, nbasis, ncsf)
           integer(i16b) :: inbasislist(:, :)
           integer(i16b), allocatable:: tpbasis(:, :)
           real(rk) :: incoeftable(:, :)
           real(rk), allocatable :: tpcoefs(:, :)
           integer, intent(in) ::  ncsf
           integer :: i, j, nordered, k, nbasis, w
           integer, allocatable :: order(:) !indices of dets, from small to large
            write(*, *) '========================== Enter sortBasisCoefTable_removeDups ================='
            write(*, *) 'Input size: ', nbasis, 'x', ncsf
           allocate(tpbasis(nbasis, 2))
           allocate(tpcoefs(nbasis, ncsf))
           allocate(order(nbasis))
           do i = 1, nbasis
               tpbasis(i, 1:2) = inbasislist(i, 1:2)
               tpcoefs(i, 1:ncsf) = incoeftable(i, 1:ncsf)
           enddo
           order(1) = 1 !
           nordered = 1 ! number of ordered dets
           do i = 2, nbasis
                 write(*, *) '.......... processing deter #i in tpbasislist, i =', i, '...........'
                 write(*, '(2B16)') tpbasis(i, 1:2)
                do j = 1, nordered
                     write(*, *) '...............At j=', j

                    if(tpbasis(i, 1).lt.tpbasis(order(j), 1)) then  !1<
                         write(*, *) 'basis(i, 1) < basis(order(j), 1)'
                        do k = nordered, j, -1
                            order(k + 1) = order(k)
                        enddo
                        order(j) = i
                        nordered = nordered + 1
                        write(*, *) 'exiting current j loop, going to next i'
                        exit! exit do loop for j

                    else if(tpbasis(i, 1).eq.tpbasis(order(j), 1)) then !1=
                         write(*, *) 'basis(i, 1) = basis(order(j), 1)'
                        if(tpbasis(i, 2).lt.tpbasis(order(j), 2)) then  !2<
                             write(*, *) '         basis(i, 2) <  basis(order(j), 2)'
                            do k = nordered, j, -1
                                order(k + 1) = order(k)
                            enddo
                            order(j) = i
                            nordered = nordered + 1
                            write(*, *) '         exiting current j loop, going to next i'
                            exit
                        elseif(tpbasis(i, 2).gt.tpbasis(order(j), 2)) then !2>
                             write(*, *) '         basis(i, 2) > basis(order(j), 2)'
                            !i has same updet as order(j) but bigger dndet
                            !should be placed behind order(j)
                            if(nordered.eq.j) then   !3
                                order(j+1) = i
                                nordered = nordered + 1
                                exit
                            else
                                !do k = nordered, j+1, -1
                                !    order(k + 1) = order(k)
                                !enddo
                                !order(j + 1) = i
                                !nordered = nordered + 1
                                !write(*, *) '         exiting current j loop, going to next i'
                                !exit
                                cycle
                            endif
                        else         !2=
                            !det_i det_order(j) have same detup and detdn
                            !Combine coefcients to the first occurance
                            write(*, *) 'basis(i, 2) = basis(order(j), 2)'
                            write(*, *) 'Found duplicate>>>>>>>>>>>>>'
                             write(*, *) 'This determinant is '
                             write(*, '(2B16)') tpbasis(i, 1:2)
                             write(*, '(2B16)') tpbasis(order(j), 1:2)
                             write(*, *) 'Current index is', i
                             write(*, *) 'Combine to index', order(j)

                            do k = 1, ncsf
                               tpcoefs(order(j), k) = tpcoefs(order(j), k) + tpcoefs(i, k)
                            enddo
                             write(*, *) '         exiting current j loop, going to next i'
                            exit
                            !nordered doesn't change
                        endif        !2

                    else!1 basis i, 1 > basis order(j)
                        write(*, *) 'basis(i, 1) > basis(order(j) , 1))'
                        if(nordered.eq.j) then
                            order(j+1) = i
                            nordered = nordered + 1
                            write(*, *) '         exiting current j loop, going to next i'
                            exit
                        else
                            cycle
                        endif
                    endif !1

                enddo !j
                write(*, *) '------- one loop done, i = ', i, '---------'
                do w = 1, nordered
                     write(*, *) order(w)
                enddo

            enddo !i

            ! write(*, *) '----------------------- Sorting csf table by dets----------------'

            do i = 1, nordered
                ! write(*, *) order(i)
            enddo

            k = 0
            do i = 1, nordered
                if(.not.all0(tpcoefs(order(i), 1:ncsf), ncsf, COEFTOL)) then
                    k = k+1
                    inbasislist(k, 1:2) = tpbasis(order(i), 1:2)
                    incoeftable(k, 1:ncsf) = tpcoefs(order(i), 1:ncsf)
                endif
            enddo
            nbasis = k
             write(*, *) 'Final number of dets = ', nbasis
            deallocate(tpbasis)
            deallocate(tpcoefs)
            deallocate(order)

            do i = 1, nbasis
                 write(*, '(2B16)') inbasislist(i, 1:2)
                 !write(*, *) incoeftable(i, 1:ncsf)
            end do
       endsubroutine sortBasisCoefTable_removeDups
end module checkcsfs


* ======================================================================
* NIST Guide to Available Math Software. Not for redistribution.
* Fullsource for module DZERO from package PORT.
* Retrieved from NETLIB on Wed Feb  4 07:35:36 1998.
* ======================================================================
      DOUBLE PRECISION FUNCTION DZERO(F,A,B,T)
C
C  FINDS THE REAL ROOT OF THE FUNCTION F LYING BETWEEN A AND B
C  TO WITHIN A TOLERANCE OF
C
C         6*D1MACH(3) * DABS(DZERO) + 2 * T
C
C  F(A) AND F(B) MUST HAVE OPPOSITE SIGNS
C
C  THIS IS BRENTS ALGORITHM
C
C  A, STORED IN SA, IS THE PREVIOUS BEST APPROXIMATION (I.E. THE OLD B)
C  B, STORED IN SB, IS THE CURRENT BEST APPROXIMATION
C  C IS THE MOST RECENTLY COMPUTED POINT SATISFYING F(B)*F(C) .LT. 0
C  D CONTAINS THE CORRECTION TO THE APPROXIMATION
C  E CONTAINS THE PREVIOUS VALUE OF D
C  M CONTAINS THE BISECTION QUANTITY (C-B)/2
C
      DOUBLE PRECISION F,A,B,T,TT,SA,SB,C,D,E,FA,FB,FC,TOL,M,P,Q,R,S
      EXTERNAL F
      DOUBLE PRECISION D1MACH
C
      TT = T
      IF (T .LE. 0.0D0) TT = 10.D0*D1MACH(1)
C
      SA = A
      SB = B
      FA = F(SA)
      FB = F(SB)
      IF (FA .NE. 0.0D0) GO TO 5
      DZERO = SA
      RETURN
 5    IF (FB .EQ. 0.0D0) GO TO 140
C/6S
C     IF (DSIGN(FA,FB) .EQ. FA) CALL SETERR(
C    1   47H DZERO - F(A) AND F(B) ARE NOT OF OPPOSITE SIGN, 47, 1, 1)
C/7S
!!!!!!!!      IF (DSIGN(FA,FB) .EQ. FA) CALL SETERR(
!!!!!!!!     1 ' DZERO - F(A) AND F(B) ARE NOT OF OPPOSITE SIGN', 47, 1, 1)
      if (DSIGN(FA,FB) .EQ. FA) then
         write(*,*)  'F(A) AND F(B) ARE NOT OF OPPOSITE SIGN'
         stop
      end if
C/
C
 10   C  = SA
      FC = FA
      E  = SB-SA
      D  = E
C
C  INTERCHANGE B AND C IF DABS F(C) .LT. DABS F(B)
C
 20   IF (DABS(FC).GE.DABS(FB)) GO TO 30
      SA = SB
      SB = C
      C  = SA
      FA = FB
      FB = FC
      FC = FA
C
 30   TOL = 2.0D0*D1MACH(4)*DABS(SB)+TT
      M = 0.5D0*(C-SB)
C
C  SUCCESS INDICATED BY M REDUCES TO UNDER TOLERANCE OR
C  BY F(B) = 0
C
      IF ((DABS(M).LE.TOL).OR.(FB.EQ.0.0D0)) GO TO 140
C
C  A BISECTION IS FORCED IF E, THE NEXT-TO-LAST CORRECTION
C  WAS LESS THAN THE TOLERANCE OR IF THE PREVIOUS B GAVE
C  A SMALLER F(B).  OTHERWISE GO TO 40.
C
      IF ((DABS(E).GE.TOL).AND.(DABS(FA).GE.DABS(FB))) GO TO 40
      E = M
      D = E
      GO TO 100
 40   S = FB/FA
C
C  QUADRATIC INTERPOLATION CAN ONLY BE DONE IF A (IN SA)
C  AND C ARE DIFFERENT POINTS.
C  OTHERWISE DO THE FOLLOWING LINEAR INTERPOLATION
C
      IF (SA.NE.C) GO TO 50
      P = 2.0D0*M*S
      Q = 1.0D0-S
      GO TO 60
C
C  INVERSE QUADRATIC INTERPOLATION
C
 50   Q = FA/FC
      R = FB/FC
      P = S*(2.0D0*M*Q*(Q-R)-(SB-SA)*(R-1.0D0))
      Q = (Q-1.0D0)*(R-1.0D0)*(S-1.0D0)
 60   IF (P.LE.0.0D0) GO TO 70
      Q = -Q
      GO TO 80
 70   P = -P
C
C  UPDATE THE QUANTITIES USING THE NEWLY COMPUTED
C  INTERPOLATE UNLESS IT WOULD EITHER FORCE THE
C  NEW POINT TOO FAR TO ONE SIDE OF THE INTERVAL
C  OR WOULD REPRESENT A CORRECTION GREATER THAN
C  HALF THE PREVIOUS CORRECTION.
C
C  IN THESE LAST TWO CASES - DO THE BISECTION
C  BELOW (FROM STATEMENT 90 TO 100)
C
 80   S = E
      E = D
      IF ((2.0D0*P.GE.3.0D0*M*Q-DABS(TOL*Q)).OR.
     1    (P.GE.DABS(0.5D0*S*Q))) GO TO 90
      D = P/Q
      GO TO 100
 90   E = M
      D = E
C
C  SET A TO THE PREVIOUS B
C
 100  SA = SB
      FA = FB
C
C  IF THE CORRECTION TO BE MADE IS SMALLER THAN
C  THE TOLERANCE, JUST TAKE A  DELTA STEP  (DELTA=TOLERANCE)
C         B = B + DELTA * SIGN(M)
C
      IF (DABS(D).LE.TOL) GO TO 110
      SB = SB+D
      GO TO 130
C
 110  IF (M.LE.0.0D0) GO TO 120
      SB = SB+TOL
      GO TO 130
C
 120  SB = SB-TOL
 130  FB = F(SB)
C
C  IF F(B) AND F(C) HAVE THE SAME SIGN ONLY
C  LINEAR INTERPOLATION (NOT INVERSE QUADRATIC)
C  CAN BE DONE
C
      IF ((FB.GT.0.0D0).AND.(FC.GT.0.0D0)) GO TO 10
      IF ((FB.LE.0.0D0).AND.(FC.LE.0.0D0)) GO TO 10
      GO TO 20
C
C***SUCCESS***
 140  DZERO = SB
      RETURN
      END


module Mod_plcd_Isotrop2DPlainStress
   use typre
   use Mod_plcd_ConstitutiveTensor
   use Mod_Listen
   implicit none
   private
   public Isotrop2dPlainStress

   type, extends(ConstitutiveTensor) :: Isotrop2dPlainStress

      real(rp) :: E
      real(rp) :: nu
      real(rp) :: invK,CNorm
      real(rp) :: C(3,3),CDev(3,3),CSph(3,3)

contains
      procedure :: ReadData => RDIsotrop
      procedure :: ScatterData => SPIsotrop
      procedure :: CheckData => CheckData
      procedure :: Build => BuildIsotrop
      procedure :: GetPointerToC => GetPointerToCIsotrop
      procedure :: GetPointerToCDeviatoric => GetPointerToCDeviatoricIsotrop
      procedure :: GetPointerToCSpherical => GetPointerToCSphericalIsotrop
      procedure :: GetYoungModulusEquiv => GetYoungModulusEquivIsotrop

      procedure :: GetInverseVolumetricDeformationModulus
      procedure :: GetCNorm
      procedure :: ComputeStressNorm
      procedure :: ComputeMeanStress
      procedure :: GetYoungModulusForDirection
      procedure :: GetPoissonRatio
      procedure :: GetYoungModulus
      procedure :: SetYoungModulusForDirection
      procedure :: SetPoissonRatio
      procedure :: SetYoungModulus
   end type

contains

   subroutine RDIsotrop(a,Listener)
      implicit none
      class(Isotrop2dPlainStress) :: a
      type(ListenFile) :: Listener

      call Listener%Listen('RDIsotrop')
      if (Listener%exists('YOUNG')) a%E = Listener%GetRea('YOUNG',1.0_rp,'Young Modulus')
      call Listener%Listen('RDIsotrop')
      if (Listener%exists('POISS')) a%nu = Listener%GetRea('POISS',0.3_rp,'Poisson Coefficient')
   end subroutine

   subroutine SPIsotrop(a)
      use MPI
      implicit none
      class(Isotrop2dPlainStress) :: a
      integer(ip) :: ierr
      call MPI_BCAST(a%E,1,MPI_REAL8,a%MPIroot,a%MPIcomm,ierr)
      call MPI_BCAST(a%nu,1,MPI_REAL8,a%MPIroot,a%MPIcomm,ierr)
   end subroutine

   subroutine CheckData(a)
      implicit none
      class(Isotrop2dPlainStress) :: a
      if(a%E <= 0.0_rp) call runend('Negatice value for the Young Modulus')
      if((a%nu <= -1.0_rp).OR.(a%nu > 0.5_rp)) call runend('Poisson Coefficient out of range (-1 < v <= 0.5)')

   end subroutine

   subroutine BuildIsotrop(a)
      implicit none
      class(Isotrop2dPlainStress) :: a

      real(rp) :: raux
      real(rp) :: Eigenvalues(3)

      !Deviatoric Part
      a%CDev(:,1) = a%E/(3.0_rp*(a%nu**2.0_rp - 1.0_rp))*(/ (a%nu - 2.0_rp), -(2.0_rp*a%nu - 1.0_rp),  0.0_rp /)
      a%CDev(:,2) = a%E/(3.0_rp*(a%nu**2.0_rp - 1.0_rp))*(/ -(2.0_rp*a%nu - 1.0_rp),  (a%nu - 2.0_rp), 0.0_rp /)
      a%CDev(:,3) = (/           0.0_rp,                              0.0_rp, a%E/(2.0_rp*(a%nu + 1.0_rp))/)

      if (.not. a%OnlyDeviatoricPart) then
         raux = a%nu/(1-2*a%nu)
         a%C(:,1) = a%E/(1-a%nu*a%nu)*(/ 1.0_rp,a%nu,0.0_rp /)
         a%C(:,2) = a%E/(1-a%nu*a%nu)*(/ a%nu,1.0_rp,0.0_rp /)
         a%C(:,3) = a%E/(1-a%nu*a%nu)*(/ 0.0_rp,0.0_rp,(1-a%nu)*0.5_rp /)

      elseif (a%OnlyDeviatoricPart) then
         a%C = a%CDev
      endif

      !We take into account that the divergence does not include the z component
      !and we correct it here: a%invK = (3*(1-2*a%nu))/a%E/(1-2*a%nu) = 3/a%E

      a%invK = 3/a%E

      a%CSph = a%C-a%CDev

      call MatrixEigenValues(3,a%C,Eigenvalues)

      a%CNorm = maxval(abs(Eigenvalues))

   end subroutine

   subroutine GetPointerToCIsotrop(a,C)
      implicit none
      class(Isotrop2dPlainStress), target :: a
      real(rp), pointer :: C(:,:)

      C => a%C
   end subroutine

   subroutine GetPointerToCDeviatoricIsotrop(a,C)
      implicit none
      class(Isotrop2dPlainStress), target :: a
      real(rp), pointer :: C(:,:)

      C => a%CDev
   end subroutine

   subroutine GetPointerToCSphericalIsotrop(a,C)
      implicit none
      class(Isotrop2dPlainStress), target :: a
      real(rp), pointer :: C(:,:)

      C => a%CSph
   end subroutine

   subroutine GetInverseVolumetricDeformationModulus(a,invK)
      implicit none
      class(Isotrop2dPlainStress) :: a
      real(rp) :: invK

      invk = a%invK

   end subroutine

   subroutine GetCNorm(a,CNorm)
      implicit none
      class(Isotrop2dPlainStress) :: a
      real(rp) :: CNorm

      CNorm = a%CNorm

   end subroutine

   subroutine ComputeStressNorm(a,Stress,StressNorm)
      implicit none
      class(Isotrop2dPlainStress), target :: a
      real(rp) :: Stress(:),StressNorm

      real(rp) :: dot1,dot2

      dot1 = dot_product(Stress(1:2),Stress(1:2))
      dot2 = Stress(3)*Stress(3)

      StressNorm = sqrt(dot1+2.0_rp*dot2)

   end subroutine

   subroutine ComputeMeanStress(a,Stress,MeanStress)
      implicit none
      class(Isotrop2dPlainStress), target :: a
      real(rp) :: Stress(:),MeanStress

      meanStress = sum(Stress(1:2))/3.0
   end subroutine

   subroutine GetYoungModulusForDirection(a,Direction,E)
      implicit none
      class(Isotrop2dPlainStress) :: a
      real(rp) :: Direction(:)
      real(rp) :: E

      E = a%E
   end subroutine

   subroutine GetPoissonRatio(a,nu)
      implicit none
      class(Isotrop2dPlainStress) :: a
      real(rp) :: nu

      nu = a%nu
   end subroutine

   subroutine GetYoungModulusEquivIsotrop(a,E)
      implicit none
      class(Isotrop2dPlainStress) :: a
      real(rp) :: E

      E = a%E
   end subroutine

   subroutine GetYoungModulus(a,E)
      implicit none
      class(Isotrop2dPlainStress) :: a
      real(rp) :: E

      E = a%E
   end subroutine

   subroutine SetYoungModulusForDirection(a,Direction,E)
      implicit none
      class(Isotrop2dPlainStress) :: a
      real(rp) :: Direction(:)
      real(rp) :: E

      a%E = E
   end subroutine

   subroutine SetPoissonRatio(a,nu)
      implicit none
      class(Isotrop2dPlainStress) :: a
      real(rp) :: nu

      a%nu = nu
   end subroutine

   subroutine SetYoungModulus(a,E)
      implicit none
      class(Isotrop2dPlainStress) :: a
      real(rp) :: E

      a%E = E
   end subroutine

end module


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

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

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

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

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

! End MIT license text.

      SUBROUTINE BUSH ( INT_ELEM_ID, OPT, WRITE_WARN )

! Calculates, for BUSH element

!  1) KE        = element stiffness matrix if OPT(4) = 'Y'
!  2) SE1, STE1 = element stress data recovery matrices if OPT(3) = 'Y'

      USE PENTIUM_II_KIND, ONLY       :  BYTE, LONG, DOUBLE
      USE IOUNT1, ONLY                :  F04, F06, WRT_LOG
      USE SCONTR, ONLY                :  BLNK_SUB_NAM
      USE TIMDAT, ONLY                :  TSEC
      USE CONSTANTS_1, ONLY           :  ZERO
      USE MODEL_STUF, ONLY            :  BE1, BE2, BUSH_DXA, BUSH_DXB, BUSH_DY, BUSH_DZ, EPROP, KE, OFFDIS_GA_GB, SE1, SE2
      USE SUBR_BEGEND_LEVELS, ONLY    :  BUSH_BEGEND

      USE BUSH_USE_IFs

      IMPLICIT NONE

      CHARACTER(LEN=LEN(BLNK_SUB_NAM)):: SUBR_NAME = 'BUSH'
      CHARACTER(LEN=*) , INTENT(IN)   :: WRITE_WARN        ! If 'Y" write warning messages, otherwise do not
      CHARACTER(1*BYTE), INTENT(IN)   :: OPT(6)            ! 'Y'/'N' flags for whether to calc certain elem matrices

      INTEGER(LONG), PARAMETER        :: SUBR_BEGEND = BUSH_BEGEND
      INTEGER(LONG), INTENT(IN)       :: INT_ELEM_ID       ! Internal element ID
      INTEGER(LONG)                   :: I,J               ! DO loop indices

      REAL(DOUBLE)                    :: DUM0(6,12)        ! Intermediate matrix in a calc
      REAL(DOUBLE)                    :: DUM1(6,12)        ! Intermediate matrix in a calc
      REAL(DOUBLE)                    :: STIFF(12,12)      ! 12x12 partition from KE (complete stiff matrix for this elem)
      REAL(DOUBLE)                    :: STRE_RCV(2)       ! Two stress recovery values
      REAL(DOUBLE)                    :: STRN_RCV(2)       ! Two strain recovery values

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

! **********************************************************************************************************************************
! Initialize:

      BUSH_DXA = OFFDIS_GA_GB(1,1)
      BUSH_DY  = OFFDIS_GA_GB(1,2)
      BUSH_DZ  = OFFDIS_GA_GB(1,3)
      BUSH_DXB = OFFDIS_GA_GB(2,1)

! Calc stiffness matrix

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

         DO I=1,12
            DO J=1,12
               KE(I,J) = ZERO
            ENDDO
         ENDDO

         KE( 1, 1) =  EPROP(1)                             ! NB *** new 09/10/21. Big change. KE (before offsets): just EPROP vals
         KE( 2, 2) =  EPROP(2)
         KE( 3, 3) =  EPROP(3)
         KE( 4, 4) =  EPROP(4)
         KE( 5, 5) =  EPROP(5)
         KE( 6, 6) =  EPROP(6)

         KE( 7, 7) =  KE( 1, 1)
         KE( 8, 8) =  KE( 2, 2)
         KE( 9, 9) =  KE( 3, 3)
         KE(10,10) =  KE( 4, 4)
         KE(11,11) =  KE( 5, 5)
         KE(12,12) =  KE( 6, 6)

         KE( 1, 7) = -KE( 1, 1)
         KE( 2, 8) = -KE( 2, 2)
         KE( 3, 9) = -KE( 3, 3)
         KE( 4,10) = -KE( 4, 4)
         KE( 5,11) = -KE( 5, 5)
         KE( 6,12) = -KE( 6, 6)

         DO I=1,12
            DO J=I,12
               KE(J,I) = KE(I,J)
            ENDDO
         ENDDO

      ENDIF

! Calc stress and strain recovery matrices. The stress recovery coefficients on the Bulk Data entry are intended to be
! multiplied times the element ENGINEERING (NOT NODE) forces to get stresses. In order to calc stresses in the MYSTRAN fasion
! of multiplying SEi times displacements, the SEi matrices are generated with the aid of the matrix that converts element
! nodal forces to element engineering forces. The DUM0 matrix (below), if all of the STRE_RCV numbers were 1.0 is that matrix
! (see conversion code in subr OFP3_ELFE_1D. With the STRE_RCV values in DUM0, then DUM0 times KE would be the SE matrices
! that convert displacements to stresses

      IF (OPT(3) == 'Y') THEN
                                                           ! Calc SEi stress recovery matrices
         STRE_RCV(1) = EPROP(19)                           ! --- Stress/strain recovery coefficients from PBUSH entry
         STRE_RCV(2) = EPROP(20)
         STRN_RCV(1) = EPROP(21)
         STRN_RCV(2) = EPROP(22)

         DO I=1,6
            DO J=1,12
               DUM0(I,J) = ZERO
               DUM1(I,J) = ZERO
            ENDDO
         ENDDO

         DUM0(1, 7) =  STRE_RCV(1)
         DUM0(2, 8) =  STRE_RCV(1)
         DUM0(3, 9) =  STRE_RCV(1)

         DUM0(4,10) =  STRE_RCV(2)
         DUM0(5,11) =  STRE_RCV(2)
         DUM0(6,12) =  STRE_RCV(2)

         DUM0(4, 8) =  STRE_RCV(2)*BUSH_DZ
         DUM0(4, 9) = -STRE_RCV(2)*BUSH_DY
         DUM0(5, 7) = -STRE_RCV(2)*BUSH_DZ
         DUM0(5, 9) = -STRE_RCV(2)*BUSH_DXB
         DUM0(6, 7) =  STRE_RCV(2)*BUSH_DY
         DUM0(6, 8) =  STRE_RCV(2)*BUSH_DXB

         DO I=1,12
            DO J=1,12
               STIFF(I,J) = KE(I,J)
            ENDDO
         ENDDO

         CALL MATMULT_FFF ( DUM0, STIFF, 6, 12, 12, DUM1 ) ! --- Dummy 6x12 matrix that has the SEi matrix data

         DO    I=1,3                                                                                !    ---    SEi
            DO    J=1,12
               SE1(I,J,1) = DUM1(I  ,J)
               SE2(I,J,1) = DUM1(I+3,J)
            ENDDO
         ENDDO

         BE1(1, 1,1) = -STRN_RCV(1)                        ! Calc BEi strain recovery matrices
         BE1(1, 7,1) =  STRN_RCV(1)

         BE1(2, 2,1) = -STRN_RCV(1)
         BE1(2, 8,1) =  STRN_RCV(1)

         BE1(3, 3,1) = -STRN_RCV(1)
         BE1(3, 9,1) =  STRN_RCV(1)

         BE2(1, 4,1) = -STRN_RCV(2)
         BE2(1,10,1) =  STRN_RCV(2)

         BE2(2, 5,1) = -STRN_RCV(2)
         BE2(2,11,1) =  STRN_RCV(2)

         BE2(3, 6,1) = -STRN_RCV(2)
         BE2(3,12,1) =  STRN_RCV(2)

      ENDIF

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

      RETURN

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

      END SUBROUTINE BUSH


      program type12
      real a, b

      read *, a, b

      if (a+b.eq.0.0) then
         print *, 'a+b = 0'
         print *, a+b
      endif

      end


C     *****************************************************************
C     *****************************************************************

      subroutine sevalal(n,x,m,lambda,rho,equatn,linear,al,inform)

      implicit none

C     SCALAR ARGUMENTS
      double precision al
      integer inform,m,n

C     ARRAY ARGUMENTS
      logical equatn(m),linear(m)
      double precision lambda(m),rho(m),x(n)

      include "dim.par"
      include "graddat.inc"
      include "algparam.inc"

C     LOCAL SCALARS
      integer j
      double precision f,p

      if ( innercall ) then
          call minsqf(n,x,al,inform)
          return
      end if

      if ( fccoded ) then

C         COMPUTE OBJECTIVE FUNTION AND CONSTRAINTS
          call sevalfc(n,x,f,m,c,inform)
          if ( inform .lt. 0 ) return

C         COMPUTES AL = f + sum_j P(c_j, rho_j, lambda_j)
          al = f

          do j = 1,m
C             ADD P(c_j, rho_j, lambda_j)
              call evalp(c(j),rho(j),lambda(j),equatn(j),p)
              al = al + p
          end do

      else if ( fcoded .and. ( ccoded .or. m .eq. 0 ) ) then

C         COMPUTE OBJECTIVE FUNCTION
          call sevalf(n,x,f,inform)
          if ( inform .lt. 0 ) return

C         COMPUTES AL = f + sum_j P(c_j, rho_j, lambda_j)
          al = f

          do j = 1,m
C             COMPUTE j-TH CONSTRAINT
              call sevalc(n,x,j,c(j),inform)
              if ( inform .lt. 0 ) return

C             ADD P(c_j, rho_j, lambda_j)
              call evalp(c(j),rho(j),lambda(j),equatn(j),p)
              al = al + p
          end do

      end if

      gotc = .true.

      end

C     *****************************************************************
C     *****************************************************************

      subroutine sevalnl(n,x,m,lambda,equatn,linear,nl,inform)

      implicit none

C     SCALAR ARGUMENTS
      integer inform,m,n

C     ARRAY ARGUMENTS
      logical equatn(m),linear(m)
      double precision lambda(m),nl(n),x(n)

      include "dim.par"
      include "algparam.inc"
      include "graddat.inc"

C     LOCAL SCALARS
      logical alllin,allnol
      integer i,ind,j,jcnnz
      double precision dum

C     LOCAL ARRAYS
      integer jcfun(jcnnzmax)
      double precision p(mmax)

      if ( fccoded ) then

          if ( gjacpcoded ) then

C             COMPUTE p
              constrc = .false.
              do j = 1,m
                  if ( equatn(j) .or. lambda(j) .gt. 0.0d0 ) then
                      p(j) = lambda(j)
                      constrc = .true.
                  else
                      p(j) = 0.0d0
                  end if
              end do

C             COMPUTE nl = Jacobian^T p
              call sevalgjacp(n,x,g,m,p,nl,'T',gotj,inform)
              if ( inform .lt. 0 ) return

C             COMPUTE gparc = Jacobian^t p IGNORING LINEAR CONSTRAINTS
              allnol = .true.
              alllin = .true.
              do j = 1,m
                  if ( linear(j) ) then
                      p(j) = 0.0d0
                      allnol = .false.
                  else
                      alllin = .false.
                  end if
              end do

              if ( allnol ) then
                  do i = 1,n
                      gparc(i) = nl(i)
                  end do
              else if ( alllin ) then
                  do i = 1,n
                      gparc(i) = 0.0d0
                  end do
              else
                  call sevalgjacp(n,x,dum,m,p,gparc,'t',gotj,inform)
                  if ( inform .lt. 0 ) return
              end if

C             ADD THE GRADIENT OF THE OBJECTIVE FUNCTION
              do i = 1,n
                  nl(i)    = nl(i)    + g(i)
                  gparc(i) = gparc(i) + g(i)
              end do

          else ! if ( gjaccoded ) then
C         In fact, this else is the choice for gjaccoded or 'not
C         gjacpcoded and not gjaccoded', in which case the calling
C         sequence starting with sevalgjac below will end up using
C         finite differences to approximate first derivatives of the
C         objective function and the constraints

C             COMPUTE THE GRADIENT OF THE OBJECTIVE FUNCTION AND
C             JACOBIAN OF CONSTRAINTS
              call sevalgjac(n,x,g,m,jcfun,jcvar,jcval,jcnnz,inform)
              if ( inform .lt. 0 ) return

C             CONVERT JACOBIAN OF CONSTRAINTS FROM COORDINATE FORMAT TO
C             COMPRESSED SPARSE ROW FORMAT
              call coo2csr(m,jcnnz,jcfun,jcvar,jcval,jclen,jcsta)

C             COMPUTE \nabla L = \nabla f + \sum_j lambda_j * \nabla c_j
              constrc = .false.

              do i = 1,n
                  nl(i)    = g(i)
                  gparc(i) = g(i)
              end do

              do j = 1,m
                  if ( equatn(j) .or. lambda(j) .gt. 0.0d0 ) then
                      do i = jcsta(j),jcsta(j) + jclen(j) - 1
                          nl(jcvar(i)) =
     +                    nl(jcvar(i)) + lambda(j) * jcval(i)
                      end do

                      if ( .not. linear(j) ) then
                          do i = jcsta(j),jcsta(j) + jclen(j) - 1
                              gparc(jcvar(i)) =
     +                        gparc(jcvar(i)) + lambda(j) * jcval(i)
                          end do
                      end if

                      constrc = .true.
                  end if
              end do

          end if

      else if ( fcoded .and. ( ccoded .or. m .eq. 0 ) ) then

C         COMPUTE THE GRADIENT OF THE OBJECTIVE FUNCTION
          call sevalg(n,x,g,inform)
          if ( inform .lt. 0 ) return

          do i = 1,n
              nl(i)    = g(i)
              gparc(i) = g(i)
          end do

C         ADD nl = Jacobian^T lambda
          constrc = .false.

          ind = 0

          do j = 1,m
              if ( equatn(j) .or. lambda(j) .gt. 0.0d0 ) then

                  jcsta(j) = ind + 1

C                 COMPUTE THE GRADIENT OF THE j-TH CONSTRAINT
                  call sevaljac(n,x,j,jcvar(ind+1),jcval(ind+1),
     +            jclen(j),inform)
                  if ( inform .lt. 0 ) return

                  ind = ind + jclen(j)

C                 ADD lambda_j * \nabla c_j
                  do i = jcsta(j),jcsta(j) + jclen(j) - 1
                      nl(jcvar(i)) =
     +                nl(jcvar(i)) + lambda(j) * jcval(i)
                  end do

                  if ( .not. linear(j) ) then
                      do i = jcsta(j),jcsta(j) + jclen(j) - 1
                          gparc(jcvar(i)) =
     +                    gparc(jcvar(i)) + lambda(j) * jcval(i)
                      end do
                  end if

                  constrc = .true.

              end if
          end do

      end if

      gotj = .true.

      end

C     *****************************************************************
C     *****************************************************************

      subroutine sevalnal(n,x,m,lambda,rho,equatn,linear,nal,inform)

      implicit none

C     SCALAR ARGUMENTS
      integer inform,m,n

C     ARRAY ARGUMENTS
      logical equatn(m),linear(m)
      double precision lambda(m),nal(n),rho(m),x(n)

      include "dim.par"
      include "graddat.inc"
      include "algparam.inc"

C     LOCAL SCALARS
      integer j
      double precision dum

      if ( innercall ) then
          call minsqg(n,x,nal,inform)
          return
      end if

      if ( fccoded ) then
C         COMPUTE CONSTRAINTS
          if ( .not. gotc .and. m .gt. 0 ) then
              call sevalfc(n,x,dum,m,c,inform)
              if ( inform .lt. 0 ) return
          end if

      else if ( fcoded .and. ( ccoded .or. m .eq. 0 ) ) then
C         COMPUTE CONSTRAINTS ONE BY ONE
          if ( .not. gotc ) then
              do j = 1,m
                  call sevalc(n,x,j,c(j),inform)
                  if ( inform .lt. 0 ) return
              end do
          end if
      end if

      gotc = .true.

C     COMPUTE dP/dc
      do j = 1,m
          call evaldpdy(c(j),rho(j),lambda(j),equatn(j),dpdc(j))
      end do

C     COMPUTE GRADIENT OF THE LAGRANGIAN WITH DPDC INSTEAD OF LAMBDA
      call sevalnl(n,x,m,dpdc,equatn,linear,nal,inform)
      if ( inform .lt. 0 ) return

      end

C     *****************************************************************
C     *****************************************************************

      subroutine evalp(y,rho,lambda,equatn,p)

      implicit none

C     SCALAR ARGUMENTS
      logical equatn
      double precision lambda,p,rho,y

      if ( equatn ) then
          p  = y * ( lambda + 0.5d0 * rho * y )
      else
          if ( lambda + rho * y .ge. 0.0d0 ) then
              p = y * ( lambda + 0.5d0 * rho * y )
          else
              p = - 0.5d0 * lambda ** 2 / rho
          end if
      end if

      end

C     *****************************************************************
C     *****************************************************************

      subroutine evaldpdy(y,rho,lambda,equatn,dpdy)

      implicit none

C     SCALAR ARGUMENTS
      logical equatn
      double precision y,rho,lambda,dpdy

      if ( equatn ) then
          dpdy = lambda + rho * y
      else
          dpdy = max( 0.0d0, lambda + rho * y )
      end if

      end

C     *****************************************************************
C     *****************************************************************

      subroutine ievalnal(n,xp,m,lambda,rho,equatn,linear,iglin,nalp,
     +inform)

      implicit none

C     SCALAR ARGUMENTS
      logical iglin
      integer inform,m,n

C     ARRAY ARGUMENTS
      logical equatn(m),linear(m)
      double precision lambda(m),nalp(n),rho(m),xp(n)

C     This subroutine computes the gradient of the Augmented Lagrangian
C     function at a point xp, which is near to x, taking care of the
C     non-differentiability (i.e. considering the contribution of the
C     same constraints that contributed to compute the gradient of the
C     augmented Lagrangian at x). The augmented Lagrangian gradient must
C     have been previously computed at x. If iglin = true, linear
C     constraints are ignored.

      include "dim.par"
      include "algparam.inc"
      include "graddat.inc"

C     LOCAL SCALARS
      logical alllin,allnol
      integer i,j,jcpnnz
      double precision cpj,dpdcpj,dum

C     LOCAL ARRAYS
      integer jcpfun(jcnnzmax),jcplen(mmax),jcpsta(mmax),
     +        jcpvar(jcnnzmax)
      double precision cp(mmax),gp(nmax),jcpval(jcnnzmax),dpdcp(mmax)

      if ( innercall ) then
          call minsqg(n,xp,nalp,inform)
          return
      end if

      if ( fccoded ) then

C         COMPUTE nalp = gp + Jacobian^T dpdcp IGNORING THE LINEAR
C         CONSTRAINTS IF iglin IS TRUE

          if ( gjacpcoded ) then

C             COMPUTE CONSTRAINTS AT xp
              if ( m .gt. 0 ) then
                  call sevalfc(n,xp,dum,m,cp,inform)
                  if ( inform .lt. 0 ) return
              end if

C             COMPUTE dpdcp
              do j = 1,m
                  if ( ( equatn(j) .or. dpdc(j) .gt. 0.0d0 ) .and.
     +                 .not. ( iglin .and. linear(j) ) ) then
                      dpdcp(j) = lambda(j) + rho(j) * cp(j)
                  else
                      dpdcp(j) = 0.0d0
                  end if
              end do

C             COMPUTE nalp = Jacobian^T dpdcp
              call sevalgjacp(n,xp,gp,m,dpdcp,nalp,'T',gotj,inform)
              if ( inform .lt. 0 ) return

C             ADD THE GRADIENT OF THE OBJECTIVE FUNCTION
              do i = 1,n
                  nalp(i) = nalp(i) + gp(i)
              end do

          else ! if ( gjaccoded ) then
C         In fact, this else is the choice for gjaccoded or 'not
C         gjacpcoded and not gjaccoded', in which case the calling
C         sequence starting with sevalgjac below will end up using
C         finite differences to approximate first derivatives of the
C         objective function and the constraints

C             COMPUTE CONSTRAINTS AT xp
              if ( m .gt. 0 ) then
                  call sevalfc(n,xp,dum,m,cp,inform)
                  if ( inform .lt. 0 ) return
              end if

C             COMPUTE THE GRADIENT OF THE OBJECTIVE FUNCTION AND JACOBIAN
C             OF CONSTRAINTS AT xp
              call sevalgjac(n,xp,gp,m,jcpfun,jcpvar,jcpval,jcpnnz,
     +        inform)
              if ( inform .lt. 0 ) return

              do i = 1,n
                  nalp(i) = gp(i)
              end do

C             CONVERT JACOBIAN OF CONSTRAINTS FROM COODINATE FORMAT TO
C             COMPRESSED SPARSE ROW FORMAT
              call coo2csr(m,jcpnnz,jcpfun,jcpvar,jcpval,jcplen,jcpsta)

C             COMPUTE dpdcp AND ADD dPdc * dcdx
              do j = 1,m
                  if ( ( equatn(j) .or. dpdc(j) .gt. 0.0d0 ) .and.
     +                 .not. ( iglin .and. linear(j) ) ) then
                      dpdcpj = lambda(j) + rho(j) * cp(j)

                      do i = jcpsta(j),jcpsta(j) + jcplen(j) - 1
                          nalp(jcpvar(i)) =
     +                    nalp(jcpvar(i)) + dpdcpj * jcpval(i)
                      end do
                  end if
              end do
          end if

      else if ( fcoded .and. ( ccoded .or. m .eq. 0 ) ) then

C         COMPUTE GRADIENT OF THE OBJECTIVE FUNCTION
          call sevalg(n,xp,gp,inform)
          if ( inform .lt. 0 ) return

          do i = 1,n
              nalp(i) = gp(i)
          end do

C         ADD Jacobian^T dpdcp IGNORING THE LINEAR CONSTRAINTS
C         IF iglin IS TRUE
          do j = 1,m
              if ( ( equatn(j) .or. dpdc(j) .gt. 0.0d0 ) .and.
     +             .not. ( iglin .and. linear(j) ) ) then

C                 COMPUTE j-TH CONSTRAINT
                  call sevalc(n,xp,j,cpj,inform)
                  if ( inform .lt. 0 ) return

                  dpdcpj = lambda(j) + rho(j) * cpj

C                 COMPUTE THE GRADIENT OF THE j-TH CONSTRAINT
                  call sevaljac(n,xp,j,jcpvar,jcpval,jcpnnz,inform)
                  if ( inform .lt. 0 ) return

C                 ADD dPdc * dcdx
                  do i = 1,jcpnnz
                      nalp(jcpvar(i)) =
     +                nalp(jcpvar(i)) + dpdcpj * jcpval(i)
                  end do
              end if
          end do

      end if

      end

C     ******************************************************************
C     ******************************************************************

      subroutine sevalhal(n,x,m,lambda,rho,equatn,linear,hallin,halcol,
     +halval,halnnz,inform)

      implicit none

C     SCALAR ARGUMENTS
      integer halnnz,inform,m,n

C     ARRAY ARGUMENTS
      logical equatn(m),linear(m)
      integer hallin(*),halcol(*)
      double precision lambda(m),halval(*),rho(m),x(n)

C     This subroutine computes the Hessian of the augmented Lagrangian.

      include "dim.par"
      include "graddat.inc"

C     LOCAL SCALARS
      integer i,lin,j,k,l,var

C     LOCAL ARRAYS
      integer stlin(nmax)
      double precision r(nmax)

      call sevalhl(n,x,m,dpdc,hallin,halcol,halval,halnnz,inform)
      if ( inform .lt. 0 ) return

      if ( m .eq. 0 ) return

C     PUT MATRIX INTO A ROW-LINKED LIST

      do i = 1,n
         r(i) = 0.0d0
      end do

      do i = 1,n
          stlin(i) = 0
      end do

      do i = 1,halnnz
          lin = hallin(i)
          k   = stlin(lin)
          stlin(lin) = i
          hallin(i)  = k
      end do

C     ADD \sum_j \rho_j * \nabla c_j \nabla c_j^t

      do j = 1,m

          if ( equatn(j) .or. dpdc(j) .gt. 0.0d0 ) then

C             ADD \rho_j * \nabla c_j \nabla c_j^t

              do k = jcsta(j),jcsta(j) + jclen(j) - 1

                  var = jcvar(k)

C                 PUT ROW jcvar(k) INTO A DENSE VECTOR

                  lin = stlin(var)
 10               if ( lin .ne. 0 ) then
                      r(halcol(lin)) = r(halcol(lin)) + halval(lin)
                      lin = hallin(lin)
                      go to 10
                  end if

C                 ADD VALUE

                  do l = jcsta(j),jcsta(j) + jclen(j) - 1
                      if ( jcvar(l) .le. var ) then
                          r(jcvar(l)) =
     +                    r(jcvar(l)) + rho(j) * jcval(l) * jcval(k)
                      end if
                  end do

C                 UPDATE VALUES IN HALVAL

                  lin = stlin(var)
 20               if ( lin .ne. 0 ) then
                      halval(lin) = r(halcol(lin))
                      r(halcol(lin)) = 0.0d0
                      lin = hallin(lin)
                      go to 20
                  end if

C                 INSERT NEW ELEMENTS IN HAL REPRESENTATION

                  do i = jcsta(j),jcsta(j) + jclen(j) - 1
                      l = jcvar(i)
                      if ( r(l) .ne. 0.0d0 ) then
                          halnnz = halnnz + 1
                          halval(halnnz) = r(l)
                          halcol(halnnz) = l
                          hallin(halnnz) = stlin(var)
                          stlin(var) = halnnz
                          r(l) = 0.0d0
                      end if
                  end do

              end do

          end if

      end do

C     PUT MATRIX BACK INTO COORDINATE SQUEME

      do i = 1,n

          lin = stlin(i)
 30       if ( lin .ne. 0 ) then
              k = hallin(lin)
              hallin(lin) = i
              lin = k
              go to 30
          end if

      end do

      end

C     *****************************************************************
C     *****************************************************************

      subroutine sevalhalp(n,x,m,lambda,rho,equatn,linear,p,hp,gothl,
     +inform)

      implicit none

C     SCALAR ARGUMENTS
      logical gothl
      integer inform,m,n

C     ARRAY ARGUMENTS
      logical equatn(m),linear(m)
      double precision hp(n),lambda(m),p(n),rho(m),x(n)

      include "dim.par"
      include "graddat.inc"
      include "algparam.inc"

C     LOCAL SCALARS
      integer i,j
      double precision ajtp,dum

C     LOCAL ARRAYS
      double precision ap(mmax),atap(nmax)

      if ( innercall ) then
          call minsqhp(n,x,p,hp,gothl,inform)
          return
      end if

C     --------------------------------------------------------------
C     Hessian approximation
C     --------------------------------------------------------------

      if ( hptype .eq. 'HAPPRO' .and. constrc ) then

          call applyhapp(n,m,rho,equatn,gothl,p,hp)

C     --------------------------------------------------------------
C     Incremental quotients
C     --------------------------------------------------------------

      else if ( hptype .eq. 'INCQUO' .or. hptype .eq. 'HAPPRO' ) then

          call ievalhalp(n,x,m,lambda,rho,equatn,linear,p,hp,inform)
          if ( inform .lt. 0 ) return

C     --------------------------------------------------------------
C     True Hessian
C     --------------------------------------------------------------

      else if ( hptype .eq. 'TRUEHP' ) then

C         Compute Hessian of Lagrangian times p using dpdc
C         instead of lambda

          call sevalhlp(n,x,m,dpdc,p,hp,gothl,inform)
          if ( inform .lt. 0 ) return

C         Add rho A^T A

          if ( gjacpcoded ) then
              call sevalgjacp(n,x,dum,m,ap,p,'j',gotj,inform)
              if ( inform .lt. 0 ) return

              do j = 1,m
                  ap(j) = ap(j) * rho(j)
              end do

              call sevalgjacp(n,x,dum,m,ap,atap,'t',gotj,inform)
              if ( inform .lt. 0 ) return

              do i = 1,n
                  hp(i) = hp(i) + atap(i)
              end do

          else
              do j = 1,m
                  if ( equatn(j) .or. dpdc(j) .gt. 0.0d0 ) then

                      ajtp = 0.0d0
                      do i = jcsta(j),jcsta(j) + jclen(j) - 1
                          ajtp = ajtp + jcval(i) * p(jcvar(i))
                      end do

                      ajtp = ajtp * rho(j)

                      do i = jcsta(j),jcsta(j) + jclen(j) - 1
                          hp(jcvar(i)) = hp(jcvar(i)) + ajtp * jcval(i)
                      end do

                  end if
              end do
          end if

      end if

      end

C     *****************************************************************
C     *****************************************************************

      subroutine ievalhalp(n,x,m,lambda,rho,equatn,linear,p,hp,inform)

      implicit none

C     SCALAR ARGUMENTS
      integer inform,m,n

C     ARRAY ARGUMENTS
      logical equatn(m),linear(m)
      double precision hp(n),lambda(m),p(n),rho(m),x(n)

C     Computes an approximation of the product of the Hessian of the
C     Augmented Lagrangian times a vector using incremental quotients.

      include "machconst.inc"
      include "dim.par"
      include "graddat.inc"
      include "algparam.inc"

C     LOCAL SCALARS
      logical iglin
      integer i,j
      double precision ajtp,dum,psupn,step,xsupn

C     LOCAL  ARRAYS
      double precision ap(mmax),atap(nmax),gpparc(nmax),ptmp(nmax),
     +        xp(nmax)

C     ------------------------------------------------------------------
C     Set auxiliary point
C     ------------------------------------------------------------------

      xsupn = 0.0d0
      psupn = 0.0d0
      do i = 1,n
          xsupn = max( xsupn, abs( x(i) ) )
          psupn = max( psupn, abs( p(i) ) )
      end do

      step = macheps12 * max( xsupn / psupn, 1.0d0 )

      do i = 1,n
          xp(i) = x(i) + step * p(i)
      end do

C     ------------------------------------------------------------------
C     Compute gradient of the augmented Lagrangian at xp considering the
C     same constraints considered at x and ignoring linear constraints
C     ------------------------------------------------------------------

      call ssetp(n,xp)

      iglin = .true.
      call ievalnal(n,xp,m,lambda,rho,equatn,linear,iglin,gpparc,inform)
      if ( inform .lt. 0 ) return

C     ------------------------------------------------------------------
C     Compute gradients difference
C     ------------------------------------------------------------------

      do i = 1,n
          hp(i) = ( gpparc(i) - gparc(i) ) / step
      end do

C     ------------------------------------------------------------------
C     Add contribution of linear constraints
C     ------------------------------------------------------------------

      if ( gjacpcoded ) then
          do i = 1,n
              if ( linear(i) ) then
                  ptmp(i) = p(i)
              else
                  ptmp(i) = 0.0d0
              end if
          end do

          call sevalgjacp(n,x,dum,m,ap,ptmp,'j',gotj,inform)
          if ( inform .lt. 0 ) return

          do j = 1,m
              ap(j) = ap(j) * rho(j)
          end do

          call sevalgjacp(n,x,dum,m,ap,atap,'t',gotj,inform)
          if ( inform .lt. 0 ) return

          do i = 1,n
              hp(i) = hp(i) + atap(i)
          end do

      else
          do j = 1,m
              if ( equatn(j) .or. dpdc(j) .gt. 0.0d0 ) then
                  if ( linear(j) ) then

C                     Compute inner product <a,p>
                      ajtp = 0.0d0
                      do i = jcsta(j),jcsta(j) + jclen(j) - 1
                          ajtp = ajtp + jcval(i) * p(jcvar(i))
                      end do

                      ajtp = ajtp * rho(j)

C                     Add rho * ajtp * a
                      do i = jcsta(j),jcsta(j) + jclen(j) - 1
                          hp(jcvar(i)) = hp(jcvar(i)) + ajtp * jcval(i)
                      end do

                  end if
              end if
          end do
      end if

      end

C     ******************************************************************
C     ******************************************************************

      subroutine coo2csr(m,nnz,alin,acol,aval,alen,asta)

      implicit none

C     SCALAR ARGUMENTS
      integer m,nnz

C     ARRAY ARGUMENTS
      integer acol(nnz),alen(m),alin(nnz),asta(m)
      double precision aval(nnz)

C     This subroutines converts a matrix from coordinate format to
C     compressed sparse row format.

C     LOCAL SCALARS
      integer i,j,col,coltmp,lin,lintmp
      double precision val,valtmp

      do i = 1,m
          alen(i) = 0
      end do

      do i = 1,nnz
          lin = alin(i)
          alen(lin) = alen(lin) + 1
      end do

      asta(1) = 1
      do i = 2,m
          asta(i) = asta(i-1) + alen(i-1)
      end do

      do i = 1,nnz

          val = aval(i)
          col = acol(i)
          lin = alin(i)

          alin(i) = - 1

 10       if ( lin .ge. 0 ) then

              j = asta(lin)
              asta(lin) = j + 1

              valtmp = aval(j)
              coltmp = acol(j)
              lintmp = alin(j)

              aval(j) = val
              acol(j) = col
              alin(j) = - 1

              val = valtmp
              col = coltmp
              lin = lintmp

              go to 10

          end if

      end do

      do i = 1,m
          asta(i) = asta(i) - alen(i)
      end do

      end

C     ******************************************************************
C     ******************************************************************

      logical function sstop(n,x,m,lambda,rho,equatn,linear,inform)

      implicit none

C     SCALAR ARGUMENTS
      integer inform,m,n

C     ARRAY ARGUMENTS
      logical equatn(m),linear(m)
      double precision lambda(m),rho(m),x(n)

      include "algparam.inc"

C     EXTERNAL FUNCTIONS
      logical minsqstop

      if ( innercall ) then
          sstop = minsqstop(n,x,inform)
          return
      end if

      end


c*************************************************************************
c                        DRIFT_DAN.F
c*************************************************************************
c This subroutine does the Danby and decides which vbles to use
c
c             Input:
c                 nbod          ==>  number of massive bodies (int scalar)
c                 mass          ==>  mass of bodies (real array)
c                 x0,y0,z0         ==>  initial position in jacobi coord
c                                    (real scalar)
c                 vx0,vy0,vz0      ==>  initial position in jacobi coord
c                                    (real scalar)
c                 dt0            ==>  time step
c             Output:
c                 x0,y0,z0         ==>  final position in jacobi coord
c                                       (real scalars)
c                 vx0,vy0,vz0      ==>  final position in jacobi coord
c                                       (real scalars)
c                 iflg             ==>  integer flag (zero if satisfactory)
c					      (non-zero if nonconvergence)
c
c Authors:  Hal Levison & Martin Duncan
c Date:    2/10/93
c Last revision: April 6/93 - MD adds dt and keeps dt0 unchanged

      subroutine drift_dan(mu,x0,y0,z0,vx0,vy0,vz0,dt0,iflg)

      include '../../swift.inc'

c...  Inputs Only:
      real*8 mu,dt0

c...  Inputs and Outputs:
      real*8 x0,y0,z0
      real*8 vx0,vy0,vz0

c...  Output
      integer iflg

c...  Internals:
      real*8 x,y,z,vx,vy,vz,dt
      real*8 f,g,fdot,c1,c2
      real*8 c3,gdot
      real*8 u,alpha,fp,r0,v0s
      real*8 a,asq,en
      real*8 dm,ec,es,esq,xkep
      real*8 fchk,s,c

c----
c...  Executable code

c...  Set dt = dt0 to be sure timestep is not altered while solving
c...  for new coords.
	dt = dt0
	iflg = 0
        r0 = sqrt(x0*x0 + y0*y0 + z0*z0)
        v0s = vx0*vx0 + vy0*vy0 + vz0*vz0
        u = x0*vx0 + y0*vy0 + z0*vz0
        alpha = 2.0*mu/r0 - v0s

	if (alpha.gt.0.d0) then
           a = mu/alpha
           asq = a*a
           en = sqrt(mu/(a*asq))
           ec = 1.0d0 - r0/a
           es = u/(en*asq)
	   esq = ec*ec + es*es
	   dm = dt*en - int(dt*en/TWOPI)*TWOPI
	   dt = dm/en
	   if((dm*dm .gt. 0.16d0) .or. (esq.gt.0.36d0)) goto 100

	   if(esq*dm*dm .lt. 0.0016) then

               call drift_kepmd(dm,es,ec,xkep,s,c)
	       fchk = (xkep - ec*s +es*(1.-c) - dm)

	       if(fchk*fchk .gt. DANBYB) then
		  iflg = 1
		  return
	       endif

               fp = 1. - ec*c + es*s
               f = (a/r0) * (c-1.) + 1.
               g = dt + (s-xkep)/en
               fdot = - (a/(r0*fp))*en*s
               gdot = (c-1.)/fp + 1.

               x = x0*f + vx0*g
               y = y0*f + vy0*g
               z = z0*f + vz0*g
               vx = x0*fdot + vx0*gdot
               vy = y0*fdot + vy0*gdot
               vz = z0*fdot + vz0*gdot

               x0 = x
               y0 = y
               z0 = z
               vx0 = vx
               vy0 = vy
               vz0 = vz

	       iflg = 0
	       return

	   endif

         endif

100      call drift_kepu(dt,r0,mu,alpha,u,fp,c1,c2,c3,iflg)

         if(iflg .eq.0) then
           f = 1.0 - (mu/r0)*c2
           g = dt - mu*c3
           fdot = -(mu/(fp*r0))*c1
           gdot = 1. - (mu/fp)*c2

           x = x0*f + vx0*g
           y = y0*f + vy0*g
           z = z0*f + vz0*g
           vx = x0*fdot + vx0*gdot
           vy = y0*fdot + vy0*gdot
           vz = z0*fdot + vz0*gdot

           x0 = x
           y0 = y
           z0 = z
           vx0 = vx
           vy0 = vy
           vz0 = vz
	endif

        return
        end   ! drift_dan


      subroutine qqb_dm_qqb_Samps(p,i1,i2,i3,i4,za,zb,amp)
      implicit none
!---------- amplitudes for q(i1)+Q(i2)+Qb(i3)+q(i4)
!---------- with i1 i4 coupling to DM
      include 'dm_params.f'
      include 'constants.f'
      include 'zprods_decl.f'
      double precision p(mxpart,4),q(mxpart,4)
      integer i1,i2,i3,i4
      double complex amp(2,2)
      double precision s(mxpart,mxpart)
      integer h1,h2
      double precision fac

!====== fac w.r.t. vector !--- note in gg we defined fac as
!------- at ME level, here we are at amp so sqrt(fac)
      fac=sqrt(1d0)

      if(xmass.gt.1d-8) then
!---------generate massless phase space
         call gen_masslessvecs(p,q,3,4)
!---------generate spinors
         call spinoru(6,q,za,zb)
      else
!--------massless dm can use usual spinoru
         call spinoru(6,p,za,zb)
      endif

      do h1=1,6
         do h2=1,6
            s(h1,h2)=Dble(za(h1,h2)*zb(h2,h1))
         enddo
      enddo

      amp(1,1)= (za(i1,i2)*(za(i1,i4)*zb(i3,i1) + za(i2,i4)*zb(i3,i2)))/
     -   ((s(i1,i2) + s(i1,i3) + s(i2,i3))*za(i2,i3)*zb(i3,i2)) -
     -  (za(i2,i4)*(-(za(i1,i2)*zb(i3,i2)) + za(i1,i4)*zb(i4,i3)))/
     -   ((s(i2,i3) + s(i2,i4) + s(i3,i4))*za(i2,i3)*zb(i3,i2))
      amp(1,2)=(za(i1,i3)*(za(i1,i4)*zb(i2,i1) - za(i3,i4)*zb(i3,i2)))/
     -   ((s(i1,i2) + s(i1,i3) + s(i2,i3))*za(i2,i3)*zb(i3,i2)) -
     -  (za(i3,i4)*(za(i1,i3)*zb(i3,i2) + za(i1,i4)*zb(i4,i2)))/
     -   ((s(i2,i3) + s(i2,i4) + s(i3,i4))*za(i2,i3)*zb(i3,i2))
      amp(2,1)= ((za(i2,i3)*zb(i3,i1) + za(i2,i4)*zb(i4,i1))*zb(i4,i3))/
     -   ((s(i2,i3) + s(i2,i4) + s(i3,i4))*za(i2,i3)*zb(i3,i2)) -
     -  (zb(i3,i1)*(za(i1,i2)*zb(i4,i1) - za(i2,i3)*zb(i4,i3)))/
     -   ((s(i1,i2) + s(i1,i3) + s(i2,i3))*za(i2,i3)*zb(i3,i2))
      amp(2,2)=((-(za(i2,i3)*zb(i2,i1)) + za(i3,i4)*zb(i4,i1))
     &     *zb(i4,i2))/
     -   ((s(i2,i3) + s(i2,i4) + s(i3,i4))*za(i2,i3)*zb(i3,i2)) -
     -  (zb(i2,i1)*(za(i1,i3)*zb(i4,i1) + za(i2,i3)*zb(i4,i2)))/
     -   ((s(i1,i2) + s(i1,i3) + s(i2,i3))*za(i2,i3)*zb(i3,i2))

      do h1=1,2
         do h2=1,2
            amp(h1,h2)=fac*amp(h1,h2)
         enddo
      enddo

      return
      end


!-----------------------------------------------------------------------
! Skeleton 2D Electrostatic OpenMP PIC code
! written by Viktor K. Decyk, UCLA
      program mpic2
      use mpush2_h
      use omplib_h
      implicit none
! indx/indy = exponent which determines grid points in x/y direction:
! nx = 2**indx, ny = 2**indy.
      integer, parameter :: indx =   9, indy =   9
! npx/npy = number of electrons distributed in x/y direction.
      integer, parameter :: npx =  3072, npy =   3072
! ndim = number of velocity coordinates = 2
      integer, parameter :: ndim = 2
! tend = time at end of simulation, in units of plasma frequency.
! dt = time interval between successive calculations.
! qme = charge on electron, in units of e.
      real, parameter :: tend = 10.0, dt = 0.1, qme = -1.0
! vtx/vty = thermal velocity of electrons in x/y direction
! vx0/vy0 = drift velocity of electrons in x/y direction.
      real, parameter :: vtx = 1.0, vty = 1.0, vx0 = 0.0, vy0 = 0.0
! ax/ay = smoothed particle size in x/y direction
      real :: ax = .912871, ay = .912871
! idimp = number of particle coordinates = 4
! ipbc = particle boundary condition: 1 = periodic
      integer :: idimp = 4, ipbc = 1
! wke/we/wt = particle kinetic/electric field/total energy
      real :: wke = 0.0, we = 0.0, wt = 0.0
! mx/my = number of grids in x/y in sorting tiles
      integer :: mx = 16, my = 16
! xtras = fraction of extra particles needed for particle management
      real :: xtras = 0.2
! declare scalars for standard code
      integer :: np, nx, ny, nxh, nyh, nxe, nye, nxeh, nxyh, nxhy
      integer :: mx1, my1, mxy1, ntime, nloop, isign
      real :: qbme, affp
!
! declare scalars for OpenMP code
      integer :: nppmx, nppmx0, ntmax, npbmx, irc
      integer :: nvp
!
! declare arrays for standard code:
! part = original particle array
      real, dimension(:,:), pointer :: part
! qe = electron charge density with guard cells
      real, dimension(:,:), pointer :: qe
! fxye = smoothed electric field with guard cells
      real, dimension(:,:,:), pointer :: fxye
! ffc = form factor array for poisson solver
      complex, dimension(:,:), pointer :: ffc
! mixup = bit reverse table for FFT
      integer, dimension(:), pointer :: mixup
! sct = sine/cosine table for FFT
      complex, dimension(:), pointer :: sct
!
! declare arrays for OpenMP (tiled) code:
! ppart = tiled particle array
! ppbuff = buffer array for reordering tiled particle array
      real, dimension(:,:,:), pointer :: ppart, ppbuff
! kpic = number of particles in each tile
      integer, dimension(:), pointer :: kpic
! ncl = number of particles departing tile in each direction
      integer, dimension(:,:), pointer :: ncl
! ihole = location/destination of each particle departing tile
      integer, dimension(:,:,:), pointer :: ihole
!
! declare and initialize timing data
      real :: time
      integer, dimension(4) :: itime
      real :: tdpost = 0.0, tguard = 0.0, tfft = 0.0, tfield = 0.0
      real :: tpush = 0.0, tsort = 0.0
      double precision :: dtime
!
      irc = 0
! nvp = number of shared memory nodes (0=default)
      nvp = 0
!     write (*,*) 'enter number of nodes:'
!     read (5,*) nvp
! initialize for shared memory parallel processing
      call INIT_OMP(nvp)
!
! initialize scalars for standard code
! np = total number of particles in simulation
! nx/ny = number of grid points in x/y direction
      np = npx*npy; nx = 2**indx; ny = 2**indy
      nxh = nx/2; nyh = max(1,ny/2)
      nxe = nx + 2; nye = ny + 1; nxeh = nxe/2
      nxyh = max(nx,ny)/2; nxhy = max(nxh,ny)
! mx1/my1 = number of tiles in x/y direction
      mx1 = (nx - 1)/mx + 1; my1 = (ny - 1)/my + 1; mxy1 = mx1*my1
! nloop = number of time steps in simulation
! ntime = current time step
      nloop = tend/dt + .0001; ntime = 0
      qbme = qme
      affp = real(nx*ny)/real(np)
!
! allocate data for standard code
      allocate(part(idimp,np))
      allocate(qe(nxe,nye),fxye(ndim,nxe,nye))
      allocate(ffc(nxh,nyh),mixup(nxhy),sct(nxyh))
      allocate(kpic(mxy1))
!
! prepare fft tables
      call WFFT2RINIT(mixup,sct,indx,indy,nxhy,nxyh)
! calculate form factors
      isign = 0
      call MPOIS22(qe,fxye,isign,ffc,ax,ay,affp,we,nx,ny,nxeh,nye,nxh,  &
     &nyh)
! initialize electrons
      call DISTR2(part,vtx,vty,vx0,vy0,npx,npy,idimp,np,nx,ny,ipbc)
!
! find number of particles in each of mx, my tiles: updates kpic, nppmx
      call DBLKP2L(part,kpic,nppmx,idimp,np,mx,my,mx1,mxy1,irc)
      if (irc /= 0) then
         write (*,*) 'DBLKP2L error, irc=', irc
         stop
      endif
! allocate vector particle data
      nppmx0 = (1.0 + xtras)*nppmx
      ntmax = xtras*nppmx
      npbmx = xtras*nppmx
      allocate(ppart(idimp,nppmx0,mxy1))
      allocate(ppbuff(idimp,npbmx,mxy1))
      allocate(ncl(8,mxy1))
      allocate(ihole(2,ntmax+1,mxy1))
! copy ordered particle data for OpenMP: updates ppart and kpic
      call PPMOVIN2L(part,ppart,kpic,nppmx0,idimp,np,mx,my,mx1,mxy1,irc)
      if (irc /= 0) then
         write (*,*) 'PPMOVIN2L overflow error, irc=', irc
         stop
      endif
! sanity check
      call PPCHECK2L(ppart,kpic,idimp,nppmx0,nx,ny,mx,my,mx1,my1,irc)
      if (irc /= 0) then
         write (*,*) 'PPCHECK2L error: irc=', irc
         stop
      endif
!
! * * * start main iteration loop * * *
!
  500 if (nloop <= ntime) go to 2000
!     write (*,*) 'ntime = ', ntime
!
! deposit charge with OpenMP: updates qe
      call dtimer(dtime,itime,-1)
      qe = 0.0
      call GPPOST2L(ppart,qe,kpic,qme,nppmx0,idimp,mx,my,nxe,nye,mx1,   &
     &mxy1)
      call dtimer(dtime,itime,1)
      time = real(dtime)
      tdpost = tdpost + time
!
! add guard cells with OpenMP: updates qe
      call dtimer(dtime,itime,-1)
      call AGUARD2L(qe,nx,ny,nxe,nye)
      call dtimer(dtime,itime,1)
      time = real(dtime)
      tguard = tguard + time
!
! transform charge to fourier space with OpenMP: updates qe
      call dtimer(dtime,itime,-1)
      isign = -1
      call WFFT2RMX(qe,isign,mixup,sct,indx,indy,nxeh,nye,nxhy,nxyh)
      call dtimer(dtime,itime,1)
      time = real(dtime)
      tfft = tfft + time
!
! calculate force/charge in fourier space with OpenMP: updates fxye, we
      call dtimer(dtime,itime,-1)
      isign = -1
      call MPOIS22(qe,fxye,isign,ffc,ax,ay,affp,we,nx,ny,nxeh,nye,nxh,  &
     &nyh)
      call dtimer(dtime,itime,1)
      time = real(dtime)
      tfield = tfield + time
!
! transform force to real space with OpenMP: updates fxye
      call dtimer(dtime,itime,-1)
      isign = 1
      call WFFT2RM2(fxye,isign,mixup,sct,indx,indy,nxeh,nye,nxhy,nxyh)
      call dtimer(dtime,itime,1)
      time = real(dtime)
      tfft = tfft + time
!
! copy guard cells with OpenMP: updates fxye
      call dtimer(dtime,itime,-1)
      call CGUARD2L(fxye,nx,ny,nxe,nye)
      call dtimer(dtime,itime,1)
      time = real(dtime)
      tguard = tguard + time
!
! push particles with OpenMP:
      wke = 0.0
      call dtimer(dtime,itime,-1)
! updates ppart, wke
!     call GPPUSH2L(ppart,fxye,kpic,qbme,dt,wke,idimp,nppmx0,nx,ny,mx,my&
!    &,nxe,nye,mx1,mxy1,ipbc)
! updates ppart, ncl, ihole, wke, irc
      call GPPUSHF2L(ppart,fxye,kpic,ncl,ihole,qbme,dt,wke,idimp,nppmx0,&
     &nx,ny,mx,my,nxe,nye,mx1,mxy1,ntmax,irc)
      call dtimer(dtime,itime,1)
      time = real(dtime)
      tpush = tpush + time
      if (irc /= 0) then
         write (*,*) 'GPPUSHF2L error: irc=', irc
         stop
      endif
!
! reorder particles by tile with OpenMP:
      call dtimer(dtime,itime,-1)
! updates ppart, ppbuff, kpic, ncl, ihole, and irc
!     call PPORDER2L(ppart,ppbuff,kpic,ncl,ihole,idimp,nppmx0,nx,ny,mx, &
!    &my,mx1,my1,npbmx,ntmax,irc)
! updates ppart, ppbuff, kpic, ncl, and irc
      call PPORDERF2L(ppart,ppbuff,kpic,ncl,ihole,idimp,nppmx0,mx1,my1, &
     &npbmx,ntmax,irc)
      call dtimer(dtime,itime,1)
      time = real(dtime)
      tsort = tsort + time
      if (irc /= 0) then
         write (*,*) 'PPORDERF2L error: ntmax, irc=', ntmax, irc
         stop
      endif
!
      if (ntime==0) then
         write (*,*) 'Initial Field, Kinetic and Total Energies:'
         write (*,'(3e14.7)') we, wke, wke + we
      endif
      ntime = ntime + 1
      go to 500
 2000 continue
!
! * * * end main iteration loop * * *
!
      write (*,*) 'ntime = ', ntime
      write (*,*) 'Final Field, Kinetic and Total Energies:'
      write (*,'(3e14.7)') we, wke, wke + we
!
      write (*,*)
      write (*,*) 'deposit time = ', tdpost
      write (*,*) 'guard time = ', tguard
      write (*,*) 'solver time = ', tfield
      write (*,*) 'fft time = ', tfft
      write (*,*) 'push time = ', tpush
      write (*,*) 'sort time = ', tsort
      tfield = tfield + tguard + tfft
      write (*,*) 'total solver time = ', tfield
      time = tdpost + tpush + tsort
      write (*,*) 'total particle time = ', time
      wt = time + tfield
      write (*,*) 'total time = ', wt
      write (*,*)
!
      wt = 1.0e+09/(real(nloop)*real(np))
      write (*,*) 'Push Time (nsec) = ', tpush*wt
      write (*,*) 'Deposit Time (nsec) = ', tdpost*wt
      write (*,*) 'Sort Time (nsec) = ', tsort*wt
      write (*,*) 'Total Particle Time (nsec) = ', time*wt
      write (*,*)
!
      stop
      end program