function [Kernel,Lambda_G] = GRAPPA_construct_kernel(ACS,Ry,Rz,Bx,By,Bz,Lambda,lambda_tk)
%
% function [Kernel,Lambda_G] = GRAPPA_construct_kernel(ACS,Ry,Rz,Bx,By,Bz,Lambda,lambda_tk)
%
% Perform Bx*By*Bz GRAPPA kernel calibration from ACS lines for 1*Ry*Rz
% acceleration and compute amplified noise for each (source) and target
% point based on noise covariance Lambda.
%
% Inputs:
%   ACS - 4-D ACS region to use for calibration (4th dim is coil channels)
%   Ry, Rz - accel. in y and z (phase-encode) directions
%   Bx, By, Bz - kernel size in x, y, and z directions (in 1*Ry*Rz blocks)
%   Lambda - noise covariance matrix
%   lambda_tk - (optional) Tikhonov regularization parameter (default: 0)
%
% Outputs:
%   Kernel - structure containing calibrated kernels and related info
%   Lambda_G - matrix of Ry*Rz amplified noise covariance matrices
%       (including source noise covariance Lambda in Lambda_G(:,:,1)
%
% Copyright 2012-10-09, Daniel Weller, University of Michigan

if ~exist('lambda_tk','var') || isempty(lambda_tk), lambda_tk = 0; end

% get ACS dimensions
[NACSx,NACSy,NACSz,NCha] = size(ACS);

% get indexes of Bx*By*Bz*NCha source points
srcx = ((1:Bx) - ceil(Bx/2));
srcy = ((1:By) - ceil(By/2))*Ry;
srcz = ((1:Bz) - ceil(Bz/2))*Rz;
[srcsx,srcsy,srcsz,srcsc] = ndgrid(srcx,srcy,srcz,1:NCha);

Kernel.offx = Bx - ceil(Bx/2); % = floor(Bx/2)
Kernel.offy = By - ceil(By/2);
Kernel.offz = Bz - ceil(Bz/2);

% get indexes of (Ry*Rz-1)*NCha target points
trgy = 0:Ry-1;
trgz = 0:Rz-1;
[trgsy,trgsz] = ndgrid(trgy,trgz);
trgsy = ndgrid(trgsy(2:end),1:NCha);
[trgsz,trgsc] = ndgrid(trgsz(2:end),1:NCha);

ptxmin = 1-min([srcsx(:);0]);
ptxmax = NACSx-max([srcsx(:);0]);
ptymin = 1-min([srcsy(:);trgsy(:)]);
ptymax = NACSy-max([srcsy(:);trgsy(:)]);
ptzmin = 1-min([srcsz(:);trgsz(:)]);
ptzmax = NACSz-max([srcsz(:);trgsz(:)]);
ptx = ptxmin:ptxmax;
pty = ptymin:ptymax;
ptz = ptzmin:ptzmax;
[ptsx,ptsy,ptsz] = ndgrid(ptx,pty,ptz);

% verify we have enough fit equations for # of source points
NFits = numel(ptsx);
NSrcs = numel(srcsx);
NTrgs = numel(trgsy);
if NFits < NSrcs
    error('BayesDESIGN:toofewACS','Too few ACS lines: only %d fits to calibrate %d weights.',NFits,NSrcs);
end
Kernel.NFits = NFits;
Kernel.NSrcs = NSrcs;
Kernel.NTrgs = NTrgs;
Kernel.Ry = Ry;
Kernel.Rz = Rz;
Kernel.Bx = Bx;
Kernel.By = By;
Kernel.Bz = Bz;

% fill calibration matrices
inds = outersum(ptsx(:),srcsx(:));
inds = inds + NACSx.*(outersum(ptsy(:),srcsy(:))-1);
inds = inds + (NACSx*NACSy).*(outersum(ptsz(:),srcsz(:))-1);
inds = inds + (NACSx*NACSy*NACSz).*(outersum(zeros(NFits,1),srcsc(:))-1);
srcmat = ACS(inds);

Kernel.srcvalues = srcmat;

inds = outersum(ptsx(:),zeros(NTrgs,1));
inds = inds + NACSx.*(outersum(ptsy(:),trgsy(:))-1);
inds = inds + (NACSx*NACSy).*(outersum(ptsz(:),trgsz(:))-1);
inds = inds + (NACSx*NACSy*NACSz).*(outersum(zeros(NFits,1),trgsc(:))-1);
trgmat = ACS(inds);

Kernel.trgvalues = trgmat;

if lambda_tk > 0
    trgmat = srcmat'*trgmat;
    srcmat = srcmat'*srcmat;
    srcmatnorm = norm(srcmat,'fro');
    srcmat = srcmat + (lambda_tk*srcmatnorm/size(srcmat,1)).*eye(size(srcmat,1));
    fprintf(1,'Condition number of Tikhonov-regularized %dx%d ACS source matrix is %g.\n',NFits,NSrcs,sqrt(cond(srcmat)));
    kernels = srcmat\trgmat;
else
    % compute kernels using SVD-based inverse
    [U,S,V] = svd(srcmat,'econ');
    s = abs(diag(S));
    fprintf(1,'Condition number of %dx%d ACS source matrix is %g.\n',NFits,NSrcs,s(1)/s(end));
    smin = eps(s(1));
    sinv = 1./s;
    sinv(s <= smin) = 0;
    Sinv = diag(sinv);
    kernels = V*Sinv*(U'*trgmat);
end
kernels = reshape(kernels,[Bx,By,Bz,NCha,Ry*Rz-1,NCha]);
Kernel.kernels = kernels;

% now, time to compute amplified noise
Lambda_G = zeros(NCha,NCha,Ry*Rz);
Lambda_G(:,:,1) = Lambda;
for ibx = 1:Bx
    kernels_bx = kernels(ibx,:,:,:,:,:);
    for iby = 1:By
        kernels_by = kernels_bx(:,iby,:,:,:,:);
        for ibz = 1:Bz
            kernels_bz = kernels_by(:,:,ibz,:,:,:);
            for ir = 1:Ry*Rz-1
                kernels_r = kernels_bz(:,:,:,:,ir,:);
                kernels_r = reshape(kernels_r,NCha,NCha).';
                Lambda_G(:,:,ir+1) = Lambda_G(:,:,ir+1) + kernels_r*Lambda*kernels_r';
            end
        end
    end
end

for ir = 1:Ry*Rz
    fprintf(1,'Gain/Condition number of Lambda_G(:,:,%d) = %g/%g.\n',ir,norm(Lambda_G(:,:,ir)),cond(Lambda_G(:,:,ir))); drawnow;
end

end

function c = outersum(a,b)

sza = size(a);
da = find(sza > 1,1,'last');
if isempty(da), da = 0; end
szb = size(b);
db = length(szb);
c = repmat(a,[ones(1,da),szb]);
b = shiftdim(b,-da);
c = c + repmat(b,[sza(1:da),ones(1,db)]);

end
