c***  ensemble square root filter that uses an optional cut-off
c***  radius and boost factor.

      subroutine EnSRF3d(xens,yo,iobsloc,ngp,mobs,Rs,nens,nx,ny,nz,mask)

      implicit none

! Arguments
      integer, intent(in) :: nens, mobs, ngp, nx, ny, nz
      real*8, intent(inout) :: xens(ngp,nens)
      real*8, intent(in)    :: yo(mobs), Rs(mobs)
      integer, intent(in)    :: iobsloc(mobs)
      real*4, intent(in) :: mask(ny,nx)

! Local Variables
      integer :: xob(mobs), yob(mobs), zob(mobs),kkk,ytmp(ngp)
      integer :: ind, k, j, i, r2, kk, jj, ll, ko, jo, kjj, kj, g, dkx
      integer :: zz, zzob
      real*8    :: PHT(ngp), Ks(ngp), Khat(ngp)
      real*8    :: xp(ngp), xa(ngp), zp(ngp,nens), R(mobs)
      real*8    :: HPHT, alpha, boost, mx
      integer   :: count, count1, scount
! Filter Stuff
      integer :: d, rad, rad2
      real*8    :: dr, rrad, filt
      real*8, external :: cov_factor

c***  observational standard deviation
      R = sqrt(Rs)

c***  set cutoff radius
!      rad = 100
!      rad = 6 ! Case a6k-000
!      rad = 10  !Case a6k-001
        rad = 5  !Case a6k-002, ask-003
      rad2 = 2*rad
      r2 = rad*rad
      rrad = float(rad)

c***  set inflation factor
      boost = 1.00
      boost = 1.05
!      boost = 1.10

c***  rename ensemble matrix
      zp = xens

!*** Find the initial ensemble mean
      do j = 1, ngp
         xp(j) = sum(zp(j,:))/float(nens) 
      end do

!*** Apply inflation factor to initial ensemble
c      do k = 1, nens
c         zp(:,k) = boost*(zp(:,k) - xp) + xp
c      end do

!*** Find the xob and yob arrays from H, be careful!
!*** Things can go very disastrously wrong here... SR.
      do k = 1, mobs
         j = iobsloc(k)
         zob(k) = (j - 1)/(nx*ny)+1
         yob(k) = (j-(zob(k)-1)*nx*ny - 1)/nx + 1
         xob(k) = (j - ((zob(k)-1)*nx*ny + (yob(k)-1)*nx))
      end do
      scount = 0
!*** Now process each observation sequentially abiding by cut-off radius
      do j =  1,mobs

         ind = nx*( yob(j) - 1 ) + nx*ny*(zob(j)-1)+xob(j)
!*** Find PH' first
         PHT = 0.

         zzob = zob(j) - (zob(j)/nz)*nz
         if (zzob == 0) zzob = nz
c         write(*,*)  j, xob(j), yob(j), zzob, zob(j)
         do jj = yob(j)-rad2, yob(j)+rad2 
            do kk = xob(j)-rad2, xob(j)+rad2 
               do ll = zob(j)-rad2, zob(j)+rad2 
                  if (ll>=1 .and. ll<=nz*4) then
                     zz =     ll - (ll/nz)*nz
                     if (zz == 0)  zz = nz
                if (jj>8 .and. jj<ny .and. zz>=1 .and. zz<=nz) then
                        kjj = kk
                        if ( kk <= 0) kjj = nx - kk 
                        if (kk > nx)  kjj = kk-nx
                        d = (kjj - xob(j))**2 + (jj - yob(j))**2  +
     &                         (zz - zzob)**2   
                        dr = sqrt( float( d ) ) 
                        
                        kj = nx*(jj-1) + kjj + nx*ny*(ll-1)
                        filt = cov_factor(dr,rrad)
                        do g = 1, nens
                           PHT(kj) = PHT(kj) + filt*(zp(kj,g) - xp(kj))*  
     &                          (zp(ind,g) - xp(ind))
                        end do
                     end if
                  end if
                  end do
               end do
            end do
         PHT = PHT/float(nens - 1)

         
!*** Now find HPH' from PH'.  Because of cut-off radius, there is a
!      (good) chance that HPH' will be zero.
         HPHT = PHT(ind)
!*** Evaluate Ks
         Ks = PHT/( HPHT + Rs(j) )
!*** Update all effected elements in the mean
         xa = xp + Ks*( yo(j) - xp(ind) )
!*** Now update all ensemble members as perturbations about mean
         alpha = 1./( 1. + sqrt( Rs(j)/( HPHT + Rs(j) ) ) ) 
         Khat = alpha*Ks
         call system_clock(count)
         do g = 1, nens
            zp(:,g) = ((zp(:,g) - xp) - Khat*( zp(ind,g) - 
     &           xp(ind) )) + xa
         end do
c         call system_clock(count1)
c         write(*,*) (count1-count)
!*** Use analysis ensemble as the background for the next observation
         xp = xa

      end do

      write(*,*) scount, mobs
c      print*, 'EnSRF:: Done Mobs Loop'

      xens = zp

      return
  
      end subroutine EnSRF3D


 
!----------------------------------------------------------------------------
c***  distance filter
      function cov_factor(z_in, c)

      implicit none

      real*8             :: cov_factor
      real*8, intent(in) :: z_in, c
      real*8             :: z, r

      z = abs(z_in)
      r = z / c

      if ( z >= 2*c ) then
         cov_factor = 0.
      else if ( z >= c .and. z < 2*c ) then
         cov_factor = r**5/12. - r**4/2. + r**3 * 5./8. + 
     &      r**2 * 5./3. - 5*r + 4. - (2 * c) / (3 * z)
      else
         cov_factor = r**5 * (-1./4.) + r**4/2. + r**3 * 
     &      5./8. - r**2 * 5./3. + 1.
      end if

      end function cov_factor