function ds = spm_uw_estimate(P,par)
% Estimation of partial derivatives of EPI deformation fields
%
% FORMAT [ds] = spm_uw_estimate((P),(par))
%
% P            - List of file names or headers.
% par          - Structure containing parameters governing the specifics
%                of how to estimate the fields.
% .M           - When performing multi-session realignment and Unwarp we
%                want to realign everything to the space of the first
%                image in the first time-series. M defines the space of
%                that.
% .order       - Number of basis functions to use for each dimension.
%                If the third dimension is left out, the order for
%                that dimension is calculated to yield a roughly
%                equal spatial cut-off in all directions.
%                Default: [12 12 *]
% .sfP         - Static field supplied by the user. It should be a
%                filename or handle to a voxel-displacement map in
%                the same space as the first EPI image of the time-
%                series. If using the FieldMap toolbox, realignment
%                should (if necessary) have been performed as part of
%                the process of creating the VDM. Note also that the
%                VDM must be in undistorted space, i.e. if it is
%                calculated from an EPI based field-map sequence
%                it should have been inverted before passing it to
%                spm_uw_estimate. Again, the FieldMap toolbox will
%                do this for you.
% .regorder    - Regularisation of derivative fields is based on the
%                regorder'th (spatial) derivative of the field.
%                Default: 1
% .lambda      - Fudge factor used to decide relative weights of
%                data and regularisation.
%                Default: 1e5
% .jm          - Jacobian Modulation. If set, intensity (Jacobian)
%                deformations are included in the model. If zero,
%                intensity deformations are not considered.
% .fot         - List of indexes for first order terms to model
%                derivatives for. Order of parameters as defined
%                by spm_imatrix.
%                Default: [4 5]
% .sot         - List of second order terms to model second
%                derivatives of. Should be an nx2 matrix where
%                e.g. [4 4; 4 5; 5 5] means that second partial
%                derivatives of rotation around x- and y-axis
%                should be modelled.
%                Default: []
% .fwhm        - FWHM (mm) of smoothing filter applied to images prior
%                to estimation of deformation fields.
%                Default: 6
% .rem         - Re-Estimation of Movement parameters. Set to unity means
%                that movement-parameters should be re-estimated at each
%                iteration.
%                Default: 0
% .noi         - Maximum number of Iterations.
%                Default: 5
% .exp_round   - Point in position space to do Taylor expansion around.
%                'First', 'Last' or 'Average'.
%                Default: 'Average'.
% ds           - The returned structure contains the following fields
% .P           - Copy of P on input.
% .sfP         - Copy of sfP on input (if non-empty).
% .order       - Copy of order on input, or default.
% .regorder    - Copy of regorder on input, or default.
% .lambda      - Copy of lambda on input, or default.
% .fot         - Copy of fot on input, or default.
% .sot         - Copy of sot on input, or default.
% .fwhm        - Copy of fwhm on input, or default.
% .rem         - Copy of rem on input, or default.
% .p0          - Average position vector (three translations in mm
%                and three rotations in degrees) of scans in P.
% .q           - Deviations from mean position vector of modelled
%                effects. Corresponds to deviations (and deviations
%                squared) of a Taylor expansion of deformation fields.
% .beta        - Coeffeicents of DCT basis functions for partial
%                derivatives of deformation fields w.r.t. modelled
%                effects. Scaled such that resulting deformation
%                fields have units mm^-1 or deg^-1 (and squares
%                thereof).
% .SS          - Sum of squared errors for each iteration.
%
%__________________________________________________________________________
% Copyright (C) 2003-2011 Wellcome Trust Centre for Neuroimaging

% Jesper Andersson
% $Id: spm_uw_estimate.m 6824 2016-06-27 16:19:51Z guillaume $

% This is a major rewrite which uses some new ideas to speed up
% the estimation of the field. The time consuming part is the
% evaluation of A'*A where A is a matrix with the partial
% derivatives for each scan with respect to the parameters
% describing the warp-fields. If we denote the derivative
% matrix for a single scan by Ai, then the estimation of A'*A
% is A'*A = A1'*A1 + A2'*A2 + ... +An'*An where n is the number
% of scans in the time-series. If we model the partial-derivative
% fields w.r.t. two movement parameters (e.g. pitch and roll), each
% by [8 8 8] basis-functions and the image dimensions are
% 64x64x64 then each Ai is a 262144x1024 matrix and we need to
% evaluate and add n of these 1024x1024 matrices. It takes a while.
%
% The new idea is based on the realisation that each of these
% matrices is the kroneceker-product of the relevant movement
% parameters, our basis set and a linear combination (given by
% one column of the inverse of the rotation matrix) of the image
% gradients in the x-, y- and z-directions. This means that
% they really aren't all that unique, and that the amount of
% information in these matrices doesn't really warrant all
% those calculations. After a lot of head-scratching I arrived
% at the following
%
% First some definitions
%
% n: no. of voxels
% m: no. of scans
% l: no. of effects to model
% order: [xorder yorder zorder] no. of basis functions
% q: mxl matrix of scaled realignment parameters for effects to model.
% T{i}: inv(inv(P(i).mat)*P(1).mat);
% t(i,:) = T{i}(1:3,2)';
% B: kron(Bz,By,Bx);
% Ax = repmat(dx,1,prod(order)).*B;
% Ay = repmat(dy,1,prod(order)).*B;
% Az = repmat(dz,1,prod(order)).*B;
%
% Now, my hypothesis is that A'*A is given by
%
% AtA = kron((q.*kron(ones(1,l),t(:,1)))'*(q.*kron(ones(1,l),t(:,1))),Ax'*Ax) +...
% kron((q.*kron(ones(1,l),t(:,2)))'*(q.*kron(ones(1,l),t(:,2))),Ay'*Ay) +...
% kron((q.*kron(ones(1,l),t(:,3)))'*(q.*kron(ones(1,l),t(:,3))),Az'*Az) +...
% 2*kron((q.*kron(ones(1,l),t(:,1)))'*(q.*kron(ones(1,l),t(:,2))),Ax'*Ay) +...
% 2*kron((q.*kron(ones(1,l),t(:,1)))'*(q.*kron(ones(1,l),t(:,3))),Ax'*Az) +...
% 2*kron((q.*kron(ones(1,l),t(:,2)))'*(q.*kron(ones(1,l),t(:,3))),Ay'*Az);
%
% Which turns out to be true. This means that regardless of how many
% scans we have we will always be able to create AtA as a sum of
% six individual AtAs. It has been tested against the previous
% implementation and yields exactly identical results.
%
% I know this isn't much of a derivation, but there will be a paper soon. Sorry.
%
% Other things that are new for the rewrite is
% 1. We have removed the possibility to try and estimate the "static" field.
%    There simply isn't enough information about that field, and for a
%    given time series the estimation is just too likely to fail.
% 2. New option to pass a static field (e.g. estimated from a dual
%    echo-time measurement) as an input parameter.
% 3. Re-estimation of the movement parameters at each iteration.
%    Let us say we have a nodding motion in the time series, and
%    at each nod the brain appear to shrink in the phase-encode
%    direction. The realignment will find the nods, but might in
%    addition mistake the "shrinks" for translations, leading to
%    biased estimates. We attempt to correct that by, at iteration,
%    updating both parameters pertaining to distortions and to
%    movement. In order to do so in an unbiased fashion we have
%    also switched from sampling of images (and deformation fields)
%    on a grid centered on the voxel-centers, to a grid whith a
%    different sampling frequency.
% 4. Inclusion of a regularisation term. The speed-up has facilitated
%    using more basis-functions, which makes it neccessary to impose
%    regularisation (i.e. punisihing some order derivative of the
%    deformation field).
% 5. Change of interpolation model from tri-linear/Sinc to
%    tri-linear/B-spline.
% 6. Option to include the Jacobian compression/stretching effects
%    in the model for the estimation of the displacement fields.
%    Our tests have indicated that this is NOT a good idea though.

SVNid = '$Rev: 6824 $';

%-Say hello
%--------------------------------------------------------------------------
SPMid = spm('FnBanner',mfilename,SVNid);

%-Parameters
%--------------------------------------------------------------------------
if nargin < 1 || isempty(P), P = spm_select(Inf,'image'); end
if ~isstruct(P), P = spm_vol(P); end

%
% Hardcoded default input parameters.
%
defpar = struct('sfP',             [],...
                'M',               P(1).mat,...
                'fot',             [4 5],...
                'sot',             [],...
                'hold',            [1 1 1 0 1 0]);

%
% Merge hardcoded defaults and spm_defaults. Translate spm_defaults
% settings to internal defaults.
%
ud = spm_get_defaults('unwarp.estimate');
if isfield(ud,'basfcn'),    defpar.order = ud.basfcn; end
if isfield(ud,'regorder'),  defpar.regorder = ud.regorder; end
if isfield(ud,'regwgt'),    defpar.lambda = ud.regwgt; end
if isfield(ud,'jm'),        defpar.jm = ud.jm; end
if isfield(ud,'fwhm'),      defpar.fwhm = ud.fwhm; end
if isfield(ud,'rem'),       defpar.rem = ud.rem; end
if isfield(ud,'noi'),       defpar.noi = ud.noi; end
if isfield(ud,'expround'),  defpar.exp_round = ud.expround; end

defnames = fieldnames(defpar);

%
% Go through input parameters, chosing the default
% for any parameters that are missing, warning the
% user if there are "unknown" parameters (probably
% reflecting a misspelling).
%

if nargin < 2 || isempty(par)
   par = defpar;
end
ds = [];
for i=1:length(defnames)
   if isfield(par,defnames{i}) && ~isempty(par.(defnames{i}))
      ds.(defnames{i}) = par.(defnames{i});
   else
      ds.(defnames{i}) = defpar.(defnames{i});
   end
end
parnames = fieldnames(par);
for i=1:length(parnames)
   if ~isfield(defpar,parnames{i})
      warning('Unknown par field %s',parnames{i});
   end
end

%
% Resolve ambiguities.
%
if length(ds.order) == 2
   mm          = sqrt(sum(P(1).mat(1:3,1:3).^2)).*P(1).dim(1:3);
   ds.order(3) = round(ds.order(1)*mm(3)/mm(1));
end
if isfield(ds,'sfP') && ~isempty(ds.sfP)
   if ~isstruct(ds.sfP)
      ds.sfP = spm_vol(ds.sfP);
   end
end
nscan = length(P);
nof   = prod(size(ds.fot)) + size(ds.sot,1);
ds.P  = P;

%
% Get matrix of 'expansion point'-corrected movement parameters
% for which we seek partial derivative fields.
%
[ds.q,ds.p0] = make_q(P,ds.fot,ds.sot,ds.exp_round);

%
% Create matrix for regularisation of warps.
%
H = make_H(P,ds.order,ds.regorder);

%
% Create a temporary smooth time series to use for
% the estimation.
%
old_P = P;
if ds.fwhm ~= 0
   spm_uw_show('SmoothStart',length(P));
   for i=1:length(old_P)
      spm_uw_show('SmoothUpdate',i);
      sfname(i,:) = [tempname spm_file_ext ',1,1'];
      to_smooth = sprintf('%s,%d,%d',old_P(i).fname,old_P(i).n);
      spm_smooth(to_smooth,sfname(i,:),ds.fwhm);
   end
   P = spm_vol(sfname);
   spm_uw_show('SmoothEnd');
end

% Now that we have littered the disk with smooth
% temporary files we  should use a try-catch
% block for the rest of the function, to ensure files
% get deleted in the event of an error.

try        % Try block starts here

%
% Initialize some stuff.
%
beta0    = zeros(nof*prod(ds.order),10);
beta     = zeros(nof*prod(ds.order),1);
old_beta = zeros(nof*prod(ds.order),1);

%
% Sample images on irregular grid to avoid biasing
% re-estimated movement parameters with smoothing
% effect from voxel-interpolation (See Andersson ,
% EJNM (1998), 25:575-586.).
%

ss         = [1.1 1.1 0.9];
xs         = 1:ss(1):P(1).dim(1); xs = xs + (P(1).dim(1)-xs(end)) / 2;
ys         = 1:ss(2):P(1).dim(2); ys = ys + (P(1).dim(2)-ys(end)) / 2;
zs         = 1:ss(3):P(1).dim(3); zs = zs + (P(1).dim(3)-zs(end)) / 2;
M          = spm_matrix([-xs(1)/ss(1)+1 -ys(1)/ss(2)+1 -zs(1)/ss(3)+1])*inv(diag([ss 1]))*eye(4);
nx         = length(xs);
ny         = length(ys);
nz         = length(zs);
[x,y,z]    = ndgrid(xs,ys,zs);
Bx         = spm_dctmtx(P(1).dim(1),ds.order(1),x(:,1,1));
By         = spm_dctmtx(P(1).dim(2),ds.order(2),y(1,:,1));
Bz         = spm_dctmtx(P(1).dim(3),ds.order(3),z(1,1,:));
dBy        = spm_dctmtx(P(1).dim(2),ds.order(2),y(1,:,1),'diff');
xyz        = [x(:) y(:) z(:) ones(length(x(:)),1)]; clear x y z;
def        = zeros(size(xyz,1),nof);
ddefa      = zeros(size(xyz,1),nof);

%
% Create file struct for use with spm_orthviews to draw
% representations of the field.
%
dispP       = P(1);
dispP       = rmfield(dispP,{'fname','descrip','n','private'});
dispP.dim   = [nx ny nz];
dispP.dt    = [64 spm_platform('bigend')];
dispP.pinfo = [1 0]';
p          = spm_imatrix(dispP.mat); p = p.*[zeros(1,6) ss 0 0 0];
p(1)       = -mean(1:nx)*p(7);
p(2)       = -mean(1:ny)*p(8);
p(3)       = -mean(1:nz)*p(9);
dispP.mat  = spm_matrix(p); clear p;

%
% We will need to resample the static field (if one was supplied)
% on the same grid (given by xs, ys and zs) as we are going to use
% for the time series. We will assume that the fieldmap has been
% realigned to the space of the first EPI image in the time-series.
%
if isfield(ds,'sfP') && ~isempty(ds.sfP)
   T = ds.sfP.mat\ds.M;
   txyz = xyz*T(1:3,:)';
   c = spm_bsplinc(ds.sfP,ds.hold);
   ds.sfield = spm_bsplins(c,txyz(:,1),txyz(:,2),txyz(:,3),ds.hold);
   ds.sfield = ds.sfield(:);
   clear c txyz;
else
   ds.sfield = [];
end

msk = get_mask(P,xyz,ds,[nx ny nz]);
ssq = [];
%
% Here starts iterative search for deformation fields.
%
for iter=1:ds.noi
   spm_uw_show('NewIter',iter);

   [ref,dx,dy,dz,P,ds,sf,dispP] = make_ref(ds,def,ddefa,P,xyz,M,msk,beta,dispP);
   AtA       = build_AtA(ref,dx,dy,dz,Bx,By,Bz,dBy,ds,P);
   [Aty,yty] = build_Aty(ref,dx,dy,dz,Bx,By,Bz,dBy,ds,P,xyz,beta,sf,def,ddefa,msk);

   % Clean up a bit, cause inverting AtA may use a lot of memory
   clear ref dx dy dz

   % Check that residual error still decreases.
   if iter > 1 && yty > ssq(iter-1)
      %
      % This means previous iteration was no good,
      % and we should go back to old_beta.
      %
      beta = old_beta;
      break;
   else
      ssq(iter) = yty;

      spm_uw_show('StartInv',1);

      % Solve for beta
      Aty = Aty + AtA*beta;
      AtA = AtA + ds.lambda * kron(eye(nof),diag(H)) * ssq(iter)/(nscan*sum(msk));

      try % Fastest if it works
         beta0(:,iter) = AtA\Aty;
      catch % Sometimes necessary
         beta0(:,iter) = pinv(AtA)*Aty;
      end

      old_beta = beta;
      beta     = beta0(:,iter);

      for i=1:nof
         def(:,i)   = spm_get_def(Bx, By,Bz,beta((i-1)*prod(ds.order)+1:i*prod(ds.order)));
         ddefa(:,i) = spm_get_def(Bx,dBy,Bz,beta((i-1)*prod(ds.order)+1:i*prod(ds.order)));
      end

      % If we are re-estimating the movement parameters, remove any DC
      % components from the deformation fields, so that they end up in
      % the movement-parameters instead. It shouldn't make any difference
      % to the variance reduction, but might potentially lead to a better
      % explanation of the variance components.
      % Note that since we sub-sample the images (and the DCT basis set)
      % it is NOT sufficient to reset beta(1) (and beta(prod(order)+1) etc,
      % instead we explicitly subtract the DC component. Note that a DC
      % component does NOT affect the Jacobian.
      %
      if ds.rem ~= 0
         def = def - repmat(mean(def),length(def),1);
      end
      spm_uw_show('EndInv');
      tmp = dispP.mat;
      dispP.mat = P(1).mat;
      spm_uw_show('FinIter',ssq,def,ds.fot,ds.sot,dispP,ds.q);
      dispP.mat = tmp;
   end
   clear AtA
end

ds.P = old_P;
for i=1:length(ds.P)
   ds.P(i).mat = P(i).mat;    % Save P with new movement parameters.
end
ds.beta = reshape(refit(P,dispP,ds,def),prod(ds.order),nof);
ds.SS   = ssq;
if isfield(ds,'sfield');
   ds = rmfield(ds,'sfield');
end

cleanup(P,ds)
spm_uw_show('FinTot');

% Document outcome
F = spm_figure('FindWin','Graphics');
if ~isempty(F), spm_figure('Focus', F), spm_print; end

catch
   cleanup(P,ds)
   spm_uw_show('FinTot');
   fprintf('Unwarp terminated abnormally.\n');
   rethrow(lasterror);
end

fprintf('%-40s: %30s\n','Completed',spm('time'))                        %-#

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Utility functions.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [q,ep] = make_q(P,fot,sot,exp_round)
%
% P         : Array of file-handles
% fot       : nx1 array of indicies for first order effect.
% sot       : nx2 matrix of indicies of second order effects.
% exp_round : Point in position space to perform Taylor-expansion
%             around ('First','Last' or 'Average'). 'Average' should
%             (in principle) give the best variance reduction. If a
%             field-map acquired before the time-series is supplied
%             then expansion around the 'First' MIGHT give a slightly
%             better average geometric fidelity.

if strcmpi(exp_round,'average');
   %
   % Get geometric mean of all transformation matrices.
   % This will be used as the zero-point in the space
   % of object position vectors (i.e. the point at which
   % the partial derivatives are estimated). This is presumably
   % a little more correct than taking the average of the
   % parameter estimates.
   %
   mT = zeros(4);
   for i=1:length(P)
      mT = mT+logm(inv(P(i).mat) * P(1).mat);
   end
   mT = real(expm(mT/length(P)));

elseif strcmpi(exp_round,'first');
   mT = eye(4);
elseif strcmpi(exp_round,'last');
   mT = inv(P(end).mat) * P(1).mat;
else
   warning(sprintf('Unknown expansion point %s',exp_round));
end

%
% Rescaling to degrees makes translations and rotations
% roughly equally scaled. Since the scaling influences
% strongly the effects of regularisation, it would surely
% be nice with a more principled approach. Suggestions
% anyone?
%
ep = [1 1 1 180/pi 180/pi 180/pi zeros(1,6)] .* spm_imatrix(mT);

%
% Now, get a nscan-by-nof matrix containing
% mean (expansion point) corrected values for each effect we
% model.
%
q = zeros(length(P),prod(size(fot)) + size(sot,1));
for i=1:length(P)
   p = [1 1 1 180/pi 180/pi 180/pi zeros(1,6)] .*...
      spm_imatrix(inv(P(i).mat) * P(1).mat);
   for j=1:prod(size(fot))
      q(i,j) = p(fot(j)) - ep(fot(j));
   end
   for j=1:size(sot,1)
      q(i,prod(size(fot))+j) = (p(sot(j,1)) - ep(sot(j,1))) * (p(sot(j,2)) - ep(sot(j,2)));
   end
end
return
%_______________________________________________________________________

%_______________________________________________________________________
function H = make_H(P,order,regorder)
% Utility Function to create Regularisation term of AtA.

mm = sqrt(sum(P(1).mat(1:3,1:3).^2)).*P(1).dim(1:3);
kx=(pi*((1:order(1))'-1)/mm(1)).^2;
ky=(pi*((1:order(2))'-1)/mm(2)).^2;
kz=(pi*((1:order(3))'-1)/mm(3)).^2;

if regorder == 0
   %
   % Cost function based on sum of squares
   %
   H = (1*kron(kz.^0,kron(ky.^0,kx.^0)) +...
        1*kron(kz.^0,kron(ky.^0,kx.^0)) +...
        1*kron(kz.^0,kron(ky.^0,kx.^0)) );
elseif regorder == 1
   %
   % Cost function based on sum of squared 1st derivatives
   %
   H  = (1*kron(kz.^1,kron(ky.^0,kx.^0)) +...
         1*kron(kz.^0,kron(ky.^1,kx.^0)) +...
         1*kron(kz.^0,kron(ky.^0,kx.^1)) );
elseif regorder == 2
   %
   % Cost function based on sum of squared 2nd derivatives
   %
   H = (1*kron(kz.^2,kron(ky.^0,kx.^0)) +...
        1*kron(kz.^0,kron(ky.^2,kx.^0)) +...
        1*kron(kz.^0,kron(ky.^0,kx.^2)) +...
        3*kron(kz.^1,kron(ky.^1,kx.^0)) +...
        3*kron(kz.^1,kron(ky.^0,kx.^1)) +...
        3*kron(kz.^0,kron(ky.^1,kx.^1)) );
elseif regorder == 3
   %
   % Cost function based on sum of squared 3rd derivatives
   %
   H = (1*kron(kz.^3,kron(ky.^0,kx.^0)) +...
        1*kron(kz.^0,kron(ky.^3,kx.^0)) +...
        1*kron(kz.^0,kron(ky.^0,kx.^3)) +...
        3*kron(kz.^2,kron(ky.^1,kx.^0)) +...
        3*kron(kz.^2,kron(ky.^0,kx.^1)) +...
        3*kron(kz.^1,kron(ky.^2,kx.^0)) +...
        3*kron(kz.^0,kron(ky.^2,kx.^1)) +...
        3*kron(kz.^1,kron(ky.^0,kx.^2)) +...
        3*kron(kz.^0,kron(ky.^1,kx.^2)) +...
        6*kron(kz.^1,kron(ky.^1,kx.^1)) );
else
   error('Invalid order of regularisation');
end
return
%_______________________________________________________________________

%_______________________________________________________________________
function AtA = build_AtA(ref,dx,dy,dz,Bx,By,Bz,dBy,ds,P)
% Now lets build up the design matrix A, or rather A'*A  since
% A itself would be forbiddingly large.

if ds.jm, spm_uw_show('StartAtA',10);
else spm_uw_show('StartAtA',6); end

nof   = prod(size(ds.fot)) + size(ds.sot,1);

AxtAx = uwAtA1(dx.*dx,Bx,By,Bz); spm_uw_show('NewAtA',1);
AytAy = uwAtA1(dy.*dy,Bx,By,Bz); spm_uw_show('NewAtA',2);
AztAz = uwAtA1(dz.*dz,Bx,By,Bz); spm_uw_show('NewAtA',3);
AxtAy = uwAtA1(dx.*dy,Bx,By,Bz); spm_uw_show('NewAtA',4);
AxtAz = uwAtA1(dx.*dz,Bx,By,Bz); spm_uw_show('NewAtA',5);
AytAz = uwAtA1(dy.*dz,Bx,By,Bz); spm_uw_show('NewAtA',6);

if ds.jm
   AjtAj = uwAtA1(ref.*ref,Bx,dBy,Bz); spm_uw_show('NewAtA',7);
   AxtAj = uwAtA2( dx.*ref,Bx, By,Bz,Bx,dBy,Bz); spm_uw_show('NewAtA',8);
   AytAj = uwAtA2( dy.*ref,Bx, By,Bz,Bx,dBy,Bz); spm_uw_show('NewAtA',9);
   AztAj = uwAtA2( dz.*ref,Bx, By,Bz,Bx,dBy,Bz); spm_uw_show('NewAtA',10);
end

R = zeros(length(P),3);
for i=1:length(P)
   tmp    = inv(P(i).mat\P(1).mat);
   R(i,:) = tmp(1:3,2)';
end

tmp  = ones(1,nof);
tmp1 = ds.q.*kron(tmp,R(:,1));
tmp2 = ds.q.*kron(tmp,R(:,2));
tmp3 = ds.q.*kron(tmp,R(:,3));
AtA  =    kron(tmp1'*tmp1,AxtAx) + kron(tmp2'*tmp2,AytAy) + kron(tmp3'*tmp3,AztAz) +...
       2*(kron(tmp1'*tmp2,AxtAy) + kron(tmp1'*tmp3,AxtAz) + kron(tmp2'*tmp3,AytAz));

if ds.jm
   tmp  = [ds.q.*kron(tmp,ones(length(P),1))];
   AtA  = AtA + kron(tmp'*tmp,AjtAj) +...
             2*(kron(tmp1'*tmp,AxtAj) + kron(tmp2'*tmp,AytAj) + kron(tmp3'*tmp,AztAj));
end
spm_uw_show('EndAtA');
return;
%_______________________________________________________________________

%_______________________________________________________________________
function AtA = uwAtA1(y,Bx,By,Bz)
% Calculating off-diagonal block of AtA.

[nx,mx] = size(Bx);
[ny,my] = size(By);
[nz,mz] = size(Bz);
AtA     = zeros(mx*my*mz);
for sl =1:nz
   tmp  = reshape(y((sl-1)*nx*ny+1:sl*nx*ny),nx,ny);
   spm_krutil(Bz(sl,:)'*Bz(sl,:),spm_krutil(tmp,Bx,By,1),AtA);
%    AtA  = AtA + kron(Bz(sl,:)'*Bz(sl,:),spm_krutil(tmp,Bx,By,1));
end
return
%_______________________________________________________________________

%_______________________________________________________________________
function AtA = uwAtA2(y,Bx1,By1,Bz1,Bx2,By2,Bz2)
% Calculating cross-term of diagonal block of AtA
% when A is a sum of the type A1+A2 and where both
% A1 and A2 are possible to express as a kronecker
% product of lesser matrices.

[nx,mx1] = size(Bx1); [nx,mx2] = size(Bx2);
[ny,my1] = size(By1); [ny,my2] = size(By2);
[nz,mz1] = size(Bz1); [nz,mz2] = size(Bz2);
AtA      = zeros(mx1*my1*mz1,mx2*my2*mz2);
for sl =1:nz
   tmp  = reshape(y((sl-1)*nx*ny+1:sl*nx*ny),nx,ny);
   spm_krutil(Bz1(sl,:)'*Bz2(sl,:),spm_krutil(tmp,Bx1,By1,Bx2,By2),AtA);
%   AtA  = AtA + kron(Bz1(sl,:)'*Bz2(sl,:),spm_krutil(tmp,Bx1,By1,Bx2,By2));
end
return
%_______________________________________________________________________

%_______________________________________________________________________
function [Aty,yty] = build_Aty(ref,dx,dy,dz,Bx,By,Bz,dBy,ds,P,xyz,beta,sf,def,ddefa,msk)
% Building Aty.

nof = prod(size(ds.fot)) + size(ds.sot,1);
Aty = zeros(nof*prod(ds.order),1);
yty = 0;

spm_uw_show('StartAty',length(P));
for scan = 1:length(P)
   spm_uw_show('NewAty',scan);
   T    = P(scan).mat\ds.M;
   txyz = xyz*T(1:3,:)';
   if ~(all(beta == 0) && isempty(ds.sfield))
      [idef,jac] = spm_get_image_def(scan,ds,def,ddefa);
      txyz(:,2)  = txyz(:,2)+idef;
   end;
   c    = spm_bsplinc(P(scan),ds.hold);
   y    = sf(scan) * spm_bsplins(c,txyz(:,1),txyz(:,2),txyz(:,3),ds.hold);
   if ds.jm && ~(all(beta == 0) && isempty(ds.sfield))
      y = y .* jac;
   end;

   y_diff       = (ref - y).*msk;
   indx         = find(isnan(y));
   y_diff(indx) = 0;
   iTcol        = inv(T(1:3,1:3));
   tmpAty       = spm_get_def(Bx',By',Bz',([dx dy dz]*iTcol(:,2)).*y_diff);
   if ds.jm ~= 0
      tmpAty = tmpAty + spm_get_def(Bx',dBy',Bz',ref.*y_diff);
   end
   for i=1:nof
      rindx      = (i-1)*prod(ds.order)+1:i*prod(ds.order);
      Aty(rindx) = Aty(rindx) + ds.q(scan,i)*tmpAty;
   end
   yty = yty + y_diff'*y_diff;
end
spm_uw_show('EndAty');
return;
%_______________________________________________________________________

%_______________________________________________________________________
function [ref,dx,dy,dz,P,ds,sf,dispP] = make_ref(ds,def,ddefa,P,xyz,M,msk,beta,dispP)
% First of all get the mean of all scans given their
% present deformation fields. When using this as the
% "reference" we will explicitly be minimising variance.
%
% First scan in P is still reference in terms of
% y-direction (in the scanner framework). A single set of
% image gradients is estimated from the mean of all scans,
% transformed into the space of P(1), and used for all scans.
%
spm_uw_show('StartRef',length(P));

rem = ds.rem;
if all(beta==0), rem=0; end;

if rem
   [m_ref,D] = get_refD(ds,def,ddefa,P,xyz,msk);
end

ref = zeros(size(xyz,1),1);
for i=1:length(P)
   spm_uw_show('NewRef',i);
   T    = P(i).mat\ds.M;
   txyz = xyz*T(1:3,:)';
   if ~(all(beta == 0) && isempty(ds.sfield))
      [idef,jac] = spm_get_image_def(i,ds,def,ddefa);
      txyz(:,2)  = txyz(:,2)+idef;
   end;
   c     = spm_bsplinc(P(i),ds.hold);
   f     = spm_bsplins(c,txyz(:,1),txyz(:,2),txyz(:,3),ds.hold);
   if ds.jm ~= 0 && exist('jac')==1
      f = f .* jac;
   end
   indx  = find(~isnan(f));
   sf(i) = 1 / (sum(f(indx).*msk(indx)) / sum(msk(indx)));
   ref   = ref + f;

   if rem
      indx   = find(isnan(f)); f(indx) = 0;
      Dty{i} = (((m_ref - sf(i)*f).*msk)'*D)';
   end
end
ref  = ref / length(P);
ref  = reshape(ref,dispP.dim(1:3));
indx = find(~isnan(ref));

% Scale to roughly 100 mean intensity to ensure
% consistent weighting of regularisation.
gl             = spm_global(ref);
sf             = (100 * (sum(ref(indx).*msk(indx)) / sum(msk(indx))) / gl) * sf;
ref            = (100 / gl) * ref;
dispP.dat      = ref;
c              = spm_bsplinc(ref,ds.hold);
txyz           = xyz*M(1:3,:)';
[ref,dx,dy,dz] = spm_bsplins(c,txyz(:,1),txyz(:,2),txyz(:,3),ds.hold);
ref            = ref.*msk;
dx             =  dx.*msk;
dy             =  dy.*msk;
dz             =  dz.*msk;

% Re-estimate (or rather nudge) movement parameters.
if rem ~= 0
   iDtD = inv(D'*D);
   for i=2:length(P)
      P(i).mat = inv(spm_matrix((iDtD*Dty{i})'))*P(i).mat;
   end
   [ds.q,ds.p0] = make_q(P,ds.fot,ds.sot,ds.exp_round);
end
spm_uw_show('EndRef');
return
%_______________________________________________________________________

%_______________________________________________________________________
function [m_ref,D] = get_refD(ds,def,ddefa,P,xyz,msk)
% Get partials w.r.t. movements from first scan.

[idef,jac] = spm_get_image_def(1,ds,def,ddefa);
T    = P(1).mat\ds.M;
txyz = xyz*T';
c    = spm_bsplinc(P(1),ds.hold);
[m_ref,dx,dy,dz] = spm_bsplins(c,txyz(:,1),...
                   txyz(:,2)+idef,txyz(:,3),ds.hold);
indx    = ~isnan(m_ref);
mref_sf = 1 / (sum(m_ref(indx).*msk(indx)) / sum(msk(indx)));
m_ref(indx) = m_ref(indx) .* mref_sf; m_ref(~indx) = 0;
dx(indx)    =    dx(indx) .* mref_sf; dx(~indx)    = 0;
dy(indx)    =    dy(indx) .* mref_sf; dy(~indx)    = 0;
dz(indx)    =    dz(indx) .* mref_sf; dz(~indx)    = 0;
if ds.jm ~= 0
   m_ref = m_ref .* jac;
   dx    = dx    .* jac;
   dy    = dy    .* jac;
   dz    = dz    .* jac;
end
D = make_D(dx.*msk,dy.*msk,dz.*msk,txyz,P(1).mat);
return;
%_______________________________________________________________________

%_______________________________________________________________________
function D = make_D(dx,dy,dz,xyz,M)
% Utility function that creates a matrix of partial
% derivatives in units of /mm and /radian.
%
% dx,dy,dz   - Partial derivatives (/pixel) wrt x-, y- and z-translation.
% xyz        - xyz-matrix (original positions).
% M          - Current voxel->world matrix.

D = zeros(length(xyz),6);
tiny = 0.0001;
for i=1:6
   p      = [0 0 0 0 0 0 1 1 1 0 0 0];
   p(i)   = p(i)+tiny;
   T      = M\spm_matrix(p)*M;
   dxyz   = xyz*T';
   dxyz   = dxyz(:,1:3)-xyz(:,1:3);
   D(:,i) = sum(dxyz.*[dx dy dz],2)/tiny;
end
return
%_______________________________________________________________________

%_______________________________________________________________________
function cleanup(P,ds)
% Delete temporary smooth files.
%
if ~isempty(ds.fwhm) && ds.fwhm > 0
   for i=1:length(P)
      spm_unlink(P(i).fname);
      [fpath,fname,ext] = fileparts(P(i).fname);
      spm_unlink(fullfile(fpath,[fname '.hdr']));
      spm_unlink(fullfile(fpath,[fname '.mat']));
   end
end
return;
%_______________________________________________________________________

%_______________________________________________________________________
function beta = refit(P,dispP,ds,def)
% We have now estimated the fields on a grid that
% does not coincide with the voxel centers. We must
% now calculate the betas that correspond to a
% a grid centered on the voxel centers.
%
spm_uw_show('StartRefit',1);
rsP     = P(1);
p       = spm_imatrix(rsP.mat);
p       = [zeros(1,6) p(7:9) 0 0 0];
p(1)    = -mean(1:rsP.dim(1))*p(7);
p(2)    = -mean(1:rsP.dim(2))*p(8);
p(3)    = -mean(1:rsP.dim(3))*p(9);
rsP.mat = spm_matrix(p); clear p;
[x,y,z] = ndgrid(1:rsP.dim(1),1:rsP.dim(2),1:rsP.dim(3));
xyz     = [x(:) y(:) z(:) ones(numel(x),1)];
txyz    = ((inv(dispP.mat)*rsP.mat)*xyz')';
Bx      = spm_dctmtx(rsP.dim(1),ds.order(1));
By      = spm_dctmtx(rsP.dim(2),ds.order(2));
Bz      = spm_dctmtx(rsP.dim(3),ds.order(3));
nx      = rsP.dim(1); mx = ds.order(1);
ny      = rsP.dim(2); my = ds.order(2);
nz      = rsP.dim(3); mz = ds.order(3);
nof     = numel(ds.fot) + size(ds.sot,1);
for i=1:nof
   dispP.dat   = reshape(def(:,i),dispP.dim(1:3));
   c           = spm_bsplinc(dispP,ds.hold);
   field       = spm_bsplins(c,txyz(:,1),txyz(:,2),txyz(:,3),ds.hold);
   indx        = isnan(field);
   wgt         = ones(size(field));
   wgt(indx)   = 0;
   field(indx) = 0;
   AtA         = zeros(mx*my*mz);
   Aty         = zeros(mx*my*mz,1);
   for sl = 1:nz
      indx  = (sl-1)*nx*ny+1:sl*nx*ny;
      tmp1  = reshape(field(indx).*wgt(indx),nx,ny);
      tmp2  = reshape(wgt(indx),nx,ny);
      spm_krutil(Bz(sl,:)'*Bz(sl,:),spm_krutil(tmp2,Bx,By,1),AtA);
%      AtA   = AtA + kron(Bz(sl,:)'*Bz(sl,:),spm_krutil(tmp2,Bx,By,1));
      Aty   = Aty + kron(Bz(sl,:)',spm_krutil(tmp1,Bx,By,0));
   end
   beta((i-1)*prod(ds.order)+1:i*prod(ds.order)) = AtA\Aty;
end
spm_uw_show('EndRefit');
return;
%_______________________________________________________________________

%_______________________________________________________________________
function msk = get_mask(P,xyz,ds,dm)
%
% Create a mask to avoid regions where data doesnt exist
% for all scans. This mask is slightly less Q&D than that
% of version 1 of Unwarp. It checks where data exist for
% all scans with present movement parameters and given
% (optionally) the static field. It creates a mask with
% non-zero values only for those voxels, and then does a
% 3D erode to preempt effects of re-estimated movement
% parameters and movement-by-susceptibility effects.
%
spm_uw_show('MaskStart',length(P));
msk = true(length(xyz),1);
for i=1:length(P)
   txyz = xyz * (P(i).mat\ds.M)';
   tmsk = (txyz(:,1)>=1 & txyz(:,1)<=P(1).dim(1) &...
           txyz(:,2)>=1 & txyz(:,2)<=P(1).dim(2) &...
           txyz(:,3)>=1 & txyz(:,3)<=P(1).dim(3));
   msk  = msk & tmsk;
   spm_uw_show('MaskUpdate',i);
end
%
% Include static field in mask estimation
% if one has been supplied.
%
if isfield(ds,'sfP') && ~isempty(ds.sfP)
   txyz = xyz * (ds.sfP.mat\ds.M)';
   tmsk = (txyz(:,1)>=1 & txyz(:,1)<=ds.sfP.dim(1) &...
           txyz(:,2)>=1 & txyz(:,2)<=ds.sfP.dim(2) &...
           txyz(:,3)>=1 & txyz(:,3)<=ds.sfP.dim(3));
   msk  = msk & tmsk;
end
msk = erode_msk(msk,dm);
spm_uw_show('MaskEnd');

% maskP                 = P(1);
% [mypath,myname,myext] = fileparts(maskP.fname);
% maskP.fname           = fullfile(mypath,['mask' myext]);
% maskP.dim             = [nx ny nz];
% maskP.dt              = P(1).dt;
% maskP                 = spm_write_vol(maskP,reshape(msk,[nx ny nz]));
return;
%_______________________________________________________________________

%_______________________________________________________________________
function omsk = erode_msk(msk,dim)
omsk = zeros(dim+[4 4 4]);
omsk(3:end-2,3:end-2,3:end-2) = reshape(msk,dim);
omsk = spm_erode(omsk(:),dim+[4 4 4]);
omsk = reshape(omsk,dim+[4 4 4]);
omsk = omsk(3:end-2,3:end-2,3:end-2);
omsk = omsk(:);
return
%_______________________________________________________________________


function gpcf = gpcf_additive(varargin)
%GPCF_ADDITIVE  Create a mixture over products of kernels for each dimension
%
%  Description
%    GPCF = GPCF_ADDITIVE('PARAM1',VALUE1, 'PARAM2,VALUE2, ...)
%    creates a mixture over all possible product combinations of given
%    covariance functions for each input dimension separately.
%
%    Parameters [default]:
%      cf           - cell array {CF_1, CF_2, ... , CF_N} of covariance
%                     functions for each dimension [no default]
%      max_deg      - maximum order of interaction (must be less or equal
%                     to the number of covariance functions N) [2]
%      sigma2       - 1D array of length max_deg defining the variance for
%                     each order of interaction [0.1*ones]
%      sigma2_prior - prior for sigma2 [prior_logunif]
%
%    For example N = 3, max_deg = 2:
%      k1 = CF_1 + CF_2 + CF_3
%      k2 = CF_1*CF_2 + CF_1*CF_3 + CF_2*CF_3
%      k3 = CF_1*CF_2*CF_3
%      GPCF = sigam2(1)*k1 + sigam2(2)*k2,
%
%  See also
%    GP_SET, GPCF_*
%
%  References:
%    Duvenaud, D. K., Nickisch, H., & Rasmussen, C. E. (2011). Additive
%    Gaussian processes. In Advances in neural information processing
%    systems (pp. 226-234).

% Copyright (c) 2009-2010 Jarno Vanhatalo
% Copyright (c) 2010 Aki Vehtari
% Copyright (c) 2014 Tuomas Sivula

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'GPCF_ADDITIVE';
  ip.addOptional('gpcf', [], @isstruct);
  ip.addParamValue('cf',[], @iscell);
  ip.addParamValue('max_deg', 2, @(x) isscalar(x) && isnumeric(x) ...
                                      && x>0 && mod(x,1) == 0);
  ip.addParamValue('sigma2', [], @(x) isnumeric(x) && isvector(x) && all(x>0));
  ip.addParamValue('sigma2_prior', prior_logunif(), ...
                   @(x) isstruct(x) || isempty(x))
  ip.parse(varargin{:});
  gpcf=ip.Results.gpcf;

  if isempty(gpcf)
    init=true;
    gpcf.type = 'gpcf_additive';
  else
    if ~isfield(gpcf,'type') && ~isequal(gpcf.type,'gpcf_additive')
      error('First argument does not seem to be a valid covariance function structure')
    end
    init=false;
  end

  % Initialize parameters

  % Kernels
  if init || ~ismember('cf',ip.UsingDefaults)
    if length(ip.Results.cf) < 2
      error('At least two covariance functions has to be given in cf');
    end
    gpcf.cf = ip.Results.cf;
  end
  ncf = length(gpcf.cf);

  % Max degree
  if init || ~ismember('max_deg',ip.UsingDefaults)
    if ip.Results.max_deg <= ncf
      gpcf.max_deg = ip.Results.max_deg;
    else
      warning('max_deg in additive kernel can not be greater than number of dimensions, max_deg truncated.');
      gpcf.max_deg = ncf;
    end
  end

  % Degree variances
  if init || ~ismember('sigma2',ip.UsingDefaults)
    if isempty(ip.Results.sigma2)
      gpcf.sigma2 = 0.1*ones(1,gpcf.max_deg);
    elseif length(ip.Results.sigma2) == gpcf.max_deg
      gpcf.sigma2 = ip.Results.sigma2;
    else
      error('Wrong number of elements in degree variance parameter vector sigma2')
    end
    % Ensure the right direction
    if size(gpcf.sigma2,1) ~= 1
      gpcf.sigma2 = gpcf.sigma2';
    end
  end

  % Degree variance priors
  if init || ~ismember('sigma2_prior',ip.UsingDefaults)
    gpcf.p.sigma2 = ip.Results.sigma2_prior;
  end

  % Ensure sigma2 matches max_deg
  if length(gpcf.sigma2) ~= gpcf.max_deg
    error('Parameters sigma2 and max_deg sizes does not match')
  end

  if init
    % Set the function handles to the subfunctions
    gpcf.fh.pak = @gpcf_additive_pak;
    gpcf.fh.unpak = @gpcf_additive_unpak;
    gpcf.fh.lp = @gpcf_additive_lp;
    gpcf.fh.lpg = @gpcf_additive_lpg;
    gpcf.fh.cfg = @gpcf_additive_cfg;
    gpcf.fh.ginput = @gpcf_additive_ginput;
    gpcf.fh.cov = @gpcf_additive_cov;
    gpcf.fh.trcov  = @gpcf_additive_trcov;
    gpcf.fh.trvar  = @gpcf_additive_trvar;
    gpcf.fh.recappend = @gpcf_additive_recappend;
  end

end

function [w, s, h] = gpcf_additive_pak(gpcf)
%GPCF_ADDITIVE_PAK  Combine GP covariance function parameters into one vector
%
%  Description
%    W = GPCF_ADDITIVE_PAK(GPCF, W) loops through all the covariance
%    functions and packs their parameters into one vector as
%    described in the respective functions. This is a mandatory
%    subfunction used for example in energy and gradient computations.
%
%  See also
%    GPCF_ADDITIVE_UNPAK

  ncf = length(gpcf.cf);
  w = []; s = {}; h=[];

  if ~isempty(gpcf.p.sigma2)
    w = [w log(gpcf.sigma2)];
    s = [s; sprintf('log(additive.sigma2 x %d)',numel(gpcf.sigma2))];
    h = [h ones(1,numel(gpcf.sigma2))];
    % Hyperparameters of lengthScale
    [wh, sh, hh] = gpcf.p.sigma2.fh.pak(gpcf.p.sigma2);
    sh=strcat(repmat('prior-', size(sh,1),1),sh);
    w = [w wh];
    s = [s; sh];
    h = [h 1+hh];
  end

  for i=1:ncf
    cf = gpcf.cf{i};
    [wi, si, hi] = cf.fh.pak(cf);
    w = [w wi];
    s = [s; si];
    h = [h hi];
  end
end

function [gpcf, w] = gpcf_additive_unpak(gpcf, w)
%GPCF_ADDITIVE_UNPAK  Sets the covariance function parameters into
%                 the structures
%
%  Description
%    [GPCF, W] = GPCF_ADDITIVE_UNPAK(GPCF, W) loops through all the
%    covariance functions and unpacks their parameters from W to
%    each covariance function structure. This is a mandatory
%    subfunction used for example in energy and gradient computations.
%
%  See also
%    GPCF_ADDITIVE_PAK
%
  ncf = length(gpcf.cf);

  if isfield(gpcf.p,'sigma2') && ~isempty(gpcf.p.sigma2)
    i1=1;
    i2=length(gpcf.sigma2);
    gpcf.sigma2 = exp(w(i1:i2));
    w = w(i2+1:end);
    % Hyperparameters of lengthScale
    [p, w] = gpcf.p.sigma2.fh.unpak(gpcf.p.sigma2, w);
    gpcf.p.sigma2 = p;
  end

  for i=1:ncf
    cf = gpcf.cf{i};
    [cf, w] = cf.fh.unpak(cf, w);
    gpcf.cf{i} = cf;
  end

end

function lp = gpcf_additive_lp(gpcf)
%GPCF_ADDITIVE_LP  Evaluate the log prior of covariance function parameters
%
%  Description
%    LP = GPCF_ADDITIVE_LP(GPCF, X, T) takes a covariance function
%    structure GPCF and returns log(p(th)), where th collects the
%    parameters. This is a mandatory subfunction used for example
%    in energy computations.
%
%  See also
%    GPCF_ADDITIVE_PAK, GPCF_ADDITIVE_UNPAK, GPCF_ADDITIVE_LPG, GP_E

  lp = 0;
  gpp = gpcf.p;

  if isfield(gpp,'sigma2') && ~isempty(gpp.sigma2)
    lp = lp +gpp.sigma2.fh.lp(gpcf.sigma2, gpp.sigma2) +sum(log(gpcf.sigma2));
  end

  ncf = length(gpcf.cf);
  for i=1:ncf
    cf = gpcf.cf{i};
    lp = lp + cf.fh.lp(cf);
  end

end

function lpg = gpcf_additive_lpg(gpcf)
%GPCF_ADDITIVE_LPG  Evaluate gradient of the log prior with respect
%               to the parameters.
%
%  Description
%    LPG = GPCF_ADDITIVE_LPG(GPCF) takes a covariance function
%    structure GPCF and returns LPG = d log (p(th))/dth, where th
%    is the vector of parameters. This is a mandatory subfunction
%    used for example in gradient computations.
%
%  See also
%    GPCF_ADDITIVE_PAK, GPCF_ADDITIVE_UNPAK, GPCF_ADDITIVE_LP, GP_G

  % Evaluate the gradients
  lpg = [];
  gpp=gpcf.p;

  if isfield(gpp,'sigma2') && ~isempty(gpp.sigma2)
    lll = length(gpcf.sigma2);
    lpgs = gpp.sigma2.fh.lpg(gpcf.sigma2, gpp.sigma2);
    lpg = [lpg lpgs(1:lll).*gpcf.sigma2+1 lpgs(lll+1:end)];
  end

  ncf = length(gpcf.cf);
  for i=1:ncf
    cf = gpcf.cf{i};
    lpg=[lpg cf.fh.lpg(cf)];
  end

end

function es = degrees(r, zs, inds)
% DEGREEs - Internal function used to calculate the covariance degree
% components e_k(z_inds(1), ..., z_inds(n))
%
%   Parameters:
%     r    - max degree
%     zs   - all the covariances of the underlying kernels
%     inds - indexes of the used kernels (empty means all)
%
%   N.B. Here all Cs and inds are given instead using slicing because the
%   latter is memory inefficient. Compare the memory usage in the
%   following:
%
%     inds = 1:100;
%     zs = rand(1000,1000,100);
%
%     % Memory inefficient
%     t = sum(zs(:,:,inds(inds~=13)),3);
%
%     % Memory efficient
%     t = zeros(1000,1000);
%     for i = inds(inds~=13)
%       t = t + zs(:,:,i);
%     end
%

  if isempty(inds)

    ncf = size(zs,3);

    if r == 1
      % Only one degree (implemented here for completeness)
      es = sum(zs,3);

    elseif r == 2
      % Only two degrees (implemented here for completeness)
      es = zeros(size(zs,1),size(zs,2),r);
      es(:,:,1) = sum(zs,3);
      for i1 = 1:ncf-1
        for i2 = i1+1:ncf
          es(:,:,2) = es(:,:,2) + zs(:,:,i1).*zs(:,:,i2);
        end
      end

    elseif r > 2
      % Over two degrees ... use Newton-Girard formulae
      sk = zeros(size(zs,1),size(zs,2),r);
      sk(:,:,1) = sum(zs,3);
      for i = 2:r
        sk(:,:,i) = sum(zs.^i, 3);
      end
      es = zeros(size(zs,1),size(zs,2),r);
      es(:,:,1) = sk(:,:,1);
      for i = 2:r
        presign = true;
        for k = 1:i-1
          if presign
            es(:,:,i) = es(:,:,i) + es(:,:,i-k).*sk(:,:,k);
            presign = false;
          else
            es(:,:,i) = es(:,:,i) - es(:,:,i-k).*sk(:,:,k);
            presign = true;
          end
        end
        if presign
          es(:,:,i) = es(:,:,i) + sk(:,:,i);
        else
          es(:,:,i) = es(:,:,i) - sk(:,:,i);
        end
        es(:,:,i) = es(:,:,i)./i;
      end

    else
      error('Invalid max_deg parameter')
    end

  else
    % Use only selected kernels

    % Check direction
    if r > length(inds)
      error('Invalid max degree relative to inds')
    end
    if size(inds,1) ~= 1
      inds = inds';
      if size(inds,1) ~= 1
        error('Invalid parameter inds')
      end
    end

    if r == 1
      % Only one degree
      es = 0;
      for i = inds
        es = es + zs(:,:,i);
      end

    elseif r == 2
      % Only two degrees
      es = zeros(size(zs,1),size(zs,2),r);
      for i = inds
        es(:,:,1) = es(:,:,1) + zs(:,:,i);
      end
      for i1 = 1:length(inds)-1
        for i2 = i1+1:length(inds)
          es(:,:,2) = es(:,:,2) + zs(:,:,inds(i1)).*zs(:,:,inds(i2));
        end
      end

    elseif r > 2
      % Over two degrees ... use Newton-Girard formulae
      sk = zeros(size(zs,1),size(zs,2),r);
      for i = inds
        sk(:,:,1) = sk(:,:,1) + zs(:,:,i);
      end
      for i = 2:r
        for j = inds
          sk(:,:,i) = sk(:,:,i) + zs(:,:,j).^i;
        end
      end
      es = zeros(size(zs,1),size(zs,2),r);
      es(:,:,1) = sk(:,:,1);
      for i = 2:r
        presign = true;
        for k = 1:i-1
          if presign
            es(:,:,i) = es(:,:,i) + es(:,:,i-k).*sk(:,:,k);
            presign = false;
          else
            es(:,:,i) = es(:,:,i) - es(:,:,i-k).*sk(:,:,k);
            presign = true;
          end
        end
        if presign
          es(:,:,i) = es(:,:,i) + sk(:,:,i);
        else
          es(:,:,i) = es(:,:,i) - sk(:,:,i);
        end
        es(:,:,i) = es(:,:,i)./i;
      end

    else
      error('Invalid max_deg parameter')
    end

  end

end

function DKff = gpcf_additive_cfg(gpcf, x, x2, mask, i1)
%GPCF_ADDITIVE_CFG  Evaluate gradient of covariance function
%               with respect to the parameters.
%
%  Description
%    DKff = GPCF_ADDITIVE_CFG(GPCF, X) takes a covariance function
%    structure GPCF, a matrix X of input vectors and returns
%    DKff, the gradients of covariance matrix Kff = k(X,X) with
%    respect to th (cell array with matrix elements). This is a
%    mandatory subfunction used in gradient computations.
%
%    DKff = GPCF_ADDITIVE_CFG(GPCF, X, X2) takes a covariance
%    function structure GPCF, a matrix X of input vectors and
%    returns DKff, the gradients of covariance matrix Kff =
%    k(X,X2) with respect to th (cell array with matrix
%    elements). This subfunction is needed when using sparse
%    approximations (e.g. FIC).
%
%    DKff = GPCF_ADDITIVE_CFG(GPCF, X, [], MASK) takes a covariance
%    function structure GPCF, a matrix X of input vectors and
%    returns DKff, the diagonal of gradients of covariance matrix
%    Kff = k(X,X2) with respect to th (cell array with matrix
%    elements). This subfunction is needed when using sparse
%    approximations (e.g. FIC).
%
%    DKff = GPCF_ADDITIVE_CFG(GPCF, X, X2, [], i) takes a covariance
%    function structure GPCF, a matrix X of input vectors and
%    returns DKff, the gradients of covariance matrix Kff =
%    k(X,X2), or k(X,X) if X2 is empty, with respect to ith
%    hyperparameter. This subfunction is needed when using
%    memory save option in gp_set.
%
%  See also
%    GPCF_ADDITIVE_PAK, GPCF_ADDITIVE_UNPAK, GPCF_ADDITIVE_LP, GP_G

  [n, m] =size(x);
  ncf = length(gpcf.cf);
  r = gpcf.max_deg;

  DKff = {};

  if nargin==5
    % Use memory save option
    savememory=1;
    i3 = zeros(1,ncf);
    for k=1:ncf
      % Number of hyperparameters for each covariance function
      cf = gpcf.cf{k};
      i3(k) = cf.fh.cfg(cf,[],[],[],0);
    end
    if i1==0
      % Return number of hyperparameters
      DKff = sum(i3) + gpcf.max_deg;
      return
    end
    if i1 > gpcf.max_deg
      i1 = i1 - gpcf.max_deg;
      % The parameter belongs to one of the underlying kernels
      % Now i1 is [kernel_index, param_index_in_that_kernel]
      i3 = cumsum(i3);
      ind = find(i3 >= i1, 1);
      if ind > 1
        i1 = [ind, i1-i3(ind-1)];
      else
        i1 = [ind, i1];
      end
    end
  else
    savememory=0;
  end

  % Evaluate: DKff{1} = d Kff / d magnSigma2
  %           DKff{2} = d Kff / d lengthScale
  % NOTE! Here we have already taken into account that the parameters are transformed
  % through log() and thus dK/dlog(p) = p * dK/dp

  % evaluate the gradient for training covariance
  if nargin == 2 || (isempty(x2) && isempty(mask))

    % evaluate the individual covariance functions
    zs = zeros(n,n,ncf);
    for i=1:ncf
      cf = gpcf.cf{i};
      zs(:,:,i) = cf.fh.trcov(cf, x(:,i));
    end

    % Evaluate the gradients
    ind = 1:ncf;

    if ~savememory

      DKff = {};

      % Order variances sigma2
      es = degrees(r, zs, []);
      for i = 1:r
        % dlog(p) ... See NOTE above
        DKff{end+1} = gpcf.sigma2(i).*es(:,:,i);
      end

      % Subkernel hyperparameters
      for i=1:ncf
        cf = gpcf.cf{i};
        DK = cf.fh.cfg(cf, x(:,i));

        CC = gpcf.sigma2(1).*ones(n,n);
        if r > 1
          es = degrees(r-1, zs, ind(ind~=i));
          for j = 2:r
            CC = CC + gpcf.sigma2(j).*es(:,:,j-1);
          end
        end
        for j = 1:length(DK)
          DKff{end+1} = DK{j}.*CC;
        end
      end

    else
      if length(i1) == 1
        % Order variance sigma2
        es = degrees(i1, zs, []);
        % dlog(p) ... See NOTE above
        DKff = gpcf.sigma2(i1).*es(:,:,end);
      else

        cf = gpcf.cf{i1(1)};
        DK = cf.fh.cfg(cf,x(:,i1(1)),[],[],i1(2));

        CC = gpcf.sigma2(1).*ones(n,n);
        if r > 1
          es = degrees(r-1, zs, ind(ind~=i1(1)));
          for j = 2:r
            CC = CC + gpcf.sigma2(j).*es(:,:,j-1);
          end
        end
        DKff = DK.*CC;

      end

    end

    % Evaluate the gradient of non-symmetric covariance (e.g. K_fu)
  elseif nargin == 3 || isempty(mask)
    if size(x,2) ~= size(x2,2)
      error('gpcf_prod -> _ghyper: The number of columns in x and x2 has to be the same. ')
    end

    % evaluate the individual covariance functions
    zs = zeros(size(x,1),size(x2,1),ncf);
    for i=1:ncf
      cf = gpcf.cf{i};
      zs(:,:,i) = cf.fh.cov(cf, x(:,i), x2(:,i));
    end

    % Evaluate the gradients
    ind = 1:ncf;

    if ~savememory

      DKff = {};

      % Order variances sigma2
      es = degrees(r, zs, []);
      for i = 1:r
        % dlog(p) ... See NOTE above
        DKff{end+1} = gpcf.sigma2(i).*es(:,:,i);
      end

      % Subkernel hyperparameters
      for i=1:ncf
        cf = gpcf.cf{i};
        DK = cf.fh.cfg(cf, x(:,i), x2(:,i));

        CC = gpcf.sigma2(1).*ones(n,n);
        if r > 1
          es = degrees(r-1, zs, ind(ind~=i));
          for j = 2:r
            CC = CC + gpcf.sigma2(j).*es(:,:,j-1);
          end
        end
        for j = 1:length(DK)
          DKff{end+1} = DK{j}.*CC;
        end
      end

    else
      if length(i1) == 1
        % Order variance sigma2
        es = degrees(i1, zs, []);
        % dlog(p) ... See NOTE above
        DKff = gpcf.sigma2(i1).*es(:,:,end);
      else

        cf = gpcf.cf{i1(1)};
        DK = cf.fh.cfg(cf, x(:,i), x2(:,i), [], i1(2));

        CC = gpcf.sigma2(1).*ones(n,n);
        if r > 1
          es = degrees(r-1, zs, ind(ind~=i1(1)));
          for j = 2:r
            CC = CC + gpcf.sigma2(j).*es(:,:,j-1);
          end
        end
        DKff = DK.*CC;

      end

    end

    % Evaluate: DKff{1}    = d mask(Kff,I) / d magnSigma2
    %           DKff{2...} = d mask(Kff,I) / d lengthScale
  elseif nargin == 4 || nargin == 5

    % evaluate the individual covariance functions
    zs = zeros(size(x,1),ncf);
    for i=1:ncf
      cf = gpcf.cf{i};
      zs(:,i) = cf.fh.trvar(cf, x(:,i));
    end

    % Evaluate the gradients
    ind = 1:ncf;

    if ~savememory

      DKff = {};

      % Order variances sigma2
      es = degrees(r, zs, []);
      for i = 1:r
        % dlog(p) ... See NOTE above
        DKff{end+1} = gpcf.sigma2(i).*es(:,:,i);
      end

      % Subkernel hyperparameters
      for i=1:ncf
        cf = gpcf.cf{i};
        DK = cf.fh.cfg(cf, x(:,i), [], 1);

        CC = gpcf.sigma2(1).*ones(n,n);
        if r > 1
          es = degrees(r-1, zs, ind(ind~=i));
          for j = 2:r
            CC = CC + gpcf.sigma2(j).*es(:,:,j-1);
          end
        end
        for j = 1:length(DK)
          DKff{end+1} = DK{j}.*CC;
        end
      end

    else
      if length(i1) == 1
        % Order variance sigma2
        es = degrees(i1, zs, []);
        % dlog(p) ... See NOTE above
        DKff = gpcf.sigma2(i1).*es(:,:,end);
      else

        cf = gpcf.cf{i1(1)};
        DK = cf.fh.cfg(cf, x(:,i1(1)), [], 1, i1(2));

        CC = gpcf.sigma2(1).*ones(n,n);
        if r > 1
          es = degrees(r-1, zs, ind(ind~=i1(1)));
          for j = 2:r
            CC = CC + gpcf.sigma2(j).*es(:,:,j-1);
          end
        end
        DKff = DK.*CC;

      end

    end

  end

end

function DKff = gpcf_additive_ginput(gpcf, x, x2, i1)
%GPCF_ADDITIVE_GINPUT  Evaluate gradient of covariance function with
%                  respect to x
%
%  Description
%    DKff = GPCF_ADDITIVE_GINPUT(GPCF, X) takes a covariance function
%    structure GPCF, a matrix X of input vectors and returns
%    DKff, the gradients of covariance matrix Kff = k(X,X) with
%    respect to X (cell array with matrix elements). This subfunction
%    is needed when computing gradients with respect to inducing
%    inputs in sparse approximations.
%
%    DKff = GPCF_ADDITIVE_GINPUT(GPCF, X, X2) takes a covariance
%    function structure GPCF, a matrix X of input vectors and
%    returns DKff, the gradients of covariance matrix Kff =
%    k(X,X2) with respect to X (cell array with matrix elements).
%    This subfunction is needed when computing gradients with
%    respect to inducing inputs in sparse approximations.
%
%    DKff = GPCF_ADDITIVE_GINPUT(GPCF, X, X2, i) takes a covariance
%    function structure GPCF, a matrix X of input vectors and
%    returns DKff, the gradients of covariance matrix Kff =
%    k(X,X2), or k(X,X) if X2 is empty, with respect to ith
%    covariate in X (cell array with matrix elements). This
%    subfunction is needed when using memory save option in
%    gp_set.
%
%  See also
%    GPCF_ADDITIVE_PAK, GPCF_ADDITIVE_UNPAK, GPCF_ADDITIVE_LP, GP_G

  [n, m] =size(x);
  r = gpcf.max_deg;

  if nargin==4
    % Use memory save option
    savememory=1;
    if i1==0
      % Return number of covariates
      if isfield(gpcf,'selectedVariables')
        DKff=length(gpcf.selectedVariables);
      else
        DKff=m;
      end
      return
    end
  else
    savememory=0;
  end
  % evaluate the gradient for training covariance
  if nargin == 2 || isempty(x2)

    ncf = length(gpcf.cf);

    % evaluate the individual covariance functions
    zs = zeros(n,n,ncf);
    for i=1:ncf
      cf = gpcf.cf{i};
      zs(:,:,i) = cf.fh.trcov(cf, x(:,i));
    end

    % Evaluate the gradients
    ind = 1:ncf;
    if ~savememory
      DKff=cellfun(@(a) zeros(n,n), cell(1,numel(x)), 'UniformOutput', 0);
    else
      DKff=cellfun(@(a) zeros(n,n), cell(1,n), 'UniformOutput', 0);
    end
    for i=1:ncf
      cf = gpcf.cf{i};
      if ~savememory
        DK = cf.fh.ginput(cf, x(:,i));
      else
        DK = cf.fh.ginput(cf, x(:,i), [], i1);
      end

      CC = gpcf.sigma2(1).*ones(n,n);
      if r > 1
        es = degrees(r-1, zs, ind(ind~=i));
        for j = 2:r
          CC = CC + gpcf.sigma2(j).*es(:,:,j-1);
        end
      end
      for j = 1:length(DK)
        DKff{j} = DKff{j} + DK{j}.*CC;
      end
    end

    % Evaluate the gradient of non-symmetric covariance (e.g. K_fu)
  elseif nargin == 3 ||  nargin == 4
    if size(x,2) ~= size(x2,2)
      error('gpcf_prod -> _ghyper: The number of columns in x and x2 has to be the same. ')
    end

    ncf = length(gpcf.cf);

    % evaluate the individual covariance functions
    zs = zeros(n,n,ncf);
    for i=1:ncf
      cf = gpcf.cf{i};
      zs(:,:,i) = cf.fh.cov(cf, x(:,i), x2(:,i));
    end

    % Evaluate the gradients
    ind = 1:ncf;
    if ~savememory
      DKff=cellfun(@(a) zeros(n,n), cell(1,numel(x)), 'UniformOutput', 0);
    else
      DKff=cellfun(@(a) zeros(n,n), cell(1,n), 'UniformOutput', 0);
    end
    for i=1:ncf
      cf = gpcf.cf{i};
      if ~savememory
        DK = cf.fh.ginput(cf, x(:,i), x2(:,i));
      else
        DK = cf.fh.ginput(cf, x(:,i), x2(:,i), i1);
      end

      CC = gpcf.sigma2(1).*ones(n,n);
      if r > 1
        es = degrees(r-1, zs, ind(ind~=i));
        for j = 2:r
          CC = CC + gpcf.sigma2(j).*es(:,:,j-1);
        end
      end
      for j = 1:length(DK)
        DKff{j} = DKff{j} + DK{j}.*CC;
      end
    end
  end

end

function C = gpcf_additive_cov(gpcf, x1, x2)
%GP_ADDITIVE_COV  Evaluate covariance matrix between two input vectors
%
%  Description
%    C = GP_ADDITIVE_COV(GP, TX, X) takes in covariance function of a
%    Gaussian process GP and two matrixes TX and X that contain
%    input vectors to GP. Returns covariance matrix C. Every
%    element ij of C contains covariance between inputs i in TX
%    and j in X. This is a mandatory subfunction used for example in
%    prediction and energy computations.
%
%
%  See also
%    GPCF_ADDITIVE_TRCOV, GPCF_ADDITIVE_TRVAR, GP_COV, GP_TRCOV

  if isempty(x2)
    x2=x1;
  end
  [n1,m1]=size(x1);
  [n2,m2]=size(x2);

  if m1~=m2
    error('the number of columns of X1 and X2 has to be same')
  end

  ncf = length(gpcf.cf);

  if m1~=ncf
    error('input dimension does not match with number of additive kernels')
  end

  r = gpcf.max_deg;

  if r == 1
    % Only one degree
    C = 0;
    for d = 1:ncf
      cf = gpcf.cf{d};
      C = C + cf.fh.cov(cf, x1(:,d), x2(:,d));
    end
    C = C.*gpcf.sigma2(1);

  elseif r == 2
    % Only two degrees
    zs = zeros(n1,n2,ncf);
    for d = 1:ncf
      cf = gpcf.cf{d};
      zs(:,:,d) = cf.fh.cov(cf, x1(:,d), x2(:,d));
    end
    C = 0;
    for i1 = 1:ncf-1
      for i2 = i1+1:ncf
        C = C + zs(:,:,i1).*zs(:,:,i2);
      end
    end
    C = C.*gpcf.sigma2(2);
    C = C + sum(zs,3).*gpcf.sigma2(1);

  elseif r > 2
    % Over two degrees ... use Newton-Girard formulae
    zs = zeros(n1,n2,ncf);
    for d = 1:ncf
      cf = gpcf.cf{d};
      zs(:,:,d) = cf.fh.cov(cf, x1(:,d), x2(:,d));
    end
    sk = zeros(n1,n2,r);
    sk(:,:,1) = sum(zs,3);
    for i = 2:r
      sk(:,:,i) = sum(zs.^i, 3);
    end
    clear zs
    es = zeros(n1,n2,r);
    es(:,:,1) = sk(:,:,1);
    for i = 2:r
      presign = true;
      for k = 1:i-1
        if presign
          es(:,:,i) = es(:,:,i) + es(:,:,i-k).*sk(:,:,k);
          presign = false;
        else
          es(:,:,i) = es(:,:,i) - es(:,:,i-k).*sk(:,:,k);
          presign = true;
        end
      end
      if presign
        es(:,:,i) = es(:,:,i) + sk(:,:,i);
      else
        es(:,:,i) = es(:,:,i) - sk(:,:,i);
      end
      es(:,:,i) = es(:,:,i)./i;
    end
    for i = 1:r
      es(:,:,i) = es(:,:,i).*gpcf.sigma2(i);
    end
    C = sum(es,3);

  else
    error('Invalid max_deg parameter')
  end

end

function C = gpcf_additive_trcov(gpcf, x)
%GP_ADDITIVE_TRCOV     Evaluate training covariance matrix of inputs
%
%  Description
%    C = GP_ADDITIVE_TRCOV(GP, TX) takes in covariance function of a
%    Gaussian process GP and matrix TX that contains training
%    input vectors. Returns covariance matrix C. Every element ij
%    of C contains covariance between inputs i and j in TX. This
%    is a mandatory subfunction used for example in prediction and
%    energy computations.
%
%  See also
%    GPCF_ADDITIVE_COV, GPCF_ADDITIVE_TRVAR, GP_COV, GP_TRCOV

  [n,m]=size(x);

  ncf = length(gpcf.cf);

  if m~=ncf
    error('input dimension does not match with number of additive kernels')
  end

  r = gpcf.max_deg;

  if r == 1
    % Only one degree
    C = 0;
    for d = 1:ncf
      cf = gpcf.cf{d};
      C = C + cf.fh.trcov(cf, x(:,d));
    end
    C = C.*gpcf.sigma2(1);

  elseif r == 2
    % Only two degrees
    zs = zeros(n,n,ncf);
    for d = 1:ncf
      cf = gpcf.cf{d};
      zs(:,:,d) = cf.fh.trcov(cf, x(:,d));
    end
    C = 0;
    for i1 = 1:ncf-1
      for i2 = i1+1:ncf
        C = C + zs(:,:,i1).*zs(:,:,i2);
      end
    end
    C = C.*gpcf.sigma2(2);
    C = C + sum(zs,3).*gpcf.sigma2(1);

  elseif r > 2
    % Over two degrees ... use Newton-Girard formulae
    zs = zeros(n,n,ncf);
    for d = 1:ncf
      cf = gpcf.cf{d};
      zs(:,:,d) = cf.fh.trcov(cf, x(:,d));
    end
    sk = zeros(n,n,r);
    sk(:,:,1) = sum(zs,3);
    for i = 2:r
      sk(:,:,i) = sum(zs.^i, 3);
    end
    clear zs
    es = zeros(n,n,r);
    es(:,:,1) = sk(:,:,1);
    for i = 2:r
      presign = true;
      for k = 1:i-1
        if presign
          es(:,:,i) = es(:,:,i) + es(:,:,i-k).*sk(:,:,k);
          presign = false;
        else
          es(:,:,i) = es(:,:,i) - es(:,:,i-k).*sk(:,:,k);
          presign = true;
        end
      end
      if presign
        es(:,:,i) = es(:,:,i) + sk(:,:,i);
      else
        es(:,:,i) = es(:,:,i) - sk(:,:,i);
      end
      es(:,:,i) = es(:,:,i)./i;
    end
    for i = 1:r
      es(:,:,i) = es(:,:,i).*gpcf.sigma2(i);
    end
    C = sum(es,3);

  else
    error('Invalid max_deg parameter')
  end
end

function C = gpcf_additive_trvar(gpcf, x)
% GP_ADDITIVE_TRVAR     Evaluate training variance vector
%
%  Description
%    C = GP_ADDITIVE_TRVAR(GPCF, TX) takes in covariance function of
%    a Gaussian process GPCF and matrix TX that contains training
%    inputs. Returns variance vector C. Every element i of C
%    contains variance of input i in TX. This is a mandatory
%    subfunction used for example in prediction and energy computations.
%
%  See also
%    GPCF_ADDITIVE_COV, GP_COV, GP_TRCOV

  [n,m]=size(x);

  ncf = length(gpcf.cf);

  if m~=ncf
    error('input dimension does not match with number of additive kernels')
  end

  r = gpcf.max_deg;

  if r == 1
    % Only one degree
    C = 0;
    for d = 1:ncf
      cf = gpcf.cf{d};
      C = C + cf.fh.trvar(cf, x(:,d));
    end
    C = C.*gpcf.sigma2(1);

  elseif r == 2
    % Only two degrees
    zs = zeros(n,ncf);
    for d = 1:ncf
      cf = gpcf.cf{d};
      zs(:,d) = cf.fh.trvar(cf, x(:,d));
    end
    C = 0;
    for i1 = 1:ncf-1
      for i2 = i1+1:ncf
        C = C + zs(:,i1).*zs(:,i2);
      end
    end
    C = C.*gpcf.sigma2(2);
    C = C + sum(zs,2).*gpcf.sigma2(1);

  elseif r > 2
    % Over two degrees ... use Newton-Girard formulae
    zs = zeros(n,ncf);
    for d = 1:ncf
      cf = gpcf.cf{d};
      zs(:,d) = cf.fh.trvar(cf, x(:,d));
    end
    sk = zeros(n,r);
    sk(:,1) = sum(zs,2);
    for i = 2:r
      sk(:,i) = sum(zs.^i, 2);
    end
    clear zs
    es = zeros(n,r);
    es(:,1) = sk(:,1);
    for i = 2:r
      presign = true;
      for k = 1:i-1
        if presign
          es(:,i) = es(:,i) + es(:,i-k).*sk(:,k);
          presign = false;
        else
          es(:,i) = es(:,i) - es(:,i-k).*sk(:,k);
          presign = true;
        end
      end
      if presign
        es(:,i) = es(:,i) + sk(:,i);
      else
        es(:,i) = es(:,i) - sk(:,i);
      end
      es(:,i) = es(:,i)./i;
    end
    for i = 1:r
      es(:,i) = es(:,i).*gpcf.sigma2(i);
    end
    C = sum(es,2);

  else
    error('Invalid max_deg parameter')
  end

end

function reccf = gpcf_additive_recappend(reccf, ri, gpcf)
%RECAPPEND  Record append
%
%  Description
%    RECCF = GPCF_ADDITIVE_RECAPPEND(RECCF, RI, GPCF) takes a
%    covariance function record structure RECCF, record index RI
%    and covariance function structure GPCF with the current MCMC
%    samples of the parameters. Returns RECCF which contains all
%    the old samples and the current samples from GPCF. This
%    subfunction is needed when using MCMC sampling (gp_mc).
%
%  See also
%    GP_MC, GP_MC->RECAPPEND

  if nargin == 2
    % Initialize the record
    reccf.type = 'gpcf_additive';

    % Initialize parameters
    reccf.sigma2=[];

    ncf = length(ri.cf);
    for i=1:ncf
      cf = ri.cf{i};
      reccf.cf{i} = cf.fh.recappend([], ri.cf{i});
    end

    % Set the function handles
    reccf.fh.pak = @gpcf_additive_pak;
    reccf.fh.unpak = @gpcf_additive_unpak;
    reccf.fh.lp = @gpcf_additive_lp;
    reccf.fh.lpg = @gpcf_additive_lpg;
    reccf.fh.cfg = @gpcf_additive_cfg;
    reccf.fh.cov = @gpcf_additive_cov;
    reccf.fh.trcov  = @gpcf_additive_trcov;
    reccf.fh.trvar  = @gpcf_additive_trvar;
    reccf.fh.recappend = @gpcf_additive_recappend;

    % Set other
    reccf.max_deg = ri.max_deg;
    reccf.p=[];
    reccf.p.sigma2=[];
    if isfield(ri.p,'sigma2') && ~isempty(ri.p.sigma2)
      reccf.p.sigma2 = ri.p.sigma2;
    end

  else
    % Append to the record
    gpp = gpcf.p;

    % record sigma2
    reccf.sigma2(ri,:) = gpcf.sigma2;
    if isfield(gpp,'sigma2') && ~isempty(gpp.sigma2)
      reccf.p.sigma2 = gpp.sigma2.fh.recappend(reccf.p.sigma2, ri, gpcf.p.sigma2);
    end

    % Loop over all of the covariance functions
    ncf = length(gpcf.cf);
    for i=1:ncf
      cf = gpcf.cf{i};
      reccf.cf{i} = cf.fh.recappend(reccf.cf{i}, ri, cf);
    end
  end
end


%% DEMO_febio_0047_cylinder_embedded_probe_01
% Below is a demonstration for:
%
% * Building geometry for a tissue segment with an embedded probe
% * Defining the boundary conditions
% * Coding the febio structure
% * Running the model
% * Importing and visualizing the displacement results

%% Keywords
%
% * febio_spec version 3.0
% * febio, FEBio
% * probe
% * rigid body constraints
% * tetrahedral elements, tet4
% * triangular elements, tri3
% * slab, block, rectangular
% * sphere
% * static, solid
% * hyperelastic, Ogden
% * displacement logfile
% * stress logfile

%%
clear; close all; clc;

%%
% Plot settings
fontSize=15;
faceAlpha=1;
lineWidth1=1.5;
lineWidth2=3;
markerSize1=15;
markerSize2=30;
edgeWidth=2;
edgeColor='k';
faceAlpha1=1;

%% Control parameters

% Path names
defaultFolder = fileparts(fileparts(mfilename('fullpath')));
savePath=fullfile(defaultFolder,'data','temp');

% Defining file names
febioFebFileNamePart='tempModel';
febioFebFileName=fullfile(savePath,[febioFebFileNamePart,'.feb']); %FEB file name
febioLogFileName=[febioFebFileNamePart,'.txt']; %FEBio log file name
febioLogFileName_disp=[febioFebFileNamePart,'_disp_out.txt']; %Log file name for exporting displacement
febioLogFileName_force=[febioFebFileNamePart,'_force_out.txt']; %Log file name for exporting force
febioLogFileName_strainEnergy=[febioFebFileNamePart,'_energy_out.txt']; %Log file name for exporting strain energy density
febioLogFileName_strain=[febioFebFileNamePart,'_strain_out.txt']; %Log file name for exporting strain

probeHeight=75;

probeRadius=2; % The radius of the hemi-spher portion
nRefine=0; % Number of |subtri| refinements for icosahedron

pointSpacing=3;
dAdd=7;
tissueRadius=probeRadius+dAdd;
tissueHeight=probeHeight+dAdd;
volumeFactor=5;

displacementMagnitude=-1;

%Material parameter set
c1=1e-3; %Shear-modulus-like parameter
m1=2; %Material parameter setting degree of non-linearity
k_factor=1e2; %Bulk modulus factor
k=c1*k_factor; %Bulk modulus

% FEA control settings
numTimeSteps=10; %Number of time steps desired
max_refs=25; %Max reforms
max_ups=0; %Set to zero to use full-Newton iterations
opt_iter=6; %Optimum number of iterations
max_retries=5; %Maximum number of retires
dtmin=(1/numTimeSteps)/100; %Minimum time step size
dtmax=1/numTimeSteps; %Maximum time step size

%% Build probe

probeMeshInputStruct.sphereRadius=probeRadius;% => The radius of the hemi-spher portion
probeMeshInputStruct.nRefine=nRefine;% => Number of |subtri| refinements for icosahedron
probeMeshInputStruct.cylinderHeight=probeHeight-probeRadius;% => height of the cylinder part
probeMeshInputStruct.cylinderStepSize=[];% => Aproximate node spacing for cylinder portion
probeMeshInputStruct.patchType='tri';

[Fp,Vp,Cp]=hemiSphereCylMesh(probeMeshInputStruct);
Fp=fliplr(Fp); %Invert face orientation

Vp(:,3)=Vp(:,3)-max(Vp(:,3));

%Get top curve
Eb=patchBoundary(Fp);
indProbeTop=edgeListToCurve(Eb);
indProbeTop=indProbeTop(1:end-1);
Vst=Vp(indProbeTop,:);

%%
cFigure; hold on;
gpatch(Fp,Vp,'gw','k');
plotV(Vst,'b.-','lineWidth',lineWidth1,'MarkerSize',markerSize1);
patchNormPlot(Fp,Vp);
axisGeom(gca,fontSize);
drawnow;

%% Build tissue

%Sketching profile
ns=150;
t=linspace(0,2*pi,ns);
t=t(1:end-1);

x=tissueRadius*cos(t);
y=tissueRadius*sin(t);
z=zeros(size(x));
Vc=[x(:) y(:) z(:)];
np=ceil(max(pathLength(Vc))./pointSpacing);
[Vc]=evenlySampleCurve(Vc,np,'pchip',1);

% Extruding model
cPar.numSteps=round(tissueHeight/pointSpacing);
cPar.depth=tissueHeight;
cPar.patchType='tri';
cPar.dir=-1;
cPar.closeLoopOpt=1;
[Fg,Vg]=polyExtrude(Vc,cPar);
Fg=fliplr(Fg);

Vgb=Vg(cPar.numSteps:cPar.numSteps:end,:);

Vgt=Vg(1:cPar.numSteps:end,:);

%% Cap ends

regionCell={Vgt(:,[1 2]),Vst(:,[1 2])};
[Ft,Vt]=regionTriMesh2D(regionCell,pointSpacing,0,0);
Vt(:,3)=mean(Vgt(:,3));

regionCell={Vgb(:,[1 2])};
[Fb,Vb]=regionTriMesh2D(regionCell,pointSpacing,0,0);
Fb=fliplr(Fb); %flip face orientation
Vb(:,3)=mean(Vgb(:,3));

%%
% Visualize

cFigure; hold on;

gpatch(Fp,Vp,'rw','k',0.5);
gpatch(Fg,Vg,'gw','k',0.5);
gpatch(Fb,Vb,'bw','k',0.5);
gpatch(Ft,Vt,'bw','k',0.5);

plotV(Vgb,'b.-','lineWidth',lineWidth1,'MarkerSize',markerSize1);
plotV(Vgt,'b.-','lineWidth',lineWidth1,'MarkerSize',markerSize1);
plotV(Vst,'b.-','lineWidth',lineWidth1,'MarkerSize',markerSize1);

axisGeom(gca,fontSize);
drawnow;

%% Merge model components

[F,V,C]=joinElementSets({Fg,Ft,Fb,Fp},{Vg,Vt,Vb,Vp});
[F,V]=mergeVertices(F,V);

%%

cFigure;
subplot(1,2,1); hold on;
gpatch(F,V,C,'none',0.5);
axisGeom(gca,fontSize);
colormap gjet; icolorbar;

subplot(1,2,2); hold on;
gpatch(F,V,C);
patchNormPlot(F,V,2);
plotV(Vst,'b.-','lineWidth',lineWidth1,'MarkerSize',markerSize1);
axisGeom(gca,fontSize);
colormap gjet; icolorbar;

drawnow;

%% Mesh solid using tetgen

%%
% Create tetgen meshing input structure

[regionA]=tetVolMeanEst(F,V); %Volume for a regular tet based on edge lengths
V_inner=getInnerPoint(F,V); %Interior point for region

inputStruct.stringOpt='-pq1.2AaY';
inputStruct.Faces=F;
inputStruct.Nodes=V;
inputStruct.holePoints=[];
inputStruct.faceBoundaryMarker=C; %Face boundary markers
inputStruct.regionPoints=V_inner; %region points
inputStruct.regionA=regionA*volumeFactor; %Desired volume for tets
inputStruct.minRegionMarker=2; %Minimum region marker

%%
% Mesh model using tetrahedral elements using tetGen

[meshOutput]=runTetGen(inputStruct); %Run tetGen

%%
% Visualize mesh

meshView(meshOutput);

%%
% Access model element and patch data
F=meshOutput.faces;
V=meshOutput.nodes;
C=meshOutput.faceMaterialID;
E=meshOutput.elements;
elementMaterialID=meshOutput.elementMaterialID;

Fb=meshOutput.facesBoundary;
Cb=meshOutput.boundaryMarker;

%% Define boundary condition node sets

logicRigid= Cb==1 | Cb==3;
bcSupportList=Fb(logicRigid,:);
bcSupportList=unique(bcSupportList(:));

logicIndentor= Cb==4;
bcPrescribeList=Fb(logicIndentor,:);
bcPrescribeList=unique(bcPrescribeList(:));

%%
% Visualize boundary conditions

cFigure; hold on;
gpatch(Fb,V,'bw','none',0.5);
hp(1)=plotV(V(bcSupportList,:),'k.','lineWidth',lineWidth1,'MarkerSize',markerSize1);
hp(2)=plotV(V(bcPrescribeList,:),'r.','lineWidth',lineWidth1,'MarkerSize',markerSize1);
legend(hp,{'BC Full support','BC Prescribed displacement'});
axisGeom(gca,fontSize);
drawnow;

%% Defining the FEBio input structure
% See also |febioStructTemplate| and |febioStruct2xml| and the FEBio user
% manual.

%Get a template with default settings
[febio_spec]=febioStructTemplate;

%febio_spec version
febio_spec.ATTR.version='3.0';

%Module section
febio_spec.Module.ATTR.type='solid';

%Create control structure for use by all steps
stepStruct.Control.time_steps=numTimeSteps;
stepStruct.Control.step_size=1/numTimeSteps;
stepStruct.Control.solver.max_refs=max_refs;
stepStruct.Control.solver.max_ups=max_ups;
stepStruct.Control.time_stepper.dtmin=dtmin;
stepStruct.Control.time_stepper.dtmax=dtmax;
stepStruct.Control.time_stepper.max_retries=max_retries;
stepStruct.Control.time_stepper.opt_iter=opt_iter;

%Add template based default settings to proposed control section
[stepStruct.Control]=structComplete(stepStruct.Control,febio_spec.Control,1); %Complement provided with default if missing

%Remove control field (part of template) since step specific control sections are used
febio_spec=rmfield(febio_spec,'Control');

febio_spec.Step.step{1}.Control=stepStruct.Control;
febio_spec.Step.step{1}.ATTR.id=1;
febio_spec.Step.step{2}.Control=stepStruct.Control;
febio_spec.Step.step{2}.ATTR.id=2;

%Material section
materialName1='Material1';
febio_spec.Material.material{1}.ATTR.name=materialName1;
febio_spec.Material.material{1}.ATTR.type='Ogden';
febio_spec.Material.material{1}.ATTR.id=1;
febio_spec.Material.material{1}.c1=c1;
febio_spec.Material.material{1}.m1=m1;
febio_spec.Material.material{1}.c2=c1;
febio_spec.Material.material{1}.m2=-m1;
febio_spec.Material.material{1}.k=k;
% Mesh section
% -> Nodes
febio_spec.Mesh.Nodes{1}.ATTR.name='Object1'; %The node set name
febio_spec.Mesh.Nodes{1}.node.ATTR.id=(1:size(V,1))'; %The node id's
febio_spec.Mesh.Nodes{1}.node.VAL=V; %The nodel coordinates

% -> Elements
partName1='Part1';
febio_spec.Mesh.Elements{1}.ATTR.name=partName1; %Name of this part
febio_spec.Mesh.Elements{1}.ATTR.type='tet4'; %Element type
febio_spec.Mesh.Elements{1}.elem.ATTR.id=(1:1:size(E,1))'; %Element id's
febio_spec.Mesh.Elements{1}.elem.VAL=E; %The element matrix

% -> NodeSets
nodeSetName1='bcSupportList';
nodeSetName2='bcPrescribeList';

febio_spec.Mesh.NodeSet{1}.ATTR.name=nodeSetName1;
febio_spec.Mesh.NodeSet{1}.node.ATTR.id=bcSupportList(:);

febio_spec.Mesh.NodeSet{2}.ATTR.name=nodeSetName2;
febio_spec.Mesh.NodeSet{2}.node.ATTR.id=bcPrescribeList(:);

%MeshDomains section
febio_spec.MeshDomains.SolidDomain.ATTR.name=partName1;
febio_spec.MeshDomains.SolidDomain.ATTR.mat=materialName1;

%Boundary condition section
%-> Fix boundary conditions
febio_spec.Boundary.bc{1}.ATTR.type='fix';
febio_spec.Boundary.bc{1}.ATTR.node_set=nodeSetName1;
febio_spec.Boundary.bc{1}.dofs='x,y,z';

% -> Prescribe boundary conditions
%STEP 1 Up/down
febio_spec.Step.step{1}.Boundary.bc{1}.ATTR.type='prescribe';
febio_spec.Step.step{1}.Boundary.bc{1}.ATTR.node_set=nodeSetName2;
febio_spec.Step.step{1}.Boundary.bc{1}.dof='z';
febio_spec.Step.step{1}.Boundary.bc{1}.scale.ATTR.lc=1;
febio_spec.Step.step{1}.Boundary.bc{1}.scale.VAL=displacementMagnitude;
febio_spec.Step.step{1}.Boundary.bc{1}.relative=1;

febio_spec.Step.step{1}.Boundary.bc{2}.ATTR.type='fix';
febio_spec.Step.step{1}.Boundary.bc{2}.ATTR.node_set=nodeSetName2;
febio_spec.Step.step{1}.Boundary.bc{2}.dofs='x,y';

%STEP 2 Sideways
febio_spec.Step.step{2}.Boundary.bc{1}.ATTR.type='prescribe';
febio_spec.Step.step{2}.Boundary.bc{1}.ATTR.node_set=nodeSetName2;
febio_spec.Step.step{2}.Boundary.bc{1}.dof='x';
febio_spec.Step.step{2}.Boundary.bc{1}.scale.ATTR.lc=2;
febio_spec.Step.step{2}.Boundary.bc{1}.scale.VAL=displacementMagnitude;
febio_spec.Step.step{2}.Boundary.bc{1}.relative=1;

febio_spec.Step.step{2}.Boundary.bc{2}.ATTR.type='fix';
febio_spec.Step.step{2}.Boundary.bc{2}.ATTR.node_set=nodeSetName2;
febio_spec.Step.step{2}.Boundary.bc{2}.dofs='y,z';

%LoadData section
% -> load_controller
febio_spec.LoadData.load_controller{1}.ATTR.id=1;
febio_spec.LoadData.load_controller{1}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{1}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{1}.points.point.VAL=[0 0; 0.25 1; 0.5 0; 0.75 -1; 1 0];

febio_spec.LoadData.load_controller{2}.ATTR.id=2;
febio_spec.LoadData.load_controller{2}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{2}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{2}.points.point.VAL=[1 0; 1.25 1; 1.5 0; 1.75 -1; 2 0];

%Output section
% -> log file
febio_spec.Output.logfile.ATTR.file=febioLogFileName;
febio_spec.Output.logfile.node_data{1}.ATTR.file=febioLogFileName_disp;
febio_spec.Output.logfile.node_data{1}.ATTR.data='ux;uy;uz';
febio_spec.Output.logfile.node_data{1}.ATTR.delim=',';

febio_spec.Output.logfile.node_data{2}.ATTR.file=febioLogFileName_force;
febio_spec.Output.logfile.node_data{2}.ATTR.data='Rx;Ry;Rz';
febio_spec.Output.logfile.node_data{2}.ATTR.delim=',';

febio_spec.Output.logfile.element_data{1}.ATTR.file=febioLogFileName_strain;
febio_spec.Output.logfile.element_data{1}.ATTR.data='E1;E2;E3';
febio_spec.Output.logfile.element_data{1}.ATTR.delim=',';

%% Quick viewing of the FEBio input file structure
% The |febView| function can be used to view the xml structure in a MATLAB
% figure window.

%%
% |febView(febio_spec); %Viewing the febio file|

%% Exporting the FEBio input file
% Exporting the febio_spec structure to an FEBio input file is done using
% the |febioStruct2xml| function.

febioStruct2xml(febio_spec,febioFebFileName); %Exporting to file and domNode

%% Running the FEBio analysis
% To run the analysis defined by the created FEBio input file the
% |runMonitorFEBio| function is used. The input for this function is a
% structure defining job settings e.g. the FEBio input file name. The
% optional output runFlag informs the user if the analysis was run
% succesfully.

febioAnalysis.run_filename=febioFebFileName; %The input file name
febioAnalysis.run_logname=febioLogFileName; %The name for the log file
febioAnalysis.disp_on=1; %Display information on the command window
febioAnalysis.runMode='external';%'internal' or 'external';

[runFlag]=runMonitorFEBio(febioAnalysis);%START FEBio NOW!!!!!!!!

%% Import FEBio results

if runFlag==1 %i.e. a succesful run

    % Importing nodal displacements from a log file
    [time_mat, N_disp_mat,~]=importFEBio_logfile(fullfile(savePath,febioLogFileName_disp)); %Nodal displacements
    time_mat=[0; time_mat(:)]; %Time

    N_disp_mat=N_disp_mat(:,2:end,:);
    sizImport=size(N_disp_mat);
    sizImport(3)=sizImport(3)+1;
    N_disp_mat_n=zeros(sizImport);
    N_disp_mat_n(:,:,2:end)=N_disp_mat;
    N_disp_mat=N_disp_mat_n;

    DN_MAG=sqrt(sum(N_disp_mat.^2,2));
    DN=N_disp_mat(:,:,end);
    DN_magnitude=sqrt(sum(DN(:,3).^2,2));
    V_def=V+DN;
    V_DEF=N_disp_mat+repmat(V,[1 1 size(N_disp_mat,3)]);
    X_DEF=V_DEF(:,1,:);
    Y_DEF=V_DEF(:,2,:);
    Z_DEF=V_DEF(:,3,:);
    [CF_def]=vertexToFaceMeasure(Fb,DN_magnitude);

     %%
    % Importing element data from a log file
    [~,E_data,~]=importFEBio_logfile(fullfile(savePath,febioLogFileName_strain)); %Element data

    %Remove nodal index column
    E_data=E_data(:,2:end,:);

    E_data=sqrt(0.5*( (E_data(:,1,:)-E_data(:,2,:)).^2 + (E_data(:,2,:)-E_data(:,3,:)).^2 + (E_data(:,3,:)-E_data(:,1,:)).^2 ));

    %Add initial state i.e. zero
    sizImport=size(E_data);
    sizImport(3)=sizImport(3)+1;
    E_data_mat_n=zeros(sizImport);
    E_data_mat_n(:,:,2:end)=E_data;
    E_data=E_data_mat_n;

    VE=patchCentre(E,V);
    logicCutElements=VE(:,2)>=0;

    [F_cut,CF_cut_data]=element2patch(E(logicCutElements,:),E_data(logicCutElements,:,1));
    [indBoundary]=tesBoundary(F_cut);

    CV=faceToVertexMeasure(F_cut(indBoundary,:),V,CF_cut_data(indBoundary,:));

    %%
    % Plotting the simulated results using |anim8| to visualize and animate
    % deformations

    % Create basic view and store graphics handle to initiate animation
    hf=cFigure; %Open figure
    gtitle([febioFebFileNamePart,': Press play to animate']);
%     subplot(1,2,1);
    hp1=gpatch(Fb,V_def,'kw','none',0.25); %Add graphics object to animate
    hp2=gpatch(F_cut(indBoundary,:),V_def,CV,'k',1); %Add graphics object to animate
    hp2.FaceColor='interp';
%     gpatch(Fb,V,0.5*ones(1,3),'k',0.25); %A static graphics object
    colormap(gjet(250)); colorbar;
    caxis([0 max(E_data(:))/3]);
    axisGeom(gca,fontSize);
    axis([min(X_DEF(:)) max(X_DEF(:)) min(Y_DEF(:)) max(Y_DEF(:)) min(Z_DEF(:)) max(Z_DEF(:))]);
    axis manual;
    camlight headlight;

    drawnow;

    % Set up animation features
    animStruct.Time=time_mat; %The time vector
    for qt=1:1:size(N_disp_mat,3) %Loop over time increments
        DN=N_disp_mat(:,:,qt); %Current displacement
        DN_magnitude=sqrt(sum(DN.^2,2)); %Current displacement magnitude
        V_def=V+DN; %Current nodal coordinates
%         [CF_def]=vertexToFaceMeasure(Fb,DN_magnitude); %Current color data to use

        [~,CF_cut_data]=element2patch(E(logicCutElements,:),E_data(logicCutElements,:,qt));
        CV=faceToVertexMeasure(F_cut(indBoundary,:),V,CF_cut_data(indBoundary,:));

        %Set entries in animation structure
        animStruct.Handles{qt}=[hp1 hp2 hp2]; %Handles of objects to animate
        animStruct.Props{qt}={'Vertices','Vertices','CData'}; %Properties of objects to animate
        animStruct.Set{qt}={V_def,V_def,CV}; %Property values for to set in order to animate
    end
    anim8(hf,animStruct); %Initiate animation feature
    drawnow;

end

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function Volume=polygon2voxel_double(FacesA,FacesB,FacesC,VerticesX,VerticesY,VerticesZ,VolumeSize,Wrap)
Vertices=[VerticesX(:) VerticesY(:) VerticesZ(:)]-1;

% List with all vertices coordinates of a face
FaceVertices=[Vertices(FacesA,:) Vertices(FacesB,:) Vertices(FacesC,:)];
Volume=false(VolumeSize);
Volume=DrawSplitFaces(FaceVertices,Volume,Wrap);

function Volume=DrawSplitFaces(FaceVertices,Volume,Wrap)
VolumeSize=size(Volume);
% Calculate squared edge distances
dist1=(FaceVertices(:,1)-FaceVertices(:,4)).*(FaceVertices(:,1)-FaceVertices(:,4))+(FaceVertices(:,2)-FaceVertices(:,5)).*(FaceVertices(:,2)-FaceVertices(:,5))+(FaceVertices(:,3)-FaceVertices(:,6)).*(FaceVertices(:,3)-FaceVertices(:,6));
dist2=(FaceVertices(:,7)-FaceVertices(:,4)).*(FaceVertices(:,7)-FaceVertices(:,4))+(FaceVertices(:,8)-FaceVertices(:,5)).*(FaceVertices(:,8)-FaceVertices(:,5))+(FaceVertices(:,9)-FaceVertices(:,6)).*(FaceVertices(:,9)-FaceVertices(:,6));
dist3=(FaceVertices(:,1)-FaceVertices(:,7)).*(FaceVertices(:,1)-FaceVertices(:,7))+(FaceVertices(:,2)-FaceVertices(:,8)).*(FaceVertices(:,2)-FaceVertices(:,8))+(FaceVertices(:,3)-FaceVertices(:,9)).*(FaceVertices(:,3)-FaceVertices(:,9));

% Calculate mFaceVertices(:,1) distance
maxdist=max([dist1(:) dist2(:),dist3(:)],[],2);

% Draw triangle if distance <=0.5 pixel
check=maxdist>0.5;

FVR=FaceVertices(~check,:);
% Select Vertices which must be split
FaceVertices=FaceVertices(check,:);
if(~isempty(FaceVertices))
    dist1=dist1(check);
    dist2=dist2(check);
    dist3=dist3(check);

    DX=(FaceVertices(:,1)+FaceVertices(:,4))/2; DY=(FaceVertices(:,2)+FaceVertices(:,5))/2; DZ=(FaceVertices(:,3)+FaceVertices(:,6))/2;
    FA1=[DX,DY,DZ,FaceVertices(:,4),FaceVertices(:,5),FaceVertices(:,6),FaceVertices(:,7),FaceVertices(:,8),FaceVertices(:,9)];
    FB1=[FaceVertices(:,1),FaceVertices(:,2),FaceVertices(:,3),DX,DY,DZ,FaceVertices(:,7),FaceVertices(:,8),FaceVertices(:,9)];

    DX=(FaceVertices(:,1)+FaceVertices(:,7))/2; DY=(FaceVertices(:,2)+FaceVertices(:,8))/2; DZ=(FaceVertices(:,3)+FaceVertices(:,9))/2;
    FA2=[DX,DY,DZ,FaceVertices(:,4),FaceVertices(:,5),FaceVertices(:,6),FaceVertices(:,7),FaceVertices(:,8),FaceVertices(:,9)];
    FB2=[FaceVertices(:,1),FaceVertices(:,2),FaceVertices(:,3),FaceVertices(:,4),FaceVertices(:,5),FaceVertices(:,6),DX,DY,DZ];

    DX=(FaceVertices(:,7)+FaceVertices(:,4))/2; DY=(FaceVertices(:,8)+FaceVertices(:,5))/2; DZ=(FaceVertices(:,9)+FaceVertices(:,6))/2;
    FA3=[FaceVertices(:,1),FaceVertices(:,2),FaceVertices(:,3),DX,DY,DZ,FaceVertices(:,7),FaceVertices(:,8),FaceVertices(:,9)];
    FB3=[FaceVertices(:,1),FaceVertices(:,2),FaceVertices(:,3),FaceVertices(:,4),FaceVertices(:,5),FaceVertices(:,6),DX,DY,DZ];

    DX=(FaceVertices(:,1)+FaceVertices(:,7))/2; DY=(FaceVertices(:,2)+FaceVertices(:,8))/2; DZ=(FaceVertices(:,3)+FaceVertices(:,9))/2;
    FA4=[DX,DY,DZ,FaceVertices(:,4),FaceVertices(:,5),FaceVertices(:,6),FaceVertices(:,7),FaceVertices(:,8),FaceVertices(:,9)];
    FB4=[FaceVertices(:,1),FaceVertices(:,2),FaceVertices(:,3),FaceVertices(:,4),FaceVertices(:,5),FaceVertices(:,6),DX,DY,DZ];

    dist12=dist1>dist2;
    dist12n=~dist12;
    FA1(dist12n,:)=FA3(dist12n,:);
    FB1(dist12n,:)=FB3(dist12n,:);
    FA2(dist12n,:)=FA4(dist12n,:);
    FB2(dist12n,:)=FB4(dist12n,:);
    dist1(dist12n,:)=dist2(dist12n,:);

    dist13=dist1>dist3;
    dist13n=~dist13;
    FA1(dist13n,:)=FA2(dist13n,:);
    FB1(dist13n,:)=FB2(dist13n,:);

    FaceVertices=[FA1;FB1];

    % Split / Draw Vertices
    Volume=DrawSplitFaces(FaceVertices,Volume,Wrap);
end

% Draw remaining faces
FaceVertices=FVR;

if(Wrap==0)
    % Calculate 1D volume indices
    indexA=mindex3(floor(FaceVertices(:,1)+0.5),floor(FaceVertices(:,2)+0.5), floor(FaceVertices(:,3)+0.5), VolumeSize(1), VolumeSize(2), VolumeSize(3), Wrap);
    indexB=mindex3(floor(FaceVertices(:,4)+0.5),floor(FaceVertices(:,5)+0.5), floor(FaceVertices(:,6)+0.5), VolumeSize(1), VolumeSize(2), VolumeSize(3), Wrap);
    indexC=mindex3(floor(FaceVertices(:,7)+0.5),floor(FaceVertices(:,8)+0.5), floor(FaceVertices(:,9)+0.5), VolumeSize(1), VolumeSize(2), VolumeSize(3), Wrap);

    % Remove outside vertices
    checkA=(FaceVertices(:,1)<0)|(FaceVertices(:,2)<0)|(FaceVertices(:,3)<0)|(FaceVertices(:,1)>(VolumeSize(1)-1))|(FaceVertices(:,2)>(VolumeSize(2)-1))|(FaceVertices(:,3)>(VolumeSize(3)-1));
    checkB=(FaceVertices(:,4)<0)|(FaceVertices(:,5)<0)|(FaceVertices(:,6)<0)|(FaceVertices(:,4)>(VolumeSize(1)-1))|(FaceVertices(:,5)>(VolumeSize(2)-1))|(FaceVertices(:,6)>(VolumeSize(3)-1));
    checkC=(FaceVertices(:,7)<0)|(FaceVertices(:,8)<0)|(FaceVertices(:,9)<0)|(FaceVertices(:,7)>(VolumeSize(1)-1))|(FaceVertices(:,8)>(VolumeSize(2)-1))|(FaceVertices(:,9)>(VolumeSize(3)-1));
    indexA(checkA)=[];
    indexB(checkB)=[];
    indexC(checkC)=[];

    % Draw the vertices
    Volume(indexA)=true;
    Volume(indexB)=true;
    Volume(indexC)=true;
else
     Volume(mindex3(floor(FaceVertices(:,1)+0.5),floor(FaceVertices(:,2)+0.5), floor(FaceVertices(:,3)+0.5), VolumeSize(1), VolumeSize(2), VolumeSize(3), Wrap))=1;
     Volume(mindex3(floor(FaceVertices(:,4)+0.5),floor(FaceVertices(:,5)+0.5), floor(FaceVertices(:,6)+0.5), VolumeSize(1), VolumeSize(2), VolumeSize(3), Wrap))=1;
     Volume(mindex3(floor(FaceVertices(:,7)+0.5),floor(FaceVertices(:,8)+0.5), floor(FaceVertices(:,9)+0.5), VolumeSize(1), VolumeSize(2), VolumeSize(3), Wrap))=1;
end

function index=mindex3(x, y, z, sizx, sizy, sizz, Wrap)
if(Wrap==1)
    % Positive modules
    x=mod(x,sizx);
    y=mod(y,sizy);
    z=mod(z,sizz);
elseif(Wrap>1)
    % Clamp
    x=max(x,0); x=min(x,sizx-1);
    y=max(y,0); y=min(y,sizy-1);
    z=max(z,0); z=min(z,sizz-1);
end
index=z*sizx*sizy+y*sizx+x;
% matlab
index=index+1;


% =========================================================================
% An example code for the algorithm proposed in
%
%   Zhuolin Jiang, Zhe Lin, Larry S. Davis.
%   "Learning A Discriminative Dictionary for Sparse Coding via Label
%    Consistent K-SVD", CVPR 2011.
%
% Author: Zhuolin Jiang (zhuolin@umiacs.umd.edu)
% Date: 10-16-2011
% =========================================================================

clear all;
clc;
addpath(genpath('.\ksvdbox'));  % add K-SVD box
addpath(genpath('.\OMPbox')); % add sparse coding algorithem OMP
load('.\trainingdata\featurevectors.mat','training_feats', 'testing_feats', 'H_train', 'H_test');

%% constant
sparsitythres = 30; % sparsity prior
sqrt_alpha = 4; % weights for label constraint term
sqrt_beta = 2; % weights for classification err term
dictsize = 570; % dictionary size
iterations = 50; % iteration number
iterations4ini = 20; % iteration number for initialization

%% dictionary learning process
% get initial dictionary Dinit and Winit
fprintf('\nLC-KSVD initialization... ');
[Dinit,Tinit,Winit,Q_train] = initialization4LCKSVD(training_feats,H_train,dictsize,iterations4ini,sparsitythres);
fprintf('done!');

% run LC K-SVD Training (reconstruction err + class penalty)
fprintf('\nDictionary learning by LC-KSVD1...');
[D1,X1,T1,W1] = labelconsistentksvd1(training_feats,Dinit,Q_train,Tinit,H_train,iterations,sparsitythres,sqrt_alpha);
save('.\trainingdata\dictionarydata1.mat','D1','X1','W1','T1');
fprintf('done!');

% run LC k-svd training (reconstruction err + class penalty + classifier err)
fprintf('\nDictionary and classifier learning by LC-KSVD2...')
[D2,X2,T2,W2] = labelconsistentksvd2(training_feats,Dinit,Q_train,Tinit,H_train,Winit,iterations,sparsitythres,sqrt_alpha,sqrt_beta);
save('.\trainingdata\dictionarydata2.mat','D2','X2','W2','T2');
fprintf('done!');

%% classification process
[prediction1,accuracy1] = classification(D1, W1, testing_feats, H_test, sparsitythres);
fprintf('\nFinal recognition rate for LC-KSVD1 is : %.03f ', accuracy1);

[prediction2,accuracy2] = classification(D2, W2, testing_feats, H_test, sparsitythres);
fprintf('\nFinal recognition rate for LC-KSVD2 is : %.03f ', accuracy2);


%This Matlab script can be used to reproduce Figures 4.18-19 in the monograph:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Empty workspace and close figures
close all;
clear;

%Number of BSs
L = 16;

%Number of UEs per BS
K = 10;

%Define the range of BS antennas
Mrange = 10:10:100;

%Extract maximum number of BS antennas
Mmax = max(Mrange);

%Define the range of pilot reuse factors
fRange = [1 2 4];

%Select the number of setups with random UE locations
nbrOfSetups = 50;

%Select the number of channel realizations per setup
nbrOfRealizations = 500;

%% Propagation parameters

%Communication bandwidth
B = 20e6;

%Total uplink transmit power per UE (mW)
p = 100;

%Total downlink transmit power per UE (mW)
rho = 100;

%Noise figure at the BS (in dB)
noiseFigure = 7;

%Compute noise power
noiseVariancedBm = -174 + 10*log10(B) + noiseFigure;

%Select length of coherence block
tau_c = 200;

%Use the approximation of the Gaussian local scattering model
accuracy = 2;

%Angular standard deviation in the local scattering model (in degrees)
ASDdeg = 10;

%Prepare to save simulation results
sumSE_MR = zeros(length(Mrange),length(fRange),nbrOfSetups);
sumSE_ZF = zeros(length(Mrange),length(fRange),nbrOfSetups);
sumSE_SMMSE = zeros(length(Mrange),length(fRange),nbrOfSetups);
sumSE_RZF = zeros(length(Mrange),length(fRange),nbrOfSetups);
sumSE_MMMSE = zeros(length(Mrange),length(fRange),nbrOfSetups);

%% Go through all setups
for n = 1:nbrOfSetups

    %Output simulation progress
    disp([num2str(n) ' setups out of ' num2str(nbrOfSetups)]);

    %Compute channel statistics for one setup
    [R,channelGaindB] = functionExampleSetup(L,K,Mmax,accuracy,ASDdeg);

    %Compute the normalized average channel gain, where the normalization
    %is based on the noise power
    channelGainOverNoise = channelGaindB - noiseVariancedBm;

    %Go through all number of antennas
    for m = 1:length(Mrange)

        %Output simulation progress
        disp([num2str(m) ' antennas out of ' num2str(length(Mrange))]);

        %Go through all pilot reuse factors
        for s = 1:length(fRange)

            %Extract pilot reuse factor
            f = fRange(s);

            %Generate channel realizations with estimates and estimation
            %error correlation matrices
            [Hhat,C,tau_p,Rscaled,H] = functionChannelEstimates(R(1:Mrange(m),1:Mrange(m),:,:,:),channelGainOverNoise,nbrOfRealizations,Mrange(m),K,L,p,f);

            %Compute SEs with the estimation bound in Theorem 4.6 using
            %Monte Carlo simulations
            [SE_hardening_MR,SE_hardening_RZF,SE_hardening_MMMSE,SE_hardening_ZF,SE_hardening_SMMSE] = functionComputeSE_DL_hardening(H,Hhat,C,Rscaled,tau_c,tau_p,nbrOfRealizations,Mrange(m),K,L,p,rho);

            %Compute SEs with the estimation bound in Theorem 4.9 using
            %Monte Carlo simulations
            [SE_MR,SE_RZF,SE_MMMSE,SE_ZF,SE_SMMSE] = functionComputeSE_DL_estimation(H,Hhat,C,Rscaled,tau_c,tau_p,nbrOfRealizations,Mrange(m),K,L,p,rho);

            %Use the largest of the two bounds
            SE_MR(SE_hardening_MR>SE_MR) = SE_hardening_MR(SE_hardening_MR>SE_MR);
            SE_RZF(SE_hardening_RZF>SE_RZF) = SE_hardening_RZF(SE_hardening_RZF>SE_RZF);
            SE_MMMSE(SE_hardening_MMMSE>SE_MMMSE) = SE_hardening_MMMSE(SE_hardening_MMMSE>SE_MMMSE);
            SE_ZF(SE_hardening_ZF>SE_ZF) = SE_hardening_ZF(SE_hardening_ZF>SE_ZF);
            SE_SMMSE(SE_hardening_SMMSE>SE_SMMSE) = SE_hardening_SMMSE(SE_hardening_SMMSE>SE_SMMSE);

            %Save results
            sumSE_MR(m,s,n) = mean(sum(SE_MR,1));
            sumSE_ZF(m,s,n) = mean(sum(SE_ZF,1));
            sumSE_SMMSE(m,s,n) = mean(sum(SE_SMMSE,1));
            sumSE_RZF(m,s,n) = mean(sum(SE_RZF,1));
            sumSE_MMMSE(m,s,n) = mean(sum(SE_MMMSE,1));

            %Delete large matrices
            clear H Hhat C Rscaled;

        end

    end

    %Delete large matrices
    clear R;

end

%% Plot the simulation results
for s = 1:length(fRange)

    figure(s);
    hold on; box on;

    plot(Mrange,mean(sumSE_MMMSE(:,s,:),3),'rd-','LineWidth',1);
    plot(Mrange,mean(sumSE_SMMSE(:,s,:),3),'b:','LineWidth',1);
    plot(Mrange,mean(sumSE_RZF(:,s,:),3),'k-.','LineWidth',1);
    plot(Mrange,mean(sumSE_ZF(:,s,:),3),'r--','LineWidth',1);
    plot(Mrange,mean(sumSE_MR(:,s,:),3),'bs-','LineWidth',1);

    xlabel('Number of antennas (M)');
    ylabel('Average sum SE [bit/s/Hz/cell]');
    legend('M-MMSE','S-MMSE','RZF','ZF','MR','Location','NorthWest');
    ylim([0 60]);

end


function  gradH = lfmwhiteComputeGradThetaH2(gamma1, gamma2, t1, t2, ...
    gradTheta, isStationary1, isStationary2)

% LFMWHITECOMPUTEGRADTHETAH2 computes a portion of the LFM-WHITE kernel's gradient w.r.t. theta.
% FORMAT
% DESC Helper function for computing part of the gradient of
% the LFM-WHITE kernel w.r.t. to a generic parameter theta (mass, spring or
% damper). Used for obtaining the gradients w.r.t. parameters related to
% the second argument (gamma2).
% ARG gamma1 : Gamma value for first system.
% ARG gamma2 : Gamma value for second system.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG gradTheta : gradient of gamma w.r.t. the generic parameter theta.
% ARG isStationary1: indicates whether the stationary version of the first
% kernel is used (TRUE) or not (FALSE). Set to FALSE by default.
% ARG isStationary2: indicates whether the stationary version of the second
% kernel is used (TRUE) or not (FALSE). Set to FALSE by default.
% RETURN h : result of this subcomponent of the kernel for the given values.
%
% COPYRIGHT : David Luengo, 2009
%
% SEEALSO : lfmwhiteKernParamInit, lfmwhiteXlfmwhiteKernGradient,
% lfmwhiteComputeH, lfmwhiteComputeGradThetaH1

% KERN

if size(t1, 2) > 1 | size(t2, 2) > 1
  error('Input can only have one column');
end

if nargin < 6
    isStationary1 = false;
end
if nargin < 7
    isStationary2 = false;
end

T1 = repmat(t1, 1, size(t2, 1));
T2 = repmat(t2.', size(t1, 1), 1);
deltaT = T1 - T2;
indT = deltaT >= 0;
psi = gamma1 * (1-indT) + gamma2 * indT;
gradH = -(lfmwhiteComputeH(gamma1, gamma2, t1, t2, isStationary1, isStationary2) ...
    + abs(deltaT) .* exp(-psi .* abs(deltaT)) .* indT);
if ((isStationary1 == false) | (isStationary2 == false))
    gradH = gradH + T1 .* exp(-(gamma2 * T1 * double(isStationary2 == false) ...
        + gamma1 * T2 * double(isStationary1 == false)));
end
gradH = gradH * gradTheta / (gamma1 + gamma2);