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

      subroutine gregfilt_loc(xens,yo,iobsloc,ngp,mobs,Rs,nens,nx,ny)

      implicit none

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

! Local Variables
      integer :: xob(mobs), yob(mobs)
      integer :: ind, k, j, i, r2, kk, jj, ko, jo, kj, g
      real*8    :: PHT(ngp), Ks(ngp), Khat(ngp)
      real*8    :: xp(ngp), xa(ngp), zp(ngp,nens), R(mobs)
      real*8    :: HPHT, alpha, boost

! 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
      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
      do k = 1, nens
         zp(:,k) = boost*(zp(:,k) - xp) + xp
      end do

!*** Find the xob and yob arrays from H
c      do j = 1, ngp
         do k = 1, mobs
c            if ( H(k,j) == 1. ) then
c               xob(k) = mod(j-1,nx) + 1
c               yob(k) = (j-1)/nx + 1
               xob(k) = mod(iobsloc(k)-1,nx) + 1
               yob(k) = (iobsloc(k)-1)/nx + 1
 c           end if
         end do
c      end do

!*** Now process each observation sequentially abiding by cut-off radius
      do j = 1, mobs

         ind = nx*( yob(j) - 1 ) + xob(j)

!*** Find PH' first
         PHT = 0.
         do jj = yob(j)-rad2, yob(j)+rad2 
            do kk = xob(j)-rad2, xob(j)+rad2 
               jo = jj
               ko = kk
           
!*** Point is within block of radius, but it may not be within the 
!      basin boundaries
               if ( ko>0 .and. ko<=nx .and. jo>0 .and. jo<=ny ) then

!*** Since we've sequestered a square of side 2*rad and the
!      cut-off radius assumes a circle, we need to check to
!      make sure the point we're considering is actually 
!      within the circle.

                  d = (ko - xob(j))**2 + (jo - yob(j))**2   

                  dr = sqrt( float( d ) ) 
           
!*** The element of interest in the 1D vector according to addresses 
!      kk and jj is:
                  kj = nx*(jo-1) + ko

!*** Evaluate the filter coefficient based on distance from center d
!		  filt = cov_factor(dr,rrad)
		  filt = 1.

!*** Now contribute to PHT sum
                  do g = 1, nens
                     PHT(kj) = PHT(kj) + filt*(zp(kj,g) - xp(kj))*  
     &                  (zp(ind,g) - xp(ind))
                  end do
              
               end if
            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
            
         do g = 1, nens
            zp(:,g) = ((zp(:,g) - xp) - Khat*( zp(ind,g) - 
     &           xp(ind) )) + xa
         end do

!*** Use analysis ensemble as the background for the next observation
         xp = xa

      end do

c      print*, 'EnSRF:: Done Mobs Loop'

      xens = zp

      return
  
      end subroutine gregfilt_loc

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

      implicit none

      double precision             :: cov_factor
      double precision, intent(in) :: z_in, c
      double precision             :: 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