function [z0,MSE] = solve_fd_gradient_system(N,M,L,D,d,m,bc_flag)
% SOLVE_FD_GRADIENT_SYSTEM: build up A matrix and b vector for
% a given universal kriging systme of observation and estimation locations
% using the finite-difference approach to approximating gradients using
% only head values.

% Copyright (c) 2010 Kristopher L. Kuhlman (klkuhlm at sandia dot gov)
% 
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
% copies of the Software, and to permit persons to whom the Software is
% furnished to do so, subject to the following conditions:
% 
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
% 
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
% AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
% OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
% THE SOFTWARE.
%
% this function is part of the set of MATLAB scripts 
% comprising the implementation of a universal kriging
% program that computes estimates of potential via
% cokriging, taking into account boundary conditions.
%
% Kuhlman, K.L., and E. Pardo-Igúzquiza, 2010. Universal
% cokriging of hydraulic heads accounting for boundary 
% conditions, Journal of Hydrology, 384(1–2), 14–25.
% http://dx.doi.org/10.1016/j.jhydrol.2010.01.002

% m is a structure containing variogram model info
% d is a structure containing data 
% N,M are the # of head and gradient observations
% L is the order of the universal kriging monomial function
% D is the number of observations where the head is to be estimated
% m.rho is the small distance used to approximate the derivative
% bc_flag is 0 if no boundary condition info is to be used, 1 if it is

%% A is covariance matrix 
% (+ 1 Lagrange multiplier constraint)
if bc_flag == 1
    nd = N+M+L+1;
elseif bc_flag == 0
    nd = N+L+1;
else
    error('invalid choice for bc_flag')
end
A = zeros(nd, nd);

% b is data / prediction covariance vector
b = zeros(nd, D);

%% A: upper-left head-head covariance (A submatrix)
for i = 1:N
  for j = 1:i
    % lower triangle
    A(i,j) = covar_fcns(m, d.zlocs(i,1:2), d.zlocs(j,1:2), m.nugh);
    % upper triangle 
    A(j,i) = A(i,j);
  end
end

%% unbiasedness constraints for universal cokriging system
if L==0 % constant
    A(1:N,nd) = ones(N,1); % F
    A(nd,1:N) = ones(1,N); % F^T
    % G and G^T are zero for L=0 case
elseif L==2  % linear
    % 1 and x,y coordinates
    bloc = [ones(N,1), d.zlocs(1:N,1:2)];
    A(1:N,nd-2:nd) = bloc;  % F
    A(nd-2:nd,1:N) = transpose(bloc);  %F^T
    
    if bc_flag == 1
        bloc = [zeros(M,1), d.wlocs(1:M,1:2,1) - d.wlocs(1:M,1:2,2)]./(2*m.rho);
        A(N+1:N+M,N+M+1:N+M+3) =  bloc; % G
        A(N+M+1:N+M+3,N+1:N+M) = transpose(bloc); % G^T
    end
elseif L==5 % quadratic
    % 1, x,y, x^2, y^2, x*y
    bloc = [ones(N,1), d.zlocs(1:N,1:2), d.zlocs(1:N,1:2).^2, ...
            prod(d.zlocs(1:N,1:2),2)];
    A(1:N,nd-5:nd) = bloc;  % F
    A(nd-5:nd,1:N) = transpose(bloc);  %F^T
    
    if bc_flag == 1
        bloc = [zeros(M,1), d.wlocs(1:M,1:2,1) - d.wlocs(1:M,1:2,2), ...
            d.wlocs(1:M,1:2,1).^2 - d.wlocs(1:M,1:2,2).^2, ...
            prod(d.wlocs(1:M,1:2,1),2) - prod(d.wlocs(1:M,1:2,2),2)]./(2*m.rho);
        A(N+1:N+M,N+M+1:N+M+6) = bloc; % G
        A(N+M+1:N+M+6,N+1:N+M) = transpose(bloc); % G^T
    end
else
    error('invalid choice of L');
end

if bc_flag == 1
    
    %% A: off-diagonal head-gradient covariance (B submatrix)
    for j = N+1:N+M
        for i = 1:N
            A(i,j) = (covar_fcns(m, d.zlocs(i,1:2), d.wlocs(j-N,1:2,1), m.nugc) - ...
                      covar_fcns(m, d.zlocs(i,1:2), d.wlocs(j-N,1:2,2), m.nugc))./(2*m.rho);
            A(j,i) = A(i,j);
        end
    end

    %% A: lower-right gradient-gradient covariance (D submatrix)
    for i = N+1:N+M
        for j = N+1:i
            A(i,j) = (covar_fcns(m, d.wlocs(i-N,1:2,1), d.wlocs(j-N,1:2,1), m.nugg) - ...
                      covar_fcns(m, d.wlocs(i-N,1:2,2), d.wlocs(j-N,1:2,1), m.nugg) - ...
                      covar_fcns(m, d.wlocs(i-N,1:2,1), d.wlocs(j-N,1:2,2), m.nugg) + ...
                      covar_fcns(m, d.wlocs(i-N,1:2,2), d.wlocs(j-N,1:2,2), m.nugg))./(2*m.rho)^2;
            A(j,i) = A(i,j);
        end
    end
end

% % b is right-hand-side vector of kriging system
% calculation is vectorized across calculation dimension
% which is assumed to be the largest dimention

if L==0
    b(nd,1:D) = ones(1,D);
elseif L==2
    b(nd-2:nd,1:D) = [ones(1,D); transpose(d.estlocs(1:D,1:2))];
elseif L==5
    b(nd-5:nd,1:D) = [ones(1,D); transpose(d.estlocs(1:D,1:2)); ...
                      transpose(d.estlocs(1:D,1:2)).^2; ...
                      prod(transpose(d.estlocs(1:D,1:2)))];
end

for i = 1:N
    b(i,1:D) = covar_fcns(m, transpose(squeeze(d.zlocs(i,1:2,ones(D,1)))), ...
                            d.estlocs(1:D,1:2), m.nugc);
end
if bc_flag == 1
    for i = N+1:N+M
        b(i,1:D) = (covar_fcns(m, transpose(squeeze(d.wlocs(i-N,1:2,ones(D,1)))), ...
                                          d.estlocs(1:D,1:2), m.nugc) - ...
                    covar_fcns(m, transpose(squeeze(d.wlocs(i-N,1:2,2*ones(D,1)))), ...
                                          d.estlocs(1:D,1:2), m.nugc))./(2*m.rho);
    end
end

z0 = zeros(D,1);
MSE = zeros(D,1);
coeff = zeros(nd,D);

%% solve kriging system for all datapoints
coeff(1:nd,1:D) = A(1:nd,1:nd)\b(1:nd,1:D);

% find best estimate
z0(1:D,1) = dot(coeff(1:N,1:D), d.zdata(1:N,ones(D,1)),1);

% find MSE
MSE(1:D,1) = m.sigsq + m.nugh - dot(b(1:nd,1:D),coeff(1:nd,1:D),1);

