! ============================ Module Description ===========================
! kdv_mod stores experiment parameters and subroutines for evaluating Linear
! and Nonlinear Operators for the KDV equation when solving in Fourier space
! (Diagonal Lambda).
!
! Contains 5 subroutines:
!   1. init         - initializes spatial grid, fft plans, experiment
!                     parameters.
!   2. ic           - sets initial condition in physical space
!   3. L            - returns diagonal entries of linear operator Lambda
!   4. N            - subroutine for evaluating nonlinear operator
!   5. error_filter - defines error function used for numerical experiments
! ===========================================================================

module kdv_mod

    use tools_mod, only: dp, PI, II, logspace, plinspace, fourierwavenum, relerror_c, antialias
    implicit none
    ! Numerical Parameters
    integer,    parameter     :: Np                   = 2**9                          ! Number of spatial points (Must be Even)
    real(dp),   parameter     :: tspan(2)             = [ 0.d0, 3.6_dp/PI ]           ! Time integration window
    logical,    parameter     :: reference_methods(3) = [ .true., .true., .false. ]   ! Methods for Reference Solution (ETDSDC,IMEXSDC,ETDRK)
    integer,    parameter     :: num_tests            = 16                            ! Number of Numerical Tests
    real,       parameter     :: smallest_F           = 1.d3                          ! Smallest Number of Function Evaluations
    real,       parameter     :: largest_F            = 1.d5                          ! Maximum Number of Function Evaluations
    logical,    parameter     :: antialiasing_enabled = .true.                        ! determines if 2/3 antialiasing rule should be applied.
    ! Equation parameters
    real(dp), parameter       :: Lx       = 2.0_dp        ! Spatial Domain Size
    real(dp), parameter       :: delta    = 2.2e-2_dp
    character(len=*), parameter :: eqn_name = "kdv"     ! used to save results to correct folder
    ! Storage Arrays
    complex(dp), dimension(:), allocatable :: ks,y0
    complex(dp), dimension(:, :), allocatable :: y, yh
    real(dp),    dimension(:), allocatable :: xs
    ! FFT Parameters
    integer(kind=8), allocatable :: plan_backward(:), plan_forward(:)
    include "fftw3.f90"
    ! Additional Settings
    real(dp), allocatable :: Fs(:) ! Function Counts to test (set in init function)

    contains

    subroutine init(max_num_threads)
        ! arguments
        integer, intent(in), optional :: max_num_threads
        ! variables
        integer :: i, nt
        if(present(max_num_threads)) then
            nt = max_num_threads
        else
            nt = 1
        endif
        ! Init FFT plans
        allocate(y(Np, nt), yh(Np, nt), plan_backward(nt), plan_forward(nt))
        do i = 1, nt
            call dfftw_plan_dft_1d_ (plan_forward(i), Np, y(:, i), yh(:, i), FFTW_FORWARD, FFTW_MEASURE)
            call dfftw_plan_dft_1d_ (plan_backward(i), Np, yh(:, i), y(:, i), FFTW_BACKWARD, FFTW_MEASURE)
        end do
        ! allocate arrays
        allocate(ks(Np), xs(Np), Fs(num_tests))
        ! set x domain
        xs = plinspace(0.0_dp,Lx,Np)
        ! set FFT plans
        call dfftw_plan_dft_1d_ (plan_forward, Np, y, yh, FFTW_FORWARD, FFTW_MEASURE)
        call dfftw_plan_dft_1d_ (plan_backward, Np, yh, y, FFTW_BACKWARD, FFTW_MEASURE)
        ! set initial condition
        call ic()
        ! Set Fourier wave numbers
        ks = fourierWaveNum(Np,Lx)
        ! Set Function Evalutations for tests
        Fs = logspace(log(smallest_f)/log(10.0_dp),log(largest_F)/log(10.0_dp),num_tests)
    end subroutine

    ! Initial Condition
    subroutine ic()
        y(:,1) = cos(PI * xs) ! NOTE: xs global var set in init
        call dfftw_execute(plan_forward(1))
        y0 = yh(:,1)
        if(antialiasing_enabled) then
            call antialias(y0)
        endif
    end subroutine ic

    ! Linear Operator
    subroutine L(lambda)
        complex(dp), dimension(Np), intent(out) :: lambda
        lambda = (-1.0_dp*(delta)**2)*((II*ks)**3)
        if(antialiasing_enabled) then
            call antialias(lambda)
        endif
    end subroutine L

    ! Nonlinear Operator
    subroutine N(t,yh_in, N_out, thread_id)
        complex(dp),    intent(in) :: t
        complex(dp), dimension(:), intent(in)  :: yh_in
        complex(dp), dimension(:), intent(out) :: N_out
        integer, intent(in), optional :: thread_id
        integer :: tid
        if(present(thread_id)) then
            tid = thread_id
        else
            tid = 1
        endif
        yh(:,tid) = yh_in
        call dfftw_execute(plan_backward(tid))
        y(:,tid) = (y(:,tid)/Np)**2;
        call dfftw_execute(plan_forward(tid))
        N_out = -0.5_dp * II * ks * yh(:,tid);
        if(antialiasing_enabled) then
            call antialias(N_out)
        endif
    end subroutine N

    ! Error Filter (Inf norm in physical space)
    function error_filter(estimate,exact) result(error)

        complex(dp), intent(in)  :: estimate(Np)
        complex(dp), intent(in)  :: exact(Np)

        real(dp)  :: error
        complex(dp), allocatable :: a(:),b(:)

        allocate(a(Np),b(Np))

        yh(:,1) = estimate/Np
        call dfftw_execute(plan_backward(1))
        a = y(:,1)

        yh(:,1) = exact/Np
        call dfftw_execute(plan_backward(1))
        b = y(:,1)

        error = relerror_c(a,b)
    end function error_filter

end module kdv_mod


c$$$      real*8 function gamma(x)
c$$$      implicit none
c$$$      real*8 x,factorial
c$$$      gamma=factorial(x)/x
c$$$      return
c$$$      end

      module gammaf
      integer*4 ,parameter ::itmaxg=67
      real*8 ,parameter ::hg=0.2d0
      real*8 ag(itmaxg)

      contains
      subroutine aginit
      implicit none
      integer*4 i
      real*8 an
      if(ag(1) .eq. 0.d0)then
        an=-1.d0
        do i=1,itmaxg
          an=an+2.d0
          ag(i)=exp(-(an*hg)**2)
        enddo
      endif
c      write(*,*)'agint ',itmaxg,ag(itmaxg)
      return
      end subroutine

      real*8 pure function factorial(x)
      use macmath
      implicit none
c     Including pi = m_pi
      real*8 ,intent(in):: x
      integer*4 ix
      integer*4 ,parameter ::lt=23
      real*8 ,parameter :: fac(0:lt)=[
     $1d0,
     $1d0,
     $2d0,
     $6d0,
     $24d0,
     $120d0,
     $720d0,
     $5040d0,
     $40320d0,
     $362880d0,
     $3628800d0,
     $39916800d0,
     $479001600d0,
     $6227020800d0,
     $87178291200d0,
     $1307674368000d0,
     $20922789888000d0,
     $355687428096000d0,
     $6402373705728000d0,
     $121645100408832000d0,
     $2432902008176640000d0,
     $51090942171709440000d0,
     $1124000727777607680000d0,
     $25852016738884976640000d0]
      if(x .lt. 0.d0)then
        factorial=exp(-aloggamma1(-x))*pi*x/sin(pi*x)
      else
        ix=int(x)
        if(ix .le. lt .and. x-ix .eq. 0.d0)then
          factorial=fac(ix)
        else
          factorial=exp(aloggamma1(x))
        endif
      endif
      return
      end

      real*8 pure function aloggamma1(x)
      use macmath
      implicit none
c     Including pi = m_pi
      real*8 ,intent(in):: x
      real*8 x1
      real*8 ,parameter ::gamma=5.d0,alogsqrt2pi=0.918938533204672742d0,
     $     alogpi=1.144729885849400174d0,
     $     c0=0.999999999999997524d0,
     $      c1=76.1800917309326077d0,
     $      c2=-86.5053204008552d0,
     $      c3=24.01409906379226027d0,
     $      c4=-1.231743354454618365d0,
     $      c5=0.001216872118636531519d0,
     $      c6=-0.00001408915744128554778d0,
     $      c7=4.00491935010387864d-6,
     $      c8=-4.93961492826482964d-7
      if(x .lt. 0.d0)then
        x1=-x
        aloggamma1=log(pi*x/sin(pi*x))-
     $       ((x1+.5d0)*log(x1+gamma+.5d0)-(x1+gamma+.5d0)
     $       +alogsqrt2pi+log(
     $       c0+c1/(1.d0+x1)+c2/(2.d0+x1)+c3/(3.d0+x1)
     $       +c4/(4.d0+x1)+c5/(5.d0+x1)+c6/(6.d0+x1)
     $       +c7/(7.d0+x1)+c8/(8.d0+x1)))
      else
        aloggamma1=(x+.5d0)*log(x+gamma+.5d0)-(x+gamma+.5d0)
     $       +alogsqrt2pi+log(
     $       c0+c1/(1.d0+x)+c2/(2.d0+x)+c3/(3.d0+x)
     $       +c4/(4.d0+x)+c5/(5.d0+x)+c6/(6.d0+x)
     $       +c7/(7.d0+x)+c8/(8.d0+x))
      endif
      return
      end

      complex*16 pure function cgamma(x)
      implicit none
      complex*16 ,intent(in):: x
      cgamma=exp(cloggamma1(x))/x
      return
      end

      complex*16 pure function cfactorial(x)
      implicit none
      complex*16 ,intent(in):: x
      cfactorial=exp(cloggamma1(x))
      return
      end

      complex*16 pure function cloggamma1(z)
      use macmath
      implicit none
c     Including pi = m_pi
      complex*16 ,intent(in):: z
      complex*16 z1
      real*8 ,parameter ::gamma=5.d0,alogsqrt2pi=0.918938533204672742d0,
     $     alogpi=1.144729885849400174d0,c0=0.999999999999997524d0,
     $      c1=76.1800917309326077d0,
     $      c2=-86.5053204008552d0,
     $      c3=24.01409906379226027d0,
     $      c4=-1.231743354454618365d0,
     $      c5=0.001216872118636531519d0,
     $      c6=-0.00001408915744128554778d0,
     $      c7=4.00491935010387864d-6,
     $      c8=-4.93961492826482964d-7
      if(dble(z) .lt. 0.d0)then
        z1=-z
        cloggamma1=log(pi*z/sin(pi*z))-
     $       ((z1+.5d0)*log(z1+gamma+.5d0)-(z1+gamma+.5d0)
     $       +alogsqrt2pi+log(
     $       c0+c1/(1.d0+z1)+c2/(2.d0+z1)+c3/(3.d0+z1)
     $       +c4/(4.d0+z1)+c5/(5.d0+z1)+c6/(6.d0+z1)
     $       +c7/(7.d0+z1)+c8/(8.d0+z1)))
      else
        cloggamma1=(z+.5d0)*log(z+gamma+.5d0)-(z+gamma+.5d0)
     $       +alogsqrt2pi+log(
     $       c0+c1/(1.d0+z)+c2/(2.d0+z)+c3/(3.d0+z)
     $       +c4/(4.d0+z)+c5/(5.d0+z)+c6/(6.d0+z)
     $       +c7/(7.d0+z)+c8/(8.d0+z))
      endif
      return
      end

      real*8 pure function ferf(x)
      implicit none
      real*8 , intent(in)::x
c      ferf=sign(gammap(.5d0,x**2),x)
      ferf=erf(x)
      return
      end

      real*8 pure function ferfc(x)
      implicit none
      real*8 ,intent(in):: x
      ferfc=erfc(x)
c      real*8 x,gammaq,gammap
c      if(x .lt. 0.d0)then
c        erfc=1.d0+gammap(.5d0,x**2)
c      else
c        erfc=gammaq(.5d0,x**2)
c      endif
      return
      end

      real*8 pure function gammap(a,x)
      implicit none
      real*8 ,intent(in):: a,x
      if(x .lt. 0.d0 .or. a .lt. 0.d0)then
        gammap=x/0.d0
        return
      endif
      if(a .eq. 0.d0)then
        gammap=1.d0
      elseif(x .lt. a+1.d0)then
        gammap=gammaser(a,x)
      else
        gammap=1.d0-gammacf(a,x)
      endif
      return
      end

      real*8 pure function gammaq(a,x)
      implicit none
      real*8 ,intent(in):: a,x
      if(x .lt. 0.d0 .or. a .lt. 0.d0)then
        gammaq=x/0.d0
        return
      endif
      if(a .eq. 0.d0)then
        gammaq=0.d0
      elseif(x .lt. a+1.d0)then
        gammaq=1.d0-gammaser(a,x)
      else
        gammaq=gammacf(a,x)
      endif
      return
      end

      real*8 pure function gamma0(x)
      use macmath
      implicit none
c     Including Euler's gamma(euler)
      real*8 ,intent(in):: x
      if(x .lt. 0.d0)then
        gamma0=x/0.d0
        return
      endif
      if(x .lt. 1.d0)then
        gamma0=gamma0ser(x)-log(x)-euler
      else
        gamma0=gamma0cf(x)
      endif
      return
      end

      real*8 pure function gamma0log(x)
      use macmath
      implicit none
c     Including Euler's gamma(euler)
      real*8 ,intent(in):: x
      if(x .lt. 0.d0)then
        gamma0log=x/0.d0
        return
      endif
      if(x .lt. 1.d0)then
        gamma0log=gamma0ser(x)
      else
        gamma0log=gamma0cf(x)+log(x)+euler
      endif
      return
      end

      real*8 pure function gammaser(a,x)
      implicit none
      integer*4 i
      real*8 ,intent(in):: a,x
      real*8 gln,ap,sum,del
      integer*4 ,parameter::itmax=300
      real*8 ,parameter ::eps=1.d-13
      if(x .lt. 0.d0)then
        gammaser=x/0.d0
        return
      elseif(x .eq. 0.d0)then
        gammaser=0.d0
        return
      elseif(a .eq. 0.d0)then
        gammaser=1.d0
        return
      endif
      ap=a
      sum=1.d0
      del=sum
      do i=1,itmax
        ap=ap+1.d0
        del=del*x/ap
        sum=sum+del
        if(abs(del) .lt. abs(sum)*eps)then
          go to 1
        endif
      enddo
 1    gln=aloggamma1(a-1.d0)
      gammaser=sum*exp(-x+a*log(x)-gln)/a
      return
      end

      real*8 pure function gammacf(a,x)
      implicit none
      real*8 ,intent(in):: a,x
      integer*4 i
      real*8 a1,gln,b,c,d,h,an,del
      integer*4 ,parameter::itmax=300
      real*8 ,parameter ::eps=1.d-13,fpmin=1.d-30
      if(a .eq. 0.d0)then
        gammacf=0.d0
        return
      endif
      a1=a-1.d0
      b=x-a1
      c=1.d0/fpmin
      d=1.d0/b
      h=d
      do i=1,itmax
        an=-i*(i-a)
        b=b+2.d0
        d=an*d+b
        if(abs(d) .lt. fpmin)then
          d=fpmin
        endif
        c=b+an/c
        if(abs(c) .lt. fpmin)then
          c=fpmin
        endif
        d=1.d0/d
        del=d*c
        h=h*del
        if(abs(del-1.d0) .lt. eps)then
          go to 1
        endif
      enddo
 1    gln=aloggamma1(a1)
      gammacf=exp(-x+a*log(x)-gln)*h
      return
      end

      real*8 pure function gamma0ser(x)
      implicit none
      real*8 ,intent(in):: x
      integer*4 i
      real*8 ap,sum,del
      integer*4 ,parameter ::itmax=300
      real*8 ,parameter ::eps=1.d-13
      if(x .le. 0.d0)then
        gamma0ser=x/0.d0
        return
      endif
      ap=1.d0
      sum=x
      del=x
      do i=1,itmax
        del=-del*x*ap
        ap=ap+1.d0
        del=del/ap**2
        sum=sum+del
        if(abs(del) .lt. abs(sum)*eps)then
          go to 1
        endif
      enddo
 1    gamma0ser=sum
      return
      end

      real*8 pure function gamma0cf(x)
      implicit none
      integer*4 i
      real*8 ,intent(in):: x
      real*8 a1,b,c,d,h,an,del
      integer*4 ,parameter ::itmax=300
      real*8 ,parameter ::eps=1.d-13,fpmin=1.d-30
      a1=-1.d0
      b=x-a1
      c=1.d0/fpmin
      d=1.d0/b
      h=d
      do i=1,itmax
        an=-i**2
        b=b+2.d0
        d=an*d+b
        if(abs(d) .lt. fpmin)then
          d=fpmin
        endif
        c=b+an/c
        if(abs(c) .lt. fpmin)then
          c=fpmin
        endif
        d=1.d0/d
        del=d*c
        h=h*del
        if(abs(del-1.d0) .lt. eps)then
          go to 1
        endif
      enddo
 1    gamma0cf=exp(-x)*h
      return
      end

      complex*16 pure function cerf(z)
      implicit none
      real*8 a
      complex*16 ,intent(in):: z
      a=dble(z)**2+imag(z)**2
      if(dble(z) .lt. 0.d0)then
        if(dble(-z) .gt. 1.d0)then
          cerf=-1.d0+cerfcf(-z)
        elseif(a .gt. 0.01d0)then
          cerf=-1.d0+cerfcd(-z)
        else
          cerf=-cerfs(-z)
        endif
      else
        if(dble(z) .gt. 1.d0)then
          cerf=1.d0-cerfcf(z)
        elseif(a .gt. 0.01d0)then
          cerf=1.d0-cerfcd(z)
        else
          cerf=cerfs(z)
        endif
      endif
      return
      end

      complex*16 pure function cerfc(z)
      implicit none
      complex*16 ,intent(in):: z
      real*8 a
      a=dble(z)**2+imag(z)**2
      if(dble(z) .lt. 0.d0)then
        if(dble(-z) .gt. 1.d0)then
          cerfc=2.d0-cerfcf(-z)
        elseif(a .gt. 0.01d0)then
          cerfc=2.d0-cerfcd(-z)
        else
          cerfc=1.d0+cerfs(-z)
        endif
      else
        if(dble(z) .gt. 1.d0)then
          cerfc=cerfcf(z)
        elseif(a .gt. 0.01d0)then
          cerfc=cerfcd(z)
        else
          cerfc=1.d0-cerfs(z)
        endif
      endif
      return
      end

      complex*16 pure function cerfs(z)
      use macmath
      implicit none
      complex*16 ,intent(in):: z
c     Including m_2_sqrtpi:	2 / Sqrt[Pi]
      real*8 ,parameter ::r=m_2_sqrtpi
      complex*16 z2
      z2=z**2
      cerfs=r*z*(1.d0-z2*(1.d0/3.d0-z2*(1.d0/10.d0-
     $     z2*(1.d0/42.d0-z2*(1.d0/216.d0-z2/1320.d0)))))
      return
      end

      complex*16 pure function cerfcd(z)
      use macmath
      implicit none
c     Including m_2_sqrtpi:	2 / Pi
      complex*16 ,intent(in):: z
      real*8 an1,an2,an0
      real*8 ,parameter ::r=m_2_pi
      complex*16 cs,cs1,ca,ca2,cd,z1,z2
      integer*4 i
      if(imag(z) .gt. 0.d0)then
        z1=dcmplx(imag(z),dble(z))
        an0=anint(dble(z1)/2.d0/hg)*2.d0
        z2=z1-an0*hg
        an1=an0+1.d0
        an2=an0-1.d0
        ca=exp(2.d0*hg*z2)
        ca2=ca**2
        cs=ag(1)*(ca/an1+1.d0/an2/ca)
        do i=2,itmaxg
          an1=an1+2.d0
          an2=an2-2.d0
          ca=ca*ca2
          cd=ag(i)*(ca/an1+1.d0/an2/ca)
          cs1=cs+cd
          if(cs1 .eq. cs)then
            go to 11
          endif
          cs=cs1
        enddo
c        write(*,*)'cerfcd convergence error'
 11     cs=exp(z1**2-z2**2)*cs
        cerfcd=dcmplx(1.d0-r*imag(cs),-r*dble(cs))
      else
        z1=dcmplx(-imag(z),dble(z))
        an0=anint(dble(z1)/2.d0/hg)*2.d0
        z2=z1-an0*hg
        an1=an0+1.d0
        an2=an0-1.d0
        ca=exp(2.d0*hg*z2)
        ca2=ca**2
        cs=ag(1)*(ca/an1+1.d0/an2/ca)
        do i=2,itmaxg
          an1=an1+2.d0
          an2=an2-2.d0
          ca=ca*ca2
          cd=ag(i)*(ca/an1+1.d0/an2/ca)
          cs1=cs+cd
          if(cs1 .eq. cs)then
            go to 10
          endif
          cs=cs1
        enddo
c        write(*,*)'cerfcd convergence error'
 10     cs=exp(z1**2-z2**2)*cs
        cerfcd=dcmplx(1.d0-r*imag(cs),r*dble(cs))
      endif
      return
      end

      complex*16 pure function cerfcf(z)
      use macmath
      implicit none
c     Including m_2_sqrtpi:	2 / Sqrt[Pi]
      complex*16 ,intent(in):: z
      real*8 ,parameter:: fpmin=1.d-300,eps=1.d-15,r=m_2_sqrtpi
      integer*4 i
      integer*4 ,parameter::itmax=1000
      real*8 an,a
      complex*16 cb,cd,ch,cdel,cc,z2
      z2=z**2
      cb=2.d0*z2+1.d0
      cc=1.d0/fpmin
      cd=1.d0/cb
      ch=cd
      an=-1.d0
      do i=1,itmax
        an=an+2.d0
        a=-an*(an+1.d0)
        cb=cb+4.d0
        cd=1.d0/(a*cd+cb)
        cc=cb+a/cc
        cdel=cc*cd
        ch=ch*cdel
        if(abs(cdel-1.d0) .lt. eps)then
          go to 2
        endif
      enddo
c      write(*,*)'erfc convergence error'
 2    cerfcf=ch*z*r*exp(-z2)
      return
      end

      real*8 pure function productlog(x)
      use macmath
      use mathfun
      implicit none
c     Including m_e(Napier's constant: Exp[1])
      real*8 ,intent(in):: x
      real*8 w,f1,d
      real*8 ,parameter ::eps=3.d-16,en=m_e
      if(x .le. -1.d0/en)then
        productlog=-1.d0
        return
      endif
      if(x .eq. 0.d0)then
        productlog=0.d0
        return
      endif
      if(x .lt. -0.25d0)then
        w=sqrt1(2.d0*en*x+1.d0)
c        w=sqrt(2.d0*(en*x+1.d0))-1.d0
      elseif(x .lt. 2.d0)then
        w=.5d0*sqrt1(4.d0*x)
c        w=.5d0*(sqrt(4.d0*x+1.d0)-1.d0)
      else
        w=log(x/log(x/log(x)))
      endif
      f1=x*exp(-w)
      d=(f1-w)/(f1+1.d0)
      do while(abs(d) .gt. eps*abs(w))
        w=w+d
        f1=x*exp(-w)
        d=(f1-w)/(f1+1.d0)
      enddo
      productlog=w+d
      return
      end

      complex*16 pure function cproductlog(z)
      use macmath
      implicit none
c     Including m_e(Napier's constant: Exp[1])
      complex*16 ,intent(in):: z
      complex*16 w,f1,d
      real*8 ,parameter ::eps=3.d-16,en=m_e
      if(imag(z) .eq. 0.d0)then
        if(dble(z) .ge. -1.d0/en)then
          cproductlog=productlog(dble(z))
          return
        endif
      endif
      if(abs(z+1.d0/en) .lt. 0.2d0)then
        w=sqrt(2.d0*(en*z+1.d0))-1.d0
      elseif(abs(z) .lt. 3.d0)then
        w=.5d0*(sqrt(4.d0*z+1.d0)-1.d0)
      else
        w=log(z/log(z/log(z)))
      endif
      f1=z*exp(-w)
      d=(f1-w)/(f1+1.d0)
      do while(abs(d) .gt. abs(w)*eps)
        w=w+d
        f1=z*exp(-w)
        d=(f1-w)/(f1+1.d0)
      enddo
      cproductlog=w+d
      return
      end

      real*8 pure function inverseerf(x)
      implicit none
      real*8 ,intent(in):: x
      real*8 y,dy,ady0,y1
      real*8, parameter:: c=1.12837916709551257d0, a=0.140012d0,
     $     pa=0.461814d0
c      if(abs(x) .gt. 0.6d0)then
c        xl2=log((1.d0-x)*(1.d0+x))
c        xp=2.d0/pa+xl2*.5d0
c        y=sign(sqrt(-xl2/(sqrt(xp**2-xl2/a)+xp)/a),x)
c      else
        y=x
c      endif
      ady0=1.d100
      do while(.true.)
        dy=(x-erf(y))/c/exp(-y**2)
        if(abs(dy) .ge. ady0)then
          exit
        endif
        y1=y+dy
        if(y .eq. y1)then
          exit
        endif
        y=y1
        ady0=abs(dy)
      enddo
      inverseerf=y
      return
      end

      end module


CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
C                                                                      C
C     SUBROUTINE BLOCH                                                 C
C                                                                      C
C     Read the couple of DOF with bloch-periodicity constraints        C
C                                                                      C
C                                                                      C
C     NCOND       :NUMBER OF CONSTRAINTS BETWEEN DOF                   C
C     IMDOF       :IMAGE DOF                                           C
C     IRDOF       :REFERENCES DOF                                      C
C                                                                      C
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
C                                                                      C
C     AUTHOR: NICOLAS GUARIN Z.                                        C
C     GRUPO DE MECANICA APLICADA- UNIVERSIDAD EAFIT                    C
C     LAST MOD: 20 AUGUST 2012                                         C
C                                                                      C
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
C23456789012345678901234567890123456789012345678901234567890123456789012
C
      SUBROUTINE BLOCH(NDOF,SKG,SMG,NCOR,COORDS,IDOFCOOR,NCON_WO,NCON,
     1           IRDOF_WO,IMDOF,IRDOF,NEVALS,AKXMIN,
     2           AKYMIN,AKXMAX,AKYMAX,NKX,NKY,EIGVALS)

      IMPLICIT REAL*8(A-H,O-Z)
      PARAMETER (PI = 3.141592653589793)

      DIMENSION SKG(NDOF,NDOF),SMG(NDOF,NDOF),COORDS(2,NCOR),
     1          IRDOF_WO(NCON_WO),
     2          IMDOF(NCON),IRDOF(NCON),
     3          EIGVALS(NKX*NKY,NEVALS+2),
     4          IDOFCOOR(NDOF)

C     LOCAL VARIABLES
      COMPLEX*16 SAUX, SKBLOCH, SMBLOCH, XJ, WORK
      DIMENSION SAUX(NDOF,NDOF),
     1          SKBLOCH(NDOF - NCON,NDOF - NCON),
     2          SMBLOCH(NDOF - NCON,NDOF - NCON),
     3          EVALS(NDOF - NCON),
     4          RWORK(3*(NDOF - NCON)-2), WORK(2*(NDOF - NCON)-1)

      N = NDOF - NCON

      XJ=(0.0,1.0)
      SKBLOCH = 0.0

      DKX = (AKXMAX - AKXMIN)/(NKX-1)  ! X Wave Number step
      DKY = (AKYMAX - AKYMIN)/(NKY-1)  ! Y Wave Number step
      AKX = AKXMIN
      ICONT = 1
      DO IKX=1,NKX

        AKY = AKYMIN
        DO IKY=1,NKY

          SAUX = DCMPLX(SKG)
          CALL BLOCH_BC(SAUX,NDOF,COORDS,IDOFCOOR,NCON,NCON_WO,  ! Bloch BC Imposition
     1                  NCOR,AKX,AKY,IRDOF,IRDOF_WO,
     2                  IMDOF,SKBLOCH)

          SAUX = DCMPLX(SMG)
          CALL BLOCH_BC(SAUX,NDOF,COORDS,IDOFCOOR,NCON,NCON_WO,  ! Bloch BC Imposition
     1                  NCOR,AKX,AKY,IRDOF,IRDOF_WO,
     2                  IMDOF,SMBLOCH)

          CALL ZHEGV(1, 'N', 'L', N, SKBLOCH, N, SMBLOCH, N, EVALS,
     1               WORK, 2*N-1, RWORK, INFO)

          IF(INFO.EQ.0)THEN
            WRITE(*,777) ICONT, NKX*NKY
          ELSE
            WRITE(*,666) ICONT, INFO
          END IF

          EIGVALS(ICONT,1) = AKX
          EIGVALS(ICONT,2) = AKY
          EIGVALS(ICONT,3:NEVALS+2) = EVALS(1:NEVALS)

          ICONT = ICONT+1

          AKY = AKY + DKY
        END DO

        AKX = AKX + DKX
      END DO

      RETURN

 777  FORMAT(/,'WAVE NUMBER: ',I5,'/',I5)
 666  FORMAT(/,'ERROR AT WAVE NUMBER: ',I5,'  SOLVER RETURNED: ',I5)

      END SUBROUTINE BLOCH

C                                                                      C
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
C                                                                      C
C     SUBROUTINE  BLOCH_BC()                                           C
C                                                                      C
C     Pending...                                                       C
C                                                                      C
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
C                                                                      C
C     AUTHOR: NICOLAS GUARIN Z.                                        C
C     GRUPO DE MECANICA APLICADA- UNIVERSIDAD EAFIT                    C
C     LAST MOD: 11 MARCH 2012                                          C
C                                                                      C
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
C23456789012345678901234567890123456789012345678901234567890123456789012
      SUBROUTINE BLOCH_BC(SKAUX,NDOF,COORDS,IDOFCOOR,NCOND,NCOND_WO,
     1                    NCOR,AKX,AKY,IRDOF,IRDOF_WO,
     2                    IMDOF,SKBLOCH)

      IMPLICIT REAL*8(A-H,O-Z)

      COMPLEX*16 SKAUX,SKBLOCH,FI, FCI, FR,
     1           FCR, XJ

      DIMENSION SKAUX(NDOF,NDOF),
     1          SKBLOCH(NDOF - NCOND,NDOF - NCOND),
     2          COORDS(2,NCOR),
     3          IRDOF_WO(NCOND_WO),
     4          IMDOF(NCOND),IRDOF(NCOND),
     5          IDOFCOOR(NDOF)

      XJ=(0.0,1.0)

      DO II=1,NCOND_WO    ! Assigning the Phase shifts for Reference DOF
        IR = IRDOF_WO(II)
        IDCOR = IDOFCOOR(IR)
        XR = COORDS(1,IDCOR); YR = COORDS(2,IDCOR)
        FR = EXP(XJ*AKX*XR)*EXP(XJ*AKY*YR)
        FCR= EXP(-XJ*AKX*XR)*EXP(-XJ*AKY*YR)
        CALL CROWMULT(SKAUX,NDOF,SKAUX,IR,FCR)
        CALL CCOLMULT(SKAUX,NDOF,SKAUX,IR,FR)
      END DO

      DO II=1,NCOND    ! Assigning the Phase shifts for Image DOF
        IM = IMDOF(II)
        IDCOR = IDOFCOOR(IM)
        XI = COORDS(1,IDCOR); YI = COORDS(2,IDCOR)
        FI = EXP(XJ*AKX*XI)*EXP(XJ*AKY*YI)
        FCI= EXP(-XJ*AKX*XI)*EXP(-XJ*AKY*YI)
        CALL CROWMULT(SKAUX,NDOF,SKAUX,IM,FCI)
        CALL CCOLMULT(SKAUX,NDOF,SKAUX,IM,FI)
      END DO

      DO II=1,NCOND    ! Summing rows and columns
        IR = IRDOF(II); IM = IMDOF(II)
        CALL CROWADD(SKAUX,NDOF,SKAUX,IM,IR)
        CALL CCOLADD(SKAUX,NDOF,SKAUX,IM,IR)
      END DO

      CALL COLROWDEL(SKAUX,NDOF,NCOND,SKBLOCH,IMDOF)    ! Deleting redundant equations

      RETURN

      END SUBROUTINE BLOCH_BC

C
C
C23456789012345678901234567890123456789012345678901234567890123456789012
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
C                                                                      C
C   SUBROUTINE COLROWDEL(A,NA,NRED,B,COLS)                             C
C                                                                      C
C    Delete NRED rows and columns from the matrix A and returns the    C
C    submatrix B. COLS are the columns/rows to be deleted.             C
C    NA is the dimension of A.                                         C
C                                                                      C
C    Both matrices, A and B are complex.
C                                                                      C
C                                                                      C
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
C23456789012345678901234567890123456789012345678901234567890123456789012
C
      SUBROUTINE COLROWDEL(A,NA,NRED,B,COLS)
C
      COMPLEX*16 A, B
      INTEGER COLS, COLN, CONTII, CONTJJ
      DIMENSION A(NA,NA),B(NA-NRED,NA-NRED), COLS(NRED), COLN(NA)

      COLN = 0

      DO II=1,NRED
        COLN(COLS(II)) = 1
      END DO

      CONTII = 1
      DO II=1,NA
        IF(COLN(II).EQ.0)THEN
          CONTJJ = 1
          DO JJ=1,NA
            IF(COLN(JJ).EQ.0)THEN
              B(CONTII,CONTJJ) = A(II,JJ)
              CONTJJ = CONTJJ + 1
            END IF
          END DO
         CONTII = CONTII +1
        END IF

      END DO

      RETURN

C
      END SUBROUTINE COLROWDEL
C
C
C23456789012345678901234567890123456789012345678901234567890123456789012
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
C                                                                      C
C   SUBROUTINE CCOLMULT                                                C
C                                                                      C
C    Multiply the COL column of the complex matrix A by a factor FACT, C
C    the result is stored in the complex matrix B.                     C
C                                                                      C
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
C23456789012345678901234567890123456789012345678901234567890123456789012
C
      SUBROUTINE CCOLMULT(A,N,B,COL,FACT)
C
      COMPLEX*16 A, B, FACT
      INTEGER COL
      DIMENSION A(N,N),B(N,N)

       DO II=1,N
          DO JJ=1,N
            IF (JJ.EQ.COL) THEN
              B(II,JJ) = A(II,JJ)*FACT
            ELSE
              B(II,JJ) = A(II,JJ)
            END IF
          END DO
      END DO

      RETURN

      END SUBROUTINE CCOLMULT
C
C
C23456789012345678901234567890123456789012345678901234567890123456789012
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
C                                                                      C
C   SUBROUTINE CROWMULT                                                C
C                                                                      C
C    Multiply the ROW row of the complex matrix A by a factor FACT,    C
C    the result is stored in the complex matrix B.                     C
C                                                                      C
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
C23456789012345678901234567890123456789012345678901234567890123456789012
C
      SUBROUTINE CROWMULT(A,N,B,ROW,FACT)
C
      COMPLEX*16 A, B, FACT
      INTEGER ROW
      DIMENSION A(N,N),B(N,N)

       DO II=1,N
          DO JJ=1,N
            IF (II.EQ.ROW) THEN
              B(II,JJ) = A(II,JJ)*FACT
            ELSE
              B(II,JJ) = A(II,JJ)
            END IF
          END DO
      END DO

      RETURN

      END SUBROUTINE CROWMULT
C
C
C23456789012345678901234567890123456789012345678901234567890123456789012
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
C                                                                      C
C   SUBROUTINE CROWADD                                                 C
C                                                                      C
C    PENDING...                                                        C
C                                                                      C
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
C23456789012345678901234567890123456789012345678901234567890123456789012
C
      SUBROUTINE CROWADD(A,N,B,ROW1,ROW2)

      COMPLEX*16 A, B
      INTEGER ROW1, ROW2
      DIMENSION A(N,N),B(N,N)

      DO II=1,N
          DO JJ=1,N
            IF (II.EQ.ROW2) THEN
              B(II,JJ) = A(II,JJ) + A(ROW1,JJ)

            ELSE
              B(II,JJ) = A(II,JJ)
            END IF
          END DO
      END DO

      RETURN

      END SUBROUTINE CROWADD
C
C
C23456789012345678901234567890123456789012345678901234567890123456789012
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
C                                                                      C
C   SUBROUTINE CCOLADD                                                 C
C                                                                      C
C    PENDING...                                                        C
C                                                                      C
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
C23456789012345678901234567890123456789012345678901234567890123456789012
C
      SUBROUTINE CCOLADD(A,N,B,COL1,COL2)

      COMPLEX*16 A, B
      INTEGER COL1, COL2
      DIMENSION A(N,N),B(N,N)

      DO II=1,N
          DO JJ=1,N
            IF (JJ.EQ.COL2) THEN
              B(II,JJ) = A(II,JJ) + A(II,COL1)

            ELSE
              B(II,JJ) = A(II,JJ)
            END IF
          END DO
      END DO

      RETURN

      END SUBROUTINE CCOLADD
C
C
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
C                                                                      C
C     SUBROUTINE NOD2DOF                                               C
C                                                                      C
C                                                                      C
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
C
      SUBROUTINE NOD2DOF(NUMN,MXDOFDIM,NEQ,ID,NCOND,NCOND_WO,NCON_DOF,
     1                  NCON_DOF_WO,IMNODES,IRNODES,IRNODES_WO,IMDOF,
     2                  IRDOF,IRDOF_WO,IDOFCOOR)
C
      IMPLICIT REAL*8 (A-H,O-Z)
C
      DIMENSION ID(MXDOFDIM,NUMN),IMNODES(NCOND),IRNODES(NCOND),
     1          IRNODES_WO(NCOND_WO), IMDOF(NCON_DOF),IRDOF(NCON_DOF),
     2          IRDOF_WO(NCON_DOF_WO), IDOFCOOR(NEQ)

      ICONT = 1
      JCONT = 1
      DO J=1,NCOND
        DO  I=1,MXDOFDIM
            IF (ID(I,IMNODES(J)).NE.0)THEN
              IMDOF(ICONT) = ID(I,IMNODES(J))
              ICONT = ICONT + 1
            END IF
            IF (ID(I,IRNODES(J)).NE.0)THEN
              IRDOF(JCONT) = ID(I,IRNODES(J))
              JCONT = JCONT + 1
            END IF
        END DO
      END DO

      ICONT = 1
      DO J=1,NCOND_WO
        DO  I=1,MXDOFDIM
            IF (ID(I,IRNODES_WO(J)).NE.0)THEN
              IRDOF_WO(ICONT) = ID(I,IRNODES_WO(J))
              ICONT = ICONT + 1
            END IF
        END DO
      END DO

      DO J=1,NUMN
        DO  I=1,MXDOFDIM
              IDOFCOOR(ID(I,J)) = J
        END DO
      END DO

C
      RETURN
C
      END SUBROUTINE NOD2DOF
C


*> \brief \b CCKGLM
*
*  =========== DOCUMENTATION ===========
*
* Online html documentation available at
*            http://www.netlib.org/lapack/explore-html/
*
*  Definition:
*  ===========
*
*       SUBROUTINE CCKGLM( NN, NVAL, MVAL, PVAL, NMATS, ISEED, THRESH,
*                          NMAX, A, AF, B, BF, X, WORK, RWORK, NIN, NOUT,
*                          INFO )
*
*       .. Scalar Arguments ..
*       INTEGER            INFO, NIN, NMATS, NMAX, NN, NOUT
*       REAL               THRESH
*       ..
*       .. Array Arguments ..
*       INTEGER            ISEED( 4 ), MVAL( * ), NVAL( * ), PVAL( * )
*       REAL               RWORK( * )
*       COMPLEX            A( * ), AF( * ), B( * ), BF( * ), WORK( * ),
*      $                   X( * )
*       ..
*
*
*> \par Purpose:
*  =============
*>
*> \verbatim
*>
*> CCKGLM tests CGGGLM - subroutine for solving generalized linear
*>                       model problem.
*> \endverbatim
*
*  Arguments:
*  ==========
*
*> \param[in] NN
*> \verbatim
*>          NN is INTEGER
*>          The number of values of N, M and P contained in the vectors
*>          NVAL, MVAL and PVAL.
*> \endverbatim
*>
*> \param[in] NVAL
*> \verbatim
*>          NVAL is INTEGER array, dimension (NN)
*>          The values of the matrix row dimension N.
*> \endverbatim
*>
*> \param[in] MVAL
*> \verbatim
*>          MVAL is INTEGER array, dimension (NN)
*>          The values of the matrix column dimension M.
*> \endverbatim
*>
*> \param[in] PVAL
*> \verbatim
*>          PVAL is INTEGER array, dimension (NN)
*>          The values of the matrix column dimension P.
*> \endverbatim
*>
*> \param[in] NMATS
*> \verbatim
*>          NMATS is INTEGER
*>          The number of matrix types to be tested for each combination
*>          of matrix dimensions.  If NMATS >= NTYPES (the maximum
*>          number of matrix types), then all the different types are
*>          generated for testing.  If NMATS < NTYPES, another input line
*>          is read to get the numbers of the matrix types to be used.
*> \endverbatim
*>
*> \param[in,out] ISEED
*> \verbatim
*>          ISEED is INTEGER array, dimension (4)
*>          On entry, the seed of the random number generator.  The array
*>          elements should be between 0 and 4095, otherwise they will be
*>          reduced mod 4096, and ISEED(4) must be odd.
*>          On exit, the next seed in the random number sequence after
*>          all the test matrices have been generated.
*> \endverbatim
*>
*> \param[in] THRESH
*> \verbatim
*>          THRESH is REAL
*>          The threshold value for the test ratios.  A result is
*>          included in the output file if RESID >= THRESH.  To have
*>          every test ratio printed, use THRESH = 0.
*> \endverbatim
*>
*> \param[in] NMAX
*> \verbatim
*>          NMAX is INTEGER
*>          The maximum value permitted for M or N, used in dimensioning
*>          the work arrays.
*> \endverbatim
*>
*> \param[out] A
*> \verbatim
*>          A is COMPLEX array, dimension (NMAX*NMAX)
*> \endverbatim
*>
*> \param[out] AF
*> \verbatim
*>          AF is COMPLEX array, dimension (NMAX*NMAX)
*> \endverbatim
*>
*> \param[out] B
*> \verbatim
*>          B is COMPLEX array, dimension (NMAX*NMAX)
*> \endverbatim
*>
*> \param[out] BF
*> \verbatim
*>          BF is COMPLEX array, dimension (NMAX*NMAX)
*> \endverbatim
*>
*> \param[out] X
*> \verbatim
*>          X is COMPLEX array, dimension (4*NMAX)
*> \endverbatim
*>
*> \param[out] RWORK
*> \verbatim
*>          RWORK is REAL array, dimension (NMAX)
*> \endverbatim
*>
*> \param[out] WORK
*> \verbatim
*>          WORK is COMPLEX array, dimension (NMAX*NMAX)
*> \endverbatim
*>
*> \param[in] NIN
*> \verbatim
*>          NIN is INTEGER
*>          The unit number for input.
*> \endverbatim
*>
*> \param[in] NOUT
*> \verbatim
*>          NOUT is INTEGER
*>          The unit number for output.
*> \endverbatim
*>
*> \param[out] INFO
*> \verbatim
*>          INFO is INTEGER
*>          = 0 :  successful exit
*>          > 0 :  If CLATMS returns an error code, the absolute value
*>                 of it is returned.
*> \endverbatim
*
*  Authors:
*  ========
*
*> \author Univ. of Tennessee
*> \author Univ. of California Berkeley
*> \author Univ. of Colorado Denver
*> \author NAG Ltd.
*
*> \ingroup complex_eig
*
*  =====================================================================
      SUBROUTINE CCKGLM( NN, NVAL, MVAL, PVAL, NMATS, ISEED, THRESH,
     $                   NMAX, A, AF, B, BF, X, WORK, RWORK, NIN, NOUT,
     $                   INFO )
*
*  -- LAPACK test routine --
*  -- LAPACK is a software package provided by Univ. of Tennessee,    --
*  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
*
*     .. Scalar Arguments ..
      INTEGER            INFO, NIN, NMATS, NMAX, NN, NOUT
      REAL               THRESH
*     ..
*     .. Array Arguments ..
      INTEGER            ISEED( 4 ), MVAL( * ), NVAL( * ), PVAL( * )
      REAL               RWORK( * )
      COMPLEX            A( * ), AF( * ), B( * ), BF( * ), WORK( * ),
     $                   X( * )
*     ..
*
*  =====================================================================
*
*     .. Parameters ..
      INTEGER            NTYPES
      PARAMETER          ( NTYPES = 8 )
*     ..
*     .. Local Scalars ..
      LOGICAL            FIRSTT
      CHARACTER          DISTA, DISTB, TYPE
      CHARACTER*3        PATH
      INTEGER            I, IINFO, IK, IMAT, KLA, KLB, KUA, KUB, LDA,
     $                   LDB, LWORK, M, MODEA, MODEB, N, NFAIL, NRUN, P
      REAL               ANORM, BNORM, CNDNMA, CNDNMB, RESID
*     ..
*     .. Local Arrays ..
      LOGICAL            DOTYPE( NTYPES )
*     ..
*     .. External Functions ..
      COMPLEX            CLARND
      EXTERNAL           CLARND
*     ..
*     .. External Subroutines ..
      EXTERNAL           ALAHDG, ALAREQ, ALASUM, CGLMTS, CLATMS, SLATB9
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          ABS
*     ..
*     .. Executable Statements ..
*
*     Initialize constants.
*
      PATH( 1: 3 ) = 'GLM'
      INFO = 0
      NRUN = 0
      NFAIL = 0
      FIRSTT = .TRUE.
      CALL ALAREQ( PATH, NMATS, DOTYPE, NTYPES, NIN, NOUT )
      LDA = NMAX
      LDB = NMAX
      LWORK = NMAX*NMAX
*
*     Check for valid input values.
*
      DO 10 IK = 1, NN
         M = MVAL( IK )
         P = PVAL( IK )
         N = NVAL( IK )
         IF( M.GT.N .OR. N.GT.M+P ) THEN
            IF( FIRSTT ) THEN
               WRITE( NOUT, FMT = * )
               FIRSTT = .FALSE.
            END IF
            WRITE( NOUT, FMT = 9997 )M, P, N
         END IF
   10 CONTINUE
      FIRSTT = .TRUE.
*
*     Do for each value of M in MVAL.
*
      DO 40 IK = 1, NN
         M = MVAL( IK )
         P = PVAL( IK )
         N = NVAL( IK )
         IF( M.GT.N .OR. N.GT.M+P )
     $      GO TO 40
*
         DO 30 IMAT = 1, NTYPES
*
*           Do the tests only if DOTYPE( IMAT ) is true.
*
            IF( .NOT.DOTYPE( IMAT ) )
     $         GO TO 30
*
*           Set up parameters with SLATB9 and generate test
*           matrices A and B with CLATMS.
*
            CALL SLATB9( PATH, IMAT, M, P, N, TYPE, KLA, KUA, KLB, KUB,
     $                   ANORM, BNORM, MODEA, MODEB, CNDNMA, CNDNMB,
     $                   DISTA, DISTB )
*
            CALL CLATMS( N, M, DISTA, ISEED, TYPE, RWORK, MODEA, CNDNMA,
     $                   ANORM, KLA, KUA, 'No packing', A, LDA, WORK,
     $                   IINFO )
            IF( IINFO.NE.0 ) THEN
               WRITE( NOUT, FMT = 9999 )IINFO
               INFO = ABS( IINFO )
               GO TO 30
            END IF
*
            CALL CLATMS( N, P, DISTB, ISEED, TYPE, RWORK, MODEB, CNDNMB,
     $                   BNORM, KLB, KUB, 'No packing', B, LDB, WORK,
     $                   IINFO )
            IF( IINFO.NE.0 ) THEN
               WRITE( NOUT, FMT = 9999 )IINFO
               INFO = ABS( IINFO )
               GO TO 30
            END IF
*
*           Generate random left hand side vector of GLM
*
            DO 20 I = 1, N
               X( I ) = CLARND( 2, ISEED )
   20       CONTINUE
*
            CALL CGLMTS( N, M, P, A, AF, LDA, B, BF, LDB, X,
     $                   X( NMAX+1 ), X( 2*NMAX+1 ), X( 3*NMAX+1 ),
     $                   WORK, LWORK, RWORK, RESID )
*
*           Print information about the tests that did not
*           pass the threshold.
*
            IF( RESID.GE.THRESH ) THEN
               IF( NFAIL.EQ.0 .AND. FIRSTT ) THEN
                  FIRSTT = .FALSE.
                  CALL ALAHDG( NOUT, PATH )
               END IF
               WRITE( NOUT, FMT = 9998 )N, M, P, IMAT, 1, RESID
               NFAIL = NFAIL + 1
            END IF
            NRUN = NRUN + 1
*
   30    CONTINUE
   40 CONTINUE
*
*     Print a summary of the results.
*
      CALL ALASUM( PATH, NOUT, NFAIL, NRUN, 0 )
*
 9999 FORMAT( ' CLATMS in CCKGLM INFO = ', I5 )
 9998 FORMAT( ' N=', I4, ' M=', I4, ', P=', I4, ', type ', I2,
     $      ', test ', I2, ', ratio=', G13.6 )
 9997 FORMAT( ' *** Invalid input  for GLM:  M = ', I6, ', P = ', I6,
     $      ', N = ', I6, ';', / '     must satisfy M <= N <= M+P  ',
     $      '(this set of values will be skipped)' )
      RETURN
*
*     End of CCKGLM
*
      END


c data are 708 monthly values (59 years) in file co2_2017
      real y(708), season(708), trend(708), rw(708), work(732,7)
      real start, finish
      logical robust
      read(5,*)(y(i), i=1, 708)
      n=708
      np=12
      ns=35
      nt=19
      nl=13
      no=0
      ni=2
      nsjump=4
      ntjump=2
      nljump=2
      isdeg=1
      itdeg=1
      ildeg=1
      iterations = 10000

      call cpu_time(start)

      do 1001 i = 1, iterations

         call stl(y,n,np,ns,nt,nl,isdeg,itdeg,ildeg,nsjump,ntjump,
     &        nljump,ni,no,rw,season, trend, work)

 1001 continue

      call cpu_time(finish)

      print '("Time = ",f6.3," ms.")', 1000.0 * (finish-start)
     &     / iterations

      open(unit=99, file="output.csv", status='replace')

      do 2002 i = 1, 708
         d = y(i)
         s = season(i)
         t = trend(i)
         r = y(i) - season(i) - trend(i)
         write(unit=99,  FMT="(1x, E24.16, (3(',',E24.16)))") d, s, t, r
 2002 continue

      close(unit=99)

      end


module constants_mod

! use ISO_FORTRAN_ENV

integer, parameter :: N_NODES   = PP_N_NODES
integer, parameter :: N_DIMS    = PP_N_DIMS
integer, parameter :: N_VARS    = PP_N_VARS

! integer, parameter :: i1 = INT8
! integer, parameter :: i2 = INT16
! integer, parameter :: i4 = INT32
! integer, parameter :: i8 = INT64

! integer, parameter :: sp = REAL32
! integer, parameter :: dp = REAL64
! integer, parameter :: qp = REAL128

! integer, parameter :: i4 = selected_int_kind(15)
integer, parameter :: SP = selected_real_kind(7)
integer, parameter :: dp = selected_real_kind(15)
integer, parameter :: qp = selected_real_kind(31)

real(kind=dp), parameter :: PI = 4.0_dp * atan(1.0_dp)
real(kind=dp), parameter :: TOL = 4.0e-16_dp
real(kind=dp), parameter :: EPSIL = 4.0e-16_dp

!! direction ID codes
integer, parameter :: X_DIR = 1
integer, parameter :: Y_DIR = 2
integer, parameter :: Z_DIR = 3
integer, parameter :: W_DIR = 4

!! face ID codes
integer, parameter :: ZEN = 0
integer, parameter :: NOR = 1
integer, parameter :: SOU = 2
integer, parameter :: WES = 3
integer, parameter :: EAS = 4
integer, parameter :: FRO = 5
integer, parameter :: BAC = 6

!! corner ID codes
integer, parameter :: NWF = 1
integer, parameter :: SWF = 2
integer, parameter :: NEF = 3
integer, parameter :: SEF = 4

integer, parameter :: NWB = 5
integer, parameter :: SWB = 6
integer, parameter :: NEB = 7
integer, parameter :: SEB = 8

end module


PROGRAM MULTIMODEFLOQUET

  USE ATOMIC_PROPERTIES
  USE TYPES
  USE SUBINTERFACE
  USE SUBINTERFACE_LAPACK
  !USE FLOQUETINIT_
  USE ARRAYS
  USE FLOQUETINITINTERFACE

  IMPLICIT NONE
  TYPE(MODE),       DIMENSION(:),   ALLOCATABLE :: FIELDS
  TYPE(ATOM)                                       ID
  INTEGER,          DIMENSION(:),   ALLOCATABLE :: MODES_NUM
  INTEGER                                          TOTAL_FREQUENCIES,D_BARE
  INTEGER                                          INFO,m,INDEX0,r
  DOUBLE PRECISION, DIMENSION(:),   ALLOCATABLE :: ENERGY,E_FLOQUET,E_BARE
  COMPLEX*16,       DIMENSION(:,:), ALLOCATABLE :: H__,U_F,U_AUX,U_B2D,H_BARE
  DOUBLE PRECISION, DIMENSION(:,:), ALLOCATABLE :: P_AVG
  DOUBLE PRECISION                              :: T1,T2

  INTEGER index

OPEN(UNIT=3,FILE="Hamiltonian_oscillation.dat",ACTION="WRITE")

  INFO = 0
  D_BARE = 33
  CALL FLOQUETINIT(ID,'lattice',1.0D0*D_BARE,INFO)

  ALLOCATE(E_BARE(D_BARE))
  ALLOCATE(H_BARE(D_BARE,D_BARE))
  ALLOCATE(U_AUX(D_BARE,D_BARE))
  ALLOCATE(MODES_NUM(2))

  MODES_NUM(1) = 1 !(STATIC FIELD)
  MODES_NUM(2) = 1 !(DRIVING BY ONE HARMONIC)

  TOTAL_FREQUENCIES = SUM(MODES_NUM,1)
  ALLOCATE(FIELDS(TOTAL_FREQUENCIES))
  DO m=1,TOTAL_FREQUENCIES
     ALLOCATE(FIELDS(m)%V(D_BARE,D_BARE))
     FIELDS(m)%V = 0.0
  END DO

  ! STATIC COMPONENT OF THE HAMILTONIAN
  ! E.G. A 1D LATTICE WITH THREE SITES IN A UNIT CELL
  DO m=2,D_BARE,3 ! CENTRAL SITES
     FIELDS(1)%V(m,m)  = 0.0
  END DO
  DO m=1,D_BARE,3 ! LEFT SITES
     FIELDS(1)%V(m,m)  = -0.25
  END DO
  DO m=3,D_BARE,3 ! RIGHT SITES
     FIELDS(1)%V(m,m)  =  0.25
  END DO
  ! AND NEAR NEIGHBOURGH COUPLING
  DO m=1,D_BARE-1
     FIELDS(1)%V(m,m+1)  = 0.125*EXP(DCMPLX(0.0, 1.0)*COS(pi*2*PI*m/D_BARE))
     FIELDS(1)%V(m+1,m)  = 0.125*EXP(DCMPLX(0.0,-1.0)*COS(pi*2*PI*m/D_BARE))
  END DO
  FIELDS(1)%V(1,D_BARE) = 0.125 ! WITH PERIODIC BOUNDARY CONDITIONS
  FIELDS(1)%V(D_BARE,1) = 0.125 ! WITH PERIODIC BOUNDARY CONDITIONS
  FIELDS(1)%omega     = 0.0
  FIELDS(1)%N_Floquet = 0
  H_BARE = FIELDS(1)%V

  CALL LAPACK_FULLEIGENVALUES(FIELDS(1)%V,D_BARE,E_BARE,INFO)
!  U_AUX = FIELDS(1)%V
  U_aux = 0.0
  index = 1
  DO m=1,D_BARE/2,2
!     WRITE(*,*) 2.0*pi*m/D_bare,E_bare(m)!m*4*pi/D_bare - pi,E_BARE(m)
     U_AUX(:,index)  = FIELDS(1)%V(:,m)
     index = index + 1
  END DO
!  WRITE(*,*)
  DO m=D_BARE/2,2,-2
!     WRITE(*,*) 2.0*(pi-pi*(m-1)/D_bare),E_bare(m)!m*4*pi/D_bare - pi,E_BARE(m)
     U_AUX(:,index)  = FIELDS(1)%V(:,m)
     index = index + 1
  END DO
!  write(*,*)
  DO m=D_BARE/2+1,D_BARE,2
!     WRITE(*,*) 2.0*(pi*m/D_bare-pi/2),E_bare(m)!m*4*pi/D_bare - pi,E_BARE(m)
     U_AUX(:,index)  = FIELDS(1)%V(:,m)
     index = index + 1
  END DO
!  WRITE(*,*)
  DO m=D_BARE,D_BARE/2+2,-2!D_BARE/2+2,D_BARE,2
!     WRITE(*,*) 2.0*(2*pi-pi*(m-1)/D_bare - pi/2.0),E_bare(m)!m*4*pi/D_bare - pi,E_BARE(m)
     U_AUX(:,index)  = FIELDS(1)%V(:,m)
     index = index + 1
  END DO
!  write(*,*)
!  write(*,*)
  ! BUILD THE STATIC HAMILTONIANN IN THE DIAGONAL BASIS
  FIELDS(1)%V = MATMUL(TRANSPOSE(CONJG(U_AUX)),MATMUL(H_BARE,U_AUX))
!  CALL WRITE_MATRIX(REAL(FIELDS(1)%V))

!  DO m=1,D_BARE
!     write(*,*) real(FIELDS(1)%V(m,m))
!  END DO

!  CALL LAPACK_FULLEIGENVALUES(FIELDS(1)%V,D_BARE,E_BARE,INFO)
!  write(*,*)
!  write(*,*)
!  DO m=1,D_BARE
!     write(*,*) E_BARE(m)
!  END DO

  ! DEFINE HARMONICS COUPLINGS IN THE ORIGINAL BASIS: FIELD ONE
  DO m=1,D_BARE-1
     FIELDS(2)%V(m,m+1)  = 0.125/2
     FIELDS(2)%V(m+1,m)  = 0.125/2
  END DO
  FIELDS(2)%V(1,D_BARE) = 0.125/2 ! WITH PERIODIC BOUNDARY CONDITIONS
  FIELDS(2)%V(D_BARE,1) = 0.125/2 ! WITH PERIODIC BOUNDARY CONDITIONS
  FIELDS(2)%omega     = 0.5
  FIELDS(2)%N_Floquet = 6

  ! TRANSFORM TO THE BASIS OF EIGNSTATES OF THE STATIC SYSTEM
  FIELDS(2)%V = MATMUL(TRANSPOSE(CONJG(U_AUX)),MATMUL(FIELDS(2)%V,U_AUX))

!  write(*,*)
!  write(*,*)

  !CALL WRITE_MATRIX(REAL(FIELDS(2)%V))

  DO m=1,256,64
     ! --- SET DRIVING PARAMETERS
     FIELDS(2)%omega = 0.2 + (m-1.0)*0.6/256.0
     !write(*,*) total_frequencies,ID%ID_SYSTEM
     !--- FIND THE MULTIMODE FLOQUET SPECTRUM
     CALL MULTIMODEFLOQUETMATRIX(ID,size(modes_num,1),total_frequencies,MODES_NUM,FIELDS,INFO)
     !write(*,*)
!     write(*,*)

     !call write_matrix(real(h_floquet))
     ALLOCATE(E_FLOQUET(SIZE(H_FLOQUET,1)))
     ALLOCATE(U_F(SIZE(H_FLOQUET,1),SIZE(H_FLOQUET,1)))
     E_FLOQUET = 0.0
!     DO r=1,SIZE(H_FLOQUET,1)
!        write(*,*) real(H_FLOQUET(r,r))
!     END DO
!     write(*,*)
!     write(*,*)

     CALL LAPACK_FULLEIGENVALUES(H_FLOQUET,SIZE(H_FLOQUET,1),E_FLOQUET,INFO)
     U_F = H_FLOQUET ! FOURIER DECOMPOSITION OF THE STATES DRESSED BY MODE NUMBER
     DEALLOCATE(H_FLOQUET)
     DO r=1,SIZE(U_F,1)
        WRITE(3,*) FIELDS(2)%omega,r,E_FLOQUET(r)
     END DO

     DEALLOCATE(E_FLOQUET)
     DEALLOCATE(U_F)
  END DO

END PROGRAM MULTIMODEFLOQUET


real function calk(pi, hs, tp, gamma)
implicit none
  real, intent(in) :: pi, hs, tp, gamma
  calk = (5./(32.*pi)) * (hs**2)  * tp * (1 - 0.287 * log(gamma))
end function calk

real function tc(a,b,hs,f,pi,n)
  real, intent(in) :: a,b,hs,f,pi
  integer, intent(in) :: n
  real m2,tz
  m2 = (b-a)/n * f
  write(*,*) 'm2 fra funksjon er',m2
  tc = pi*hs/(2.*sqrt(m2))
  write(*,*) 'tz fra funksjon er',tc
end function tc


! Copyright (C) 2015-2017, Simone Marsili
! All rights reserved.
! License: BSD 3 clause

module scoring
  use kinds
  use constants
  implicit none
  private

  public :: gauge
  public :: compute_scores
  public :: print_scores

contains

  subroutine gauge(nvars,nclasses,fields,couplings)
    ! double-center the matrix of couplings for each pair of variables
    integer,    intent(in)    :: nvars,nclasses       ! nvars, nclasses
    real(kflt), intent(inout) :: fields(nclasses,nvars)
    real(kflt), intent(inout) :: couplings(nclasses,nclasses,nvars*(nvars-1)/2)
    real(kflt) :: mi(nclasses),mj(nclasses),mm
    integer    :: iv,jv
    integer    :: is,js
    integer    :: k

    k = 0
    do jv = 1,nvars-1
       do iv = jv+1,nvars
          k = k + 1

          mm = sum(couplings(:,:,k)) / real(nclasses**2)

          do is = 1,nclasses
             mi(is) = sum(couplings(is,:,k)) / real(nclasses)
             mj(is) = sum(couplings(:,is,k)) / real(nclasses)
          end do

          do js = 1,nclasses
             do is = 1,nclasses
                couplings(is,js,k) = &
                     couplings(is,js,k) - mi(is) - mj(js) + mm
             end do
          end do

       end do
    end do

  end subroutine gauge

  subroutine compute_scores(nvars,nclasses,couplings,scores)
    integer,    intent(in)  :: nvars,nclasses       ! nvars, nclasses
    real(kflt), intent(in)  :: couplings(nclasses,nclasses,nvars*(nvars-1)/2)
    real(kflt), intent(out) :: scores(nvars,nvars)

    integer :: iv,jv
    integer :: k

    scores = 0.0_kflt
    k = 0
    do jv = 1,nvars-1
       do iv = jv+1,nvars
          k = k + 1
          scores(iv,jv) = sqrt(sum(couplings(:,:,k)**2))
          scores(jv,iv) = scores(iv,jv)
       end do
    end do

    call apc_correction(nvars,scores)

  end subroutine compute_scores

  subroutine apc_correction(nvars,scores)
    integer,    intent(in)    :: nvars
    real(kflt), intent(inout) :: scores(nvars,nvars)
    real(kflt) :: mi(nvars),mj(nvars),mm
    integer    :: iv,jv

    mm = sum(scores) / real(nvars**2)
    do iv = 1,nvars
       mi(iv) = sum(scores(iv,:)) / real(nvars)
       mj(iv) = sum(scores(:,iv)) / real(nvars)
    end do

    do jv = 1,nvars
       do iv = jv,nvars
          scores(iv,jv) = scores(iv,jv) - mi(iv)*mj(jv)/mm
          scores(jv,iv) = scores(iv,jv)
       end do
    end do

  end subroutine apc_correction

  subroutine print_scores(nvars,scores, unt)
    integer,    intent(in)    :: nvars
    real(kflt), intent(inout) :: scores(nvars,nvars)
    integer, intent(in) :: unt
    integer :: iv,jv

    do iv = 1,nvars-1
       do jv = iv+1,nvars
          write(unt,'(i5,1x,i5,1x,f8.4)') iv, jv, scores(iv,jv)
       end do
    end do

  end subroutine print_scores

end module scoring


!---------------------------------------------------!
! Copyright (c) 2017 Shunsuke A. Sato               !
! Released under the MIT license                    !
! https://opensource.org/licenses/mit-license.php   !
!---------------------------------------------------!
module density_matrix
  use global_variables
  implicit none

  private
! temporary
  complex(zp),allocatable :: zdensity_matrix(:,:)

  public :: initialize_density_matrix &
           ,calc_occ_natural_orbital

contains
!-----------------------------------------------------------------------------------------
  subroutine initialize_density_matrix
    implicit none

    allocate(zdensity_matrix(0:Nx,0:Nx))

  end subroutine initialize_density_matrix
!-----------------------------------------------------------------------------------------
  subroutine calc_occ_natural_orbital(occ_nat)
    implicit none
    real(8),intent(out) :: occ_nat(Nx+1)
    integer :: ix,iy,iz
    complex(zp) :: zs
!LAPACK
    integer :: lwork
    integer :: Nmat
    complex(8),allocatable :: work_lp(:)
    real(8),allocatable :: rwork(:),w(:)
    integer :: info

    lwork = 60*(Nx+1)
    Nmat = Nx+1
    allocate(work_lp(lwork),rwork(3*Nmat-2),w(Nmat))

!$omp parallel do private(ix,iy,iz,zs)
    do ix = 0,Nx
      do iy = 0,Nx
        zs = 0d0
        do iz = 0,Nx
          zs = zs + zwfn(ix,iz)*conjg(zwfn(iy,iz))
        end do
        zs = zs * dx**2
        zdensity_matrix(ix,iy) = zs
      end do
    end do

    call zheev('N', 'U', Nx+1, zdensity_matrix(0:Nx,0:Nx), Nx+1, w, work_lp, lwork, rwork, info)

    do ix = 1,Nx+1
      occ_nat(ix) = w(Nx+2-ix)
    end do

  end subroutine calc_occ_natural_orbital
!-----------------------------------------------------------------------------------------
end module density_matrix


c
c -------------------------------------------------------------
c
      subroutine vrmc(qr,ql,rx,ixmin,ixmax,iflip,msize)

      implicit double precision (a-h,o-z)
      include "cuserdt.i"

      dimension qr(4,msize),ql(4,msize)
      dimension rx(4,msize), rlx(4), rrx(4), stateR(4), stateL(4)
      dimension rhor(msize),ur(msize),pr(msize),utr(msize)
      dimension rhol(msize),ul(msize),pl(msize),utl(msize)

      data      itno/2/, small/1.d-6/
c
c
c     # HLL vector Riemann solver
c     # Given primitive variables, compute
c     # conserved variables at interface by solving riemann problem
c     # only solve neighboring vertical interfaces
c
c      iflip = 1 indicates the normal velocity is in
c                qr(k,2) and the tangential in qr(k,3)
c            = 0 indicates the reverse.
c   done so a single riemann solver can be used for both columns and rows.
c
      if (ixmax .gt. msize) then
         write(6,*)" need to increase msize in vrm to ", ixmax
         stop
      endif
      smallP = 1.d-7

      inorm = 3 - iflip
      itan  = 2 + iflip
      ggm1 = gamma/(gamma-1.d0)

      do 10 k = ixmin, ixmax
         rhor(k) = qr(1,k)
         ur(k)   = qr(inorm,k)/qr(1,k)
         utr(k)  = qr(itan,k)/qr(1,k)
         pr(k)   = gamma1*(qr(4,k)-0.5d0*(qr(2,k)**2
     &             + qr(3,k)**2)/qr(1,k))
         pr(k) = max(pr(k),smallP)
         rhol(k) = ql(1,k)
         ul(k)   = ql(inorm,k)/ql(1,k)
         utl(k)  = ql(itan,k)/ql(1,k)
         pl(k)   = gamma1*(ql(4,k)-0.5d0*(ql(2,k)**2
     &             + ql(3,k)**2)/ql(1,k))
         pl(k) = max(pl(k),smallP)
  10     continue
c
      do 20 k = ixmin, ixmax
        aL  = dsqrt(gamma*pl(k)/rhol(k))
        aR  = dsqrt(gamma*pr(k)/rhor(k))

c  get Roe average speeds
        dd = rhol(k)/rhor(k)
        odd = 1.d0/(dd+1.d0)
        roeAvg_normvel = ur(k) + dd*ul(k)*odd
        roeAvg_tanvel  = utr(k) + dd*utl(k)*odd
        hL = (pl(k)*ggm1)/rhol(k) + 0.5d0*(ul(k)**2+utl(k)**2)
        hR = (pR(k)*ggm1)/rhor(k) + 0.5d0*(ur(k)**2+utr(k)**2)
        avg_H = (hR+dd*hL)*odd
        q2 = roeAvg_normvel**2 + roeAvg_tanvel**2
        roeAvg_ssp = sqrt(max(gamma1*(avg_H - 0.5d0*q2),1d-6))

        sL = min((ul(k) - aL), (roeAvg_normvel - roeAvg_ssp)) ! fancier option
        sR = max((ur(k) + aL), (roeAvg_normvel + roeAvg_ssp))
        sM = (rhor(k)*ur(k) * (sR-ur(k)) - rhol(k)*ul(k)*(sL-ul(k)) +
     .       pl(k)-pr(k)) / (rhor(k)*(sR-ur(k))-rhol(k)*(sL-ul(k)))

c       if (.not.((sL .le. sM) .and. (sM.le.sR))) then
c           write(*,*)"sM outside roe avg speeds"
c       endif

        sL = min((ul(k) - aL), (uR(k)-aR))
        sR = max((ul(k) + aL), (uR(k)+aR))
        sM = (rhor(k)*ur(k) * (sR-ur(k)) - rhol(k)*ul(k)*(sL-ul(k)) +
     .        pl(k)-pr(k)) / (rhor(k)*(sR-ur(k))-rhol(k)*(sL-ul(k)))

       if (.not.((sL .le. sM) .and. (sM.le.sR))) then
            write(*,*)"sM still outside more diffusive wave speeds"
        endif

        if (0.d0 .lt. sL) then   ! use left flux

         rx(1,k)     = rhol(k)*ul(k)
         rx(inorm,k) = rhol(k)*ul(k)*ul(k) + pl(k)
         rx(itan,k)  = rhol(k)*ul(k)*utl(k)
         etot        = .5d0*rhol(k)*(ul(k)*ul(k)+utl(k)*utl(k)) +
     .                  pl(k)/gamma1
         rx(4,k)     = ul(k)*(etot + pl(k))

        else if (0. .gt. sR) then

         rx(1,k)     = rhor(k)*ur(k)
         rx(inorm,k) = rhor(k)*ur(k)*ur(k) + pr(k)
         rx(itan,k)  = rhor(k)*ur(k)*utr(k)
         etot        = .5d0*rhor(k)*(ur(k)*ur(k)+utr(k)*utr(k)) +
     .                  pr(k)/gamma1
         rx(4,k)     = ur(k)*(etot + pr(k))

        else

         pStar  = rhol(k)*(ul(k)-sL)*(ul(k)-sM) + pl(k)

         if (sM .ge. 0.d0) then
            rlx(1)     = rhol(k)*ul(k)
            rlx(inorm) = rhol(k)*ul(k)*ul(k) + pl(k)
            rlx(itan)  = rhol(k)*ul(k)*utl(k)
            etotl        = .5d0*rhol(k)*(ul(k)*ul(k)+utl(k)*utl(k)) +
     .           pl(k)/gamma1
            rlx(4)     = ul(k)*(etotl + pl(k))

            omegaL = 1.d0/(sL-sM)
            rhoStar = omegaL*rhol(k)*(sL-ul(k))
            rhoUnStar = omegaL*(rhol(k)*(sL-ul(k))*ul(k) + pStar-pl(k))
            rhotanStar = omegaL*rhol(k)*(sL-ul(k))*utl(k)
            eStar = omegaL*((sL-ul(k))*etotL - pl(k)*ul(k) + pStar*sM)

            rx(1,k)     = rlx(1) + sL*(rhoStar - rhol(k))
            rx(inorm,k) = rlx(inorm) + sL*(rhoUnstar - rhol(k)*ul(k))
            rx(itan,k)  = rlx(itan)  + sL*(rhotanStar - rhol(k)*utl(k))
            rx(4,k)       = rlx(4) + sL*(eStar - etotL)

         else

            rrx(1)     = rhor(k)*ur(k)
            rrx(inorm) = rhor(k)*ur(k)*ur(k) + pr(k)
            rrx(itan)  = rhor(k)*ur(k)*utr(k)
            etotR        = .5d0*rhor(k)*(ur(k)*ur(k)+utr(k)*utr(k)) +
     .           pr(k)/gamma1
            rrx(4)     = ur(k)*(etotr + pr(k))

            omegaR = 1.d0/(sR-sM)
            rhoStar = omegaR*rhor(k)*(sR-ur(k))
            rhoUnStar = omegaR*(rhor(k)*(sR-ur(k))*ur(k) + pStar-pr(k))
            rhotanStar = omegaR*rhor(k)*(sR-ur(k))*utr(k)
            eStar = omegaR*((sR-ur(k))*etotR - pr(k)*uR(k) + pStar*sM)

            rx(1,k)     = rrx(1)     + sR*(rhoStar - rhor(k))
            rx(inorm,k) = rrx(inorm) + sR*(rhoUnstar - rhor(k)*ur(k))
            rx(itan,k)  = rrx(itan)  + sR*(rhotanStar - rhor(k)*utr(k))
            rx(4,k)       = rrx(4)     + sR*(eStar - etotR)

          endif
         endif

 20   continue

c
      return
      end


module Mod_MatVec4D
   implicit none
contains
   subroutine MatVec4D(matrix,vector,unkno)
      use typre
      implicit none
      real(rp), CONTIGUOUS, target :: matrix(:,:,:,:), vector(:,:), unkno(:,:)

      real(rp), pointer :: auxmat(:,:) => NULL(), auxvec(:) => NULL(), auxunkno(:) => NULL()

      integer(ip) :: s1,s2,s3,s4,s5,s6

      s1 = size(matrix,1)*size(matrix,2)
      s2 = size(matrix,3)*size(matrix,4)

      s3 = size(vector,1)*size(vector,2)
      S4 =size(unkno,1)*size(unkno,2)
      auxmat(1:s1,1:s2) => matrix
      auxvec(1:s3) => vector
      auxunkno(1:s4) => unkno(:,:)

      auxunkno = matmul(auxmat,auxvec)

   end subroutine
end module


c *****************************************************
c    dmhdengen.f
c 16 June., 2011   written by D. Kawata
c *****************************************************
c70*******************************************************************

      subroutine dmhdengen(npg,omg0,omz,hub,cnfw,rvir,zt,xcut,rt,fb
     &  ,flag)
      include './define.f'

      integer npg
      integer i,flag
      double precision omg0,omz,hub,cnfw,rvir,zt,xcut,rt,fb
      double precision rhoc,delc,facnfw
      double precision rp,zp
      double precision rhonfwsw
      external rhonfwsw

      rhoc = (3.0d0*HUB0*HUB0*hub*hub/(8.0d0*M_PI*G))
     &  *(omg0/omz)*((1.0d0+zt)**3)
      delc = (200.0d0/3.0d0)*(cnfw**3/(dlog(1.0d0+cnfw)
     &  -cnfw/(1.0d0+cnfw)))
      facnfw = delc*rhoc*(1.0d0-fb)

      do i=0,npg-1
        if(x_rzg(i).gt.0.0d0.or.z_rzg(i).gt.0.0d0) then
          rp=dsqrt(x_rzg(i)**2+z_rzg(i)**2)
        else
          rp=10.0d0**lri_rzg
        endif
        if(z_rzg(i).gt.0.0d0) then
          zp=z_rzg(i)
        else
          zp=10.0d0**lzi_rzg
        endif
        rho_rzg(i)=rhonfwsw(rp,cnfw,rvir,rt,flag)*facnfw
      enddo

      return
      end


!    Copyright (C) 2012 The SPEED FOUNDATION
!    Author: Ilario Mazzieri
!
!    This file is part of SPEED.
!
!    SPEED is free software; you can redistribute it and/or modify it
!    under the terms of the GNU Affero General Public License as
!    published by the Free Software Foundation, either version 3 of the
!    License, or (at your option) any later version.
!
!    SPEED is distributed in the hope that it will be useful, but
!    WITHOUT ANY WARRANTY; without even the implied warranty of
!    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
!    Affero General Public License for more details.
!
!    You should have received a copy of the GNU Affero General Public License
!    along with SPEED.  If not, see <http://www.gnu.org/licenses/>.

!> @brief Findes node coordinates in the reference element.
!! @author Ilario Mazzieri
!> @date September, 2013
!> @version 1.0
!> @param[in] nb_el number of elements
!> @param[in] A11 costant values for the bilinear map
!> @param[in] A12 costant values for the bilinear map
!> @param[in] A13 costant values for the bilinear map
!> @param[in] A21 costant values for the bilinear map
!> @param[in] A22 costant values for the bilinear map
!> @param[in] A23 costant values for the bilinear map
!> @param[in] A31 costant values for the bilinear map
!> @param[in] A32 costant values for the bilinear map
!> @param[in] A33 costant values for the bilinear map
!> @param[in] B11 costant values for the bilinear map
!> @param[in] B12 costant values for the bilinear map
!> @param[in] B13 costant values for the bilinear map
!> @param[in] B21 costant values for the bilinear map
!> @param[in] B22 costant values for the bilinear map
!> @param[in] B23 costant values for the bilinear map
!> @param[in] B31 costant values for the bilinear map
!> @param[in] B32 costant values for the bilinear map
!> @param[in] B33 costant values for the bilinear map
!> @param[in] GG1 costant values for the bilinear map
!> @param[in] GG2 costant values for the bilinear map
!> @param[in] GG3 costant values for the bilinear map
!> @param[in] DD1 costant values for the bilinear map
!> @param[in] DD2 costant values for the bilinear map
!> @param[in] DD3 costant values for the bilinear map
!> @param[in] x_true x-coordinate of the node
!> @param[in] y_true y-coordinate of the node
!> @param[in] z_true z-coordinate of the node
!> @param[in] highest  elevation of the elemnt
!> @param[in] elevation  maximal elevation
!> @param[out] el  element index
!> @param[out] xref  x-coordinate in the refernce element
!> @param[out] yref  y-coordinate in the refernce element
!> @param[out] zref  z-coordinate in the refernce element

      subroutine GET_PNT_POS_PGM(nb_el,&
                                 AA11,AA12,AA13,AA21,AA22,AA23,&
                                 AA31,AA32,AA33,BB11,BB12,BB13,&
                                 BB21,BB22,BB23,BB31,BB32,BB33,&
                                 GG1,GG2,GG3,DD1,DD2,DD3,&
                                 x_true,y_true,z_true,el,xref,yref,zref,&
                                 highest,elevation)

      implicit none

      integer*4 :: nb_el,el
      integer*4 :: inr,ie

      real*8 :: x_true,y_true,z_true,xref,yref,zref
      real*8 :: csi,eta,zeta,delta_csi,delta_eta,delta_zeta,dist,dist_min
      real*8 :: termX,termY,termZ,det_j
      real*8 :: dxdx,dxdy,dxdz
      real*8 :: dydx,dydy,dydz
      real*8 :: dzdx,dzdy,dzdz
      real*8 :: a11,a12,a13
      real*8 :: a21,a22,a23
      real*8 :: a31,a32,a33
      real*8 :: elevation

      real*8, dimension(nb_el) :: AA11,AA12,AA13
      real*8, dimension(nb_el) :: AA21,AA22,AA23
      real*8, dimension(nb_el) :: AA31,AA32,AA33
      real*8, dimension(nb_el) :: BB11,BB12,BB13
      real*8, dimension(nb_el) :: BB21,BB22,BB23
      real*8, dimension(nb_el) :: BB31,BB32,BB33
      real*8, dimension(nb_el) :: GG1,GG2,GG3
      real*8, dimension(nb_el) :: DD1,DD2,DD3
      real*8, dimension(nb_el) :: highest

      dist_min = 1.0E20
      xref = 0.0;      yref = 0.0;      zref = 0.0
      el = 1

      do ie = 1,nb_el
         if (highest(ie) .gt. elevation) then
            csi = 0.0;             eta = 0.0;     zeta = 0.0

            do inr = 1, 100
                dxdx = AA11(ie) + BB12(ie)*zeta + BB13(ie)*eta + GG1(ie)*eta*zeta
                dydx = AA21(ie) + BB22(ie)*zeta + BB23(ie)*eta + GG2(ie)*eta*zeta
                dzdx = AA31(ie) + BB32(ie)*zeta + BB33(ie)*eta + GG3(ie)*eta*zeta

                dxdy = AA12(ie) + BB11(ie)*zeta + BB13(ie)*csi + GG1(ie)*zeta*csi
                dydy = AA22(ie) + BB21(ie)*zeta + BB23(ie)*csi + GG2(ie)*zeta*csi
                dzdy = AA32(ie) + BB31(ie)*zeta + BB33(ie)*csi + GG3(ie)*zeta*csi

                dxdz = AA13(ie) + BB11(ie)*eta + BB12(ie)*csi  + GG1(ie)*csi*eta
                dydz = AA23(ie) + BB21(ie)*eta + BB22(ie)*csi  + GG2(ie)*csi*eta
                dzdz = AA33(ie) + BB31(ie)*eta + BB32(ie)*csi  + GG3(ie)*csi*eta

                det_j = dxdz * (dydx*dzdy - dzdx*dydy) - dydz * (dxdx*dzdy - dzdx*dxdy) &
                          + dzdz * (dxdx*dydy - dydx*dxdy)

                a11 = +dydy*dzdz -dydz*dzdy
                a12 = -dydx*dzdz +dydz*dzdx
                a13 = +dydx*dzdy -dydy*dzdx

                a21 = -dxdy*dzdz +dxdz*dzdy
                a22 = +dxdx*dzdz -dxdz*dzdx
                a23 = -dxdx*dzdy +dxdy*dzdx

                a31 = +dxdy*dydz -dxdz*dydy
                a32 = -dxdx*dydz +dxdz*dydx
                a33 = +dxdx*dydy -dxdy*dydx

                termX = AA11(ie)*csi + AA12(ie)*eta &
                   + AA13(ie)*zeta + BB11(ie)*eta*zeta &
                   + BB12(ie)*csi*zeta + BB13(ie)*csi*eta &
                   + GG1(ie)*csi*eta*zeta + DD1(ie) -x_true

                termY = AA21(ie)*csi + AA22(ie)*eta &
                   + AA23(ie)*zeta + BB21(ie)*eta*zeta &
                   + BB22(ie)*csi*zeta + BB23(ie)*csi*eta &
                   + GG2(ie)*csi*eta*zeta + DD2(ie) -y_true

                termZ = AA31(ie)*csi + AA32(ie)*eta &
                   + AA33(ie)*zeta + BB31(ie)*eta*zeta &
                   + BB32(ie)*csi*zeta + BB33(ie)*csi*eta &
                   + GG3(ie)*csi*eta*zeta + DD3(ie) -z_true

                    delta_csi  = -(a11*termX +a21*termY +a31*termZ) / det_j
                delta_eta  = -(a12*termX +a22*termY +a32*termZ) / det_j
                delta_zeta = -(a13*termX +a23*termY +a33*termZ) / det_j

                csi = csi + delta_csi
                eta = eta + delta_eta
                zeta = zeta + delta_zeta

                    if (csi.lt.-1.0) csi = -1.0
                if (csi.gt. 1.0) csi =  1.0
                if (eta.lt.-1.0) eta = -1.0
                if (eta.gt. 1.0) eta =  1.0
                if (zeta.lt.-1.0) zeta = -1.0
                if (zeta.gt. 1.0) zeta =  1.0

            enddo

                 dist = termX*termX +termY*termY +termZ*termZ

                 if (dist.lt.dist_min) then
                    el = ie
                    xref = csi
                    yref = eta
                    zref = zeta
                    dist_min = dist
                 endif

        endif !if (highest(ie).gt.elevation) then

      enddo !do ie = 1,nb_el

      return

      end subroutine GET_PNT_POS_PGM


cc Copyright (C) 2004-2009: Leslie Greengard and June-Yub Lee
cc Contact: greengard@cims.nyu.edu
cc
cc This software is being release%d under a FreeBSD license
cc (see license.txt in this directory).
cc Single-prec version Barnett 4/5/17
cc
      subroutine dirft2d1f(nj,xj,yj,cj, iflag, ms,mt,fk)
      implicit none
      integer nj, iflag, ms, mt
      real*4 xj(nj), yj(nj)
      complex*8 cj(nj), fk(-ms/2:(ms-1)/2,-mt/2:(mt-1)/2)
c     ------------------------------------------------------------------
c     direct computation of nonuniform FFT
c
c                   nj
c     fk(k1,k2) =  SUM cj(j) exp(+/-i k1 xj(j)) exp(+/-i k2 yj(j))
c                  j=1
c
c     for -ms/2 <= k1 <= (ms-1)/2, -mt/2 <= k2 <= (mt-1)/2
c
c     If (iflag .ge.0) the + sign is used in the exponential.
c     If (iflag .lt.0) the - sign is used in the exponential.
c
c***********************************************************************
      integer j, k1, k2
      complex*8 zf, cm1, z1n(-ms/2:(ms-1)/2)
c
      do k2 = -mt/2, (mt-1)/2
         do k1 = -ms/2, (ms-1)/2
            fk(k1,k2) = cmplx(0d0,0d0)
         enddo
      enddo
c
      do j = 1, nj
c
c     ----------------------------------------------------------
c     Precompute exponential for exp(+/-i k1 xj)
c     ----------------------------------------------------------
c
         if (iflag .ge. 0) then
            zf = cmplx(cos(xj(j)),+sin(xj(j)))
         else
            zf = cmplx(cos(xj(j)),-sin(xj(j)))
         endif
         z1n(0) = (1d0,0d0)
         do k1 = 1, (ms-1)/2
            z1n(k1) = zf*z1n(k1-1)
            z1n(-k1)= conjg(z1n(k1))
         enddo
         if (ms/2*2.eq.ms) z1n(-ms/2) = conjg(zf*z1n(ms/2-1))
c
c     ----------------------------------------------------------
c     Loop over k2 for yj
c     ----------------------------------------------------------
         if (iflag .ge. 0) then
            zf = cmplx(cos(yj(j)),+sin(yj(j)))
         else
            zf = cmplx(cos(yj(j)),-sin(yj(j)))
         endif
c
         cm1 = cj(j)
         do k2 = 0, (mt-1)/2
            do k1 = -ms/2, (ms-1)/2
              fk(k1,k2) = fk(k1,k2) + cm1*z1n(k1)
            enddo
            cm1 = cm1*zf
         enddo
c
         zf = conjg(zf)
         cm1 = cj(j)
         do k2 = -1, -mt/2, -1
            cm1 = cm1*zf
            do k1 = -ms/2, (ms-1)/2
              fk(k1,k2) = fk(k1,k2) + cm1*z1n(k1)
            enddo
         enddo
      enddo
      end
c
c
c
c
c
c************************************************************************
      subroutine dirft2d2f(nj,xj,yj,cj, iflag, ms,mt,fk)
      implicit none
      integer nj, iflag, ms, mt
      real*4 xj(nj), yj(nj)
      complex*8 cj(nj), fk(-ms/2:(ms-1)/2,-mt/2:(mt-1)/2)
c     ----------------------------------------------------------------------
c     direct computation of nonuniform FFT
c
c
c     cj(j) = SUM  SUM  fk(k1,k2) exp(+/-i k1 xj(j)) exp(+/-i k2 yj(j))
c             k1   k2
c
c                            for j = 1,...,nj
c
c     where -ms/2 <= k1 <= (ms-1)/2, -mt/2 <= k2 <= (mt-1)/2
c
c
c     If (iflag .ge.0) the + sign is used in the exponential.
c     If (iflag .lt.0) the - sign is used in the exponential.
************************************************************************
      integer j, k1, k2
      complex*8 zf, cm1, cm2, z1n(-ms/2:(ms-1)/2)
c
      do j = 1, nj
c
c     ----------------------------------------------------------
c     Precompute exponential for exp(+/-i k1 xj)
c     ----------------------------------------------------------
         if (iflag .ge. 0) then
            zf = cmplx(cos(xj(j)),+sin(xj(j)))
         else
            zf = cmplx(cos(xj(j)),-sin(xj(j)))
         endif
         z1n(0) = (1d0,0d0)
         do k1 = 1, (ms-1)/2
            z1n(k1) = zf*z1n(k1-1)
            z1n(-k1)= conjg(z1n(k1))
         enddo
         if (ms/2*2.eq.ms) z1n(-ms/2) = conjg(zf*z1n(ms/2-1))
         if (iflag .ge. 0) then
            zf = cmplx(cos(yj(j)),+sin(yj(j)))
         else
            zf = cmplx(cos(yj(j)),-sin(yj(j)))
         endif
c
         cm1 = (0d0, 0d0)
         do k1 = -ms/2, (ms-1)/2
           cm1 = cm1 + z1n(k1) * fk(k1,0)
         enddo
         cj(j) = cm1
c
c     ----------------------------------------------------------
c     Loop over k2 for yj
c     ----------------------------------------------------------
c
         cm2 = zf
         do k2 = 1, (mt-1)/2
            cm1 = (0d0, 0d0)
            do k1 = -ms/2, (ms-1)/2
              cm1 = cm1 + z1n(k1) * fk(k1,k2)
            enddo
            cj(j) = cj(j) + cm2 * cm1

            cm1 = (0d0, 0d0)
            do k1 = -ms/2, (ms-1)/2
              cm1 = cm1 + z1n(k1) * fk(k1,-k2)
            enddo
            cj(j) = cj(j) + conjg(cm2) * cm1
            cm2 = cm2*zf
         enddo
c
         if (mt/2*2.eq.mt) then
            cm1 = (0d0, 0d0)
            do k1 = -ms/2, (ms-1)/2
              cm1 = cm1 + z1n(k1) * fk(k1,-mt/2)
            enddo
            cj(j) = cj(j) + conjg(cm2) * cm1
         endif

      enddo
      end

************************************************************************
      subroutine dirft2d3f(nj,xj,yj,cj, iflag, nk,sk,tk,fk)
      implicit none
      integer nj, iflag, nk
      real*4 xj(nj), yj(nj), sk(nk), tk(nk)
      complex*8 cj(nj), fk(nk)
c ----------------------------------------------------------------------
c     direct computation of nonuniform FFT
c
c              nj
c     fk(k) = SUM cj(j) exp(+/-i s(k) xj(j)) exp(+/-i t(k) yj(j))
c             j=1
c
c                    for k = 1, ..., nk
c
c     If (iflag .ge.0) the + sign is used in the exponential.
c     If (iflag .lt.0) the - sign is used in the exponential.
c
************************************************************************
      integer k, j
      real*4 ssk, stk
c
      do k = 1, nk
         if (iflag .ge. 0) then
            ssk =  sk(k)
            stk =  tk(k)
         else
            ssk =  -sk(k)
            stk =  -tk(k)
         endif
c
         fk(k) = cmplx(0d0,0d0)
         do j = 1, nj
            fk(k) = fk(k) + cj(j) * cmplx
     &        ( cos(ssk*xj(j)+stk*yj(j)), sin(ssk*xj(j)+stk*yj(j)) )
         enddo
      enddo
      end


!	Estimate the cross information matrix for a complex sample.
!	This version is designed for random sampling of psu's within strata.
!	Sampling is with replacement.
!	npsu is the number of psus's per stratum.
!	nstratum is the number of strata.
!	psu indicates the psu of an observation.
!	stratum indicates the stratum of an observation.
!	stratify is .true. for stratified sampling.
!	usepsu is .true. for primary sampling units within strata.
!	grads is the array of observation gradients.
!	grads1 is another array of observation gradients.
!	obsweight contains observation weights.

function crossinformation_complex(npsu,nstratum,psu,stratum,stratify,usepsu,grads,grads1,obsweight)
implicit none
!
integer,intent(in)::nstratum
integer,intent(in),optional::npsu(:),psu(:),stratum(:)
logical,intent(in)::stratify,usepsu
real(kind=8),intent(in)::grads(:,:),grads1(:,:),obsweight(:)
real(kind=8)::crossinformation_complex(size(grads,1),size(grads1,1))
!	al indicates allocation error.
!	col counts columns.
!	col1 counts columns.
!	counts count strata.
!	locpsu is psu locator.
!	obs is an observation counter.
!	point indicates the location of a psu.
!	row counts rows.
!	row1 counts rows.
integer::al,col,col1,counts(nstratum),locpsu(nstratum),obs,point,row,row1

!	diff is for differences for grads.
!	diff1 is for differences for grads1.
!	gradstrat is used for weighted summations within strata for grads.
!	grad1strat is used for weighted summations with strata for grads1
!	crossinform is used for cross information within a psu.

!	ratio is a ratio.

real(kind=8)::diff(size(grads,1)),diff1(size(grads1,1)),&
	gradstrat(size(grads,1)),grad1strat(size(grads1,1)),&
	crossinform(size(grads,1),size(grads1,1)),ratio
!	gradsum is for weighted summations within psu units for grads.
!	grad1sum is for weighted summations within psu units for grads1.
!	crossinformst is used for cross information within a stratum.
real(kind=8),allocatable::gradsum(:,:),grad1sum(:,:),crossinformst(:,:,:)

crossinformation_complex=0.0_8
!	Something must be present.
if(count(obsweight>0.0_8)==0)return
counts=0
gradstrat=0.0_8
grad1strat=0.0_8

if(usepsu)then

	allocate(gradsum(size(grads,1),sum(npsu)),grad1sum(size(grads1,1),sum(npsu)),stat=al)
	if(al/=0) stop "Unable to allocate arrays for asymptotic variances computations for complex sampling."
	gradsum=0.0_8
	grad1sum=0.0_8
	point=1
	do row=1,nstratum
		locpsu(row)=point
		point=point+npsu(row)
	end do
	if(stratify) then
		do obs=1,size(stratum)
			if(obsweight(obs)<=0.0_8) cycle
			point=locpsu(stratum(obs))+psu(obs)-1
			gradsum(:,point)=0.0_8
			grad1sum(:,point)=0.0_8
			do row=1,size(grads,1)
				if(grads(row,obs)/=0.0_8)gradsum(row,point)=gradsum(row,point)+obsweight(obs)*grads(row,obs)
			end do
			do row=1,size(grads1,1)
				if(grads1(row,obs)/=0.0_8)grad1sum(row,point)=grad1sum(row,point)+obsweight(obs)*grads1(row,obs)
			end do

		end do
		point=1
		do row=1,nstratum
			gradstrat=0.0_8
			do row1=1,size(grads,1)
				do col=point,point+npsu(row)-1
					if(gradsum(row1,col)/=0.0_8)gradstrat(row1)=gradstrat(row1)+gradsum(row1,col)
				end do
			end do
			gradstrat=gradstrat/npsu(row)
			grad1strat=0.0_8
			do row1=1,size(grads1,1)
				do col=point,point+npsu(row)-1
					if(grad1sum(row1,col)/=0.0_8)grad1strat(row1)=grad1strat(row1)+grad1sum(row1,col)
				end do
			end do
			grad1strat=grad1strat/npsu(row)

			crossinform=0.0_8
			ratio=1.0_8
			if(npsu(row)>1)ratio=npsu(row)/(npsu(row)-1.0_8)
			do col=1,npsu(row)
				diff=gradsum(:,point)-gradstrat
				diff1=grad1sum(:,point)-grad1strat
				do row1=1,size(grads,1)
					crossinform(row1,:)=crossinform(row1,:)&
						+diff(row1)*diff1
				end do
				point=point+1
			end do
			crossinformation_complex=crossinformation_complex+ratio*crossinform

		end do
	else
		do obs=1,size(psu)
			if(obsweight(obs)>0.0_8)then
				do row=1,size(grads,1)
					if(grads(row,obs)/=0.0_8)gradsum(row,psu(obs))=&
						gradsum(row,psu(obs))+obsweight(obs)*grads(row,obs)
				end do
				do row=1,size(grads1,1)
					if(grads1(row,obs)/=0.0_8)grad1sum(row,psu(obs))=&
						grad1sum(row,psu(obs))+obsweight(obs)*grads1(row,obs)
				end do

			end if
		end do
		gradstrat=0.0_8
		grad1strat=0.0_8
		do col=1,npsu(1)
			do row=1,size(grads,1)
				if(gradsum(row,col)/=0.0_8)gradstrat(row)=&
					gradstrat(row)+gradsum(row,col)
			end do
			do row=1,size(grads,1)
				if(grad1sum(row,col)/=0.0_8)grad1strat(row)=&
					grad1strat(row)+grad1sum(row,col)
			end do
		end do
		gradstrat=gradstrat/npsu(1)
		grad1strat=grad1strat/npsu(1)
		ratio=1.0_8
		if(npsu(1)>1)ratio=npsu(1)/(npsu(1)-1.0_8)

		do col=1,npsu(1)
			do row1=1,size(grads,1)
				diff(row1)=gradsum(row1,col)-gradstrat(row1)
			end do
			do row1=1,size(grads1,1)
				diff1(row1)=grad1sum(row1,col)-grad1strat(row1)
			end do
			do row=1,size(grads,1)
				if(diff(row)==0.0_8) cycle
				do row1=1,size(grads1,1)

					if(diff1(row1)/=0.0_8)crossinformation_complex(row,row1)&
						=crossinformation_complex(row,row1)+diff(row)*diff1(row1)
				end do
			end do
		end do
		crossinformation_complex=ratio*crossinformation_complex

	end if
else
	if(stratify) then
		allocate(gradsum(size(grads,1),nstratum),grad1sum(size(grads1,1),nstratum),&
			crossinformst(size(grads,1),size(grads1,1),nstratum),stat=al)
		if(al/=0) stop "Unable to allocate arrays for asymptotic variances computations for complex sampling."
		gradsum=0.0_8
		grad1sum=0.0_8
		crossinformst=0.0_8
		do obs=1,size(stratum)
			if(obsweight(obs)<=0.0_8)cycle
			gradsum(:,stratum(obs))=&
				gradsum(:,stratum(obs))+obsweight(obs)*grads(:,obs)
			grad1sum(:,stratum(obs))=&
				grad1sum(:,stratum(obs))+obsweight(obs)*grads1(:,obs)
			counts(stratum(obs))=counts(stratum(obs))+1
		end do
		do row=1,nstratum
			if(counts(row)>0)then
				gradsum(:,row)=gradsum(:,row)/counts(row)
				grad1sum(:,row)=grad1sum(:,row)/counts(row)
			end if
		end do
		do obs=1,size(stratum)
			if(obsweight(obs)<=0.0_8)cycle
			diff=obsweight(obs)*grads(:,obs)-gradsum(:,stratum(obs))
			diff1=obsweight(obs)*grads1(:,obs)-grad1sum(:,stratum(obs))
			do row=1,size(grads,1)
				if(diff(row)==0.0_8)cycle
				do col=1,size(grads1,1)
					if(diff1(col)/=0.0_8)crossinformst(row,col,stratum(obs))&
						=crossinformst(row,col,stratum(obs))+diff(row)*diff1(col)
				end do
			end do
		end do
		do row1=1,nstratum
			if(counts(row1)>1)then
				ratio=counts(row1)/(counts(row1)-1.0_8)
				crossinformation_complex=crossinformation_complex&
						+ratio*crossinformst(:,:,row1)

			end if
		end do
	else
		gradstrat=0.0_8
		do obs=1,size(obsweight)
			if(obsweight(obs)==0.0_8)cycle
			do row=1,size(grads,1)
				if(grads(row,obs)/=0.0_8)gradstrat(row)=gradstrat(row)+obsweight(obs)*grads(row,obs)
			end do
		end do
		gradstrat=gradstrat/count(obsweight>0.0_8)
		grad1strat=0.0_8
		do obs=1,size(obsweight)
			if(obsweight(obs)==0.0_8)cycle
			do row=1,size(grads,1)
				if(grads1(row,obs)/=0.0_8)grad1strat(row)=grad1strat(row)+obsweight(obs)*grads1(row,obs)
			end do
		end do
		grad1strat=grad1strat/count(obsweight>0.0_8)

		do obs=1,size(obsweight)
			if(obsweight(obs)<=0.0_8)cycle
			diff=obsweight(obs)*grads(:,obs)-gradstrat
			diff1=obsweight(obs)*grads1(:,obs)-grad1strat
			do row=1,size(grads,1)
				if(diff(row)==0.0_8)cycle
				do col=1,size(grads1,1)
					if(diff1(col)/=0.0_8)crossinformation_complex(row,col)&
						=crossinformation_complex(row,col)+diff(row)*diff1(col)
				end do
			end do
		end do
		ratio=1.0_8
		if(count(obsweight>0.0_8)>1)ratio=count(obsweight>0.0_8)/(count(obsweight>0.0_8)-1.0_8)
		crossinformation_complex=ratio*crossinformation_complex
	end if
end if

return
end function crossinformation_complex

!


SUBROUTINE PoissonSolver( Model,Solver,dt,TransientSimulation )
!------------------------------------------------------------------------------
!******************************************************************************
!
!  Solve the Poisson equation!
!
!  ARGUMENTS:
!
!  TYPE(Model_t) :: Model,
!     INPUT: All model information (mesh, materials, BCs, etc...)
!
!  TYPE(Solver_t) :: Solver
!     INPUT: Linear & nonlinear equation solver options
!
!  REAL(KIND=dp) :: dt,
!     INPUT: Timestep size for time dependent simulations
!
!  LOGICAL :: TransientSimulation
!     INPUT: Steady state or transient simulation
!
!******************************************************************************
  USE DefUtils

  IMPLICIT NONE
!------------------------------------------------------------------------------
  TYPE(Solver_t) :: Solver
  TYPE(Model_t) :: Model

  REAL(KIND=dp) :: dt
  LOGICAL :: TransientSimulation
!------------------------------------------------------------------------------
! Local variables
!------------------------------------------------------------------------------
  LOGICAL :: AllocationsDone = .FALSE., Found
  TYPE(Element_t),POINTER :: Element

  REAL(KIND=dp) :: Norm,  Energy
  INTEGER :: i,j,n, nb, nd, t, istat, Active

  TYPE(ValueList_t), POINTER :: BodyForce,BC
  REAL(KIND=dp), ALLOCATABLE :: STIFF(:,:), LOAD(:), FORCE(:), &
               NEWX(:), NEWY(:), POT(:)

  TYPE(Matrix_t), POINTER :: A
  TYPE(Mesh_t),   POINTER :: Mesh

  SAVE STIFF, LOAD, FORCE, POT, AllocationsDone
!------------------------------------------------------------------------------

  ! Allocate some permanent storage, this is done first time only:
  ! --------------------------------------------------------------
  Mesh => GetMesh()

  IF ( .NOT. AllocationsDone ) THEN
     N = Mesh % MaxElementDOFs
     ALLOCATE( FORCE(N), LOAD(N), STIFF(N,N), POT(N), STAT=istat )
     IF ( istat /= 0 ) THEN
        CALL Fatal( 'PoissonSolve', 'Memory allocation error.' )
     END IF
     AllocationsDone = .TRUE.
  END IF

   ! System assembly:
   ! ----------------
   CALL DefaultInitialize()
   active = GetNOFActive()
   DO t=1,active
      Element => GetActiveElement(t)
      n  = GetElementNOFNodes()
      nd = GetElementNOFDOFs()

      BodyForce => GetBodyForce()
      LOAD = 0.0d0
      IF ( ASSOCIATED(BodyForce) ) &
        Load(1:n) = GetReal( BodyForce, 'Source', Found )

      CALL LocalMatrix(  STIFF, FORCE, LOAD, Element, n, nd )
      CALL DefaultUpdateEquations( STIFF, FORCE )
   END DO
   CALL DefaultFinishAssembly()
   CALL DefaultDirichletBCs()
   Norm = DefaultSolve()

   Energy  = 0.0_dp
   LOAD    = 0.0_dp
   DO t=1,active
      Element => GetActiveElement(t)
      nd = GetElementNOFDOFs()
      n  = GetElementNOFNodes()
      CALL LocalMatrix( STIFF, FORCE, LOAD, Element, n, nd )
      CALL GetScalarLocalSolution(POT)
      Energy = Energy + SUM( POT*MATMUL(STIFF,POT) )
   END DO
   PRINT*,Energy
   A => GetMatrix()
   PRINT*,'DOFs, Error in energy: ',A % NumberOfRows, ABS(Energy-PI/2.0_dp)

CONTAINS

!------------------------------------------------------------------------------
  SUBROUTINE LocalMatrix(  STIFF, FORCE, LOAD, Element, n, nd )
!------------------------------------------------------------------------------
    REAL(KIND=dp) :: STIFF(:,:), FORCE(:), LOAD(:)
    INTEGER :: n, nd
    TYPE(Element_t), POINTER :: Element
!------------------------------------------------------------------------------
    REAL(KIND=dp) :: Basis(nd),dBasisdx(nd,3),ddBasisddx(nd,3,3),DetJ,LoadAtIP
    LOGICAL :: Stat
    INTEGER :: i,t
    TYPE(GaussIntegrationPoints_t) :: IP

    TYPE(Nodes_t) :: Nodes
    SAVE Nodes
!------------------------------------------------------------------------------
    CALL GetElementNodes( Nodes )
    STIFF = 0.0_dp
    FORCE = 0.0_dp

    !Numerical integration:
    !----------------------
    IP = GaussPoints( Element )
    DO t=1,IP % n
      ! Basis function values & derivatives at the integration point:
      !--------------------------------------------------------------
      stat = ElementInfo( Element, Nodes, IP % U(t), IP % V(t), &
                IP % W(t), detJ, Basis, dBasisdx )

      ! The source term at the integration point:
      !------------------------------------------
      LoadAtIP = SUM( Basis(1:n) * LOAD(1:n) )

      ! Finally, the elemental matrix & vector:
      !----------------------------------------
      STIFF(1:nd,1:nd) = STIFF(1:nd,1:nd) + IP % s(t) * DetJ * &
             MATMUL( dBasisdx, TRANSPOSE(dBasisdx) )
      FORCE(1:nd) = FORCE(1:nd) + IP % s(t) * DetJ * LoadAtIP * Basis(1:nd)
    END DO
!------------------------------------------------------------------------------
  END SUBROUTINE LocalMatrix
!------------------------------------------------------------------------------

!------------------------------------------------------------------------------
END SUBROUTINE PoissonSolver
!------------------------------------------------------------------------------

!------------------------------------------------------------------------------
FUNCTION CircX(Model,x,y,z) RESULT(s)
!------------------------------------------------------------------------------
  USE Types
  TYPE(Model_t) :: Model
  REAL(KIND=dp)::x,y,z,s,r

  r = SQRT(x**2 + y**2 )
  s = SQRT(2.0_dp)*x/r - x
!------------------------------------------------------------------------------
END FUNCTION CircX
!------------------------------------------------------------------------------

!------------------------------------------------------------------------------
FUNCTION CircY(Model,x,y,z) RESULT(s)
!------------------------------------------------------------------------------
  USE Types
  TYPE(Model_t) :: Model
  REAL(KIND=dp)::x,y,z,s,r

  r = SQRT(x**2 + y**2 )
  s = SQRT(2.0_dp)*y/r - y
!------------------------------------------------------------------------------
END FUNCTION CircY
!------------------------------------------------------------------------------


c
c-----------------------------------------------------------------------
c subroutine: iftasm
c compute idft for real, antisymmetric, n-point sequence x(m) using
c n/2-point fft
c antisymmetric sequence means x(m)=-x(n-m), m=1,...,n/2-1
c note: index m is sequence index--not fortran index
c-----------------------------------------------------------------------
c
      subroutine iftasm(x, n, y)
      dimension x(1), y(1)
c
c x = imaginary array which on input contains the n/2+1 real points of
c     the transform of the input--i.e. the zero valued real parts
c     are not given as input
c     on output x contains the n/2 points of the time sequence
c     (antisymmetrical)
c n = true size of input
c y = scratch array of size n/2+2
c
c
c for n = 2, assume x(1)=0, x(2)=0
c
      if (n.gt.2) go to 10
      x(1) = 0
      x(2) = 0
      return
  10  twopi = 8.*atan(1.0)
c
c first compute x1=x(1) term directly
c use recursion on the sine cosine terms
c
      no2 = n/2
      no4 = n/4
      tpn = twopi/float(n)
c
c scramble original dft (x(k)) to give y(k)
c use recursion relation to give sin(tpn*i) multiplier
c
      cosi = cos(tpn)
      sini = sin(tpn)
      cosd = cosi
      sind = sini
      nind = no4 + 1
      do 20 i=2,nind
        ind = 2*i
        ind1 = no2 + 2 - i
        ak = (x(i)-x(ind1))/2.
        bk = -(x(i)+x(ind1))
        y(ind) = ak
        y(ind-1) = bk*sini
        temp = cosi*cosd - sini*sind
        sini = cosi*sind + sini*cosd
        cosi = temp
  20  continue
      y(1) = 0.
      y(2) = 0.
c
c take n/2 point idft of y
c
      call fsst(y, no2)
c
c form x sequence from y sequence
c
      x(2) = y(1)/2.
      x(1) = 0.
      if (n.eq.4) go to 40
      do 30 i=2,no4
        ind = 2*i
        ind1 = no2 + 2 - i
        x(ind-1) = (y(i)-y(ind1))/2.
        t1 = (y(i)+y(ind1))/2.
        x(ind) = t1 + x(ind-2)
  30  continue
  40  x(no2) = -y(no4+1)/2.
      return
      end