function M = elliptopefactory(n, k)
% Manifold of n-by-n psd matrices of rank k with unit diagonal elements.
%
% function M = elliptopefactory(n, k)
%
% A point X on the manifold is parameterized as YY^T where Y is a matrix of
% size nxk. As such, X is symmetric, positive semidefinite. We restrict to
% full-rank Y's, such that X has rank exactly k. The point X is numerically
% represented by Y (this is more efficient than working with X, which may
% be big). Tangent vectors are represented as matrices of the same size as
% Y, call them Ydot, so that Xdot = Y Ydot' + Ydot Y and diag(Xdot) == 0.
% The metric is the canonical Euclidean metric on Y.
%
% The diagonal constraints on X (X(i, i) == 1 for all i) translate to
% unit-norm constraints on the rows of Y: norm(Y(i, :)) == 1 for all i.
% The set of such Y's forms the oblique manifold. But because for any
% orthogonal Q of size k it holds that (YQ)(YQ)' = YY', we "group" all
% matrices of the form YQ in an equivalence class. The set of equivalence
% classes is a Riemannian quotient manifold, implemented here.
%
% Note that this geometry formally breaks down at rank-deficient Y's.
% This does not appear to be a major issue in practice when optimization
% algorithms converge to rank-deficient Y's, but convergence theorems no
% longer hold. As an alternative, you may use the oblique manifold (it has
% larger dimension, but does not break down at rank drop.)
%
% The geometry is taken from the 2010 paper:
% M. Journee, P.-A. Absil, F. Bach and R. Sepulchre,
% "Low-Rank Optimization on the Cone of Positive Semidefinite Matrices".
% Paper link: http://www.di.ens.fr/~fbach/journee2010_sdp.pdf
%
%
% Please cite the Manopt paper as well as the research paper:
%     @Article{journee2010low,
%       Title   = {Low-rank optimization on the cone of positive semidefinite matrices},
%       Author  = {Journ{\'e}e, M. and Bach, F. and Absil, P.-A. and Sepulchre, R.},
%       Journal = {SIAM Journal on Optimization},
%       Year    = {2010},
%       Number  = {5},
%       Pages   = {2327--2351},
%       Volume  = {20},
%       Doi     = {10.1137/080731359}
%     }
%
%
% See also: obliquefactory symfixedrankYYfactory spectrahedronfactory

% This file is part of Manopt: www.manopt.org.
% Original author: Bamdev Mishra, July 12, 2013.
% Contributors:
% Change log:
%   July 18, 2013 (NB):
%       Fixed projection operator for rank-deficient Y'Y.
%
%   Aug.  8, 2013 (NB):
%       No longer using nested functions, to aim at Octave compatibility.
%       Sign error in right hand side of the call to minres corrected.
%
%   June 24, 2014 (NB):
%       Used code snippets from obliquefactory to speed up projection,
%       retraction, egrad2rgrad and rand: the code now uses bsxfun for this.
%
%   Apr. 3, 2015 (NB):
%       Replaced trace(A'*B) by A(:)'*B(:) : equivalent but faster.
%
%   Apr. 17, 2018 (NB):
%       Removed dependency on lyap entirely.
%
%   Sep.  6, 2018 (NB):
%       Removed M.exp() as it was not implemented.

% TODO: modify normalize_rows and project_rows to work without transposes.
% TODO: enhance ehess2rhess to also use bsxfun.

    if k < 2
        warning('manopt:elliptopefactory:lowk', ...
                'k should be an integer >= 2. At k = 1, the set is discrete.');
    end

    M.name = @() sprintf('YY'' quotient manifold of %dx%d psd matrices of rank %d with diagonal elements being 1', n, k);

    M.dim = @() n*(k-1) - k*(k-1)/2; % Extra -1 is because of the diagonal constraint that

    % Euclidean metric on the total space
    M.inner = @(Y, eta, zeta) eta(:)'*zeta(:);

    M.norm = @(Y, eta) sqrt(M.inner(Y, eta, eta));

    M.dist = @(Y, Z) error('elliptopefactory.dist not implemented yet.');

    M.typicaldist = @() 10*k;

    M.proj = @projection;

    M.tangent = M.proj;
    M.tangent2ambient = @(Y, eta) eta;

    M.retr = @retraction;

    M.egrad2rgrad = @egrad2rgrad;

    M.ehess2rhess = @ehess2rhess;

    % Notice that the hash of two equivalent points will be different...
    M.hash = @(Y) ['z' hashmd5(Y(:))];

    M.rand = @() random(n, k);

    M.randvec = @randomvec;

    M.lincomb = @matrixlincomb;

    M.zerovec = @(Y) zeros(n, k);

    M.transp = @(Y1, Y2, d) projection(Y2, d);

    M.vec = @(Y, u_mat) u_mat(:);
    M.mat = @(Y, u_vec) reshape(u_vec, [n, k]);
    M.vecmatareisometries = @() true;

end

% Given a matrix X, returns the same matrix but with each column scaled so
% that they have unit 2-norm.
% See obliquefactory.
function X = normalize_rows(X)
    X = X';
    norms = sqrt(sum(X.^2, 1));
    X = bsxfun(@times, X, 1./norms);
    X = X';
end

% Orthogonal projection of each row of H to the tangent space at the
% corresponding row of X, seen as a point on a sphere.
% See obliquefactory.
function PXH = project_rows(X, H)
    X = X';
    H = H';
    % Compute the inner product between each vector H(:, i) with its root
    % point X(:, i), that is, X(:, i).' * H(:, i). Returns a row vector.
    inners = sum(X.*H, 1);
    % Subtract from H the components of the H(:, i)'s that are parallel to
    % the root points X(:, i).
    PXH = H - bsxfun(@times, X, inners);
    PXH = PXH';
end

% Projection onto the tangent space, i.e., on the tangent space of
% ||Y(i, :)|| = 1
function etaproj = projection(Y, eta)
    eta = project_rows(Y, eta);

    % Projection onto the horizontal space
    YtY = Y'*Y;
    SS = YtY;
    AS = Y'*eta - eta'*Y;
    Omega = lyapunov_symmetric(SS, AS);

    % It does not seem necessary to enforce skew-symmetry numerically.
    % Omega = (Omega-Omega')/2;

    etaproj = eta - Y*Omega;
end

% Retraction
function Ynew = retraction(Y, eta, t)
    if nargin < 3
        t = 1.0;
    end
    Ynew = Y + t*eta;
    Ynew = normalize_rows(Ynew);
end

% Euclidean gradient to Riemannian gradient conversion.
% We only need the ambient space projection: the remainder of the
% projection function is not necessary because the Euclidean gradient must
% already be orthogonal to the vertical space.
function rgrad = egrad2rgrad(Y, egrad)
    rgrad = project_rows(Y, egrad);
end

% Euclidean Hessian to Riemannian Hessian conversion.
% TODO: speed this function up using bsxfun.
function Hess = ehess2rhess(Y, egrad, ehess, eta)
    k = size(Y, 2);

    % Directional derivative of the Riemannian gradient
    scaling_grad = sum((egrad.*Y), 2); % column vector of size n
    scaling_grad_repeat = scaling_grad*ones(1, k);

    Hess = ehess - scaling_grad_repeat.*eta;

    scaling_hess = sum((eta.*egrad) + (Y.*ehess), 2);
    scaling_hess_repeat = scaling_hess*ones(1, k);
    % directional derivative of scaling_grad_repeat
    Hess = Hess - scaling_hess_repeat.*Y;

    % Project on the horizontal space
    Hess = projection(Y, Hess);
end

% Random point generation on the manifold
function Y = random(n, k)
    Y = randn(n, k);
    Y = normalize_rows(Y);
end

% Random vector generation at Y
function eta = randomvec(Y)
    eta = randn(size(Y));
    eta = projection(Y, eta);
    nrm = norm(eta, 'fro');
    eta = eta / nrm;
end


src = gtzan_src('/path/to/gtzan');

N = 5*2^17;

filt1_opt.Q = 8;
filt1_opt.J = 80;
filt1_opt.gabor = 1;

filt2_opt.Q = 1;
filt2_opt.J = 13;

filters1 = morlet_filter_bank_1d(N,filt1_opt);
filters2 = morlet_filter_bank_1d(N,filt2_opt);

scatt_fun = @(x)(scatt_1d(x,{@(x)(wavemod_1d(x,filters1)),@(x)(wavemod_1d(x,filters2)),@(x)(wavemod_1d(x,filters2))}));
scatt_fun = @(x)(format_scatt(log_scatt(renorm_scatt(scatt_fun(x))),'table'));

db = prepare_database(src,{scatt_fun});

[train_set,test_set] = create_partition(src);

model = affine_train(db,train_set);
labels = affine_test(db,model,test_set);

err = classif_err(labels,test_set,src);


    function [ ] = showNoisyStatistics(noisyStats)
    %% Plot statistics for a dataset collection based on single noisy structure

    %% Extract the fields
    collectionTitle = noisyStats.collectionTitle;
    statisticsTitles = noisyStats.statisticsTitles;
    s = noisyStats.statisticsIndex;
    statistics = noisyStats.statistics;
    divideColor = [0.85, 0.85, 0.85];

    %% Plot median versus SDR overall channel deviation
    minDev = min(statistics(:, s.medDev));
    maxDev = max(statistics(:, s.medDev));
    minrSD = min(statistics(:, s.rSDDev));
    maxrSD = max(statistics(:, s.rSDDev));
    baseTitle = 'Median versus SDR overall channel deviation';
    figure ('Name', baseTitle, 'Color', [1, 1, 1]);
    hold on
    plot(statistics(:, s.medDev), statistics(:, s.rSDDev), 'ok')
    xlabel(statisticsTitles{s.medDev});
    ylabel(statisticsTitles{s.rSDDev});
    title(collectionTitle)
    medDev = median(statistics(:, s.medDev));
    rSDDev = median(statistics(:, s.rSDDev));
    plot(medDev, rSDDev, '+r', 'MarkerSize', 14, 'LineWidth', 3);
    set(gca, 'XLim', [minDev, maxDev], 'XLimMode', 'manual', ...
        'YLim', [minrSD, maxrSD], 'YLimMode', 'manual');
    title({collectionTitle; baseTitle; ...
        ['[Median of dataset medians: ' num2str(medDev) '(med dev) ' ...
        num2str(rSDDev) '(rSD dev)]']}, 'interpreter', 'none');
    legend('Dataset', 'Collection median', 'Location', 'NorthWest')
    box on
    hold off

    %% Plot median versus SDR window channel deviation
    minWinDev = min(statistics(:, s.medWinDev));
    maxWinDev = max(statistics(:, s.medWinDev));
    minWinrSD = min(statistics(:, s.rSDWinDev));
    maxWinrSD = max(statistics(:, s.rSDWinDev));
    baseTitle = 'Median versus SDR window channel deviation';
    figure ('Name', baseTitle, 'Color', [1, 1, 1]);
    hold on
    plot(statistics(:, s.medWinDev), statistics(:, s.rSDWinDev), 'ok')
    xlabel(statisticsTitles{s.medWinDev});
    ylabel(statisticsTitles{s.rSDWinDev});
    title(collectionTitle)
    medWinDev = median(statistics(:, s.medWinDev));
    rSDWinDev = median(statistics(:, s.rSDWinDev));
    plot(medWinDev, rSDWinDev, '+r', 'MarkerSize', 14, 'LineWidth', 3);
    line([minWinDev; maxWinDev], [minWinrSD; maxWinrSD], 'LineWidth', 3, ...
        'Color', divideColor);
    set(gca, 'XLim', [minWinDev, maxWinDev], 'XLimMode', 'manual', ...
        'YLim', [minWinrSD, maxWinrSD], 'YLimMode', 'manual');
    title({collectionTitle; baseTitle; ...
        ['[Median of dataset medians: ' num2str(medWinDev) '(med dev) ' ...
        num2str(rSDWinDev) '(rSD dev)]']}, 'interpreter', 'none');
    legend('Dataset', 'Collection median', 'Location', 'NorthWest')
    box on
    hold off

%% Plot median maximum correlation versus median window channel deviation
    minWinDev = min(statistics(:, s.medWinDev));
    maxWinDev = max(statistics(:, s.medWinDev));
    minCorr = min(statistics(:, s.medCor));
    baseTitle = 'Median maximum correlation versus median window channel deviation';
    figure ('Name', baseTitle, 'Color', [1, 1, 1]);
    hold on
    plot(statistics(:, s.medCor), statistics(:, s.medWinDev), 'ok')

    xlabel(statisticsTitles{s.medCor});
    ylabel(statisticsTitles{s.medWinDev});
    medWinDev = median(statistics(:, s.medWinDev));
    medCorr = median(statistics(:, s.medCor));
    plot(medCorr, medWinDev, '+r', 'MarkerSize', 14, 'LineWidth', 3);
    set(gca, 'XLim', [minCorr, 1], 'XLimMode', 'manual', ...
        'YLim', [minWinDev, maxWinDev], 'YLimMode', 'manual');
    title({collectionTitle; baseTitle; ...
        ['[Median of dataset medians: ' num2str(medCorr) '(corr) ' ...
        num2str(medWinDev) '(med dev)]']}, 'interpreter', 'none');
    legend('Dataset', 'Collection median', 'Location', 'NorthWest')
    box on
    hold off

    %% Plot median maximum correlation versus window channel deviation ratio
    devRatio = statistics(:, s.rSDWinDev)./statistics(:, s.medWinDev);
    minWinRatio = min(devRatio);
    maxWinRatio = max(devRatio);
    yLabelString = 'rSDR/median window channel deviation';
    minCorr = min(statistics(:, s.medCor));
    baseTitle = 'Median maximum correlation versus window channel deviation ratio';
    figure ('Name', baseTitle, 'Color', [1, 1, 1]);
    hold on
    plot(statistics(:, s.medCor), devRatio, 'ok')
    xlabel(statisticsTitles{s.medCor});
    ylabel(yLabelString);
    medRatio = median(devRatio);
    medCorr = median(statistics(:, s.medCor));
    plot(medCorr, medRatio, '+r', 'MarkerSize', 14, 'LineWidth', 3);
    set(gca, 'XLim', [minCorr, 1], 'XLimMode', 'manual', ...
        'YLim', [minWinRatio, maxWinRatio], 'YLimMode', 'manual');
    title({collectionTitle; baseTitle; ...
        ['[Median of dataset medians: ' num2str(medCorr) '(corr) ' ...
        num2str(medRatio) '(rSD/med dev)]']}, 'interpreter', 'none');
    legend('Dataset', 'Collection median', 'Location', 'NorthWest')
    box on
    hold off

%% Plot average maximum correlation versus median window channel deviation
    minWinDev = min(statistics(:, s.medWinDev));
    maxWinDev = max(statistics(:, s.medWinDev));
    minCorr = min(statistics(:, s.medCor));
    baseTitle = 'Average maximum correlation versus median window channel deviation';
    figure ('Name', baseTitle, 'Color', [1, 1, 1]);
    hold on
    plot(statistics(:, s.aveCor), statistics(:, s.medWinDev), 'ok')

    xlabel(statisticsTitles{s.aveCor});
    ylabel(statisticsTitles{s.medWinDev});
    medWinDev = median(statistics(:, s.medWinDev));
    aveCorr = median(statistics(:, s.aveCor));
    plot(aveCorr, medWinDev, '+r', 'MarkerSize', 14, 'LineWidth', 3);
    set(gca, 'XLim', [minCorr, 1], 'XLimMode', 'manual', ...
        'YLim', [minWinDev, maxWinDev], 'YLimMode', 'manual');
    title({collectionTitle; baseTitle; ...
        ['[Median of dataset medians: ' num2str(aveCorr) '(corr) ' ...
        num2str(medWinDev) '(med dev)]']}, 'interpreter', 'none');
    legend('Dataset', 'Collection median', 'Location', 'NorthWest')
    box on
    hold off

    %% Plot average maximum correlation versus window channel deviation ratio
    devRatio = statistics(:, s.rSDWinDev)./statistics(:, s.medWinDev);
    minWinRatio = min(devRatio);
    maxWinRatio = max(devRatio);
    yLabelString = 'rSDR/median window channel deviation';
    minCorr = min(statistics(:, s.aveCor));
    baseTitle = 'Average maximum correlation versus window channel deviation ratio';
    figure ('Name', baseTitle, 'Color', [1, 1, 1]);
    hold on
    plot(statistics(:, s.aveCor), devRatio, 'ok')
    xlabel(statisticsTitles{s.aveCor});
    ylabel(yLabelString);
    medRatio = median(devRatio);
    aveCorr = median(statistics(:, s.aveCor));
    plot(aveCorr, medRatio, '+r', 'MarkerSize', 14, 'LineWidth', 3);
    set(gca, 'XLim', [minCorr, 1], 'XLimMode', 'manual', ...
        'YLim', [minWinRatio, maxWinRatio], 'YLimMode', 'manual');
    title({collectionTitle; baseTitle; ...
        ['[Median of dataset medians: ' num2str(aveCorr) '(corr) ' ...
        num2str(medRatio) '(rSD/med dev)]']}, 'interpreter', 'none');
    legend('Dataset', 'Collection median', 'Location', 'NorthWest')
    box on
    hold off

    %% Plot median versus SDR overall HF noise
    minHF = min(statistics(:, s.medHF));
    maxHF = max(statistics(:, s.medHF));
    minrSD = min(statistics(:, s.rSDHF));
    maxrSD = max(statistics(:, s.rSDHF));
    baseTitle = 'Median versus SDR overall channel HF noise';
    figure ('Name', baseTitle, 'Color', [1, 1, 1]);
    hold on
    plot(statistics(:, s.medHF), statistics(:, s.rSDHF), 'ok')
    xlabel(statisticsTitles{s.medHF});
    ylabel(statisticsTitles{s.rSDHF});
    title(collectionTitle)
    medHF = median(statistics(:, s.medHF));
    rSDHF = median(statistics(:, s.rSDHF));
    plot(medHF, rSDHF, '+r', 'MarkerSize', 14, 'LineWidth', 3);
    set(gca, 'XLim', [minHF, maxHF], 'XLimMode', 'manual', ...
        'YLim', [minrSD, maxrSD], 'YLimMode', 'manual');
    title({collectionTitle; baseTitle; ...
        ['[Median of dataset medians: ' num2str(medHF) '(med HF) ' ...
        num2str(rSDDev) '(rSD HF)]']}, 'interpreter', 'none');
    legend('Dataset', 'Collection median', 'Location', 'NorthWest')
    box on
    hold off

    %% Plot median versus SDR window channel HF noise
    minWinHF = min(statistics(:, s.medWinHF));
    maxWinHF = max(statistics(:, s.medWinHF));
    minWinrSD = min(statistics(:, s.rSDWinHF));
    maxWinrSD = max(statistics(:, s.rSDWinHF));
    baseTitle = 'Median versus SDR window channel HF noise';
    figure ('Name', baseTitle, 'Color', [1, 1, 1]);
    hold on
    plot(statistics(:, s.medWinHF), statistics(:, s.rSDWinHF), 'ok')
    xlabel(statisticsTitles{s.medWinHF});
    ylabel(statisticsTitles{s.rSDWinHF});
    title(collectionTitle)
    medWinHF = median(statistics(:, s.medWinHF));
    rSDWinHF = median(statistics(:, s.rSDWinHF));
    plot(medWinHF, rSDWinHF, '+r', 'MarkerSize', 14, 'LineWidth', 3);
    set(gca, 'XLim', [minWinHF, maxWinHF], 'XLimMode', 'manual', ...
        'YLim', [minWinrSD, maxWinrSD], 'YLimMode', 'manual');
    title({collectionTitle; baseTitle; ...
        ['[Median of dataset medians: ' num2str(medWinHF) '(med HF) ' ...
        num2str(rSDWinHF) '(rSD HF)]']}, 'interpreter', 'none');
    legend('Dataset', 'Collection median', 'Location', 'NorthWest')
    box on
    hold off

%% Plot median maximum correlation versus median window channel HF noise
    minWinHF = min(statistics(:, s.medWinHF));
    maxWinHF = max(statistics(:, s.medWinHF));
    minCorr = min(statistics(:, s.medCor));
    baseTitle = 'Median maximum correlation versus median window channel HF noise';
    figure ('Name', baseTitle, 'Color', [1, 1, 1]);
    hold on
    plot(statistics(:, s.medCor), statistics(:, s.medWinHF), 'ok')

    xlabel(statisticsTitles{s.medCor});
    ylabel(statisticsTitles{s.medWinHF});
    medWinHF = median(statistics(:, s.medWinHF));
    medCorr = median(statistics(:, s.medCor));
    plot(medCorr, medWinHF, '+r', 'MarkerSize', 14, 'LineWidth', 3);
    set(gca, 'XLim', [minCorr, 1], 'XLimMode', 'manual', ...
        'YLim', [minWinHF, maxWinHF], 'YLimMode', 'manual');
    title({collectionTitle; baseTitle; ...
        ['[Median of dataset medians: ' num2str(medCorr) '(corr) ' ...
        num2str(medWinHF) '(med HF)]']}, 'interpreter', 'none');
    legend('Dataset', 'Collection median', 'Location', 'NorthWest')
    box on
    hold off

    %% Plot median maximum correlation versus window channel HF ratio
    HFRatio = statistics(:, s.rSDWinHF)./statistics(:, s.medWinHF);
    minWinRatio = min(HFRatio);
    maxWinRatio = max(HFRatio);
    yLabelString = 'rSDR/median window channel HF noise';
    minCorr = min(statistics(:, s.medCor));
    baseTitle = 'Median maximum correlation versus window channel HF noise ratio';
    figure ('Name', baseTitle, 'Color', [1, 1, 1]);
    hold on
    plot(statistics(:, s.medCor), HFRatio, 'ok')
    xlabel(statisticsTitles{s.medCor});
    ylabel(yLabelString);
    medRatio = median(HFRatio);
    medCorr = median(statistics(:, s.medCor));
    plot(medCorr, medRatio, '+r', 'MarkerSize', 14, 'LineWidth', 3);
    set(gca, 'XLim', [minCorr, 1], 'XLimMode', 'manual', ...
        'YLim', [minWinRatio, maxWinRatio], 'YLimMode', 'manual');
    title({collectionTitle; baseTitle; ...
        ['[Median of dataset medians: ' num2str(medCorr) '(corr) ' ...
        num2str(medRatio) '(rSD/med HF)]']}, 'interpreter', 'none');
    legend('Dataset', 'Collection median', 'Location', 'NorthWest')
    box on
    hold off

%% Plot average maximum correlation versus median window channel deviation
    minWinHF = min(statistics(:, s.medWinHF));
    maxWinHF = max(statistics(:, s.medWinHF));
    minCorr = min(statistics(:, s.medCor));
    baseTitle = 'Average maximum correlation versus median window HF noise';
    figure ('Name', baseTitle, 'Color', [1, 1, 1]);
    hold on
    plot(statistics(:, s.aveCor), statistics(:, s.medWinHF), 'ok')

    xlabel(statisticsTitles{s.aveCor});
    ylabel(statisticsTitles{s.medWinHF});
    medWinHF = median(statistics(:, s.medWinHF));
    aveCorr = median(statistics(:, s.aveCor));
    plot(aveCorr, medWinHF, '+r', 'MarkerSize', 14, 'LineWidth', 3);
    set(gca, 'XLim', [minCorr, 1], 'XLimMode', 'manual', ...
        'YLim', [minWinHF, maxWinHF], 'YLimMode', 'manual');
    title({collectionTitle; baseTitle; ...
        ['[Median of dataset medians: ' num2str(aveCorr) '(corr) ' ...
        num2str(medWinHF) '(med HF)]']}, 'interpreter', 'none');
    legend('Dataset', 'Collection median', 'Location', 'NorthWest')
    box on
    hold off

    %% Plot average maximum correlation versus window channel HF noise ratio
    devRatio = statistics(:, s.rSDWinHF)./statistics(:, s.medWinHF);
    minWinRatio = min(devRatio);
    maxWinRatio = max(devRatio);
    yLabelString = 'rSDR/median window channel HF noise';
    minCorr = min(statistics(:, s.aveCor));
    baseTitle = 'Average maximum correlation versus window channel HF noise ratio';
    figure ('Name', baseTitle, 'Color', [1, 1, 1]);
    hold on
    plot(statistics(:, s.aveCor), devRatio, 'ok')
    xlabel(statisticsTitles{s.aveCor});
    ylabel(yLabelString);
    medRatio = median(devRatio);
    aveCorr = median(statistics(:, s.aveCor));
    plot(aveCorr, medRatio, '+r', 'MarkerSize', 14, 'LineWidth', 3);
    set(gca, 'XLim', [minCorr, 1], 'XLimMode', 'manual', ...
        'YLim', [minWinRatio, maxWinRatio], 'YLimMode', 'manual');
    title({collectionTitle; baseTitle; ...
        ['[Median of dataset medians: ' num2str(aveCorr) '(corr) ' ...
        num2str(medRatio) '(rSD/med HF)]']}, 'interpreter', 'none');
    legend('Dataset', 'Collection median', 'Location', 'NorthWest')
    box on
    hold off


function varargout = linop_test( op, cmode, maxits )

%LINOP_TEST Performs an adjoint test on a linear operator.
%    LINOP_TEST( OP ) attempts to verify that a linear operator OP obeys
%    the inner product test: <A*x,y> = <x,A'*y> for all x, y. OP must be a
%    TFOCS linear operator with hard-coded size information; that is,
%    OP([],0) must return valid size info.
%
%    When called with a single argument, LINOP_TEST creates real test
%    vectors for X and Y. To test complex operators, use the two-argument
%    version LINOP_TEST( OP, cmode ), where:
%        cmode = 'R2R': real input, real output
%        cmode = 'R2C': real input, complex output
%        cmode = 'R2CC': real input, conjugate-symmetric complex output
%        cmode = 'C2R': complex input, real output
%        cmode = 'CC2R': conjugate-symmetric complex input, real output
%        cmode = 'C2C': complex input, complex output
%
%    The conjugate-symmetric options follow the symmetry conventions of Matlab's FFT.
%
%    LINOP_TEST( OP, CMODE, MAXITS ) performs MAXITS iterations of the
%    test loop. MAXITS=25 is the default.
%
%   myNorm = LINOP_TEST(...) returns an estimate of the myNorm of
%       the linear operator.

error(nargchk(1,3,nargin));
if isnumeric( op ),
    if nargin < 2 || isempty( cmode )
        disp('warning: for matrix inputs, this assumes the cmode is ''C2C'' unless you specify otherwise');
        cmode = 'C2C';
    end
    op = linop_matrix( op, cmode );
end
y_conjSymmetric     = false;
x_conjSymmetric     = false;
if nargin < 2 || isempty( cmode ),
    x_real = true;
    y_real = true;
else
    switch upper( cmode ),
        case 'R2R', x_real = true; y_real = true;
        case 'R2C', x_real = true; y_real = false;
        case 'R2CC', x_real = true; y_real = false;  y_conjSymmetric    = true;
        case 'C2R', x_real = false; y_real = true;
        case 'CC2R', x_real = false; y_real = true;  x_conjSymmetric    = true;
        case 'C2C', x_real = false; y_real = false;
        case 'CC2CC', x_real = false; y_real = false;
            x_conjSymmetric    = true;
            y_conjSymmetric    = true; % this is probably never going to happen though
        otherwise, error( 'Invalid cmode: %s', cmode );
    end
end
if nargin < 3 || isempty(maxits),
    maxits = 25;
end
sz = op([],0);
if ~iscell(sz)
    sz = { [sz(2),1], [sz(1),1] };
elseif isempty(sz{1})
    disp('warning: could not detect the size; this often happens when using a scalar input, which represents scaling');
    if isempty( sz{2} )
        disp('  Proceeding under the assumption that the domain is 1D');
        sz{1} = [1,1];
        sz{2} = [1,1];
    else
        disp('  Proceeding under the assumption that the domain equals the range');
        sz{1}   = sz{2};
    end
elseif isempty(sz{2})
    disp('warning: could not detect the size; this often happens when using a scalar input, which represents scaling');
    disp('  Proceeding under the assumption that the domain equals the range');
    sz{2}   = sz{1};
end
nf = 0;
na = 0;
errs = zeros(1,maxits+1);
nxe = 0; nye = 0;
for k = 1 : maxits,

    %
    % The adjoint test
    %

    if x_real,
        x = randn(sz{1});
    else
        x = randn(sz{1})+1j*randn(sz{1});
        if x_conjSymmetric
            x = make_conj_symmetrix( x );
        end
    end

    if y_real,
        y = randn(sz{2});
    else
        y = randn(sz{2})+1j*randn(sz{2});
        if y_conjSymmetric
            y = make_conj_symmetrix( y );
        end
    end

    nx = myNorm(x);
    Ax = op(x,1);
    nf = max( nf, myNorm(Ax)/nx );
    Ax_y = tfocs_dot( Ax, y );

    ny = myNorm(y);
    Ay = op(y,2);
    na = max( na, myNorm(Ay) / ny );
    Ay_x = tfocs_dot( x, Ay );

    errs(k) = abs(Ax_y-Ay_x)/(nx*ny);

    %
    % The myNorm iteration
    %

    if nxe == 0,
        if x_real,
            xx = randn(sz{1});
        else
            xx = randn(sz{1}) + 1j*randn(sz{1});
            if x_conjSymmetric
                xx = make_conj_symmetrix( xx );
            end
        end
        nxe = myNorm(xx);
    end
    yy = op(xx/nxe,1);
    nye = max(realmin,myNorm(yy));
    xx = op(yy/nye,2);
    nxe = myNorm(xx);

end

%
% Use the estimated singular vectors for a final adjoint est
%

if nxe > 0,
    Ax_y = tfocs_dot( op(xx,1), yy );
    Ay_x = tfocs_dot( op(yy,2), xx );
    errs(end) = abs(Ax_y-Ay_x) / (nxe*nye);
end

%
% Display the output
%

nmax = max(nye,nxe);
myNorm_err = abs(nye-nxe) / nmax;
peak_err = max(errs) / nmax;
mean_err = mean(errs) / nmax;
rc = { 'complex', 'real', 'complex symmetric' };
fprintf( 'TFOCS linear operator test:\n' );
fprintf( '   Input size:  [' ); fprintf( ' %d', sz{1} ); fprintf( ' ], %s\n', rc{x_real+1+2*x_conjSymmetric} );
fprintf( '   Output size: [' ); fprintf( ' %d', sz{2} ); fprintf( ' ], %s\n', rc{y_real+1+2*y_conjSymmetric} );
fprintf( 'After %d iterations:\n', maxits  );
fprintf( '    myNorm estimates (forward/adjoint/error): %g/%g/%g\n', nye, nxe, myNorm_err );
fprintf( '       Gains: forward %g, adjoint %g\n', nf, na );
fprintf( '    Inner product error:\n' );
fprintf( '       Mean (absolute/relative): %g/%g\n', mean(errs), mean_err );
fprintf( '       Peak (absolute/relative): %g/%g\n', max(errs), peak_err );
fprintf( '       (inner product errors should 1e-10 or smaller)\n');

good = true;
if myNorm_err/max( nye, nxe ) > 1e-4
    fprintf('  Detected mismatch in forward/adjoint norm estimates. This is potentially a bad sign. Check your implementation\n');
    good = false;
end
if mean_err > 1e-8
    fprintf('  The mean error (relative) is high. This is a bad sign. Check your implementation\n');
    good = false;
end
if good
    fprintf('  Allowing for some roundoff error, there are no obvious errors. This is good.\n');
end

if nargout > 0
    varargout{1} = mean([nye,nxe]);
end

% Improvement as suggested by Graham Coleman
% Allows for 3D arrays.
% This also changes default behavior of 2D arrays
% to now use the Frobenius norm instead of spectral norm.
% This is wise, since it's a much quicker computation.
function y = myNorm(x)
y = norm( x(:) );

function y = make_conj_symmetrix( y )
ny = size( y, 1 );
y(1,:)  = real(y(1,:));         % DC component is 0
if round(ny/2) == ny/2  % even
    y(ny/2+1,:)     = real(y(ny/2+1,:));    % Nyquist component is 0
    y( ny:-1:(ny/2+2) )     = conj( y(2:ny/2) );
else                    % odd
    y( ny:-1:((ny+1)/2+1) ) = conj( y(2:((ny+1)/2)) );
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function z = maxdiff(x, y)
% Written by Mo Chen (sth4nth@gmail.com).
assert(all(size(x)==size(y)));
z = max(abs(x(:)-y(:)));


function [poly, keepInds] = simplifyPolygon(poly, varargin)
%SIMPLIFYPOLYGON  Douglas-Peucker simplification of a polygon
%
%   POLY2 = simplifyPolygon(POLY, TOL)
%   Simplifies the input polygon using the Douglas-Peucker algorithm.
%
%   Example
%     elli = [20 30 40 20 30];
%     poly = ellipseToPolygon(elli, 500);
%     poly2 = simplifyPolygon(poly, 1); % use a tolerance equal to 1.
%     figure; hold on;
%     drawEllipse(elli);
%     drawPoint(poly2, 'mo');
%
%   See also
%   polygons2d, smoothPolygon, simplifyPolyline, resamplePolygon
%
%   References
%   http://en.wikipedia.org/wiki/Ramer-Douglas-Peucker_algorithm
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2013-03-14,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2013 INRA - Cepia Software Platform.

% call the simplifyPolyline function by ensuring the last vertex is present
poly = poly([1:end 1], :);
[poly, keepInds] = simplifyPolyline(poly, varargin{:});

% remove last vertex
poly(end, :) = [];
keepInds(end) = [];


function [min] = s2min(s)
% Convert time from seconds to minutes.
% Chad A. Greene 2012
min = s/60;


function [E]=dtiComputeApproximateEmbeddingVectors(fgfile, NfibersInA, method, npoints, kernelsigma, nvec)

%[E]=dtiComputeApproximateEmbeddingVectors(fgfile, [NfibersInA=1500', [method='pairwise_dist'], [npoints=15], [kernelsigma=30   ], [nvec=15])

%Computes embedded coordinates on a large fiberset
%Parameters: full fiber set (fgfile)
% NfibersInA: how many fibers form the "A" marix (full affinity matrix).
% Recommended/max my laptom with 2GB RAM can handle is 1500
%npoints (nodes in a resampled fiber)
%Note that your sample for A matrix will be picked from the first
%NfibersInA fibers. I is therefore crucial that the original fiberset is
%reshuffled using  fg = dtiShuffleFibers(fg) before it is passed into this
%function.

%Kernel is a parameter used for gaussian transformation from distances to
%affinities  (sigma of affinity=-distance^2/sigmasquare)

%ER 03/2008
%ER 11/2008 added default parameters

%TODO: = Specify recommended params
%= Add reshuffling by default
%= Add an option of using NfibersInA=size(fg.fibers, 2)

%%%%%%%%%%%%%%%%%%%%%%%%%
if(~exist('npoints','var')||isempty(npoints))
    npoints = 15;
end

if(~exist('kernelsigma','var')||isempty(kernelsigma))
    kernelsigma = 30; %30 for short distances, like clustering 1/2 brain; 60 for full brain distances
end

if(~exist('nvec','var')||isempty(nvec))
    nvec = 15;
end

if(~exist('method','var')||isempty(method))
    method = 'pairwise_dist';
end

if(~exist('NfibersInA','var')||isempty(NfibersInA))
    NfibersInA = 1500;
end

load(fgfile);
NumFibersTotal=size(fg.fibers, 1);
clear fg;
if (NumFibersTotal<NfibersInA)
NfibersInA=NumFibersTotal;
end

%Compute distances for A. Compute distances for B.
range1start=1; range2start=1; range1end=NfibersInA; range2end=NumFibersTotal;

compute_interfiber_distances(fgfile, npoints, method, range1start, range1end, range2start,  range2end, 1);
%If you want, use compute_interfiber_distances(fgfile, npoints, method, range1start, range1end, range2start,  range2end, 1);
%This will save some intermediate results on disk.

outfile=[prefix(fgfile) 'dist' num2str(range1start) 'to' num2str(range1end) 'vs' num2str(range2start) 'to' num2str(range2end) method '.mat'];

%Transform distances into proximities. Use kernel sigma=30; That makes sigmasquare=900.
load(outfile);

if (strcmp(method,'frenet'))

    for i=1:size(distmsr(:));
    distmsr(i)=log(distmsr(i));
    end

    distmsr(distmsr<-3)=-3; %rescale so that zero is the smalles distance
    distmsr= distmsr+3;
    distmsr(isinf(distmsr))=0;
end

%figure; hist(distmsr(:));
%kernelsigma=mean(distmsr(:))    ;

distmsr(1:NfibersInA, 1:NfibersInA)=(distmsr(1:NfibersInA, 1:NfibersInA)'+distmsr(1:NfibersInA, 1:NfibersInA))./2;
%just to fix assymmetry which shldnt be there  btw.

affinities=dtiComputeAffinitiesFromDistances(distmsr, kernelsigma);	clear distmsr;
clear fibergroup1 fibergroup2

%figure; hist(affinities(:));

%Perform estimation such that the size of B matrix is 5xSizeOfA which makes
%A about 20%

NSamples=min((NumFibersTotal-NfibersInA), NfibersInA*5);
%display(['Nsamples ' num2str(NSamples) ' NumFibersTotal ' num2str(NumFibersTotal) ' NFibersInA ' num2str(NfibersInA)]);
A=affinities(1:NfibersInA, 1:NfibersInA);
B=affinities(1:NfibersInA, NfibersInA+(1:NSamples));
clear affinities;

%Compute embedding vectors;
[E, embbasis]=dtiApproximateEmbeddingVectors(A, B, nvec);
display('Embedded space basis computed');

clear B;

maxlast=NfibersInA+NSamples;

while maxlast<NumFibersTotal

display(['Embedding fibers ' num2str(maxlast+1) ' to' num2str(min(maxlast+NSamples, NumFibersTotal))]);

load(outfile);
affinities=dtiComputeAffinitiesFromDistances(distmsr, kernelsigma);	clear distmsr;
clear fibergroup1 fibergroup2
S=affinities(1:NfibersInA, (maxlast+1):min(maxlast+NSamples, NumFibersTotal));
clear affinities;

E=[E; dtiNewDataOntoEmbeddingVectors(A, S, embbasis, nvec)];
maxlast=maxlast+NSamples;
end

%The next step will be computing actual clustering which we will put into a
%different function in case we wanted to stop here and use current embedded
%vectors for atlas creation purposes.

save([prefix(fgfile) 'EV' method], 'A', 'E', 'embbasis', 'npoints', 'kernelsigma', 'method');


%==============================================================================
% This code is part of the Matlab-based toolbox
% LagLDDDM - A Lagrangian Gauss--Newton--Krylov Solver for Mass- and
%                        Intensity-Preserving Diffeomorphic Image Registration
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM
%
% function [yc,dy,para] = getTrafoFromVelocityRK4(vc,yc,varargin)
%
% compute transformation yc by integrating velocity field in time using
% a fourth-order Runge-Kutta method. Here, vc is assumed to be stationary.
% For instationary velocities, use getTrafoFromInstatinaryVelocityRK4.m
%
% For more details see Sec. 3 of the paper:
%
% @article{MangRuthotto2017,
%   Title = {A {L}agrangian {G}auss--{N}ewton--{K}rylov solver for mass- and intensity-preserving diffeomorphic image registration},
%   Year = {2017},
%   Journal = {SIAM Journal on Scientific Computing},
%   Author = {A. Mang, L. Ruthotto},
% }
%
% Input:
%
%  vc   - discrete velocity field (nodal, cell-centered, or staggered)
%  yc   - particle positions
%
% Additional REQUIRED Input (provided through varargin)
%
%  omega        - spatial domain (required!)
%  m            - number of cells in each direction (required!)
%
% Optional Input (provided through varargin)
%
%  doDerivative - compute derivative w.r.t. velocity
%  tspan        - time interval (default: [0 1])
%  N            - number of time discretization points (nodal) (default: 5)
%  storeInter   - store intermediate transformation (e.g., for visualization)
%
% Output:
%
%  yc           - end point of characteristics
%  dy           - derivative w.r.t. vc
%  para         - info such as CFL
%
% =========================================================================
function [yc,dy,para] = getTrafoFromVelocityRK4(vc,yc,varargin)

if nargin==0
    runMinimalExample
    return;
end
doDerivative = (nargout>1);
omega = [];
m     = [];
tspan = [0 1];
N     = 5;
storeInter = false;
for k=1:2:length(varargin)     % overwrites default parameter
    eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;
if isempty(omega) || isempty(m)
    error('%s - omega and m must be provided through varargin or trafo(''set'',''omega'',omega,''m'',m')
end
dt   = (tspan(2)-tspan(1))/(N-1); % note dt is negatave when going backwards in time
dy = [];
if doDerivative
    dy = sparse(numel(yc),numel(vc));
end
% compute the CFL number (not actually required for stability in Lagrangian
% methods, but an indicator of how many voxels the particles may move in
% one time step.
h    = (omega(2:2:end)-omega(1:2:end))./m;
% vt   =  sqrt(sum(reshape(vc.^2,[],dim),2));
% CFL  = max(vt)*dt/min(h);
CFL = 0.0;
para = struct('CFL',CFL,'dt',dt,'N',N,'omega',omega,'m',m,'h',h);
if storeInter
    para.YC = zeros(numel(yc),N);
    para.YC(:,1) = yc;
end
for k=1:N-1
    [vi,dvidy] = linearInterGrid(vc,omega,m,yc,'doDerivative',doDerivative);
    if doDerivative
        Ty = getLinearInterGridMatrix(omega,m,yc);
        dyi = Ty + dvidy*dy;
        dytemp = dyi;
    end
    ytemp = vi;
    yi    = yc + .5*dt*vi;

    [vi,dvidy] = linearInterGrid(vc,omega,m,yi,'doDerivative',doDerivative);
    if doDerivative
        Ty = getLinearInterGridMatrix(omega,m,yi);
        dyi = Ty + dvidy*(dy+.5*dt*dyi);
        dytemp = dytemp + 2*dyi;
    end
    ytemp = ytemp + 2*vi;
    yi    = yc + .5*dt*vi;

    [vi,dvidy] = linearInterGrid(vc,omega,m,yi,'doDerivative',doDerivative);
    if doDerivative
        Ty = getLinearInterGridMatrix(omega,m,yi);
        dyi = Ty + dvidy*(dy+.5*dt*dyi);
        dytemp = dytemp + 2*dyi;
    end
    ytemp = ytemp + 2*vi;
    yi    = yc + dt*vi;
    [vi,dvidy] = linearInterGrid(vc,omega,m,yi,'doDerivative',doDerivative);
    if doDerivative
        Ty = getLinearInterGridMatrix(omega,m,yi);
        dyi = Ty + dvidy*(dy+dt*dyi);
        dytemp = dytemp + dyi;
        dy  = dy + (dt/6)*dytemp;
    end
    ytemp = ytemp + vi;
    yc    = yc + (dt/6)*ytemp;
    if storeInter, para.YC(:,k+1) = yc; end
end

function runMinimalExample

omegaV = [-1 1 -1 1];
omega = .1*omegaV;
m     = [24 24];
tspan     = [2 3];
N     = 2;
yc    = getNodalGrid(omega,m)+.0002;

% test cell-centered grid
regularizer('set','regularizer','mbCurvature','alpha',1)
xc      = reshape(getCellCenteredGrid(omegaV,m),[],2);
vc      = [.3*sign(xc(:,1)).*xc(:,1).^2 sin(pi*xc(:,2))];
fctn    = @(vc) getTrafoFromVelocityRK4(vc,yc,'omega',omegaV,'m',m,'T',tspan,'N',N);
checkDerivative(fctn,vc(:))

% test nodal grid
regularizer('set','regularizer','mbElasticNodal','alpha',1)
xc      = reshape(getNodalGrid(omegaV,m),[],2);
vc      = [.3*sign(xc(:,1)).*xc(:,1).^2 sin(pi*xc(:,2))];
fctn    = @(vc) getTrafoFromVelocityRK4(vc,yc,'omega',omegaV,'m',m,'T',tspan,'N',N);
checkDerivative(fctn,vc(:))

% test staggered grid
regularizer('set','regularizer','mbElastic','alpha',1)
vc  = grid2grid(vc(:),m,'nodal','staggered');
fctn    = @(vc) getTrafoFromVelocityRK4(vc,yc,'omega',omegaV,'m',m,'T',tspan,'N',N);
checkDerivative(fctn,vc(:))

omegaV = [-1 1 -1 1 -1 1];
omega = .1*omegaV;
m     = [16 16 8];
tspan = [4 3];
N     = 10;
yc    = getNodalGrid(omega,m)+.0002;

regularizer('set','regularizer','mbCurvature','alpha',1)
xc      = reshape(getCellCenteredGrid(omegaV,m),[],3);
vc      = [.3*sign(xc(:,1)).*xc(:,1).^2 sin(pi*xc(:,2)) xc(:,3)];
fctn    = @(vc) getTrafoFromVelocityRK4(vc,yc,'omega',omegaV,'m',m,'T',tspan,'N',N);
% [yc,dy] = fctn(vc(:));
checkDerivative(fctn,0*vc(:))

% test nodal grid
regularizer('set','regularizer','mbElasticNodal','alpha',1)
xc      = reshape(getNodalGrid(omegaV,m),[],3);
vc      = [.3*sign(xc(:,1)).*xc(:,1).^2 sin(pi*xc(:,2)) xc(:,3)];
fctn    = @(vc) getTrafoFromVelocityRK4(vc,yc,'omega',omegaV,'m',m,'T',tspan,'N',N);
checkDerivative(fctn,vc(:))

% test staggered grid
regularizer('set','regularizer','mbElastic','alpha',1)
vc  = grid2grid(vc(:),m,'nodal','staggered');
fctn    = @(vc) getTrafoFromVelocityRK4(vc,yc,'omega',omegaV,'m',m,'T',tspan,'N',N);
checkDerivative(fctn,vc(:))


function f = luflop (L, U)						    %#ok
%LUFLOP given L and U, computes # of flops required to compute them
%
% Example:
% f = luflop (L, U)
%
% Given an LU factorization, compute how many flops took to compute it.  This
% is the same as (assuming U has a zero-free diagonal):
%
%   Lnz = full (sum (spones (L))) - 1 ;
%   Unz = full (sum (spones (U')))' - 1 ;
%   f = 2*Lnz*Unz + sum (Lnz) ;
%
% except that no extra workspace is allocated for spones (L) and spones (U).
% L and U must be sparse.
%
% Note: the above expression has a subtle undercount when exact numerical
% cancelation occurs.  Try [L,U,P] = lu (sparse (ones (10))) and then
% luflop (L,U).
%
% See also LU

% Copyright 1995-2007 by Timothy A. Davis.

help luflop
error ('luflop mexFunction not found!  Use umfpack_make to compile luflop.') ;


function c = rdivide(a,b)
% implements e ./ c and c ./ e
%
% Description
% If |e| is a matrix of embeddings and |c| is a matrix of coefficients
% then |e ./ c| is again a matrix of embeddings defined by
%
% $$ [\mathrm{e ./ c}]_{j\ell} = mathrm{e}_{j\ell} / \mathrm{c}_{j \ell}$$
%
% Syntax
%   out = e ./ c
%   out = c ./ e
%   out = e1 ./ e2
%
% Input
%  e, e1, e2 - @embedding
%  c - double
%
% Output
%  out- @embedding
%

if isa(a,'embedding')

  if isa(b,'embedding')

    for i = 1:length(a.u), a.u{i} = a.u{i} ./ b.u{i}; end

  else
    for i = 1:length(a.u), a.u{i} = a.u{i} ./ b; end
  end
  c = a;

else

  for i = 1:length(b.u), b.u{i} = a ./ b.u{i}; end
  c = b;

end

end


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

%Empty workspace and close figures
close all;
clear;

%% Define parameters
lambda = 1; %Wavelength (normalized)
M_H = 8; %Number of antenna per horizontal row
M_V = 4; %Number of rows
d_H = 0.5*lambda; %Horizontal antenna spacing
d_V = 0.5*lambda; %Vertical antenna spacing

%Define the antenna geometry
M = M_H*M_V; %Total number of antennas
U = zeros(3,M); %Matrix containing the position of the antennas

i = @(m) mod(m-1,M_H); %Horizontal index
j = @(m) floor((m-1)/M_H); %Vertical index

for m = 1:M
    U(:,m) = [0; i(m)*d_H; j(m)*d_V]; %Position of the mth element
end

%% Compute the spatial signature for various directions
varphi = linspace(-pi/2,pi/2,1000);
theta= [0,-pi/3];
P = zeros(length(varphi),length(theta));
v = ones(M,1);

for i = 1:length(varphi)

    for j = 1:length(theta)

        P(i,j) = abs(v'*functionSpatialSignature3DLoS(U,varphi(i),theta(j),lambda))/M;

    end

end

P = P/max(max(P));
P = 10*log10(P);

%% Plot the simulation results
figure;
hold on; box on;
plot(varphi/pi,P(:,1)','-k','LineWidth',1)
plot(varphi/pi,P(:,2)','r--','LineWidth',1)

ylim([-30,0])
ax = gca;
set(ax, 'XTick', [-0.5, -0.25, 0, 0.25, 0.5]);
xlabel('Azimuth angle $\varphi$ in multiples of $\pi$','Interpreter','latex');
ylabel('Normalized array response [dB]');
legend({'$\theta = 0$', '$\theta = -\pi/3$   '},'Interpreter','latex');


function [xUpdate,PUpdate,innov,Pzz,W]=QMCKalUpdateWithPred(z,R,zPred,PzPred,otherInfo)
%%QMCKALUPDATEWITHPRED Given the output of the measurement prediction step
%           from QMCKalMeasPred and a measurement, complete the measurement
%           update step of the quasi-Monte Carlo Kalman filter with
%           additive measurement noise. Separating the measurement
%           prediction step from the rest of the update step can make the
%           creation of multiple measurement association hypotheses from a
%           single target prediction more efficient. The full measurement
%           update function is QMCKalUpdate.
%
%INPUTS: z The zDimX1 vector measurement.
%        R The zDim X zDim measurement covariance matrix in the native
%          coordinate system of the measurement.
%    zPred The zDimXnumComp measurement predictions from the filter.
%   PzPred The zDimXzDimXnumComp covariance matrices associated with zPred.
% otherInfo The intermediate results returned in the otherInfo output of
%          the QMCKalMeasPred function.
%
%OUTPUTS: xUpdate The xDimXnumComp updated state vectors.
%         PUpdate The updated xDimXxDimXnumComp state covariance matrices
%                 associated with xUpdate.
%      innov, Pzz The zDimXnumComp innovations and the zDimXzDim innovation
%                 covariance matrices are returned in case one wishes to
%                 analyze the consistency of the estimator or use those
%                 values in gating or likelihood evaluation.
%               W The xDimXzDimXnumComp gains used in the update.
%
%See the comments to the function QMCKalMeasPred for an example of usage of
%this function. See the comments to QMCKalUpdate for more information on
%the algorithm.
%
%June 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

innovTrans=otherInfo.innovTrans;
stateTrans=otherInfo.stateTrans;
xPred=otherInfo.xPred;
PPred=otherInfo.PPred;
Pxz=otherInfo.Pxz;

xDim=size(xPred,1);
numComp=size(xPred,2);
zDim=size(z,1);

xUpdate=zeros(xDim,numComp);
PUpdate=zeros(xDim,xDim,numComp);
innov=zeros(zDim,numComp);
Pzz=zeros(zDim,zDim,numComp);
W=zeros(xDim,zDim,numComp);

for k=1:numComp
    Pzz(:,:,k)=PzPred(:,:,k)+R;

    %The innovation, transformed as necessary to keep values in a desired
    %range.
    innov(:,k)=innovTrans(z,zPred(:,k));

    %The filter gain
    W(:,:,k)=Pxz(:,:,k)/Pzz(:,:,k);

    %Updated state estimate
    xUpdate(:,k)=stateTrans(xPred(:,k)+W(:,:,k)*innov(:,k));

    %Updated state covariance matrix
    PUpdate(:,:,k)=PPred(:,:,k)-W(:,:,k)*Pzz(:,:,k)*W(:,:,k)';
end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


addpath('bin');
% volsz = [512, 512, 255];
volsz = [100, 100, 100];
% volsz = [250, 250, 200];
spacing = [8, 8, 8];
spacing = [4, 4, 4];
k = ceil(volsz ./ spacing) + 1;
Knots = (rand([k, 3]) - 0.5) * 20;
sKnots = single(Knots);
dKnots = double(Knots);
gsKnots = gpuArray(sKnots);
gdKnots = gpuArray(dKnots);
vol = single(rand(volsz))*0;
vol(20:end-20, 20:end-20, 20:end-20) = 1;
dvol = double(vol);
gvol = gpuArray(vol);
gdvol = gpuArray(dvol);
%%
tic
disp = mex_3d_linear_disp_float(sKnots, single(volsz), single(spacing));
toc

tic
dispg = mex_3d_linear_disp_GPU_float(gsKnots, single(volsz), single(spacing));
toc

% dispg = gather(dispg);
sum(abs(disp(:) - gather(dispg(:))))
max(abs(disp(:) - gather(dispg(:))))
%%

tic
vdef = mex_3d_volume_warp_float(vol, disp, single(0));
toc

tic
vdefg = mex_3d_volume_warp_GPU_float(gvol, dispg, single(0));
toc
max(abs(vdef(:) - vdefg(:)))

%%
display('---');
tic
for i = 1 : 10
disp = mex_3d_linear_disp_float(sKnots, single(volsz), single(spacing));
vdef = mex_3d_volume_warp_float(vol, disp, single(0));
end
toc
%%
display('...');
tic
for i = 1 : 10
dispg = mex_3d_linear_disp_GPU_float(gsKnots, single(volsz), single(spacing));
vdefg = mex_3d_volume_warp_GPU_float(gvol, dispg, single(0));
end
toc

%%
volsz = [200, 255, 257];
spacing = [3, 3, 3];
ksz = ceil(volsz ./ spacing) + 1;

G1 = rand(volsz);
G2 = rand(volsz) * 0;
G2(30:end-30, 30:end-30, 30:end-30) = 1;
G3 = rand(volsz);

G1 = single(G1);
G2 = single(G2);
G3 = single(G3);

gG1 = gpuArray(G1);
gG2 = gpuArray(G2);
gG3 = gpuArray(G3);

tic
[gr1, gr2, gr3] = mex_3d_linear_partial_conv_float(G1, G2, G3, single(ksz), single(spacing));
toc

tic
[ggr1, ggr2, ggr3] = mex_3d_linear_partial_conv_GPU_float(gG1, gG2, gG3, single(ksz), single(spacing));
toc

[max(abs(gr1(:) - ggr1(:))), max(abs(gr2(:) - ggr2(:))), max(abs(gr3(:) - ggr3(:)))]

%%
addpath('..');
% addpath('mex_functions/bin');

tic
for i = 1 : 30
    voldef = linear_disp_and_warp_3d((sKnots), (vol), spacing, 0, 0, 0);
end
toc
%%
tic
for i = 1 :30
voldefg = linear_disp_and_warp_3d((gdKnots), (gdvol), spacing, 0, 0, 0);
end
toc
%%
display('----')
tic
D = mex_3d_linear_disp_GPU_float(gsKnots, single(size(gvol)), single(spacing));
toc
tic
voldef1 = mex_3d_volume_warp_GPU_float(gvol, D, single(0));
toc
% tic
% voldef2 = mex_3d_volume_warp_GPU_float(gvol, D, single(0));
% toc
% tic
% voldef3 = mex_3d_volume_warp_GPU_float(gvol, D, single(0));
% toc

%%
addpath('bin');
mex  -output bin/mex_3d_cubic_disp_double CXX='g++' CXXFLAGS='\$CXXFLAGS -O3 -funroll-loops -fopenmp -I../../mex_helpers -I../../deformation_tools_cpp' LDFLAGS="\$LDFLAGS -fopenmp" mex_3d_cubic_disp.cpp ../cubic_grid.cpp
vols = [270,270,270];
spc = [8,8,8];
k = ceil(vols ./ spc) + 3;
Knots = rand([k, 3]);
Knots = Knots * 0;
Knots(2, 2, 2,1) = 5;
Knots(5, 5, 2,1) = 10;
% Knots(2, 3, 1,1) = -5;
tic
D = mex_3d_cubic_disp_double(Knots, vols, spc);
toc
subplot(121)
imagesc(D(:,:,1,1)); colorbar
subplot(122)
imagesc(D(:,:,2,1)); colorbar
%%
spc = 4;
b = -ones(spc*4, 1);
for i = 1 : 2*spc + 1
    t = (i-1)/spc;
    if i <= spc
        k = t;
        f = 3*k^3 - 5 * k * k + 2;
    else
        k = t - 1;
        f = -k^3 + 2*k^2 - k;
    end
    b(2*spc+i-1) = f/2;
    if i <= 2*spc
        b(2*spc - i+1) = f/2;
    end
end
plot(b(1:end-1), 'r.-');
size(b)
%%
% mex  -output bin/mex_3d_cubic_partial_conv_double CXX='g++' CXXFLAGS='\$CXXFLAGS -O3 -funroll-loops -fopenmp -I../../mex_helpers -I../../deformation_tools_cpp' LDFLAGS="\$LDFLAGS -fopenmp" mex_3d_cubic_partial_conv.cpp ../cubic_grid.cpp
tr = rand(270, 270, 270);
spc = [2,2,2];
spc = [8,8,8];
ksz = ceil(size(tr)./spc) + 3;
tic
[g1, g2, g3] = mex_3d_cubic_partial_conv_double(tr, tr, tr, ksz, spc);
toc
ksz2 = ceil(size(tr)./spc) + 1;
tic
% [gg1, gg2, gg3] = mex_3d_linear_partial_conv_double(tr, tr, tr, ksz2, spc);
toc

%% upsmpl test
vol = zeros(100, 100, 100);
spc = [8,8,8];
k1 = ceil( (size(vol)-1) ./ spc) + 3;
k2 = ceil( (2*size(vol)-1) ./ spc) + 3;
Knots1 = rand([k1, 3]);
Knots1(4:round(end), 4:round(end/2), :, 1) = 13;
Knots2 = refine_cubic_grid_3d(Knots1, spc, spc, [1,1,1], size(vol)*2, size(vol));
%
D1 = mex_3d_cubic_disp_double(Knots1, size(vol), spc);
D2 = mex_3d_cubic_disp_double(Knots2, 2*size(vol), spc);
subplot(221)
imagesc(D1(:,:, 1, 1));
subplot(222)
imagesc(D2(:,:, 1, 1));

subplot(223)
imagesc(Knots1(:,:, round(end/2), 1));
subplot(224)
imagesc(Knots2(:,:, round(end/2), 1));
%%

subplot(121);
hold off;
ti1 = 0:(size(vol, 1)-1);
ti2 = (0:(2*size(vol, 1)-1))/2;
plot(ti1, squeeze(D1(round(end/3),:, 1,1)), 'bo-')
hold on;
plot(ti2, squeeze(D2(round(end/3),:, 1,1)), 'r.-')
subplot(121);
t1 = interp1(ti1, squeeze(D1(33,:, 1,1)), ti2);
plot(ti2, t1 - squeeze(D2(round(end/3),:,1,1)), 'k');
grid on;
subplot(122);
hold off
plot( (0:(k1(1)-1))-spc(1), squeeze(Knots1(15, :, 1,1)), 'bx-');
hold on;
plot((0:k2(1)-1)/2 - spc(1)*7/8, squeeze(Knots2(15, :, 1,1)), 'ro-');
%%
hold off;
plot(1:size(vol, 1), D1(:, 1, 1, 1), 'r.-');
hold on;
Ktmp =Knots1(2:end-1, 2, 2, 1);
plot(1 + (0:size(Ktmp,1)-1)*spc(1), Ktmp, 'bo-');

Ktmp =Knots2(2:end-1, 2, 2, 1);
plot(1 + (0:size(Ktmp,1)-1)*spc(1)/2, Ktmp, 'ks-');


function y = cumsum( varargin )

%Disciplined convex/geometric programming information for SUM:
%   CUMSUM(X) and CUMSUM(X,DIM) are vectorized forms of addition. So
%   when CUMSUM is used in a DCP or DGP, elements in each subvector
%   must satisfy the corresponding combination rules for addition (see
%   PLUS). For example, suppose that X looks like this:
%      X = [ convex concave affine  ;
%            affine concave concave ]
%   Then CUMSUM(X,1) would be permittted, but CUMSUM(X,2) would not,
%   because the top row contains the sum of convex and concave terms, in
%   violation of the DCP ruleset. For DGPs, addition rules dictate that
%   the elements of X must be log-convex or log-affine.

persistent P
if isempty( P ),
    P.map      = cvx_remap( { 'constant' ; 'affine' ; 'convex' ; 'concave' } );
    P.funcs    = { @cumsum_1, @cumsum_2, @cumsum_2 };
    P.zero     = 0;
    P.reduce   = false;
    P.reverse  = true;
    P.dimarg   = 2;
    P.constant = 1;
    P.name     = 'cumsum';
end
y = cvx_reduce_op( P, varargin{:} );

function x = cumsum_1( x )
x = builtin( 'cumsum', x, 2 );

function x = cumsum_2( x )
s = x.size_;
if s(2) ~= 1,
    b = reshape( x.basis_, [], s(2) );
    b = cumsum( b, 2 );
    x = cvx( s, reshape( b, [], prod(s) ) );
end

% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function demoEquivalentEllipsoid
%DEMOEQUIVALENTELLIPSOID Demo program for the use of ellipsoids
%
%   Example
%     demoEquivalentEllipsoid
%
%   See also
%     demoRevolutionSurface, demoDrawTubularMesh

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2011-06-21,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

%% Data generation

% Generate gaussian 3D data
nPoints = 1000;
points = randn(nPoints, 3);

% point clouds parameters
center = [20 30 40];
sizes  = [70 40 10];
orient = [50 30 30];

% transform points to make a gaussian cloud
transfo = composeTransforms3d(...
    createScaling3d(sizes), ...
    eulerAnglesToRotation3d(orient), ...
    createTranslation3d(center));
points = transformPoint3d(points, transfo);

% display data
figure;
drawPoint3d(points, '.');
hold on;
axis equal;
view([80 -10]);

%% Equivalent ellipsoid computation and display

% Fit a 3D equivalent ellipsoid to data
elli = equivalentEllipsoid(points);

% draw the ellipsoid with transparency
drawEllipsoid(elli, 'FaceColor', 'g', 'FaceAlpha', .5);

%% Add ellipses and main axes

drawEllipsoid(elli, 'FaceColor', 'g', 'FaceAlpha', .5, ...
          'drawEllipses', true, 'EllipseColor', 'b', 'EllipseWidth', 2, ...
          'drawAxes', true);


function [U,G] = shell(V,F,th,varargin)
  % SHELL Compute a thin shell around a triangle mesh (V,F) with thickness th.
  %
  % Inputs:
  %   V  #V by 3 list of mesh vertex positions
  %   F  #F by 3 list of triangle indices into V
  %   th  desired thickness of shell
  %   Optional:
  %     'Normals' followed by #V by 3 list of vertex normals to use to
  %     construct shell (will be multiplied by th): {area-weighted}
  % Outputs:
  %   U  2*#V by 3 list of output mesh vertex positions: V always comes first
  %   G  #G by 3 list of triangle indices into U
  %

  N = [];
  % Map of parameter names to variable names
  params_to_variables = containers.Map( ...
    {'Normals'}, ...
    {'N'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end

  if isempty(N)
    N = normalizerow(per_vertex_normals(V,F));
  end

  % scale by thickness
  N = N*th;

  O = outline(F);
  n = size(V,1);
  U = [V;V+N];
  G = [ ...
    fliplr(F); ...
    n+F; ...
    0+O(:,1) n+O(:,[2 1]); ...
    n+O(:,2) 0+O(:,1:2)];

end


function [I,B1,B2,B3] = in_element(V,F,P,varargin)
  % IN_ELEMENT test for each p in P whether it lies inside each f in F defined
  % over V.
  %
  % I = in_element(V,F,P)
  % [I,B1,B2,B3] = in_element(V,F,P,'ParameterName',ParameterValue,...)
  %
  % Inputs:
  %   V  #V by dim list of vertex positions
  %   F  #F by dim+1 list of element indices
  %   P  #P by dim list of query positions
  %   Optional:
  %     'Method' followed by one of the following {'knn'}:
  %       'brute-force' no acceleration O(#P * #F)
  %       'edge-walk' walk along edges ~O(#P * sqrt(#F)) Starting with a random
  %         barycenter step along the edges that intersect with the ray toward
  %         the query point. If a boundary is reached then search along all
  %         boundary edges and jump to farthest hit. **dim=2 only**
  %       'knn' use knnsearch to find closest element barycenters
  %       'spatial-hash' spatial hash on regular grid ~O(#P * sqrt(#F)) **dim=2
  %         only**
  %     'First'  only keep first match {false}
  %     'Quiet' suppress warnings {false}
  %     'Epsilon' epsilon used for determining inclusion {eps}
  % Outputs:
  %   I  #P by #F matrix of bools
  %   B1  #P by #F list of barycentric coordinates
  %   B2  #P by #F list of barycentric coordinates
  %   B3  #P by #F list of barycentric coordinates
  %
  % Example:
  %   P = bsxfun(@plus,min(V),bsxfun(@times,rand(100,2),max(V)-min(V)));
  %   [I,B1,B2,B3] = in_element(V,F,P);
  %   % Only keep first
  %   [mI,J] = max(I,[],2);
  %   I = sparse(1:size(I,1),J,mI,size(I,1),size(I,2));
  %   % Mask barycentric coordinates
  %   B1 = B1.*I;
  %   B2 = B2.*I;
  %   B3 = B3.*I;
  %   Q = B1*V(F(:,1),:) + B2*V(F(:,2),:) + B3*V(F(:,3),:);
  %   Q = Q(any(I,2),:);
  %   tsurf(F,V);
  %   hold on;
  %   plot(Q(:,1),Q(:,2),'or','LineWidth',6);
  %   plot(P(:,1),P(:,2),'ob','LineWidth',2);
  %   hold off;
  %

  function [I] = in_element_brute_force(V,F,P)
    dim = size(V,2);
    assert(dim+1 == size(F,2));

    % number of elements
    m = size(F,1);
    % number of query points
    np = size(P,1);

    switch dim
    case 3
      T = F;
      % tet face ares
      vol = abs(volume(V,T));
      allF = [ ...
        T(:,2) T(:,4) T(:,3); ...
        T(:,1) T(:,3) T(:,4); ...
        T(:,1) T(:,4) T(:,2); ...
        T(:,1) T(:,2) T(:,3); ...
        ];
      % List of tets for each face f of each tet t for each point p
      TP = cat(2, ...
        repmat(allF,[1 1 np]), ...
        permute(repmat(size(V,1)+(1:np),m*4,1),[1 3 2]));
      TP = reshape(permute(TP,[1 3 2]),m*4*np,dim+1);
      Pvol = abs(volume([V;P],TP));
      % Pvol(t,f,p) --> volume of tet t, face f with point p
      Pvol = reshape(Pvol,[size(T,1) dim+1 np]);
      % sumvol(p,t) --> sum of volumes of tets made with faces of t and p
      sumvol = permute(sum(Pvol,2),[3 1 2]);
      I = sparse(abs(bsxfun(@minus,sumvol,vol')) < sqrt(epsilon));
    case 2
      % triangle side lengths
      l = [ ...
        sqrt(sum((V(F(:,2),:)-V(F(:,3),:)).^2,2)) ...
        sqrt(sum((V(F(:,3),:)-V(F(:,1),:)).^2,2)) ...
        sqrt(sum((V(F(:,1),:)-V(F(:,2),:)).^2,2)) ...
        ];

      B = zeros([np m dim+1]);
      for ii = 1:(dim+1)
        jj = mod(ii+1,dim+1)+1;
        kk = mod(ii,dim+1)+1;
        ljj = pdist2(P,V(F(:,jj),:));
        lkk = pdist2(P,V(F(:,kk),:));

        % semiperimeters
        s = bsxfun(@plus,l(:,ii)',ljj + lkk)*0.5;
        % Heron's formula for area
        B(:,:,ii) = 2*sqrt(s.*(bsxfun(@minus,s,l(:,ii)').*(s-ljj).*(s-lkk)));
      end
      % sum of barycentric coordinates
      sumA = sum(B,3);
      % area of element
      dblA = doublearea(V,F);
      %% check whether sum is more than true are
      %I = ~bsxfun(@gt,sumA,dblA');
      I = sparse((bsxfun(@minus,sumA,dblA')) < sqrt(epsilon));
    case 1
      % To-do: this is actually being computed in a dense way
      I = sparse( ...
        ((P <= V(F(:,1))') & (P >= V(F(:,2))')) | ...
        ((P <= V(F(:,2))') & (P >= V(F(:,1))')));
    end
    %B1 = sparse(B(:,:,1));
    %B2 = sparse(B(:,:,2));
    %B3 = sparse(B(:,:,3));
  end

  function I = in_element_hash_helper(V,F,P)
    assert(size(F,2) == 3, ...
      'F must contain triangles for Method=''spatial-hash''');
    num_bins = ceil(sqrt(size(F,1)));
    bin_x = ceil(sqrt(num_bins));
    bin_y = ceil(num_bins/bin_x);
    num_bins = bin_x*bin_y;
    % spatial hash
    function VH = hash(V,MN,MX,bin_x,bin_y)
      [~,X] = histc(V(:,1),linspace(MN(1),MX(1),bin_x));
      [~,Y] = histc(V(:,2),linspace(MN(2),MX(2),bin_y));
      VH = sub2ind([bin_x bin_y],X,Y);
    end
    %% http://stackoverflow.com/a/5929567/148668
    %primes = [ 40960001, 59969537 45212177];
    %hash = @(X) mod( ...
    %  bitxor( int32(V(:,1)*primes(1)), int32(V(:,2)*primes(2))), ...
    %  num_bins);
    MN = min([V;P]);
    MX = max([V;P]);
    VH = hash(V,MN,MX,bin_x,bin_y);
    PH = hash(P,MN,MX,bin_x,bin_y);
    % This is wrong for triangles that span more hash cells than their vertices:
    % any time a triangle lands on the corner....
    FH = sparse(repmat(1:size(F,1),1,size(F,2))',VH(F(:)),1,size(F,1),num_bins)~=0;
    [~,FHI,FHJ] = find(FH);
    [FHJX,FHJY] = ind2sub([bin_x,bin_y],FHJ);
    % This assumes that #P >> #F
    I = sparse(size(P,1),size(F,1));
    for h = 1:num_bins
      %Vh = V(VH==h,:);
      IFh = FH(:,h);
      if any(IFh)
        IPh = PH==h;
        Ph = P(IPh,:);
        if any(IPh)
          Fh = F(IFh,:);
          Ih = in_element_brute_force(V,Fh,Ph);
          I(IPh,IFh) = Ih;
        end
      end
    end
  end

  % default values
  method = [];
  first = false;
  quiet = false;
  epsilon = eps;
  % Map of parameter names to variable names
  params_to_variables = containers.Map( {'Method','First','Quiet','Epsilon'}, ...
    {'method','first','quiet','epsilon'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this
      % workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end
  if isempty(method)
    switch size(V,2)
    case 1
      method = 'brute-force';
    otherwise
      method = 'knn';
    end
  end

  switch method
  case 'brute-force'
    I = in_element_brute_force(V,F,P);
  case 'spatial-hash'
    % Try 45?? spatial grid, too (reduce corner cases)
    switch size(V,2)
    case 2
      R = [cos(pi/4) -sin(pi/4);sin(pi/4) cos(pi/4)];
    case 3
      R = [cos(pi/4) -sin(pi/4) 0 ;sin(pi/4) cos(pi/4) 0; 0 0 1];
    end
    I = in_element_hash_helper(V,F,P) | in_element_hash_helper(V*R,F,P*R);

    % Find any obviously incorrect values: not inside but winding number says
    % inside. Could still missing something if mesh overlaps itself.
    NI = ~any(I,2);
    switch size(F,2)
    case 3
      O = outline(F);
    case 4
      O = boundary_faces(F);
    end
    WI = abs(winding_number(V,O,P(NI,:))/(2*pi))>0.5;
    WI = sparse(find(NI),1,WI,size(P,1),1)~=0;
    % redo any that currently are not in any but winding number says are inside
    RI = NI & WI;
    I(RI,:) = in_element_brute_force(V,F,P(RI,:));

  case 'edge-walk'

    assert(size(F,2) == 3,'F must contain triangles for Method=''edge-walk''');
    % List of all "half"-edges: 3*#F by 2
    allE = [F(:,[2 3]); F(:,[3 1]); F(:,[1 2])];
    % Sort each row
    sortallE = sort(allE,2);
    % IC(i) tells us where to find sortallE(i,:) in uE:
    % so that sortallE(i,:) = uE(IC(i),:)
    [uE,~,IC] = unique(sortallE,'rows');
    % uE2F(e,f) = i means face f's ith edge is unique edge e
    uE2F = sparse(IC(:),repmat(1:size(F,1),1,3)',reshape(repmat(1:3,size(F,1),1),[],1));
    % uE2F(e,f) = 1 means face f is adjacent to unique edge e
    uE2F1 = sparse(IC(:),repmat(1:size(F,1),1,3)',1);
    % Face-face Adjacency matrix
    A = uE2F1'*uE2F;
    % A(f,g) = i means face f's ith edge is shared with g
    A = A-diag(diag(A));

    I = sparse(size(P,1),size(F,1));
    B1 = sparse(size(I,1),size(I,2));
    B2 = sparse(size(I,1),size(I,2));
    B3 = sparse(size(I,1),size(I,2));

    [~,is_b] = on_boundary(F);
    EF = repmat(1:size(F,1),1,3)';
    EFI = reshape(repmat(1:3,size(F,1),1),[],1);
    O = allE(is_b(:),:);
    OF = EF(is_b(:));
    OFI = EFI(is_b(:));

    % initial closest vertex
    BC = barycenter(V,F);
    % Centroid
    f_init = snap_points(mean(V),BC);
    %% Random initial guess
    %f_init = ceil(rand(1,1)*size(F,1));
    for p = 1:size(P,1)
      % current closest face, barycenter
      f = f_init;
      q = BC(f_init,:);
      % incoming edge
      e_in = [];
      while true
        % current point
        % edges to test
        E = ... %mod(bsxfun(@plus,setdiff([1;2;3],e_in),-1+(1:2)),3)+1;
          [2 3;3 1;1 2];
        out = ...
          lineSegmentIntersect([q P(p,:)],[V(F(f,E(:,1)),:) V(F(f,E(:,2)),:)]);
        out.intAdjacencyMatrix(e_in) = false;
        e_out = find(out.intAdjacencyMatrix);
        if isempty(e_out)
          % no hits so we're in the element
          I(p,f) = 1;
          B = barycentric_coordinates( ...
            P(p,:),V(F(f,1),:),V(F(f,2),:),V(F(f,3),:));
          B1(p,f) = B(1); B2(p,f) = B(2); B3(p,f) = B(3);
        else
          f_prev = f;
          if numel(e_out) > 1
            % Take farthests
            [sd,si] = sort(out.intNormalizedDistance1To2(e_out),'descend');
            e_out = e_out(si(1));
            % TODO: recurse on all in f
          end
          f = find(A(:,f_prev)==e_out);
          % Todo if
          if isempty(f)
            % no neighbors so we're shooting outside.
            out = ...
              lineSegmentIntersect([q P(p,:)],[V(O(:,1),:) V(O(:,2),:)]);
            hits = find(out.intAdjacencyMatrix);
            [sd,si] = sort(out.intNormalizedDistance1To2(hits),'descend');
            f = OF(hits(si(1)));
            e_in = OFI(hits(si(1)));
            if f==f_prev
              error('Point is outside');
            end
          else
            if numel(f) > 1
              f = f(1);
              if ~quiet
                warning('Ignoring non-manifold edge: might miss multiply inside.');
              end
              % TODO: recurse on all in f
            end
            e_in = A(f_prev,f);
          end
        end
        %tsurf(F,V);
        %hold on;
        %tsurf(F(f,:),V,'CData',-1);
        %tsurf(F(f_prev,:),V,'CData',1);
        %plot_edges([q;P(p,:)],[1 2],'r');
        %plot_edges(V,[F(f_prev,E(:,1));F(f_prev,E(:,2))]','b');
        %plot(P(p,1),P(p,2),'*');
        %plot_edges(V,[F(f_prev,E(e_out,1));F(f_prev,E(e_out,2))]', ...
        %  'y','LineWidth',2);
        %hold off;
        %drawnow;
        %input('');
      end
    end

    return
  case 'knn'
    % assumes samples are all inside exactly one element
    BC = barycenter(V,F);
    I = sparse(size(P,1),size(F,1));
    B1 = sparse(size(P,1),size(F,1));
    B2 = sparse(size(P,1),size(F,1));
    B3 = sparse(size(P,1),size(F,1));
    B4 = sparse(size(P,1),size(F,1));
    % indices of points we haven't found yet
    IP = 1:size(P,1);

    prev_k = 0;
    k = 2;
    while true
      K = knnsearch(BC,P(IP,:),'K',k);
      K = K(:,prev_k+1:end);
      for ki = 1:size(K,2)
        switch size(F,2)
        case 3
          B = abs(barycentric_coordinates( ...
            P(IP,:), ...
            V(F(K(:,ki),1),:), ...
            V(F(K(:,ki),2),:), ...
            V(F(K(:,ki),3),:)));
        case 4
          B = abs(barycentric_coordinates( ...
            P(IP,:), ...
            V(F(K(:,ki),1),:), ...
            V(F(K(:,ki),2),:), ...
            V(F(K(:,ki),3),:), ...
            V(F(K(:,ki),4),:)));
        end
        found = abs(sum(B,2)-1)<sqrt(epsilon);
        I(sub2ind(size(I),IP,K(:,ki)')) = found;
        % DOESN'T REALLY PAY OFF TO COMPUTE THESE HERE
        %B1(sub2ind(size(I),IP,K(:,ki)')) = found.*B(:,1);
        %B2(sub2ind(size(I),IP,K(:,ki)')) = found.*B(:,2);
        %B3(sub2ind(size(I),IP,K(:,ki)')) = found.*B(:,3);
        %if size(F,2) == 4
        %  B4(sub2ind(size(I),IP,K(:,ki)')) = found.*B(:,4);
        %end
        % Peel off found
        IP = IP(~found);
        if isempty(IP)
          break;
        end
        K = K(~found,:);
      end

      %for p = 1:numel(IP)
      %  Kp = K(p,:);
      %  U = V(F(Kp,:),:);
      %  FKp = reshape((1:size(U,1))',size(U,1)/size(F,2),size(F,2));
      %  I(IP(p),Kp) = in_element_brute_force(U,FKp,P(IP(p),:));
      %end
      %IP = find(~any(I,2));

      if isempty(IP)
        break;
      end
      prev_k = k;
      k = min(prev_k*2,size(BC,1));
      if k == size(BC,1)
        if ~quiet
          warning('Some points not found');
        end
        break;
      end
    end

    %return;

  end

  if first
    % Only keep first
    [mI,J] = max(I,[],2);
    I = sparse(1:size(I,1),J,mI,size(I,1),size(I,2));
  end

  % Compute barycentric coordinates
  if nargout > 1
    B1 = sparse(size(I,1),size(I,2));
    B2 = sparse(size(I,1),size(I,2));
    B3 = sparse(size(I,1),size(I,2));
    B4 = sparse(size(I,1),size(I,2));
    work_I = I;
    while true
      % Peel off a layer
      [mIf,Jf] = max(work_I,[],2);
      If = find(mIf);
      Jf = Jf(If);
      mIf = mIf(If);
      if isempty(If)
        break
      end
      work_I = work_I - sparse(If,Jf,mIf,size(work_I,1),size(work_I,2));;
      Pf = P(If,:);
      Ff = F(Jf,:);
      switch size(Ff,2)
      case 4
        Bf = barycentric_coordinates(Pf, ...
          V(Ff(:,1),:),V(Ff(:,2),:),V(Ff(:,3),:),V(Ff(:,4),:));
      case 3
        Bf = barycentric_coordinates(Pf,V(Ff(:,1),:),V(Ff(:,2),:),V(Ff(:,3),:));
      case 2
        Bf = barycentric_coordinates(Pf,V(Ff(:,1),:),V(Ff(:,2),:));
      end
      B1(sub2ind(size(B1),If,Jf)) = Bf(:,1);
      B2(sub2ind(size(B2),If,Jf)) = Bf(:,2);
      if size(Bf,2) >= 3
        B3(sub2ind(size(B3),If,Jf)) = Bf(:,3);
      end
      if size(Bf,2) >= 4
        B4(sub2ind(size(B3),If,Jf)) = Bf(:,4);
      end
    end
  end

end


function [ y, m, d, ierror ] = ymd_check_eg_civil ( y, m, d )

%*****************************************************************************80
%
%% YMD_CHECK_EG_CIVIL checks an Egyptian Civil YMD date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input/output, integer Y, M, D, the YMD date, which may
%    be corrected if necessary and possible.
%
%    Output, integer  IERROR, is 0 if the date is legal.
%

%
%  Check the year.
%
  if ( y <= 0 )
    ierror = 1;
    return
  end
%
%  Check the month.
%
  [ y, m, ierror ] = ym_check_eg_civil ( y, m );

  if ( ierror ~= 0 )
    return
  end
%
%  Check the day.
%
  [ y, m, d ] = day_borrow_eg_civil ( y, m, d );

  [ y, m, d ] = day_carry_eg_civil ( y, m, d );

  return
end


function OLPS_cli(dataset)
% OLPS_cli: run OLPS toolbox in CLI (available in both Matlab and Octave)
% this program demos different strategies of on-line portfolio selection
% in the mode of command-line interface (CLI)
%
% OLPS_cli(dataset)
%
% dataset: one can choose any of the following data sets
%  - 'djia':    (default) DJIA (US) perioid 01/14/2001 - 01/14/2003
%  - 'msci':    MSCI (global)       peroid 04/01/2006 - 03/31/2010
%  - 'nyse-n':  NYSE (US)           peroid 01/01/1985 - 06/30/2010
%  - 'nyse-o':  NYSE (US)           peroid 07/03/1962 - 12/31/1984
%  - 'sp500':   S&P500(US)          period 01/02/1998 - 01/31/2003
%  - 'tse':     TSE (CA)            peroid 01/04/1994 - 12/31/1998

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file is part of OLPS: http://OLPS.stevenhoi.org/
% Original authors: Bin LI, Doyen Sahoo, Steven C.H. Hoi
% Contributors:
% Change log:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

cd Strategy;

opts.quiet_mode = 1; opts.display_interval = 500;
opts.log_mode = 1; opts.mat_mode = 1;
opts.analyze_mode = 1; opts.progress = 0;
opts.his = 0;

if (nargin<1)
    dataset = 'djia';
end

%----------Benchmarks--------------
manager('ubah', dataset, {0}, opts);
manager('best', dataset, {0}, opts);
manager('ucrp', dataset, {0}, opts);
manager('bcrp', dataset, {0}, opts);

%---------Follow the Winner-------------------
manager('up', dataset, {0}, opts);
manager('eg',  dataset, {0.05, 0}, opts);
manager('ons', dataset, {0, 1, 1/8, 0}, opts);

% manager('sp_start', dataset, {0.25, 0}, opts);
% manager('grw_start', dataset, {0.00005, 0}, opts);
% manager('m0_start', dataset, {0.5, 0}, opts);

%----------Follow the Loser------------------------
manager('anticor', dataset, {30, 0}, opts);
manager('anticor_anticor', dataset, {30, 0}, opts);
manager('pamr', dataset, {0.5, 0}, opts);
manager('pamr_1', dataset, {0.5, 500, 0}, opts);
manager('pamr_2', dataset, {0.5, 500, 0}, opts);
manager('cwmr_var', dataset, {2, 0.5, 0}, opts);
manager('cwmr_stdev', dataset, {2, 0.5, 0}, opts);
manager('olmar1', dataset, {10, 5, 0}, opts);
manager('olmar2', dataset, {10, 0.5, 0}, opts);

%--------Pattern Matching based approach---------
manager('bk', dataset, {5, 10, 1, 0}, opts);
manager('bnn', dataset, {5, 10, 0}, opts);
manager('corn', dataset, {5, 0.1, 0}, opts);
manager('cornu', dataset, {5, 1, 0.1, 0}, opts);
manager('cornk', dataset, {5, 10, 0.1, 0, 1}, opts);

end


function [DataTest DataTrain CTest CTrain Loc_test] = samplesdivide(indian_pines_corrected,indian_pines_gt,train,randpp);

CTrain = [];
CTest = [];
DataTest  = [];
DataTrain = [];

[m n p] = size(indian_pines_corrected);
indian_pines_map = uint8(zeros(m,n));
data_col = reshape(indian_pines_corrected,m*n,p);
[mm nn] = ind2sub([m n],1:m*n);
data_col = [mm' nn' data_col];

for i = 1:max(indian_pines_gt(:))
    ci = length(find(indian_pines_gt==i));
    [v]=find(indian_pines_gt==i);
    datai = data_col(find(indian_pines_gt==i),:);
    if train>1
        cTrain = round(train);
    else
        cTrain  = round(train*ci);
    end
    cTest  = ci-cTrain;
    CTrain = [CTrain cTrain];
    CTest = [CTest cTest];
    index = randpp{i};
    DataTest = [DataTest; datai(index(1:cTest),:)];
    DataTrain = [DataTrain; datai(index(cTest+1:cTest+cTrain),:)];
end

Normalize = max(max(DataTrain(:,3:end)));
DataTrain(:,3:end) = DataTrain(:,3:end)./Normalize;
DataTest(:,3:end)  = DataTest(:,3:end)./Normalize;


function value = i4_mach ( i )

%*****************************************************************************80
%
%% I4_MACH returns integer machine constants.
%
%  Discussion:
%
%    Input/output unit numbers.
%
%      I1MACH(1) = the standard input unit.
%      I1MACH(2) = the standard output unit.
%      I1MACH(3) = the standard punch unit.
%      I1MACH(4) = the standard error message unit.
%
%    Words.
%
%      I1MACH(5) = the number of bits per integer storage unit.
%      I1MACH(6) = the number of characters per integer storage unit.
%
%    Integers.
%
%    Assume integers are represented in the S digit base A form:
%
%      Sign * (X(S-1)*A**(S-1) + ... + X(1)*A + X(0))
%
%    where 0 <= X(1:S-1) < A.
%
%      I1MACH(7) = A, the base.
%      I1MACH(8) = S, the number of base A digits.
%      I1MACH(9) = A**S-1, the largest integer.
%
%    Floating point numbers
%
%    Assume floating point numbers are represented in the T digit
%    base B form:
%
%      Sign * (B**E) * ((X(1)/B) + ... + (X(T)/B**T) )
%
%    where 0 <= X(I) < B for I=1 to T, 0 < X(1) and EMIN <= E <= EMAX.
%
%      I1MACH(10) = B, the base.
%
%    Single precision
%
%      I1MACH(11) = T, the number of base B digits.
%      I1MACH(12) = EMIN, the smallest exponent E.
%      I1MACH(13) = EMAX, the largest exponent E.
%
%    Double precision
%
%      I1MACH(14) = T, the number of base B digits.
%      I1MACH(15) = EMIN, the smallest exponent E.
%      I1MACH(16) = EMAX, the largest exponent E.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 April 2007
%
%  Author:
%
%    Original FORTRAN77 version by Phyllis Fox, Andrew Hall, Norman Schryer
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Phyllis Fox, Andrew Hall, Norman Schryer,
%    Algorithm 528,
%    Framework for a Portable Library,
%    ACM Transactions on Mathematical Software,
%    Volume 4, Number 2, June 1978, page 176-188.
%
%  Parameters:
%
%    Input, integer I, chooses the parameter to be returned.
%    1 <= I <= 16.
%
%    Output, integer VALUE, the value of the chosen parameter.
%
  if ( i < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4_MACH - Fatal error!\n' );
    fprintf ( 1, '  The input argument I is out of bounds.\n' );
    fprintf ( 1, '  Legal values satisfy 1 <= I <= 16.\n' );
    fprintf ( 1, '  I = %d\n', i );
    value = 0;
    error ( 'I4_MACH - Fatal error!' );
  elseif ( i == 1 )
    value = 5;
  elseif ( i == 2 )
    value = 6;
  elseif ( i == 3 )
    value = 7;
  elseif ( i == 4 )
    value = 6;
  elseif ( i == 5 )
    value = 32;
  elseif ( i == 6 )
    value = 4;
  elseif ( i == 7 )
    value = 2;
  elseif ( i == 8 )
    value = 31;
  elseif ( i == 9 )
    value = 2147483647;
  elseif ( i == 10 )
    value = 2;
  elseif ( i == 11 )
    value = 24;
  elseif ( i == 12 )
    value = -125;
  elseif ( i == 13 )
    value = 128;
  elseif ( i == 14 )
    value = 53;
  elseif ( i == 15 )
    value = -1021;
  elseif ( i == 16 )
    value = 1024;
  elseif ( 16 < i )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4_MACH - Fatal error!\n' );
    fprintf ( 1, '  The input argument I is out of bounds.\n' );
    fprintf ( 1, '  Legal values satisfy 1 <= I <= 16.\n' );
    fprintf ( 1, '  I = %d\n', i );
    value = 0;
    error ( 'I4_MACH - Fatal error!' );
  end

  return
end


%MTRAJ Multi-axis trajectory between two points
%
% [Q,QD,QDD] = MTRAJ(TFUNC, Q0, QF, M) is a multi-axis trajectory (MxN) varying
% from configuration Q0 (1xN) to QF (1xN) according to the scalar trajectory function
% TFUNC in M steps. Joint velocity and acceleration can be optionally returned as
% QD (MxN) and QDD (MxN) respectively.  The trajectory outputs have one row per
% time step, and one column per axis.
%
% The shape of the trajectory is given by the scalar trajectory function
% TFUNC which is applied to each axis:
%      [S,SD,SDD] = TFUNC(S0, SF, M);
% and possible values of TFUNC include @lspb for a trapezoidal trajectory, or
% @tpoly for a polynomial trajectory.
%
% [Q,QD,QDD] = MTRAJ(TFUNC, Q0, QF, T) as above but T (Mx1) is a time
% vector which dictates the number of points on the trajectory.
%
% Notes::
% - If no output arguments are specified Q, QD, and QDD are plotted.
% - When TFUNC is @tpoly the result is functionally equivalent to JTRAJ except
%   that no initial velocities can be specified. JTRAJ is computationally a little
%   more efficient.
%
% See also JTRAJ, MSTRAJ, LSPB, TPOLY.

% Copyright (C) 1993-2017, 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

function [S,Sd,Sdd] = mtraj(tfunc, q0, qf, M)

    if ~isa(tfunc, 'function_handle')
        error('first argument must be a function handle');
    end

    M0 = M;
    if ~isscalar(M)
        M = length(M);
    end
    if numcols(q0) ~= numcols(qf)
        error('must be same number of columns in q0 and qf')
    end

    s = zeros(M, numcols(q0));
    sd = zeros(M, numcols(q0));
    sdd = zeros(M, numcols(q0));

    for i=1:numcols(q0)
        % for each axis
        [s(:,i),sd(:,i),sdd(:,i)] = tfunc(q0(i), qf(i), M);
    end

% - If no output arguments are specified S, SD, and SDD are plotted
%   against time.

    switch nargout
        case 0
            clf

            if isscalar(M0)
                t = [1:M0]';
            else
                t = M0;
            end
            subplot(311)
            plot(t, s); grid; ylabel('s');

            subplot(312)
            plot(t, sd); grid; ylabel('sd');

            subplot(313)
            plot(t, sdd); grid; ylabel('sdd');
            if ~isscalar(M0)
                xlabel('time')
            else
                for c=get(gcf, 'Children');
                    set(c, 'XLim', [1 M0]);
                end
            end
            shg
        case 1
            S = s;
        case 2
            S = s;
            Sd = sd;
        case 3
            S = s;
            Sd = sd;
            Sdd = sdd;
    end


function value = meixner ( n, beta, c, x )

%*****************************************************************************80
%
%% MEIXNER evaluates Meixner polynomials at a point.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Walter Gautschi,
%    Orthogonal Polynomials: Computation and Approximation,
%    Oxford, 2004,
%    ISBN: 0-19-850672-4,
%    LC: QA404.5 G3555.
%
%  Parameters:
%
%    Input, integer N, the maximum order of the polynomial.
%    N must be at least 0.
%
%    Input, real BETA, the Beta parameter.  0 < BETA.
%
%    Input, real C, the C parameter.  0 < C < 1.
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE(N+1), the value of the polynomials at X.
%
  if ( beta <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MEIXNER - Fatal error!\n' );
    fprintf ( 1, '  Parameter BETA must be positive.\n' );
    error ( 'MEIXNER - Fatal error!\n');
  end

  if ( c <= 0.0 || 1.0 <= c )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MEIXNER - Fatal error!\n' );
    fprintf ( 1, '  Parameter C must be strictly between 0 and 1.\n' );
    error ( 'MEIXNER - Fatal error!\n');
  end

  if ( n < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MEIXNER - Fatal error!\n' );
    fprintf ( 1, '  Parameter N must be nonnegative.\n' );
    error ( 'MEIXNER - Fatal error!\n');
  end

  OFFSET = 1;

  value(0+OFFSET) = 1.0;

  if ( n == 0 )
    return
  end

  value(1+OFFSET) = ( c - 1 ) * x / beta / c + 1.0;

  if ( n == 1 )
    return
  end

  for i = 1 : n - 1
    value(i+1+OFFSET) = ( ...
      ( ( c - 1.0 ) * x + ( 1.0 + c ) * i + beta * c ) * value(i+OFFSET) ...
      - i * value(i-1+OFFSET) ...
      ) / ( i + beta );
  end

  return
end


function [dfdx, fx] = VBA_numericDiff(fName, idxArg2Diff, varargin)
% // VBA toolbox //////////////////////////////////////////////////////////
%
% [dfdx, fx] = numericDiff(fname, idxArg2Diff, fArg1, fArg2, ...)
% numerical evaluation of derivatives
%
% This function evaluates numerically the derivatives of the function
% 'fname' with respect to its 'idxArg2Diff' input argument at the point
% defined by the input arguments [fArg1, fArg2, ..., fArgN]
%
% IN:
%   - fname: handle or name of the function to be differentiated.
%     ie either its name or a function handle. The latter can be very useful
%     if the function is itself a subfunction of a function!
%   - idxArg2Diff: the index of the argument to differentiate the function
%     with (1 <= idxArg2Diff <= numel(varargin))
%   - fArg1, fArg2, ..., fArgN: list of arguments which is required to call
%     the function 'fname', and which defines the ordinate at which the
%     derivative will be numerically evaluated
%
% OUT:
%   - dfdx: an m x p matrix containing the numerical differentiation
%     of the function evaluated at {fArg1, ..., fArgN}, where p is the
%     number of elements of the function's first output and m is the number
%     of elements of the idxArg2Diff input argument. If the function output
%     or the differentiated argument are in matrix form, they will be
%     vectorized first.
%   - f: the function evaluation at x
%
% NB: Mixed partials can be obtained by recursive call of this routine,
% e.g. consider:
%
% dfdx_idx_j = numericDiff(@numericDiff,i+2,fname,j,arg1,arg2,...,argn)
% dfdx_idx_j = reshape(dfdx_idx_j,mi,mj,p)
%
% The first line evaluates the numerical derivative of the function
% numericDiff(fname,j,arg1,arg2,...,argn) wrt to its (i+2)th entry, which
% is arg_i.
% The second line reshapes the output such that the first dimension is the
% dimension of argi (mi), the second is of the dimension of argj (mj), and
% the last dimension is the one of the function output itself (p).
%
% /////////////////////////////////////////////////////////////////////////

% perturbation scale
epsilon = 1e-4;

% shortcut
fArgs = varargin;

% evaluate function at the specified argument
% =========================================================================
try
    fx = VBA_vec(fName(fArgs{:}));
catch
    message = sprintf(...
        '*** VBA_numericDiff: Can not call %s with the provided arguments (nArgs = %d).', ...
        func2str(fName), numel(fArgs));
    error(message);
end

% evaluate the perturbation df of the function fname in the neighbourhood
% of the specified argument (ie at x + dx) and calculate the derivative wrt x
% =========================================================================

% pre-allocate the variables
m = numel (fArgs{idxArg2Diff});
p = numel (fx);
dfdx = zeros (m, p);

% loop over the dimension of idxArg2Diff-th argument
dx = epsilon * fArgs{idxArg2Diff};
dx (abs(dx) <= eps) = epsilon;

% compute effect of pertubations
for i = 1 : m
    xpdx = fArgs;
    xpdx{idxArg2Diff}(i) = xpdx{idxArg2Diff}(i) + dx(i);
    dfdx(i,:) = (VBA_vec (fName (xpdx{:})) - fx)' / dx(i);
end


function scale_change_factor = scale_filter_track(im, pos, base_target_sz, currentScaleFactor, scale_filter, params)

% Track the scale using the scale filter.

% Get scale filter features
scales = currentScaleFactor*scale_filter.scaleSizeFactors;
xs = extract_scale_sample(im, pos, base_target_sz, scales, params.scale_model_sz, params.use_mexResize);

% Project
xs = feature_projection_scale(xs, scale_filter.basis, scale_filter.window);

% Get scores
xsf = fft(xs, [], 2);
scale_responsef = sum(scale_filter.sf_num .* xsf, 1) ./ (scale_filter.sf_den + params.lambda);
interp_scale_response = ifft(resizeDFT(scale_responsef, params.number_of_interp_scales), 'symmetric');

recovered_scale_index = find(interp_scale_response == max(interp_scale_response(:)), 1);

if params.do_poly_interp
    % Fit a quadratic polynomial to get a refined scale
    % estimate.
    id1 = mod(recovered_scale_index -1 -1,params.number_of_interp_scales)+1;
    id2 = mod(recovered_scale_index +1 -1,params.number_of_interp_scales)+1;

    poly_x = [scale_filter.interpScaleFactors(id1), scale_filter.interpScaleFactors(recovered_scale_index), scale_filter.interpScaleFactors(id2)];
    poly_y = [interp_scale_response(id1), interp_scale_response(recovered_scale_index), interp_scale_response(id2)];

    poly_A_mat = [poly_x(1)^2, poly_x(1), 1;...
        poly_x(2)^2, poly_x(2), 1;...
        poly_x(3)^2, poly_x(3), 1 ];

    poly = poly_A_mat\poly_y';

    scale_change_factor = -poly(2)/(2*poly(1));
else
    scale_change_factor = scale_filter.interpScaleFactors(recovered_scale_index);
end


function [results]=VBA_Shapley(posterior,out,varargin)
% [results]=VBA_Shapley(posterior,out,[options])
%
% Compute the Shapley values of the model's factors (inputs or paramters).
% These scores measure the relative influence of these factor on the variaince
% explained by the model.
% -------------------------------------------------------------------------
% IN:
%  - posterior, out: model structures
%  - varargin:       list of option/values pairs (see below)
% OUT:
%  - results: structure of results with the following fields, depending on
%  the chose coalitions
%  .parameters/inputs:
%         Shapley values (first order) of paramters/inputs. This is a n x p
%         matrix where n is the number of factors of interest and p the
%         number of observations in the model.
%  - interaction: Interaction score, ie relative change in Shapley values for a
%         perturbation in respective inputs. It is an array with the same
%         size as the inputs whose elements are similar to sv.
% -------------------------------------------------------------------------
% options:
%   coalitions:         ('parameters') | 'inputs' | 'interactions'
%      > factor of interest of the analysis. For 'interaction', compute the
%      Shapley value of parameters and their relative change for selective
%      input perturbations.
%   inputPerturbation   -> ('zero') | 'average' | 'average_nonzero'
%      > type of perturbation to apply on inputs: set to zero, average
%      accross the experiment, or set non zero inputs to their average
%   paramType           -> ('phi')  | 'theta'
%      > parameters of interest.
%   paramIdx            -> (all)  | paramIdx
%      > restrict parameters of interest to the index list paramIdx
%   inputIdx            -> (all)  | inputIdx
%      > restrict inputs of interest to the index list inputIdx
%   obsIdx              -> (all)  | ObsIdx
%      > restrict observations of interest to the index list obsIdx
% -------------------------------------------------------------------------

%% Complete option structure
% -------------------------------------------------------------------------

if numel(varargin) == 1 && isstruct(varargin{1})
    options = varargin{1};
else
    options.coalitions = {'parameters','inputs','interactions'};
    options.inputPerturbation = {'zero','average','average_nonzero'};
    options.paramType = {'phi','theta'};
    options.paramIdx = 1:out.dim.n_phi ;
    options.inputIdx = 1:out.dim.u ;
    if size(out.y,2) == 1 % catch vertical data as unique observation
        options.obsIdx = 0;
    else
        options.obsIdx = 1:out.dim.p ;
    end
    parser = inputParser;
    parser.parse (varargin{:});
    options = parser.Results;
    %options = parseargs(options,varargin{:});
end

%% Prepare perturbation scheme
% -------------------------------------------------------------------------

% dimensions
nu = numel(options.inputIdx);
nw = numel(options.paramIdx);
if options.obsIdx==0 % catch vertical data as unique observation
    nResps=1;
else
    nResps = numel(options.obsIdx);
end

% factorial perturbations on coalitions of interest
switch options.coalitions
    case 'interactions'
        % factorial perturbation of parmeeters with normal inputs,
        kw = full(VBA_spm_perm_mtx(nw));
        k = [kw ones(2^nw,nu)] ;
        % factorial perturbation of parameters with each input respectively
        % pertrubed
        ku = ones(nu)-eye(nu);
        for i=1:nu
            k = vertcat(k, [kw repmat(ku(i,:),size(kw,1),1)]);
        end
        % plus factorial perturbation of inputs alone
        % (ie 2^nw + nu x 2^nw + 2^nu coalitions)
        k = vertcat(k,[ones(2^nu,nw) full(VBA_spm_perm_mtx(nu))]);
    case 'parameters'
        % factorial perturbation of paramters, normal inputs
        k = full(VBA_spm_perm_mtx(nw));
        k = [k ones(size(k,1),nu)];
    case 'inputs'
        % factorial perturbation of inputs, normal paramters
        k = full(VBA_spm_perm_mtx(nu));
        k = [ones(size(k,1),nw) k];
end
nk = size(k,1);

%% Compute explained variances
% -------------------------------------------------------------------------

% loop over coalitions
ve = nan(nk,nResps);
parfor t = 1:size(k,1)
    kt = k(t,:);
    w_perm = kt(1:nw);
    u_perm = kt(nw+(1:nu));
    ve(t,:) = explainedVar(posterior,out,options,u_perm,w_perm) ;
end

% normalize
ve1 = ve(1,:);
ve0 = ve(end,:);
for i=1:nResps
    ve(:,i) = (ve(:,i) - ve0(i) )/(ve1(i)-ve0(i)) ;
end

%% Compute Shapley values
% -------------------------------------------------------------------------

% restrict coalitions to effects of interest
switch options.coalitions
    case 'interactions'
        spl = [2^nw*ones(nu+1,1); 2^nu];
        k = mat2cell(k,spl,nw+nu);
        for i=1:nu+1
            k{i} = k{i}(:,1:nw);
        end
        k{nu+2} = k{nu+2}(:,nw+(1:nu));
        ve = mat2cell(ve,spl,nResps);
    case 'parameters'
        k = {k(:,1:nw)};
        ve = {ve};
    case 'inputs'
        k = {k(:,nw+(1:nu))};
        ve = {ve};
end

% compute first order scores
for ii = 1:numel(k)
    n = size(k{ii},2);
    nn = factorial(n);
%     v{ii} = nan(n,nResps);
    for m=1:n % loop over players
        % Shapley coeficients
        i = k{ii}(:,m);
        z = sum(k{ii},2);
        coef = (2*i-1).*factorial(z-i).*factorial(n-z-(1-i))/nn;
        % compute shapley values per se
        v{ii}(m,:) =  coef'*ve{ii};
    end
end

% compute interactions if necessary
sv = v{1};
if strcmp(options.coalitions,'interactions')
    for iu=1:nu
        svi{iu} = (v{1}-v{iu+1})./v{1};
    end
    % shapley value of inputs
    svu = v{nu+2};
else
    svi={};
end

%% Store results
% -------------------------------------------------------------------------
switch options.coalitions
    case 'parameters'
        results.parameters = sv;
    case 'inputs'
        results.inputs = sv;
    case 'interactions'
        results.parameters = sv;
        results.inputs = svu;
        results.interactions = svi;
end

end
%% Subfunctions
% =========================================================================

% -------------------------------------------------------------------------
% Compute explained variance of the model given by posterior and out, with
% the inputs and paramters pertrubed according to u_swicth and w_switch
% respectively (switch = 1 -> normal, switch=0 -> perturbation)
% -------------------------------------------------------------------------
function v=explainedVar(posterior,out,options,u_switch,w_switch)

    % prevent unecessary bells and whistles
    out.options.verbose = 0;
    out.options.DisplayWin = 0;
    out.options.inF{1}.fast = true;

    % prepare degraded model
    % .....................................................................

    % == pertub inputs
    % index of inputs to perturb
    inputIdx = options.inputIdx(u_switch==0);
    % apply perturbation
    switch options.inputPerturbation
        case 'zero'
            out.u(inputIdx,:) = 0;
        case 'average'
            out.u(inputIdx,:) = mean(out.u(inputIdx,:),2);
        case 'average_nonzero'
            for iu=inputIdx
                idxNZ = find(out.u(iu,:)~=0);
                out.u(iu,idxNZ) = mean(out.u(iu,idxNZ));
            end
    end

    % == perturb parameters
    paramIdx = options.paramIdx(w_switch==0);
    switch options.paramType
        case 'phi'
            posterior.muPhi(paramIdx) = 0*posterior.muPhi(paramIdx);
        case 'theta'
            posterior.muTheta(paramIdx) = 0*posterior.muTheta(paramIdx);
    end

    % predict data
    % .....................................................................
    [yp,~,~,~,er] = VBA_simulate (...
        out.options.dim.n_t,...
        out.options.f_fname,...
        out.options.g_fname,...
        posterior.muTheta,...
        posterior.muPhi,...
        out.u,...
        Inf,...
        Inf,...
        out.options,...
        posterior.muX0);
    g = yp-er;
    y = out.y;

    % if vertical data, transpose everything
    if options.obsIdx == 0
        g = g';
        y = y';
        options.obsIdx = 1;
        out.options.isYout = out.options.isYout';
    end

    % compute explained variance
    % .....................................................................
    v = nan(1,numel(options.obsIdx));
    for i=1:numel(options.obsIdx) % for each observation of interest
        obsIdx = options.obsIdx(i);
        in_idx = find(out.options.isYout(obsIdx,:) == 0);
        v(i) = 1-((var(y(obsIdx,in_idx)-g(obsIdx,in_idx))/var(y(obsIdx,in_idx))))  ;
    end

end