module module_wavefun
  implicit none
  double precision                            :: Drmsd    = 0.0d0

! Convergence
contains

!#############################################
!#        Semicanonical Transformation
!#############################################
subroutine SemiCanonical()
  use module_data, only      : Fock,dim_1e,Spins,nOccA,nOccB,Eps
  use module_io, only        : print_Mat
  implicit none
  integer                         :: iSpin,nOcc,nVirt,lWork,i,j,inf
  double precision, allocatable   :: temp(:,:)
  double precision, allocatable   :: Vec(:)
  double precision, allocatable   :: work(:)

  !write(*,*) ""
  !write(*,*) "    Semicanonical Transformation ..."
  !write(*,*) ""

! Occupied-Occupied Block
  do iSpin=1,Spins
    if(iSpin==1)then
      nOcc=nOccA
    else
      nOcc=nOccB
    endif
    lwork = nOcc*(3+nOcc/2)
    allocate(work(lwork))
    allocate(temp(1:nOcc,1:nOcc),Vec(1:nOcc))

    temp(1:nOcc,1:nOcc) = Fock(1:nOcc,1:nOcc,iSpin)

    call dsyev('V','U',nOcc,temp(1:nOcc,1:nOcc),nOcc,Vec(1:nOcc),work,lwork,inf)

    temp = 0.0d0
    do i=1,nOcc
      temp(i,i) = Vec(i)
      Eps(i,iSpin) = Vec(i)
    enddo
    Fock(1:nOcc,1:nOcc,iSpin) = temp(1:nOcc,1:nOcc)

    deallocate(temp,vec,work)
  enddo

! Virtual-Virtual Block
  do iSpin=1,Spins
    if(iSpin==1)then
      nOcc  = nOccA
      nVirt = dim_1e-nOcc
    else
      nOcc=nOccB
      nVirt = dim_1e-nOcc
    endif
    lwork = nVirt*(3+nVirt/2)
    allocate(work(lwork))
    allocate(temp(1:nVirt,1:nVirt),Vec(1:nVirt))

    temp(1:nVirt,1:nVirt) = Fock(nOcc+1:dim_1e,nOcc+1:dim_1e,iSpin)

    call dsyev('V','U',nVirt,temp(1:nVirt,1:nVirt),nVirt,Vec(1:nVirt),work,lwork,inf)

    temp = 0.0d0
    do i=1,nVirt
      temp(i,i) = Vec(i)
      Eps(nOcc+i,iSpin) = Vec(i)
    enddo
    Fock(nOcc+1:dim_1e,nOcc+1:dim_1e,iSpin) = temp(1:nVirt,1:nVirt)

    deallocate(temp,vec,work)
  enddo

end subroutine SemiCanonical

!#############################################
!#       Transform Fockian to MO Basis
!#############################################
subroutine Fock_to_MO()
  use module_data, only      : Fock,Coef,dim_1e,Spins
  use module_io, only        : print_Mat
  implicit none
  integer                         :: i
  double precision, allocatable   :: temp(:,:)

!  write(*,*) ""
!  write(*,*) "    transform Fockian to MO-basis ..."
!  write(*,*) ""

  allocate(temp(dim_1e,dim_1e))

  do i=1,Spins
!  $::Fock = transpose($::Coef) x $::Fock x $::Coef;
!    call print_Mat(Fock(:,:,i),dim_1e,7,"AO Fock")
    call dgemm('N','N',dim_1e,dim_1e,dim_1e,1.0d0,  &
                Fock(1:dim_1e,1:dim_1e,i),dim_1e,   &
                Coef(1:dim_1e,1:dim_1e,i),dim_1e,   &
                0.0d0,temp(1:dim_1e,1:dim_1e),dim_1e)
    call dgemm('T','N',dim_1e,dim_1e,dim_1e,1.0d0,  &
                Coef(1:dim_1e,1:dim_1e,i),dim_1e,   &
                temp(1:dim_1e,1:dim_1e),dim_1e,     &
                0.0d0,Fock(1:dim_1e,1:dim_1e,i),dim_1e)
!    call print_Mat(Fock(:,:,i),dim_1e,7,"MO Fock")
  enddo

  deallocate(temp)

end subroutine Fock_to_MO

!#############################################
!#       Transform Fockian to AO Basis
!#############################################
subroutine Fock_to_AO()
  use module_data, only      : Fock,Coef,Sij,dim_1e,Spins
  use module_io, only        : print_Mat
  implicit none
  integer                         :: i
  double precision, allocatable   :: temp1(:,:)
  double precision, allocatable   :: temp2(:,:)
  double precision, allocatable   :: temp3(:,:)

!  write(*,*) ""
!  write(*,*) "    transform Fockian to AO-basis ..."
!  write(*,*) ""

  allocate(temp1(dim_1e,dim_1e))
  allocate(temp2(dim_1e,dim_1e))
  allocate(temp3(dim_1e,dim_1e))

  do i=1,Spins
!  $::Fock = $::Sij x $::Coeff x $::Fock x transpose($::Coeff) x $::Sij;
    call dgemm('N','N',dim_1e,dim_1e,dim_1e,1.0d0,    &
                Sij(1:dim_1e,1:dim_1e),dim_1e,        &
                Coef(1:dim_1e,1:dim_1e,i),dim_1e,     &
                0.0d0,temp1(1:dim_1e,1:dim_1e),dim_1e)
    call dgemm('T','N',dim_1e,dim_1e,dim_1e,1.0d0,    &
                Coef(1:dim_1e,1:dim_1e,i),dim_1e,     &
                Sij(1:dim_1e,1:dim_1e),dim_1e,        &
                0.0d0,temp2(1:dim_1e,1:dim_1e),dim_1e)
    call dgemm('N','N',dim_1e,dim_1e,dim_1e,1.0d0,    &
                Fock(1:dim_1e,1:dim_1e,i),dim_1e,     &
                temp2(1:dim_1e,1:dim_1e),dim_1e,      &
                0.0d0,temp3(1:dim_1e,1:dim_1e),dim_1e)
    call dgemm('N','N',dim_1e,dim_1e,dim_1e,1.0d0,    &
                temp1(1:dim_1e,1:dim_1e),dim_1e,      &
                temp3(1:dim_1e,1:dim_1e),dim_1e,      &
                0.0d0,Fock(1:dim_1e,1:dim_1e,i),dim_1e)
!    call print_Mat(Fock(:,:,i),dim_1e,7,"AO Fock")
  enddo

  deallocate(temp1,temp2,temp3)

end subroutine Fock_to_AO

!#############################################
!#              Initial Guess
!#############################################
subroutine do_guess()
  use module_data,      only : Fock,Hcore,Spins,dim_1e
  use module_data,      only : guess,nel,breaksymmetry
  use module_io,        only : print_Mat
  implicit none
  integer                   :: i

!  write(*,*) ""
!  write(*,*) "    Initial guess..."
!  write(*,*) ""

!  write(*,*) "guess = ", guess

  if(guess == "core")then
    write(*,*) ""
    write(*,*) "    core initial guess..."
    write(*,*) ""
    do i=1,Spins
      Fock(:,:,i) = Hcore(:,:)
      ! add random perturbation
      if(i>1 .and. breaksymmetry)then
          Fock(nel/2,nel/2,i) = Fock(nel/2,nel/2,i) + 1.0d-3
      endif
   enddo
  elseif(Guess == "huck")then
    call guess_huckel()
  endif

!  call print_Mat(Hcore(:,:),dim_1e,5,"Hcore")
!  call print_Mat(Fock(:,:,1),dim_1e,4,"Fock")

end subroutine do_guess

!#############################################
!#               Hückel Guess
!#############################################
subroutine guess_huckel()
  use module_data,          only : Fock,Hcore,Spins,dim_1e,Sij
  use module_data,          only : Guess,atom,Bastype,Basis
  use module_io,            only : print_Mat
  use module_constants,     only : CoreQ,eV2Ha
  implicit none
  integer                       :: iSpin,i,j,Zi,Zj
  double precision              :: K = 1.75d0
!  double precision              :: Hij
  double precision              :: Par_ii(8,3)

  write(*,*) ""
  write(*,*) "    Extended Hückel guess..."
  write(*,*) ""

!  allocate(Hii(dim_1e))
  Par_ii(1,1) = -13.06d0
  Par_ii(1,2) = 0.0d0
  Par_ii(1,3) = 0.0d0
  Par_ii(2,1) = -23.836940d0
  Par_ii(2,2) = 0.0d0
  Par_ii(2,3) = 0.0d0
  Par_ii(3,1) = -64.465135d0
  Par_ii(3,2) = -5.41d0
  Par_ii(3,3) = -3.61d0
  Par_ii(4,1) = -122.009435d0
  Par_ii(4,2) = -9.33d0
  Par_ii(4,3) = -5.88d0
  Par_ii(5,1) = -197.703359d0
  Par_ii(5,2) = -14.0d0
  Par_ii(5,3) = -8.24d0
  Par_ii(6,1) = -297.452179d0
  Par_ii(6,2) = -19.42d0
  Par_ii(6,3) = -10.7d0
  Par_ii(7,1) = -417.519956d0
  Par_ii(7,2) = -25.58d0
  Par_ii(7,3) = -13.25d0
  Par_ii(8,1) = -552.668469d0
  Par_ii(8,2) = -32.49d0
  Par_ii(8,3) = -15.88d0

  Par_ii = Par_ii*ev2Ha

  do iSpin=1,Spins
    do i=1,dim_1e
      do j=1,dim_1e
!        Fock(i,j,iSpin) = Hcore(i,j)
        Fock(i,j,iSpin) = 0.0d0
        call CoreQ(atom(Basis(i)),Zi)
        call CoreQ(atom(Basis(j)),Zj)

!        if(Bastype(i) > 1 .and. Bastype(j) > 1) then
!          Fock(i,j,iSpin)  = K * Sij(i,j)    &
!                           * (Par_ii(Zi,Bastype(i)) + Par_ii(Zj,Bastype(j)))/2.0d0
!        endif
        if(i == j) then
          Fock(i,j,iSpin)  = K * Sij(i,j)    &
                           * (Par_ii(Zi,Bastype(i)) + Par_ii(Zj,Bastype(j)))/2.0d0
        endif
      enddo
    enddo
  enddo

!  call print_Mat(Hcore(:,:),dim_1e,5,"Hcore")
!  call print_Mat(Fock(:,:,1),dim_1e,4,"Fock")

end subroutine guess_huckel

!#############################################
!#           Calculate Fock Matrix
!#############################################
subroutine calc_Fock()
  use module_data, only      : Dens,Fock,Spins,dim_1e,ERI,Hcore
  use module_data, only      : Sij,Par,Bastype,scaletype,Fockold,Basis
  use module_io, only        : print_Mat
  implicit none
  integer                         :: iSpin,i,j,k,l
  integer                         :: ij,kl,ijkl
  integer                         :: ik,jl,ikjl
!  double precision, allocatable   :: Fockold(:,:,:)
  double precision                :: ScaleFactor=1.0d0
  double precision                :: AddFactor=0.0d0
  double precision                :: TempPar

!  write(*,*) ""
!  write(*,*) "    Build new Fock..."
!  write(*,*) ""

!  allocate(Fockold(dim_1e,dim_1e,Spins))

  Fockold = Fock

  do iSpin=1,Spins
    Fock(1:dim_1e,1:dim_1e,iSpin) = Hcore(1:dim_1e,1:dim_1e)
  enddo

!$OMP PARALLEL PRIVATE(j,k,l,ij,kl,ik,jl,ijkl,ikjl,iSpin, &
!$OMP                  TempPar,ScaleFactor,AddFactor )
!$OMP DO
  do i=0,dim_1e-1
    do j=0,dim_1e-1
      do k=0,dim_1e-1
        do l=0,dim_1e-1
          ij = i*(i+1)/2 + j
          kl = k*(k+1)/2 + l
          ik = i*(i+1)/2 + k
          jl = j*(j+1)/2 + l
          if(j>i) ij = j*(j+1)/2 + i
          if(l>k) kl = l*(l+1)/2 + k
          if(k>i) ik = k*(k+1)/2 + i
          if(l>j) jl = l*(l+1)/2 + j
          ijkl = ij*(ij+1)/2 + kl +1
          ikjl = ik*(ik+1)/2 + jl +1
          if(kl>ij) ijkl = kl*(kl+1)/2 + ij +1
          if(jl>ik) ikjl = jl*(jl+1)/2 + ik +1

          ScaleFactor=1.0d0
          AddFactor=0.0d0

          if(scaletype==1)then
!            ScaleFactor = 1.0d0 - ((Par(Bastype(i+1))+Par(Bastype(j+1)))/2.0d0)*Sij(i+1,j+1)
            if(Bastype(i+1)==4 .or. Bastype(j+1)==4)then
              TempPar = 0.0d0
            elseif(Bastype(i+1)==3 .or. Bastype(j+1)==3)then
              TempPar = Par(3)
            elseif(Bastype(i+1)==2 .or. Bastype(j+1)==2)then
              TempPar = Par(2)
            elseif(Bastype(i+1)==1 .or. Bastype(j+1)==1)then
              TempPar = Par(1)
            endif
            ScaleFactor = 1.0d0 - (TempPar)*Sij(i+1,j+1)

          elseif(scaletype==2)then
            ScaleFactor = 1.0d0 -                                               &
                                  ((Par(Bastype(i+1))+Par(Bastype(j+1)))/2.0d0) &
                                 *Sij(i+1,j+1) *Sij(i+1,k+1) *Sij(i+1,l+1)      &
                                 *Sij(j+1,k+1) *Sij(j+1,l+1) *Sij(k+1,l+1)
          elseif(scaletype==3)then
            ScaleFactor = 1.0d0 -                                                  &
                                  ((Par(Bastype(i+1))+Par(Bastype(j+1)))/2.0d0)    &
                                 *(Sij(i+1,j+1) +Sij(i+1,k+1) +Sij(i+1,l+1)        &
                                 + Sij(j+1,k+1) +Sij(j+1,l+1) +Sij(k+1,l+1))/6.0d0
          elseif(scaletype==4)then
            ScaleFactor = 1.0d0 -                                                &
                                  ((Par(Bastype(i+1))+Par(Bastype(j+1)))/2.0d0)  &
                              *abs(Sij(i+1,j+1) *Sij(i+1,k+1) *Sij(i+1,l+1)      &
                                 * Sij(j+1,k+1) *Sij(j+1,l+1) *Sij(k+1,l+1))
          elseif(scaletype==5)then
            ScaleFactor = 1.0d0 -                                                 &
                                  ((Par(Bastype(i+1))+Par(Bastype(j+1)))/2.0d0)   &
                              *abs(Sij(i+1,j+1) *Sij(i+1,k+1) *Sij(i+1,l+1)       &
                                  *Sij(j+1,k+1) *Sij(j+1,l+1) *Sij(k+1,l+1))**(1.0d0/6.0d0)
          elseif(scaletype==6)then
            do iSpin=1,Spins
              ScaleFactor = 1.0d0 -                                                 &
                                    ((Par(Bastype(i+1))+Par(Bastype(j+1)))/2.0d0)   &
                                   *(Dens(i+1,j+1,iSpin) *Dens(i+1,k+1,iSpin)       &
                                   * Dens(i+1,l+1,iSpin) *Dens(j+1,k+1,iSpin)       &
                                   * Dens(j+1,l+1,iSpin) *Dens(k+1,l+1,iSpin))
            enddo
          elseif(scaletype==7)then
            do iSpin=1,Spins
              ScaleFactor = 1.0d0 -                                                 &
                                    ((Par(Bastype(i+1))+Par(Bastype(j+1)))/2.0d0)   &
                                   *(Dens(i+1,j+1,iSpin) *Dens(i+1,k+1,iSpin)       &
                                   * Dens(i+1,l+1,iSpin) *Dens(j+1,k+1,iSpin)       &
                                   * Dens(j+1,l+1,iSpin) *Dens(k+1,l+1,iSpin))      &
                                   *(Sij(i+1,j+1) *Sij(i+1,k+1) *Sij(i+1,l+1)       &
                                   * Sij(j+1,k+1) *Sij(j+1,l+1) *Sij(k+1,l+1))
            enddo
!          elseif(scaletype==8)then
!            ScaleFactor = 1.0d0 - (Par(1))*Sij(i+1,j+1)
!            AddFactor   = Par(2)*Sij(i+1,j+1)*Sij(k+1,l+1) + Par(3)
          elseif(scaletype==8)then
            ScaleFactor = 1.0d0 - (((Par(Bastype(i+1))+Par(Bastype(j+1)))/2.0d0))*Sij(i+1,j+1)*Sij(k+1,l+1)
!           AddFactor   = ((Par(Bastype(i+1))+Par(Bastype(i+1)))/2.0d0)*Sij(i+1,j+1)*Sij(k+1,l+1)
          elseif(scaletype==9)then
!           AddFactor   =   (Par(1))*Sij(i+1,j+1)*Sij(k+1,l+1)
!           AddFactor   =   ((Par(Bastype(i+1))+Par(Bastype(i+1)))/2.0d0)*Sij(i+1,j+1)*Sij(k+1,l+1)
!           write(*,*)  "",
!           AddFactor   =   -sqrt(abs(Par(Bastype(i+1))*Par(Bastype(j+1))))*Sij(i+1,j+1)*Sij(k+1,l+1)
!           AddFactor   = - Par(Bastype(i+1))*Par(Bastype(j+1))*Sij(i+1,j+1)*Sij(k+1,l+1)
           AddFactor   = -1.0d0*(Par(Bastype(i+1))*Par(Bastype(j+1)))*Sij(i+1,j+1)
          elseif(scaletype==10)then
            ScaleFactor = 0.0d0
            AddFactor   = 0.0d0
            if(Basis(i+1)==Basis(j+1) .and. Basis(i+1)==Basis(k+1) .and. Basis(i+1)==Basis(l+1))then
                ScaleFactor = 1.0d0
            endif
          endif

          !UHF
          if(Spins==2)then
            do iSpin=1,Spins
              Fock(i+1,j+1,iSpin) = Fock(i+1,j+1,iSpin)            &
                                  +(Dens(k+1,l+1,1)*ERI(ijkl)      &
                                  + Dens(k+1,l+1,2)*ERI(ijkl)      &
                                  - Dens(k+1,l+1,iSpin)*ERI(ikjl)) &
                                  * ScaleFactor + AddFactor
            enddo
          !RHF
          elseif(Spins==1)then
            Fock(i+1,j+1,1)       = Fock(i+1,j+1,1)                                 &
                                  +(Dens(k+1,l+1,1)*(2.0d0*ERI(ijkl) - ERI(ikjl)))  &
                                  * ScaleFactor + AddFactor
          endif
        enddo
      enddo

      if(scaletype==10)then
        if( Basis(i+1) /= Basis(j+1) )then
          Fock(i+1,j+1,1)     = 0.0d0
          Fock(i+1,j+1,Spins) = 0.0d0
        endif
      endif

    enddo
  enddo
!$OMP END DO
!$OMP END PARALLEL

  ! Damping
!  if(DoDamp)then
!    Fock = Damp*Fock + (1.0d0-Damp)*Fockold
!  endif

  !do iSpin=1,Spins
  !  call print_Mat(Fock(:,:,iSpin),dim_1e,4,"Fock")
  !enddo

!  deallocate(Fockold)

end subroutine calc_Fock

!#############################################
!#         Calculate Density Matrix
!#############################################
subroutine calc_Dens()
  use module_data, only      : Dens,Coef,Spins,dim_1e,Densold
  use module_data, only      : noccA,noccB,Occ,DoDamp,Damp,DynDamp
  use module_io, only        : print_Mat
  implicit none
  integer                         :: iSpin,i,j,k,l
!  double precision, allocatable   :: Densold(:,:,:)

!  allocate(Densold(dim_1e,dim_1e,Spins))
  Drmsd = 0.0d0
  Densold = Dens

!  write(*,*) ""
!  write(*,*) "    Calc density matrix"
!  write(*,*) ""

  do iSpin=1,Spins
    do i=1,dim_1e
      do j=1,i
        Dens(i,j,iSpin) = 0.0d0
        do k=1,dim_1e
          Dens(i,j,iSpin) = Dens(i,j,iSpin) + Occ(k,iSpin)*Coef(i,k,iSpin)*Coef(j,k,iSpin)*0.5d0*Spins
        enddo
        Dens(j,i,iSpin) = Dens(i,j,iSpin)
        Drmsd = Drmsd + (Dens(i,j,iSpin)-Densold(i,j,iSpin))**2
      enddo
    enddo
!    call print_Mat(Dens(:,:,iSpin),dim_1e,5,"DensX")
  enddo

  ! Damping
  if(DoDamp)then
    Dens = Damp*Dens + (1.0d0-Damp)*Densold
!    call calc_DynDamp()
  elseif(DynDamp)then
    call calc_DynDamp()
    Dens = Damp*Dens + (1.0d0-Damp)*Densold
  endif

  Drmsd = sqrt(Drmsd/dble(dim_1e*dim_1e*Spins))

end subroutine calc_Dens

!#############################################
!#       Calculate Dynamic Damping Factor
!#############################################
subroutine calc_DynDamp()
  use module_data, only      : Fock,Coef,dim_1e,Spins,Occ
  use module_data, only      : Dens,Densold,Hcore,Fockold,Damp
  implicit none
  double precision          :: E1a, E2a, E1b, E2b, E2ab, temp,tempa,tempb
  integer                   :: i,j,iSpin

  E1a  = 0.0d0
  E1b  = 0.0d0
  E2a  = 0.0d0
  E2b  = 0.0d0
  E2ab = 0.0d0

!        Eelec = Eelec + Dens(i,j,iSpin)                &
!                      *(Hcore(i,j) + Fock(i,j,iSpin))  &
!                      *(1.0d0/dble(Spins))

  do iSpin=1,Spins
    do i=1,dim_1e
      do j=1,dim_1e
        E1a   = E1a + Dens(i,j,iSpin)*(Hcore(i,j)*(1.0d0/dble(Spins)))
        E2a   = E2a + Dens(i,j,iSpin)*((Fock(i,j,iSpin))*(1.0d0/dble(Spins)))

        E1b   = E1b + Densold(i,j,iSpin)*(Hcore(i,j)*(1.0d0/dble(Spins)))
        E2b   = E2b + Densold(i,j,iSpin)*((Fockold(i,j,iSpin))*(1.0d0/dble(Spins)))

        E2ab  = E2ab + Dens(i,j,iSpin)*((Fockold(i,j,iSpin))*(1.0d0/dble(Spins)))

      enddo
    enddo
  enddo

  tempa = E1a + E2a
  tempb = E1b + E2b
  temp  = (E1a-E1b+E2ab-2.0d0*E2b)/(4.0d0*E2ab-2.0d0*E2a-2.0d0*E2b)
  damp = min(abs(temp),1.0d0)

!  write(*,*) E1a,"-",E1b,"+",E2ab,"-2",E2b
!  write(*,*) "4",E2ab,"-2",E2a,"-2",E2b
!  write(*,*) tempa,tempb,E2ab,temp

end subroutine calc_DynDamp

!#############################################
!#          Orthogonalize Fock
!#############################################
subroutine ort_fock()
  use module_data, only      : Fock,Coef,Sm12,dim_1e,Spins
  use module_io, only        : print_Mat
  implicit none
  integer                         :: i
  double precision, allocatable   :: temp(:,:)

!  write(*,*) ""
!  write(*,*) "    orthogonalizing Fock ..."
!  write(*,*) ""

  allocate(temp(dim_1e,dim_1e))

  do i=1,Spins
!  $::Coeff = transpose($::Sm12) x $::Fock x $::Sm12;
    call dgemm('N','N',dim_1e,dim_1e,dim_1e,1.0d0,  &
                Fock(1:dim_1e,1:dim_1e,i),dim_1e,   &
                Sm12(1:dim_1e,1:dim_1e),dim_1e,      &
                0.0d0,temp(1:dim_1e,1:dim_1e),dim_1e)
    call dgemm('T','N',dim_1e,dim_1e,dim_1e,1.0d0,  &
                Sm12(1:dim_1e,1:dim_1e),dim_1e,      &
                temp(1:dim_1e,1:dim_1e),dim_1e,     &
                0.0d0,Coef(1:dim_1e,1:dim_1e,i),dim_1e)
!    call print_Mat(Coef(:,:,i),dim_1e,8,"ort Fock")
  enddo

  deallocate(temp)

end subroutine ort_fock

!#############################################
!#            Deorthogonalize Fock
!#############################################
subroutine deort_fock()
  use module_data, only      : Coef,Sm12,dim_1e,Spins
  use module_io, only        : print_Mat
  implicit none
  integer                         :: i
  double precision, allocatable   :: temp(:,:)

!  write(*,*) ""
!  write(*,*) "    deorthogonalizing Fock ..."
!  write(*,*) ""

  allocate(temp(dim_1e,dim_1e))

  do i=1,Spins
!  $::Coeff = $::Sm12 x $::Coeff;
    temp(1:dim_1e,1:dim_1e) = coef(1:dim_1e,1:dim_1e,i)
    call dgemm('N','N',dim_1e,dim_1e,dim_1e,1.0d0,  &
                Sm12(1:dim_1e,1:dim_1e),dim_1e,      &
                temp(1:dim_1e,1:dim_1e),dim_1e,   &
                0.0d0,Coef(1:dim_1e,1:dim_1e,i),dim_1e)
!    call print_Mat(Coef(:,:,i),dim_1e,8,"deort WF")
  enddo

  deallocate(temp)

end subroutine deort_fock

!#############################################
!#            Orthogonalize Coef
!#############################################
subroutine ort_coef()
  use module_data, only      : Coef,S12,dim_1e,Spins
  use module_io, only        : print_Mat
  implicit none
  integer                         :: i
  double precision, allocatable   :: temp(:,:)

!  write(*,*) ""
!  write(*,*) "    orthogonalizing Coef ..."
!  write(*,*) ""

  allocate(temp(dim_1e,dim_1e))

  do i=1,Spins
!  $::Coeff = $::S12 x $::Coeff;
    temp(1:dim_1e,1:dim_1e) = coef(1:dim_1e,1:dim_1e,i)
    call dgemm('N','N',dim_1e,dim_1e,dim_1e,1.0d0,  &
                S12(1:dim_1e,1:dim_1e),dim_1e,      &
                temp(1:dim_1e,1:dim_1e),dim_1e,   &
                0.0d0,Coef(1:dim_1e,1:dim_1e,i),dim_1e)
!    call print_Mat(Coef(:,:,i),dim_1e,8,"deort WF")
  enddo

  deallocate(temp)

end subroutine ort_coef

!#############################################
!#          Diagonalize Fockian
!#############################################
subroutine dia_fock()
  use module_data, only      : Fock,Coef,Eps,dim_1e,Spins
  use module_io, only        : print_Mat,print_Vec
  implicit none
  integer                         :: i,j,lwork,inf
  double precision, allocatable   :: work(:)

  lwork = dim_1e*(3+dim_1e/2)
  allocate(work(lwork))

!  write(*,*) ""
!  write(*,*) "    Diagonalizing..."
!  write(*,*) ""

  do i=1,Spins
    call dsyev('V','U',dim_1e,Coef(1:dim_1e,1:dim_1e,i),dim_1e,Eps(1:dim_1e,i),work,lwork,inf)
!    call print_Mat(Coef(:,:,i),dim_1e,6,"ort WF")
  enddo

  do i=1,Spins
!    call print_Vec(Eps(:,i),dim_1e,11,"Eigenvalues")
  enddo

  deallocate(work)

end subroutine dia_fock

!#############################################
!#              Diagonalize 1-RDM
!#############################################
subroutine calc_natorbs()
! see Pulay + Hamilton JCP 88, 4926 (1988)
  use module_data, only      : Dens,NOCoef,NOEps,dim_1e,Spins,S12,nel,NoccA,NoccB
  use module_io, only        : print_Mat,print_Vec
  implicit none
  integer                         :: i,j,lwork,inf
  double precision, allocatable   :: work(:),temp(:,:)
  double precision                :: SpinQN,N,Na,Nb,sumocc2

  lwork = dim_1e*(3+dim_1e/2)
  allocate(work(lwork))
  allocate(NOCoef(1:dim_1e,1:dim_1e,Spins),NOEps(1:dim_1e,Spins),temp(1:dim_1e,1:dim_1e))

  NOCoef = 0.0d0
  NOEps  = 0.0d0
  temp   = 0.0d0

  write(*,*) ""
  write(*,*) "    Diagonalizing 1-RDM..."
  write(*,*) ""

  do i=1,Spins
    temp = temp + Dens(1:dim_1e,1:dim_1e,i)*2.0d0/dble(Spins)
  enddo

  call build_S12()

! S12 = S12 x D x S12
!  call print_Mat(S12(:,:),dim_1e,3,"S12")
!  call print_Mat(temp(:,:),dim_1e,4,"temp")
! D x S12
  call dgemm('N','N',dim_1e,dim_1e,dim_1e,1.0d0,temp,dim_1e,S12,dim_1e,0.0d0,NOCoef(:,:,1),dim_1e)
!  call print_Mat(NOCoef(:,:,1),dim_1e,7,"NO Coef")
  temp = NOCoef(:,:,1)
! S12 x temp
  call dgemm('N','N',dim_1e,dim_1e,dim_1e,1.0d0,S12,dim_1e,temp,dim_1e,0.0d0,NOCoef(:,:,1),dim_1e)
!  call print_Mat(NOCoef(:,:,1),dim_1e,7,"NO Coef")

  deallocate(temp)

!  do i=1,Spins
!  call print_Mat(Dens(:,:,1),dim_1e,5,"1-RDM")
  call dsyev('V','U',dim_1e,NOCoef(1:dim_1e,1:dim_1e,1),dim_1e,NOEps(1:dim_1e,1),work,lwork,inf)
!  call print_Mat(NOCoef(:,:,1),dim_1e,7,"NO Coef")
!  enddo

!  do i=1,Spins
  call print_Vec(NOEps(:,1),dim_1e,15,"(U)NO occ. num.")
!  enddo

  sumocc2 = 0.0d0
  do i=1,dim_1e
    sumocc2 = sumocc2 + NOEps(i,1)*NOEps(i,1)
  enddo

  N = dble(nel)
  Na = dble(NoccA)
  Nb = dble(NoccB)

  SpinQN = N*(N+4.0d0)/4.0d0 - Na*Nb - 0.5d0*sumocc2

  write(*,*) ""
  write(*,*) "    <S**2> = ",SpinQN
  write(*,*) ""

  deallocate(work)

end subroutine calc_natorbs

!#############################################
!#              Build S**1/2
!#############################################
subroutine build_S12()
  use module_data, only      : S12,Sij,dim_1e
  implicit none
  integer                         :: i,j,lwork,inf
  double precision, allocatable   :: work(:)
  double precision, allocatable   :: S_vec(:,:)
  double precision, allocatable   :: S_val(:)
  double precision, allocatable   :: S_mat(:,:)
  double precision, allocatable   :: temp(:,:)

  if(allocated(S12))then
    deallocate(S12)
  endif

  lwork = dim_1e*(3+dim_1e/2)
  allocate(work(lwork),            &
           S_vec(dim_1e,dim_1e),   &
           S_mat(dim_1e,dim_1e),   &
           S_val(dim_1e),          &
           S12(1:dim_1e,1:dim_1e), &
           temp(dim_1e,dim_1e))

!  write(*,*) ""
!  write(*,*) "    Diagonalizing S..."
!  write(*,*) ""

  S_vec = Sij
  call dsyev('V','U',dim_1e,S_vec(1:dim_1e,1:dim_1e),dim_1e,S_val(1:dim_1e),work,lwork,inf)

!  call print_Vec(S_val(:),dim_1e,11,"Ovrlpvalues")

  deallocate(work)

  S_val = S_val**(0.5d0)
! stretch matrix
  do i=1,dim_1e
    do j=1,dim_1e
      if(i == j)then
        S_mat(i,j) = S_val(i)
      else
        S_mat(i,j) = 0.0d0
      endif
    enddo
  enddo

! S12 = S_vec x S_mat x transpose(S_vec)
! temp = S_mat x transpose(S_vec)
  call dgemm('N','T',dim_1e,dim_1e,dim_1e,1.0d0,S_mat,dim_1e,S_vec,dim_1e,0.0d0,temp,dim_1e)
! S12 = S_vec x temp
  call dgemm('N','N',dim_1e,dim_1e,dim_1e,1.0d0,S_vec,dim_1e,temp,dim_1e,0.0d0,S12,dim_1e)

  deallocate(temp,S_vec,S_mat,S_val)

end subroutine build_S12

!#############################################
!#          Diagonalize Overlap
!#############################################
subroutine dia_S()
  use module_data, only      : Sm12,Sij,dim_1e
  implicit none
  integer                         :: i,j,lwork,inf
  double precision, allocatable   :: work(:)
  double precision, allocatable   :: S_vec(:,:)
  double precision, allocatable   :: S_val(:)
  double precision, allocatable   :: S_mat(:,:)
  double precision, allocatable   :: temp(:,:)

  lwork = dim_1e*(3+dim_1e/2)
  allocate(work(lwork),           &
           S_vec(dim_1e,dim_1e),  &
           S_mat(dim_1e,dim_1e),  &
           S_val(dim_1e),         &
           temp(dim_1e,dim_1e))

!  write(*,*) ""
!  write(*,*) "    Diagonalizing S..."
!  write(*,*) ""

  S_vec = Sij
  call dsyev('V','U',dim_1e,S_vec(1:dim_1e,1:dim_1e),dim_1e,S_val(1:dim_1e),work,lwork,inf)

!  call print_Vec(S_val(:),dim_1e,11,"Ovrlpvalues")

  deallocate(work)

  S_val = S_val**(-0.5d0)
! stretch matrix
  do i=1,dim_1e
    do j=1,dim_1e
      if(i == j)then
        S_mat(i,j) = S_val(i)
      else
        S_mat(i,j) = 0.0d0
      endif
    enddo
  enddo

! Sm12 = S_vec x S_mat x transpose(S_vec)
! temp = S_mat x transpose(S_vec)
  call dgemm('N','T',dim_1e,dim_1e,dim_1e,1.0d0,S_mat,dim_1e,S_vec,dim_1e,0.0d0,temp,dim_1e)
! S12 = S_vec x temp
  call dgemm('N','N',dim_1e,dim_1e,dim_1e,1.0d0,S_vec,dim_1e,temp,dim_1e,0.0d0,Sm12,dim_1e)

  deallocate(temp,S_vec,S_mat,S_val)

end subroutine dia_S

end module module_wavefun


************************************************************************
* Matrix-vector-multiplication
*
* This performs the following matrix vector multiplication:
*
*          DD:= C1*(A*DU) + C2*DD
*
* for the vectors  D=(D1,D2,DP)  and  U=(U1,U2,P)  with the
* given scalar variables C1,C2. The matrix A is the linearised
* nonlinear system matrix for velocity and pressure. Its structure
* is described in "smat2dns.inc"!
*
* In:
*   DU     - array [1..*] of double
*            Input vector
*   DD     - array [1..*] of double
*            Input vector
*   C1,
*   C2     - The constant multipliers
*   MATDAT - array [1..SZN2MI] of integer
*            TNS2DMatrixParams matrix structure specifying
*            the system matrix that is to multiply with.
* Out:
*   DD     = C1*(A*DU) + C2*DD
************************************************************************

      SUBROUTINE MTMUL2 (DU,DD,C1,C2,MATDAT)

      IMPLICIT NONE

      INCLUDE 'cmem.inc'

      INCLUDE 'cbasictria.inc'

      INCLUDE 'smat2dns.inc'

C     parameters

      DOUBLE PRECISION DD(*),DU(*),C1,C2
      INTEGER MATDAT(SZN2MI)

C     local variables

      INTEGER KA,KCOLA,KLDA,NEQA,NEQB
      INTEGER KB1,KCOLB1,KLDB1
      INTEGER KB2,KCOLB2,KLDB2

C     Calculation of  D1=C2*D1+C1*(A*U1+B1*P)

      NEQA  = MATDAT(ONEQA)
      NEQB  = MATDAT(ONEQB)

      KA    = L(MATDAT(OLA1))
      KCOLA = L(MATDAT(OLCLA1))
      KLDA  = L(MATDAT(OLLDA1))
      KB1    = L(MATDAT(OLB1))
      KCOLB1 = L(MATDAT(OLCLB1))
      KLDB1  = L(MATDAT(OLLDB1))

      CALL LAX17 (DWORK(KA),KWORK(KCOLA),KWORK(KLDA),NEQA,
     *            DU(1),DD(1),C1,C2)
      CALL LAX19 (DWORK(KB1),KWORK(KCOLB1),KWORK(KLDB1),NEQA,
     *            DU(1+2*NEQA),DD(1),C1,1D0)

C     Calculation of  D2=C2*D2+C1*(A*U2+B2*P)

      KA    = L(MATDAT(OLA2))
      KCOLA = L(MATDAT(OLCLA2))
      KLDA  = L(MATDAT(OLLDA2))
      KB2    = L(MATDAT(OLB2))
      KCOLB2 = L(MATDAT(OLCLB2))
      KLDB2  = L(MATDAT(OLLDB2))

      CALL LAX17 (DWORK(KA),KWORK(KCOLA),KWORK(KLDA),NEQA,
     *            DU(1+NEQA),DD(1+NEQA),C1,C2)
      CALL LAX19 (DWORK(KB2),KWORK(KCOLB2),KWORK(KLDB2),NEQA,
     *            DU(1+2*NEQA),DD(1+NEQA),C1,1D0)

C     Calculation of  DP=C2*DP+C1*(B1T*U1+B2T*U2)

      CALL LTX19 (DWORK(KB1),KWORK(KCOLB1),KWORK(KLDB1),
     *            NEQA,NEQB,DU(1),DD(1+2*NEQA),C1,C2)
      CALL LTX19 (DWORK(KB2),KWORK(KCOLB2),KWORK(KLDB2),
     *            NEQA,NEQB,DU(1+NEQA),DD(1+2*NEQA),C1,1D0)

      END


subroutine sumslightbias(dlight,lightges0,mype,nobs,nobs_loc,sum_loc)
!$$$  subprogram documentation block
!                .      .    .                                       .
! subprogram: sumslightbias  calculation of variance in parallel (MPI).

!   prgmmr: k apodaca <karina.apodaca@colostate.edu>
!      org: CSU/CIRA, Data Assimilation Group
!     date: 2016-09-08
!
! abstract:  This subroutine computes a local summation and the number of
!            observations assigned to a given mpi task (geographic region).
!
!            The former calculations are used as input in the bias correction
!            procedure applied to the nonlinear observation operator for
!            lightning flash rate, which is included in the "setuprhsall.f90"
!            subroutine.
!
! program history log:
!   2016-09-08  apodaca  -  first version of sumslightbias
!
!---
!
!   input argument list:
!     eps0  - guess value of lightning flash rate
!     mype  - mpi task id
!
!   output argument list:
!     sum_loc  - array containing  the summation, over all observations, of
!                the logarithmic transformation of the observed lightning
!                flash rate, divided by the forward model of lightning
!                flash rate.
!
!     nobs_loc - number of observations per given mpi task (CPU)
!
! attributes:
!   language: Fortran 90 and/or above
!   machine:
!
  use kinds, only: r_kind,r_single,r_double,i_kind
  use constants, only: zero
  implicit none

! Declare local variables

  real(r_kind)                    :: dlight
  real(r_kind)                    :: eps0
  real(r_kind)                    :: lightges0
  integer(i_kind)                 :: i,nobs
  integer(i_kind),intent(inout)   :: nobs_loc,mype
  real(r_kind),intent(out)        :: sum_loc

! File(s) for postprocessing
  character :: post_file*40

!---
! Online bias correction, as in Apodaca et al. (2014);
! eps = eps0 * exp[ (1/nobs) * sum(log(y/(eps0*h(x)))) / (1+r0/w0)]

! In this program, sum(log(y/(eps0*h(x)))) and the commulative count
! of lightning observations are calculated (nobs_loc).

!-- set initial bias parameter values

  eps0=1._r_kind

  sum_loc=zero
  nobs_loc=zero

!-- save sums here
!-- for each i there is an associated error(i), lightges0(i), and dlight(i)
!-- Step 1:  Estimate sums

! Open file with lightning output for local sums

  write(post_file,199)mype
199 format('sums_lfr_',i3.3,'.bin')
  open(unit=200,file=trim(post_file),form='formatted',action='read')

  do i=1,nobs

     read(200,*)nobs_loc,dlight,lightges0

     nobs=nobs_loc

     if(((eps0*lightges0) > 0_r_kind).and.(lightges0 > 0_r_kind).and.(dlight >0_r_kind)) then

        nobs_loc=nobs_loc+1
        sum_loc=sum_loc+log(dlight/(eps0*lightges0))

     endif

  enddo

close(unit=200)

end subroutine sumslightbias


! Monte Carlo simulation: jet-corrected luminosity
include 'num_pars.F90'
module global
implicit none
real*8,save :: Lb,v1,v2,Ep,norm
real*8,save :: AL,sigmaL,logLc
real*8,save :: A_ang,sigma_ang,log_angc
real*8,parameter :: omegam=0.3,omegal=0.7,h=0.71
end module global

program main
use global
use Numerical_Parameters, only : PI,RAD2DEGREES,M2CM
implicit none
integer*4,parameter :: nz=500,nL=500,num=4000000
integer*4 :: i,j,k,LM,idum,cnt(50),cnt2(50),idum2,cnt3(50)
real*8 :: z1,z2,ss,dl_m,dl_mpc,dtdz,dvdz,gammln,gammp,tot,tot2,L1,L2,LF2,sfr,ran1
real*8 :: x,y,k1,k2,Liso,ang1,ang2,ang,L_gam,psi,func,eta_z,eta_a
real*8,dimension(nz) :: z,tz,dndz,bin,f_metal
real*8,dimension(nL) :: logL,L,log_ang
real*8,dimension(num) :: z0,logL0,Fp,ang0
external ez,gammln,gammp,LF2,psi,band,bandE

!Calculate the pdf of redshift:
z1=0.001;z2=10.0
call tzt(z1,z2,nz,z,tz)
dndz=0.0
do i=1,nz
  call qromb(ez,0.0d0,z(i),ss)
  dl_m=9.26d25/h*(1.0+z(i))*ss  ! in units M
  dl_mpc=dl_m*3.24d-23 ! in Mpc
  dtdz=9.78d9/h/((1.0+z(i))*sqrt(omegal+omegam*(1.0+z(i))**3)) ! in yr
  dvdz=4.0*pi*3.065948d-7*dl_mpc**2/(1.0+z(i))*dtdz ! in Mpc^3
!  sfr=0.01*(1.0+z(i))**2.6/(1.0+((1.0+z(i))/3.2)**6.2) !SFR of Madau 2017
  if (z(i)<=1.0) then
    sfr=(1.0+z(i))**3.44
  else
    sfr=2.0**3.44
  end if
  f_metal(i)=gammp(0.84d0,0.4**2*10.0**(0.3*z(i)))
  dndz(i)=f_metal(i)*sfr*dvdz/(1+z(i))
enddo
call trapz(z,dndz,nz,tot)
open(11,file="pdf_redshift.txt")
do i=1,nz
  write(11,"(f7.3,2e11.3)") z(i),dndz(i)/tot,dndz(i)
end do

! Generate redshift z0:
call random_seed()
!idum=-1257
i=1;cnt=0
do while (i<=num)
  call random_number(x)
  call random_number(y)
  x=10.0*x
  y=0.2*y
!    x=10.0*ran1(idum)
!    y=0.2*ran1(idum)
    call locate(z,nz,x,j)
    if (y<=dndz(j)/tot) then
      z0(i)=x
      do k=1,50
        if (x>=(k-1)*0.2 .and. x<k*0.2) then
          cnt(k)=cnt(k)+1
        end if
      end do
      i=i+1
    end if
end do
open(21,file='result/input_z_dis.txt')
do k=1,50
  write(21,*) (k-0.5)*0.2,cnt(k)/0.2/real(num)
end do

! Compute the pdf of the jet-corrected luminosity:
L1=46.0;L2=52.0  !log10(L_gam)
logL=(/((L1+(L2-L1)/(nL-1)*LM),LM=0,nL-1)/)

!1.log-normal function:
! Set parameters:
AL=1.0
sigmaL=0.4
logLc=49.69

call qromb(LF2,L1,L2,tot)
AL=1.0/tot
open(12,file="pdf_lf2.txt")
do i=1,nL
  write(12,*) logL(i),LF2(logL(i))
end do

!2.broken power law:
! Set parameters:
! norm=1.0
! Lb=log10(2.0d52)
! v1=0.2
! v2=1.4
! call qromb(LF,L1,L2,tot)
! norm=1.0/tot

! Generate the jet-corrected luminosity:
!idum=-1358
!call random_seed()
i=1;cnt=0
do while (i<=num)
  call random_number(x)
  call random_number(y)
    x=(L2-L1)*x+L1
!    x=(L2-L1)*ran1(idum)+L1
!    y=ran1(idum)
    call locate(logL,nL,x,j)
    if (y<=LF2(logL(j))) then
      logL0(i)=x   !log10(L_gam)
      do k=1,10
        if (x>=L1+(k-1)*0.5 .and. x<L1+k*0.5) then
          cnt(k)=cnt(k)+1
        end if
      end do
      i=i+1
    end if
end do
open(22,file='result/input_Lgam_dis.txt')
do k=1,10
  write(22,*) L1+(k-0.5)*0.5,cnt(k)/0.5/real(num)
end do

! Compute the pdf of the jet opening angle:
ang1=-2.5;ang2=-0.1  ! log10(theta/rad)
log_ang=(/((ang1+(ang2-ang1)/(nL-1)*LM),LM=0,nL-1)/)

! Set parameters:
A_ang=1.0
sigma_ang=0.6
log_angc=-1.27

call qromb(psi,ang1,ang2,tot)
A_ang=1.0/tot
open(13,file="pdf_angle.txt")
do i=1,nL
  write(13,*) log_ang(i),psi(log_ang(i))
end do

! Generate the jet opening angle::
!idum=-1388
!call random_seed()
i=1;cnt=0
do while (i<=num)
  call random_number(x)
  call random_number(y)
    x=(ang2-ang1)*x+ang1
    y=0.8*y
!    x=(ang2-ang1)*ran1(idum)+ang1
!    y=0.7*ran1(idum)
    call locate(log_ang,nL,x,j)
    if (y<=psi(log_ang(j))) then
      ang0(i)=10.0**x  !angle in rad
      do k=1,12
        if (x>=(ang1+(k-1)*0.2) .and. x<(ang1+k*0.2)) then
          cnt(k)=cnt(k)+1
        end if
      end do
      i=i+1
    end if
end do
open(14,file='result/input_angle.txt')
do k=1,12
  write(14,*) ang1+(k-0.5)*0.2,cnt(k)/0.2/real(num)
end do

write(*,*) "End step input"

! Compute the peak flux using z and Liso:
!idum=-1322
call random_seed()
open(15,file='result/total_jc_sample.txt')
open(16,file='result/exp_jc_sample.txt')
open(17,file='result/exp_ang_dis.txt')
k=0;cnt=0;cnt2=0
iloop: do i=1,num
  ! Compute the luminosity distance at redshift z0:
  call qromb(ez,0.0d0,z0(i),ss)
  dl_m=9.26d25/h*(1.0+z0(i))*ss  !in units M
  ! Calculate Epeak using L-Ep relation:
  L_gam=10.0**logL0(i)
  Liso=L_gam/(1.0-cos(ang0(i)))  !Liso in erg/s
  call random_number(y)
  y=0.9*y+0.1
!  y=0.9*ran1(idum)+0.1
  Ep=200.0*(Liso/1.0d52)**0.5/y/(1.0+z0(i))
  ! Compute k-correction:
  call qromb(band,1.5d1,1.5d2,k1)
  call qromb(bandE,1.0d0/(1.0+z0(i)),1.0d4/(1.0+z0(i)),k2)
  k2=k2*1.60219d-9  ! keV to erg
  Fp(i)=Liso/(4.0*PI*(dl_m*1.0d2)**2)*k1/k2
!  ang=ang0(i)*RAD2DEGREES ! rad to degree
  write(15,"(f7.3,e11.3,2f9.3)") z0(i),Liso,log10(ang0(i)),Fp(i)

! Calculate the detection probability:
  if (Fp(i)>0.2) then
    call random_number(x)
    if (x<func(Fp(i))) then
      eta_z=0.26+0.032*exp(1.61*log10(Fp(i)))
      call random_number(x)
      if (x<eta_z) then
        eta_a=1.0-cos(ang0(i))
        call random_number(x)
        if (x<0.2*eta_a) then
          k=k+1
          write(*,"(i4,f7.3,e11.3,2f10.3)") k,z0(i),Liso,log10(ang0(i)),Fp(i)
          write(16,"(f7.3,e11.3,2f10.3)") log10(1.0+z0(i)),Liso,log10(ang0(i)),Fp(i)
          do j=1,14
            if(log10(ang0(i))>=(ang1+(j-1)*0.2).and.log10(ang0(i))<(ang1+j*0.2)) then
              cnt(j)=cnt(j)+1
            end if
          end do
          do j=1,12
            if (log10(Liso)>=49.0+(j-1)*0.5 .and. log10(Liso)<49.0+j*0.5) then
              cnt2(j)=cnt2(j)+1
            end if
          end do
          do j=1,25
            if (log10(1.0+z0(i))>=(j-1)*0.04 .and. log10(1.0+z0(i))<j*0.04) then
              cnt3(j)=cnt3(j)+1
            end if
          end do
          if(k==150) exit
        end if
      end if
    end if
  end if
end do iloop

do j=1,14
  write(17,*) ang1+(j-0.5)*0.2,cnt(j)/real(k)
end do
open(18,file='result/exp_L_dis.txt')
do j=1,12
  write(18,*) 49.0+(j-0.5)*0.5,cnt2(j)/real(k)
end do
open(19,file='result/exp_z_dis.txt')
do j=1,25
  write(19,*) (j-0.5)*0.04,cnt3(j)/real(k)
end do

end

!------------------------------Functions----------------------------------
function ez(zprime)
use global, only : omegal, omegam
implicit none
real*8 zprime,ez
ez=1.0/sqrt(omegal+omegam*(1+zprime)**3)
end function

function band(E)
!band funciton for lgrb:
use global, only : Ep
implicit none
real*8 band,E,a,b

a=-1.0
b=-2.25

if (E<=(a-b)*Ep/(2.0+a)) then
	band=(E/100.0)**a*exp(-E*(2.0+a)/Ep)
else
	band=((a-b)*Ep/(100.0*(2.0+a)))**(a-b)*exp(b-a)*(E/100.0)**b
endif
end function

function bandE(E)
!band funciton for lgrb:
use global, only : Ep
implicit none
real*8 bandE,E,a,b

a=-1.0
b=-2.25

if (E<=(a-b)*Ep/(2.0+a)) then
	bandE=(E/100.0)**a*exp(-E*(2.0+a)/Ep)*E
else
	bandE=((a-b)*Ep/(100.0*(2.0+a)))**(a-b)*exp(b-a)*(E/100.0)**b*E
endif
end function

function LF(logL)
! borken power-law luminosity function
use global, only : Lb,v1,v2,norm
implicit none
real*8 :: LF,logL
if (logL<4.9d1) then
  LF=0.0
else if (logL<=Lb) then
  LF=norm*10.0**(-v1*(logL-Lb))
else
  LF=norm*10.0**(-v2*(logL-Lb))
end if
end function

function LF2(logL)
! log-normal luminosity function
use global, only : AL,sigmaL,logLc
use Numerical_Parameters, only : PI
implicit none
real*8 LF2,logL
LF2=AL/sqrt(2.0*pi)/sigmaL*exp(-(logL-logLc)**2/(2.0*sigmaL**2))
end function

function psi(log_ang)
! the intrinsic distribution of angle
use global, only : A_ang,sigma_ang,log_angc
use Numerical_Parameters, only : PI
implicit none
real*8 psi,log_ang
psi=A_ang/sqrt(2.0*pi)/sigma_ang*exp(-(log_ang-log_angc)**2/(2.0*sigma_ang**2))
end function

function func(P)
! detection probability
implicit none
real*8 func,P
if (P<0.45) then
!  func=5.0*P**3.85
  func=P**2.0
else
  func=0.67*(1.0-0.4/P)**0.52
end if
end function

include 'mathlib/trapzd.f90'
include 'mathlib/trapz.f90'
include 'mathlib/qromb.f90'
include 'mathlib/gammp.f90'
include 'mathlib/gammln.f90'
include 'mathlib/erf.f90'
include 'lib/tzt.f90'
include 'mathlib/polint.f90'
include 'mathlib/gcf.f90'
include 'mathlib/gser.f90'
include 'lib/locate.f90'
include 'mathlib/ran1.f90'


        SUBROUTINE BOUND(NMX,XR,ro)
C
        PARAMETER (IMX=41 , JMX=41)
C
        DIMENSION XR(NMX),ro(nmx)
        COMMON  /XXX/  XF(IMX,JMX),XC,YC,DX,DY
        COMMON  /POSIT/ XOLD,YOLD
        PI = 4.*ATAN(1.0)
c        fact=cos(yold*pi/180.)
        fact=1.0
        DO 10 I=1,NMX
        THETA= 2.*PI*FLOAT(I-1)/FLOAT(NMX)
        X=RO(i)/fact*COS(THETA)+XC +1.
        Y=RO(i)*SIN(THETA)+YC +1.
        IX=INT(X/DX)
        IY=INT(Y/DY)
        IX1=IX+1
        IY1=IY+1
        P=X/DX-FLOAT(IX)
        Q=Y/DY-FLOAT(IY)
        XR(I)=(1.-P)*(1.-Q)*XF(IX,IY) +(1.-P)*Q*XF(IX,IY+1)
     1      +  (1.-Q)*P*XF(IX+1,IY) + P*Q*XF(IX+1,IY+1)
c        PRINT*,'QLIU TEST, BOUND=',XR(I),XF(IX,IY),XF(IX,IY+1),
c     2          XF(IX+1,IY),XF(IX+1,IY+1),I

10     CONTINUE
         RETURN
         END


!***********************************/
!	Name:
!	Alias: UCalcInscribedCircleOfCell
!	Description:
!	Type:UnstructuredGrid
!	Input:Volume and Area
!	Output:Radius
!	Note:
!	Author:Akitaka Toyota
!	Date:2018.02.03
!	Update:-
!	Other:現時点でハイブリッド格子・3次元格子への対応は未定(準備はある)
!***********************************/
subroutine UCalcInscribedCircleOfCell(UG)
    use StructVar_Mod
    use LoopVar_Mod
    use FrequentOperation
    implicit none
    type(UnstructuredGrid), intent(inout) :: UG

    do iCell=1, UG%GI%RealCells
        UG%InscribedCircle(iCell) = 2.0d0/3.0d0*UG%GM%Volume(iCell)/(UG%GM%AverageWidth(iCell)) !AverageWidth = a+b+c !Three Edge Length
    end do

    return
end subroutine  UCalcInscribedCircleOfCell


!=============================================================================!
        subroutine genA(A, v)
!
!  Form the A matrix which multiplies U,x
!
!=============================================================================!
        use global
        use local
        implicit none

        real    :: A(ny*nx,ndof,ndof), v(ny*nx,ndof)
        real    :: fact1(ny*nx), fact2(ny*nx), gmsinv
!=============================================================================!

!.... Continuity equation

        A(:,1,1) = u1
        A(:,1,2) = rho
        A(:,1,3) = zero
        A(:,1,4) = zero
        A(:,1,5) = zero

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

!.... Momentum equation -- x_1

        fact1  = rhoinv / Re
        gmsinv = one / (gamma * Ma**2)

        A(:,2,1) = rhoinv * t * gmsinv
        A(:,2,2) = u1 - fact1 * g1lm - fact1 * two * g1mu
        A(:,2,3) = -fact1 * g2mu
        A(:,2,4) = -fact1 * g3mu
        A(:,2,5) = gmsinv - fact1 * dlm * divu - &
                   fact1 * dmu * two * S(:,1,1)

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

!.... Momentum equation -- x_2

        A(:,3,1) = zero
        A(:,3,2) = -fact1 * g2lm
        A(:,3,3) = u1 - fact1 * g1mu
        A(:,3,4) = zero
        A(:,3,5) = -fact1 * dmu * two * S(:,2,1)

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

!.... Momentum equation -- x_3

        A(:,4,1) = zero
        A(:,4,2) = -fact1 * g3lm
        A(:,4,3) = zero
        A(:,4,4) = u1 - fact1 * g1mu
        A(:,4,5) = -fact1 * dmu * two * S(:,3,1)

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

!.... Energy equation

        fact1 = gamma * rhoinv / (Pr * Re)
        fact2 = gamma * gamma1 * Ma**2 * rhoinv / Re

        A(:,5,1) =  zero
        A(:,5,2) =  gamma1 * t - fact2 * two * lm * divu - &
                    four * fact2 * mu * S(:,1,1)
        A(:,5,3) = -four * fact2 * mu * S(:,2,1)
        A(:,5,4) = -four * fact2 * mu * S(:,3,1)
        A(:,5,5) = u1 - fact1 * (g1con + dcon * gt(:,1))

!.... correct for the Lele & Poinsot boundary conditions

        if (linear.eq.0) call genA_n(A, v)
        if (linear.eq.1) call genA_l(A, v)

        return
        end

!=============================================================================!
        subroutine bcA(A)
!
!  WARNING: Not updated to new equations
!
!  Apply the viscous BC to outflow
!
!=============================================================================!
        use global
        use local
        implicit none

        real    :: A(ny*nx,ndof,ndof)
        real    :: fact1, fact2, fact3, gmsinv, ginv

        integer :: i, j, ji
!=============================================================================!
        fact1  = one / Re
        fact2  = one / Re
        gmsinv = one / (gamma * Ma**2)
        ginv   = one / gamma
!=============================================================================!
        write(*,*) 'WARNING: Not updated to new equations'

        do j = 1, ny
          i = 1
          ji = j + (i-1)*ny

          A(ji,2,2) = rho(ji) * u1(ji)
          A(ji,2,5) = rho(ji) * gmsinv

          A(ji,3,3) = rho(ji) * u1(ji)
          A(ji,3,5) = zero

          A(ji,4,4) = rho(ji) * u1(ji)
          A(ji,4,5) = zero

          A(ji,5,5) = rho(ji) * u1(ji) * ginv

          i = nx
          ji = j + (i-1)*ny

          A(ji,2,2) = rho(ji) * u1(ji)
          A(ji,2,5) = rho(ji) * gmsinv

          A(ji,3,3) = rho(ji) * u1(ji)
          A(ji,3,5) = zero

          A(ji,4,4) = rho(ji) * u1(ji)
          A(ji,4,5) = zero

          A(ji,5,5) = rho(ji) * u1(ji) * ginv

        end do

        return
        end