% dbm - training DBM using Gibbs sampling
% Copyright (C) 2011 KyungHyun Cho, Tapani Raiko, Alexander Ilin
%
% This program is free software; you can redistribute it and/or
% modify it under the terms of the GNU General Public License
% as published by the Free Software Foundation; either version 2
% of the License, or (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
%
function [D] = dbm(D, patches, use_Qpre, Qpre, Qpre_mask);

if nargin < 3
    use_Qpre = 0;
end

actual_lrate = D.learning.lrate;

if D.adaptive_lrate.use == 1
    initial_lrate = D.learning.lrate;
    actual_lrate = initial_lrate;
end

n_samples = size(patches, 1);
if D.structure.layers(1) ~= size(patches, 2)
    error('Data is not properly aligned');
end

minibatch_sz = D.learning.minibatch_sz;
n_minibatches = ceil(n_samples / minibatch_sz);

n_epochs = D.iteration.n_epochs;

cd_k = D.learning.cd_k;
persistent_cd = D.learning.persistent_cd;
momentum = D.learning.momentum;
weight_decay = D.learning.weight_decay;

adaptive_lrate = D.adaptive_lrate.use;
enhanced_grad = D.enhanced_grad.use;

lrate_lb = D.adaptive_lrate.lrate_lb;
lrate_ub = D.adaptive_lrate.lrate_ub;
exp_up = D.adaptive_lrate.exp_up;
exp_down = D.adaptive_lrate.exp_down;
max_iter_up = D.adaptive_lrate.max_iter_up;
max_iter_down = D.adaptive_lrate.max_iter_down;

layers = D.structure.layers;
n_layers = length(layers);

min_recon_error = Inf;
min_recon_error_update_idx = 0;
stopping = 0;

do_normalize = D.grbm.do_normalize;
do_normalize_std = D.grbm.do_normalize_std;
update_sigmas = D.grbm.learn_sigmas;
do_vsample = D.grbm.do_vsample;

if D.data.binary == 0
    if do_normalize == 1
        % make it zero-mean
        patches_mean = mean(patches, 1);
        patches = bsxfun(@minus, patches, patches_mean);
    end

    if do_normalize_std == 1
        % make it unit-variance
        patches_std = std(patches, [], 1);
        patches = bsxfun(@rdivide, patches, patches_std);
    end
end

n_samples = size(patches, 1);

logsigmas = log(D.sigmas.^2);

% upper-bound.. but is there any need for it?
sigmas_ub = D.grbm.sigmas_ub;
logsigmas_ub = log(D.grbm.sigmas_ub);

biases_grad_old = cell(n_layers, 1);
W_grad_old = cell(n_layers, 1);
for l = 1:n_layers
    biases_grad_old{l} = zeros(size(D.biases{l}))';
    if l < n_layers
        W_grad_old{l} = zeros(size(D.W{l}));
    end
end
sigma_grad_old = zeros(size(D.biases{1}))';

n_minibatches = ceil(n_samples / minibatch_sz);
n_updates = 0;

epsilon_sigma = 1e-8;
epsilon_logsigma = log(epsilon_sigma^2);

min_recon_error = Inf;
min_recon_error_update_idx = 0;
stopping = 0;

anneal_counter = 0;
actual_lrate0 = actual_lrate;

if D.debug.do_display == 1
    figure(D.debug.display_fid);
end

try
    use_gpu = gpuDeviceCount;
catch errgpu
    use_gpu = false;
    disp(['Could not use CUDA. Error: ' errgpu.identifier])
end

if use_gpu
    % push
    logsigmas = gpuArray(single(logsigmas));
end

for step=1:n_epochs
    if D.verbose
        fprintf(2, 'Epoch %d/%d: ', step, n_epochs)
    end

    if use_gpu
        % push
        for l = 1:n_layers
            if l < n_layers
                D.W{l} = gpuArray(single(D.W{l}));
            end
            D.biases{l} = gpuArray(single(D.biases{l}));
            if D.centering.use
                D.centering.centers{l} = gpuArray(single(D.centering.centers{l}));
            end
        end

        D.sigmas = gpuArray(single(D.sigmas));
    end

    for mb=1:n_minibatches
        D.iteration.n_updates = D.iteration.n_updates + 1;

        if D.verbose
            tic;
        end

        % p_0
        mb_start = (mb-1) * minibatch_sz + 1;
        mb_end = min(mb * minibatch_sz, n_samples);
        v0 = patches(mb_start:mb_end, :);

        if use_gpu > 0
            v0 = gpuArray(single(v0));
        end

        if D.data.binary
            v0 = binornd(1, v0);
        end
        mb_sz = size(v0,1);

        % just for a bit of speed-up
        if persistent_cd && exist('h1') ~= 0
            fmb_sigma2s = repmat(D.sigmas', [size(h1{1}, 1) 1]);
        else
            fmb_sigma2s = repmat(D.sigmas', [mb_sz 1]);
        end

        if use_Qpre
            % for pretraining
            h0 = cell(n_layers, 1);
            h0{1} = v0;
            for l = 2:n_layers
                if Qpre_mask(l)
                    h0{l} = binornd(1, Qpre{l}(mb_start:mb_end, :));
                else
                    h0{l} = zeros(mb_end - mb_start + 1, layers(l));
                end
                if use_gpu
                    h0{l} = gpuArray(single(h0{l}));
                end
            end

            for l = 2:n_layers
                if Qpre_mask(l)
                    continue;
                end

                h0{l} = h0{l} * 0;
                if l > 1
                    if l == 2 && D.data.binary == 0
                        h0{l} = h0{l} + bsxfun(@rdivide, h0{l-1}, D.sigmas.^2') * D.W{l-1};
                    else
                        h0{l} = h0{l} + h0{l-1} * D.W{l-1};
                    end
                end

                if l < n_layers
                    h0{l} = h0{l} + h0{l+1} * D.W{l}';
                end

                h0{l} = sigmoid(bsxfun(@plus, h0{l}, D.biases{l}'));
            end

            if adaptive_lrate
                if mb == n_minibatches
                    mb_next = 1;
                else
                    mb_next = mb + 1;
                end

                nmb_start = (mb_next-1) * minibatch_sz + 1;
                nmb_end = min(mb_next * minibatch_sz, n_samples);
                if use_gpu
                    v0_next = gpuArray(single(patches(nmb_start:nmb_end, :)));
                else
                    v0_next = single(patches(nmb_start:nmb_end, :));
                end

                if D.data.binary == 0
                    next_mb_sz = size(v0_next,1);

                    if persistent_cd == 0
                        nmb_sigma2s = repmat(D.sigmas, [next_mb_sz 1]);
                    else
                        if next_mb_sz ~= minibatch_sz
                            nmb_sigma2s = repmat(D.sigmas, [next_mb_sz 1]);
                        else
                            nmb_sigma2s = fmb_sigma2s;
                        end
                    end
                end
            end
        else
            % for finetuning
            h0 = dbm_get_hidden(v0, D, 10, 1e-6, D.mf.reg);
            h0{1} = v0;

            if adaptive_lrate
                if mb == n_minibatches
                    mb_next = 1;
                else
                    mb_next = mb + 1;
                end
                if use_gpu
                    v0_next = gpuArray(single(patches((mb_next-1) * minibatch_sz + 1:min(mb_next * minibatch_sz, n_samples), :)));
                else
                    v0_next = single(patches((mb_next-1) * minibatch_sz + 1:min(mb_next * minibatch_sz, n_samples), :));
                end
                if D.data.binary == 0
                    next_mb_sz = size(v0_next,1);

                    if persistent_cd == 0
                        nmb_sigma2s = repmat(D.sigmas, [next_mb_sz 1]);
                    else
                        if next_mb_sz ~= minibatch_sz
                            nmb_sigma2s = repmat(D.sigmas, [next_mb_sz 1]);
                        else
                            nmb_sigma2s = fmb_sigma2s;
                        end
                    end
                end
            end
        end

        % p_1
        if (persistent_cd ~= 0 && exist('h1') == 0)
            h1 = h0;
        end

        if (persistent_cd == 0)
            h1 = h0;

            if use_Qpre
                for l = 2:n_layers
                    if Qpre_mask(l)
                        continue;
                    end

                    h1{l} = binornd(1, h1{l});
                end
            end
        end

        if D.centering.use
            for l = 1:n_layers
                if D.data.binary == 0 && l == 1
                    continue;
                end
                h0{l} = bsxfun(@minus, h0{l}, D.centering.centers{l}');
            end
        end

        % compute reconstruction error
        if D.data.binary == 1
            vr = sigmoid(bsxfun(@plus, h0{2} * D.W{1}',D.biases{1}'));
        else
            vr = bsxfun(@plus, h0{2} * D.W{1}',D.biases{1}');
        end

        rerr = mean(sum((v0 - vr).^2,2));
        if use_gpu > 0
            rerr = gather(rerr);
        end
        D.signals.recon_errors = [D.signals.recon_errors rerr];

        for k=1:cd_k
            for oddeven = [1 0]
                for l = 1:n_layers
                    if mod(l, 2) == oddeven
                        continue;
                    end
                    h1{l} = h1{l} * 0;
                    if D.centering.use
                        if l > 1
                            if l == 2 && D.data.binary == 0
                                h1{l} = h1{l} + bsxfun(@rdivide, h1{l-1}, D.sigmas.^2') * D.W{l-1};
                            else
                                h1{l} = h1{l} + bsxfun(@minus, h1{l-1}, D.centering.centers{l-1}') * D.W{l-1};
                            end
                        end

                        if l < n_layers
                            h1{l} = h1{l} + bsxfun(@minus, h1{l+1}, D.centering.centers{l+1}') * D.W{l}';
                        end
                    else
                        if l > 1
                            if l == 2 && D.data.binary == 0
                                h1{l} = h1{l} + bsxfun(@rdivide, h1{l-1}, D.sigmas.^2') * D.W{l-1};
                            else
                                h1{l} = h1{l} + h1{l-1} * D.W{l-1};
                            end
                        end

                        if l < n_layers
                            h1{l} = h1{l} + h1{l+1} * D.W{l}';
                        end
                    end

                    h1{l} = bsxfun(@plus, h1{l}, D.biases{l}');

                    if l > 1 || D.data.binary == 1
                        h1{l} = sigmoid(h1{l});
                        h1{l} = binornd(1, h1{l});
                    else
                        if do_vsample
                            h1{l} = normrnd(h1{l}, fmb_sigma2s);
                        end
                    end
                end
                if (sum(sum(isnan(h1{1}))) > 0)
                    error('NaN found in the visual fantasy particles.\n It is advisable to adjust learning parameters.');
                end
            end
        end

        if D.centering.use
            for l = 1:n_layers
                if D.data.binary == 0 && l == 1
                    continue;
                end
                h1{l} = bsxfun(@minus, h1{l}, D.centering.centers{l}');
            end
        end

        % get base distribution
        base_vbias = mean(h1{1}, 1);
        base_sigma = std(h1{1}, [], 1)';

        % get gradient
        for l = 1:n_layers
            if D.data.binary == 0 && l == 1
                bias0 = bsxfun(@rdivide, mean(h0{l}, 1), D.sigmas.^2');
                bias1 = bsxfun(@rdivide, mean(h1{l}, 1), D.sigmas.^2');
            else
                bias0 = mean(h0{l}, 1);
                bias1 = mean(h1{l}, 1);
            end

            biases_grad{l} = bias0 - bias1;

            clear bias0 bias1;

            if l < n_layers
                if D.data.binary == 0 && l == 1
                    W0 = bsxfun(@rdivide, (h0{l}' * h0{l+1}) / mb_sz, D.sigmas.^2);
                    W1 = bsxfun(@rdivide, (h1{l}' * h1{l+1}) / size(h1{1},1), D.sigmas.^2);
                else
                    W0 = (h0{l}' * h0{l+1}) / mb_sz;
                    W1 = (h1{l}' * h1{l+1}) / size(h1{1},1);
                end

                W_grad{l} = W0 - W1;

                clear W0 W1;
            end
        end

        if D.data.binary == 0
            sigma0 = mean((bsxfun(@minus, h0{1}, D.biases{1}').^2) - ...
                h0{1} .* (h0{2} * D.W{1}'), 1);
            sigma1 = mean((bsxfun(@minus, h1{1}, D.biases{1}').^2) - ...
                h1{1} .* (h1{2} * D.W{1}'), 1);
            sigma_grad = (sigma0 - sigma1) ./ D.sigmas.^2';

            if D.grbm.use_single_sigma == 1
                mean_sigma_grad = mean(sigma_grad);
                sigma_grad = mean_sigma_grad * ones(size(sigma_grad));
            end

            clear sigma0 sigma1;
        end

        % enhanced grad
        if enhanced_grad == 1
            acts = cell(n_layers, 1);
            for l = 1:n_layers
                acts{l} = (mean(h0{l}, 1) + mean(h1{l}, 1))/2;
            end
%            if D.data.binary == 0
%                acts{1} = acts{1} ./ D.sigmas.^2';
%            end

            for l = 1:n_layers-1
                W_grad{l} = W_grad{l} - biases_grad{l}' * acts{l+1} ...
                    - acts{l}' * biases_grad{l+1};
            end

            for l = 1:n_layers
                if l > 1
                    acts1 = acts{l-1};
                    biases_grad{l} = biases_grad{l} - acts1 * W_grad{l-1};
                end
                if l < n_layers
                    acts2 = acts{l+1};
                    biases_grad{l} = biases_grad{l} - acts2 * W_grad{l}';
                end
            end

            clear acts;
        end

        if D.learning.lrate_anneal > 0 && (step >= D.learning.lrate_anneal * n_epochs)
            anneal_counter = anneal_counter + 1;
            actual_lrate = actual_lrate0 / anneal_counter;
        else
            if adaptive_lrate == 1
                if use_Qpre
                    h0_next = cell(n_layers, 1);
                    h0_next{1} = v0_next;
                    for l = 2:n_layers
                        if Qpre_mask(l)
                            h0_next{l} = Qpre{l}(nmb_start:nmb_end, :);
                        else
                            h0_next{l} = zeros(nmb_end - nmb_start + 1, layers(l));
                        end
                        if use_gpu
                            h0_next{l} = gpuArray(single(h0_next{l}));
                        end
                    end

                    for l = 2:n_layers
                        if Qpre_mask(l)
                            continue;
                        end

                        h0_next{l} = h0_next{l} * 0;
                        if l > 1
                            if l == 2 && D.data.binary == 0
                                h0_next{l} = h0_next{l} + bsxfun(@rdivide, h0_next{l-1}, D.sigmas.^2') * D.W{l-1};
                            else
                                h0_next{l} = h0_next{l} + h0_next{l-1} * D.W{l-1};
                            end
                        end

                        if l < n_layers
                            h0_next{l} = h0_next{l} + h0_next{l+1} * D.W{l}';
                        end

                        h0_next{l} = sigmoid(bsxfun(@plus, h0_next{l}, D.biases{l}'));
                    end
                else
                    h0_next = dbm_get_hidden(v0_next, D, 10, 1e-6, D.mf.reg);
                    h0_next{1} = v0_next;
                end

                if D.centering.use
                    for l = 1:n_layers
                        if D.data.binary == 0 && l == 1
                            continue;
                        end
                        h0_next{l} = bsxfun(@minus, h0_next{l}, D.centering.centers{l}');
                    end
                end

                [cE, cEmin, cEmax, cEs] = dbm_energy(h1, D.W, D.biases, D.data.binary, 1., D.sigmas, base_sigma, base_vbias);

                base_lrate = actual_lrate;
                candidate_lrates;

                costs = zeros(1, length(cand_lrates));
                for s=1:length(cand_lrates)
                    W_test = cell(size(D.W));
                    biases_test = cell(size(D.biases));
                    if use_gpu
                        logsigmas_test = gpuArray(single(zeros(size(logsigmas))));
                    else
                        logsigmas_test = single(zeros(size(logsigmas)));
                    end
                    cand_lrate = cand_lrates(s);

                    for l = 1:n_layers
                        biases_test{l} = D.biases{l} + cand_lrate * (((1 - momentum) * biases_grad{l} + momentum * biases_grad_old{l})' - weight_decay * D.biases{l});
                        if l < n_layers
                            W_test{l} = D.W{l} + cand_lrate * ((1 - momentum) * W_grad{l} + momentum * W_grad_old{l} - weight_decay * D.W{l});
                        end
                    end
                    if D.data.binary == 0
                        if update_sigmas == 1
                            logsigmas_test = logsigmas + cand_lrate * (((1-momentum) * sigma_grad + momentum * sigma_grad_old)' - weight_decay * logsigmas);
                            logsigmas_test = max(epsilon_logsigma, min(logsigmas_ub, logsigmas_test));
                            sigmas_test = sqrt(exp(logsigmas));
                        else
                            sigmas_test = sqrt(exp(logsigmas_test));
                        end
                    else
                        sigmas_test = sqrt(exp(logsigmas_test));
                    end

%                    % FIXME: Should we?
%                    h0_next = dbm_get_hidden_raw(v0_next, D.data.binary, D.structure.layers, ...
%                        W_test, biases_test, sigmas_test, 5, 1e-5);
%                    h0_next{1} = v0_next;
                    [dE, dEmin, dEmax, dEs] = dbm_energy(h0_next, W_test, biases_test, D.data.binary, 1., sigmas_test, base_sigma, base_vbias);
                    [fE, fEmin, fEmax, fEs] = dbm_energy(h1, W_test, biases_test, D.data.binary, 1., sigmas_test, base_sigma, base_vbias);

                    now_cost = sum(-double(gather(dEs)) - logsum(double(gather(-fEs + cEs))) + log(size(h1{1},1)));

                    costs(s) = now_cost;

                    clear W_test biases_test logsigmas_test sigmas_test;
                    %clear h0_next;
                end

                [chosen_cost chosen_index] = max(costs);
                actual_lrate = min(lrate_ub, max(lrate_lb, cand_lrates(chosen_index)));
            else
                actual_lrate = D.learning.lrate / (1 + D.iteration.n_updates / D.learning.lrate0);
            end
            actual_lrate0 = actual_lrate;
        end

        D.signals.lrates = [D.signals.lrates actual_lrate];

%        if D.debug.do_display == 1 && mod(D.iteration.n_updates, D.debug.display_interval) == 0
%            D.debug.display_function (D.debug.display_fid, D, v0, v1, W_grad, vbias_grad, hbias_grad, sigma_grad);
%            drawnow;
%        end
%
        % update
        for l = 1:n_layers
            biases_grad_old{l} = (1 - momentum) * biases_grad{l} + momentum * biases_grad_old{l};
            D.biases{l} = D.biases{l} + actual_lrate * (biases_grad_old{l}' - weight_decay * D.biases{l});
            if l < n_layers
                W_grad_old{l} = (1 - momentum) * W_grad{l} + momentum * W_grad_old{l};
                D.W{l} = D.W{l} + actual_lrate * (W_grad_old{l} - weight_decay * D.W{l});
            end
        end
        if D.data.binary == 0
            if update_sigmas == 1
                sigma_grad_old = (1-momentum) * sigma_grad + momentum * sigma_grad_old;
                logsigmas = logsigmas + actual_lrate * (sigma_grad_old' - weight_decay * logsigmas);
                logsigmas = max(epsilon_logsigma, min(logsigmas_ub, logsigmas));
                D.sigmas = sqrt(exp(logsigmas));
            end
        end

        if D.verbose == 1
            fprintf(2, '%2.3fs.', toc);
        end

        if D.stop.criterion > 0
            if D.stop.criterion == 1
                if min_recon_error > D.signals.recon_errors(end)
                    min_recon_error = D.signals.recon_errors(end);
                    min_recon_error_update_idx = D.iteration.n_updates;
                else
                    if D.iteration.n_updates > min_recon_error_update_idx + D.stop.recon_error.tolerate_count
                        fprintf(2, '\nStopping criterion reached (recon error) %f > %f\n', ...
                            D.signals.recon_errors(end), min_recon_error);
                        stopping = 1;
                        break;
                    end
                end
            else
                error ('Unknown stopping criterion %d', D.stop.criterion);
            end
        end

        if length(D.hook.per_update) > 1
            err = D.hook.per_update{1}(D, D.hook.per_update{2});

            if err == -1
                stopping = 1;
                break;
            end
        end

        if D.centering.use
            for l = 1:n_layers
                if D.data.binary == 0 && l == 1
                    continue;
                end
                h0{l} = bsxfun(@plus, h0{l}, D.centering.centers{l}');
                h1{l} = bsxfun(@plus, h1{l}, D.centering.centers{l}');
            end
        end

        if use_gpu > 0
            clear v0 h0;
            clear v0_next h0_next;
            if persistent_cd == 0
                clear h1;
            end

            clear fmb_sigma2s;
            clear base_sigma base_vbias;
        end

    end

    if use_gpu > 0
        % pull
        for l = 1:n_layers
            if l < n_layers
                D.W{l} = gather(D.W{l});
            end
            D.biases{l} = gather(D.biases{l});
            if D.centering.use
                D.centering.centers{l} = gather(D.centering.centers{l});
            end
        end
        D.sigmas = gather(D.sigmas);
    end

    if length(D.hook.per_epoch) > 1
        err = D.hook.per_epoch{1}(D, D.hook.per_epoch{2});

        if err == -1
            stopping = 1;
        end
    end

    if stopping == 1
        break;
    end

    if D.verbose == 1
        fprintf(2, '\n');
    end

    fprintf(2, 'Epoch %d/%d - recon_error: %f\n', step, n_epochs, ...
        D.signals.recon_errors(end));
end

if use_gpu > 0
    % pull
    for l = 1:n_layers
        if l < n_layers
            D.W{l} = gather(D.W{l});
        end
        D.biases{l} = gather(D.biases{l});
        if D.centering.use
            D.centering.centers{l} = gather(D.centering.centers{l});
        end
    end
    D.sigmas = gather(D.sigmas);

    clear h1 logsigmas;
end


function [g1, g2] = lfmwhiteXrbfwhiteKernGradient(lfmKern, rbfKern, t1, varargin)

% LFMWHITEXRBFWHITEKERNGRADIENT Compute a cross gradient between an LFM-WHITE
% and an RBF-WHITE kernels.
% FORMAT
% DESC computes cross gradient of parameters of a cross kernel between an
% LFM-WHITE and an RBF-WHITE kernels for the multiple output kernel.
% ARG lfmKern : the kernel structure associated with the LFM-WHITE kernel.
% ARG rbfKern : the kernel structure associated with the RBF-WHITE kernel.
% ARG t1 : inputs for which kernel is to be computed.
% ARG covGrad : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% RETURN g1 : gradient of the parameters of the LFM-WHITE kernel, for
% ordering see lfmwhiteKernExtractParam.
% RETURN g2 : gradient of the parameters of the RBF-WHITE kernel, for
% ordering see rbfwhiteKernExtractParam.
%
% FORMAT
% DESC computes cross gradient of parameters of a cross kernel between an
% LFM-WHITE and an RBF-WHITE kernels for the multiple output kernel.
% ARG lfmKern : the kernel structure associated with the LFM-WHITE kernel.
% ARG rbfKern : the kernel structure associated with the RBF-WHITE kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% ARG covGrad : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% RETURN g1 : gradient of the parameters of the LFM-WHITE kernel, for
% ordering see lfmwhiteKernExtractParam.
% RETURN g2 : gradient of the parameters of the RBF-WHITE kernel, for
% ordering see rbfwhiteKernExtractParam.
%
% SEEALSO : multiKernParamInit, multiKernCompute, lfmwhiteKernParamInit,
% rbfwhiteKernParamInit, lfmwhiteKernExtractParam, rbfwhiteKernExtractParam
%
% COPYRIGHT : David Luengo, 2009

% KERN

if nargin < 5
    t2 = t1;
else
    t2 = varargin{1};
end
covGrad = varargin{end};

if size(t1, 2) > 1 | size(t2, 2) > 1
  error('Input can only have one column');
end
if lfmKern.variance ~= rbfKern.variance
  error('Kernels cannot be cross combined if they have different variances.')
end
if lfmKern.isStationary ~= rbfKern.isStationary
  error('Stationary and non-stationary kernels cannot be cross combined.')
end

g1 = zeros(1, lfmKern.nParams);
g2 = zeros(1, rbfKern.nParams);

T1 = repmat(t1, 1, size(t2, 1));
T2 = repmat(t2.', size(t1, 1), 1);
deltaT = T1 - T2;
indT = double(T1 >= T2);

% Parameters required in the computation of the kernel
isStationary = lfmKern.isStationary;
mass = lfmKern.mass;
spring = lfmKern.spring;
damper = lfmKern.damper;
variance = lfmKern.variance;
sensitivity = lfmKern.sensitivity;

invWidth = rbfKern.inverseWidth;

alpha = lfmKern.alpha;
omega = lfmKern.omega;
gamma = lfmKern.gamma;
gammaTilde = alpha - j*omega;

% Gradients of theta w.r.t. mass, spring and damper

gradThetaMass = [1 0 0];
gradThetaAlpha = [-damper/(2*(mass^2)) 0 1/(2*mass)];
gradThetaOmega = [(damper^2-2*mass*spring)/(2*(mass^2)*sqrt(4*mass*spring-damper^2)) ...
    1/(sqrt(4*mass*spring-damper^2)) -damper/(2*mass*sqrt(4*mass*spring-damper^2))];
gradThetaGamma = gradThetaAlpha + j*gradThetaOmega;
gradThetaGammaTilde = gradThetaAlpha - j*gradThetaOmega;

% Computation of the normalised kernel (i.e. variance = 1 and sensitivity = 1)
% and auxiliary functions and constants

lfmKern.variance = 1;
lfmKern.sensitivity = 1;
rbfKern.variance = 1;
K = lfmwhiteXrbfwhiteKernCompute(lfmKern, rbfKern, t1, t2);

c = 1 / (j*4*mass*omega);

varphiT1T2 = gamma * deltaT .* indT + 0.5 * invWidth * (deltaT.^2) .* (1-indT);
zT1T2 = sqrt(0.5*invWidth) * (-deltaT .* (1-indT) + gamma / invWidth);
varphiT1T2Tilde = gammaTilde * deltaT .* indT + 0.5 * invWidth * (deltaT.^2) .* (1-indT);
zT1T2Tilde = sqrt(0.5*invWidth) * (-deltaT .* (1-indT) + gammaTilde / invWidth);
if (isStationary == false)
    varphiT10 = gamma * T1;
    varphi0T2 = 0.5 * invWidth * (T2.^2);
    z0T2 = sqrt(0.5*invWidth) * (T2 + gamma/invWidth);
    varphiT10Tilde = gammaTilde * T1;
    z0T2Tilde = sqrt(0.5*invWidth) * (T2 + gammaTilde/invWidth);
    %gradThetaVarphiT10 = T1;
end

gradThetaVarphiT1T2 = deltaT .* indT;
%gradThetaZ = 1/sqrt(2*invWidth);

% Gradient w.r.t. the mass, spring and damper (lfmKern)
for i = 1:3
    gradThetaPsi = - exp(-varphiT1T2) ...
        .* (gradThetaGamma(i) * wofzHui(j*zT1T2) .* gradThetaVarphiT1T2 ...
        + sqrt(2/invWidth) * gradThetaGamma(i) * (1/sqrt(pi)-zT1T2.*wofzHui(j*zT1T2)));
    gradThetaPsiTilde = - exp(-varphiT1T2Tilde) ...
        .* (gradThetaGammaTilde(i) * wofzHui(j*zT1T2Tilde) .* gradThetaVarphiT1T2 ...
        + sqrt(2/invWidth) * gradThetaGammaTilde(i) * (1/sqrt(pi)-zT1T2Tilde.*wofzHui(j*zT1T2Tilde)));
    if (isStationary == false)
        gradThetaPsi = gradThetaPsi + exp(-(varphiT10 + varphi0T2)) ...
            .* (gradThetaGamma(i) * T1 .* wofzhui(j*z0T2) ...
            + sqrt(2/invWidth) * gradThetaGamma(i) * (1/sqrt(pi)-z0T2.*wofzHui(j*z0T2)));
        gradThetaPsiTilde = gradThetaPsiTilde + exp(-(varphiT10Tilde + varphi0T2)) ...
            .* (gradThetaGammaTilde(i) * T1 .* wofzhui(j*z0T2Tilde) ...
            + sqrt(2/invWidth) * gradThetaGammaTilde(i) * (1/sqrt(pi)-z0T2Tilde.*wofzHui(j*z0T2Tilde)));
    end
    g1(i) = sensitivity * variance * sum(sum((-(gradThetaMass(i)/mass + gradThetaOmega(i)/omega) ...
        * K + c * (gradThetaPsiTilde - gradThetaPsi)) .* covGrad));
end

% Gradient w.r.t. the inverse width (rbfKern)
gradInvWidthVarphiT1T2 = 0.5 * (deltaT.^2) .* (1-indT);
gradInvWidthZT1T2 = -(deltaT .* (1-indT) + gamma / invWidth) / sqrt(8*invWidth);
gradInvWidthZT1T2Tilde = -(deltaT .* (1-indT) + gammaTilde / invWidth) / sqrt(8*invWidth);
gradInvWidthPsi = - exp(-varphiT1T2) ...
    .* (wofzHui(j*zT1T2) .* gradInvWidthVarphiT1T2 ...
    + 2 * (1/sqrt(pi)-zT1T2.*wofzHui(j*zT1T2)) .* gradInvWidthZT1T2);
gradInvWidthPsiTilde = - exp(-varphiT1T2Tilde) ...
    .* (wofzHui(j*zT1T2Tilde) .* gradInvWidthVarphiT1T2 ...
    + 2 * (1/sqrt(pi)-zT1T2Tilde.*wofzHui(j*zT1T2Tilde)) .* gradInvWidthZT1T2Tilde);
if (isStationary == false)
    gradInvWidthVarphi0T2 = 0.5*(T2.^2);
    gradInvWidthZ0T2 = (T2 - gamma / invWidth) / sqrt(8*invWidth);
    gradInvWidthZ0T2Tilde = (T2 - gammaTilde / invWidth) / sqrt(8*invWidth);
    gradInvWidthPsi = gradInvWidthPsi + exp(-(varphiT10 + varphi0T2)) ...
        .* (gradInvWidthVarphi0T2 .* wofzhui(j*z0T2) ...
        + 2 * (1/sqrt(pi)-z0T2.*wofzHui(j*z0T2)) .* gradInvWidthZ0T2);
    gradInvWidthPsiTilde = gradInvWidthPsiTilde + exp(-(varphiT10Tilde + varphi0T2)) ...
        .* (gradInvWidthVarphi0T2 .* wofzhui(j*z0T2Tilde) ...
        + 2 * (1/sqrt(pi)-z0T2Tilde.*wofzHui(j*z0T2Tilde)) .* gradInvWidthZ0T2Tilde);
end
g2(1) = sensitivity * variance * sum(sum((c * (gradInvWidthPsiTilde - gradInvWidthPsi)) .* covGrad));

% Gradient w.r.t. sigma_r^2
g1(4) = sensitivity * sum(sum(K .* covGrad));
g2(2) = 0; % Otherwise it is counted twice

% Gradient w.r.t. sensitivity (only lfmKern)
g1(5) = variance * sum(sum(K .* covGrad));

% Ensure that the gradients are real
g1 = real(g1);
g2 = real(g2);


function [data] = ft_nirs_transform_ODs(cfg, data)

% FT_NIRS_TRANSFORM_ODs computes the transformation from optical densities (OD)
% to chromophore concentrations such as (de-)oxygenated  hemoglobin, or the
% other way around.
%
% Use as either
%   [data]      = ft_nirs_transform_ODs(cfg, data)
%
% The configuration "cfg" is a structure containing information about
% target of the transformation. The configuration should contain
%   cfg.channel            = Nx1 cell-array with selection of channels
%                            (default = 'nirs'), see FT_CHANNELSELECTION for
%                            more details
%   cfg.target             = Mx1 cell-array, can be 'O2Hb' (oxygenated hemo-
%                            globin), 'HHb' de-oxygenated hemoglobin') or
%                            'tHb' (total hemoglobin), or a combination of
%                            those (default: {'O2Hb', 'HHb'})
%
% Optional configuration settings are
%   cfg.age                = scalar, age of the subject (necessary to
%                            automatically select the appropriate DPF, or
%   cfg.dpf                = scalar, differential path length factor
%   cfg.dpffile            = string, location to a lookup table for the
%                            relation between participant age and DPF
%
% Note that the DPF might be different across channels, and is usually
% stored in the optode structure contained in the data.
%
% The function returns data transformed to the specified chromophore
% concentrations that were requested.
%
% To facilitate data-handling and distributed computing you can use
%   cfg.inputfile   =  ...
% If you specify this option the input data will be read from a *.mat
% file on disk. This mat files should contain only a single variable named 'data',
% corresponding to the input structure.
%
% See also FT_NIRS_PREPARE_ODTRANSFORMATION, FT_APPLY_MONTAGE

% Options to be implemented:
%   cfg.opto          = structure with optode definition or filename, see FT_READ_SENS
%   cfg.siunits       = yes/no, ensure that SI units are used consistently
%   cfg.logarithm     = string, can be 'natural' or 'base10' (default = 'base10')
%   cfg.inverse       = string, whether the multiplicative inverse should be take
%
% Note that HomER uses the inverse, natural logarithm. The inverse is taken
% after the ln transform.

% You are using the FieldTrip NIRS toolbox developed and maintained by
% Artinis Medical Systems (http://www.artinis.com). For more information
% on FieldTrip, see http://www.fieldtriptoolbox.org
%
% This work is licensed under a Creative Commons Attribution-ShareAlike 4.0
% International License. To view a copy of this license, visit
% http://creativecommons.org/licenses/by-sa/4.0/ or send a letter to
% Creative Commons, PO Box 1866, Mountain View, CA 94042, USA.
%
% Creative Commons Attribution-ShareAlike 4.0 International License:
% -----------------------------------
% You are free to:
%
%     Share - copy and redistribute the material in any medium or format
%     Adapt - remix, transform, and build upon the material
%     for any purpose, even commercially.
%
%     The licensor cannot revoke these freedoms as long as you follow the
%     license terms.
%
% Under the following terms:
%
%     Attribution - You must give appropriate credit, provide a link to
%                    the license, and indicate if changes were made. You
%                    may do so in any reasonable manner, but not in any way
%                    that suggests the licensor endorses you or your use.
%
%     ShareAlike - If you remix, transform, or build upon the material,
%                   you must distribute your contributions under the same
%                   license as the original.
%
%     No additional restrictions - You may not apply legal terms or
%                                   technological measures that legally
%                                   restrict others from doing anything the
%                                   license permits.
%
% -----------------------------------
%
% This toolbox is not to be used for medical or clinical purposes.
%
% Copyright (c) 2015-2016 by Artinis Medical Systems.
% Contact: askforinfo@artinis.com
%
% Main programmer: Jörn M. Horschig
% $Id$

% these are used by the ft_preamble/ft_postamble function and scripts
ft_revision = '$Id$';
ft_nargin   = nargin;
ft_nargout  = nargout;

% do the general setup of the function
ft_defaults
ft_preamble init
ft_preamble loadvar     data
ft_preamble provenance  data

ft_preamble debug

% the ft_abort variable is set to true or false in ft_preamble_init
if ft_abort
  % do not continue function execution in case the outputfile is present and the user indicated to keep it
  return
end

data = ft_checkdata(data, 'datatype', 'raw', 'feedback', 'yes');

% set the defaults
cfg.channel = ft_getopt(cfg, 'channel', 'nirs');
cfg.target  = ft_getopt(cfg, 'target', {'O2Hb', 'HHb'});

% determine the NIRS channels
cfg.channel = ft_channelselection(cfg.channel, data);

% make a copy of the data
origdata = data;

% keep only the NIRS channels
data = ft_selectdata(cfg, data);

% also keep the non-NIRS channels
tmpcfg = cfg;
tmpcfg.channel = setdiff(origdata.label, cfg.channel);
dataextra = ft_selectdata(tmpcfg, origdata);

target = cfg.target;
if ~iscell(target)
  target     = {target};
end

computetHb  = any(ismember(target, 'tHb'));
computeHHb  = any(ismember(target, 'HHb'));
computeO2Hb = any(ismember(target, 'O2Hb'));

% cfg-handling is done inside here
[montage, cfg] = ft_nirs_prepare_ODtransformation(cfg, data);

% save montage in the cfg
cfg.montage = montage;

% apply the (combined) montages
dataout = ft_apply_montage(data, montage, 'keepunused', 'yes');

if computetHb
  % total hemoglobin is the sum of oxygenated and deoxygenated hemoglobin
  tmpcfg = [];
  tmpcfg.channel = '*[O2Hb]';
  dataO2Hb = ft_selectdata(tmpcfg, dataout);
  dataO2Hb.label = strrep(dataO2Hb.label, 'O2Hb', 'tHb');

  tmpcfg = [];
  tmpcfg.channel = '*[HHb]';
  dataHHb = ft_selectdata(tmpcfg, dataout);
  dataHHb.label = strrep(dataHHb.label, 'HHb', 'tHb');

  tmpcfg = [];
  tmpcfg.operation = 'add';
  tmpcfg.parameter = 'trial';
  dataTotal = ft_math(tmpcfg, dataO2Hb, dataHHb);

  if ~computeHHb && ~computeO2Hb
    % replace dataout
    dataout.label = dataTotal.label;
    dataout.trial = dataTotal.trial;
  else
    % concat to dataout
    dataout.label = vertcat(dataout.label, dataTotal.label);
    dataout.trial = cellfun(@vertcat, dataout.trial, dataTotal.trial, 'UniformOutput', false);
  end
end

% remove O2 or H if not desired
tmpcfg = [];
tmpcfg.channel = [];
if computetHb
  tmpcfg.channel = horzcat(tmpcfg.channel, {'*tHb]'});
end
if computeHHb
  tmpcfg.channel =  horzcat(tmpcfg.channel, {'*HHb]'});
end
if computeO2Hb
  tmpcfg.channel =  horzcat(tmpcfg.channel, {'*O2Hb]'});
end

dataout = ft_selectdata(tmpcfg, dataout);

if length(dataextra.label)>0
  % append the extra data
  data = ft_appenddata([], dataout, dataextra);
else
  data = dataout;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% do the general cleanup and bookkeeping at the end of the function
ft_postamble debug

ft_postamble previous    data
ft_postamble provenance  data
ft_postamble history     data


function [wheelslam_rms, wheelins_rms] = wheelslam_func(paras)

% clear
% close all
format longG

datapath         = paras.datapath;
rollSeqdimension = paras.rollSeqdimension; %Dimension of the roll sequence measurement
NPARTICLES       = paras.NPARTICLES;
gridlen          = paras.gridlen;
NEFFECTIVE       = paras.NEFFECTIVE;   %Threshold of the effective particles
sigmaDis_scale   = paras.sigmaDis_scale; % unit: m
sigmaPhi         = paras.sigmaPhi;     % unit: radians
initheading      = paras.initheading; % Initial heading of the vehicle
conRevisitNumThr = paras.conRevisitNumThr;% The number threshold for determining the revisit of the particle
corrCoefThr      = paras.corrCoefThr;
corrCoefNumThr   = paras.corrCoefNumThr;
gt_path          = paras.gt_path;

freq             = 10;
odom             = read_bin(datapath, 4)';% time, dis(equal), headingInc, roll

%Import odometry info from Wheel-INS, including timestamp, dis_increment,
%heading increment and roll angle

revisit_tThr   = 30;      %Minimal time interval for revisit identification. Sometimes car stop
initmapsize    = 101;
progrssThr     = -100;
start_t        = odom(1,1);
odomsize       = floor(size(odom, 1)*paras.odomdata_scale);

particles      = init_particles(NPARTICLES, initmapsize, odomsize, start_t, initheading, corrCoefNumThr);
res            = zeros(odomsize, 4);
res(1, :)      = [start_t particles(1).xv'];

pre_refstate      = [start_t particles(1).xv'];
wheelinsDR        = zeros(odomsize, 4);
wheelinsDR(1,:)   = pre_refstate;
revisitParticles  = 0;

% This is used to set the randanstream of the particles, so as to
% make the results the same everytime.
% for i = 1:NPARTICLES
%     particles(i).randstream = RandStream.create('mlfg6331_64','Seed', i);
% end
%
% resample_randstream = RandStream.create('mlfg6331_64','Seed', idx);
%---------------------------------------------------------------------

for i = 2:odomsize

    % Current measurement of roll sequence.
    % curRollmeas = odom(i, 5:size(odom, 2));
    curRollmeas     = odom(i, 4);
    ref_state       = state_predict_ref(odom(i, :), pre_refstate);% Get the DR results from the odometry file for comparision.
    wheelinsDR(i,:) = ref_state;
    pre_refstate    = ref_state;

    t_s             = clock;
    revisitNum      = 1;
    curodom = odom(i, :);

    for j = 1:NPARTICLES

        particles(j)                 = state_predict(particles(j), curodom, sigmaDis_scale, sigmaPhi); %State prediction for every particle.
        [particles(j), atRow, atCol] = extendMap(particles(j), gridlen); %Extend (or not) the grid map maintained by every particle.

        if (particles(j).conRevisitNum ~= conRevisitNumThr)
            particles(j).conRevisitNum = particles(j).conRevisitNum + 1;
        else
            % sliding window for loop closure detection.

            particles(j).conCorrCoef(1:(conRevisitNumThr-1)) = particles(j).conCorrCoef(2:conRevisitNumThr);
            particles(j).conCorrCoef(conRevisitNumThr)       = -1;

        end

        particles(j).conCorrCoef(particles(j).conRevisitNum)    = -1;

        if (atRow == particles(j).preIdxinMap(1) && atCol == particles(j).preIdxinMap(2)...
                && particles(j).lastTrajRevisitIdx < i-1)

            % If one step is too short (still in the same grid) and if last step is not a revisit
            numforgrid = particles(j).mapattr(particles(j).gridmap(atRow, atCol)).Num;
            particles(j).mapattr(particles(j).gridmap(atRow, atCol)).Value...
            = (particles(j).mapattr(particles(j).gridmap(atRow, atCol)).Value ...
            * (numforgrid/(numforgrid+1)) + curRollmeas./(numforgrid +1));

            % Average the roll measurement in the same grid.
            particles(j).mapattr(particles(j).gridmap(atRow, atCol)).Num = numforgrid + 1;

        else

            innerGridsIndex  = [];
            revisitGrids     = [];
            allRollSeq       = [];
            allheading       = [];

            curallheading = [allheading;particles(j).xv(3)];
            curAllRollSeq = [allRollSeq;curRollmeas];
            curGrid       = [atRow, atCol];
            curAllGrids   = [innerGridsIndex; curGrid];
            curAllGrids   = unique(curAllGrids, 'rows','stable');

            if (~isempty(curAllGrids))

                %Update (or not) the grid map maintained by every particle
                %and get the revisit grids after revisit time interval check.
                %Check if current grid is a new grid.
                [particles(j), revisitGrids] = updateGridmap(particles(j), curAllGrids, curAllRollSeq, curallheading, revisit_tThr, i);

            end

            if (~isempty(revisitGrids))

                for ii = 1:size(revisitGrids, 1)
                    %Threshold for the heading difference between current entance and last one
                    headingdiff = abs(getheadingdiff(particles(j).xv(3), particles(j).mapattr(particles(j).gridmap(revisitGrids(ii,1), revisitGrids(ii,2))).enterheading));
                    if(headingdiff > (pi/6))
                        break;
                    end

                    particles(j).mapattr(particles(j).gridmap(revisitGrids(ii,1), revisitGrids(ii,2))).revisit = ...
                    particles(j).mapattr(particles(j).gridmap(revisitGrids(ii,1), revisitGrids(ii,2))).revisit + 1;
                    particles(j).mapattr(particles(j).gridmap(revisitGrids(ii,1), revisitGrids(ii,2))).Num = ...
                    particles(j).mapattr(particles(j).gridmap(revisitGrids(ii,1), revisitGrids(ii,2))).Num + 1;

                    %Get the historical index of the revisited grid in the
                    %roll sequence.
                    revisitIdx = particles(j).mapattr(particles(j).gridmap(revisitGrids(ii,1), revisitGrids(ii,2))).idx;
                    if( revisitIdx < rollSeqdimension)
                        break;
                    else
                        particles(j).lastTrajRevisitIdx = i; %where the particle detected a revisit by the traj.

                        currollseq = odom(i-rollSeqdimension+1:i,4);% Current roll sequence from the revisited grid
                        maprollseq = odom(revisitIdx-rollSeqdimension+1:revisitIdx,4); %Historical roll sequence
                        corrMat    = corrcoef(currollseq, maprollseq);
                        corrvalue  = corrMat(1,2); % Get the correlation value
                        particles(j).conCorrCoef(particles(j).conRevisitNum) = corrvalue;
                        corrCoefNumtmp = length(particles(j).conCorrCoef(particles(j).conCorrCoef> corrCoefThr));

                        if(particles(j).conRevisitNum == conRevisitNumThr && ...
                           corrCoefNumtmp > corrCoefNumThr &&...
                           corrvalue > corrCoefThr)
                           %criteria 1. Full of the correlation window;
                           %2.Enough value in the window larger than the
                           %threshold
                           %3.Current correlation value larger than the
                           %threshold;

                            tmpSeq = particles(j).conCorrCoef((particles(j).conCorrCoef > corrCoefThr ));

                            particles(j).w = particles(j).w * exp(rms(tmpSeq)* (corrCoefNumtmp/conRevisitNumThr)); %Update the weight of the particles reported a convinced loop closure
                            particles(j).mapattr(particles(j).gridmap(revisitGrids(ii,1), revisitGrids(ii,2))).last_vt = particles(j).t;
                            particles(j).totalrevisitNum = particles(j).totalrevisitNum + 1;
                            particles(j).revisitpos(particles(j).totalrevisitNum, :) = [particles(j).t particles(j).xv(1) particles(j).xv(2)];

                        end

                        % count the revisited particle only once
                        if (ii == 1)
                            revisitParticles(revisitNum) = j;
                            revisitNum = revisitNum + 1;
                        end

                    end

                end

            end

        end

        particles(j).preIdxinMap = [atRow, atCol];
        particles(j).prepos      = particles(j).xv;
        particles(j).traj(i,:)   = (particles(j).xv)';

    end

    % Check if resample is needed, if yes, do it. Generally this function
    % needn't to be modified, we can set the parameters "NEFFECTIVE"
    particles    = resampleParticles(particles, NEFFECTIVE);%, resample_randstream
    %particles    = resampleParticles(particles, NEFFECTIVE, resample_randstream);

    %save system output
    w            = [particles.w];
    xv           = [particles.xv];
    if (mean(abs(xv(3,:))) > abs(mean(abs(xv(3,:))) - pi))
        xv(3, :)     = zero_to_2pi(xv(3, :));
    else
        xv(3, :)     = pi_to_pi(xv(3, :));
    end

    ii       = find(w == max(w),1);
    w        = w/sum(w);
    xvmean   = [mean(sum(w.* xv(1, :))) mean(sum(w.* xv(2, :))) mean(sum(w.* xv(3, :)))];
    res(i,:) = [particles(1).t xvmean];

    revisitParticles = 0;

    runProgress  = floor((i-1)/(odomsize) * 1000);
    if(runProgress > progrssThr)
        clc;
        progrssThr = runProgress;
        disp(['******Wheel SLAM RUNNING:' num2str(0.1*runProgress) '%******']);
    end

end

% figure,
% plot3(res(:,3), res(:,2), res(:,1)-res(1,1), 'LineWidth', 1.5),grid on;
% title('Weighted mean traj.');hold on;
% plot(res(1,3), res(1,2), 'go');hold on;
% plot(res(end,3), res(end,2), 'ro');
% set(gca,'fontsize',10,'fontname','Times');

% plotMap(particles(ii),i, path);

% write_bin(resfile, res);
% write_bin(wheelinsDRpath, wheelinsDR);

particleres = [res(:, 1) particles(ii).traj];% trajectory of the particle with highest weight
[wheelslam_err, wheelslam_rms, wheelins_err, wheelins_rms, wheelslam_err_p, wheelslam_rms_p] =...
    ResCompare(res, particleres, wheelinsDR, gt_path, freq);

%% plot the total revisit num of all the particles
% revisitNums           = [particles.totalrevisitNum];
% figure,
% set(gcf,'unit','normalized','position',[0.05,0.05,0.64,0.48]);
% plot(revisitNums,'*'); set(gca,'fontsize',10,'fontname','Times');
% title('Total Revisit Num');
% figure,
% plot3(particles(ii).revisitpos(:,3), particles(ii).revisitpos(:,2), particles(ii).revisitpos(:,1),'*');
% title(['Particle =' num2str(ii) 'idx = ' num2str(idx)]);

% save particle data
% save([path 'particle_' num2str(ii) '_' s_curt], 'particles');

% format longG
% fprintf(fp_res,'%s\n', s_curt);
% fprintf(fp_res,'Particles = %d NEFFECTIVE = %d\n', NPARTICLES, NEFFECTIVE);
% fprintf(fp_res,'Gridlen = %.1f mode = %d\n', gridlen, mode);
% fprintf(fp_res,'Disstd    = %.5f  Phistd  = %.5f \n', sigmaDis_scale, sigmaPhi);
% fprintf(fp_res,'RollSampleDis = %.1f    RollSeqDim = %d\n', paras.sampleDis, rollSeqdimension);
% fprintf(fp_res,'RollSeqWindow = %d  CorrCoefThr = %.1f  CorrCoefNumThr = %d\n',conRevisitNumThr,corrCoefThr,corrCoefNumThr);
% fprintf(fp_res,'Start_t = %f  TotalData_t = %f  Process_t = %s s\n',start_t, totaldata_t, processtime);
% fprintf(fp_res,'%s\n', 'RMS        [North     East]');
% fprintf(fp_res,'WheelSLAM    %.5f  %.5f\n', wheelslam_rms(1), wheelslam_rms(2));
% fprintf(fp_res,'WheelSLAM_p  %.5f  %.5f\n', wheelslam_rms_p(1), wheelslam_rms_p(2));
% fprintf(fp_res,'WheelINS     %.5f  %.5f\n', wheelins_rms(1), wheelins_rms(2));
% fprintf(fp_res,'Heading RMSE:  Wheel-SLAM:%.5f Wheel-INS:%.5f\n\n', wheelslam_rms(3), wheelins_rms(3));

end

function p = init_particles(np, mapsize, odomsize, start_t, initheading, corrCoefNumThr)

    %Grid map attribute
    gridattr.Num     = 1; %how many times has this grid been visited
    gridattr.visited = 1; %if this grid has been visited
    gridattr.revisit = 0; %if this grid has been revisited
    gridattr.last_vt = start_t;%last visit time of the grid

    gridattr.Value   = zeros(1, 1);%only single roll angle
    gridattr.idx     = 1;% the index of the current roll measurement in the full odometry file
    initheadingrad   = DEG2RAD(initheading);
    gridattr.enterheading = initheadingrad;%the heading of the vehicle when it entering this grid

    p.conRevisitNum  = 0;%length of the correlation sliding window
    p.conCorrCoef    = zeros(1,corrCoefNumThr);
    p.gridmap        = zeros(mapsize, mapsize);% save the idx in the grid attibute matrix at every grid of the map
    %the coordinates of the original point is (0,0), but it is at the
    %central of the map
    p.originIdx      = [(mapsize+1)/2, (mapsize+1)/2];% the idx of the original point in the grid map (which would be updated with the extension of the map).
    p.w              = 1/np;          %weight
    p.xv             = [0;0;initheadingrad]; %state: N, E, Phi

    p.prepos         = [0;0;initheadingrad]; %last position and heading of the vehicle
    p.preIdxinMap    = p.originIdx; %last idx of the vehicle in the grid map
    p.t              = start_t;

    p.totalrevisitNum = 0; %how many convinced loop closure proposed by this particle
    p.revisitpos      = zeros(1,3);%The positions of the vehicle where the convinced loop closure proposed by this particle

    p.mapattr(1)     = gridattr;% attibute of the first grid in the map. A large grid attribute matrix would be built with the moving of the vehicle
    p.totalGrids     = 1;%
    p.gridmap(p.originIdx(1), p.originIdx(2)) = 1;% grid map maintaind by every particle
    p.traj           = zeros(odomsize,3);% the trajectory of the vehicle estimated by this particle
    p.traj(1,:)      = (p.xv)';
    p.lastTrajRevisitIdx = 0;%The Idx in the odom file where the particle detects a revisit by its traj, (no roll needed)
    p                = repmat(p, [np 1]);

end

function x_vec = zero_to_2pi(x_vec)
    lenth = length(x_vec);
    for i = 1:lenth
        if(x_vec(i)<0)
            x_vec(i) = x_vec(i) + 2*pi;
        end
    end

end

function x = pi_to_pi(x)
    if x > pi
        x= x - 2*pi;
    elseif x < -pi
        x= x + 2*pi;
    end
end

function hdiff = getheadingdiff(heading1, heading2)
    hdiff = heading1 - heading2;
    if hdiff > pi
        hdiff= hdiff - 2*pi;
    elseif hdiff < -pi
        hdiff= hdiff + 2*pi;
    end
end

function [bestIdx, lagestcorrValue] = getBestMacth(currollseq, revisitIdx, searchRad, odom, rollSeqdimension)

    lagestcorrValue = -exp(5);
    for i = (revisitIdx-searchRad):(revisitIdx+searchRad)
        maprollseq = odom(i-rollSeqdimension+1:i, 4);
        corrMat    = corrcoef(currollseq, maprollseq);
        corrvalue  = (corrMat(1,2) + corrMat(2, 1))/2;
        if corrvalue > lagestcorrValue
            lagestcorrValue = corrvalue;
            bestIdx = i;
        end
    end
end


%%*******************************************************************
%% schurmat_sblk: compute Schur complement matrix corresponding to
%%                SDP blocks.
%%
%% symm = 0, HKM
%%      = 1, NT
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*******************************************************************

   function schur = schurmat_sblk(blk,At,par,schur,p,X,Y);

   global iter smallblkdim  nnzschur  nzlistschur

   if isempty(smallblkdim); smallblkdim = 15; end
   if (nargin == 7); symm = 0; else; symm = 1; Y = X; end;
   m = length(schur);
   pblk = blk(p,:);
   if (iter == 1)
      nnzschur(size(blk,1),1) = m*m;
      nzlistschur = cell(size(blk,1),1);
   end
%%
   if (max(pblk{2}) > smallblkdim)
      %%
      %% compute schur for matrices that are very sparse.
      %%
      m1 = size(At{p,1},2);
      if issparse(schur); schur = full(schur); end;
      J = min(m1, max(find(par.nzlistA{p,1} < inf))-1);
      if (J > 0)
         if issparse(X{p}) & ~issparse(Y{p}); X{p} = full(X{p}); end
         if ~issparse(X{p}) & issparse(Y{p}); Y{p} = full(Y{p}); end
         if (iter <= 3)
            [nnzschur(p),nzlisttmp] = mexschur(pblk,At{p,1},par.nzlistA{p,1},...
            par.nzlistA{p,2},par.permA(p,:),Y{p},X{p},J,symm,schur);
            if (nnzschur(p) == mexnnz(nzlisttmp))
               nzlistschur{p} = nzlisttmp;
            else
               nzlistschur{p} = [];
            end
         else
            if isempty(nzlistschur{p})
               mexschur(pblk,At{p,1},par.nzlistA{p,1},...
               par.nzlistA{p,2},par.permA(p,:),Y{p},X{p},J,symm,schur);
            else
               mexschur(pblk,At{p,1},par.nzlistA{p,1},...
               par.nzlistA{p,2},par.permA(p,:),Y{p},X{p},J,symm,schur,nzlistschur{p});
            end
         end
      end
      %%
      %% compute schur for matrices that are not so sparse or dense.
      %%
      if (m1 < m)
         ss = [0, cumsum(pblk{3})];
         if (length(At(p,:)) > 2)
            dd = At{p,3};
         else
            dd = ones(sum(pblk{3}),1);
         end
         XVD = X{p}*At{p,2}*spdiags(dd,0,length(dd),length(dd));
         YVD = Y{p}*At{p,2}*spdiags(dd,0,length(dd),length(dd));
      end
      L = max(find(par.nzlistAsum{p,1} < inf)) -1;
      if (J < L)
         len = par.nzlistAsum{p,1}(J+1); list = par.nzlistAsum{p,2}(1:len,:);
      end
      if (m1 > 0)
         for k = J+1:m
            if (k<=m1)
               isspAk = par.isspA(p,k);
               Ak = mexsmat(blk,At,isspAk,p,k);
               if (k <= L)
                  idx1 = par.nzlistAsum{p,1}(k)+1; idx2 = par.nzlistAsum{p,1}(k+1);
                  list = [list; par.nzlistAsum{p,2}(idx1:idx2,:)];
                  list = sortrows(list,[2 1]);
                  tmp = Prod3(pblk,X{p},Ak,Y{p},symm,list);
               else
                  tmp = Prod3(pblk,X{p},Ak,Y{p},symm);
               end
            else
               idx = [ss(k-m1)+1 :ss(k-m1+1)];
               tmp = XVD(:,idx)* (Y{p}*At{p,2}(:,idx))';
            end
            if (~symm)
               tmp = 0.5*(mexsvec(pblk,tmp) + mexsvec(pblk,tmp,[],1));
            else
               tmp = mexsvec(pblk,tmp);
            end
            permk = par.permA(p,k);
            idx  = par.permA(p,1:min(k,m1));
            tmp2 = schur(idx,permk) + mexinprod(blk,At,tmp,min(k,m1),p);
            schur(idx,permk) = tmp2;
            schur(permk,idx) = tmp2';
         end
      end
      if (m1 < m)
         m2 = m - m1;
         YVtmp = At{p,2}'*YVD;
         XVtmp = XVD'*At{p,2};
         for k = 1:m2
            idx0 = [ss(k)+1 : ss(k+1)];
            tmp = XVtmp(:,idx0) .* YVtmp(:,idx0);
            tmp = tmp*ones(length(idx0),1);
            tmp3 = schur(m1+[1:m2],m1+k) + mexqops(pblk{3},tmp,ones(length(tmp),1),1);
            schur(m1+[1:m2],m1+k) = tmp3;
         end
      end
   else
      if issparse(X{p}) & ~issparse(Y{p}); Y{p} = sparse(Y{p}); end
      if ~issparse(X{p}) & issparse(Y{p}); X{p} = sparse(X{p}); end
      tmp  = mexskron(pblk,X{p},Y{p});
      Perm = spconvert([(1:m)' par.permA(p,:)' ones(m,1)]);
      schurtmp = At{p,1}'*tmp*At{p,1};
      schurtmp = 0.5*(schurtmp + schurtmp');
      schur = schur + Perm'*schurtmp*Perm;
   end
%%*******************************************************************


function val = rmGetVoxelData(param, vw, roi, varargin)
% rmGetVoxelData - Get Retinotopy Model data for a selected set of voxels.
%
% val = rmGetVoxelData(param, [view or RM model], [voxels=cur ROI], [options]);
%
%
% INPUTS:
%   param: name of the parameter to get for each voxel. Possible values:
%       'prf', 'rf': constructed 2D population receptive fields for each voxel.
%       If several voxels are specified, returns a 3D matrix in which the
%       slices correspond to the pRF for each voxel.
%
%		'prfvector', 'rfvector': same as 'prf', but each voxel's RF is
%		provided in a single vector format. The resulting matrix is size
%		pixels (in visual space) x voxels.
%
%       'x', 'x0': X position center of the pRF for the voxel.
%
%       'y', 'y0': Y position center of the pRF for the voxel.
%
%		'ecc', eccentricity of pRF center.
%
%		'pol', polar angle of pRF center.
%
%       'sigma', 'sigmamajor': pRF size of the first/only Gaussian.
%
%       'sigmaminor': pRF size of the second Gaussian, if it exists.
%
%       'theta': relative rotation of the major/minor axes.
%
%       'sig', 'log10p': significance level (-log10(p)) for the model for
%        each voxel.
%
%       'beta': scaling coefficient ("beta value") for the Gaussian pRF for
%       each voxel.
%
%		'amp': amplitude of the response. This is the scale factor
%		necessary to take a predicted response for each voxel, and fit it
%		to the time series. (This is the "beta value" for a GLM, constructed
%		from a pRF-derived esign matrix).
%
%
%   model: can either be a retinotopy model struct, or a mrVista view.
%   If the latter, grabs the first model loaded into the view (prompting
%   the user to select and RM file if none are loaded). Can also be
%   a number, indexing into loaded model structs. [Default: cur view
%   model]
%
%   voxels: ROI specification. Can be a numeric index into the view's ROIs
%   field, the ROI struct itself, a 3xN set of coordinates relative to the
%   view, or the ROI name of a loaded ROI. (Uses tc_roiStruct to
%   disambiguate these possibilities.) [Default: cur ROI of cur view]
%
% options include:
%   'rot', [value]: rotate RF params clockwise by [value] degrees. Ths will
%                   happen if the stimulus specification doesn't quite
%                   match the actual presentation regime. (Shouldn't
%                   happen, I know, but it seems to for some data sets, to
%                   a small degree.)
%   'xRange', [value]: range of X values for estimating the pRF. Defaults
%                   to -14:.2:14.
%   'yRange', [value]: same as for xRange, but along the Y axis.
%
%
% ras, 10/2006.
if notDefined('param'),     error('Need to specify param name.');	end
if notDefined('vw'),        vw = getCurView;						end
if notDefined('roi'),       roi = viewGet(vw, 'curROI');            end

% params/defaults
rot = 0;

% parse options
varargin = unNestCell(varargin); % allow recursive passing of options
for i = 1:length(varargin)
    if ischar(varargin{i})
        switch lower(varargin{i})
            case 'rot', rot = varargin{i+1};
            case 'x', X = varargin{i+1};
            case 'y', Y = varargin{i+1};
            case 'rmparams', rmParams = varargin{i+1};
        end
    end
end

% test for a view struct being passed in
if ~isfield(vw, 'x0') && isfield(vw, 'viewType')
    if ~checkfields(vw, 'rm', 'retinotopyModels')
        vw = rmSelect(vw, 1);
    end
    rmParams = vw.rm.retinotopyParams;
    model = vw.rm.retinotopyModels{1};
else
    % this is messy, need to reorganize...
    model = vw;
    vw = getCurView;
end

% disambiguate ROI sepecification
if ~isstruct(roi), roi = tc_roiStruct(vw, roi); end

% get indices of coords which correspond to selected voxels
if ismember(roi.viewType, {'Volume' 'Gray'})
    % get the indices corresponding to the ROI, preserving the voxel order
    % (this may take some time and memory for large ROIs)
    I = roiIndices(vw, roi.coords, 1);

    ok = find( ~isnan(I) );

else
    % not yet implemented for other view types
    error('Sorry, Not Yet Implemented.')
end

% plural/singular flexibility: ignore any 's' at the end of the param name
if lower(param(end))=='s'
    param = param(1:end-1);
end

% get the relevant value for the specified parameter
val = NaN(1, length(I));
switch lower(param)
    case {'amp' 'voxelamplitude' 'voxamp' 'glmbeta'}
        % compute the scale factor which maps from a normalized (-1 - 1
        % range) prediction to the fitted data. This runs a GLM for each
        % voxel in the ROI.
        %
        % I'm going to go ahead and assume vw is a view structure; it's too
        % hard to do this otherwise.
        [tSeries coords] = voxelTSeries(vw, roi.coords, [], 0, 0);

        pred = rmGetVoxelData('predictedtseries', vw, roi, varargin);

        % we called voxelTSeries with the preserveCoords flag set to zero.
        % This means the tSeries will have some voxels discarded (if
        % there's no data), and the coord order shuffled. Make sure the
        % order of predictions matches this order.
        [coords ok map2roi] = intersectCols(roi.coords, coords); %#ok<*ASGLU>
        pred = pred(:,ok);

        % take the prediction and make a design matrix (add trends terms)
        trends = rmMakeTrends(rmParams, prefsVerboseCheck);

        % initialize an empty output value
        val = NaN(1, length(I));

        % run a GLM for each non-NaN voxel, returning the scale factor
        for v = 1:length(ok)
            p = pred(:,v);
            if all(p==0) || all( isnan(p) )
                continue
            end
            p = p ./ abs(max(p));
            X = [p trends];
            [t df RSS B] = rmGLM(tSeries(:,v), X);

            % because of the sorting we introduced in voxelTSeries, the
            % order of tSeries may not match the order of our output amps
            % value. Map this back to the original order of the ROI coords.
            whichVoxel = map2roi(v);
            val(whichVoxel) = B(1);
        end

    case {'pred' 'prediction' 'predictedtserie'}
        % predicted time series of response based on the pRF for each voxel
        % and the stimulus.

        % we need pRF params for each voxel
        X = rmParams.analysis.X;
        Y = rmParams.analysis.Y;
        x0 = zeros(1, length(I));
        y0 = x0;
        sigma = x0;
        beta = x0;
        x0(ok) = model.x0(I(ok));
        y0(ok) = model.y0(I(ok));
        sigma(ok) = model.sigma.major(I(ok));
        beta(ok) = model.beta(1,I(ok),1);

        % rotation compensation if selected
        if rot ~= 0
            R = sqrt(x0.^2 + y0.^2);
            theta = atan2(y0, x0);
            theta = theta - deg2rad(rot);
            theta = mod(theta, 2*pi);
            x0 = R .* cos(theta);
            y0 = R .* sin(theta);
        end

        % initalize an empty output matrix
        nFrames = size(rmParams.analysis.allstimimages, 1);
        val = NaN(nFrames, length(I));

        for v = ok
            pRF = rfGaussian2D(X, Y, sigma(v), sigma(v), 0, x0(v), y0(v));
            val(:,v) = rmParams.analysis.allstimimages * pRF;
        end

    case {'prf' 'rf' 'prfs' 'rfs'}
        % we need to find a sampling grid for the pRF.
        if notDefined('X') || notDefined('Y')
            [X Y] = prfSamplingGrid(rmParams);
        end

        x0 = zeros(1, length(I));
        y0 = x0;
        sigma = x0;
        beta = x0;
        x0(ok) = model.x0(I(ok));
        y0(ok) = model.y0(I(ok));
        sigma(ok) = model.sigma.major(I(ok));
        beta(ok) = model.beta(1,I(ok),1);

        % rotation compensation if selected
        if rot ~= 0
            R = sqrt(x0.^2 + y0.^2);
            theta = atan2(y0, x0);
            theta = theta - deg2rad(rot);
            theta = mod(theta, 2*pi);
            x0 = R .* cos(theta);
            y0 = R .* sin(theta);
        end

        % re-initialize the return value as a matrix of NaNs
        val = NaN(size(X, 1), size(X, 2), length(I));

        for v = ok
            pRF = rfGaussian2D(X, Y, sigma(v), sigma(v), 0, x0(v), y0(v));
            val(:,:,v) = pRF;
        end

    case {'prfvector' 'rfvector' 'prfsvector' 'rfsvector' 'prfvectors' 'rfvectors'}
        % we need to find a sampling grid for the pRF.
        if notDefined('X') || notDefined('Y')
            [X Y] = prfSamplingGrid(rmParams);
        end

        x0 = zeros(1, length(I));
        y0 = x0;
        sigma = x0;
        beta = x0;
        x0(ok) = model.x0(I(ok));
        y0(ok) = model.y0(I(ok));
        sigma(ok) = model.sigma.major(I(ok));
        beta(ok) = model.beta(1,I(ok),1);

        % rotation compensation if selected
        if rot ~= 0
            R = sqrt(x0.^2 + y0.^2);
            theta = atan2(y0, x0);
            theta = theta - deg2rad(rot);
            theta = mod(theta, 2*pi);
            x0 = R .* cos(theta);
            y0 = R .* sin(theta);
        end

        % re-initialize the return value as a matrix of NaNs
        val = NaN(numel(X), length(I));

        for v = ok
            pRF = rfGaussian2D(X(:), Y(:), sigma(v), sigma(v), 0, x0(v), y0(v));
            val(:,v) = pRF;
        end

    case {'x' 'x0'}, val(ok) = model.x0(I(ok));

    case {'y' 'y0'}, val(ok) = model.y0(I(ok));

    case {'sigma' 'sigmamajor'}, val(ok) = model.sigma.major(I(ok));

    case {'sigmaminor'}, val(ok) = model.sigma.minor(I(ok));

    case {'theta'}, val(ok) = model.sigma.theta(I(ok));

    case {'beta'}, val(ok) = model.beta(1,I(ok),1);

    case {'sig' 'log10p' 'logp'}
        allsig = rmGet(model, 'log10p');
        val(ok) = allsig(I(ok));

    otherwise,
        try
            allvals = rmGet(model, param);
            val(ok) = allvals(I(ok));
        catch %#ok<*CTCH>
            fprintf('[%s] Unknown parameter name %s \n', mfilename, param);
        end

end

return


function [posr, trir, texr] = refine(pos, tri, method, varargin)

% REFINE a 3D surface that is described by a triangulation
%
% Use as
%   [pos, tri]          = refine(pos, tri)
%   [pos, tri]          = refine(pos, tri, 'banks')
%   [pos, tri, texture] = refine(pos, tri, 'banks', texture)
%   [pos, tri]          = refine(pos, tri, 'updown', numtri)
%
% If no method is specified, the default is to refine the mesh globally by bisecting
% each edge according to the algorithm described in Banks, 1983.
%
% The Banks method allows the specification of a subset of triangles to be refined
% according to Banks' algorithm. Adjacent triangles will be gracefully dealt with.
%
% The alternative 'updown' method refines the mesh a couple of times
% using Banks' algorithm, followed by a downsampling using the REDUCEPATCH
% function.
%
% If the textures of the vertices are specified, the textures for the new
% vertices are computed
%
% The Banks method is a memory efficient implementation which remembers the
% previously inserted vertices. The refinement algorithm executes in linear
% time with the number of triangles. It is mentioned in
% http://www.cs.rpi.edu/~flaherje/pdf/fea8.pdf, which also contains the original
% reference.

% Copyright (C) 2002-2014, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%

if nargin<3
  method = 'banks';
end

texture = [];
numtri  = [];

if nargin>3
  switch lower(method)
    case 'banks'
      texture = varargin{1};
    case 'updown'
      numtri = varargin{1};
  end % switch
end

switch lower(method)
  case 'banks'
    if ~isempty(texture)
      npnt   = size(pos,1);
      ntri   = size(tri,1);
      ntex   = size(texture,1);

      assert(ntex==npnt, 'invalid size of texture');

      insert = spalloc(3*npnt,3*npnt,3*ntri);
      trir  = zeros(4*ntri,3);      % allocate memory for the new triangles
      posr  = zeros(npnt+3*ntri,3); % allocate memory for the maximum number of new vertices
      texr  = zeros(ntex+3*ntri,2);
      posr(1:npnt,:) = pos;         % insert the original vertices
      texr(1:ntex,:) = texture;
      current = npnt;

      for i=1:ntri

        if ~insert(tri(i,1),tri(i,2))
          current = current + 1;
          posr(current,:) = (pos(tri(i,1),:) + pos(tri(i,2),:))/2;
          texr(current,:) = (texture(tri(i,1),:) + texture(tri(i,2),:))/2;
          insert(tri(i,1),tri(i,2)) = current;
          insert(tri(i,2),tri(i,1)) = current;
          v12 = current;
        else
          v12 = insert(tri(i,1),tri(i,2));
        end

        if ~insert(tri(i,2),tri(i,3))
          current = current + 1;
          posr(current,:) = (pos(tri(i,2),:) + pos(tri(i,3),:))/2;
          texr(current,:) = (texture(tri(i,2),:) + texture(tri(i,3),:))/2;
          insert(tri(i,2),tri(i,3)) = current;
          insert(tri(i,3),tri(i,2)) = current;
          v23 = current;
        else
          v23 = insert(tri(i,2),tri(i,3));
        end

        if ~insert(tri(i,3),tri(i,1))
          current = current + 1;
          posr(current,:) = (pos(tri(i,3),:) + pos(tri(i,1),:))/2;
          texr(current,:) = (texture(tri(i,3),:) + texture(tri(i,1),:))/2;
          insert(tri(i,3),tri(i,1)) = current;
          insert(tri(i,1),tri(i,3)) = current;
          v31 = current;
        else
          v31 = insert(tri(i,3),tri(i,1));
        end

        % add the 4 new triangles with the correct indices
        trir(4*(i-1)+1, :) = [tri(i,1) v12 v31];
        trir(4*(i-1)+2, :) = [tri(i,2) v23 v12];
        trir(4*(i-1)+3, :) = [tri(i,3) v31 v23];
        trir(4*(i-1)+4, :) = [v12 v23 v31];

      end
      posr = posr(1:current, :);
      texr = texr(1:current, :);

    else
      % there is no texture

      npnt   = size(pos,1);
      ntri   = size(tri,1);
      insert = spalloc(3*npnt,3*npnt,3*ntri);

      trir  = zeros(4*ntri,3);      % allocate memory for the new triangles
      posr  = zeros(npnt+3*ntri,3); % allocate memory for the maximum number of new vertices
      posr(1:npnt,:) = pos;         % insert the original vertices
      current = npnt;

      for i=1:ntri

        if ~insert(tri(i,1),tri(i,2))
          current = current + 1;
          posr(current,:) = (pos(tri(i,1),:) + pos(tri(i,2),:))/2;
          insert(tri(i,1),tri(i,2)) = current;
          insert(tri(i,2),tri(i,1)) = current;
          v12 = current;
        else
          v12 = insert(tri(i,1),tri(i,2));
        end

        if ~insert(tri(i,2),tri(i,3))
          current = current + 1;
          posr(current,:) = (pos(tri(i,2),:) + pos(tri(i,3),:))/2;
          insert(tri(i,2),tri(i,3)) = current;
          insert(tri(i,3),tri(i,2)) = current;
          v23 = current;
        else
          v23 = insert(tri(i,2),tri(i,3));
        end

        if ~insert(tri(i,3),tri(i,1))
          current = current + 1;
          posr(current,:) = (pos(tri(i,3),:) + pos(tri(i,1),:))/2;
          insert(tri(i,3),tri(i,1)) = current;
          insert(tri(i,1),tri(i,3)) = current;
          v31 = current;
        else
          v31 = insert(tri(i,3),tri(i,1));
        end

        % add the 4 new triangles with the correct indices
        trir(4*(i-1)+1, :) = [tri(i,1) v12 v31];
        trir(4*(i-1)+2, :) = [tri(i,2) v23 v12];
        trir(4*(i-1)+3, :) = [tri(i,3) v31 v23];
        trir(4*(i-1)+4, :) = [v12 v23 v31];
      end
      % remove the space for the vertices that was not used
      posr = posr(1:current, :);
    end

  case 'updown'
    ntri = size(tri,1);
    while ntri<numtri
      % increase the number of triangles by a factor of 4
      [pos, tri] = refine(pos, tri, 'banks');
      ntri = size(tri,1);
    end
    % reduce number of triangles using MATLAB function
    [trir, posr] = reducepatch(tri, pos, numtri);

  otherwise
    ft_error('unsupported method "%s"', method);
end


%Vehicle Car-like vehicle class
%
% This class models the kinematics of a car-like vehicle (bicycle model) on
% a plane that moves in SE(2).  For given steering and velocity inputs it
% updates the true vehicle state and returns noise-corrupted odometry
% readings.
%
% Methods::
%   init         initialize vehicle state
%   f            predict next state based on odometry
%   step         move one time step and return noisy odometry
%   control      generate the control inputs for the vehicle
%   update       update the vehicle state
%   run          run for multiple time steps
%   Fx           Jacobian of f wrt x
%   Fv           Jacobian of f wrt odometry noise
%   gstep        like step() but displays vehicle
%   plot         plot/animate vehicle on current figure
%   plot_xy      plot the true path of the vehicle
%   add_driver   attach a driver object to this vehicle
%   display      display state/parameters in human readable form
%   char         convert to string
%
% Class methods::
%   plotv        plot/animate a pose on current figure
%
% Properties (read/write)::
%   x               true vehicle state: x, y, theta (3x1)
%   V               odometry covariance (2x2)
%   odometry        distance moved in the last interval (2x1)
%   rdim             dimension of the robot (for drawing)
%   L               length of the vehicle (wheelbase)
%   alphalim        steering wheel limit
%   maxspeed        maximum vehicle speed
%   T               sample interval
%   verbose         verbosity
%   x_hist          history of true vehicle state (Nx3)
%   driver          reference to the driver object
%   x0              initial state, restored on init()
%
% Examples::
%
% Create a vehicle with odometry covariance
%       v = Vehicle( diag([0.1 0.01].^2 );
% and display its initial state
%       v
% now apply a speed (0.2m/s) and steer angle (0.1rad) for 1 time step
%       odo = v.update([0.2, 0.1])
% where odo is the noisy odometry estimate, and the new true vehicle state
%       v
%
% We can add a driver object
%      v.add_driver( RandomPath(10) )
% which will move the vehicle within the region -10<x<10, -10<y<10 which we
% can see by
%      v.run(1000)
% which shows an animation of the vehicle moving for 1000 time steps
% between randomly selected wayoints.
%
% Notes::
% - Subclasses the MATLAB handle class which means that pass by reference semantics
%   apply.
%
% Reference::
%
%   Robotics, Vision & Control, Chap 6
%   Peter Corke,
%   Springer 2011
%
% See also RandomPath, EKF.

% Copyright (C) 1993-2015, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

classdef Vehicle < handle

    properties
        % state
        x           % true state (x,y,theta)
        x_hist      % x history

        % parameters
        L           % length of vehicle
        alphalim    % steering wheel limit
        maxspeed    % maximum speed
        dim         % dimension of the world -dim -> +dim in x and y
        rdim    % dimension of the robot
        dt           % sample interval
        V           % odometry covariance

        odometry    % distance moved in last interval

        verbose

        driver      % driver object
        x0          % initial state
    end

    methods

        function veh = Vehicle(V, varargin)
        %Vehicle Vehicle object constructor
        %
        % V = Vehicle(V_ACT, OPTIONS)  creates a Vehicle object with actual odometry
        % covariance V_ACT (2x2) matrix corresponding to the odometry vector [dx dtheta].
        %
        % Options::
        % 'stlim',A       Steering angle limited to -A to +A (default 0.5 rad)
        % 'vmax',S        Maximum speed (default 5m/s)
        % 'L',L           Wheel base (default 1m)
        % 'x0',x0         Initial state (default (0,0,0) )
        % 'dt',T          Time interval
        % 'rdim',R        Robot size as fraction of plot window (default 0.2)
        % 'verbose'       Be verbose
        %
        % Notes::
        % - Subclasses the MATLAB handle class which means that pass by reference semantics
        %   apply.

            if ~isnumeric(V)
                error('first arg is V');
            end
            veh.x = zeros(3,1);
            if nargin < 1
                V = zeros(2,2);
            end

            opt.stlim = 0.5;
            opt.vmax = 5;
            opt.L = 1;
            opt.rdim = 0.2;
            opt.dt = 0.1;
            opt.x0 = zeros(3,1);
            opt = tb_optparse(opt, varargin);

            veh.V = V;

            veh.dt = opt.dt;
            veh.alphalim = opt.stlim;
            veh.maxspeed = opt.vmax;
            veh.L = opt.L;
            veh.x0 = opt.x0(:);
            veh.rdim = opt.rdim;
            veh.verbose = opt.verbose;

            veh.x_hist = [];
        end

        function init(veh, x0)
            %Vehicle.init Reset state of vehicle object
            %
            % V.init() sets the state V.x := V.x0, initializes the driver
            % object (if attached) and clears the history.
            %
            % V.init(X0) as above but the state is initialized to X0.
            if nargin > 1
                veh.x = x0(:);
            else
                veh.x = veh.x0;
            end
            veh.x_hist = [];
            if ~isempty(veh.driver)
                veh.driver.init()
            end
        end

        function add_driver(veh, driver)
            %Vehicle.add_driver Add a driver for the vehicle
            %
            % V.add_driver(D) connects a driver object D to the vehicle.  The driver
            % object has one public method:
            %        [speed, steer] = D.demand();
            % that returns a speed and steer angle.
            %
            % Notes::
            % - The Vehicle.step() method invokes the driver if one is attached.
            %
            % See also Vehicle.step, RandomPath.
            veh.driver = driver;
            driver.veh = veh;
        end

        function xnext = f(veh, x, odo, w)
            %Vehicle.f Predict next state based on odometry
            %
            % XN = V.f(X, ODO) is the predicted next state XN (1x3) based on current
            % state X (1x3) and odometry ODO (1x2) = [distance, heading_change].
            %
            % XN = V.f(X, ODO, W) as above but with odometry noise W.
            %
            % Notes::
            % - Supports vectorized operation where X and XN (Nx3).
            if nargin < 4
                w = [0 0];
            end

            dd = odo(1) + w(1); dth = odo(2);

            % straightforward code:
            % thp = x(3) + dth;
            % xnext = zeros(1,3);
            % xnext(1) = x(1) + (dd + w(1))*cos(thp);
            % xnext(2) = x(2) + (dd + w(1))*sin(thp);
            % xnext(3) = x(3) + dth + w(2);
            %
            % vectorized code:

            thp = x(:,3) + dth;
            xnext = x + [(dd+w(1))*cos(thp)  (dd+w(1))*sin(thp) ones(size(x,1),1)*dth+w(2)];
        end

        function odo = update(veh, u)
            %Vehicle.update Update the vehicle state
            %
            % ODO = V.update(U) is the true odometry value for
            % motion with U=[speed,steer].
            %
            % Notes::
            % - Appends new state to state history property x_hist.
            % - Odometry is also saved as property odometry.

            xp = veh.x; % previous state
            veh.x(1) = veh.x(1) + u(1)*veh.dt*cos(veh.x(3));
            veh.x(2) = veh.x(2) + u(1)*veh.dt*sin(veh.x(3));
            veh.x(3) = veh.x(3) + u(1)*veh.dt/veh.L * u(2);
            odo = [colnorm(veh.x(1:2)-xp(1:2)) veh.x(3)-xp(3)];
            veh.odometry = odo;

            veh.x_hist = [veh.x_hist; veh.x'];   % maintain history
        end

        function J = Fx(veh, x, odo)
        %Vehicle.Fx  Jacobian df/dx
        %
        % J = V.Fx(X, ODO) is the Jacobian df/dx (3x3) at the state X, for
        % odometry input ODO (1x2) = [distance, heading_change].
        %
        % See also Vehicle.f, Vehicle.Fv.
            dd = odo(1); dth = odo(2);
            thp = x(3) + dth;

            J = [
                1   0   -dd*sin(thp)
                0   1   dd*cos(thp)
                0   0   1
                ];
        end

        function J = Fv(veh, x, odo)
            %Vehicle.Fv  Jacobian df/dv
            %
            % J = V.Fv(X, ODO) is the Jacobian df/dv (3x2) at the state X, for
            % odometry input ODO (1x2) = [distance, heading_change].
            %
            % See also Vehicle.F, Vehicle.Fx.
            dd = odo(1); dth = odo(2);
            thp = x(3) + dth;

            J = [
                cos(thp)    -dd*sin(thp)
                sin(thp)    dd*cos(thp)
                0           1
                ];
        end

        function odo = step(veh, varargin)
            %Vehicle.step Advance one timestep
            %
            % ODO = V.step(SPEED, STEER) updates the vehicle state for one timestep
            % of motion at specified SPEED and STEER angle, and returns noisy odometry.
            %
            % ODO = V.step() updates the vehicle state for one timestep of motion and
            % returns noisy odometry.  If a "driver" is attached then its DEMAND() method
            % is invoked to compute speed and steer angle.  If no driver is attached
            % then speed and steer angle are assumed to be zero.
            %
            % Notes::
            % - Noise covariance is the property V.
            %
            % See also Vehicle.control, Vehicle.update, Vehicle.add_driver.

            % get the control input to the vehicle from either passed demand or driver
            u = veh.control(varargin{:});

            % compute the true odometry and update the state
            odo = veh.update(u);

            % add noise to the odometry
            if veh.V
                odo = veh.odometry + randn(1,2)*veh.V;
            end
        end

        function u = control(veh, speed, steer)
            %Vehicle.control Compute the control input to vehicle
            %
            % U = V.control(SPEED, STEER) is a control input (1x2) = [speed,steer]
            % based on provided controls SPEED,STEER to which speed and steering angle
            % limits have been applied.
            %
            % U = V.control() as above but demand originates with a "driver" object if
            % one is attached, the driver's DEMAND() method is invoked. If no driver is
            % attached then speed and steer angle are assumed to be zero.
            %
            % See also Vehicle.step, RandomPath.
            if nargin < 2
                % if no explicit demand, and a driver is attached, use
                % it to provide demand
                if ~isempty(veh.driver)
                    [speed, steer] = veh.driver.demand();
                else
                    % no demand, do something safe
                    speed = 0;
                    steer = 0;
                end
            end

            % clip the speed
            u(1) = min(veh.maxspeed, max(-veh.maxspeed, speed));

            % clip the steering angle
            u(2) = max(-veh.alphalim, min(veh.alphalim, steer));
        end

        function p = run(veh, nsteps)
            %Vehicle.run Run the vehicle simulation
            %
            % V.run(N) runs the vehicle model for N timesteps and plots
            % the vehicle pose at each step.
            %
            % P = V.run(N) runs the vehicle simulation for N timesteps and
            % return the state history (Nx3) without plotting.  Each row
            % is (x,y,theta).
            %
            % See also Vehicle.step.

            if nargin < 2
                nsteps = 1000;
            end

            %veh.clear();
            if ~isempty(veh.driver)
                veh.driver.visualize();
            end

            veh.visualize();
            for i=1:nsteps
                veh.step();
                if nargout == 0
                    % if no output arguments then plot each step
                    veh.plot();
                    drawnow
                end
            end
            p = veh.x_hist;
        end

        % TODO run and run2 should become superclass methods...

        function p = run2(veh, T, x0, speed, steer)
            %Vehicle.run2 Run the vehicle simulation with control inputs
            %
            % P = V.run2(T, X0, SPEED, STEER) runs the vehicle model for a time T with
            % speed SPEED and steering angle STEER.  P (Nx3) is the path followed and
            % each row is (x,y,theta).
            %
            % Notes::
            % - Faster and more specific version of run() method.
            % - Used by the RRT planner.
            %
            % See also Vehicle.run, Vehicle.step, RRT.
            veh.init(x0);

            for i=1:(T/veh.dt)
                veh.update([speed steer]);
            end
            p = veh.x_hist;
        end

        function h = plot(veh, varargin)
        %Vehicle.plot Plot vehicle
        %
        % V.plot(OPTIONS) plots the vehicle on the current axes at a pose given by
        % the current state.  If the vehicle has been previously plotted its
        % pose is updated.  The vehicle is depicted as a narrow triangle that
        % travels "point first" and has a length V.rdim.
        %
        % V.plot(X, OPTIONS) plots the vehicle on the current axes at the pose X.
        %
        % H = V.plotv(X, OPTIONS) draws a representation of a ground robot as an
        % oriented triangle with pose X (1x3) [x,y,theta].  H is a graphics handle.
        %
        % V.plotv(H, X) as above but updates the pose of the graphic represented
        % by the handle H to pose X.
        %
        % Options::
        % 'scale',S    Draw vehicle with length S x maximum axis dimension
        % 'size',S     Draw vehicle with length S
        % 'color',C    Color of vehicle.
        % 'fill'       Filled
        %
        % See also Vehicle.plotv.

            h = findobj(gcf, 'Tag', 'Vehicle.plot');
            if isempty(h)
                % no instance of vehicle graphical object found
                h = Vehicle.plotv(veh.x, varargin{:});
                set(h, 'Tag', 'Vehicle.plot');  % tag it
            end

            if ~isempty(varargin) && isnumeric(varargin{1})
                % V.plot(X)
                Vehicle.plotv(h, varargin{1}); % use passed value
            else
                % V.plot()
                Vehicle.plotv(h, veh.x);    % use current state
            end
        end

        function out = plot_xy(veh, varargin)
            %Vehicle.plot_xy Plots true path followed by vehicle
            %
            % V.plot_xy() plots the true xy-plane path followed by the vehicle.
            %
            % V.plot_xy(LS) as above but the line style arguments LS are passed
            % to plot.
            %
            % Notes::
            % - The path is extracted from the x_hist property.

            xyt = veh.x_hist;
            if nargout == 0
                plot(xyt(:,1), xyt(:,2), varargin{:});
            else
                out = xyt;
            end
        end

        function visualize(veh)
            grid on
        end

        function verbosity(veh, v)
        %Vehicle.verbosity Set verbosity
        %
        % V.verbosity(A) set verbosity to A.  A=0 means silent.
            veh.verbose = v;
        end

        function display(nav)
        %Vehicle.display Display vehicle parameters and state
        %
        % V.display() displays vehicle parameters and state in compact
        % human readable form.
        %
        % Notes::
        % - This method is invoked implicitly at the command line when the result
        %   of an expression is a Vehicle object and the command has no trailing
        %   semicolon.
        %
        % See also Vehicle.char.

            loose = strcmp( get(0, 'FormatSpacing'), 'loose');
            if loose
                disp(' ');
            end
            disp([inputname(1), ' = '])
            disp( char(nav) );
        end % display()

        function s = char(veh)
        %Vehicle.char Convert to a string
        %
        % s = V.char() is a string showing vehicle parameters and state in
        % a compact human readable format.
        %
        % See also Vehicle.display.

            s = 'Vehicle object';
            s = char(s, sprintf(...
            '  L=%g, maxspeed=%g, alphalim=%g, T=%f, nhist=%d', ...
                veh.L, veh.maxspeed, veh.alphalim, veh.dt, ...
                numrows(veh.x_hist)));
            if ~isempty(veh.V)
                s = char(s, sprintf(...
                '  V=(%g,%g)', ...
                    veh.V(1,1), veh.V(2,2)));
            end
            s = char(s, sprintf('  x=%g, y=%g, theta=%g', veh.x));
            if ~isempty(veh.driver)
                s = char(s, '  driven by::');
                s = char(s, [['    '; '    '] char(veh.driver)]);
            end
        end

    end % method

    methods(Static)

        function h_ = plotv(x, varargin)
        %Vehicle.plotv Plot ground vehicle pose
        %
        % H = Vehicle.plotv(X, OPTIONS) draws a representation of a ground robot as an
        % oriented triangle with pose X (1x3) [x,y,theta].  H is a graphics handle.
        % If X (Nx3) is a matrix it is considered to represent a trajectory in which case
        % the vehicle graphic is animated.
        %
        % Vehicle.plotv(H, X) as above but updates the pose of the graphic represented
        % by the handle H to pose X.
        %
        % Options::
        % 'scale',S    Draw vehicle with length S x maximum axis dimension
        % 'size',S     Draw vehicle with length S
        % 'color',C    Color of vehicle.
        % 'fill'       Filled with solid color as per 'color' option
        % 'fps',F      Frames per second in animation mode (default 10)
        %
        % Example::
        %
        % Generate some path 3xN
        %         p = PRM.plan(start, goal);
        % Set the axis dimensions to stop them rescaling for every point on the path
        %         axis([-5 5 -5 5]);
        %
        % Now invoke the static method
        %         Vehicle.plotv(p);
        %
        % Notes::
        % - This is a class method.
        %
        % See also Vehicle.plot.

            if isscalar(x) && ishandle(x)
                % plotv(h, x)
                h = x;
                x = varargin{1};
                x = x(:)';
                T = transl([x(1:2) 0]) * trotz( x(3) );
                set(h, 'Matrix', T);
                return
            end

            opt.scale = 1/60;
            opt.size = [];
            opt.fill = false;
            opt.color = 'r';
            opt.fps = 10;

            [opt,args] = tb_optparse(opt, varargin);

            lineprops = { 'Color', opt.color' };
            if opt.fill
                lineprops = [lineprops 'fill' opt.color ];
            end

            % compute the dimensions of the robot
            if ~isempty(opt.size)
                d = opt.size;
            else
                % get the current axes dimensions
                a = axis;
                d = (a(2)+a(4) - a(1)-a(3)) * opt.scale;
            end

            % draw it
            points = [
                d 0
                -d -0.6*d
                -d 0.6*d
            ]';

            h = hgtransform();
            hp = plot_poly(points, lineprops{:});
            for hh=hp
                set(hh, 'Parent', h);
            end

            if (numel(x) > 3) && (numcols(x) == 3)
                % animation mode
                for i=1:numrows(x)
                    T = transl([x(i,1:2) 0]) * trotz( x(i,3) );
                    set(h, 'Matrix', T);
                    pause(1/opt.fps);
                end
            elseif (numel(x) == 3)
                % compute the pose
                % convert vector form of pose to SE(3)

                x = x(:)';
                T = transl([x(1:2) 0]) * trotz( x(3) );
                set(h, 'Matrix', T);
            else
                error('bad pose');
            end

            if nargout > 0
                h_ = h;
            end
        end

    end % static methods

end % classdef


function [D, Isubj]=pons_cut_dir_adf(dirname,th_pval)
%         For all the subjects in the directory "dirname":
%           Computes the Dice coefficients D=2Nab/Na+Nb
%                           where:
%   Na is the volume of the Cerebellum obtrained trough the volume-based labeling
%     Nb is the volume "filled" obtained from the surface-based stream
%              Nab is the volume of the overlap
%

%
% pons_cut_dir_afd.m
%
% Original Author: Laurence Wastiaux
% CVS Revision Info:
%    $Author: nicks $
%    $Date: 2011/03/02 00:04:12 $
%    $Revision: 1.3 $
%
% Copyright © 2011 The General Hospital Corporation (Boston, MA) "MGH"
%
% Terms and conditions for use, reproduction, distribution and contribution
% are found in the 'FreeSurfer Software License Agreement' contained
% in the file 'LICENSE' found in the FreeSurfer distribution, and here:
%
% https://surfer.nmr.mgh.harvard.edu/fswiki/FreeSurferSoftwareLicense
%
% Reporting: freesurfer@nmr.mgh.harvard.edu
%

if (nargin<1 | nargin>2)
    msg=sprintf('USAGE: [D]=cc_cut_dir_adf(dirname, th_pval)');
    disp(msg)
end

%%% Get the table's directory %%%
if(getenv('FREESURFER_HOME'))
    fsh=getenv('FREESURFER_HOME');
    fsafdDir=strcat(fsh, '/fsafd');
else
    error(sprintf('Impossible to find FREESURFER_HOME\n'));
end

%%% Load stats from the Buckner data set %%%
%stat_file='/space/okapi/3/data/laurence/ADF/cutting_planes/PonsCutDice.adf';
stat_file=strcat(fsafdDir, '/PonsCutDice.adf');
fid=fopen(stat_file);
if(fid==-1)
    mess=sprintf('Could not find %s', stat_file);
    error(mess)
end
while(strfind(fgetl(fid), '#'))
    pos=ftell(fid);
end
fseek(fid, pos, 'bof');
DDpons=fscanf(fid, '%g');

files=dir(dirname);

D=[];
Isubj=[];
for i=1:length(files)
    SubjectDir=strcat(dirname,'/',files(i).name);
    %disp(SubjectDir)
    CorDir1=strcat(SubjectDir,'/mri/aseg/');
    CorDir2=strcat(SubjectDir,'/mri/filled/');
    d1=dir(CorDir1);
    d2=dir(CorDir2);
    if (length(d1)<3 | length(d2)<3 |(strcmp(files(i).name, '010128_2105')==1) |(strcmp(files(i).name, '010621_vc7110')==1) | ( length(strfind(files(i).name,'0'))==0 &&(length(strfind(files(i).name,'1'))==0 )))
    %if (length(d1)<3 | length(d2)<3 )
        i=i+1; % go to the next subject in the directory
    else
        %%% load the volumes aseg and wm %%%
    [vol1 M1 t1]=load_cor2(SubjectDir,'aseg');
    [vol2 M2 t2]=load_cor2(SubjectDir,'filled');

    if ((t1~=1) | (t2~=1))
        i=i+1;
    else
        %%% Compute the Dice coefficient %%%
        d=compute_dice(vol1,vol2);
        D=[D d];
        Isubj=[Isubj i];
        pval=compute_pval(d, DDpons);
        if (pval<th_pval)
            mess=sprintf('Cerebellum & Brain-stem not succesfully removed for subject: %s (dice=%.3g, pval=%.3g)', files(i).name, d, pval);
            disp(mess)
        else
            mess=sprintf('Cerebellum & Brain-stem succesfully removed for subject: %s (dice=%.3g, pval=%.3g)', files(i).name, d, pval);
            disp(mess)
        end
    end
    end
    vol1=[];
    vol2=[];
end

%%% sub-function [n]=compute_dice(vol1,vol2) %%%

function [n]=compute_dice(vol1,vol2)

Na=0;
Nb=0;
Nab=0;
sz=size(vol1);
sw=size(vol2);
if (sz==sw)
    for i=1:sz(1)
        for j=1:sz(2)
            for k=1:sz(3)
                if (vol1(i,j,k)==7 | vol1(i,j,k)==46 | vol1(i,j,k)==16)
                    Na=Na+1;
                end
                if (vol2(i,j,k)~=0)
                    Nb=Nb+1;
                end
                if ((vol1(i,j,k)==7 | vol1(i,j,k)==46 | vol1(i,j,k)==16) && (vol2(i,j,k)~=0))
                    Nab=Nab+1;
                end
            end
        end
    end
    n=2*Nab/(Na+Nb);
else
    mess=sprintf('The wm and aseg volumes do not have the same size');
    n=0;
    disp(mess)
end

% subfunction compute_pval() %
function [p_sup]=compute_pval(val, Dpons)
%load('/space/okapi/3/data/laurence/ADF/cutting_planes/Dice_pons.mat'); %loads Dpons
pas=0.05;
x=0:pas:1;
[h] = hist(Dpons,x);
p = h/sum(h);
dsup=find(x>val);
xsup=x(dsup);
psup=p(length(x)-length(xsup)+1:end);
if (val>=0 & length(xsup) >1)
    p_sup=trapz(xsup,psup)/pas;
elseif (val>=0 & (length(xsup)<2))
    pas2=pas/10;
    x2=0:pas2:1;
    [h2] = hist(Dpons,x2);
    p2 = h2/sum(h2);
    dsup2=find(x2>val);
    xsup2=x2(dsup2);
    psup2=p2(length(x2)-length(xsup2)+1:end);
    if(length(xsup2)>1)
        p_sup=trapz(xsup2,psup2)/pas2;
    else
        p_sup=0;
    end
else
    p_sup=0;
end