function DEM = reclassify(DEM,varargin)

%RECLASSIFY generate univariate class intervals for an instance of GRIDobj
%
% Syntax
%
%     C = reclassify(DEM);
%     C = reclassify(DEM,'method',value)
%
% Description
%
%     reclassify bins continous values of an instance of GRIDobj by setting
%     class intervals based on different classification methods. The
%     default method is equal interval classification with 10 classes.
%
% Input Arguments
%
%     DEM       Grid (class = GRIDobj)
%
%     'Methods' and [values]
%
%     'equalintervals'   [number of classes]
%     'definedintervals' [vector of class breaks]
%     'equalquantiles'   [number of classes]
%     'definedquantiles' [vector of quantiles]
%                        e.g. [0.1 0.9] results in three classes
%     'kmeans'           [number of classes]
%                        uses the k_means algorithm of Yi Cao
%                        FEX submission 19344 included in this function
%                        ! may take a while for large grids !
%     'std'              [s = scale of standard deviation, e.g. 2]
%                        equal intervals with a width of std/s. Bins are
%                        centered around sample mean
%     'otsu'             [number of classes, must be (2 4 8 16 etc)]
%                        recursive Otsu thresholding (see function
%                        graythresh)
%
% Output argument
%
%     C        Classified grid (class = GRIDobj)
%
% Example
%
%     DEM = GRIDobj('srtm_bigtujunga30m_utm11.tif');
%     C   = reclassify(DEM,'equalquantiles',10);
%     imageschs(DEM,C)
%
% See also: k_means, graythresh
%
% Author: Wolfgang Schwanghart (w.schwanghart[at]geo.uni-potsdam.de)
% Date: 18. January, 2013

narginchk(1,3)

allowedmethods = {'equalintervals',...
                  'definedintervals',...
                  'definedquantiles',...
                  'equalquantiles',...
                  'kmeans',...
                  'standarddeviation',...
                  'std',...
                  'otsu'...
                 };
% nr of classes
if nargin == 1;
    method = 'equalinterval';
    num = 10;
else
    method = validatestring(varargin{1},allowedmethods);

    if nargin==3
        num = varargin{2};
    end
end

INAN = isnan(DEM.Z) | isinf(DEM.Z);
DEM.Z(INAN) = nan;
inan = any(INAN(:));

switch method
    case 'equalintervals'
        validateattributes(num,{'numeric'},{'scalar'});

        if inan
            DEM.Z(~INAN) = mat2gray(DEM.Z(~INAN));
            DEM.Z = grayslice(DEM.Z,num);
            if num == 256;
                DEM.Z = double(DEM.Z) + 1;
                DEM.Z(INAN) = nan;
            elseif num < 256;
                DEM.Z = DEM.Z + 1;
                DEM.Z(INAN) = 0;
            else
                DEM.Z(INAN) = nan;
            end

        else
            DEM.Z = mat2gray(DEM.Z);
            DEM.Z = grayslice(DEM.Z,num);

            if num <= 255
                DEM.Z = DEM.Z + 1;
            elseif num == 256
                DEM.Z = double(DEM.Z) + 1;
            end
        end
    case 'definedintervals'
        validateattributes(num,{'numeric'},{'vector'});
        num(end+1) = inf;
        num = [-inf; num(:)];
        siz = size(DEM.Z);
        [~,DEM.Z] = histc(DEM.Z(:),num);
        DEM.Z = reshape(DEM.Z,siz);
    case 'equalquantiles'
        validateattributes(num,{'numeric'},{'scalar','integer','>',1});
        z = DEM.Z(~INAN);
        z = z(:);
        z = sort(z,'ascend');
        q = (1:num)/num;
        q = ceil(q*numel(z));
        edges = z(q);
        edges(end) = inf;
        [~,DEM.Z] = histc(DEM.Z(:),[-inf; edges]);
        DEM.Z = reshape(DEM.Z,DEM.size);
        DEM.Z(INAN) = nan;
    case 'definedquantiles'
        validateattributes(num,{'numeric'},{'vector','>',0,'<=',1});
        p = num(:);
        if p(end) < 1;
            p(end+1) = 1;
        end

        z = DEM.Z(~INAN);
        z = z(:);
        n = numel(z);
        edges = interp1((0:n-1).'/(n-1), sort(z), p);

        edges(end) = inf;
        [~,DEM.Z] = histc(DEM.Z(:),[-inf; edges]);
        DEM.Z = reshape(DEM.Z,DEM.size);
        DEM.Z(INAN) = nan;

    case {'standarddeviation','std'}
        validateattributes(num,{'numeric'},{'vector','>',0,'<=',1});
        z = DEM.Z(~INAN);
        z = z(:);
        s = std(z(:));
        s = s*num;
        mz = mean(z);
        minz = min(z);
        maxz = max(z);

        edges1 = [mz-s/2 :-s: minz];
        if edges1(end) > minz
            edges1(end+1) = -inf;
        else
            edges1(end) = -inf;
        end
        edges2 = mz+s/2 :s: maxz;

        edges2(end) = inf;

        [~,DEM.Z] = histc(DEM.Z(:),[edges1(end:-1:1) edges2]);
        DEM.Z = reshape(DEM.Z,DEM.size);
        DEM.Z(INAN) = nan;

    case 'kmeans';

        validateattributes(num,{'numeric'},{'scalar','integer','>',1});
        z = DEM.Z(~INAN);
        z = z(:);

        IX = k_means(z,num);

        DEM.Z(:,:) = nan;
        DEM.Z(~INAN) = IX;
    case 'otsu';
        validateattributes(num,{'numeric'},{'scalar','integer','>',1});
        if ceil(log2(num)) ~= log2(num);
            error('TopToolbox:GRIDobj','log2(value) must be an integer')
        end

        INAN = ~INAN;
        z  = mat2gray(DEM.Z(INAN));
        DEM.Z(INAN) = cast(otsu(z,num),class(DEM.Z));
end

end

% subfunctions

function [gIdx,c]=k_means(X,k)
% K_MEANS    k-means clustring
%   IDX = k_means(X, K) partititions the N x P data matrix X into K
%   clusters through a fully vectorized algorithm, where N is the number of
%   data points and P is the number of dimensions (variables). The
%   partition minimizes the sum of point-to-cluster-centroid Euclidean
%   distances of all clusters. The returned N x 1 vector IDX contains the
%   cluster indices of each point.
%
%   IDX = k_means(X, C) works with the initial centroids, C, (K x P).
%
%   [IDX, C] = k_means(X, K) also returns the K cluster centroid locations
%   in the K x P matrix, C.
%
% See also kmeans

% Version 2.0, by Yi Cao at Cranfield University on 27 March 2008.

% Example 1: small data set
%{
N=200;
X = [randn(N,2)+ones(N,2); randn(N,2)-ones(N,2)];
[cidx, ctrs] = k_means(X, 2);
plot(X(cidx==1,1),X(cidx==1,2),'r.',X(cidx==2,1),X(cidx==2,2),'b.', ctrs(:,1),ctrs(:,2),'kx');
%}

% Example 2: large data set
%{
N=20000;
X = [randn(N,2)+ones(N,2); randn(N,2)-ones(N,2)];
tic
[cidx, ctrs] = k_means(X, 2);
toc
plot(X(cidx==1,1),X(cidx==1,2),'r.',X(cidx==2,1),X(cidx==2,2),'b.', ctrs(:,1),ctrs(:,2),'kx');
%}

% Example 3: large data set with 5 centroids
%{
N=20000;
X = [randn(N,2)+ones(N,2); randn(N,2)-ones(N,2)];
tic
[cidx, ctrs] = k_means(X, 5);
toc
plot(X(cidx==1,1),X(cidx==1,2),'.',...
X(cidx==2,1),X(cidx==2,2),'.',...
X(cidx==3,1),X(cidx==3,2),'.',...
X(cidx==4,1),X(cidx==4,2),'.',...
X(cidx==5,1),X(cidx==5,2),'.',...
ctrs(:,1),ctrs(:,2),'+','linewidth',2)
%}

% Example 4: Comparison with kmeans in Statistics Toolbox
%{
N=20000;
X = [randn(N,2)+ones(N,2); randn(N,2)-ones(N,2)];
rand('state',0);
tic
cidx = k_means(X, 20);
toc
% Compare with kmeans in Statistis Toolbox
rand('state',0);
tic,
cidx1 = kmeans(X, 20, 'Option', statset('MaxIter',200));
toc
%}

% Check input and output
narginchk(2,2);

[n,m]=size(X);

% Check if second input is centroids
if ~isscalar(k)
    c=k;
    k=size(c,1);
else
    c=X(ceil(rand(k,1)*n),:);
end

% allocating variables
g0=ones(n,1);
gIdx=zeros(n,1);
D=zeros(n,k);

% Main loop converge if previous partition is the same as current
while any(g0~=gIdx)
%     disp(sum(g0~=gIdx))
    g0=gIdx;
    % Loop for each centroid
    for t=1:k
        d=zeros(n,1);
        % Loop for each dimension
        for s=1:m
            d=d+(X(:,s)-c(t,s)).^2;
        end
        D(:,t)=d;
    end
    % Partition data to closest centroids
    [~,gIdx]=min(D,[],2);
    % Update centroids using means of partitions
    for t=1:k
        c(t,:)=mean(X(gIdx==t,:));
    end
%     for t=1:m
%         c(:,t)=accumarray(gIdx,X(:,t),[],@mean);
%     end
end

end

function ix = otsu(x,n)

if mod(n,2)~=0
    error('number of classes is odd');
end

I = x >= graythresh(x);
ix = I*n/2;% + 1;
% do until all classes are
if n > 2
ix(I) = otsu(x(I),n/2) + ix(I);
ix(~I) = otsu(x(~I),n/2) + ix(~I);
else
    ix = ix+1;

end

end


function C = minus(A,B)
%MINUS Binary subtraction for sparse tensors.
%
%   MINUS(A,B) is called for the syntax 'A - B' when A or B is a sparse
%   tensor. A and B must have the same size, unless one is a scalar. A
%   scalar can be subtracted from a sparse tensor of any size.
%
%   Examples
%   A = sptenrand([4 3 2],5); B = sptenrand([4 3 2],3);
%   A - B %<-- sparse
%   A - 5 %<-- dense
%   A - 0 %<-- dense
%   A - full(A) %<-- dense
%
%   See also SPTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%% Observations for sparse matrix case.
% The result of a - 5 is dense!
% The result of a - 0 is dense!
% The result of a - full(a) is dense!
% The result of a - b (two sparse matrices) is sparse.

%% Case 1: One argument is a scalar
% Emulating the sparse matrix case here, which creates and returns
% a dense result, even if the scalar is zero.

% Case 1a: Second argument is a scalar or a dense tensor
if isscalar(B) || isa(B,'tensor')
    C = full(A) - B;
    return;
end

% Case 1b: First argument is a scalar or a dense tensor
if isscalar(A) || isa(A,'tensor')
    C = A - full(B);
    return;
end

%% Case 2: Both are sparse tensors
if ~isa(A,'sptensor') || ~isa(B,'sptensor') || ~isequal(size(A),size(B))
    error('Must be two sparse tensors of the same size');
end

C = sptensor([A.subs; B.subs], [A.vals; -B.vals], size(A));


%go_calib_optim_iter
%
%Main calibration function. Computes the intrinsic andextrinsic parameters.
%Runs as a script.
%
%INPUT: x_1,x_2,x_3,...: Feature locations on the images
%       X_1,X_2,X_3,...: Corresponding grid coordinates
%
%OUTPUT: fc: Camera focal length
%        cc: Principal point coordinates
%        alpha_c: Skew coefficient
%        kc: Distortion coefficients
%        KK: The camera matrix (containing fc and cc)
%        omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space
%        Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space
%        Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors
%
%Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic
%        camera parameters, and the extrinsic parameters (3D locations of the grids in space)
%
%Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through
%      the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses.
%
%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the
%      corresponding entry in the active_images vector to zero.
%
%VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m
%that is so far implemented to work only with 2D rigs.
%In the future, a more general function will be there.
%For now, if using a 3D calibration rig, quick_init is set to 1 for an easy initialization of the focal length

if ~exist('desactivated_images'),
    desactivated_images = [];
end;

if ~exist('est_aspect_ratio'),
    est_aspect_ratio = 1;
end;

if ~exist('est_fc');
    est_fc = [1;1]; % Set to zero if you do not want to estimate the focal length (it may be useful! believe it or not!)
end;

if ~exist('recompute_extrinsic'),
    recompute_extrinsic = 1; % Set this variable to 0 in case you do not want to recompute the extrinsic parameters
    % at each iterstion.
end;

if ~exist('MaxIter'),
    MaxIter = 30; % Maximum number of iterations in the gradient descent
end;

if ~exist('check_cond'),
    check_cond = 1; % Set this variable to 0 in case you don't want to extract view dynamically
end;

if ~exist('center_optim'),
    center_optim = 1; %%% Set this variable to 0 if your do not want to estimate the principal point
end;

if exist('est_dist'),
    if length(est_dist) == 4,
        est_dist = [est_dist ; 0];
    end;
end;

if ~exist('est_dist'),
    est_dist = [1;1;1;1;0];
end;

if ~exist('est_alpha'),
    est_alpha = 0; % by default, do not estimate skew
end;

% Little fix in case of stupid values in the binary variables:
center_optim = double(~~center_optim);
est_alpha = double(~~est_alpha);
est_dist = double(~~est_dist);
est_fc = double(~~est_fc);
est_aspect_ratio = double(~~est_aspect_ratio);

fprintf(1,'\n');

if ~exist('nx')&~exist('ny'),
    fprintf(1,'WARNING: No image size (nx,ny) available. Setting nx=640 and ny=480. If these are not the right values, change values manually.\n');
    nx = 640;
    ny = 480;
end;

check_active_images;

quick_init = 0; % Set to 1 for using a quick init (necessary when using 3D rigs)

% Check 3D-ness of the calibration rig:
rig3D = 0;
for kk = ind_active,
    eval(['X_kk = X_' num2str(kk) ';']);
    if is3D(X_kk),
        rig3D = 1;
    end;
end;

if center_optim & (length(ind_active) < 2) & ~rig3D,
    fprintf(1,'WARNING: Principal point rejected from the optimization when using one image and planar rig (center_optim = 1).\n');
    center_optim = 0; %%% when using a single image, please, no principal point estimation!!!
    est_alpha = 0;
end;

if ~exist('dont_ask'),
    dont_ask = 0;
end;

if center_optim & (length(ind_active) < 5) & ~rig3D,
    fprintf(1,'WARNING: The principal point estimation may be unreliable (using less than 5 images for calibration).\n');
    %if ~dont_ask,
    %   quest = input('Are you sure you want to keep the principal point in the optimization process? ([]=yes, other=no) ');
    %   center_optim = isempty(quest);
    %end;
end;

% A quick fix for solving conflict
if ~isequal(est_fc,[1;1]),
    est_aspect_ratio=1;
end;
if ~est_aspect_ratio,
    est_fc=[1;1];
end;

if ~est_aspect_ratio,
    fprintf(1,'Aspect ratio not optimized (est_aspect_ratio = 0) -> fc(1)=fc(2). Set est_aspect_ratio to 1 for estimating aspect ratio.\n');
else
    if isequal(est_fc,[1;1]),
        fprintf(1,'Aspect ratio optimized (est_aspect_ratio = 1) -> both components of fc are estimated (DEFAULT).\n');
    end;
end;

if ~isequal(est_fc,[1;1]),
    if isequal(est_fc,[1;0]),
        fprintf(1,'The first component of focal (fc(1)) is estimated, but not the second one (est_fc=[1;0])\n');
    else
        if isequal(est_fc,[0;1]),
            fprintf(1,'The second component of focal (fc(1)) is estimated, but not the first one (est_fc=[0;1])\n');
        else
            fprintf(1,'The focal vector fc is not optimized (est_fc=[0;0])\n');
        end;
    end;
end;

if ~center_optim, % In the case where the principal point is not estimated, keep it at the center of the image
    fprintf(1,'Principal point not optimized (center_optim=0). ');
    if ~exist('cc'),
        fprintf(1,'It is kept at the center of the image.\n');
        cc = [(nx-1)/2;(ny-1)/2];
    else
        fprintf(1,'Note: to set it in the middle of the image, clear variable cc, and run calibration again.\n');
    end;
else
    fprintf(1,'Principal point optimized (center_optim=1) - (DEFAULT). To reject principal point, set center_optim=0\n');
end;

if ~center_optim & (est_alpha),
    fprintf(1,'WARNING: Since there is no principal point estimation (center_optim=0), no skew estimation (est_alpha = 0)\n');
    est_alpha = 0;
end;

if ~est_alpha,
    fprintf(1,'Skew not optimized (est_alpha=0) - (DEFAULT)\n');
    alpha_c = 0;
else
    fprintf(1,'Skew optimized (est_alpha=1). To disable skew estimation, set est_alpha=0.\n');
end;

if ~prod(double(est_dist)),
    fprintf(1,'Distortion not fully estimated (defined by the variable est_dist):\n');
    if ~est_dist(1),
        fprintf(1,'     Second order distortion not estimated (est_dist(1)=0).\n');
    end;
    if ~est_dist(2),
        fprintf(1,'     Fourth order distortion not estimated (est_dist(2)=0).\n');
    end;
    if ~est_dist(5),
        fprintf(1,'     Sixth order distortion not estimated (est_dist(5)=0) - (DEFAULT) .\n');
    end;
    if ~prod(double(est_dist(3:4))),
        fprintf(1,'     Tangential distortion not estimated (est_dist(3:4)~=[1;1]).\n');
    end;
end;

% Check 3D-ness of the calibration rig:
rig3D = 0;
for kk = ind_active,
    eval(['X_kk = X_' num2str(kk) ';']);
    if is3D(X_kk),
        rig3D = 1;
    end;
end;

% If the rig is 3D, then no choice: the only valid initialization is manual!
if rig3D,
    quick_init = 1;
end;

alpha_smooth = 0.1; % set alpha_smooth = 1; for steepest gradient descent

% Conditioning threshold for view rejection
thresh_cond = 1e6;

% Initialization of the intrinsic parameters (if necessary)

if ~exist('cc'),
    fprintf(1,'Initialization of the principal point at the center of the image.\n');
    cc = [(nx-1)/2;(ny-1)/2];
    alpha_smooth = 0.1; % slow convergence
end;

if exist('kc'),
    if length(kc) == 4;
        fprintf(1,'Adding a new distortion coefficient to kc -> radial distortion model up to the 6th degree');
        kc = [kc;0];
    end;
end;

if ~exist('alpha_c'),
    fprintf(1,'Initialization of the image skew to zero.\n');
    alpha_c = 0;
    alpha_smooth = 0.1; % slow convergence
end;

if ~exist('fc') && quick_init,
    FOV_angle = 35; % Initial camera field of view in degrees
    fprintf(1,['Initialization of the focal length to a FOV of ' num2str(FOV_angle) ' degrees.\n']);
    fc = (nx/2)/tan(pi*FOV_angle/360) * ones(2,1);
    est_fc = [1;1];
    alpha_smooth = 0.1; % slow
end;

if ~exist('fc'),
    % Initialization of the intrinsic parameters:
    fprintf(1,'Initialization of the intrinsic parameters using the vanishing points of planar patterns.\n')
    init_intrinsic_param; % The right way to go (if quick_init is not active)!
    alpha_smooth = 0.1; % slow convergence
    est_fc = [1;1];
end;

if ~exist('kc'),
    fprintf(1,'Initialization of the image distortion to zero.\n');
    kc = zeros(5,1);
    alpha_smooth = 0.1; % slow convergence
end;

if ~est_aspect_ratio,
    fc(1) = (fc(1)+fc(2))/2;
    fc(2) = fc(1);
end;

if ~prod(double(est_dist)),
    % If no distortion estimated, set to zero the variables that are not estimated
    kc = kc .* est_dist;
end;

if ~prod(double(est_fc)),
    fprintf(1,'Warning: The focal length is not fully estimated (est_fc ~= [1;1])\n');
end;

%%% Initialization of the extrinsic parameters for global minimization:
comp_ext_calib;

%%% Initialization of the global parameter vector:

init_param = [fc;cc;alpha_c;kc;zeros(5,1)];

for kk = 1:n_ima,
    eval(['omckk = omc_' num2str(kk) ';']);
    eval(['Tckk = Tc_' num2str(kk) ';']);
    init_param = [init_param; omckk ; Tckk];
end;

%-------------------- Main Optimization:

fprintf(1,'\nMain calibration optimization procedure - Number of images: %d\n',length(ind_active));

param = init_param;
change = 1;

iter = 0;

fprintf(1,'Gradient descent iterations: ');

param_list = param;

while (change > 1e-9) && (iter < MaxIter),

    fprintf(1,'%d...',iter+1);

    % To speed up: pre-allocate the memory for the Jacobian JJ3.
    % For that, need to compute the total number of points.

    %% The first step consists of updating the whole vector of knowns (intrinsic + extrinsic of active
    %% images) through a one step steepest gradient descent.

    f = param(1:2);
    c = param(3:4);
    alpha = param(5);
    k = param(6:10);

    % Compute the size of the Jacobian matrix:
    N_points_views_active = N_points_views(ind_active);

    JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225);
    ex3 = zeros(15 + 6*n_ima,1);

    for kk = ind_active, %1:n_ima,
        %if active_images(kk),

        omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3);

        Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6);

        if isnan(omckk(1)),
            fprintf(1,'Intrinsic parameters at frame %d do not exist\n',kk);
            return;
        end;

        eval(['X_kk = X_' num2str(kk) ';']);
        eval(['x_kk = x_' num2str(kk) ';']);

        Np = N_points_views(kk);

        if ~est_aspect_ratio,
            [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,f(1),c,k,alpha);
            dxdf = repmat(dxdf,[1 2]);
        else
            [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,f,c,k,alpha);
        end;

        exkk = x_kk - x;

        A = [dxdf dxdc dxdalpha dxdk]';
        B = [dxdom dxdT]';

        JJ3(1:10,1:10) = JJ3(1:10,1:10) + sparse(A*A');
        JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = sparse(B*B');

        AB = sparse(A*B');
        JJ3(1:10,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB;
        JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:10) = (AB)';

        ex3(1:10) = ex3(1:10) + A*exkk(:);
        ex3(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = B*exkk(:);

        % Check if this view is ill-conditioned:
        if check_cond,
            JJ_kk = B'; %[dxdom dxdT];
            if (cond(JJ_kk)> thresh_cond),
                active_images(kk) = 0;
                fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive. (note: to disactivate this option, set check_cond=0)\n',kk)
                desactivated_images = [desactivated_images kk];
                param(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = NaN*ones(6,1);
            end;
        end;

        %end;

    end;

    % List of active images (necessary if changed):
    check_active_images;

    % The following vector helps to select the variables to update (for only active images):
    selected_variables = [est_fc;center_optim*ones(2,1);est_alpha;est_dist;zeros(5,1);reshape(ones(6,1)*active_images,6*n_ima,1)];
    if ~est_aspect_ratio,
        if isequal(est_fc,[1;1]) | isequal(est_fc,[1;0]),
            selected_variables(2) = 0;
        end;
    end;
    ind_Jac = find(selected_variables)';

    JJ3 = JJ3(ind_Jac,ind_Jac);
    ex3 = ex3(ind_Jac);

    JJ2_inv = inv(JJ3); % not bad for sparse matrices!!

    % Smoothing coefficient:

    alpha_smooth2 = 1-(1-alpha_smooth)^(iter+1); %set to 1 to undo any smoothing!

    param_innov = alpha_smooth2*JJ2_inv*ex3;

    param_up = param(ind_Jac) + param_innov;
    param(ind_Jac) = param_up;

    % New intrinsic parameters:

    fc_current = param(1:2);
    cc_current = param(3:4);

    if center_optim & ((param(3)<0)|(param(3)>nx)|(param(4)<0)|(param(4)>ny)),
        fprintf(1,'Warning: it appears that the principal point cannot be estimated. Setting center_optim = 0\n');
        center_optim = 0;
        cc_current = c;
    else
        cc_current = param(3:4);
    end;

    alpha_current = param(5);
    kc_current = param(6:10);

    if ~est_aspect_ratio & isequal(est_fc,[1;1]),
        fc_current(2) = fc_current(1);
        param(2) = param(1);
    end;

    % Change on the intrinsic parameters:
    change = norm([fc_current;cc_current] - [f;c])/norm([fc_current;cc_current]);

    %% Second step: (optional) - It makes convergence faster, and the region of convergence LARGER!!!
    %% Recompute the extrinsic parameters only using compute_extrinsic.m (this may be useful sometimes)
    %% The complete gradient descent method is useful to precisely update the intrinsic parameters.

    if recompute_extrinsic,
        MaxIter2 = 20;
        for kk =ind_active, %1:n_ima,
            %if active_images(kk),
            omc_current = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3);
            Tc_current = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6);
            eval(['X_kk = X_' num2str(kk) ';']);
            eval(['x_kk = x_' num2str(kk) ';']);
            [omc_current,Tc_current] = compute_extrinsic_init(x_kk,X_kk,fc_current,cc_current,kc_current,alpha_current);
            [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine(omc_current,Tc_current,x_kk,X_kk,fc_current,cc_current,kc_current,alpha_current,MaxIter2,thresh_cond);
            if check_cond,
                if (cond(JJ_kk)> thresh_cond),
                    active_images(kk) = 0;
                    fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive. (note: to disactivate this option, set check_cond=0)\n',kk);
                    desactivated_images = [desactivated_images kk];
                    omckk = NaN*ones(3,1);
                    Tckk = NaN*ones(3,1);
                end;
            end;
            param(15+6*(kk-1) + 1:15+6*(kk-1) + 3) = omckk;
            param(15+6*(kk-1) + 4:15+6*(kk-1) + 6) = Tckk;
            %end;
        end;
    end;

    param_list = [param_list param];
    iter = iter + 1;

end;

fprintf(1,'done\n');

%%%--------------------------- Computation of the error of estimation:

fprintf(1,'Estimation of uncertainties...');

check_active_images;

solution = param;

% Extraction of the paramters for computing the right reprojection error:

fc = solution(1:2);
cc = solution(3:4);
alpha_c = solution(5);
kc = solution(6:10);

for kk = 1:n_ima,

    if active_images(kk),

        omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%***
        Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%***
        Rckk = rodrigues(omckk);

    else

        omckk = NaN*ones(3,1);
        Tckk = NaN*ones(3,1);
        Rckk = NaN*ones(3,3);

    end;

    eval(['omc_' num2str(kk) ' = omckk;']);
    eval(['Rc_' num2str(kk) ' = Rckk;']);
    eval(['Tc_' num2str(kk) ' = Tckk;']);

end;

% Recompute the error (in the vector ex):
comp_error_calib;

sigma_x = std(ex(:));

% Compute the size of the Jacobian matrix:
N_points_views_active = N_points_views(ind_active);

JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225);

for kk = ind_active,

    omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3);
    Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6);

    eval(['X_kk = X_' num2str(kk) ';']);

    Np = N_points_views(kk);

    %[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c);

    if ~est_aspect_ratio,
        [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,fc(1),cc,kc,alpha_c);
        dxdf = repmat(dxdf,[1 2]);
    else
        [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c);
    end;

    A = [dxdf dxdc dxdalpha dxdk]';
    B = [dxdom dxdT]';

    JJ3(1:10,1:10) = JJ3(1:10,1:10) + sparse(A*A');
    JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = sparse(B*B');

    AB = sparse(A*B');
    JJ3(1:10,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB;
    JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:10) = (AB)';

end;

JJ3 = JJ3(ind_Jac,ind_Jac);

JJ2_inv = inv(JJ3); % not bad for sparse matrices!!

param_error = zeros(6*n_ima+15,1);
param_error(ind_Jac) =  3*sqrt(full(diag(JJ2_inv)))*sigma_x;

solution_error = param_error;

if ~est_aspect_ratio && isequal(est_fc,[1;1]),
    solution_error(2) = solution_error(1);
end;

%%% Extraction of the final intrinsic and extrinsic paramaters:

extract_parameters;

fprintf(1,'done\n');

fprintf(1,'\n\nCalibration results after optimization (with uncertainties):\n\n');
fprintf(1,'Focal Length:          fc = [ %3.5f   %3.5f ] +/- [ %3.5f   %3.5f ]\n',[fc;fc_error]);
fprintf(1,'Principal point:       cc = [ %3.5f   %3.5f ] +/- [ %3.5f   %3.5f ]\n',[cc;cc_error]);
fprintf(1,'Skew:             alpha_c = [ %3.5f ] +/- [ %3.5f  ]   => angle of pixel axes = %3.5f +/- %3.5f degrees\n',[alpha_c;alpha_c_error],90 - atan(alpha_c)*180/pi,atan(alpha_c_error)*180/pi);
fprintf(1,'Distortion:            kc = [ %3.5f   %3.5f   %3.5f   %3.5f  %5.5f ] +/- [ %3.5f   %3.5f   %3.5f   %3.5f  %5.5f ]\n',[kc;kc_error]);
fprintf(1,'Pixel error:          err = [ %3.5f   %3.5f ]\n\n',err_std);
fprintf(1,'Note: The numerical errors are approximately three times the standard deviations (for reference).\n\n\n')
%fprintf(1,'      For accurate (and stable) error estimates, it is recommended to run Calibration once again.\n\n\n')

%%% Some recommendations to the user to reject some of the difficult unkowns... Still in debug mode.

alpha_c_min = alpha_c - alpha_c_error/2;
alpha_c_max = alpha_c + alpha_c_error/2;

if (alpha_c_min < 0) && (alpha_c_max > 0),
    fprintf(1,'Recommendation: The skew coefficient alpha_c is found to be equal to zero (within its uncertainty).\n');
    fprintf(1,'                You may want to reject it from the optimization by setting est_alpha=0 and run Calibration\n\n');
end;

kc_min = kc - kc_error/2;
kc_max = kc + kc_error/2;

prob_kc = (kc_min < 0) & (kc_max > 0);

if ~(prob_kc(3) && prob_kc(4))
    prob_kc(3:4) = [0;0];
end;

if sum(prob_kc),
    fprintf(1,'Recommendation: Some distortion coefficients are found equal to zero (within their uncertainties).\n');
    fprintf(1,'                To reject them from the optimization set est_dist=[%d;%d;%d;%d;%d] and run Calibration\n\n',est_dist & ~prob_kc);
end;

return;


function [traj, infStates] = tapas_hgf_ar1_mab(r, p, varargin)
% Calculates the trajectories of the agent's representations under the AR(1)-HGF in a multi-armed
% bandit task
%
% This function can be called in two ways:
%
% (1) tapas_hgf_ar1_mab(r, p)
%
%     where r is the structure generated by tapas_fitModel and p is the parameter vector in native space;
%
% (2) tapas_hgf_ar1_mab(r, ptrans, 'trans')
%
%     where r is the structure generated by tapas_fitModel, ptrans is the parameter vector in
%     transformed space, and 'trans' is a flag indicating this.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Transform paramaters back to their native space if needed
if ~isempty(varargin) && strcmp(varargin{1},'trans');
    p = tapas_hgf_ar1_mab_transp(r, p);
end

% Number of levels
try
    l = r.c_prc.n_levels;
catch
    l = length(p)/6;

    if l ~= floor(l)
        error('tapas:hgf:UndetNumLevels', 'Cannot determine number of levels');
    end
end

% Number of bandits
try
    b = r.c_prc.n_bandits;
catch
    error('tapas:hgf:NumOfBanditsConfig', 'Number of bandits has to be configured in r.c_prc.n_bandits.');
end

% Unpack parameters
mu_0 = p(1:l);
sa_0 = p(l+1:2*l);
phi  = p(2*l+1:3*l);
m    = p(3*l+1:4*l);
ka   = p(4*l+1:5*l-1);
om   = p(5*l:6*l-2);
th   = exp(p(6*l-1));
al   = p(6*l);

% Add dummy "zeroth" trial
u = [0; r.u(:,1)];
y = [0; r.u(:,2)];

% Number of trials (including prior)
n = size(u,1);

% Construct time axis
if r.c_prc.irregular_intervals
    if size(u,2) > 1
        t = [0; r.u(:,end)];
    else
        error('tapas:hgf:InputSingleColumn', 'Input matrix must contain more than one column if irregular_intervals is set to true.');
    end
else
    t = ones(n,1);
end

% Initialize updated quantities

% Representations
mu = NaN(n,l,b);
pi = NaN(n,l,b);

% Other quantities
muhat = NaN(n,l,b);
pihat = NaN(n,l,b);
v     = NaN(n,l);
w     = NaN(n,l-1);
da    = NaN(n,l);
dau   = NaN(n,1);

% Representation priors
% Note: first entries of the other quantities remain
% NaN because they are undefined and are thrown away
% at the end; their presence simply leads to consistent
% trial indices.
mu(1,:,:) = repmat(mu_0,[1 1 b]);
pi(1,:,:) = repmat(1./sa_0,[1 1 b]);

% Representation update loop
% Pass through trials
for k = 2:1:n
    if not(ismember(k-1, r.ign))

        %%%%%%%%%%%%%%%%%%%%%%
        % Effect of input u(k)
        %%%%%%%%%%%%%%%%%%%%%%

        % 1st level
        % ~~~~~~~~~
        % Prediction
        muhat(k,1,:) = mu(k-1,1,:) +t(k) *phi(1) *(m(1) -mu(k-1,1,:));

        % Precision of prediction
        pihat(k,1,:) = 1/(1/pi(k-1,1,:) +t(k) *exp(ka(1) *mu(k-1,2,:) +om(1)));

        % Input prediction error
        dau(k) = u(k) -muhat(k,1,y(k));

        % Updates
        pi(k,1,:) = pihat(k,1,:);
        pi(k,1,y(k)) = pi(k,1,y(k)) +1/al;

        mu(k,1,:) = muhat(k,1,:);
        mu(k,1,y(k)) = mu(k,1,y(k)) +1/pihat(k,1,y(k)) *1/(1/pihat(k,1,y(k)) +al) *dau(k);

        % Volatility prediction error
        da(k,1) = (1/pi(k,1,y(k)) +(mu(k,1,y(k)) -muhat(k,1,y(k)))^2) *pihat(k,1,y(k)) -1;

        if l > 2
            % Pass through higher levels
            % ~~~~~~~~~~~~~~~~~~~~~~~~~~
            for j = 2:l-1
                % Prediction
                muhat(k,j,:) = mu(k-1,j,:) +t(k) *phi(j) *(m(j) -mu(k-1,j,:));

                % Precision of prediction
                pihat(k,j,:) = 1/(1/pi(k-1,j,:) +t(k) *exp(ka(j) *mu(k-1,j+1,:) +om(j)));

                % Weighting factor
                v(k,j-1) = t(k) *exp(ka(j-1) *mu(k-1,j,y(k)) +om(j-1));
                w(k,j-1) = v(k,j-1) *pihat(k,j-1,y(k));

                % Updates
                pi(k,j,:) = pihat(k,j,:) +1/2 *ka(j-1)^2 *w(k,j-1) *(w(k,j-1) +(2 *w(k,j-1) -1) *da(k,j-1));

                if pi(k,j,1) <= 0
                    error('tapas:hgf:NegPostPrec', 'Negative posterior precision. Parameters are in a region where model assumptions are violated.');
                end

                mu(k,j,:) = muhat(k,j,:) +1/2 *1/pi(k,j,:) *ka(j-1) *w(k,j-1) *da(k,j-1);

                % Volatility prediction error
                da(k,j) = (1/pi(k,j,y(k)) +(mu(k,j,y(k)) -muhat(k,j,y(k)))^2) *pihat(k,j,y(k)) -1;
            end
        end

        % Last level
        % ~~~~~~~~~~
        % Prediction
        muhat(k,l,:) = mu(k-1,l,:) +t(k) *phi(l) *(m(l) -mu(k-1,l,:));

        % Precision of prediction
        pihat(k,l,:) = 1/(1/pi(k-1,l,:) +t(k) *th);

        % Weighting factor
        v(k,l)   = t(k) *th;
        v(k,l-1) = t(k) *exp(ka(l-1) *mu(k-1,l,y(k)) +om(l-1));
        w(k,l-1) = v(k,l-1) *pihat(k,l-1,y(k));

        % Updates
        pi(k,l,:) = pihat(k,l,:) +1/2 *ka(l-1)^2 *w(k,l-1) *(w(k,l-1) +(2 *w(k,l-1) -1) *da(k,l-1));

        if pi(k,l,1) <= 0
            error('tapas:hgf:NegPostPrec', 'Negative posterior precision. Parameters are in a region where model assumptions are violated.');
        end

        mu(k,l,:) = muhat(k,l,:) +1/2 *1/pi(k,l,:) *ka(l-1) *w(k,l-1) *da(k,l-1);

        % Volatility prediction error
        da(k,l) = (1/pi(k,l,y(k)) +(mu(k,l,y(k)) -muhat(k,l,y(k)))^2) *pihat(k,l,y(k)) -1;
    else

        mu(k,:,:) = mu(k-1,:,:);
        pi(k,:,:) = pi(k-1,:,:);

        muhat(k,:,:) = muhat(k-1,:,:);
        pihat(k,:,:) = pihat(k-1,:,:);

        v(k,:)  = v(k-1,:);
        w(k,:)  = w(k-1,:);
        da(k,:) = da(k-1,:);

    end
end

% Remove representation priors
mu(1,:,:)  = [];
pi(1,:,:)  = [];

% Check validity of trajectories
if any(isnan(mu(:))) || any(isnan(pi(:)))
    error('tapas:hgf:VarApproxInvalid', 'Variational approximation invalid. Parameters are in a region where model assumptions are violated.');
else
    % Check for implausible jumps in trajectories
    dmu = diff(mu);
    dpi = diff(pi);
    rmdmu = repmat(sqrt(mean(dmu.^2)),length(dmu),1);
    rmdpi = repmat(sqrt(mean(dpi.^2)),length(dpi),1);

    jumpTol = 256;
    if any(abs(dmu(:)) > jumpTol*rmdmu(:)) || any(abs(dpi(:)) > jumpTol*rmdpi(:))
        error('tapas:hgf:VarApproxInvalid', 'Variational approximation invalid. Parameters are in a region where model assumptions are violated.');
    end
end

% Remove other dummy initial values
muhat(1,:,:) = [];
pihat(1,:,:) = [];
v(1,:)       = [];
w(1,:)       = [];
da(1,:)      = [];
dau(1)       = [];

% Create result data structure
traj = struct;

traj.mu     = mu;
traj.sa     = 1./pi;

traj.muhat  = muhat;
traj.sahat  = 1./pihat;

traj.v      = v;
traj.w      = w;
traj.da     = da;
traj.dau    = dau;

% Updates with respect to prediction
traj.ud = mu -muhat;

% Psi (precision weights on prediction errors)
psi      = NaN(n-1,l);
pi1      = squeeze(pi(:,1,:));
pi1obs   = pi1(sub2ind(size(pi1), (1:size(pi1,1))', y));
psi(:,1) = 1./(al*pi1obs);
for i=2:l
    pihati    = squeeze(pihat(:,i-1,:));
    pihatiobs = pihati(sub2ind(size(pihati), (1:size(pihati,1))', y));
    pii       = squeeze(pi(:,i,:));
    piiobs    = pii(sub2ind(size(pii), (1:size(pii,1))', y));
    psi(:,i)  = pihatiobs./piiobs;
end
traj.psi = psi;

% Epsilons (precision-weighted prediction errors)
epsi        = NaN(n-1,l);
epsi(:,1)   = psi(:,1) .*dau;
epsi(:,2:l) = psi(:,2:l) .*da(:,1:l-1);
traj.epsi   = epsi;

% Full learning rate (full weights on prediction errors)
wt        = NaN(n-1,l);
wt(:,1)   = psi(:,1);
wt(:,2:l) = 1/2 *(v(:,1:l-1) *diag(ka(1:l-1))) .*psi(:,2:l);
traj.wt   = wt;

% Create matrices for use by the observation model
infStates = NaN(n-1,l,b,4);
infStates(:,:,:,1) = traj.muhat;
infStates(:,:,:,2) = traj.sahat;
infStates(:,:,:,3) = traj.mu;
infStates(:,:,:,4) = traj.sa;

return;


function [MDP] = spm_MDP_game_KL(MDP,varargin)
% action selection using active inference (KL formulation)
% FORMAT [MDP] = spm_MDP_game_KL(MDP,[EU])
%
% EU              - optional flag to invoke expected utility only
%
% MDP.T           - process depth (the horizon)
% MDP.N           - number of variational iterations (default 4)
% MDP.S(N,1)      - true initial state
%
% MDP.A(O,N)      - Likelihood of O outcomes given N hidden states
% MDP.B{M}(N,N)   - transition probabilities among hidden states (priors)
% MDP.C(N,1)      - terminal cost probabilities (prior over hidden states)
% MDP.D(N,1)      - initial prior probabilities (prior over hidden states)
%
% MDP.V(T,P)      - P allowable policies (control sequences over T times)
%
% optional:
% MDP.s(1 x T)    - vector of true states  - for deterministic solutions
% MDP.o(1 x T)    - vector of observations - for deterministic solutions
% MDP.a(1 x T)    - vector of action       - for deterministic solutions
% MDP.w(1 x T)    - vector of precisions   - for deterministic solutions
%
% MDP.B{T,M}(N,N) - model transition probabilities for each time point
% MDP.G{T,M}(N,N) - true  transition probabilities for each time point
%                   (default: MDP.G{T,M} = MDP.G{M} = MDP.B{M})
%
% MDP.plot        - switch to suppress graphics: (default: [0])
% MDP.alpha       - upper bound on precision (Gamma hyperprior - shape [8])
% MDP.beta        - precision over precision (Gamma hyperprior - rate  [1])
%
% produces:
%
% MDP.P(M,T)   - probability of emitting an action 1,...,M at time 1,...,T
% MDP.Q(N,T)   - an array of conditional (posterior) expectations over
%                N hidden states and time 1,...,T
% MDP.O(O,T)   - a sparse matrix of ones encoding outcomes at time 1,...,T
% MDP.S(N,T)   - a sparse matrix of ones encoding states at time 1,...,T
% MDP.U(M,T)   - a sparse matrix of ones encoding action at time 1,...,T
% MDP.W(1,T)   - posterior expectations of precision
% MDP.d        - simulated dopamine responses
%
% This routine provides solutions of active inference (minimisation of
% variational free energy) using a generative model based upon a Markov
% decision process. This model and inference scheme is formulated
% in discrete space and time. This means that the generative model (and
% process) are  finite state  machines or hidden Markov models whose
% dynamics are given by transition probabilities among states and the
% likelihood corresponds to a particular outcome conditioned upon
% hidden states. For simplicity, this routine assumes that action
% and hidden controls are isomorphic. If the dynamics of transition
% probabilities of the true process are not provided, this routine will use
% the equivalent probabilities from the generative model.
%
% This particular scheme is designed for any allowable policies or control
% sequences specified in MDP.V. Constraints on allowable policies can limit
% the numerics or combinatorics considerable. For example, situations in
% which one action can be selected at one time can be reduced to T polices
% - with one (shift) control being emitted at all possible time points.
% This specification of polices simplifies the generative model, allowing a
% fairly exhaustive model of potential outcomes - eschewing a mean field
% approximation over successive control states. In brief, the agent simply
% represents the current state and states in the immediate and distant
% future.
%
% The transition probabilities are a cell array of probability transition
% matrices corresponding to each (discrete) the level of the control state.
%
% Mote that the conditional expectations are functions of time but also
% contain expectations about fictive states over time at each time point.
% To create time dependent transition probabilities, one can specify a
% function in place of the transition probabilities under different levels
% of control.
%
% Partially observed Markov decision processes can be modelled by
% specifying a likelihood (as part of a generative model) and absorbing any
% probabilistic mapping between (isomorphic) hidden states and outcomes
% into the transition probabilities G.
%
% See also: spm_MDP, which uses multiple future states and a mean field
% approximation for control states - but allows for different actions
% at all times (as in control problems).
%
% See also: spm_MDP_game, which generalises this scheme and replaces prior
% beliefs about KL control with minimisation of expected free energy.
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_MDP_game_KL.m 7679 2019-10-24 15:54:07Z spm $

% set up and preliminaries
%==========================================================================

% options and precision defaults
%--------------------------------------------------------------------------
try, PLOT  = MDP.plot;  catch, PLOT  = 0; end
try, alpha = MDP.alpha; catch, alpha = 8; end
try, beta  = MDP.beta;  catch, beta  = 1; end
try, N     = MDP.N;     catch, N     = 4; end

% set up figure if necessary
%--------------------------------------------------------------------------
if PLOT
    if ishandle(PLOT)
        figure(PLOT); clf
        PLOT = 2;
    else
        spm_figure('GetWin','MDP'); clf
    end
end

% generative model and initial states
%--------------------------------------------------------------------------
T     = MDP.T;                     % process depth (the horizon)
Ns    = size(MDP.B{1},1);          % number of hidden states
Nb    = size(MDP.B,1);             % number of time-dependent probabilities
Nu    = size(MDP.B,2);             % number of hidden controls
p0    = eps;                       % smallest probability

% likelihood model (for a partially observed MDP implicit in G)
%--------------------------------------------------------------------------
try
    A  = MDP.A + p0;
    No = size(MDP.A,1);           % number of outcomes
catch
    A  = speye(Ns,Ns) + p0;
    No = Ns;
end
A     = A*diag(1./sum(A));
lnA   = log(A);

% transition probabilities (priors)
%--------------------------------------------------------------------------
for i = 1:T
    for j = 1:Nu
        if i == 1 || Nb == T
            B{i,j}   = MDP.B{i,j} + p0;
            B{i,j}   = B{i,j}*diag(1./sum(B{i,j}));
        else
            B{i,j}   = B{1,j};
        end
    end
end

% terminal probabilities (priors)
%--------------------------------------------------------------------------
try
    C = spm_vec(MDP.C) + p0;
catch
    C = ones(Ns,1);
end
C     = C/sum(C);
lnC   = log(C);

% intital probabilities (priors)
%--------------------------------------------------------------------------
try
    D = spm_vec(MDP.D) + p0;
catch
    D = ones(Ns,1);
end
D     = D/sum(D);
lnD   = log(D);

% generative process (assume the true process is the same as the model)
%--------------------------------------------------------------------------
try
    G = MDP.G;
catch
    G = MDP.B;
end
Ng    = size(G,1);
for i = 1:T
    for j = 1:Nu
        if i == 1 || Ng == T
            G{i,j} = G{i,j} + p0;
            G{i,j} = G{i,j}*diag(1./sum(G{i,j}));
        else
            G{i,j} = G{1,j};
        end
    end
end

% policies and their expectations
%--------------------------------------------------------------------------
V      = MDP.V;
u      = zeros(size(V,2),1);
Np     = size(V,2);                % number of allowable policies
w      = 1:Np;                     % indices of allowable policies

% initial states and outcomes
%--------------------------------------------------------------------------
[p q]  = max(A*MDP.S(:,1));        % initial outcome (index)
s      = find( MDP.S(:,1));        % initial state   (index)
o      = sparse(1,1,q,1,T);        % observations    (index)
S      = sparse(s,1,1,Ns,T);       % states sampled  (1 in K vector)
O      = sparse(q,1,1,No,T);       % states observed (1 in K vector)
a      = sparse(1, T);             % action (index)
U      = sparse(Nu,T);             % action selected (1 in K vector)
P      = sparse(Nu,T);             % posterior beliefs about control
E      = sparse(T,Np);             % posterior beliefs about policies
W      = sparse(1,T);              % posterior precision

% sufficient statistics of hidden states (past, current and last)
%--------------------------------------------------------------------------
gamma  = [];                       % simulated dopamine responses
x      = zeros(Ns,T);

% solve
%==========================================================================
for t  = 1:T

    % allowable policies
    %----------------------------------------------------------------------
    if t > 1

        % record posterior expectations over policies
        %------------------------------------------------------------------
        E(t - 1,w) = u;

        % retain allowable policies (that are consistent with last action)
        %------------------------------------------------------------------
        j = ismember(V(t - 1,:),a(t - 1));
        V = V(:,j);
        u = u(j);
        w = w(j);

    end

    % conditional KL divergence (under allowable policies)
    %======================================================================
    Np    = size(V,2);                % number of allowable policies
    Q     = zeros(Np,Ns);             % value of policies x current state
    for k = 1:Np

        % compositon of future states
        %------------------------------------------------------------------
        Bj = 1;
        for j = t:T
            Bj = B{j,V(j,k)}*Bj;
        end

        % divergence or information gain
        %------------------------------------------------------------------
        if nargin > 1
            Q(k,:) = lnC'*Bj;
        else
            Q(k,:) = lnC'*Bj - sum(Bj.*log(Bj));
        end

    end

    % Variational iterations (assuming precise inference about past action)
    %======================================================================
    for i  = 1:N

        % present state (x)
        %------------------------------------------------------------------
        if t == 1
            v  = lnD;
        else
            v  = log(B{t - 1,a(t - 1)}*x(:,t - 1));
        end
        v      = v + lnA(o(t),:)' + W(t)*Q'*u;
        x(:,t) = spm_softmax(v);

        % precision (W)
        %------------------------------------------------------------------
        if isfield(MDP,'w')
            W(t) = MDP.w(t);
        else
            v    = beta - u'*Q*x(:,t);
            W(t) = alpha/v;
        end

        % policy (u)
        %------------------------------------------------------------------
        v      = W(t)*Q*x(:,t);
        u      = spm_softmax(v);
        E(t,w) = u;

        % re-compute precision for first iteration
        %------------------------------------------------------------------
        if t == 1
            v    = beta - u'*Q*x(:,t);
            W(t) = alpha/v;
        end

        % simulated dopamine responses (precision as each iteration)
        %------------------------------------------------------------------
        gamma(end + 1,1) = W(t);

    end

    % posterior expectations (control)
    %======================================================================
    for j = 1:Nu
        for k = t:T
            P(j,k) = sum(u(ismember(V(k,:),j)));
        end
    end

    % next action (the action that minimises expected free energy)
    %------------------------------------------------------------------
    try
        a(t) = MDP.a(t);
    catch
        try
            a(t) = find(rand < cumsum(P(:,t)),1);
        catch
            error('there are no more allowable policies')
        end
    end

    % save action
    %------------------------------------------------------------------
    U(a(t),t) = 1;

    % sampling of next state (outcome)
    %======================================================================
    if t < T

        % next sampled state
        %------------------------------------------------------------------
        try
            s(t + 1) = MDP.s(t + 1);
        catch
            s(t + 1) = find(rand < cumsum(G{t,a(t)}(:,s(t))),1);
        end

        % next obsverved state
        %------------------------------------------------------------------
        try
            o(t + 1) = MDP.o(t + 1);
        catch
            o(t + 1) = find(rand < cumsum(A(:,s(t + 1))),1);
        end

        % save outcome and state sampled
        %------------------------------------------------------------------
        W(1,t + 1)        = W(t);
        O(o(t + 1),t + 1) = 1;
        S(s(t + 1),t + 1) = 1;

    end

    % plot
    %======================================================================
    if PLOT > 0

        % posterior beliefs about hidden states
        %------------------------------------------------------------------
        subplot(4,2,1)
        imagesc(1 - [x C*max(max(x))/max(C)])
        if size(x,1) > 128
            hold on, spm_spy(x,16,1), hold off
        end
        title('Inferred states (and utility)','FontSize',14)
        xlabel('Time','FontSize',12)
        ylabel('Hidden state','FontSize',12)

        % posterior beliefs about control states
        %==================================================================
        subplot(4,2,2)

        % make previous plots dotted lines
        %------------------------------------------------------------------
        if T > 2
            h     = get(gca,'Children'); hold on
            for i = 1:length(h)
                set(h(i),'LineStyle',':');
            end
            plot(P')
            title('Inferred policy','FontSize',14)
            xlabel('Time','FontSize',12)
            ylabel('Control state','FontSize',12)
            spm_axis tight
        else
            bar(P)
            title('Inferred policy','FontSize',14)
            xlabel('Contol state','FontSize',12)
            ylabel('Posterior expectation','FontSize',12)
        end

        % policies
        %------------------------------------------------------------------
        subplot(4,2,3)
        imagesc(MDP.V')
        title('Allowable policies','FontSize',14)
        ylabel('Policy','FontSize',12)
        xlabel('Time','FontSize',12)

        % expectations over policies
        %------------------------------------------------------------------
        subplot(4,2,4)
        imagesc(E')
        title('Posterior probability','FontSize',14)
        ylabel('Policy','FontSize',12)
        xlabel('Time','FontSize',12)

        % true state (outcome)
        %------------------------------------------------------------------
        subplot(4,2,5)
        if size(S,1) > 128
            spm_spy(S,16)
        else
            imagesc(1 - S)
        end
        title('True states','FontSize',14)
        ylabel('State','FontSize',12)

        % sample (observation)
        %------------------------------------------------------------------
        subplot(4,2,7)
        if size(O,1) > 128
            spm_spy(O,16,1)
        else
            imagesc(1 - O)
        end
        title('Observed states','FontSize',14)
        xlabel('Time','FontSize',12)
        ylabel('State','FontSize',12)

        % action sampled (selected)
        %------------------------------------------------------------------
        subplot(4,2,6)
        if size(U,1) > 128
            spm_spy(U,16,1)
        else
            imagesc(1 - U)
        end
        title('Selected action','FontSize',14)
        ylabel('Action','FontSize',12)

        % expected action
        %------------------------------------------------------------------
        subplot(4,2,8)
        plot((1:length(gamma))/N,gamma)
        title('Expected precision (confidence)','FontSize',14)
        xlabel('Time','FontSize',12)
        ylabel('Precision','FontSize',12)
        spm_axis tight
        drawnow

    end

end

% deconvolve to simulate dopamine responses
%--------------------------------------------------------------------------
da     = pinv( tril(toeplitz(exp(-((1:length(gamma)) - 1)'/8))) )*gamma;

% assemble results and place in NDP structure
%--------------------------------------------------------------------------
MDP.P  = P;              % probability of action at time 1,...,T - 1
MDP.Q  = x;              % conditional expectations over N hidden states
MDP.O  = O;              % a sparse matrix, encoding outcomes at 1,...,T
MDP.S  = S;              % a sparse matrix, encoding the states
MDP.U  = U;              % a sparse matrix, encoding the action
MDP.W  = W;              % posterior expectations of precision
MDP.d  = gamma;          % simulated dopamine responses
MDP.da = da;             % simulated dopamine responses (deconvolved)


function [perimeterEdges,eulerCondition] = findLegalPerimeters2(mesh,perimDist)
%
%  [perimeterEdges,eulerCondition] = findLegalPerimeters2(mesh,perimDist)
%
% Given a mesh structure (nodes, edges, distances from start point) and a
% threshold distance this routine will return a list of edges that
% constitute separate perimeters at a distance of perimDist from the start
% point.
%
% Note that because of intrinsic curvature, there can be more than one
% perimeter at the required distance. This routine makes sure that it
% returns separate perimeters (ones with no common nodes).

% Find perims with simple thold
insideNodes=find(mesh.dist<=perimDist);
insideNodes=insideNodes(:);

insideNodes = removeHangingNodes(mesh,insideNodes); % Cleans up the mesh
[perimeterEdges,eulerCondition]=findGroupPerimeter(mesh,insideNodes); % Find perimeter(s) - there may be more than 1
badPerimNodes = findBadPerimNodes(mesh,perimeterEdges); % See if some perimeters are joined up.

numBadNodes=9999999; % Hope we don't get more than this...

while (numBadNodes>0)
    [perimeterEdges,eulerCondition]=findGroupPerimeter(mesh,insideNodes);
	length(perimeterEdges);
	length(unique(perimeterEdges,'rows'));
    fprintf('Euler number=%d\n',eulerCondition);

    badPerimNodes=findBadPerimNodes(mesh,perimeterEdges);
    numBadNodes=length(badPerimNodes);
    fprintf('There are %d bad perim nodes.\n',numBadNodes);

    if(numBadNodes)
		[insideNodes]=correctBadNodes(mesh,insideNodes,badPerimNodes); % Splits up joined perimeters
		insideNodes=removeHangingNodes(mesh,insideNodes); % Cleans up mesh again
    end
end

return;


function [M q]=modularity_probtune_und_sign(W,qtype,M,p)
%MODULARITY_PROBTUNE_UND_SIGN     Optimal community structure, modularity and degeneracy
%
%   Ci     = modularity_probtune_und_sign(W,   [],Ci0,0.01);
%   Ci     = modularity_probtune_und_sign(W,'sta',Ci0,0.01);
%   [Ci Q] = modularity_probtune_und_sign(W,'sta',Ci0,0.01);
%
%   The optimal community structure is a subdivision of the network into
%   nonoverlapping groups of nodes in a way that maximizes the number of
%   within-group edges, and minimizes the number of between-group edges.
%   The modularity is a statistic that quantifies the degree to which the
%   network may be subdivided into such clearly delineated groups.
%   High-modularity degeneracy is the presence of many topologically
%   distinct high-modularity partitions of the network.
%
%   This algorithm is inspired by the Kernighan-Lin fine-tuning algorithm
%   and is designed to probabilistically refine a previously detected
%   community by incorporating random node moves into a finetuning
%   algorithm.
%
%   Input:      W,      undirected (weighted or binary) connection matrix
%                       with positive and negative weights
%
%               qtype,  modularity type (see Rubinov and Sporns, 2011)
%                           'sta',  Q_* (default if qtype is not specified)
%                           'pos',  Q_+
%                           'smp',  Q_simple
%                           'gja',  Q_GJA
%                           'neg',  Q_-
%
%               Ci0,    initial community affiliation vector (optional)
%
%               p,      probability of random node moves
%
%
%   Output:     Ci,     refined community affiliation vector
%               Q,      modularity (qtype dependent)
%
%   Note: Ci and Q may vary from run to run, due to heuristics in the
%   algorithm. Consequently, it may be worth to compare multiple runs.
%
%   References:
%   Rubinov and Sporns (2011) NeuroImage.
%   Sun et al. (2008)  Europhysics Lett 86, 28004.
%
%
%   Mika Rubinov, UNSW, 2011

%   Modification History:
%   Mar 2011: Original

n=length(W);                                                %number of nodes/modules
if ~exist('qtype','var') || isempty(qtype);
    qtype = 'sta';
end
if isempty(M);
    M = 1:n;
else
    [dum dum M] = unique(M(:).');                           %align module indices
end

W0= W.*(W>0);                                               %positive weights matrix
W1=-W.*(W<0);                                               %negative weights matrix
s0=sum(W0(:));                                              %positive sum of weights
s1=sum(W1(:));                                              %negative sum of weights
Knm0=zeros(n,n);                                            %positive node-to-module degree
Knm1=zeros(n,n);                                            %negative node-to-module degree
for m=1:max(M)                                              %loop over modules
    Knm0(:,m)=sum(W0(:,M==m),2);
    Knm1(:,m)=sum(W1(:,M==m),2);
end
Kn0=sum(Knm0,2);                                            %positive node degree
Kn1=sum(Knm1,2);                                            %negative node degree
Km0=sum(Knm0,1);                                            %positive module degree
Km1=sum(Knm1,1);                                            %negative module degree

switch qtype
    case 'smp';  d0 = 1/s0;       d1 = 1/s1;                %dQ = dQ0/s0 - dQ1/s1;
    case 'gja';  d0 = 1/(s0+s1);  d1 = 1/(s0+s1);           %dQ = (dQ0 - dQ1)/(s0+s1);
    case 'sta';  d0 = 1/s0;       d1 = 1/(s0+s1);           %dQ = dQ0/s0 - dQ1/(s0+s1);
    case 'pos';  d0 = 1/s0;       d1 = 0;                   %dQ = dQ0/s0;
    case 'neg';  d0 = 0;          d1 = 1/s1;                %dQ = -dQ1/s1;
    otherwise; error('qtype unknown');
end
if ~s0                                                      %adjust for absent positive weights
    s0=1;
    d0=0;
end
if ~s1                                                      %adjust for absent negative weights
    s1=1;
    d1=0;
end

for u=randperm(n);                                          %loop over all nodes in random order
    ma = M(u);                                              %current module of u
    r=rand<p;
    if r                                                    %with probability p
        mb = ceil(rand*n);                                  %choose random new module
    else
        dQ0 = (Knm0(u,:)+W0(u,u)-Knm0(u,ma)) - Kn0(u).*(Km0+Kn0(u)-Km0(ma))/s0;     %positive dQ
        dQ1 = (Knm1(u,:)+W1(u,u)-Knm1(u,ma)) - Kn1(u).*(Km1+Kn1(u)-Km1(ma))/s1;     %negative dQ
        dQ = d0*dQ0 - d1*dQ1;                               %rescaled changes in modularity
        dQ(ma) = 0;                                         %no changes for same module
        [max_dQ mb] = max(dQ);                              %maximal increase in modularity and corresponding module
    end
    if r || (max_dQ>1e-10)                                  %if maximal increase is positive (equiv. dQ(mb)>dQ(ma))
        M(u) = mb;                                          %reassign module

        Knm0(:,mb)=Knm0(:,mb)+W0(:,u);
        Knm1(:,mb)=Knm1(:,mb)+W1(:,u);
        Knm0(:,ma)=Knm0(:,ma)-W0(:,u);
        Knm1(:,ma)=Knm1(:,ma)-W1(:,u);
        Km0(mb)=Km0(mb)+Kn0(u);
        Km1(mb)=Km1(mb)+Kn1(u);
        Km0(ma)=Km0(ma)-Kn0(u);
        Km1(ma)=Km1(ma)-Kn1(u);
    end
end

[dum dum M]=unique(M(:).');                                 %realign module indices
if nargout==2                                               %compute modularity
    m = M(ones(1,n),:);
    Q0 = (W0-(Kn0*Kn0.')/s0).*(m==m.');
    Q1 = (W1-(Kn1*Kn1.')/s1).*(m==m.');
    q = d0*sum(Q0(:)) - d1*sum(Q1(:));
end


% WAVELET_LAYER_1D Compute the one-dimensional wavelet transform from
% the modulus wavelet coefficients of the previous layer.
%
% Usages
%    [U_phi , U_psi] = wavelet_layer_1d(U, filters)
%
%    [U_phi , U_psi] = wavelet_layer_1d(U, filters, scat_opt)
%
%    [U_phi , U_psi] = wavelet_layer_1d(U, filters, scat_opt, wavelet)
%
% Input
%    U (struct): The input layer to be transformed.
%    filters (cell): The filters of the wavelet transform.
%    scat_opt (struct): The options that are transferred to the wavelet
%    function handle.
%    wavelet (function handle): the wavelet transform function (default is
%    @wavelet_1d).
%
% Output
%    U_phi The coefficients of in, lowpass-filtered (scattering
%       coefficients).
%    U_psi: The wavelet transform coefficients.
%
% Description
%    This function has a pivotal role between WAVELET_1D (which computes a
%    single wavelet transform), and WAVELET_FACTORY_1D (which creates the
%    whole cascade). Given inputs modulus wavelet coefficients
%    corresponding to a layer, WAVELET_LAYER_1D computes the wavelet
%    transform coefficients of the next layer using WAVELET_1D.
% See also
%   WAVELET_1D, WAVELET_FACTORY_1D, WAVELET_LAYER_2D

function [U_phi, U_psi] = wavelet_layer_1dave(U, filters, scat_opt, wavelet)
	if nargin < 3
		scat_opt = struct();
	end

	if nargin < 4
		wavelet = @wavelet_1d;
	end

	calc_U = (nargout>=2);

	[psi_xi,psi_bw,phi_bw] = filter_freq(filters.meta);

	if ~isfield(U.meta, 'bandwidth'), U.meta.bandwidth = 2*pi; end
	if ~isfield(U.meta, 'resolution'), U.meta.resolution = 0; end

	U_phi.signal = {};
	U_phi.meta.bandwidth = [];
	U_phi.meta.resolution = [];
	U_phi.meta.j = zeros(size(U.meta.j,1),0);

	U_psi.signal = {};
	U_psi.meta.bandwidth = [];
	U_psi.meta.resolution = [];
	U_psi.meta.j = zeros(size(U.meta.j,1)+1,0);

	r = 1;
	for p1 = 1:length(U.signal)
		psi_mask = calc_U&(U.meta.bandwidth(p1)>psi_xi);

		scat_opt.x_resolution = U.meta.resolution(p1);
		scat_opt.psi_mask = psi_mask;
		[x_phi, x_psi, meta_phi, meta_psi] = ...
			wavelet(U.signal{p1}, filters, scat_opt);

		U_phi.signal{1,p1} = mean(U.signal{p1}(:));
		U_phi.meta = map_meta(U.meta,p1,U_phi.meta,p1);
		U_phi.meta.bandwidth(1,p1) = meta_phi.bandwidth;
		U_phi.meta.resolution(1,p1) = meta_phi.resolution;

		ind = r:r+sum(psi_mask)-1;
		U_psi.signal(1,ind) = x_psi(1,psi_mask);
		U_psi.meta = map_meta(U.meta,p1,U_psi.meta,ind,{'j'});
		U_psi.meta.bandwidth(1,ind) = meta_psi.bandwidth(1,psi_mask);
		U_psi.meta.resolution(1,ind) = meta_psi.resolution(1,psi_mask);
		U_psi.meta.j(:,ind) = [U.meta.j(:,p1)*ones(1,length(ind)); ...
			meta_psi.j(1,psi_mask)];

		r = r+length(ind);
	end
end


function [M,scal] = spm_affreg(VG,VF,flags,M,scal)
% Affine registration using least squares.
% FORMAT [M,scal] = spm_affreg(VG,VF,flags,M0,scal0)
%
% VG        - Vector of template volumes.
% VF        - Source volume.
% flags     - a structure containing various options.  The fields are:
%             WG       - Weighting volume for template image(s).
%             WF       - Weighting volume for source image
%                        Default to [].
%             sep      - Approximate spacing between sampled points (mm).
%                        Defaults to 5.
%             regtype  - regularisation type.  Options are:
%                        'none'  - no regularisation
%                        'rigid' - almost rigid body
%                        'subj'  - inter-subject registration (default).
%                        'mni'   - registration to ICBM templates
%             globnorm - Global normalisation flag (1)
% M0        - (optional) starting estimate. Defaults to eye(4).
% scal0     - (optional) starting estimate.
%
% M         - affine transform, such that voxels in VF map to those in
%             VG by   VG.mat\M*VF.mat
% scal      - scaling factors for VG
%
% When only one template is used, then the cost function is approximately
% symmetric, although a linear combination of templates can be used.
% Regularisation is based on assuming a multi-normal distribution for the
% elements of the Henckey Tensor. See:
% "Non-linear Elastic Deformations". R. W. Ogden (Dover), 1984.
% Weighting for the regularisation is determined approximately according
% to:
% "Incorporating Prior Knowledge into Image Registration"
% J. Ashburner, P. Neelin, D. L. Collins, A. C. Evans & K. J. Friston.
% NeuroImage 6:344-352 (1997).
%
%_______________________________________________________________________
% @(#)spm_affreg.m	2.3 John Ashburner 03/02/18

if nargin<5, scal = ones(length(VG),1); end;
if nargin<4, M    = eye(4);             end;

def_flags = struct('sep',5, 'regtype','subj','WG',[],'WF',[],'globnorm',1,'debug',0);
if nargin < 2 | ~isstruct(flags),
	flags = def_flags;
else,
	fnms = fieldnames(def_flags);
	for i=1:length(fnms),
		if ~isfield(flags,fnms{i}),
			flags = setfield(flags,fnms{i},getfield(def_flags,fnms{i}));
		end;
	end;
end;

% Check to ensure inputs are valid...
% ---------------------------------------------------------------
if length(VF)>1, error('Can not use more than one source image'); end;
if ~isempty(flags.WF),
	if length(flags.WF)>1,
		error('Can only use one source weighting image');
	end;
	if any(any(VF.mat-flags.WF.mat)),
		error('Source and its weighting image must have same orientation');
	end;
	if any(any(VF.dim(1:3)-flags.WF.dim(1:3))),
		error('Source and its weighting image must have same dimensions');
	end;
end;
if ~isempty(flags.WG),
	if length(flags.WG)>1,
		error('Can only use one template weighting image');
	end;
	tmp = reshape(cat(3,VG(:).mat,flags.WG.mat),16,length(VG)+length(flags.WG));
else,
	tmp = reshape(cat(3,VG(:).mat),16,length(VG));
end;
if any(any(diff(tmp,1,2))),
	error('Reference images must all have the same orientation');
end;
if ~isempty(flags.WG),
	tmp = cat(1,VG(:).dim,flags.WG.dim);
else,
	tmp = cat(1,VG(:).dim);
end;
if any(any(diff(tmp(:,1:3),1,1))),
	error('Reference images must all have the same dimensions');
end;
% ---------------------------------------------------------------

% Generate points to sample from, adding some jitter in order to
% make the cost function smoother.
% ---------------------------------------------------------------
rand('state',0); % want the results to be consistant.
dg   = VG(1).dim(1:3);
df   = VF(1).dim(1:3);

if length(VG)==1,
	skip = sqrt(sum(VG(1).mat(1:3,1:3).^2)).^(-1)*flags.sep;
	[x1,x2,x3]=ndgrid(1:skip(1):dg(1)-.5, 1:skip(2):dg(2)-.5, 1:skip(3):dg(3)-.5);
	x1   = x1 + rand(size(x1))*0.5; x1 = x1(:);
	x2   = x2 + rand(size(x2))*0.5; x2 = x2(:);
	x3   = x3 + rand(size(x3))*0.5; x3 = x3(:);
end;

skip = sqrt(sum(VF(1).mat(1:3,1:3).^2)).^(-1)*flags.sep;
[y1,y2,y3]=ndgrid(1:skip(1):df(1)-.5, 1:skip(2):df(2)-.5, 1:skip(3):df(3)-.5);
y1   = y1 + rand(size(y1))*0.5; y1 = y1(:);
y2   = y2 + rand(size(y2))*0.5; y2 = y2(:);
y3   = y3 + rand(size(y3))*0.5; y3 = y3(:);
% ---------------------------------------------------------------

if flags.globnorm,
	% Scale all images approximately equally
	% ---------------------------------------------------------------
	for i=1:length(VG),
		VG(i).pinfo(1:2,:) = VG(i).pinfo(1:2,:)/spm_global(VG(i));
	end;
	VF(1).pinfo(1:2,:) = VF(1).pinfo(1:2,:)/spm_global(VF(1));
end;
% ---------------------------------------------------------------

if length(VG)==1,
	[G,dG1,dG2,dG3]  = spm_sample_vol(VG(1),x1,x2,x3,1);
	if ~isempty(flags.WG),
		WG = abs(spm_sample_vol(flags.WG,x1,x2,x3,1))+eps;
	end;
end;

[F,dF1,dF2,dF3]  = spm_sample_vol(VF(1),y1,y2,y3,1);
if ~isempty(flags.WF),
	WF = abs(spm_sample_vol(flags.WF,y1,y2,y3,1))+eps;
end;
% ---------------------------------------------------------------
n_main_its = 0;
ss         = Inf;
W          = [Inf Inf Inf];
est_smo    = 1;
% ---------------------------------------------------------------

for iter=1:256,
	pss   = ss;
	p0    = [0 0 0  0 0 0  1 1 1  0 0 0];

	% Initialise the cost function and its 1st and second derivatives
	% ---------------------------------------------------------------
	n     = 0;
	ss    = 0;
	Beta  = zeros(12+length(VG),1);
	Alpha = zeros(12+length(VG));

	if length(VG)==1,
		% Make the cost function symmetric
		% ---------------------------------------------------------------

		% Build a matrix to rotate the derivatives by, converting from
		% derivatives w.r.t. changes in the overall affine transformation
		% matrix, to derivatives w.r.t. the parameters p.
		% ---------------------------------------------------------------
		dt  = 0.0001;
		R   = eye(13);
		MM0 = inv(VG.mat)*inv(spm_matrix(p0))*VG.mat;
		for i1=1:12,
			p1          = p0;
			p1(i1)      = p1(i1)+dt;
			MM1         = (inv(VG.mat)*inv(spm_matrix(p1))*(VG.mat));
			R(1:12,i1)  = reshape((MM1(1:3,:)-MM0(1:3,:))/dt,12,1);
		end;
		% ---------------------------------------------------------------
		[t1,t2,t3] = coords((M*VF(1).mat)\VG(1).mat,x1,x2,x3);
		msk        = find((t1>=1 & t1<=df(1) & t2>=1 & t2<=df(2) & t3>=1 & t3<=df(3)));
		if length(msk)<32, error_message; end;
		t1         = t1(msk);
		t2         = t2(msk);
		t3         = t3(msk);
		t          = spm_sample_vol(VF(1), t1,t2,t3,1);

		% Get weights
		% ---------------------------------------------------------------
		if ~isempty(flags.WF) | ~isempty(flags.WG),
			if isempty(flags.WF),
				wt = WG(msk);
			else,
				wt = spm_sample_vol(flags.WF(1), t1,t2,t3,1)+eps;
				if ~isempty(flags.WG), wt = 1./(1./wt + 1./WG(msk)); end;
			end;
			wt = sparse(1:length(wt),1:length(wt),wt);
		else,
			wt = speye(length(msk));
			wt = [];
		end;
		% ---------------------------------------------------------------
		clear t1 t2 t3

		% Update the cost function and its 1st and second derivatives.
		% ---------------------------------------------------------------
		[AA,Ab,ss1,n1] = costfun(x1,x2,x3,dG1,dG2,dG3,msk,scal^(-2)*t,G(msk)-(1/scal)*t,wt);
		Alpha = Alpha + R'*AA*R;
		Beta  = Beta  + R'*Ab;
		ss    = ss    + ss1;
		n     = n     + n1;
		t     = G(msk) - (1/scal)*t;
	end;

	if 1,
		% Build a matrix to rotate the derivatives by, converting from
		% derivatives w.r.t. changes in the overall affine transformation
		% matrix, to derivatives w.r.t. the parameters p.
		% ---------------------------------------------------------------
		dt = 0.0001;
		R  = eye(12+length(VG));
		MM0 = inv(M*VF.mat)*spm_matrix(p0)*M*VF.mat;
		for i1=1:12,
			p1          = p0;
			p1(i1)      = p1(i1)+dt;
			MM1         = (inv(M*VF.mat)*spm_matrix(p1)*M*VF.mat);
			R(1:12,i1)  = reshape((MM1(1:3,:)-MM0(1:3,:))/dt,12,1);
		end;
		% ---------------------------------------------------------------
		[t1,t2,t3] = coords(VG(1).mat\M*VF(1).mat,y1,y2,y3);
		msk        = find((t1>=1 & t1<=dg(1) & t2>=1 & t2<=dg(2) & t3>=1 & t3<=dg(3)));
		if length(msk)<32, error_message; end;

		if length(msk)<32, error_message; end;
		t1 = t1(msk);
		t2 = t2(msk);
		t3 = t3(msk);
		t  = zeros(length(t1),length(VG));

		% Get weights
		% ---------------------------------------------------------------
		if ~isempty(flags.WF) | ~isempty(flags.WG),
			if isempty(flags.WG),
				wt = WF(msk);
			else,
				wt = spm_sample_vol(flags.WG(1), t1,t2,t3,1)+eps;
				if ~isempty(flags.WF), wt = 1./(1./wt + 1./WF(msk)); end;
			end;
			wt = sparse(1:length(wt),1:length(wt),wt);
		else,
			wt = speye(length(msk));
		end;
		% ---------------------------------------------------------------

		if est_smo,
			% Compute derivatives of residuals in the space of F
			% ---------------------------------------------------------------
			[ds1,ds2,ds3] = transform_derivs(VG(1).mat\M*VF(1).mat,dF1(msk),dF2(msk),dF3(msk));
			for i=1:length(VG),
				[t(:,i),dt1,dt2,dt3] = spm_sample_vol(VG(i), t1,t2,t3,1);
				ds1   = ds1 - dt1*scal(i); clear dt1
				ds2   = ds2 - dt2*scal(i); clear dt2
				ds3   = ds3 - dt3*scal(i); clear dt3
			end;
			dss   = [ds1'*wt*ds1 ds2'*wt*ds2 ds3'*wt*ds3];
			clear ds1 ds2 ds3
		else,
			for i=1:length(VG),
				t(:,i)= spm_sample_vol(VG(i), t1,t2,t3,1);
			end;
		end;

		clear t1 t2 t3

		% Update the cost function and its 1st and second derivatives.
		% ---------------------------------------------------------------
		[AA,Ab,ss2,n2] = costfun(y1,y2,y3,dF1,dF2,dF3,msk,-t,F(msk)-t*scal,wt);
		Alpha = Alpha  + R'*AA*R;
		Beta  = Beta   + R'*Ab;
		ss    = ss     + ss2;
		n     = n      + n2;
	end;

	if est_smo,
		% Compute a smoothness correction from the residuals and their
		% derivatives.  This is analagous to the one used in:
		% 	"Analysis of fMRI Time Series Revisited"
		% 	Friston KJ, Holmes AP, Poline JB, Grasby PJ, Williams SCR,
		% 	Frackowiak RSJ, Turner R.  Neuroimage 2:45-53 (1995).
		% ---------------------------------------------------------------
		vx     = sqrt(sum(VG(1).mat(1:3,1:3).^2));
		pW     = W;
		W      = (2*dss/ss2).^(-.5).*vx;
		W      = min(pW,W);
		if flags.debug, fprintf('\nSmoothness FWHM:  %.3g x %.3g x %.3g mm\n', W*sqrt(8*log(2))); end;
		if length(VG)==1, dens=2; else, dens=1; end;
		smo    = prod(min(dens*flags.sep/sqrt(2*pi)./W,[1 1 1]));
		est_smo=0;
		n_main_its = n_main_its + 1;
	end;

	% Update the parameter estimates
	% ---------------------------------------------------------------
	nu      = n*smo;
	sig2    = ss/nu;
	[d1,d2] = reg(M,12+length(VG),flags.regtype);

	soln    = (Alpha/sig2+d2)\(Beta/sig2-d1);
	scal    = scal - soln(13:end);
	M       = spm_matrix(p0 + soln(1:12)')*M;

	if flags.debug,
		fprintf('%d\t%g\n', iter, ss/n);
		piccies(VF,VG,M,scal,b)
	end;

	% If cost function stops decreasing, then re-estimate smoothness
	% and try again.  Repeat a few times.
	% ---------------------------------------------------------------
	ss = ss/n;
	if iter>1, spm_chi2_plot('Set',ss); end;
	if (pss-ss)/pss < 1e-6,
		est_smo = 1;
	end;
	if n_main_its>3, break; end;

end;
return;
%_______________________________________________________________________

%_______________________________________________________________________
function [X1,Y1,Z1] = transform_derivs(Mat,X,Y,Z)
% Given the derivatives of a scalar function, return those of the
% affine transformed function
%_______________________________________________________________________

t1 = Mat(1:3,1:3);
t2 = eye(3);
if sum((t1(:)-t2(:)).^2) < 1e-12,
        X1 = X;Y1 = Y; Z1 = Z;
else,
        X1    = Mat(1,1)*X + Mat(1,2)*Y + Mat(1,3)*Z;
        Y1    = Mat(2,1)*X + Mat(2,2)*Y + Mat(2,3)*Z;
        Z1    = Mat(3,1)*X + Mat(3,2)*Y + Mat(3,3)*Z;
end;
return;
%_______________________________________________________________________

%_______________________________________________________________________
function [d1,d2] = reg(M,n,typ)
% Analytically compute the first and second derivatives of a penalty
% function w.r.t. changes in parameters.

if nargin<3, typ = 'subj'; end;
if nargin<2, n   = 13;     end;

[mu,isig] = priors(typ);
ds  = 0.000001;
d1  = zeros(n,1);
d2  = zeros(n);
p0  = [0 0 0  0 0 0  1 1 1  0 0 0];
h0  = penalty(p0,M,mu,isig);
for i=7:12, % derivatives are zero w.r.t. rotations and translations
	p1    = p0;
	p1(i) = p1(i)+ds;
	h1    = penalty(p1,M,mu,isig);
	d1(i) = (h1-h0)/ds; % First derivative
	for j=7:12,
		p2    = p0;
		p2(j) = p2(j)+ds;
		h2    = penalty(p2,M,mu,isig);
		p3    = p1;
		p3(j) = p3(j)+ds;
		h3    = penalty(p3,M,mu,isig);
		d2(i,j) = ((h3-h2)/ds-(h1-h0)/ds)/ds; % Second derivative
	end;
end;
return;
%_______________________________________________________________________

%_______________________________________________________________________
function h = penalty(p,M,mu,isig)
% Return a penalty based on the elements of an affine transformation,
% which is given by:
% 	spm_matrix(p)*M
%
% The penalty is based on the 6 unique elements of the Hencky tensor
% elements being multinormally distributed.
%_______________________________________________________________________

% Unique elements of symmetric 3x3 matrix.
els = [1 2 3 5 6 9];

T = spm_matrix(p)*M;
T = T(1:3,1:3);
T = 0.5*logm(T'*T);
T = T(els)' - mu;
h = T'*isig*T;
return;
%_______________________________________________________________________

%_______________________________________________________________________
function [mu,isig] = priors(typ)
% The parameters for this distribution were derived empirically from 227
% scans, that were matched to the ICBM space.
%_______________________________________________________________________

mu   = zeros(6,1);
isig = zeros(6);
switch deblank(lower(typ)),

case 'mni', % For registering with MNI templates...
	mu   = [0.0667 0.0039 0.0008 0.0333 0.0071 0.1071]';
	isig = 1e4 * [
	    0.0902   -0.0345   -0.0106   -0.0025   -0.0005   -0.0163
	   -0.0345    0.7901    0.3883    0.0041   -0.0103   -0.0116
	   -0.0106    0.3883    2.2599    0.0113    0.0396   -0.0060
	   -0.0025    0.0041    0.0113    0.0925    0.0471   -0.0440
	   -0.0005   -0.0103    0.0396    0.0471    0.2964   -0.0062
	   -0.0163   -0.0116   -0.0060   -0.0440   -0.0062    0.1144];

case 'rigid', % Constrained to be almost rigid...
	mu   = zeros(6,1);
	isig = eye(6)*1e9;

case 'isochoric', % Volume preserving...
	error('Not implemented');

case 'isotropic', % Isotropic zoom in all directions...
	error('Not implemented');

case 'subj', % For inter-subject registration...
	mu   = zeros(6,1);
	isig = 1e3 * [
	    0.8876    0.0784    0.0784   -0.1749    0.0784   -0.1749
	    0.0784    5.3894    0.2655    0.0784    0.2655    0.0784
	    0.0784    0.2655    5.3894    0.0784    0.2655    0.0784
	   -0.1749    0.0784    0.0784    0.8876    0.0784   -0.1749
	    0.0784    0.2655    0.2655    0.0784    5.3894    0.0784
	   -0.1749    0.0784    0.0784   -0.1749    0.0784    0.8876];

case 'none', % No regularisation...
	mu   = zeros(6,1);
	isig = zeros(6);

otherwise,
	error(['"' typ '" not recognised as type of regularisation.']);
end;
return;

%_______________________________________________________________________
function [y1,y2,y3]=coords(M,x1,x2,x3)
% Affine transformation of a set of coordinates.
%_______________________________________________________________________

y1 = M(1,1)*x1 + M(1,2)*x2 + M(1,3)*x3 + M(1,4);
y2 = M(2,1)*x1 + M(2,2)*x2 + M(2,3)*x3 + M(2,4);
y3 = M(3,1)*x1 + M(3,2)*x2 + M(3,3)*x3 + M(3,4);
return;
%_______________________________________________________________________

%_______________________________________________________________________
function A = make_A(x1,x2,x3,dG1,dG2,dG3,t)
% Generate part of a design matrix using the chain rule...
% df/dm = df/dy * dy/dm
% where
% 	df/dm is the rate of change of intensity w.r.t. affine parameters
% 	df/dy is the gradient of the image f
% 	dy/dm crange of position w.r.t. change of parameters
%_______________________________________________________________________

A  = [x1.*dG1 x1.*dG2 x1.*dG3 ...
	  x2.*dG1 x2.*dG2 x2.*dG3 ...
	  x3.*dG1 x3.*dG2 x3.*dG3 ...
	      dG1     dG2     dG3    t];
return;
%_______________________________________________________________________

%_______________________________________________________________________
function [AA,Ab,ss,n] = costfun(x1,x2,x3,dG1,dG2,dG3,msk,lastcols,b,wt)
chunk = 10240;
lm    = length(msk);
AA    = zeros(12+size(lastcols,2));
Ab    = zeros(12+size(lastcols,2),1);
ss    = 0;
n     = 0;

for i=1:ceil(lm/chunk),
	ind  = (((i-1)*chunk+1):min(i*chunk,lm))';
	msk1 = msk(ind);

	A1   = make_A(x1(msk1),x2(msk1),x3(msk1),dG1(msk1),dG2(msk1),dG3(msk1),lastcols(ind,:));
	b1   = b(ind);
	if ~isempty(wt),
		wt1   = wt(ind,ind);
		AA    = AA  + A1'*wt1*A1;
		%Ab   = Ab  + A1'*wt1*b1;
		Ab    = Ab  + (b1'*wt1*A1)';
		ss    = ss  + b1'*wt1*b1;
		n     = n   + trace(wt1);
		clear wt1
	else,
		AA    = AA  + spm_atranspa(A1);
		%Ab   = Ab  + A1'*b1;
		Ab    = Ab  + (b1'*A1)';
		ss    = ss  + b1'*b1;
		n     = n   + length(msk1);
	end;
	clear A1 b1 msk1 ind
end;
return;
%_______________________________________________________________________

%_______________________________________________________________________
function error_message
% Display an error message for when things go wrong.
str = {	'There is not enough overlap in the images',...
	'to obtain a solution.',...
	' ',...
	'Please check that your header information is OK.'};
spm('alert*',str,mfilename,sqrt(-1));
error('insufficient image overlap')
return
%_______________________________________________________________________

%_______________________________________________________________________
function piccies(VF,VG,M,scal,b)
% This is for debugging purposes.
% It shows the linear combination of template images, the affine
% transformed source image, the residual image and a histogram of the
% residuals.
%_______________________________________________________________________

figure(2);
Mt = spm_matrix([0 0 (VG(1).dim(3)+1)/2]);
M  = (M*VF(1).mat)\VG(1).mat;
t  = zeros(VG(1).dim(1:2));
for i=1:length(VG);
	t  = t + spm_slice_vol(VG(i),  Mt,VG(1).dim(1:2),1)*scal(i);
end;
u  = spm_slice_vol(VF(1),M*Mt,VG(1).dim(1:2),1);
subplot(2,2,1);imagesc(t');axis image xy off
subplot(2,2,2);imagesc(u');axis image xy off
subplot(2,2,3);imagesc(u'-t');axis image xy off
subplot(2,2,4);hist(b,50); % Entropy of residuals may be a nice cost function?
drawnow;
return;
%_______________________________________________________________________


function outputImage = rms(this, varargin)
% Computes root mean square along specified dimension, i.e. sqrt(mean(Y.^2))
%
%
%   Y = MrImage()
%   Y.rms(applicationDimension)
%
% This is a method of class MrImage.
%
% IN
%   applicationDimension    image dimension along which operation is
%                           performed (e.g. 4 = time, 3 = slices)
%                           default: The last dimension with more than one
%                           value is chosen
%                           (i.e. 3 for 3D image, 4 for 4D image)
%
% OUT
%   outputImage             rms of all images along application dimension
%
% EXAMPLE
%   rms
%
%   See also MrImage MrImage.perform_unary_operation

% Author:   Saskia Klein & Lars Kasper
% Created:  2014-12-23
% Copyright (C) 2014 Institute for Biomedical Engineering
%                    University of Zurich and ETH Zurich
%
% This file is part of the TAPAS UniQC Toolbox, which is released
% under the terms of the GNU General Public Licence (GPL), version 3.
% You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version).
% For further details, see the file COPYING or
%  <http://www.gnu.org/licenses/>.

if nargin > 1
    applicationDimension = varargin{1};
    outputImage = mean(this.^2, applicationDimension).^(1/2);
else
    outputImage = mean(this.^2).^(1/2);
end


function bc = specific_bc(xbd,ybd)
%quad_bc   channel flow boundary condition for 4:1 quadrilateral domain
%   bc = specific_bc(xbd,ybd);
%   input
%          xbd          x boundary coordinate vector
%          ybd          y boundary coordinate vector
%
%   specifies streamfunction associated with Poiseuille flow
%   needs editing for general aspect ratios
%   IFISS function: DJS; 6 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
aspect=4; deform=4;
bc=0*xbd;
k=find(xbd==0); bc(k)=2*ybd(k).*ybd(k) -(4/3)*ybd(k).*ybd(k).*ybd(k);
slope=(1/aspect)*(1/deform-1);
k3=find(ybd==slope*xbd +1);  bc(k3)=2/3;
return


 function st = nufft_init(om, Nd, Jd, Kd, varargin)
%function st = nufft_init(om, Nd, Jd, Kd, [n_shift,] ...)
%
% Initialize structure for d-dimension NUFFT using KB interpolator,
% particularly the interpolation matrix in sparse format.
% caution: this routine can require a lot of memory!
% in
%	om [M,d]	"digital" frequencies in radians
%	Nd [d]		image dimensions (N1,N2,...,Nd)
%	Jd [d]		# of neighbors used (in each direction)
%	Kd [d]		FFT sizes (should be >= N1,N2,...)
% optional arguments
%	n_shift [d]	n = 0-n_shift to N-1-n_shift (must be first)
%	'minmax:kb'	minmax interpolator with excellent KB scaling!
%				(minmax:kb is recommended, and used by default)
%	'minmax:tuned'	minmax interpolator, somewhat numerically tuned
%	'minmax:user'	minmax interpolator with user ({alpha}, {beta})
%	'uniform'	uniform scaling factors (not recommended)
%	'kaiser'	kaiser-bessel (KB) interpolator (minmax best alpha, m)
%			or 'kaiser', alpha, m to specify parameter (vectors)
%	'linear'	linear interpolator (a terrible straw man)
%	kernel		user-provided inline interpolation kernel(k,J)
%			(or a cell array of kernels, one for each dimension)
%	'table'		use table-based interpolation rather than sparse matrix.
%			this can save a lot of memory for large problems.
%			example ..., 'table', 2^11, 'minmax:kb'
%			where 2^11 is the table over-sampling factor.
% out
%	st.p		[M, *Nd]	sparse interpolation matrix
%					(or empty if table-based)
%	st.sn		[(Nd)]		scaling factors
%	st.Nd,Jd,Kd,om	copies of inputs
%
% *Nd is shorthand for prod(Nd).
% (Nd) is shorthand for (N1,N2,...,Nd)
%
% Like fft(), the NUFFT expects the signals to be x(0,0), ...
% Use n_shift = [N1/2, N2/2, ...] for x(-N1/2,-N2/2,...), ...
%
% Copyright 2002-5-30	Jeff Fessler	The University of Michigan

if nargin < 4, help(mfilename), error args, end

% dimensionality of input space (usually 2 or 3)
dd = length(Nd);
if dd ~= length(Jd) | dd ~= length(Kd)
	error 'inconsistent dim'
end
if dd ~= size(om,2), error(sprintf('omega needs %d columns', dd)), end

%
% process optional arguments
%

% n_shift argument? (must be first)
if length(varargin) > 0 & isnumeric(varargin{1})
	n_shift = varargin{1};
	if dd ~= length(n_shift)
		error(sprintf('n_shift needs %d columns', dd))
	end
	varargin = {varargin{2:end}};
else
	n_shift = zeros(size(Nd));
end
st.n_shift = n_shift;

% default/recommended interpolator is minmax with KB scaling factors
if length(varargin) == 0
	varargin = {'minmax:kb'};
end

st.alpha = {};
st.beta = {};
is_kaiser_scale = logical(0);

% table based?
if ischar(varargin{1}) & streq(varargin{1}, 'table')
	st = nufft_table_init(om, Nd, Jd, Kd, n_shift, varargin{2:end});
	return
end

ktype = varargin{1};

% cell array of kernel functions: {kernel1, kernel2, ..., kernelD}
if isa(ktype, 'cell')
	if isa(ktype{1}, 'inline') | isa(ktype{1}, 'function_handle')
		ktype = 'inline';
		if length(varargin) > 1, error 'excess arguments?', end
		if length(varargin{1}) ~= dd, error 'wrong # of kernels', end
		st.kernel = varargin{1};
	else
		error 'cell array should be inline kernels!?'
	end

% or a single inline kernel for all dimension
elseif isa(ktype, 'inline') | isa(ktype, 'function_handle')
	ktype = 'inline';
	if length(varargin) > 1, error 'excess arguments?', end
	for id = 1:dd
		st.kernel{id} = varargin{1};	% all same
	end

% or a string that describes the type of interpolator
elseif ~ischar(ktype)
	error 'non-string kernel type?'

end

st.ktype = ktype;

%
% set up whatever is needed for each interpolator
%
if streq(ktype, 'inline')
	% already did it above

% linear interpolator straw man
elseif streq(ktype, 'linear')
	ktype = 'inline';
	kernel = inline('(1 - abs(k/(J/2))) .* (abs(k) < J/2)', 'k', 'J');
	for id = 1:dd
		st.kernel{id} = kernel;
	end

% KB interpolator
elseif streq(ktype, 'kaiser')
	is_kaiser_scale = logical(1);

	% with minmax-optimized parameters
	if length(varargin) == 1
		for id = 1:dd
			[st.kernel{id} st.kb_alf(id) st.kb_m(id)] = ...
				kaiser_bessel('inline', Jd(id));
		end

	% with user-defined parameters
	elseif length(varargin) == 3
		alpha_list = varargin{2};
		m_list = varargin{3};
		if (length(alpha_list) ~= dd) | (length(m_list) ~= dd)
			error 'need #dim alpha and m'
		end
		for id = 1:dd
			[st.kernel{id} st.kb_alf(id) st.kb_m(id)] = ...
				kaiser_bessel('inline', Jd(id), ...
					alpha_list(id), m_list(id));
		end
	else
		error 'kaiser should have no arguments, or both alpha and m'
	end

% minmax interpolator with KB scaling factors (recommended default)
elseif streq(ktype, 'minmax:kb')
	for id = 1:dd
		[st.alpha{id}, st.beta{id}] = ...
			nufft_alpha_kb_fit(Nd(id), Jd(id), Kd(id));
	end

% minmax interpolator with numerically "tuned" scaling factors
elseif streq(ktype, 'minmax:tuned')
	for id = 1:dd
		[st.alpha{id}, st.beta{id}, ok] = ...
			nufft_best_alpha(Jd(id), 0, Kd(id)/Nd(id));
		if ~ok, error 'unknown J,K/N', end
	end

% minmax interpolator with user-provided scaling factors
elseif streq(ktype, 'minmax:user')
	if length(varargin) ~= 3, error 'user must provide alpha/beta', end
	st.alpha = varargin{2};
	st.beta = varargin{3};
	if length(st.alpha) ~= dd | length(st.beta) ~= dd
		error 'alpha/beta size mismatch'
	end

elseif streq(ktype, 'uniform')
	for id = 1:dd
		st.alpha{id} = 1;
		st.beta{id} = 0;
	end

else
	error 'unknown kernel type'
end

st.tol	= 0;

st.Jd	= Jd;
st.Nd	= Nd;
st.Kd	= Kd;

M = size(om,1);
st.M	= M;
st.om	= om;

%
% scaling factors: "outer product" of 1D vectors
%
st.sn = 1;
for id=1:dd
	if is_kaiser_scale
		nc = [0:Nd(id)-1]'-(Nd(id)-1)/2;
		tmp = 1 ./ kaiser_bessel_ft(...
			nc/Kd(id), Jd(id), st.kb_alf(id), st.kb_m(id), 1);
	elseif streq(ktype, 'inline')
		tmp = 1 ./ nufft_interp_zn(0, Nd(id), Jd(id), Kd(id), st.kernel{id});
	else
		tmp = nufft_scale(Nd(id), Kd(id), st.alpha{id}, st.beta{id});
	end
	st.sn = st.sn(:) * tmp';
end
if length(Nd) > 1
	st.sn = reshape(st.sn, Nd);	% [(Nd)]
else
	st.sn = st.sn(:);	% [(Nd)]
end

%
% [J?,M] interpolation coefficient vectors.  will need kron of these later
%
for id=1:dd
	N = Nd(id);
	J = Jd(id);
	K = Kd(id);
	if isvar('st.kernel')
		[c, arg] = ...
		nufft_coef(om(:,id), J, K, st.kernel{id});	% [J?,M]
	else
		alpha = st.alpha{id};
		beta = st.beta{id};
		T = nufft_T(N, J, K, st.tol, alpha, beta);	% [J?,J?]
		[r, arg] = ...
		nufft_r(om(:,id), N, J, K, alpha, beta);	% [J?,M]
		c = T * r;	clear T r
	end

	gam = 2*pi/K;
	phase_scale = 1i * gam * (N-1)/2;

	phase = exp(phase_scale * arg);	% [J?,M] linear phase
	ud{id} = phase .* c;		% [J?,M]

	%
	% indices into oversampled FFT components
	%
	koff = nufft_offset(om(:,id), J, K);	% [M,1] to leftmost near nbr
	kd{id} = mod(outer_sum([1:J]', koff'), K) + 1;	% [J?,M] {1,...,K?}
	if id > 1	% trick: pre-convert these indices into offsets!
		kd{id} = (kd{id}-1) * prod(Kd(1:(id-1)));
	end

end, clear c arg gam phase phase_scale koff N J K

%
% build sparse matrix that is [M,*Kd]
% with *Jd nonzero entries per frequency point
%
if dd >= 3, printf('Needs at least %g Gbyte RAM', prod(Jd)*M*8/10^9*2), end

kk = kd{1};	% [J1,M]
uu = ud{1};	% [J1,M]
for id = 2:dd
	Jprod = prod(Jd(1:id));
	kk = block_outer_sum(kk, kd{id});	% outer sum of indices
	kk = reshape(kk, Jprod, M);
	uu = block_outer_prod(uu, ud{id});	% outer product of coefficients
	uu = reshape(uu, Jprod, M);
end	% now kk and uu are [*Jd, M]

%
% apply phase shift
% pre-do Hermitian transpose of interpolation coefficients
%
phase = exp(1i * (om * n_shift(:))).';			% [1,M]
uu = conj(uu) .* phase(ones(1,prod(Jd)),:);		% [*Jd,M]

mm = [1:M]; mm = mm(ones(prod(Jd),1),:);		% [*Jd,M]
st.p = sparse(mm(:), kk(:), uu(:), M, prod(Kd));	% sparse matrix

%
% in
%	x1	[J1,M]
%	x2	[J2,M]
% out
%	y	[J1,J2,M]	y(i1,i2,m) = x1(i1,m) + x2(i2,m)
%
function y = block_outer_sum(x1, x2)
[J1 M] = size(x1);
[J2 M] = size(x2);
xx1 = reshape(x1, [J1 1 M]);	% [J1,1,M] from [J1,M]
xx1 = xx1(:,ones(J2,1),:);	% [J1,J2,M], emulating ndgrid
xx2 = reshape(x2, [1 J2 M]);	% [1,J2,M] from [J2,M]
xx2 = xx2(ones(J1,1),:,:);	% [J1,J2,M], emulating ndgrid
y = xx1 + xx2;			% [J1,J2,M]

function y = block_outer_prod(x1, x2)
[J1 M] = size(x1);
[J2 M] = size(x2);
xx1 = reshape(x1, [J1 1 M]);	% [J1,1,M] from [J1,M]
xx1 = xx1(:,ones(J2,1),:);	% [J1,J2,M], emulating ndgrid
xx2 = reshape(x2, [1 J2 M]);	% [1,J2,M] from [J2,M]
xx2 = xx2(ones(J1,1),:,:);	% [J1,J2,M], emulating ndgrid
y = xx1 .* xx2;			% [J1,J2,M]