function OUT = krige(h,p,M,S)

%KRIGE computes ordinary kriging estimates for a list S of 
%locations relative to values for a given subset of locations.
%The Variogram Parameters are INPUTS.

%Written by: TONY E. SMITH, 2/24/98

%Functions called: spher_mat.m, spher.m, distance.m
%
%INPUTS: (i)   h = bandwidth for krige estimate
%        (ii)  p = variogram parameters [range,sill,nug]
%        (iii) M = (nx3)-matrix of locations with values:
%                  (Xi,Yi,Vi), i = 1:n
%        (iv)   S = (Nx2)-matrix of locations (Xi,Yi) to be kriged.
%
%OUTPUT: OUT = (Nx2)-vector of kriged values, Ki, and standard
%               errors, SEi, at location (Xi,Yi)

%Compute Covariogram

n = length(M(:,1)) ;

U = ones(n,1) ;

V = M(:,3) ;

%computation of covariance

D = distance(M(:,1:2)) ;

C = p(2)*U*U' - spher_mat(p,D) ;

clear('D');

%Correct C on diagonal (where nugget effect appears)

C = C + (p(3)*eye(n)) ;

A = chol(C)';
        
AA = A\eye(n) ; 

mu = AA*U\AA*V ; % same as (U'*inv(C)*V)/(U'*inv(C)*U) ;


%Compute Kriged Values

N = length(S(:,1)) ;

OUT = zeros(N,2) ;

for i = 1:N
   
   %Identify points within distance h of location i
   
   X = S(i,1)* U - M(:,1) ;
   
   Y = S(i,2)* U - M(:,2) ;
   
   DD = sqrt(X.^2 + Y.^2) ;
   
   pos = max(zeros(n,1),(h*U - DD)) ; 
   
   L = find(pos) ;  %List of nonzero positions in DD
   
   m = length(L);
   
   if m < 3    %Extend L to the three closest points in M
      
      list = [[1:n]',DD];
      
      list = sortrows(list,2);
      
      L = list(2:4,1); %choose the 3 pts closest to i      
            
   end
   
   %Compute local covariances
   
   CC = inv(C(L,L)) ; %inverse of local cov matrix
   
   d = DD(L) ; %vector of distances to i
   
   c = p(2)*U(L) - spher(p,d); %local cov vector
   
   %Correct c at any zero-distance points
   
   LL = length(L) ;
   
   for j = 1:LL
      
      if d(j) == 0 
         
         c(j) = c(j) + p(3) ;
         
      end
      
   end
   
      
   OUT(i,1) = mu + c' * CC * (V(L) - mu*U(L)) ;
   
   OUT(i,2) = sqrt( p(2) - c' * CC * c + ...
              ((1 - U(L)'*CC*c)^2)/(U(L)'*CC*U(L))) ;
   
end