submodule(Mod_CauchyElement) SUPCauchyElementSGS
      !-----------------------------------------------------------------------
      !
      ! This routine computes the elemental stabilization for
      ! the three field linear formulation.
      ! Taken from 'A mixed three-field FE formulation for stress accurate
      !             analysis including the incompressible limit'
      !             Authors: Chiumenti, Cervera, Codina
      !             doi    : 10.1016/j.cma.2014.08.004
      !
      !-----------------------------------------------------------------------

   implicit none

contains

    !-------------------Tau_u matrix calculation----------

   module subroutine sup_elm_usgs_ES(e,ndime,tn,dvol,elmat)
      !-----------------------------------------------------------------------
      !
      ! This routine computes the elemental material matrix
      !    B*B_t,   (div y,div s)
      !
      !-----------------------------------------------------------------------
      implicit none

      class(FiniteElement), intent(in)    :: e
      integer(ip),          intent(in)    :: ndime,tn
      real(rp),             intent(in)    :: dvol
      real(rp),             intent(inout) :: elmat(tn,e%pnode,tn,e%pnode)
      integer(ip) :: idime,jdime,inode,jnode,i,j
      real(rp)    :: B_i(tn,ndime),B_j(ndime,tn)
      real(rp)    :: B_aux(tn,ndime)
      real(rp)    :: BB(tn,tn)

      do inode=1,e%pnode
          do jnode=1,e%pnode

              BB  = 0.0_rp

              call sld_calculate_FdN_tensor_lin(e,ndime,tn,inode,jnode,BB)

              elmat(1:tn,inode,1:tn,jnode) = elmat(1:tn,inode,1:tn,jnode)+BB(:,:)*dvol
          end do
      end do

   end subroutine sup_elm_usgs_ES

   module subroutine sup_elm_usgs_EP(e,ndime,tn,dvol,elmat)
      !-----------------------------------------------------------------------
      !
      ! This routine computes the elemental material matrix
      !    B*G, (div y, grad p)
      !
      !-----------------------------------------------------------------------
      implicit none

      class(FiniteElement), intent(in) :: e
      integer(ip), intent(in)          :: ndime,tn
      real(rp),    intent(in)          :: dvol
      real(rp),    intent(inout)       :: elmat(tn,e%pnode,1,e%pnode)
      integer(ip)                      :: idime,jdime,inode,jnode
      real(rp)                         :: B(tn,ndime),G(ndime)
      real(rp)                         :: BG(tn)

      do inode=1,e%pnode

          call sld_calculateB(e,ndime,tn,inode,B)

          do jnode=1,e%pnode

              call sld_calculateB_inditial(e,ndime,jnode,G)

              BG = matmul(B,G)

              elmat(1:tn,inode,1,jnode) = elmat(1:tn,inode,1,jnode) + BG(:)*dvol

          end do
      end do

   end subroutine sup_elm_usgs_EP

   module subroutine sup_elm_usgs_QS(e,ndime,tn,dvol,elmat)
      !-----------------------------------------------------------------------
      !
      ! This routine computes the elemental material matrix
      !    G*B
      !
      !-----------------------------------------------------------------------
      implicit none

      class(FiniteElement), intent(in) :: e
      integer(ip), intent(in)          :: ndime,tn
      real(rp),    intent(in)          :: dvol
      real(rp),    intent(inout)       :: elmat(1,e%pnode,tn,e%pnode)
      integer(ip)                      :: idime,jdime,inode,jnode
      real(rp)                         :: G(ndime),B(ndime,tn)
      real(rp)                         :: GB(tn)

      do inode=1,e%pnode
          do jnode=1,e%pnode

              GB = 0.0_rp

              !d/dX_i(q_h)
              call sld_calculate_dqFinv_vector_lin(e,ndime,tn,inode,jnode,GB)

              !Negative sign because of -div(S_ij)
              elmat(1,inode,1:tn,jnode) = elmat(1,inode,1:tn,jnode) - GB*dvol

          end do
      end do

   end subroutine sup_elm_usgs_QS

   module subroutine sup_elm_usgs_QP(e,ndime,tn,dvol,elmat)
      !-----------------------------------------------------------------------
      !
      ! This routine computes the elemental material matrix
      !    G_t*G
      !
      !-----------------------------------------------------------------------
      implicit none

      class(FiniteElement), intent(in) :: e
      integer(ip), intent(in)          :: ndime,tn
      real(rp),    intent(in)          :: dvol
      real(rp),    intent(inout)       :: elmat(1,e%pnode,1,e%pnode)
      integer(ip)                      :: idime,jdime,inode,jnode
      real(rp)                         :: GG,G_i(ndime),G_j(ndime)

      do inode=1,e%pnode

          call sld_calculateB_inditial(e,ndime,inode,G_i)
          do jnode=1,e%pnode

              call sld_calculateB_inditial(e,ndime,jnode,G_j)

              GG = dot_product(G_i,G_j)*dvol

              elmat(1,inode,1,jnode) = elmat(1,inode,1,jnode) + GG

          end do
      end do

   end subroutine sup_elm_usgs_QP

    !-------------------Tau_s matrix calculation----------

   module subroutine sup_elm_ssgs_VU(e,ndime,tn,dvol,c_dev,elmat)
      !-----------------------------------------------------------------------
      !
      ! This routine computes the elemental material matrix
      !    B_t*P*Ñ_s
      !
      !-----------------------------------------------------------------------
      implicit none

      class(FiniteElement), intent(in)    :: e
      integer(ip),          intent(in)    :: ndime,tn
      real(rp),             intent(in)    :: dvol
      real(rp),             intent(in)    :: c_dev(tn,tn)
      real(rp),             intent(inout) :: elmat(ndime,e%pnode,ndime,e%pnode)
      integer(ip) :: idime,jdime,inode,jnode
      real(rp)    :: CB(tn,ndime)
      real(rp)    :: B_j(tn,ndime),B_i(ndime,tn)
      real(rp)    :: B_aux(tn,ndime)
      real(rp)    :: BCB(ndime,ndime)

      do inode=1,e%pnode

          call sld_calculateB(e,ndime,tn,inode,B_aux)
          B_i   = transpose(B_aux)

          do jnode=1,e%pnode

              call sld_calculateB(e,ndime,tn,jnode,B_j)

              CB  = matmul(c_dev,B_j)
              BCB = matmul(B_i,CB)*dvol

              !Notice the minus sign
              elmat(1:ndime,inode,1:ndime,jnode) = elmat(1:ndime,inode,1:ndime,jnode) - BCB(:,:)

          end do
      end do

   end subroutine sup_elm_ssgs_VU

   module subroutine sup_elm_ssgs_VS(e,ndime,tn,dvol,P,elmat)
      !-----------------------------------------------------------------------
      !
      ! This routine computes the elemental material matrix
      !    B_t*P*Ñ_s
      !
      !-----------------------------------------------------------------------
      implicit none

      class(FiniteElement), intent(in)    :: e
      integer(ip),          intent(in)    :: ndime,tn
      real(rp),             intent(in)    :: P(tn,tn),dvol
      real(rp),             intent(inout) :: elmat(ndime,e%pnode,tn,e%pnode)
      integer(ip) :: idime,jdime,inode,jnode
      real(rp) :: B(ndime,tn)
      real(rp) :: auxB(tn,ndime)
      real(rp) :: BPN(ndime,tn)
      real(rp) :: ii(tn,tn),N(tn,tn)
      real(rp) :: PN(tn,tn)

      call getDiagTensor(ndime,tn,ii)

      do inode=1,e%pnode

          call sld_calculateB(e,ndime,tn,inode,auxB)
          B   = transpose(auxB)

          do jnode=1,e%pnode

              N = e%shape(jnode,e%igaus)*ii

              PN  = matmul(P,N)
              BPN = matmul(B,PN)*dvol

              elmat(1:ndime,inode,1:tn,jnode) = elmat(1:ndime,inode,1:tn,jnode) + BPN(1:ndime,1:tn)

          end do
      end do

   end subroutine sup_elm_ssgs_VS

   module subroutine sup_elm_ssgs_EU(e,ndime,tn,dvol,P,elmat)
      !-----------------------------------------------------------------------
      !
      ! This routine computes the elemental material matrix
      !    N_t*P*B,    (y,P:grad_s u)
      !
      !-----------------------------------------------------------------------
      implicit none

      class(FiniteElement), intent(in)    :: e
      integer(ip),          intent(in)    :: ndime,tn
      real(rp),             intent(in)    :: P(tn,tn),dvol
      real(rp),             intent(inout) :: elmat(tn,e%pnode,ndime,e%pnode)
      integer(ip) :: idime,jdime,inode,jnode
      real(rp)    :: ii(tn,tn)
      real(rp)    :: N(tn,tn)
      real(rp)    :: B(tn,ndime)
      real(rp)    :: PB(tn,ndime)
      real(rp)    :: NPB(tn,ndime)

      call getDiagTensor(ndime,tn,ii)

      do inode=1,e%pnode

          N    = e%shape(inode,e%igaus)*(ii)

          do jnode=1,e%pnode

              call sld_calculateB(e,ndime,tn,jnode,B)

              PB  = matmul(P,B)
              NPB = matmul(N,PB)*dvol

              elmat(1:tn,inode,1:ndime,jnode) = elmat(1:tn,inode,1:ndime,jnode) + NPB(:,:)

          end do
      end do

   end subroutine sup_elm_ssgs_EU

   module subroutine sup_elm_ssgs_ES(e,ndime,tn,dvol,D,elmat)
      !-----------------------------------------------------------------------
      !
      ! This routine computes the elemental material matrix
      !    -Ñ_s_t*D*Ñ_s ,     (y,D:s)
      !
      !-----------------------------------------------------------------------
      implicit none

      class(FiniteElement), intent(in)    :: e
      integer(ip),          intent(in)    :: ndime,tn
      real(rp),             intent(in)    :: D(tn,tn),dvol
      real(rp),             intent(inout) :: elmat(tn,e%pnode,tn,e%pnode)
      integer(ip) :: idime,jdime,inode,jnode
      real(rp)    :: ii(tn,tn)
      real(rp)    :: N_i(tn,tn) ,N_j(tn,tn)
      real(rp)    :: B(tn,ndime)
      real(rp)    :: NN(tn,tn)

      !call getDiagTensor(ndime,tn,ii)

      !do inode=1,e%pnode

      !    N_i   = e%shape(inode,e%igaus)*(ii)

      !    do jnode=1,e%pnode

      !        N_j  = e%shape(jnode,e%igaus)*ii

      !        DN   = matmul(D,N_j)
      !        NDN  = matmul(N_i,DN)*dvol

      !        !Notice the minus sign
      !        elmat(1:tn,inode,1:tn,jnode) = elmat(1:tn,inode,1:tn,jnode) - NDN(1:tn,1:tn)

      !    end do
      !end do

      call getDiagTensor(ndime,tn,ii)

      do inode=1,e%pnode

          ! xi_ij
          N_i = e%shape(inode,e%igaus)*ii

          do jnode=1,e%pnode

              N_j  = e%shape(jnode,e%igaus)*ii

              ! (xi_ij,S_ij)
              NN = matmul(D,N_j)
              NN = matmul(N_i,NN)*dvol

              elmat(1:tn,inode,1:tn,jnode) = elmat(1:tn,inode,1:tn,jnode) - NN(1:tn,1:tn)

          end do
      end do

   end subroutine sup_elm_ssgs_ES

   !-------------------Tau_p matrix calculation----------

   module subroutine sup_elm_psgs_VU(e,ndime,dvol,K,G,elmat)
      !-----------------------------------------------------------------------
      !
      ! This routine computes the elemental material matrix
      !    G_t*G
      !
      !-----------------------------------------------------------------------
      implicit none

      class(FiniteElement), intent(in) :: e
      integer(ip), intent(in)          :: ndime
      real(rp),    intent(in)          :: dvol,K,G
      real(rp),    intent(inout)       :: elmat(ndime,e%pnode,ndime,e%pnode)
      integer(ip)                      :: idime,jdime,inode,jnode,i,j
      real(rp)                         :: GG(ndime,ndime),G_i(ndime),G_j(ndime),C_vol

      C_vol=min(K,(2.0_rp/3.0_rp)*G)

      do inode=1,e%pnode

          call sld_calculateB_inditial(e,ndime,inode,G_i)
          do jnode=1,e%pnode

              call sld_calculateB_inditial(e,ndime,jnode,G_j)

              GG = 0.0_rp
              do i=1,ndime
                  do j=1,ndime
                      GG(i,j) = G_i(i)*G_j(j)
                  end do
              end do

              elmat(1:ndime,inode,1:ndime,jnode) = elmat(1:ndime,inode,1:ndime,jnode) - C_vol*GG*dvol

          end do
      end do

   end subroutine sup_elm_psgs_VU

   module subroutine sup_elm_psgs_VP(e,ndime,dvol,elmat)
      !-----------------------------------------------------------------------
      !
      ! This routine computes the elemental material matrix
      !    GN_p
      !
      !-----------------------------------------------------------------------
      implicit none

      class(FiniteElement), intent(in) :: e
      integer(ip), intent(in)          :: ndime
      real(rp),    intent(in)          :: dvol
      real(rp),    intent(inout)       :: elmat(ndime,e%pnode,1,e%pnode)
      integer(ip)                      :: idime,jdime,inode,jnode
      real(rp)                         :: G(ndime),GN(ndime)

      G  = 0.0_rp
      GN = 0.0_rp

      do inode=1,e%pnode

          call sld_calculateB_inditial(e,ndime,inode,G)

          do jnode=1,e%pnode

              GN = G*e%shape(jnode,e%igaus)*dvol

              elmat(1:ndime,inode,1,jnode) = elmat(1:ndime,inode,1,jnode) + GN(1:ndime)

          end do
      end do

   end subroutine sup_elm_psgs_VP

   module subroutine sup_elm_psgs_QU(e,ndime,dvol,elmat)
      !-----------------------------------------------------------------------
      !
      ! This routine computes the elemental material matrix
      !    N_iG_j
      !
      !-----------------------------------------------------------------------
      implicit none

      class(FiniteElement), intent(in) :: e
      integer(ip), intent(in)          :: ndime
      real(rp),    intent(in)          :: dvol
      real(rp),    intent(inout)       :: elmat(1,e%pnode,ndime,e%pnode)
      integer(ip)                      :: idime,jdime,inode,jnode
      real(rp)                         :: G(ndime),NG(ndime),N

      G  = 0.0_rp
      NG = 0.0_rp
      N  = 0.0_rp

      do inode=1,e%pnode
          N = e%shape(inode,e%igaus)
          do jnode=1,e%pnode

              call sld_calculateB_inditial(e,ndime,jnode,G)
              NG = N*G*dvol

              elmat(1,inode,1:ndime,jnode) = elmat(1,inode,1:ndime,jnode) + NG(1:ndime)

          end do
      end do

   end subroutine sup_elm_psgs_QU

   module subroutine sup_elm_psgs_QP(e,ndime,dvol,D_vol,elmat)
      !-----------------------------------------------------------------------
      !
      ! This routine computes the elemental material matrix
      !    -D_vol*N_j_t*N_i
      !
      !-----------------------------------------------------------------------
      implicit none

      class(FiniteElement), intent(in) :: e
      integer(ip), intent(in)          :: ndime
      real(rp),    intent(in)          :: dvol,D_vol
      real(rp),    intent(inout)       :: elmat(1,e%pnode,1,e%pnode)
      integer(ip)                      :: idime,jdime,inode,jnode
      real(rp)                         :: DNN,N_i,N_j

      do inode=1,e%pnode
          N_i = e%shape(inode,e%igaus)
          do jnode=1,e%pnode

              N_j = e%shape(jnode,e%igaus)
              !Notice the minus sign
              DNN = -D_vol*N_i*N_j*dvol

              elmat(1,inode,1,jnode) = elmat(1,inode,1,jnode) + DNN

          end do
      end do

   end subroutine sup_elm_psgs_QP

end submodule SUPCauchyElementSGS


!==========================================================================
elemental function gsw_latentheat_evap_ct (sa, ct)
!==========================================================================
!
! Calculates latent heat, or enthalpy, of evaporation.
!
! sa     : Absolute Salinity                               [g/kg]
! ct     : Conservative Temperature                        [deg C]
!
! gsw_latentheat_evaporation : latent heat of evaporation  [J/kg]
!--------------------------------------------------------------------------

use gsw_mod_teos10_constants, only : gsw_sfac

use gsw_mod_kinds

implicit none

real (r8), intent(in) :: sa, ct

real (r8) :: gsw_latentheat_evap_ct

real (r8), parameter :: c0 =   2.499065844825125e6_r8
real (r8), parameter :: c1 =  -1.544590633515099e-1_r8
real (r8), parameter :: c2 =  -9.096800915831875e4_r8
real (r8), parameter :: c3 =   1.665513670736000e2_r8
real (r8), parameter :: c4 =   4.589984751248335e1_r8
real (r8), parameter :: c5 =   1.894281502222415e1_r8
real (r8), parameter :: c6 =   1.192559661490269e3_r8
real (r8), parameter :: c7 =  -6.631757848479068e3_r8
real (r8), parameter :: c8 =  -1.104989199195898e2_r8
real (r8), parameter :: c9 =  -1.207006482532330e3_r8
real (r8), parameter :: c10 = -3.148710097513822e3_r8
real (r8), parameter :: c11 =  7.437431482069087e2_r8
real (r8), parameter :: c12 =  2.519335841663499e3_r8
real (r8), parameter :: c13 =  1.186568375570869e1_r8
real (r8), parameter :: c14 =  5.731307337366114e2_r8
real (r8), parameter :: c15 =  1.213387273240204e3_r8
real (r8), parameter :: c16 =  1.062383995581363e3_r8
real (r8), parameter :: c17 = -6.399956483223386e2_r8
real (r8), parameter :: c18 = -1.541083032068263e3_r8
real (r8), parameter :: c19 =  8.460780175632090e1_r8
real (r8), parameter :: c20 = -3.233571307223379e2_r8
real (r8), parameter :: c21 = -2.031538422351553e2_r8
real (r8), parameter :: c22 =  4.351585544019463e1_r8
real (r8), parameter :: c23 = -8.062279018001309e2_r8
real (r8), parameter :: c24 =  7.510134932437941e2_r8
real (r8), parameter :: c25 =  1.797443329095446e2_r8
real (r8), parameter :: c26 = -2.389853928747630e1_r8
real (r8), parameter :: c27 =  1.021046205356775e2_r8

real (r8) :: x, y

x = sqrt(gsw_sfac*sa)
y = ct/40.0_r8

gsw_latentheat_evap_ct = c0 + x*(c1 + c4*y + x*(c3   &
    + y*(c7 + c12*y) + x*(c6 + y*(c11 + y*(c17 + c24*y)) &
    + x*(c10 + y*(c16 + c23*y) + x*(c15 + c22*y + c21*x)))))  &
    + y*(c2 + y*(c5 + c8*x + y*(c9 + x*(c13 + c18*x)  &
    + y*(c14 + x*(c19 + c25*x) + y*(c20 + c26*x + c27*y)))))

return
end function

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


      subroutine varsol (jday,month,
     a                   dsol)

      real dsol,pi,om
      integer jday,month,j

c     calculation of the variability of the solar constant during the
c     year.
c     jday is the number of the day in the month
c     dsol is a multiplicative factor to apply to the mean value of
c     solar constant

      if (month.le.2) goto 1
      if (month.gt.8) goto 2
      j=31*(month-1)-((month-1)/2)-2+jday
      goto 3
    1 j=31*(month-1)+jday
      goto 3
    2 j=31*(month-1)-((month-2)/2)-2+jday

    3 pi=2.*acos (0.)
      om=(.9856*float(j-4))*pi/180.
      dsol=1./((1.-.01673*cos(om))**2)
      return
      end


! =========================================================================
elemental function gsw_gibbs_ice_pt0 (pt0)
! =========================================================================
!
!  Part of the the first temperature derivative of Gibbs energy of ice
!  that is the outout is "gibbs_ice(1,0,pt0,0) + s0"
!
!  pt0  =  potential temperature with reference sea pressure of zero dbar
!                                                                 [ deg C ]
!
!  gsw_gibbs_ice_pt0 = part of temperature derivative     [ J kg^-1 K^-1 ]
!--------------------------------------------------------------------------

use gsw_mod_teos10_constants, only : gsw_t0

use gsw_mod_gibbs_ice_coefficients

use gsw_mod_kinds

implicit none

real (r8), intent(in) :: pt0

real (r8) :: gsw_gibbs_ice_pt0

real (r8) :: tau
complex (r8) :: g, tau_t1, tau_t2

tau = (pt0 + gsw_t0)*rec_tt

tau_t1 = tau/t1
tau_t2 = tau/t2

g = r1*(log((1.0_r8 + tau_t1)/(1.0_r8 - tau_t1)) - 2.0_r8*tau_t1) &
    + r20*(log((1.0_r8 + tau_t2)/(1.0_r8 - tau_t2)) - 2.0_r8*tau_t2)

gsw_gibbs_ice_pt0 = real(g)

return
end function

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


subroutine damping()

!  "Spange"-layer damping at the domain top region

use vars
use microphysics, only: micro_field, index_water_vapor
implicit none

real tau_min	! minimum damping time-scale (at the top)
real tau_max    ! maxim damping time-scale (base of damping layer)
real damp_depth ! damping depth as a fraction of the domain height
parameter(tau_min=600., tau_max=10800., damp_depth=0.3)
real tau(nzm)
integer i, j, k, n_damp

call t_startf ('damping')

if(tau_min.lt.2*dt) then
   print*,'Error: in damping() tau_min is too small!'
   call task_abort()
end if

do k=nzm,1,-1
 if(z(nzm)-z(k).lt.damp_depth*z(nzm)) then
   n_damp=nzm-k+1
 endif
end do

do k=nzm,nzm-n_damp,-1
 tau(k) = tau_min *(tau_max/tau_min)**((z(nzm)-z(k))/(z(nzm)-z(nzm-n_damp)))
 tau(k)=1./tau(k)
end do

do k = nzm, nzm-n_damp, -1
   do j=1,ny
    do i=1,nx
      dudt(i,j,k,na)= dudt(i,j,k,na)-(u(i,j,k)-u0(k)) * tau(k)
      dvdt(i,j,k,na)= dvdt(i,j,k,na)-(v(i,j,k)-v0(k)) * tau(k)
      dwdt(i,j,k,na)= dwdt(i,j,k,na)-w(i,j,k) * tau(k)
!      t(i,j,k)= t(i,j,k)-dtn*(t(i,j,k)-t0(k)) * tau(k)
!      micro_field(i,j,k,index_water_vapor)= micro_field(i,j,k,index_water_vapor)- &
!                                    dtn*(qv(i,j,k)+qcl(i,j,k)+qci(i,j,k)-q0(k)) * tau(k)
    end do! i
   end do! j
end do ! k

call t_stopf('damping')
end subroutine damping


      subroutine determinant(N,Nphy,Y,det)
      integer N
      double precision Y(Nphy,NPhy), det
      double precision
     . y11,y12,y13,y14,y15,y16,
     . y21,y22,y23,y24,y25,y26,
     . y31,y32,y33,y34,y35,y36,
     . y41,y42,y43,y44,y45,y46,
     . y51,y52,y53,y54,y55,y56,
     . y61,y62,y63,y64,y65,y66,
     . x11,x12,x13,x14,x15,x16

C     Takes the determinant of up to a 6*6 matrix
      if (N .eq. 1) then
      det=y(1,1)
      elseif (N .eq. 2) then
      y11=y(1,1)
      y12=y(1,2)

      y21=y(2,1)
      y22=y(2,2)
      det=y11*y22-y12*y21

      elseif (N .eq. 3) then
      y11=y(1,1)
      y12=y(1,2)
      y13=y(1,3)

      y21=y(2,1)
      y22=y(2,2)
      y23=y(2,3)

      y31=y(3,1)
      y32=y(3,2)
      y33=y(3,3)

      det = y11*(y22*y33-y23*y32)
     .    -y12*(y21*y33-y23*y31)
     .    +y13*(y21*y32-y22*y31)

      elseif (N .eq. 4) then
      y11=y(1,1)
      y12=y(1,2)
      y13=y(1,3)
      y14=y(1,4)

      y21=y(2,1)
      y22=y(2,2)
      y23=y(2,3)
      y24=y(2,4)

      y31=y(3,1)
      y32=y(3,2)
      y33=y(3,3)
      y34=y(3,4)

      y41=y(4,1)
      y42=y(4,2)
      y43=y(4,3)
      y44=y(4,4)

      det = y11*(y22*(y33*y44-y34*y43)-y23*(y32*y44-y34*y42)+y24*(y32*y
     1   43-y33*y42))-y12*(y21*(y33*y44-y34*y43)-y23*(y31*y44-y34*y41)+y
     2   24*(y31*y43-y33*y41))+y13*(y21*(y32*y44-y34*y42)-y22*(y31*y44-y
     3   34*y41)+y24*(y31*y42-y32*y41))-y14*(y21*(y32*y43-y33*y42)-y22*(
     4   y31*y43-y33*y41)+y23*(y31*y42-y32*y41))

      elseif (N .eq. 5) then
      y11=y(1,1)
      y12=y(1,2)
      y13=y(1,3)
      y14=y(1,4)
      y15=y(1,5)

      y21=y(2,1)
      y22=y(2,2)
      y23=y(2,3)
      y24=y(2,4)
      y25=y(2,5)

      y31=y(3,1)
      y32=y(3,2)
      y33=y(3,3)
      y34=y(3,4)
      y35=y(3,5)

      y41=y(4,1)
      y42=y(4,2)
      y43=y(4,3)
      y44=y(4,4)
      y45=y(4,5)

      y51=y(5,1)
      y52=y(5,2)
      y53=y(5,3)
      y54=y(5,4)
      y55=y(5,5)

      det = y11*(y22*(y33*(y44*y55-y45*y54)-y34*(y43*y55-y45*y53)+y35*(
     1   y43*y54-y44*y53))-y23*(y32*(y44*y55-y45*y54)-y34*(y42*y55-y45*y
     2   52)+y35*(y42*y54-y44*y52))+y24*(y32*(y43*y55-y45*y53)-y33*(y42*
     3   y55-y45*y52)+y35*(y42*y53-y43*y52))-y25*(y32*(y43*y54-y44*y53)-
     4   y33*(y42*y54-y44*y52)+y34*(y42*y53-y43*y52)))-y12*(y21*(y33*(y4
     5   4*y55-y45*y54)-y34*(y43*y55-y45*y53)+y35*(y43*y54-y44*y53))-y23
     6   *(y31*(y44*y55-y45*y54)-y34*(y41*y55-y45*y51)+y35*(y41*y54-y44*
     7   y51))+y24*(y31*(y43*y55-y45*y53)-y33*(y41*y55-y45*y51)+y35*(y41
     8   *y53-y43*y51))-y25*(y31*(y43*y54-y44*y53)-y33*(y41*y54-y44*y51)
     9   +y34*(y41*y53-y43*y51)))+y13*(y21*(y32*(y44*y55-y45*y54)-y34*(y
     :   42*y55-y45*y52)+y35*(y42*y54-y44*y52))-y22*(y31*(y44*y55-y45*y5
     ;   4)-y34*(y41*y55-y45*y51)+y35*(y41*y54-y44*y51))+y24*(y31*(y42*y
     <   55-y45*y52)-y32*(y41*y55-y45*y51)+y35*(y41*y52-y42*y51))-y25*(y
     =   31*(y42*y54-y44*y52)-y32*(y41*y54-y44*y51)+y34*(y41*y52-y42*y51
     >   )))-y14*(y21*(y32*(y43*y55-y45*y53)-y33*(y42*y55-y45*y52)+y35*(
     ?   y42*y53-y43*y52))-y22*(y31*(y43*y55-y45*y53)-y33*(y41*y55-y45*y
     @   51)+y35*(y41*y53-y43*y51))+y23*(y31*(y42*y55-y45*y52)-y32*(y41*
     1   y55-y45*y51)+y35*(y41*y52-y42*y51))-y25*(y31*(y42*y53-y43*y52)-
     2   y32*(y41*y53-y43*y51)+y33*(y41*y52-y42*y51)))+y15*(y21*(y32*(y4
     3   3*y54-y44*y53)-y33*(y42*y54-y44*y52)+y34*(y42*y53-y43*y52))-y22
     4   *(y31*(y43*y54-y44*y53)-y33*(y41*y54-y44*y51)+y34*(y41*y53-y43*
     5   y51))+y23*(y31*(y42*y54-y44*y52)-y32*(y41*y54-y44*y51)+y34*(y41
     6   *y52-y42*y51))-y24*(y31*(y42*y53-y43*y52)-y32*(y41*y53-y43*y51)
     7   +y33*(y41*y52-y42*y51)))

      elseif (N .eq. 6) then
      y11=y(1,1)
      y12=y(1,2)
      y13=y(1,3)
      y14=y(1,4)
      y15=y(1,5)
      y16=y(1,6)

      y21=y(2,1)
      y22=y(2,2)
      y23=y(2,3)
      y24=y(2,4)
      y25=y(2,5)
      y26=y(2,6)

      y31=y(3,1)
      y32=y(3,2)
      y33=y(3,3)
      y34=y(3,4)
      y35=y(3,5)
      y36=y(3,6)

      y41=y(4,1)
      y42=y(4,2)
      y43=y(4,3)
      y44=y(4,4)
      y45=y(4,5)
      y46=y(4,6)

      y51=y(5,1)
      y52=y(5,2)
      y53=y(5,3)
      y54=y(5,4)
      y55=y(5,5)
      y56=y(5,6)

      y61=y(6,1)
      y62=y(6,2)
      y63=y(6,3)
      y64=y(6,4)
      y65=y(6,5)
      y66=y(6,6)

      x11 = y11*(y22*(y33*(y44*(y55*y66-y56*y65)-y45*(y54*y66-y56*y64)+y
     1   46*(y54*y65-y55*y64))-y34*(y43*(y55*y66-y56*y65)-y45*(y53*y66-y
     2   56*y63)+y46*(y53*y65-y55*y63))+y35*(y43*(y54*y66-y56*y64)-y44*(
     3   y53*y66-y56*y63)+y46*(y53*y64-y54*y63))-y36*(y43*(y54*y65-y55*y
     4   64)-y44*(y53*y65-y55*y63)+y45*(y53*y64-y54*y63)))-y23*(y32*(y44
     5   *(y55*y66-y56*y65)-y45*(y54*y66-y56*y64)+y46*(y54*y65-y55*y64))
     6   -y34*(y42*(y55*y66-y56*y65)-y45*(y52*y66-y56*y62)+y46*(y52*y65-
     7   y55*y62))+y35*(y42*(y54*y66-y56*y64)-y44*(y52*y66-y56*y62)+y46*
     8   (y52*y64-y54*y62))-y36*(y42*(y54*y65-y55*y64)-y44*(y52*y65-y55*
     9   y62)+y45*(y52*y64-y54*y62)))+y24*(y32*(y43*(y55*y66-y56*y65)-y4
     :   5*(y53*y66-y56*y63)+y46*(y53*y65-y55*y63))-y33*(y42*(y55*y66-y5
     ;   6*y65)-y45*(y52*y66-y56*y62)+y46*(y52*y65-y55*y62))+y35*(y42*(y
     <   53*y66-y56*y63)-y43*(y52*y66-y56*y62)+y46*(y52*y63-y53*y62))-y3
     =   6*(y42*(y53*y65-y55*y63)-y43*(y52*y65-y55*y62)+y45*(y52*y63-y53
     >   *y62)))-y25*(y32*(y43*(y54*y66-y56*y64)-y44*(y53*y66-y56*y63)+y
     ?   46*(y53*y64-y54*y63))-y33*(y42*(y54*y66-y56*y64)-y44*(y52*y66-y
     @   56*y62)+y46*(y52*y64-y54*y62))+y34*(y42*(y53*y66-y56*y63)-y43*(
     1   y52*y66-y56*y62)+y46*(y52*y63-y53*y62))-y36*(y42*(y53*y64-y54*y
     2   63)-y43*(y52*y64-y54*y62)+y44*(y52*y63-y53*y62)))+y26*(y32*(y43
     3   *(y54*y65-y55*y64)-y44*(y53*y65-y55*y63)+y45*(y53*y64-y54*y63))
     4   -y33*(y42*(y54*y65-y55*y64)-y44*(y52*y65-y55*y62)+y45*(y52*y64-
     5   y54*y62))+y34*(y42*(y53*y65-y55*y63)-y43*(y52*y65-y55*y62)+y45*
     6   (y52*y63-y53*y62))-y35*(y42*(y53*y64-y54*y63)-y43*(y52*y64-y54*
     7   y62)+y44*(y52*y63-y53*y62))))
      x12 = -y12*(y21*(y33*(y44*(y55*y66-y56*y65)-y45*(y54*y66-y56*y64)+
     1   y46*(y54*y65-y55*y64))-y34*(y43*(y55*y66-y56*y65)-y45*(y53*y66-
     2   y56*y63)+y46*(y53*y65-y55*y63))+y35*(y43*(y54*y66-y56*y64)-y44*
     3   (y53*y66-y56*y63)+y46*(y53*y64-y54*y63))-y36*(y43*(y54*y65-y55*
     4   y64)-y44*(y53*y65-y55*y63)+y45*(y53*y64-y54*y63)))-y23*(y31*(y4
     5   4*(y55*y66-y56*y65)-y45*(y54*y66-y56*y64)+y46*(y54*y65-y55*y64)
     6   )-y34*(y41*(y55*y66-y56*y65)-y45*(y51*y66-y56*y61)+y46*(y51*y65
     7   -y55*y61))+y35*(y41*(y54*y66-y56*y64)-y44*(y51*y66-y56*y61)+y46
     8   *(y51*y64-y54*y61))-y36*(y41*(y54*y65-y55*y64)-y44*(y51*y65-y55
     9   *y61)+y45*(y51*y64-y54*y61)))+y24*(y31*(y43*(y55*y66-y56*y65)-y
     :   45*(y53*y66-y56*y63)+y46*(y53*y65-y55*y63))-y33*(y41*(y55*y66-y
     ;   56*y65)-y45*(y51*y66-y56*y61)+y46*(y51*y65-y55*y61))+y35*(y41*(
     <   y53*y66-y56*y63)-y43*(y51*y66-y56*y61)+y46*(y51*y63-y53*y61))-y
     =   36*(y41*(y53*y65-y55*y63)-y43*(y51*y65-y55*y61)+y45*(y51*y63-y5
     >   3*y61)))-y25*(y31*(y43*(y54*y66-y56*y64)-y44*(y53*y66-y56*y63)+
     ?   y46*(y53*y64-y54*y63))-y33*(y41*(y54*y66-y56*y64)-y44*(y51*y66-
     @   y56*y61)+y46*(y51*y64-y54*y61))+y34*(y41*(y53*y66-y56*y63)-y43*
     1   (y51*y66-y56*y61)+y46*(y51*y63-y53*y61))-y36*(y41*(y53*y64-y54*
     2   y63)-y43*(y51*y64-y54*y61)+y44*(y51*y63-y53*y61)))+y26*(y31*(y4
     3   3*(y54*y65-y55*y64)-y44*(y53*y65-y55*y63)+y45*(y53*y64-y54*y63)
     4   )-y33*(y41*(y54*y65-y55*y64)-y44*(y51*y65-y55*y61)+y45*(y51*y64
     5   -y54*y61))+y34*(y41*(y53*y65-y55*y63)-y43*(y51*y65-y55*y61)+y45
     6   *(y51*y63-y53*y61))-y35*(y41*(y53*y64-y54*y63)-y43*(y51*y64-y54
     7   *y61)+y44*(y51*y63-y53*y61))))
      x13 = y13*(y21*(y32*(y44*(y55*y66-y56*y65)-y45*(y54*y66-y56*y64)+y
     1   46*(y54*y65-y55*y64))-y34*(y42*(y55*y66-y56*y65)-y45*(y52*y66-y
     2   56*y62)+y46*(y52*y65-y55*y62))+y35*(y42*(y54*y66-y56*y64)-y44*(
     3   y52*y66-y56*y62)+y46*(y52*y64-y54*y62))-y36*(y42*(y54*y65-y55*y
     4   64)-y44*(y52*y65-y55*y62)+y45*(y52*y64-y54*y62)))-y22*(y31*(y44
     5   *(y55*y66-y56*y65)-y45*(y54*y66-y56*y64)+y46*(y54*y65-y55*y64))
     6   -y34*(y41*(y55*y66-y56*y65)-y45*(y51*y66-y56*y61)+y46*(y51*y65-
     7   y55*y61))+y35*(y41*(y54*y66-y56*y64)-y44*(y51*y66-y56*y61)+y46*
     8   (y51*y64-y54*y61))-y36*(y41*(y54*y65-y55*y64)-y44*(y51*y65-y55*
     9   y61)+y45*(y51*y64-y54*y61)))+y24*(y31*(y42*(y55*y66-y56*y65)-y4
     :   5*(y52*y66-y56*y62)+y46*(y52*y65-y55*y62))-y32*(y41*(y55*y66-y5
     ;   6*y65)-y45*(y51*y66-y56*y61)+y46*(y51*y65-y55*y61))+y35*(y41*(y
     <   52*y66-y56*y62)-y42*(y51*y66-y56*y61)+y46*(y51*y62-y52*y61))-y3
     =   6*(y41*(y52*y65-y55*y62)-y42*(y51*y65-y55*y61)+y45*(y51*y62-y52
     >   *y61)))-y25*(y31*(y42*(y54*y66-y56*y64)-y44*(y52*y66-y56*y62)+y
     ?   46*(y52*y64-y54*y62))-y32*(y41*(y54*y66-y56*y64)-y44*(y51*y66-y
     @   56*y61)+y46*(y51*y64-y54*y61))+y34*(y41*(y52*y66-y56*y62)-y42*(
     1   y51*y66-y56*y61)+y46*(y51*y62-y52*y61))-y36*(y41*(y52*y64-y54*y
     2   62)-y42*(y51*y64-y54*y61)+y44*(y51*y62-y52*y61)))+y26*(y31*(y42
     3   *(y54*y65-y55*y64)-y44*(y52*y65-y55*y62)+y45*(y52*y64-y54*y62))
     4   -y32*(y41*(y54*y65-y55*y64)-y44*(y51*y65-y55*y61)+y45*(y51*y64-
     5   y54*y61))+y34*(y41*(y52*y65-y55*y62)-y42*(y51*y65-y55*y61)+y45*
     6   (y51*y62-y52*y61))-y35*(y41*(y52*y64-y54*y62)-y42*(y51*y64-y54*
     7   y61)+y44*(y51*y62-y52*y61))))
      x14 = -y14*(y21*(y32*(y43*(y55*y66-y56*y65)-y45*(y53*y66-y56*y63)+
     1   y46*(y53*y65-y55*y63))-y33*(y42*(y55*y66-y56*y65)-y45*(y52*y66-
     2   y56*y62)+y46*(y52*y65-y55*y62))+y35*(y42*(y53*y66-y56*y63)-y43*
     3   (y52*y66-y56*y62)+y46*(y52*y63-y53*y62))-y36*(y42*(y53*y65-y55*
     4   y63)-y43*(y52*y65-y55*y62)+y45*(y52*y63-y53*y62)))-y22*(y31*(y4
     5   3*(y55*y66-y56*y65)-y45*(y53*y66-y56*y63)+y46*(y53*y65-y55*y63)
     6   )-y33*(y41*(y55*y66-y56*y65)-y45*(y51*y66-y56*y61)+y46*(y51*y65
     7   -y55*y61))+y35*(y41*(y53*y66-y56*y63)-y43*(y51*y66-y56*y61)+y46
     8   *(y51*y63-y53*y61))-y36*(y41*(y53*y65-y55*y63)-y43*(y51*y65-y55
     9   *y61)+y45*(y51*y63-y53*y61)))+y23*(y31*(y42*(y55*y66-y56*y65)-y
     :   45*(y52*y66-y56*y62)+y46*(y52*y65-y55*y62))-y32*(y41*(y55*y66-y
     ;   56*y65)-y45*(y51*y66-y56*y61)+y46*(y51*y65-y55*y61))+y35*(y41*(
     <   y52*y66-y56*y62)-y42*(y51*y66-y56*y61)+y46*(y51*y62-y52*y61))-y
     =   36*(y41*(y52*y65-y55*y62)-y42*(y51*y65-y55*y61)+y45*(y51*y62-y5
     >   2*y61)))-y25*(y31*(y42*(y53*y66-y56*y63)-y43*(y52*y66-y56*y62)+
     ?   y46*(y52*y63-y53*y62))-y32*(y41*(y53*y66-y56*y63)-y43*(y51*y66-
     @   y56*y61)+y46*(y51*y63-y53*y61))+y33*(y41*(y52*y66-y56*y62)-y42*
     1   (y51*y66-y56*y61)+y46*(y51*y62-y52*y61))-y36*(y41*(y52*y63-y53*
     2   y62)-y42*(y51*y63-y53*y61)+y43*(y51*y62-y52*y61)))+y26*(y31*(y4
     3   2*(y53*y65-y55*y63)-y43*(y52*y65-y55*y62)+y45*(y52*y63-y53*y62)
     4   )-y32*(y41*(y53*y65-y55*y63)-y43*(y51*y65-y55*y61)+y45*(y51*y63
     5   -y53*y61))+y33*(y41*(y52*y65-y55*y62)-y42*(y51*y65-y55*y61)+y45
     6   *(y51*y62-y52*y61))-y35*(y41*(y52*y63-y53*y62)-y42*(y51*y63-y53
     7   *y61)+y43*(y51*y62-y52*y61))))
      x15 = y15*(y21*(y32*(y43*(y54*y66-y56*y64)-y44*(y53*y66-y56*y63)+y
     1   46*(y53*y64-y54*y63))-y33*(y42*(y54*y66-y56*y64)-y44*(y52*y66-y
     2   56*y62)+y46*(y52*y64-y54*y62))+y34*(y42*(y53*y66-y56*y63)-y43*(
     3   y52*y66-y56*y62)+y46*(y52*y63-y53*y62))-y36*(y42*(y53*y64-y54*y
     4   63)-y43*(y52*y64-y54*y62)+y44*(y52*y63-y53*y62)))-y22*(y31*(y43
     5   *(y54*y66-y56*y64)-y44*(y53*y66-y56*y63)+y46*(y53*y64-y54*y63))
     6   -y33*(y41*(y54*y66-y56*y64)-y44*(y51*y66-y56*y61)+y46*(y51*y64-
     7   y54*y61))+y34*(y41*(y53*y66-y56*y63)-y43*(y51*y66-y56*y61)+y46*
     8   (y51*y63-y53*y61))-y36*(y41*(y53*y64-y54*y63)-y43*(y51*y64-y54*
     9   y61)+y44*(y51*y63-y53*y61)))+y23*(y31*(y42*(y54*y66-y56*y64)-y4
     :   4*(y52*y66-y56*y62)+y46*(y52*y64-y54*y62))-y32*(y41*(y54*y66-y5
     ;   6*y64)-y44*(y51*y66-y56*y61)+y46*(y51*y64-y54*y61))+y34*(y41*(y
     <   52*y66-y56*y62)-y42*(y51*y66-y56*y61)+y46*(y51*y62-y52*y61))-y3
     =   6*(y41*(y52*y64-y54*y62)-y42*(y51*y64-y54*y61)+y44*(y51*y62-y52
     >   *y61)))-y24*(y31*(y42*(y53*y66-y56*y63)-y43*(y52*y66-y56*y62)+y
     ?   46*(y52*y63-y53*y62))-y32*(y41*(y53*y66-y56*y63)-y43*(y51*y66-y
     @   56*y61)+y46*(y51*y63-y53*y61))+y33*(y41*(y52*y66-y56*y62)-y42*(
     1   y51*y66-y56*y61)+y46*(y51*y62-y52*y61))-y36*(y41*(y52*y63-y53*y
     2   62)-y42*(y51*y63-y53*y61)+y43*(y51*y62-y52*y61)))+y26*(y31*(y42
     3   *(y53*y64-y54*y63)-y43*(y52*y64-y54*y62)+y44*(y52*y63-y53*y62))
     4   -y32*(y41*(y53*y64-y54*y63)-y43*(y51*y64-y54*y61)+y44*(y51*y63-
     5   y53*y61))+y33*(y41*(y52*y64-y54*y62)-y42*(y51*y64-y54*y61)+y44*
     6   (y51*y62-y52*y61))-y34*(y41*(y52*y63-y53*y62)-y42*(y51*y63-y53*
     7   y61)+y43*(y51*y62-y52*y61))))
      x16 = -y16*(y21*(y32*(y43*(y54*y65-y55*y64)-y44*(y53*y65-y55*y63)+
     1   y45*(y53*y64-y54*y63))-y33*(y42*(y54*y65-y55*y64)-y44*(y52*y65-
     2   y55*y62)+y45*(y52*y64-y54*y62))+y34*(y42*(y53*y65-y55*y63)-y43*
     3   (y52*y65-y55*y62)+y45*(y52*y63-y53*y62))-y35*(y42*(y53*y64-y54*
     4   y63)-y43*(y52*y64-y54*y62)+y44*(y52*y63-y53*y62)))-y22*(y31*(y4
     5   3*(y54*y65-y55*y64)-y44*(y53*y65-y55*y63)+y45*(y53*y64-y54*y63)
     6   )-y33*(y41*(y54*y65-y55*y64)-y44*(y51*y65-y55*y61)+y45*(y51*y64
     7   -y54*y61))+y34*(y41*(y53*y65-y55*y63)-y43*(y51*y65-y55*y61)+y45
     8   *(y51*y63-y53*y61))-y35*(y41*(y53*y64-y54*y63)-y43*(y51*y64-y54
     9   *y61)+y44*(y51*y63-y53*y61)))+y23*(y31*(y42*(y54*y65-y55*y64)-y
     :   44*(y52*y65-y55*y62)+y45*(y52*y64-y54*y62))-y32*(y41*(y54*y65-y
     ;   55*y64)-y44*(y51*y65-y55*y61)+y45*(y51*y64-y54*y61))+y34*(y41*(
     <   y52*y65-y55*y62)-y42*(y51*y65-y55*y61)+y45*(y51*y62-y52*y61))-y
     =   35*(y41*(y52*y64-y54*y62)-y42*(y51*y64-y54*y61)+y44*(y51*y62-y5
     >   2*y61)))-y24*(y31*(y42*(y53*y65-y55*y63)-y43*(y52*y65-y55*y62)+
     ?   y45*(y52*y63-y53*y62))-y32*(y41*(y53*y65-y55*y63)-y43*(y51*y65-
     @   y55*y61)+y45*(y51*y63-y53*y61))+y33*(y41*(y52*y65-y55*y62)-y42*
     1   (y51*y65-y55*y61)+y45*(y51*y62-y52*y61))-y35*(y41*(y52*y63-y53*
     2   y62)-y42*(y51*y63-y53*y61)+y43*(y51*y62-y52*y61)))+y25*(y31*(y4
     3   2*(y53*y64-y54*y63)-y43*(y52*y64-y54*y62)+y44*(y52*y63-y53*y62)
     4   )-y32*(y41*(y53*y64-y54*y63)-y43*(y51*y64-y54*y61)+y44*(y51*y63
     5   -y53*y61))+y33*(y41*(y52*y64-y54*y62)-y42*(y51*y64-y54*y61)+y44
     6   *(y51*y62-y52*y61))-y34*(y41*(y52*y63-y53*y62)-y42*(y51*y63-y53
     7   *y61)+y43*(y51*y62-y52*y61))))
      det=x11+x12+x13+x14+x15+x16

      return
      else
      write(6,*) 'Unimplemented value of N in determinant, N=',N
      stop
      endif
      return
      end


      implicit none
      integer ns
      double precision, allocatable :: source(:,:)
      double complex, allocatable :: charge(:)
      double complex, allocatable :: pot(:)
      double complex, allocatable :: potex(:)

      double precision eps
      double complex eye,zk
      integer i,j,k
      integer ntest,nd
      double precision thresh,ra,erra
      double precision hkrand

      data eye/(0.0d0,1.0d0)/

c
cc      initialize printing routine
c
cc      call prini(6,13)
      write(*,*)
      write(*,*)
      write(*,*) "================================="
      print *, "This code is an example fortran driver*"
      write(*,*)
      write(*,*)

      zk = 2.2d0

      ns = 300 000 000
      print *, "nsources =",ns

      allocate(source(3,ns))
      allocate(charge(ns))
      allocate(pot(ns))

      eps = 0.5d-3

      write(*,*) "=========================================="

c
c
c       example demonstrating use of
c        source to source, charges, pot + gradient
c

c
cc      generate sources uniformly in the unit cube
c
c
      do i=1,ns
        source(1,i) = hkrand(0)**2
        source(2,i) = hkrand(0)**2
        source(3,i) = hkrand(0)**2

        charge(i) = hkrand(0) + eye*hkrand(0)

        pot(i) = 0
      enddo

      call hfmm3d_s_c_p(eps,zk,ns,source,charge,pot)

      ntest = 10
      allocate(potex(ntest))
      do i=1,ntest
        potex(i) = 0
      enddo

      thresh = 1.0d-16
      nd = 1

      call h3ddirectcp(nd,zk,source,charge,ns,source,ntest,potex,
     1   thresh)

      ra = 0
      erra = 0
      do i=1,ntest
        ra = ra + abs(potex(i))**2
        erra = erra + abs(pot(i)-potex(i))**2
      enddo

      erra = sqrt(erra/ra)
      print *, "Relative error in pot=",erra

      stop
      end
c----------------------------------------------------------
c
cc
c
c


	SUBROUTINE SSIZE
     .	(SALPHA,SBETA,SR,SP0,M,XSPSI,XN,ERR)
	IMPLICIT NONE
C
C-Description-----------------------------------------------------------
C
C  Function:
C	Determine case patient sample size N
C  Arguments:
C
C   INPUT:
C
C   i	SALPHA	(REAL)
C	Type I error probability
C   i	SBETA	(REAL)
C	Type II error probability
C   i	SR	(REAL)
C	Correlation coefficient
C   i	SP0	(REAL)
C	Probability of exposure in control group
C   i	M	(Integer)
C	Number of matched controls per case patient
C   i	XSPSI	(REAL)
C	Odds ratio
C
C   OUTPUT:
C
C   o	XN	(Real)
C	Number of case patients
C   o	ERR	(Integer)
C  Notes:
C   .	This routine was written by Dale Plummer.
C   .	Designed by Dr. William Dupont.
C   .   The LaTeX notation given in the comments is from:
C       Dupont, Biometrics 1988;44:1157-68.
C
C-Declarations----------------------------------------------------------
C
	INTEGER M,ERR
	REAL XN
	REAL SALPHA,SBETA,SR,SP0,XSPSI
        DOUBLE PRECISION ALPHA,BETA,R,P0,PSI
	DOUBLE PRECISION ZALPHA,ZBETA
	REAL ZCRVALUE
	DOUBLE PRECISION RM,RN
	DOUBLE PRECISION SQV1,SQVPSI
	DOUBLE PRECISION NM

	DOUBLE PRECISION MEAN,VAR,PSI_VALUE
	DOUBLE PRECISION E1, V1, EPSI, VPSI
C
C Code:
C
C    Assume a successful run.
C
	ERR=0
C
C    Copy arguments M and N to floating point local storage.
C
	RM=DBLE(M)
	RN=DBLE(XN)
C
C    Copy single precision arguments to double precision internal
C    representation.
C
C    ALPHA = \alpha
C    BETA = \beta
C    P0 = p_0
C    PSI = \psi
C    ZALPHA = z_{\alpha/2}
C    ZBETA = z_{\beta}
C
	ALPHA=DBLE(SALPHA)
	BETA=DBLE(SBETA)
	R=DBLE(SR)
	P0=DBLE(SP0)
	PSI=DBLE(XSPSI)
	ZALPHA=DBLE(ZCRVALUE(SALPHA/2.))
	ZBETA=DBLE(ZCRVALUE(SBETA))

	PSI_VALUE=1
	CALL MEANANDVAR(P0,R,M,RM,PSI_VALUE,MEAN,VAR)
C
C    E1 = e_1 in equation (7)
C    V1 = v_1 in equation (7)
C
	E1 = MEAN
	V1 = VAR

	PSI_VALUE=PSI
	CALL MEANANDVAR(P0,R,M,RM,PSI_VALUE,MEAN,VAR)
C
C    EPSI = e_{\PSI} in equation (7)
C    VPSI = v_{\PSI} in equation (7)
C
	EPSI = MEAN
	VPSI = VAR

C ----------------------------------------------------

	SQV1=SQRT(V1)
	SQVPSI=SQRT(VPSI)
C
C    NM is equation (7) in Dupont (Biometrics 1988).
C
	NM=(ZBETA*SQVPSI+ZALPHA*SQV1)**2/(EPSI-E1)**2
	XN=NM

	RETURN
	END

C ======================================================

	SUBROUTINE MEANANDVAR(P0,R,M,RM,PSI,MEAN,VAR)
	IMPLICIT NONE
C
C  When the alternaitve hypothesis is true this
C  routine calculates e_{\psi} and v_{\psi}
C
C  When the null hypothesis is true (\psi=1)
C  then it calculates e_1 and v_1
C
	DOUBLE PRECISION P0,R,RM,PSI
	INTEGER M
	DOUBLE PRECISION MEAN,VAR

	INTEGER IMPOSS,I,IM
	DOUBLE PRECISION P1,Q1,Q0,P01,P00,Q01,Q00,C1,C2,T(1000)

	INTEGER ERR
	REAL FM, XN
C
C Code:
C
C    Calculate P1 = p_1.
C
	CALL PONE(P0,PSI,R,P1,IMPOSS)
	IF (IMPOSS.EQ.1) THEN
	    ERR=1
	    FM=0.
	    XN=0.
	    RETURN
	END IF
C
C    Q1=q_1 Probability that a case patient is not exposed.
C
	Q1=1.-P1
C
C    Q0=q_0 Probability that a control is not exposed.
C
	Q0=1.-P0
C
C    P01 = p_{o+}
C    Probability that a control is exposed given that his matched case
C    is exposed.
C
	P01=P0+R*SQRT(Q1*P0*Q0/P1)
C
C    P00 = p_{o-}
C    Probability that a control is exposed given that his matched case
C    is NOT exposed.
C
	P00=P0-R*SQRT(P1*P0*Q0/Q1)
	Q01=1.-P01
	Q00=1.-P00
	IM=M
1	CONTINUE
	C1=1.
	C2=RM
C
C  The index I is denoted "m" in Dupont 1988.
C  T(I) = T(m) = t_m is the probability of
C  observing m exposed subjects among a case
C  and his M matched controls.
C
	DO 10 I=1,IM
	    T(I)=P1*C1*P01**(I-1)*Q01**(IM-I+1) + Q1*C2*P00**I*Q00**(IM-I)
	    C1=C2
	    C2=C2*(RM-DBLE(I))/(DBLE(I)+1.)
10	CONTINUE

	MEAN=0D0
	DO 40 I=1,IM
	    MEAN=MEAN+(DBLE(I)*T(I)*PSI/(DBLE(I)*PSI+RM-DBLE(I)+1.))
40	CONTINUE

	VAR=0D0
	DO 50 I=1,IM
	    VAR=VAR+
     .	(DBLE(I)*T(I)*PSI*(RM-DBLE(I)+1.)/(DBLE(I)*PSI+RM-DBLE(I)+1)**2)
50	CONTINUE

	RETURN
	END


      SUBROUTINE G0INTR(ICODE,XINT,YINT)
C
C          ------------------------------------------------
C          ROUTINE NO. (  47)   VERSION (A8.1)    07:MAY:92
C          ------------------------------------------------
C
C          THIS ROUTINE FINDS ANY INTERSECTION BETWEEN TWO
C          LINE SEGMENTS.
C
C          THE ARGUMENTS ARE AS FOLLOWS:
C
C          <ICODE>  IS A RETURNED VALUE INDICATING AS FOLLOWS :
C                   0 : NO INTERSECT IN LINE SEGMENTS,
C                   1 : LINES INTERSECT AT (XINT,YINT),
C          <XINT>   X COORDINATE OF INTERSECTION (IF ANY).
C          <YINT>   Y COORDINATE OF INTERSECTION (IF ANY).
C
C          THE VALUES IMPORTED ARE :
C
C          [XLINE1] ARRAY CONTAINING COORDINATES OF TWO END
C                   POINTS OF FIRST LINE.
C          [XLINE2] ARRAY CONTAINING COORDINATES OF TWO END
C                   POINTS OF SECOND LINE.
C
      COMMON /T0INTS/ XLINE1(2),YLINE1(2),XLINE2(2),YLINE2(2)
      COMMON /T3LIMS/ IMAXI,RMAXI,RMINI
C
C
C          CALCULATE GRADIENTS, IF PARALLEL CHECK FOR COINCIDENCE,
C          ELSE FIND INTERSECTION AND CHECK IF INTERSECT LIES ON
C          BOTH LINE SEGMENTS.
C
      ICODE= 0
      IF (ABS(XLINE1(2)-XLINE1(1)).LT.RMINI) GO TO 1
C
      SLOPE1= (YLINE1(2)-YLINE1(1))/(XLINE1(2)-XLINE1(1))
      IF (ABS(XLINE2(2)-XLINE2(1)).LT.RMINI) GO TO 2
C
      SLOPE2= (YLINE2(2)-YLINE2(1))/(XLINE2(2)-XLINE2(1))
      SLOPDF= SLOPE1-SLOPE2
      IF (ABS(SLOPDF).LT.RMINI) GO TO 6
      IF (ABS(YLINE1(2)-YLINE1(1)).LT.RMINI) GO TO 3
      IF (ABS(YLINE2(2)-YLINE2(1)).LT.RMINI) GO TO 4
C
      XINT= (SLOPE1*XLINE1(1)-YLINE1(1)-SLOPE2*XLINE2(1)+YLINE2(1))
     &     /SLOPDF
      YINT= SLOPE1*(XINT-XLINE1(1))+YLINE1(1)
      GO TO 5
C
C          FIRST LINE VERTICAL
C
    1 IF (ABS(XLINE2(2)-XLINE2(1)).LT.RMINI) GO TO 7
C
      XINT= XLINE1(1)
      SLOPE2= (YLINE2(2)-YLINE2(1))/(XLINE2(2)-XLINE2(1))
      YINT= SLOPE2*(XINT-XLINE2(1))+YLINE2(1)
      GO TO 5
C
C          SECOND LINE VERTICAL
C
    2 XINT= XLINE2(1)
      YINT= SLOPE1*(XINT-XLINE1(1))+YLINE1(1)
      GO TO 5
C
C          FIRST LINE HORIZONTAL
C
    3 YINT= YLINE1(1)
      XINT= (YINT-YLINE2(1))/SLOPE2+XLINE2(1)
      GO TO 5
C
C          SECOND LINE HORIZONTAL
C
    4 YINT= YLINE2(1)
      XINT= (YINT-YLINE1(1))/SLOPE1+XLINE1(1)
    5 IF (((XINT.LE.XLINE1(1).AND.XINT.GE.XLINE1(2)).OR.
     &     (XINT.GE.XLINE1(1).AND.XINT.LE.XLINE1(2))).AND.
     &    ((XINT.LE.XLINE2(1).AND.XINT.GE.XLINE2(2)).OR.
     &     (XINT.GE.XLINE2(1).AND.XINT.LE.XLINE2(2))).AND.
     &    ((YINT.LE.YLINE1(1).AND.YINT.GE.YLINE1(2)).OR.
     &     (YINT.GE.YLINE1(1).AND.YINT.LE.YLINE1(2))).AND.
     &    ((YINT.LE.YLINE2(1).AND.YINT.GE.YLINE2(2)).OR.
     &     (YINT.GE.YLINE2(1).AND.YINT.LE.YLINE2(2)))) ICODE= 1
      RETURN
C
C          LINES PARALLEL
C
    6 YT1= SLOPE1*(XLINE2(1)-XLINE1(1))+YLINE1(1)
      YT2= SLOPE2*(XLINE1(1)-XLINE2(1))+YLINE2(1)
C
C          ARE LINES PARALLEL AND COINCIDENT ?
C
      IF (ABS(YLINE2(1)-YT1).GE.RMINI.AND.
     &    ABS(YLINE1(1)-YT2).GE.RMINI) RETURN
      IF (XLINE1(1).LT.XLINE2(1).AND.XLINE1(2).GT.XLINE2(1)) GO TO 8
      IF (XLINE1(1).GT.XLINE2(1).AND.XLINE1(2).LT.XLINE2(1)) GO TO 8
      IF (XLINE1(1).LT.XLINE2(2).AND.XLINE1(2).GT.XLINE2(2)) GO TO 9
      IF (XLINE1(1).GT.XLINE2(2).AND.XLINE1(2).LT.XLINE2(2)) GO TO 9
      RETURN
C
C          BOTH LINES VERTICAL AND COINCIDENT ?
C
    7 IF (ABS(XLINE2(1)-XLINE1(1)).GE.RMINI) RETURN
      IF (YLINE1(1).LT.YLINE2(1).AND.YLINE1(2).GT.YLINE2(1)) GO TO 8
      IF (YLINE1(1).GT.YLINE2(1).AND.YLINE1(2).LT.YLINE2(1)) GO TO 8
      IF (YLINE1(1).LT.YLINE2(2).AND.YLINE1(2).GT.YLINE2(2)) GO TO 9
      IF (YLINE1(1).GT.YLINE2(2).AND.YLINE1(2).LT.YLINE2(2)) GO TO 9
      RETURN
C
C          FIRST POINT LIES WITHIN LINE.
C
    8 XINT= XLINE2(1)
      YINT= YLINE2(1)
      GO TO 10
C
C          SECOND POINT LIES WITHIN LINE.
C
    9 XINT= XLINE2(2)
      YINT= YLINE2(2)
   10 ICODE= 1
      RETURN
      END


C
C $Id: phi3sp.f,v 1.5 2008-07-27 00:17:10 haley Exp $
C
C                Copyright (C)  2000
C        University Corporation for Atmospheric Research
C                All Rights Reserved
C
C The use of this Software is governed by a License Agreement.
C
      REAL FUNCTION PHI3SP (ML,E0,E1,E2,E3)
C
C FUNCTION TO COMPUTE LATITUDE ANGLE (PHI-3).
C
      IMPLICIT REAL (A-Z)
      INTEGER II,NIT
      COMMON /ERRMZ0/ IERR
        INTEGER IERR
      SAVE   /ERRMZ0/
      COMMON /PRINZ0/ IPEMSG,IPELUN,IPPARM,IPPLUN
        INTEGER IPEMSG,IPELUN,IPPARM,IPPLUN
      SAVE   /PRINZ0/
      DATA TWO,FOUR,SIX /2.0E0,4.0E0,6.0E0/
      DATA TOL,NIT /1.0E-10,15/
C
      PHI = ML
      DO 020 II = 1,NIT
      DPHI = (ML + E1 * SIN (TWO * PHI) - E2 * SIN (FOUR * PHI)
     .       + E3 * SIN (SIX * PHI)) / E0 - PHI
      PHI = PHI + DPHI
      IF (ABS(DPHI) .GT. TOL) GO TO 020
      PHI3SP = PHI
      RETURN
  020 CONTINUE
C
      IF (IPEMSG .EQ. 0) WRITE (IPELUN,2000) NIT,ML,E0,E1,E2,E3
 2000 FORMAT (/' ERROR PHI3SP' /
     .         ' LATITUDE FAILED TO CONVERGE AFTER',I3,' ITERATIONS'/
     .         ' ML =',E25.16,'   E0 =',E25.16/
     .         ' E1 =',E25.16,'   E2 =',E25.16,'   E3=',E25.16)
      IERR = 003
      RETURN
C
      END


subroutine gls_pow(t, y, err, periods, t_ref, periodogram)

    implicit Real*4 (a-h,o-z)

    !integer, intent(in) :: n_per, n_pts
    Real*4, intent(in) :: t(:), y(:), err(:)
    Real*4, intent(in) :: periods(:), t_ref
    Real*4, intent(out):: periodogram(size(periods),5)
    Real*4 :: pwr, b(3), period
    Real*4 :: s_y, s_y2, s_cos, s_sin, s_cos2
    Real*4 :: s_sin2, s_sincos, s_ysin, s_ycos
    Real*4 :: w, x_arg, weight
    Real*4 :: A(3,3), D, SS, CC, CS
    Real*4 :: YY, YC, YS, pi
    integer :: i, j, k, pivot(3), INFO, len_periods, len_data
    !pi def.
    pi = acos(-1.0)
    weight = 0.0
    !compute W
    len_data = size(y)
    do 97 i = 1, len_data
        weight = weight + 1.0/(err(i)*err(i))
97  continue
    len_periods = size(periods)
    do 98 j = 1, len_periods
        period = periods(j)
        s_y = 0.0
        s_y2 = 0.0
        s_cos = 0.0
        s_sin = 0.0
        s_cos2 = 0.0
        s_sin2 = 0.0
        s_sincos = 0.0
        s_ysin = 0.0
        s_ycos = 0.0
        pwr = 0.0
        do 99 i = 1, len_data
            !phase of sinusoid
            x_arg = 2.0*pi*(t(i)-t_ref)/period
            !weight
            w = 1.0/(err(i)*err(i)*weight)
            s_y = s_y + w*y(i)
            s_y2 = s_y2 + w*y(i)*y(i)
            s_cos = s_cos + w*cos(x_arg)
            s_sin = s_sin + w*sin(x_arg)
            s_cos2 = s_cos2 + w*cos(x_arg)*cos(x_arg)
            s_sin2 = s_sin2 + w*sin(x_arg)*sin(x_arg)
            s_sincos = s_sincos + w*sin(x_arg)*cos(x_arg)
            s_ysin = s_ysin + w*y(i)*sin(x_arg)
            s_ycos = s_ycos + w*y(i)*cos(x_arg)
99      continue
        !definition of chi-square normalization
        !solving the matrix equation A*x=b using LAPACK library
        !define matrix A
        A(1,1) = s_cos2
        A(1,2) = s_sincos
        A(1,3) = s_cos
        A(2,1) = s_sincos
        A(2,2) = s_sin2
        A(2,3) = s_sin
        A(3,1) = s_cos
        A(3,2) = s_sin
        A(3,3) = 1.0
    ! define vector b
        b(1) = s_ycos
        b(2) = s_ysin
        b(3) = s_y
    !   find the solution using the LAPACK routine SGESV
    !   parameters in the order as they appear in the function call
    !   order of matrix A, number of right hand sides (b), matrix A,
    !   leading dimension of A, array that records pivoting,
    !   result vector b on entry, x on exit, leading dimension of b
    !   return value
        call SGESV(3, 1, A, 3, pivot, b, 3, INFO)

        !compute spectral power, p(w)
        CC = s_cos2 - s_cos*s_cos
        SS = s_sin2 - s_sin*s_sin
        CS = s_sincos - s_cos*s_sin
        YY = s_y2 - s_y*s_y
        YC = s_ycos - s_y*s_cos
        YS = s_ysin - s_y*s_sin
        D = CC*SS - (CS*CS)
        !spectral power
        pwr = (SS*YC*YC + CC*YS*YS - 2.0*CS*YC*YS)/(YY*D)
        !write result in output array [per, pwr, a,b,c]
        periodogram(j,1) = period
        periodogram(j,2) = pwr
        do k = 1,3
            periodogram(j,k+2) = b(k)
        enddo

98  continue

end subroutine gls_pow

subroutine chi2(t, y, err, t_ref, max_pwr, chi2_out)
!   computes chi2 between data and a*coswt + b*sinwt c model
    double precision, intent(in) :: t(:), y(:), err(:)
    double precision, intent(in) :: max_pwr(5), t_ref
    double precision, intent(out) :: chi2_out
    double precision :: a, b, c, per, pi
    integer :: i, n_pts
    !pi def.
    pi = acos(-1.0d0)
    n_pts = size(t)
    !model parameters
    per = max_pwr(1)
    a = max_pwr(3)
    b = max_pwr(4)
    c = max_pwr(5)

    chi2_out = 0.0
    do 97 i = 1, n_pts
        chi2_out = chi2_out + &
                  (1.0/(err(i)*err(i)))*(y(i) - (a*cos(2.0*pi*(t(i)-t_ref)/per) +  &
                  b*sin(2.0*pi*(t(i)-t_ref)/per) + c))**2.0
97  continue

end subroutine chi2

subroutine chi2_0(y, err, chi2_0_out)
!   computes chi2 between data and a*coswt + b*sinwt c model
    double precision, intent(in) :: y(:), err(:)
    double precision, intent(out) :: chi2_0_out
    double precision :: mean_y
    integer :: i, j

    chi2_0_out = 0.0
    mean_y = 0.0
    do 96 j = 1,size(y)
        mean_y = mean_y + y(j)
96  continue
    mean_y = mean_y/size(y)

    do 97 i = 1,size(y)
        chi2_0_out = chi2_0_out + &
                  (1.0/(err(i)*err(i)))*(y(i) - mean_y)**2.0
97  continue

end subroutine chi2_0


program NAPLES
! Description:
!    Program NAPLES (Numerical Analysis of PLanetary encounterS).
!    Propagates orbits in the Earth-Sun circular, restricted three-body problem
!    starting from initial conditions assigned at the minimum approach distance.
!    The initial conditions are parametrized in (d,e,theta), and *one* change of
!    primaries is performed when passing the geocentric distance RSw, according to
!    the Trajectory Splitting algorithm.
!
!    Reference: Amato D., Baù G., and Bombardelli C., "Accurate numerical orbit
!    propagation of planetary close encounters". Submitted to MNRAS. 2016.
!
! Author:
!    Davide Amato
!    Space Dynamics Group - Technical University of Madrid
!    d.amato@upm.es
!
! ==============================================================================

! MODULES
use KINDS,       only: ik,dk,qk
use SETTINGS,    only: d_int,e_int,th_int,RSW_int,n_d,n_e,n_th,n_rs,espacing,&
&tolref,tol,eqs_H,eqs_CE,integ,path
use CONSTANTS,   only: REarth,wEarth,muEarth,muSun,R_SoI,smaEarth,d2r,pi,&
&secsPerDay,twopi
use AUXILIARIES, only: JD_i,JD_stop,JD_CA,time_steps,meth_switch,RSwitch,&
&ind_time,inSoI
use INTEGRATION, only: er_avg,eval_ratio,called
use IO,          only: id_set,id_ares,id_rres,id_enres,id_stats
use IO,          only: READ_SETTINGS,WRITE_SETTINGS,CREATE_ENRES,CREATE_ABSRES,&
&CREATE_RELRES,CREATE_SMARES,CREATE_STATS,WRITE_RESULTS,WRITE_EXCEPTION
use REFERENCE_TRAJECTORY_MOD, only: REFERENCE_TRAJECTORY_COWELL
use TEST_PROP,   only: DP_TRAJECTORY
use PROCESSING,  only: RESIDUALS
use THIRDBODIES, only: QPOS_VEL_CIRC

! VARIABLES
implicit none
! Parameter meshing
integer               ::  i_d,j_e,k_t,l_r,i_sim ! Counters
real(qk),allocatable  ::  d_arr(:)              ! [R_Earth]
real(qk),allocatable  ::  e_arr(:)              ! [-]
real(qk),allocatable  ::  th_arr(:)             ! [deg]
real(qk),allocatable  ::  RSw_arr(:)            ! [AU]
real(qk)  ::  dmin,ecc,theta                    ! [km,-,rad]
real(qk)  ::  delta
! Initial conditions
real(qk)  ::  R_CA_pl(1:3),V_CA_pl(1:3)  ! Geo position and velocity @ close approach (t = 0) [km,km/s]
real(qk)  ::  R_CA(1:3),V_CA(1:3)        ! Helio position and velocity @ close approach (t = 0) [km,km/s]
real(qk)  ::  TEarth
! Trajectory data
real(qk),allocatable  ::  traj_bwd(:,:),traj_fwd(:,:),ref_traj(:,:)
real(dk),allocatable  ::  dp_H1(:,:), dp_CE(:,:), dp_H2(:,:), dp_full(:,:)
real(qk)  ::  state_cp(1:4,1:7)          ! State at checkpoints (test)
real(qk)  ::  state_cp_ref(1:4,1:7)      ! State at checkpoints (reference)
real(qk)  ::  yD_Earth(1:6)
real(qk)  ::  R_i(1:3), V_i(1:3)
integer   ::  lenref, len_H1, len_CE, len_H2
! Times
integer   ::  ind_in_bwd,ind_out_fwd
integer   ::  ind_in,ind_out
integer   ::  ind_in_ref,ind_out_ref
real(qk)  ::  JD_in,JD_out,JD_f
! Results
real(dk)  ::  dR_abs(1:3),dR_rel(1:3),dV_abs(1:3),dV_rel(1:3)
real(dk)  ::  dEn_rel(1:3),dSMA_abs(1:3)
! Sanity checks, settings and diagnostics
integer   ::  istate_bwd,istate_fwd
integer   ::  idiag(1:10)
real(dk)  ::  rdiag(1:3)
integer   ::  totsim,step_mess,i_fail
integer   ::  eqs
real(dk)  ::  RSw_eff(1:2),RSw_diff
integer   ::  XC,MF(1:3)
integer   ::  callsArr(1:2),calls_end,istepsArr(1:2),isteps_end
real(dk)  ::  mordArr(1:2),mord_end
integer   ::  ncheck
logical   ::  sanity
! Auxiliary variables
real(qk)  ::  yt_quad(1:7)

! Output
character(len=8)             ::  date,date_end
character(len=10)            ::  time,time_end
character(len=14)            ::  runID
character(len=4096)          ::  ppath

! Debug
integer  ::  ii

write(*,'(80(''*''))')
write(*,'(''*'',35x,a7,36x,''*'')') 'NAPLES'
write(*,'(''*'',17x,a43,18x,''*'')') 'Numerical Analysis of PLanetary EncounterS.'
write(*,'(80(''*''))')

! ==============================================================================
! 01. READ INPUTS FROM FILE
! ==============================================================================

call READ_SETTINGS()
write(*,'(/,a)') 'Press ENTER to continue... '
read(*,*)

call DATE_AND_TIME(date,time)
runID = date//time(1:6)
write(*,'(4a,/)') 'Simulation ',date,time(1:6),' starting.'

! ==============================================================================
! 02. GENERATE (d,e,th) MESH
! ==============================================================================

allocate(d_arr(1:n_d))
allocate(e_arr(1:n_e))
allocate(th_arr(1:n_th))
allocate(RSw_arr(1:maxval([n_rs,1])))

! d mesh (linear)
delta = (d_int(2) - d_int(1))/(n_d - 1)
d_arr(1) = d_int(1)
do i_d = 2,n_d
    d_arr(i_d) = d_arr(1) + (i_d - 1)*delta
end do

! e mesh (linear or log)
select case (espacing)
case(1)
    delta = (e_int(2) - e_int(1))/(n_e - 1)
    e_arr(1) = e_int(1)
    do j_e = 2,n_e
        e_arr(j_e) = e_arr(1) + (j_e - 1)*delta
    end do
case(2)
    delta = (log10(e_int(2)) - log10(e_int(1)))/(n_e - 1)
    e_arr(1) = e_int(1)
    do j_e = 2,n_e
        e_arr(j_e) = 10._qk**(log10(e_arr(1)) + (j_e - 1)*delta)
    end do
end select

! theta mesh (linear)
delta = (th_int(2) - th_int(1))/(n_th - 1)
th_arr(1) = th_int(1)
do k_t = 2,n_th
    th_arr(k_t) = th_arr(1) + (k_t - 1)*delta
end do

! RSw mesh (linear)
delta = (RSw_int(2) - RSw_int(1))/(n_rs - 1)
RSw_arr(1) = RSW_int(1)
do l_r = 2,n_rs
    RSw_arr(l_r) = RSw_arr(1) + (l_r - 1)*delta
end do

! ==============================================================================
! 03. OPEN OUTPUT FILES
! ==============================================================================

ppath = trim(path)//runID//'/'
call SYSTEM('mkdir '//trim(ppath))
call WRITE_SETTINGS(trim(ppath)//'settings_'//runID//'.txt',runID)
call CREATE_ENRES(trim(ppath)//'enres_'//runID//'.dat')
call CREATE_ABSRES(trim(ppath)//'absres_'//runID//'.dat')
call CREATE_RELRES(trim(ppath)//'relres_'//runID//'.dat')
call CREATE_SMARES(trim(ppath)//'smares_'//runID//'.dat')
call CREATE_STATS(trim(ppath)//'stats_'//runID//'.dat')

! ==============================================================================
! 04. LOOP OVER THE PARAMETERS
! ==============================================================================

! Further misc initializations
wEarth = sqrt((muEarth+muSun)/smaEarth**3)
R_SoI  = (muEarth/muSun)**(2._qk/5._qk)*smaEarth
d2r = pi/180._qk
i_sim = 0; i_fail = 0
totsim = n_d*n_e*n_th*maxval([n_rs,1])
er_avg = 0; eval_ratio = 0
if (totsim <= 10000) then
  step_mess = 10
else if (totsim > 10000 .and. totsim < 1000000) then
  step_mess = 100
else
  step_mess = 1000
end if
lenref = 0;
len_H1 = 0; len_CE = 0; len_H2 = 0

d_loop: do i_d = 1,n_d

  dmin = d_arr(i_d)*REarth

  e_loop: do j_e = 1,n_e

    ecc = e_arr(j_e)

    th_loop: do k_t = 1,n_th

      theta = th_arr(k_t)*d2r

      RSw_loop: do l_r = 1,maxval([n_rs,1])

        RSwitch = Rsw_arr(l_r)*smaEarth

        i_sim = i_sim + 1

        ! OUTPUT TO USER
        if (mod(i_sim,step_mess)==0) then
          write(*,'(a,f6.2,3(a,i6),2(a,f6.2))') 'Progress: ',real(i_sim)/real(totsim)*100.,&
          &'%; completed ',i_sim,' out of ',totsim,'; failed ',i_fail,' ( ',&
          &real(i_fail)/real(i_sim)*100.,'%)'
        end if

        ! ======================================================================
        ! 05. INITIALIZE HELIOCENTRIC STATE
        ! ======================================================================

        ! Geocentric state
        R_CA_pl = dmin*[cos(theta),sin(theta),0._qk]
        V_CA_pl = sqrt(muEarth/sqrt(dot_product(R_CA_Pl,R_CA_Pl))*(1._qk + ecc))*&
        [-sin(theta),cos(theta),0._qk]

        ! Heliocentric state
        R_CA = [smaEarth,0._qk,0._qk] + R_CA_pl
        V_CA = [0._qk,sqrt((muSun+muEarth)/smaEarth),0._qk] + V_CA_pl

        ! Propagation interval: 6 months before and after close approach.
        JD_CA = 0._qk/secsPerDay
        TEarth = twopi*sqrt(smaEarth**3/(muSun+muEarth))
        JD_i  = (JD_CA - 0.5_qk*TEarth)/secsPerDay
        JD_f  = (JD_CA + 0.5_qk*TEarth)/secsPerDay

        ! Integration diagnostics
        istate_bwd = 1_ik; istate_fwd = 1_ik
        meth_switch = .false.
        MF = 0
        XC = 0
        calls_end = 0; callsArr = 0
        isteps_end = 0; istepsArr = 0
        mord_end = 0._dk; mordArr = 0
        RSw_eff = 0._dk

        ! ======================================================================
        ! 05a. REFERENCE TRAJECTORY - BWD
        ! ======================================================================

        call REFERENCE_TRAJECTORY_COWELL(R_CA_pl,V_CA_pl,JD_CA,JD_i,tolref,traj_bwd,&
        &JD_in,ind_in_bwd,istate_bwd)
        if (EXCEPTION_CHECK(istate_bwd,phase=0)) cycle RSw_loop

        ! ======================================================================
        ! 05b. REFERENCE TRAJECTORY - FWD
        ! ======================================================================

        call REFERENCE_TRAJECTORY_COWELL(R_CA_pl,V_CA_pl,JD_CA,JD_f,tolref,&
        &traj_fwd,JD_out,ind_out_fwd,istate_fwd)
        if (EXCEPTION_CHECK(istate_fwd,phase=0)) cycle RSw_loop

        ! Save reference trajectory
        lenref = size(traj_bwd,1) - 1_ik + size(traj_fwd,1)
        allocate(ref_traj(lenref,1:7)); ref_traj = 0._qk
        ref_traj(1:size(traj_bwd,1) - 1,:) = traj_bwd(size(traj_bwd,1):2:-1,:)
        ref_traj(size(traj_bwd,1):lenref,:) = traj_fwd

        ! Save indices of times corresponding to switches (useful in the case in
        ! wich the reference and test trajectories are output at different times)
        ind_in_ref = size(traj_bwd,1) - ind_in_bwd + 1
        ind_out_ref = size(traj_bwd,1) + ind_out_fwd - 1

        ! Save time steps vector (excluding the switch times).
        ! This is for output by LSODAR in the test propagation.
        if (allocated(time_steps)) deallocate(time_steps)
        allocate(time_steps(1:lenref),source=ref_traj(:,1))

        ! ======================================================================
        ! 06. TEST PROPAGATION
        ! ======================================================================

        ! === START OF PHASE H1 (heliocentric) ===

        ! Initialize sphere of influence flag and set the first output time
        inSoI = .false.
        ind_time = 2

        state_cp(1,:) = ref_traj(1,:)
        if (n_rs /= 0) then
          JD_stop = JD_in
        else if (n_rs == 0) then
          JD_stop = JD_f
        end if
        eqs = eqs_H

        call DP_TRAJECTORY(state_cp(1,2:4),state_cp(1,5:7),JD_i,JD_stop,eqs,&
        &integ,tol,dp_H1,idiag,rdiag)
        if (EXCEPTION_CHECK(idiag(2),phase=1)) cycle RSw_loop

        len_H1 = size(dp_H1,1)
        if (n_rs == 0) then
          state_cp(4,:) = dp_H1(len_H1,:)

          ! Save diagnostics
          calls_end = calls_end + idiag(1)
          isteps_end = isteps_end + idiag(3)
          mord_end = mord_end + rdiag(2)

        else
          state_cp(2,:) = dp_H1(len_H1,:)

          ! Save diagnostics
          calls_end = calls_end + idiag(1);   callsArr(1) = idiag(1)
          isteps_end = isteps_end + idiag(3); istepsArr(1) = idiag(3)
          mord_end = mord_end + rdiag(2);     mordArr(1) = rdiag(2)

          ! === START OF PHASE CE (planetocentric) ===

          inSoI = .true.

          ! Switch to the planetocentric frame in quad precision.
          yD_Earth = QPOS_VEL_CIRC(real(dp_H1(len_H1,1),qk),wEarth,smaEarth)
          R_i = state_cp(2,2:4) - yD_Earth(1:3)
          V_i = state_cp(2,5:7) - yD_Earth(4:6)

          JD_stop = JD_out
          eqs = eqs_CE

          call DP_TRAJECTORY(R_i,V_i,state_cp(2,1)/secsPerDay,JD_stop,eqs,&
          &integ,tol,dp_CE,idiag,rdiag)
          if (EXCEPTION_CHECK(idiag(2),phase=2)) cycle RSw_loop

          ! Save diagnostics
          calls_end = calls_end + idiag(1);   callsArr(2) = idiag(1)
          isteps_end = isteps_end + idiag(3); istepsArr(2) = idiag(3)
          mord_end = mord_end + rdiag(2);     mordArr(2) = rdiag(2)

          len_CE = size(dp_CE,1)
          ! Save effective switch radii
          RSw_eff(1) = real(sqrt(dot_product(R_i,R_i)),dk)
          RSw_eff(2) = sqrt(dot_product(dp_CE(len_CE,2:4),dp_CE(len_CE,2:4)))
          RSw_eff    = real(RSw_eff/smaEarth,dk)

          ! dp_CE is planetocentric, convert to heliocentric in quad precision.
          helio_conv: do ii = 1,len_CE
            yt_quad = dp_CE(ii,1:7)
            dp_CE(ii,2:7) = real(yt_quad(2:7) + &
            &QPOS_VEL_CIRC(yt_quad(1),wEarth,smaEarth),dk)
          end do helio_conv

          state_cp(3,:) = dp_CE(len_CE,:)

          ! === START OF PHASE H2 (heliocentric) ===

          inSoI = .false.
          R_i = state_cp(3,2:4)
          V_i = state_cp(3,5:7)

          JD_stop = JD_f
          eqs = eqs_H

          call DP_TRAJECTORY(R_i,V_i,state_cp(3,1)/secsPerDay,JD_stop,eqs,&
          &integ,tol,dp_H2,idiag,rdiag)
          if (EXCEPTION_CHECK(idiag(2),phase=3)) cycle RSw_loop

          ! Save diagnostics
          calls_end = calls_end + idiag(1)
          isteps_end = isteps_end + idiag(3)
          mord_end = mord_end + rdiag(2); mord_end = mord_end/3._dk

          len_H2 = size(dp_H2,1)
          state_cp(4,:) = dp_H2(len_H2,:)

          ! === END OF THE PROPAGATION ===

        end if

        ! ======================================================================
        ! 07. PROCESS AND SAVE OUTPUT
        ! ======================================================================

        ! If n_rs == 0 (HELIO mode) then ind_in = len_H1 = size(dp_H1,1).
        ind_in = len_H1
        ind_out = len_H1 + len_CE - 1

        ! Merge the trajectories together in dp_full(:,:)
        ! Temporary fix: only if integ == 1
        if (allocated(dp_full)) deallocate(dp_full)
        allocate(dp_full(1:lenref,1:7))
        ! Sanity check on the length of the trajectories for LSODAR
        if (integ == 1) then
          if ( (len_H1 + len_CE + len_H2 - 2 /= lenref) .and. n_rs /= 0) then
            sanity = EXCEPTION_CHECK(-13,3)
            cycle RSw_loop
          end if
          dp_full(1:ind_in,:) = dp_H1(1:len_H1,:)
          if (n_rs /= 0) then
            dp_full(ind_in+1:ind_out,:) = dp_CE(2:len_CE,:)
            dp_full(ind_out+1:lenref,:) = dp_H2(2:len_H2,:)
          end if

        end if

        ! Save checkpoint values of the reference trajectories
        state_cp_ref(1,:) = ref_traj(1,:)
        state_cp_ref(2,:) = ref_traj(ind_in_ref,:)
        state_cp_ref(3,:) = ref_traj(ind_out_ref,:)
        state_cp_ref(4,:) = ref_traj(lenref,:)
        ncheck = 3

        call RESIDUALS(ncheck,state_cp_ref(2:4,:),state_cp(2:4,:),smaEarth,muSun,&
        &RSw_arr(l_r),dR_abs,dR_rel,dV_abs,dV_rel,dEn_rel,dSMA_abs,RSw_eff,RSw_diff)

        ! OUTPUT RESIDUALS
        call WRITE_RESULTS(i_d,j_e,k_t,l_r,d_arr(i_d),e_arr(j_e),th_arr(k_t),&
        &RSw_arr(l_r),dR_abs,dR_rel,dV_abs,dV_rel,dEn_rel,dSMA_abs,RSw_diff,MF,&
        &XC,callsArr,istepsArr,mordArr,calls_end,mord_end,isteps_end)

        ! Flush output to disk every 100 propagations
        call FLUSHO()

        ! Deallocation for next iteration
        call CLEAN()

      end do RSw_loop
    end do th_loop
  end do e_loop
end do d_loop

! ==============================================================================
! 08. FINALIZATION
! ==============================================================================

! Deallocations
deallocate(d_arr)
deallocate(e_arr)
deallocate(th_arr)
deallocate(RSw_arr)
call CLEAN()
close(id_set); close(id_ares); close(id_rres); close(id_enres); close(id_stats)

call DATE_AND_TIME(date_end,time_end)
write(*,'(/,a)') 'Simulation '//runID//&
&' completed on '//date_end(1:4)//'/'//date_end(5:6)//'/'//date_end(7:8)//&
&' at '//time_end(1:2)//':'//time_end(3:4)//':'//time_end(5:6)//'.'

if (integ == 2) then
  ! Display the average ratio of function evaluations
  er_avg = er_avg/called
  write(*,'(a,f7.1,a)') 'Root-finding evaluations / stepping evaluations = ',&
  &er_avg*100._dk,'% (avg.)'

end if

contains

subroutine CLEAN()

  implicit none
  if (allocated(ref_traj)) deallocate(ref_traj)
  if (allocated(traj_bwd)) deallocate(traj_bwd)
  if (allocated(traj_fwd)) deallocate(traj_fwd)
  if (allocated(time_steps)) deallocate(time_steps)
  if (allocated(dp_H1)) deallocate(dp_H1)
  if (allocated(dp_CE)) deallocate(dp_CE)
  if (allocated(dp_H2)) deallocate(dp_H2)
  if (allocated(dp_full)) deallocate(dp_full)

end subroutine

function EXCEPTION_CHECK(istate,phase)

integer,intent(in)  ::  istate,phase
logical  ::  EXCEPTION_CHECK

  EXCEPTION_CHECK = .false.
  if (meth_switch) MF(phase) = 1
  if (istate < 0) then
    XC = phase*100 - istate
    call WRITE_EXCEPTION(i_d,j_e,k_t,l_r,&
    &d_arr(i_d),e_arr(j_e),th_arr(k_t),Rsw_arr(l_r),MF,XC)
    call CLEAN()
    call FLUSHO()
    i_fail = i_fail + 1
    EXCEPTION_CHECK = .true.
  end if

end function EXCEPTION_CHECK

subroutine FLUSHO()
! Close and reopen the output files every 100th propagation
! to flush buffer to disk.

  if (mod(i_sim,100)==0) then
    close(id_set); close(id_ares); close(id_rres); close(id_enres)
    close(id_stats)

    open(unit=id_set,file=trim(ppath)//'settings_'//runID//'.txt',&
    &status='old',action='write',position='append')
    open(unit=id_enres,file=trim(ppath)//'enres_'//runID//'.dat',&
    &status='old',action='write',position='append')
    open(unit=id_ares,file=trim(ppath)//'absres_'//runID//'.dat',&
    &status='old',action='write',position='append')
    open(unit=id_rres,file=trim(ppath)//'relres_'//runID//'.dat',&
    &status='old',action='write',position='append')
    open(unit=id_stats,file=trim(ppath)//'stats_'//runID//'.dat',&
    &status='old',action='write',position='append')
  end if

end subroutine FLUSHO

end program NAPLES


!###############################################################
!     PROGRAM  : BUILD THERMOSTATING BATHS
!     AUTHORS  : Adriano Amaricci
!###############################################################
module NEQ_THERMOSTAT
  USE NEQ_INPUT_VARS
  USE NEQ_CONTOUR
  USE NEQ_CONTOUR_GF
  USE SF_ARRAYS
  USE SF_SPECIAL
  USE SF_IOTOOLS
  implicit none
  private
  real(8),allocatable,dimension(:) :: bath_dens,wfreq

  public                           :: get_thermostat_bath

contains

  !+-------------------------------------------------------------------+
  !PURPOSE  : Build the Bath part of the system using exclusively time
  !dependent formalism. The bath is not interacting so it is
  ! time translation invariant.
  !+-------------------------------------------------------------------+
  subroutine get_thermostat_bath(S0,params)
    type(kb_contour_gf)                   :: S0
    type(kb_contour_params)               :: params
    integer                               :: N,L
    integer                               :: iw,i,j,k,ints
    real(8)                               :: en,w,dw,wmax
    real(8)                               :: ngtr,nless,arg,Vhopping
    complex(8)                            :: peso,sless,sgtr
    complex(8),dimension(:,:),allocatable :: S0gtr
    logical                               :: check

    if(.not.params%status)stop "thermostat/get_thermostat_bath: params not allocated. "
    if(.not.S0%status)stop "thermostat/get_thermostat_bath: S0 not allocated. "
    N = params%Ntime      !<== work with the MAX size of the contour
    L = params%Ntau

    write(*,"(A)")"Bath coupling is:"//txtfy(Vbath)
    Vhopping=sqrt(Vbath*2.d0*Wbath)
    write(*,"(A)")"Bath hopping, width are:"//reg(txtfy(Vhopping))//","//reg(txtfy(2.d0*Wbath))
    write(*,"(A)")"Bath levels are:"//txtfy(Lbath)

    allocate(bath_dens(Lbath),wfreq(Lbath))
    wmax  = Wbath
    wfreq = linspace(-wmax,wmax,Lbath,mesh=dw)
    select case(reg(bath_type))
    case("bethe")
       call get_bath_bethe_dos()
    case("gaussian")
       call get_bath_gaussian_dos()
    case ("flat")
       call get_bath_flat_dos()
    case("pgflat")
       call get_bath_pgflat_dos()
    case("gapflat")
       wmax  = Wbath+Wgap       !2.d0*Wbath+Wgap
       wfreq = linspace(-wmax,wmax,L,mesh=dw)
       call get_bath_gapflat_dos()
    case default
       stop "Bath type: not supported."
    end select
    call splot("DOSbath.neqipt",wfreq,bath_dens,append=.true.)

    !Bath self-energies:
    check = check_dimension_kb_contour(S0,N,L)
    allocate(S0gtr(N,N))
    S0   = zero
    S0gtr= zero
    if(Vbath==0.d0)return
    do iw=1,Lbath
       en   = wfreq(iw)
       nless= fermi(en,beta)
       ngtr = fermi(en,beta)-1.d0 !it absorbs the minus sign of the greater functions
       do i=1,N
          !Less component:
          do j=1,i
             peso=exp(-xi*(params%t(i)-params%t(j))*en)
             S0%less(i,j)= S0%less(i,j) + xi*Vhopping**2*nless*peso*bath_dens(iw)*dw
             S0gtr(i,j) = S0gtr(i,j)    + xi*Vhopping**2*ngtr*peso*bath_dens(iw)*dw
          enddo
          !Lmix component:
          if(en>=0.d0)then
             do j=0,L
                if(beta*en>20.d0)then
                   peso=exp(-xi*en*params%t(i))*exp(-en*(beta-params%tau(j)))
                else
                   peso=exp(-xi*en*params%t(i))*exp(-en*(beta-params%tau(j)))/(1.d0+exp(-en*beta))
                endif
                S0%lmix(i,j) = S0%lmix(i,j) + xi*Vhopping**2*peso*bath_dens(iw)*dw
             enddo
          else
             do j=0,L
                if(beta*en<-20.d0)then
                   peso=exp(-xi*en*params%t(i))*exp(en*params%tau(j))
                else
                   peso=exp(-xi*en*params%t(i))*exp(-en*(beta-params%tau(j)))/(1.d0+exp(-en*beta))
                endif
                S0%lmix(i,j) = S0%lmix(i,j) + xi*Vhopping**2*peso*bath_dens(iw)*dw
             enddo
          endif
          !
       enddo
    enddo
    !Ret component:
    S0%ret = S0gtr-S0%less
    forall(i=1:params%Ntime,j=1:params%Ntime,i<j)S0%less(i,j)=-conjg(S0%less(j,i))
    call plot_kb_contour_gf("Sbath.neqipt",S0,params)
  end subroutine get_thermostat_bath

  !+-----------------------------------------------------------------+
  !PURPOSE  : Build constant BATH
  !+-----------------------------------------------------------------+
  subroutine get_bath_flat_dos()
    integer    :: i
    real(8)    :: w
    do i=1,Lbath
       w=wfreq(i)
       bath_dens(i)= step(Wbath-abs(w))/(2.d0*Wbath)
    enddo
  end subroutine get_bath_flat_dos

  subroutine get_bath_pgflat_dos()
    integer    :: i
    real(8)    :: w,norm
    norm=(Walpha+1.d0)/(2.d0*Wbath**(Walpha+1.d0))
    do i=1,Lbath
       w=wfreq(i)
       if(abs(w)>wbath)then
          bath_dens(i)=0.d0
       else
          bath_dens(i)=norm*abs(w)**Walpha
       end if
    end do
  end subroutine get_bath_pgflat_dos

  subroutine get_bath_gapflat_dos()
    integer    :: i
    real(8)    :: w,rho
    rho=1.d0/2.d0/(Wbath)!-Wgap)
    do i=1,Lbath
       w=wfreq(i)
       if(abs(w)<Wbath+Wgap.AND.abs(w)>Wgap)then
          bath_dens(i)=rho
       else
          bath_dens(i)=0.d0
       end if
    end do
  end subroutine get_bath_gapflat_dos

  subroutine get_bath_gaussian_dos()
    integer    :: i,ik
    real(8)    :: w,sig,alpha
    complex(8) :: gf,zeta
    bath_dens = exp(-0.5d0*(wfreq/Wbath)**2)/(sqrt(pi2)*Wbath) !standard Gaussian
    !bath_dens = exp(-((wfreq)/Wbath)**2)/(sqrt(pi)*Wbath) !Camille's choice
  end subroutine get_bath_gaussian_dos

  subroutine get_bath_bethe_dos()
    integer    :: i,ik
    real(8)    :: w,sig,alpha
    complex(8) :: gf,zeta
    do i=1,Lbath
       w=wfreq(i)
       zeta=cmplx(w,eps,8)
       gf=gfbether(w,zeta,wbath/2.d0)
       bath_dens(i)=-dimag(gf)/pi
    enddo
  end subroutine get_bath_bethe_dos

end module NEQ_THERMOSTAT


c
c ----------------------------------------------------------------------
c
      subroutine hiocxx(fic,foc,gc,smass,swidth , hioc)
c
c this subroutine computes an off-shell scalar current from an external
c antifermion pair.
c
c input:
c       complex fic(6)         : flow-in  antifermion              |fic>
c       complex foc(6)         : flow-out antifermion              <foc|
c       complex gc(2)          : coupling constants                 gchf
c       real    smass          : mass  of output scalar s
c       real    swidth         : width of output scalar s
c
c output:
c       complex hioc(3)        : scalar current           j(<fic|s|foc>)
c
      implicit none
      double complex fic(6),foc(6),hioc(3),gc(2),dn
      double precision q(0:3),smass,swidth,q2
c
      hioc(2) = foc(5)-fic(5)
      hioc(3) = foc(6)-fic(6)

      q(0) = dble( hioc(2))
      q(1) = dble( hioc(3))
      q(2) = dimag(hioc(3))
      q(3) = dimag(hioc(2))
      q2 = q(0)**2-(q(1)**2+q(2)**2+q(3)**2)

      dn = -dcmplx( q2-smass**2, smass*swidth )

      hioc(1) = (  gc(1)*(foc(1)*fic(1)+foc(2)*fic(2))
     &           + gc(2)*(foc(3)*fic(3)+foc(4)*fic(4)) )/dn
c
      return
      end


      subroutine locate(xx,n,is,ie,x,j)
!-----------------------------------------------------------------------
!
! Given an array "xx" of length "n", and given a value "x", this routine
! returns a value "j" such that "x" is between xx(j) and xx(j+1).  xx
! must be monotonic, either increasing or decreasing.  j=is-1 or j=ie is
! returned to indicate that x is out of range.
!
! Bisection Concept From "Numerical Recipes", Press et. al. 1986  pp 90.
!-----------------------------------------------------------------------
      dimension xx(n)
!
! Initialize lower and upper methods:
!
      if(is.le.0) is = 1
      jl = is-1
      ju = ie
      if(xx(n).le.x) then
            j = ie
            return
      end if
!
! If we are not done then compute a midpoint:
!
 10   if(ju-jl.gt.1) then
            jm = (ju+jl)/2
!
! Replace the lower or upper limit with the midpoint:
!
            if((xx(ie).gt.xx(is)).eqv.(x.gt.xx(jm))) then
                  jl = jm
            else
                  ju = jm
            endif
            go to 10
      endif
!
! Return with the array index:
!
      j = jl
      return
      end


!>------------------------------------------------
!! Perform a basic quantile mapping from one variable to another
!!
!! Develop or apply quantile mapping.
!!
!!  @author
!!  Ethan Gutmann (gutmann@ucar.edu)
!!
!!------------------------------------------------
module quantile_mapping
    use sort_mod, only : sort
    use data_structures

    implicit none
    private
    public :: develop_qm
    public :: apply_qm

    integer, parameter :: DEFAULT_QM_SEGMENTS = 100
    real, parameter :: SMALL_VALUE = 1e-20

contains

    subroutine develop_qm(input_data, data_to_match, qm, n_segments)
        implicit none
        real, intent(in), dimension(:) :: input_data, data_to_match
        integer, intent(in), optional :: n_segments
        type(qm_correction_type), intent(out) :: qm

        integer :: ni, nm, nseg
        integer :: i
        real    :: input_i, input_step, match_i, match_step ! these are real to handle more bins than elements
        real    :: denominator
        real, dimension(:), allocatable :: input_data_sorted, data_to_match_sorted

        if ( present(n_segments) ) then
            nseg = n_segments
        else
            nseg = DEFAULT_QM_SEGMENTS
        end if

        ! setup the sorted input data to process
        ni = size(input_data)
        allocate(input_data_sorted(ni))
        call sort(input_data, input_data_sorted)

        ! setup the sorted matching data to process
        nm = size(data_to_match)
        allocate(data_to_match_sorted(nm))
        call sort(data_to_match, data_to_match_sorted)

        if (nseg<10) then
            write(*,*), "Quantile mapping with <10 segments not recommended, using 10 segments. "
            nseg=10
        endif
        input_step = ni/real(nseg)
        match_step = nm/real(nseg)

        input_i = 1
        match_i = 1

        if (allocated(qm%start_idx)) then
            deallocate(qm%start_idx,qm%end_idx,qm%slope,qm%offset)
        endif
        allocate(qm%start_idx(nseg))
        allocate(qm%end_idx(nseg))
        allocate(qm%slope(nseg))
        allocate(qm%offset(nseg))

        do i=1,nseg-1
            qm%start_idx(i) = input_data_sorted( int(input_i) )
            qm%end_idx(i)   = input_data_sorted( int(input_i + input_step) )
            qm%offset(i)    = data_to_match_sorted( int(match_i) )
            denominator     = max(SMALL_VALUE, qm%end_idx(i) - qm%start_idx(i))
            qm%slope(i)     = (data_to_match_sorted( int(match_i + match_step) ) - qm%offset(i)) &
                            / denominator

            input_i = input_i + input_step
            match_i = match_i + match_step
        end do

        ! For the last segment, make sure it finishes at the final value
        ! this will not happen otherwise because nseg invariably will not divide nm and ni evenly
        i = nseg
        qm%start_idx(i) = input_data_sorted( input_i )
        qm%end_idx(i)   = input_data_sorted( ni )
        qm%offset(i)    = data_to_match_sorted( match_i )
        denominator = max(SMALL_VALUE, qm%end_idx(i) - qm%start_idx(i))
        qm%slope(i)     = (data_to_match_sorted( nm ) - qm%offset(i)) &
                        / denominator

    end subroutine develop_qm

    ! apply a quantile mapping scheme to a given input value
    function qm_value(input, qm) result(output)
        implicit none
        real, intent(in) :: input
        type(qm_correction_type), intent(in) :: qm

        real :: output
        integer :: i, n, step
        logical :: found

        found=.False.
        n = size(qm%start_idx)
        i = n/2
        step = n/4+1

        ! search for the first point in start_idx with start > input
        ! Uses a log(n) binary search because we "know" the qm data are sorted.
        do while (.not.found)
            i = max(1,min(n,i))
            if (qm%start_idx(i) > input) then
                if (qm%start_idx(max(1,i-1)) <= input) then
                    found=.True.
                else if (i==1) then
                    found=.True.
                else
                    i = i-step
                    step = ceiling(step/2.0)
                endif
            else if (i==n) then
                found=.True.
            else
                i = i+step
                step = ceiling(step/2.0)
            endif
        end do
        if (i>1) then
            if (qm%start_idx(i) > input) then
                i=i-1
            endif
        endif

        output = qm%offset(i) + qm%slope(i) * (input - qm%start_idx(i))

    end function qm_value

    !>----------------------------------------------
    !! Apply a previously developed quantile mapping to input and store the results in output
    !!
    !!----------------------------------------------
    subroutine apply_qm(input, output, qm)
        implicit none
        real, intent(in),  dimension(:) :: input
        real, intent(out), dimension(:) :: output
        type(qm_correction_type), intent(in) :: qm

        integer :: i, n

        n = size(input)
        !$omp parallel default(shared) &
        !$omp firstprivate(i,n)
        !$omp do
        do i = 1, n
            output(i) = qm_value(input(i), qm)
        end do
        !$omp end do
        !$omp end parallel

    end subroutine apply_qm

end module quantile_mapping


*> \brief \b IZMAX1 finds the index of the vector element whose real part has maximum absolute value.
*
*  =========== DOCUMENTATION ===========
*
* Online html documentation available at
*            http://www.netlib.org/lapack/explore-html/
*
*> \htmlonly
*> Download IZMAX1 + dependencies
*>
*> [TGZ]
*>
*> [ZIP]
*>
*> [TXT]
*> \endhtmlonly
*
*  Definition:
*  ===========
*
*       INTEGER          FUNCTION IZMAX1( N, CX, INCX )
*
*       .. Scalar Arguments ..
*       INTEGER            INCX, N
*       ..
*       .. Array Arguments ..
*       COMPLEX*16         CX( * )
*       ..
*
*
*> \par Purpose:
*  =============
*>
*> \verbatim
*>
*> IZMAX1 finds the index of the element whose real part has maximum
*> absolute value.
*>
*> Based on IZAMAX from Level 1 BLAS.
*> The change is to use the 'genuine' absolute value.
*> \endverbatim
*
*  Arguments:
*  ==========
*
*> \param[in] N
*> \verbatim
*>          N is INTEGER
*>          The number of elements in the vector CX.
*> \endverbatim
*>
*> \param[in] CX
*> \verbatim
*>          CX is COMPLEX*16 array, dimension (N)
*>          The vector whose elements will be summed.
*> \endverbatim
*>
*> \param[in] INCX
*> \verbatim
*>          INCX is INTEGER
*>          The spacing between successive values of CX.  INCX >= 1.
*> \endverbatim
*
*  Authors:
*  ========
*
*> \author Univ. of Tennessee
*> \author Univ. of California Berkeley
*> \author Univ. of Colorado Denver
*> \author NAG Ltd.
*
*> \date September 2012
*
*> \ingroup complex16OTHERauxiliary
*
*> \par Contributors:
*  ==================
*>
*> Nick Higham for use with ZLACON.
*
*  =====================================================================
      INTEGER          FUNCTION IZMAX1( N, CX, INCX )
*
*  -- LAPACK auxiliary routine (version 3.4.2) --
*  -- LAPACK is a software package provided by Univ. of Tennessee,    --
*  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
*     September 2012
*
*     .. Scalar Arguments ..
      INTEGER            INCX, N
*     ..
*     .. Array Arguments ..
      COMPLEX*16         CX( * )
*     ..
*
* =====================================================================
*
*     .. Local Scalars ..
      INTEGER            I, IX
      DOUBLE PRECISION   SMAX
      COMPLEX*16         ZDUM
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          ABS
*     ..
*     .. Statement Functions ..
      DOUBLE PRECISION   CABS1
*     ..
*     .. Statement Function definitions ..
*
*     NEXT LINE IS THE ONLY MODIFICATION.
      CABS1( ZDUM ) = ABS( ZDUM )
*     ..
*     .. Executable Statements ..
*
      IZMAX1 = 0
      IF( N.LT.1 )
     $   RETURN
      IZMAX1 = 1
      IF( N.EQ.1 )
     $   RETURN
      IF( INCX.EQ.1 )
     $   GO TO 30
*
*     CODE FOR INCREMENT NOT EQUAL TO 1
*
      IX = 1
      SMAX = CABS1( CX( 1 ) )
      IX = IX + INCX
      DO 20 I = 2, N
         IF( CABS1( CX( IX ) ).LE.SMAX )
     $      GO TO 10
         IZMAX1 = I
         SMAX = CABS1( CX( IX ) )
   10    CONTINUE
         IX = IX + INCX
   20 CONTINUE
      RETURN
*
*     CODE FOR INCREMENT EQUAL TO 1
*
   30 CONTINUE
      SMAX = CABS1( CX( 1 ) )
      DO 40 I = 2, N
         IF( CABS1( CX( I ) ).LE.SMAX )
     $      GO TO 40
         IZMAX1 = I
         SMAX = CABS1( CX( I ) )
   40 CONTINUE
      RETURN
*
*     End of IZMAX1
*
      END


C
C**************************  BARIC_FACE **********************************
C
C  calculate the coordinates of the centroid of a face of a tetrahedron
C
C***********************************************************************
C
      subroutine baric_face(xl,yl,zl,xb,yb,zb)

      IMPLICIT  NONE

      REAL*8    xl(3), yl(3), zl(3),xb, yb, zb

      xb = (xl(1) + xl(2) + xl(3))/3.d0
      yb = (yl(1) + yl(2) + yl(3))/3.d0
      zb = (zl(1) + zl(2) + zl(3))/3.d0

      RETURN
      END


program geo2mag_benchmark

use, intrinsic :: iso_fortran_env, only: real64
use apex, only : apex_setup,gc2gm

implicit none (type, external)

integer :: i, N = 1000000
character(8) :: buf
real :: glat, glon, gmlat, gmlon, yeardec, r
real(real64) :: tic, toc

call get_command_argument(1, buf, status=i)
if(i/=0) error stop 'missing longitude (degrees)'
read(buf, '(f8.3)') glat

call get_command_argument(2, buf, status=i)
if(i/=0) error stop 'missing longitude (degrees)'
read(buf, '(f8.3)') glon

call get_command_argument(3, buf, status=i)
if(i/=0) error stop 'missing decimal year e.g. 2020.35'
read(buf, '(f8.4)') yeardec

print *, "geographic input lat, lon (degrees), year", glat, glon, yeardec

! -------------------------
call cpu_time(tic)
call apex_setup(yeardec,altmax=1000.)
call cpu_time(toc)
print *, toc-tic, ' seconds to setup Apex'

! -- benchmark
call cpu_time(tic)
do i = 1,N
  call random_number(r)
  call gc2gm(glat*r,glon*r,gmlat,gmlon)
  if (modulo(i,100000) == 0) print '(2F8.2,A,2f8.2,A,I8)', glat, glon,' geographic => geomagnetic lat, lon ',gmlat, gmlon, ' iter',i
end do
call cpu_time(toc)
! --- end benchmark

print *, 'geomagnetic lat, lon (degrees) ',gmlat, gmlon
print *, (toc-tic)/N, ' seconds per conversion'

end program