function [HBLog2D, HBLogAv, ...
    mB2D, mBAv, ...
    names2D, namesAv, ...
    namesmB2D, namesmBAv] = ...
    extractHillBands(data, params)

nChans = size(data,2);
nBands = numel(params.HillsBands.Range);

HBLog = NaN(nBands, nChans);
for c = 1:nChans
    % T = 1/Fs;
    L = size(data,1);
    % t = (0:L-1)*T;

    Y = fft(data(:,c));

    P2 = abs(Y/L);
    P1 = P2(1:L/2+1);
    P1(2:end-1) = 2*P1(2:end-1);

    f = params.Fs*(0:(L/2))/L;

    if params.plotOn
        plot(f,P1)
        title('Single-Sided Amplitude Spectrum of X(t)')
        xlabel('f (Hz)')
        ylabel('|P1(f)|')
    end

    for h = 1:nBands
        hz = params.HillsBands.Range(h);

        HBLog(h,c) = mean(P2(round(f) == hz));

    end
end

% Log10
HBLog2D = log10(HBLog);
% Remove infs
HBLog2D(isinf(HBLog2D)) = 0;
% Mean across channels
HBLogAv = mean(HBLog2D,2)';

[~, mIdx] = max(HBLog2D);
mB2D = single(mIdx);
mBAv = mean(mB2D);

namesmB2D = (string('maxHills_c') + (1:16)')';
namesmBAv = 'maxHillsAv';

nFs = 47;
HBLog2D = reshape(HBLog2D, 1, nChans*nFs);

names2D = cellstr([repmat('hillBands2D_', nFs*16, 1), ...
    repmat(num2str((1:nFs)'), 16,1), ...
    repmat('_c', nFs*16,1), ...
    num2str(reshape(repmat((1:16),nFs,1),nFs*16,1))])';
names2D = strrep(names2D, ' ', '');

namesAv = cellstr([repmat('hillsBandAv_',47,1), num2str((1:47)')])';
namesAv = strrep(namesAv, ' ', '');


function [x, infos] = online_mu_nmf(V, rank, in_options)
% Online non-negative matrix factorization (Online-NMF) algorithm.
%
% Inputs:
%       matrix      V
%       rank        rank
%       in_options  options
% Output:
%       w           solution of w
%       infos       information
%
% References:
%       S. S. Bucak, B. Gunsel,
%       "Incremental Subspace Learning via Non-negative Matrix Factorization,"
%       Pattern Recognition, 2009.
%
%
% This file is part of NMFLibrary.
%
% Created by H.Kasai and H.Sakai on Feb. 12, 2017
%
% Change log:
%
%       Oct. 27, 2017 (Hiroyuki Kasai): Fixed algorithm.
%
%       May. 20, 2019 (Hiroyuki Kasai): Added initialization module.
%
%       Jul. 12, 2022 (Hiroyuki Kasai): Modified code structures.
%

    % set dimensions and samples
    [m, n] = size(V);

    % set local options
    local_options = [];

    % check input options
    if ~exist('in_options', 'var') || isempty(in_options)
        in_options = struct();
    end
    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    % initialize factors
    init_options = options;
    [init_factors, ~] = generate_init_factors(V, rank, init_options);
    Wt = init_factors.W;
    H = init_factors.H;
    R = init_factors.R;

    % initialize
    method_name = 'Online-MU';
    epoch = 0;
    grad_calc_count = 0;

    if options.verbose > 0
        fprintf('# %s: started ...\n', method_name);
    end

    % store initial info
    clear infos;
    [infos, f_val, optgap] = store_nmf_info(V, Wt, H, R, options, [], epoch, grad_calc_count, 0);

    if options.verbose > 1
        fprintf('%s: Epoch = 0000, cost = %.16e, optgap = %.4e\n', method_name, f_val, optgap);
    end

    % set start time
    start_time = tic();

    % main outer loop
    while true

        % check stop condition
        [stop_flag, reason, max_reached_flag] = check_stop_condition(epoch, infos, options);
        if stop_flag
            display_stop_reason(epoch, infos, options, method_name, reason, max_reached_flag);
            break;
        end

        % Reset sufficient statistic
        At = zeros(m, rank);
        Bt = zeros(rank, rank);

        % main inner loop
        for t = 1 : options.batch_size : n - 1

            % Retrieve vt and ht
            vt = V(:, t:t+options.batch_size-1);
            ht = H(:, t:t+options.batch_size-1);

            % uddate ht
            ht = ht .* (Wt.' * vt) ./ (Wt.' * (Wt * ht));
            ht = ht + (ht<eps) .* eps;

            % update sufficient statistics
            At = At + vt * ht';
            Bt = Bt + ht * ht';

            % update W
            Wt = Wt .* At ./ (Wt * Bt);
            Wt = Wt + (Wt<eps) .* eps;

            % store new h
            H(:,t:t+options.batch_size-1) = ht;

            grad_calc_count = grad_calc_count + m * options.batch_size;
        end

        % measure elapsed time
        elapsed_time = toc(start_time);

        % update epoch
        epoch = epoch + 1;

        % store info
        infos = store_nmf_info(V, Wt, H, R, options, infos, epoch, grad_calc_count, elapsed_time);

        % display info
        display_info(method_name, epoch, infos, options);

    end

    x.W = Wt;
    x.H = H;

end


function [x, timing] = blendenpik(A, b, params)
% [x, timing] = blendenpik(A, b, params)
%
% Solve the equation x = arg min norm(A * x - b, 2)
% using Blendenpik.
%
% "params" - parameters governning the method.
%    params.type - type of mixing transform. Optional values: 'DCT', 'DHT', 'WHT'.
%                  Default is DHT.
%    params.gamma - gamma * min(n,m) rows/columns will be sampled (A is m-by-n).
%                   Default is 4.
%    params.preprocess_steps - number of mixing steps to do in advance.
%                               Default is 1.
%    params.maxcond - maximum condition number of the preconditioner.
%                     Default is 1 / (5 * epsilon_machine).
%    params.tol - convergence thershold for LSQR.
%                 Default is 1e-14.
%    params.maxit - maximum number of LSQR iterations.
%                   Default is 1000.
%    params.lsvec - whether to output in "timing" the LSQR residuals.
%                   Default is false.
%    params.use_full_lsqr - whether to use LSQR with full
%                           orthogonalization. Useful for
%                           preprocess_steps=0.
%                           Default is false.
%
% Output:
%   x - the solution.
%   timing - statistics on the time spent on various phases.
%
% 6-December 2009, Version 1.3
% Copyright (C) 2009, Haim Avron and Sivan Toledo.

if (nargin < 3)
    params = struct;
end

[m, n] = size(A);
if (m >= n)
    x = blendenpik_over(A, b, params);
else
    x = blendenpik_under(A, b, params);
end


function [imax, xfin, s2fin, ufin, Cxx, uout] = OLOO(A, y, Q)
% SYNTAX:
%   [imax, xfin, s2fin, ufin, Cxx, uout] = OLOO(A, y, Q)
%
% INPUT:
%   A: design matrix
%   y: observations vector
%   Q: cofactor matrix
%
% OUTPUT:
%   imax:  index of the rejected blocks
%   x_fin: estimated parameters without outlier
%   s2fin: a posteriori sigma without outlier
%   ufin:  estimated residuals without outlier
%   Cxx:   parameters covariance of the final solution
%   uout:  residual of outlier observation
%
% DESCRIPTION:
%   perform LS on blocks of correlated observations
%   identify one (block) outlier
%   reject it
%   re-estimate unknowns
%   according to the theory in "L. Biagi and S. Caldera. An efficient leave one block out approach to identify outliers.Journal of Applied Geodesy, Volume 7, Issue 1, pages 11..19, 2013"
%
%   this version is optimized to manage l.o.o. of 1 observation at time, no blocks!
%
% CREDITS:
%   1.0: Stefano Caldera, 22.05.2014
%   1.1: Stefano Caldera, Andrea Gatti 08.12.2016 ( speedup improvements )

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:       Stefano Caldera 22.05.2014
%  Contributors:     Stefano Caldera,
%                    Andrea Gatti 08.12.2016 ( speedup improvements )
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

% when FTABLE is undefined, redefine it
global FTABLE; if isempty(FTABLE); FTABLE = finv(0.9995,1,1:size(A,1))'; end
if (size(FTABLE,1) < size(A,1)); FTABLE = finv(0.9995,1,1:size(A,1))'; end
n_blocks = length(y);
[m, n] = size(A);           % m: number of observations, n: number of unknowns
uout = NaN;

if m - n > 0

    % compute the global solution
    if isdiag(Q) % Q is tipically diagonal -> let's use this information
        Qm = diag(Q);
        Qm = Qm ./ min(Qm);
        invQ = diag(1./Qm);
        At_invQ = A' * invQ;
        Ninv = inv(At_invQ * A);
        xcap = Ninv * At_invQ * y;
        um = y - A * xcap;
        s2cap = um' * invQ * um/(m-n);
    else
        Q = Q ./ (min(diag(Q)));
        At_invQ = A'/Q;
        Ninv = inv(At_invQ * A);
        xcap = Ninv * At_invQ * y;
        um = y - A * xcap;
        s2cap = um' / Q * um/(m-n);
        Qm = diag(Q);
    end
    % convert A in a sparse matrix, faster and lighter

    use_sparse_approach = false;

    if sum(A(1,:)==0) > size(A,2) / 0.577     % if A is sparse enough
        use_sparse_approach  = true;
        A = sparse(A);
    end
    if m - n > 1
        %% start outliers rejection
        Im = eye(n);
        Bm = Ninv * A';
        Cm = diag(A * Bm);
        Km = Qm - Cm;
        Kminv = Km.^-1;
        wm = Qm .* Kminv .* um;
        s2m = ((m-n) .* s2cap - um .* Kminv .* um) ./ (m - n - 1);

        % original loop
        if use_sparse_approach
            % modified loop to exploit the sparse property of the A matrix
            Qw = zeros(n_blocks,1);
            tmp = (A .* repmat(Kminv,1,n));
            for i = 1 : n_blocks
                idOk = (tmp(i,:) ~= 0);
                Qw(i) = Qm(i) + Bm(idOk,i)' * (Im(idOk,idOk) + tmp(i,idOk)' * Bm(idOk,i)')  * A(i,idOk)';
            end
        else
            Qw = zeros(n_blocks,1);
            for i = 1 : n_blocks
                Qw(i) = Qm(i) + Bm(:,i)' * (Im + A(i,:)' * Kminv(i) * Bm(:,i)') * A(i,:)';
                %Qw3=Bm(:,i)'*(Im+A(i,:)'*Kminv(i)*Bm(:,i)');
            end
        end

        %toc
        Qwinv = Qw.^-1;
        deg2 = m - n - 1;

        F = wm .* Qwinv .* wm ./ s2m;
        Flim = FTABLE(deg2);

        %% apply final solution
        % find maximum F(i)/Flim(i)
        [Fmax, imax] = max(abs(F ./ Flim));

        if (Fmax < 1)
            % no outlier
            imax = 0;
            xfin = xcap;
            s2fin = s2cap;
            Ninvfin = Ninv;
            ufin = um;
            Cxx = s2fin * Ninvfin;

        else
            % if the maximum ratio exceedes the threshold, the observation is eliminated from the solution
            uout = um(imax);
            xfin = xcap - Bm(:, imax) * Kminv(imax) * um(imax);
            yfin = y;
            yfin(imax) = [];

            Afin = A;
            Afin(imax,:) = [];

            s2fin = s2m(imax);

            Ninvfin = Ninv + Bm(:, imax) * Kminv(imax) * Bm(:, imax)';
            ufin = yfin - Afin * xfin;
            Cxx = s2fin * Ninvfin;

        end

    else
        imax = 0;
        xfin = xcap;
        s2fin = s2cap;
        Cxx = s2cap * Ninv;
        ufin = um;
    end

else
    % detection is not possibile
    imax = 0;
    xfin = [];
    Cxx = [];
    ufin = [];
    s2fin = NaN;
end


function [patchSum] = evaluateIntegralImage(ii, row, col, delta)
% This function should calculate the sum over the patch centred at row, col
% of size patchSize of the integral image ii

 % NOTE : the integral image now has an extra row and an extra column
 % so we have to add a 1 to all our indices.
 % We can notice that when we subtract delta we should've had a minus 1 as
 % well but because we're also adding a 1 is just row-delta.
 %

 row_plus  = min(row+delta+1, size(ii,1));
 row_minus = max(row-delta, 1);
 col_plus  = min(col+delta+1, size(ii,2));
 col_minus = max(col-delta, 1);

% SOME DEBUG CODE
% disp(row_plus);
% disp(row_minus);
% disp(clu_plus);
% disp(col_minus);

 patchSum = ii(row_plus,  col_plus)     ...
         + ii(row_minus, col_minus)    ...
         - ii(row_minus, col_plus)     ...
         - ii(row_plus,  col_minus);

end


function th = threeshold_estimation(distance,method)
    if nargin < 2
        method = 'mean';
    end

    switch method
        case 'mean'
            th = mean(distance)+2.5*std(distance);
        case 'median'
            th = median(distance)+2.5*1.4826*mad(distance,1);
    end


function [G] = gsp_hypergraph(N,E, w, coords, limits)
%GSP_HYPERGRAPH  Initialize a hypergraph from a set of edges and weights
%   Usage:  G = gsp_hypergraph(N,E);
%           G = gsp_hypergraph(N,E, w);
%           G = gsp_hypergraph(N,E, w, coords);
%           G = gsp_hypergraph(N, E, w, coords, limits);
%
%   Input parameters:
%         N     : Number of nodes
%         E     : Set of edges (cell array)
%         w     : weights of the edges (default all ones)
%         coords: Coordonates of the points (optional)
%         limits: limits for the coordonates (optional)
%   Output parameters:
%         G     : Graph structure.
%
%   Example:::
%
%         N = 100;
%         Nf = 2;
%         k = 4;
%         x = rand(N,Nf);
%         paramnn.k = k;
%         [indx, indy, d] = gsp_nn_distanz(x',x',paramnn);
%         sigma = mean(d)^2;
%         wt = exp(-d.^2/sigma);
%         E = cell(N,1);
%         w = zeros(N,1);
%         for ii = 1:N
%             edge = indx((1:k)+(ii-1)*k);
%             E{ii} = edge;
%             w(ii) = sum(wt(edge));
%         end
%
%         G = gsp_hypergraph(N,E,w)
%
%   See also: gsp_nn_hypergraph gsp_graph

% Author: Nathanael Perraudin
% Date: 21  October 2015

if nargin<3
    w = ones(numel(E));
end

if nargin > 3
    G.coords = coords;
end
if nargin > 4
    G.plotting.limits = limits;
end
G.N = N;
G.Ne = numel(E);
G.W = sparse(N,G.Ne);
G.E = E;
for ii = 1:G.Ne
    % Here we use W for HW...
    G.W(G.E{ii},ii) = sqrt(w(ii));
end

G.type = 'hypergraph from edges';
G.directed = 0;
G.hypergraph = 1;

G = gsp_graph_default_parameters(G);

end


% DEMCLASSIFICATIONTWOIVM1 IVM for classification on a data-set sampled from a GP

% IVM

% Fix seeds
randn('seed', 1e5);
rand('seed', 1e5);

dataSetName = 'classificationTwo';
experimentNo = 1;

% load data
[X, y] = mapLoadData(dataSetName);

% Set up model
options = ivmOptions;
options.display = 2;
options.numActive = 200;
options.kern = {'rbf', 'white'};

model = ivmCreate(size(X, 1), size(y, 2), X, y, options);

if options.display > 1
  ivm3dPlot(model, 'ivmContour', i);
end
for i = 1:options.extIters;

  % Select the active set.
  model = ivmOptimiseIvm(model, options.display);
  % Plot the data.
  if options.display > 1
    ivm3dPlot(model, 'ivmContour', i);
  end
  % Optimise the kernel parameters.
  model = ivmOptimiseKernel(model, options.display, options.kernIters);
end
model = ivmOptimiseIvm(model, options.display);
if options.display > 1
  ivm3dPlot(model, 'ivmContour', i);
end
% display active points.
model = ivmOptimiseIvm(model, options.display);

% Display the final model.
ivmDisplay(model);

% Save the results.
capName = dataSetName;;
capName(1) = upper(capName(1));
[kern, noise, ivmInfo] = ivmDeconstruct(model);
save(['dem' capName num2str(experimentNo) '.mat'], ...
     'kern', ...
     'noise', ...
     'ivmInfo');

if exist('printDiagram') & printDiagram
  ivmPrintPlot(model, 'ivmContour', [], [], [], capName, experimentNo);
end


function [rU, WU, CU, HU] = uncovered_engine(X, T, ...
	initial_equity, distribution, risk_free_rate, stock_cost)
%uncovered_engine - Generate scenarios for an uncovered and a covered buy-write strategy.
%
%	[rU, WU, CU, HU] = uncovered_engine(X, T, ...
%	 	initial_equity, distribution, risk_free_rate, stock_cost);
%
% Inputs:
%	Stock Information
%		X - Stock total return prices including initial price [scalar].
%		T - Duration of investment period in years (terminal time - initial time) [scalar].
%	Fund Details
%		initial_equity - Initial (uncovered) total value of stock and cash held in asset [scalar].
%		distribution - Annualized fund distribution [scalar].
%	Other Details
%		risk_free_rate - Annualized risk-free rate [scalar].
%	Costs/Frictions
%		stock_cost - Proportional cost to buy or sell stock [scalar].
%
% Outputs:
%	rU - Total return for uncovered strategy [scalar].
%	WU - Sequence of total wealth for uncovered strategy [vector].
%	CU - Sequence of cash for uncovered strategy [vector].
%	HU - Sequence of holdings for uncovered strategy [vector].
%
% Comments:
%	1) This function generates scenarios for an uncovered portfolio strategy. To introduce some
%		degree of "realism" into the model, several inputs can be specified to control the
%		simulations. The next few comments provide additional details on these inputs.
%	2) X and T are stock total return prices and "times" in years that are assumed to be generated
%		by a geometric Brownian motion process with stochastic differential equation in the form
%			dX(t) = mu*X(t)*dt + volatility*X(t)*dB(t)
%		for t > 0.

% Copyright (C) 2012 The MathWorks, Inc.

% initialization

N = numel(X) - 1;				% N is the number of samples in X excluding the initial price
tau = T/N;						% tau is the time interval between samples in "years"

periods_per_day = floor(N/(252*T) + 0.5);	% periods_per_day is number of periods in a "day"

% initial position

current_price = X(1);									% initial price
current_time = 0;										% initial time

current_shares = floor(initial_equity/current_price);	% initial number of shares
current_cash = max(0, (initial_equity - current_shares*current_price));	% initial cash

initial_wealth = current_cash + current_shares*current_price;

% generate scenarios

% track shares, strike, cash, and wealth over time for testing

if nargout > 2
	HU = zeros(N+1,1);		% (H)oldings in stocks
	CU = zeros(N+1,1);		% (C)ash
	WU = zeros(N+1,1);		% (W)ealth

	HU(1) = current_shares;
	CU(1) = current_cash;
	WU(1) = current_cash + current_shares*current_price;
end

% loop over investment period for uncovered strategy

for iter = 2:N+1

	current_price = X(iter);
	current_time = tau*(iter - 1);

	% accrue interest on cash account at end of each day
	if mod(iter, periods_per_day) == 1		% note that period 1 happens outside loop
		current_cash = current_cash*(1 + risk_free_rate*tau*periods_per_day);
	end

	% buy more stock if enough cash available
	if current_cash > (current_price + stock_cost)
		adjusted_stock_price = current_price + stock_cost;
		shares_purchased = floor(current_cash/adjusted_stock_price);
		current_shares = current_shares + shares_purchased;
		current_cash = current_cash - shares_purchased*adjusted_stock_price;
	end

	% update test variables
	if nargout > 2
		HU(iter) = current_shares;
		CU(iter) = current_cash;
		WU(iter) = current_cash + current_shares*current_price;
	end
end

% at the end of the period, pay a distribution and sell shares if not enough cash

distribution = distribution*T;

needed_cash = distribution*(current_cash + current_shares*X(end)) - current_cash;

if needed_cash > 0
	adjusted_stock_price = X(end) + stock_cost;
	shares_sold = ceil(needed_cash/adjusted_stock_price);

	current_shares = current_shares - shares_sold;
	current_cash = current_cash + shares_sold*(X(end) - stock_cost);

	if nargout > 2
		HU(end) = current_shares;
		CU(end) = current_cash;
		WU(end) = current_cash + current_shares*X(end);
	end
end

% final scenario returns

rU = (current_cash + current_shares*X(end))/initial_wealth - 1;


function [u, s, U_p, U_k, U_d] = pinHoleHmg(p,k,d)

% PINHOLEHMG Pin-hole camera model for HMG points, with optional radial distortion.
%   U = PINHOLEHMG(P) gives the projected pixel U of a homogeneous point P
%   in a canonical pin-hole camera, that is, with calibration parameters
%     u0 = 0
%     v0 = 0
%     au = 1
%     av = 1
%   It uses reference frames {RDF,RD} (right-down-front for the 3D world
%   points and right-down for the pixel), according to this scheme:
%
%         / z (forward)
%        /
%       +------- x                 +------- u
%       |                          |
%       |      3D : P=[x;y;z]      |     image : U=[u;v]
%       | y                        | v
%
%   U = PINHOLE(P,K) allows the introduction of the camera's calibration
%   parameters:
%     K = [u0 v0 au av]'.
%
%   U = PINHOLE(P,K,D) allows the introduction of the camera's radial
%   distortion parameters:
%     D = [K2 K4 K6 ...]'
%   so that the new pixel is distorted following the distortion equation:
%     U_D = U * (1 + K2*R^2 + K4*R^4 + ...)
%   with R^2 = sum(U.^2), being U the projected point in the image plane
%   for a camera with unit focal length.
%
%   [U,S] = PINHOLE(...) returns the depth S from the camera center.
%
%   If P is a points matrix, PINHOLE(P,...) returns a pixel matrix U and a
%   depths row-vector S. P, U and S are defined as
%     P = [P1 ... Pn];   Pi = [xi;yi;zi]
%     U = [U1 ... Un];   Ui = [ui;vi]
%     S = [S1 ... Sn]
%
%   [U,S,U_p,U_k,U_d] returns the Jacobians of U wrt P, K and D. It only
%   works for single points P=[x;y;z].
%
%   See also PINHOLE, INVPINHOLEHMG, PINHOLEIDP.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

if nargout <= 2 % only pixel

    peuc = hmg2euc(p);

    switch nargin
        case 1
            [u, s] = pinHole(peuc);
        case 2
            [u, s] = pinHole(peuc,k);
        case 3
            [u, s] = pinHole(peuc,k,d);
    end

else % Jacobians

    if size(p,2) > 1
        error('Jacobians not available for multiple points')
    else

        [peuc, PEUC_p] = hmg2euc(p);

        switch nargin
            case 1
                [u, s, U_peuc] = pinHole(peuc);
                U_p = U_peuc*PEUC_p;

            case 2
                [u, s, U_peuc, U_k]  = pinHole(peuc,k);
                U_p = U_peuc*PEUC_p;

            case 3
                [u, s, U_peuc, U_k, U_d] = pinHole(peuc,k,d);
                U_p = U_peuc*PEUC_p;
        end

    end

end

return

%% jacobians
syms x y z t u0 v0 au av d2 d4 d6 real
p = [x;y;z;t];
k = [u0;v0;au;av];
d = [d2;d4;d6];

[u, s, U_p, U_k, U_d] = pinHoleHmg(p,k,d);

simplify(U_p - jacobian(u,p))
simplify(U_k - jacobian(u,k))
simplify(U_d - jacobian(u,d))

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

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

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function visualizeBoundary(X, y, model, varargin)
%VISUALIZEBOUNDARY plots a non-linear decision boundary learned by the SVM
%   VISUALIZEBOUNDARYLINEAR(X, y, model) plots a non-linear decision
%   boundary learned by the SVM and overlays the data on it

% Plot the training data on top of the boundary
plotData(X, y)

% Make classification predictions over a grid of values
x1plot = linspace(min(X(:,1)), max(X(:,1)), 100)';
x2plot = linspace(min(X(:,2)), max(X(:,2)), 100)';
[X1, X2] = meshgrid(x1plot, x2plot);
vals = zeros(size(X1));
for i = 1:size(X1, 2)
   this_X = [X1(:, i), X2(:, i)];
   vals(:, i) = svmPredict(model, this_X);
end

% Plot the SVM boundary
hold on
contour(X1, X2, vals, [1 1], 'b');
hold off;

end


function draw_dot(adj,varargin);
%DRAW_DOT   Draw a graph.
% DRAW_DOT(ADJ) plots the graph ADJ in the current figure window, using
% 'neato' to optimize the layout.
%
% Optional arguments can be passed as name/value pairs: [default]
%
% 'isbox'     - a vector specifying which nodes should be boxed [0]
% 'rotate'    - rotate the graph so that nodes are vertically aligned [1]
% 'tolerance' - alignment tolerance for 'rotate' [0.001]
% 'start'     - a random seed (to select different solutions)
% 'options'   - a string of command-line options for 'neato' ['']
% All of the optional arguments to graph_to_dot are also supported, such as
% 'node_label'.
%
% See also GRAPH_TO_DOT.
%
% Example:
% size=15; Adj = rand(size) > .8;
% Adj2 = triu(Adj,1)+ triu(Adj,1)' + diag(zeros(size,1));
% draw_dot(Adj2)

% Original: Leon Peshkin
% Modified by Tom Minka

% minka
N = size(adj,1);
unique_labels = cellstr(num2str((1:N)','%-1d'));
labels = unique_labels;
isbox = zeros(N,1);
rotate_flag = 1;
tolerance = 0.001;
options = '';
for i = 1:2:length(varargin)
  switch varargin{i}
    case 'node_label', labels = varargin{i+1};
      % replace with unique labels
      varargin{i+1} = unique_labels;
    case 'isbox', isbox = varargin{i+1};
    case 'rotate', rotate_flag = varargin{i+1};
    case 'tolerance', tolerance = varargin{i+1};
    case 'start', start = varargin{i+1};
      options = [options ' -Gstart=' num2str(start)];
    case 'options', options = [options ' ' varargin{i+1}];
  end
end

if ispc, shell = 'dos'; else, shell = 'unix'; end  %  Which OS ?

cmdline = strcat(shell,'(''neato -V'')');
status = eval(cmdline);
%[status, result] = dos('neato -V');  % request version to check NEATO
if status == 1,  fprintf('Complaining \n'); exit, end

tmpDOTfile = '_GtDout.dot';            % to be platform independant no use of directories
tmpLAYOUT  = '_LAYout.dot';
graph_to_dot(adj > 0, 'filename', tmpDOTfile, 'node_label', unique_labels, varargin{:});  % save in file

cmdline = strcat([shell '(''neato -Tdot ' tmpDOTfile options ' -o ' tmpLAYOUT ''')']); % preserve trailing spaces
status = eval(cmdline);         %  get NEATO todo layout

[adj, permuted_labels, x, y] = dot_to_graph(tmpLAYOUT);  %  load layout
delete(tmpLAYOUT); delete(tmpDOTfile);     % clean up temporary files

% permute the original arguments to match permuted_labels.
order = [];
for i = 1:length(permuted_labels)
  j = strmatch(permuted_labels{i},unique_labels,'exact');
  order(i) = j(1);
end
labels = labels(order);
isbox = isbox(order);
if rotate_flag
  [x,y] = best_rotation(x,y,tolerance);
end

figure(1); clf; axis square      %  now plot
[x, y, h] = draw_graph(adj>0, labels, isbox, x, y, varargin{:});

function [x,y] = best_rotation(x,y,h)
% Rotate the points to maximize the horizontal and vertical alignment.
% Written by Tom Minka.

xm = mean(x);
ym = mean(y);
xr = max(x)-min(x);
yr = max(y)-min(y);
x = (x-xm)/xr;
y = (y-ym)/yr;

xy = [x(:) y(:)];
if 1
  angle = fminbnd(@rotation_cost,-pi/4,pi/4,[],xy,h);
else
  angles = linspace(-pi/4,pi/4,40);
  e = [];
  for i = 1:length(angles)
    e(i) = rotation_cost(angles(i),xy,h);
  end
  %figure(2)
  %plot(angles*180/pi,e)
  angle = angles(argmin(e));
end
%angle*180/pi
c = cos(angle); s = sin(angle);
xy = xy*[c s; -s c];

x = xy(:,1)*xr+xm;
y = xy(:,2)*yr+ym;

function e = rotation_cost(angle,xy,h)
% xy is 2-column matrix.
% e is small if many x's and y's are aligned.

c = cos(angle); s = sin(angle);
xy = xy*[c s; -s c];
dx = sqdist(xy(:,1)',xy(:,1)');
dy = sqdist(xy(:,2)',xy(:,2)');
dx = setdiag(dx,Inf);
dy = setdiag(dy,Inf);
e = sum(exp(-dx(:)/h))+sum(exp(-dy(:)/h));
e = -e;


function [coarse_approximations,prediction_errors]=gsp_pyramid_analysis_old(signal,Gs,num_levels,varargin)
%GSP_PYRAMID_ANALYSIS Compute the graph pyramid transform coefficients
%   Usage:  [coarse_approximations,prediction_errors]=gsp_pyramid_analysis(signal,Gs,num_levels);
%           [coarse_approximations,prediction_errors]=gsp_pyramid_analysis(signal,Gs,num_levels,param);
%
%   Input parameters:
%         signal                    : Graph signal to analyze.
%         Gs                        : A multiresolution sequence of graph structures, including the idx parameters tracking the subsampling pattern.
%         num_levels                : Number of levels in the pyramid transform.
%   Output parameters:
%         coarse_approximations     : Cell array with the coarse approximations at each level.
%         prediction_errors         : Cell array with the prediction errors at each level.
%   Additional parameters:
%         param.use_exact           : To use exact graph spectral filtering instead of the Chebyshev approximation.
%         param.order               : Degree of the Chebyshev approximation (default=30).
%         param.regularize_epsilon  : Interpolation parameter.
%         param.h_filters           : A cell array of graph spectral filters. If just one filter is included, it is used at every level of the pyramid.
%
%   'gsp__pyramid_analysis(signal,Gs,num_levels)' computes
%   the graph pyramid transform coefficients of a signal $f$.
%
%   See also:
%
%   Demos:
%
%   References:

%   AUTHOR : David I Shuman.
%   TESTING:
%   REFERENCE:

% Read input parameters and check that inputs have the correct sizes
if nargin>3
    param=varargin{1};
else
    param=0;
end

if length(signal) ~= Gs{1}.N
    error('The signal to analyze should have the same dimension as the first graph');
end

if num_levels >= length(Gs)
    error('Not enough graphs provided to compute that many levels of the graph Laplacian pyramid');
end

if ~isfield(param,'h_filters')
    h_filters=cell(num_levels,1);
    for i=1:num_levels
        h_filters{i}=@(x) .5./(.5+x);
    end
elseif length(param.h_filters)==1
    h_filters=cell(num_levels,1);
    for i=1:num_levels
        h_filters{i}=param.h_filters;
    end
elseif length(param.h_filters)==num_levels
    h_filters=param.h_filters;
else
    error('param.h_filters should be a cell array of length 1 or num_levels');
end

% Compute the pyramid transform
coarse_approximations=cell(num_levels+1,1);
coarse_approximations{1}=signal;
prediction_errors=cell(num_levels,1);

for i=1:num_levels
    [coarse_approximations{i+1},prediction_errors{i}]=gsp_pyramid_analysis_single_interpolation_old(coarse_approximations{i},Gs{i},Gs{i+1}.idx,h_filters{i},param);
end


function anglepairs = samplerandfeat(num_feats)
%SAMPLERANDFEAT Summary of this function goes here
%   Function: generate the locations of pixel pairs randomly
%   Detailed explanation goes here
%   Input:
%        num_feats: number of features
%        max_radius: the maximum radius of local region
%   Output:
%         anglepairs: the angles of pixel pairs

thetas_a = 2*pi*[0:1/(num_feats-1):1];
thetas_b = 2*pi*[0:1/(num_feats-1):1];

anglepairs = [thetas_a(randperm(length(thetas_a)))' thetas_b(randperm(length(thetas_b)))'];

end


function K = matching( k, d1, d2, ind1, ind2, par)

%
%  Matching type similarity function (actually not a kernel) as described in the paper:
%  "Recognition with local features: the kernel recipe"
%  by C. Wallraven, B.Caputo and A.Graf
%
%  takes 2 parameters: { minorKernelName, minorKernelParameter}
%
%  implemented minor kernel names are:
%   'linear', 'rbf' ....  (standard kernels)
%   'normCC' ...........  normalized cross-correlation kernel
%   'spider' ...........  uses a spider kernel object (passed in minorKernelParameter),
%                         (is slower due to object-handling overhead)
%

  global VERBOSITY

  x1 = get_x( d1, ind1);
  x2 = get_x( d2, ind2);
  sz1 = size( x1, 1);
  sz2 = size( x2, 1);
  K = zeros( sz2, sz1);
  nTot = sz1*sz2;

  for ii = 1:sz1
    for jj = 1:sz2
      K( jj, ii) = Fkernel( double( x1{ ii}), double( x2{ jj}), par);
    end
    if VERBOSITY Fprogress( (sz2*(ii-1)+jj)/nTot); end
  end

  K = 0.5*( K + K');

function k = Fkernel( a, b, par)

  switch par{ 1}
   case 'linear'
    M = b*a';

   case 'rbf'
    M = rbf( a, b, par{ 2});

   case 'normCC'
    M = normCC( a, b, par{ 2});

   case 'spider'
    M = calc( par{ 2}, data( a), data( b));

   otherwise
    error( 'Kernel name unknown or not implemented: %s', par{ 1});
  end

  k = mean( max( M));

function K = rbf( a, b, sigma)
% exponential kernel
  sigma2 = sigma * sigma;
  K = b*a';
  K = K + K; % *2
  K1 = sum( a.^2, 2);
  K2 = sum( b.^2, 2);
  K = ones( length( K2), 1)*K1' + K2*ones( 1, length( K1)) - K;
  K = exp( -K ./ ( 2*sigma2));

function K = normCC( dat1, dat2, rho)
% normalized cross-correlation

  dat1 = dat1 - repmat( mean( dat1), size( dat1, 1), 1);
  dat2 = dat2 - repmat( mean( dat2), size( dat2, 1), 1);
  dat1 = dat1 ./ repmat( sqrt( diag( dat1*dat1')), 1, size( dat1, 2));
  dat2 = dat2 ./ repmat( sqrt( diag( dat2*dat2')), 1, size( dat2, 2));

  K = exp( -rho*( 1 - dat1*dat2'));


function s = i4_to_s_roman ( i )

%*****************************************************************************80
%
%% I4_TO_S_ROMAN converts an integer to a string of Roman numerals.
%
%  Example:
%
%         I  S
%
%        -2  -II <-- Not a Roman numeral
%        -1  -I  <-- Not a Roman numeral
%         0   0  <-- Not a Roman numeral
%         1   I
%         2   II
%         3   III
%         4   IV
%         5   V
%        10   X
%        20   XX
%        30   XXX
%        40   XL
%        50   L
%        60   LX
%        70   LXX
%        80   LXXX
%        90   XC
%       100   C
%       500   D
%      1000   M
%      4999   MMMMCMLXLIX
%
%  Discussion:
%
%    To generate numbers greater than 4999, the numeral 'V' had a bar
%    above it, representing a value of 5000, a barred 'X' represented
%    10,000 and so on.
%
%    In the subtractive representation of 4 by 'IV', 9 by 'IX' and so on,
%    'I' can only subtract from 'V' or 'X',
%    'X' can only subtract from 'L' or 'C',
%    'C' can only subtract from 'D' or 'M'.
%    Under these rules, 1999 cannot be written IMM!
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 December 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I, an integer to be converted.  If the integer
%    has absolute value greater than 4999, the string '?' will be returned.
%    If the integer is 0, then the string '0' will be returned.  If
%    the integer is negative, then a minus sign will precede it, even
%    though this has nothing to do with Roman numerals.
%
%    Output, string S, the representation of the integer
%    as a Roman numeral.
%
  s = '';

  if ( 4999 < abs ( i ) )
    s = '?';
    s = char ( s );
    return
  end

  if ( i == 0 )
    s = '0';
    s = char ( s );
    return
  end

  if ( i <= 0 )
    s = '-';
    i = -i;
  end

  while ( 0 < i )

    if ( 1000 <= i )
      s = [ s 'M' ];
      i = i - 1000;
    elseif ( 900 <= i )
      s = [ s 'CM' ];
      i = i - 900;
    elseif ( 500 <= i )
      s = [ s 'D' ];
      i = i - 500;
    elseif ( 400 <= i )
      s = [ s 'CD' ];
      i = i - 400;
    elseif ( 100 <= i )
      s = [ s 'C' ];
      i = i - 100;
    elseif ( 90 <= i )
      s = [ s 'XC' ];
      i = i - 90;
    elseif ( 50 <= i )
      s = [ s 'L' ];
      i = i - 50;
    elseif ( 40 <= i )
      s = [ s 'XL' ];
      i = i - 40;
    elseif ( 10 <= i )
      s = [ s 'X' ];
      i = i - 10;
    elseif ( 9 <= i )
      s = [ s 'IX' ];
      i = i - 9;
    elseif ( 5 <= i )
      s = [ s 'V' ];
      i = i - 5;
    elseif ( 4 <= i )
      s = [ s 'IV' ];
      i = i - 4;
    else
      s = [ s 'I' ];
      i = i - 1;
    end

  end

  s = char ( s );

  return
end


function [p,ellipse]=phantom3dAniso(varargin)

%PHANTOM3D Three-dimensional analogue of MATLAB Shepp-Logan phantom
%   P = PHANTOM3D(DEF,N) generates a 3D head phantom that can
%   be used to test 3-D reconstruction algorithms.
%
%   DEF is a string that specifies the type of head phantom to generate.
%   Valid values are:
%
%      'Shepp-Logan'            A test image used widely by researchers in
%                               tomography
%      'Modified Shepp-Logan'   (default) A variant of the Shepp-Logan phantom
%                               in which the contrast is improved for better
%                               visual perception.
%      'yu-ye-wang'             Another version of the modified Shepp-Logan
%                               phantom from "Katsevich-Type Algorithms for
%                               Variable Radius Spiral Cone-BeamCT"
%
%   N specifies the 3D grid size of P
%   If N is a scalar, P will have isotropic size [N, N, N]
%   If N is a 3-vector, P will have size [N(1) N(2) N(3)]
%   If you omit the argument, N defaults to [64 64 64].
%
%   P = PHANTOM3D(E,N) generates a user-defined phantom, where each row
%   of the matrix E specifies an ellipsoid in the image.  E has ten columns,
%   with each column containing a different parameter for the ellipsoids:
%
%     Column 1:  A      the additive intensity value of the ellipsoid
%     Column 2:  a      the length of the x semi-axis of the ellipsoid
%     Column 3:  b      the length of the y semi-axis of the ellipsoid
%     Column 4:  c      the length of the z semi-axis of the ellipsoid
%     Column 5:  x0     the x-coordinate of the center of the ellipsoid
%     Column 6:  y0     the y-coordinate of the center of the ellipsoid
%     Column 7:  z0     the z-coordinate of the center of the ellipsoid
%     Column 8:  phi    phi Euler angle (in degrees) (rotation about z-axis)
%     Column 9:  theta  theta Euler angle (in degrees) (rotation about x-axis)
%     Column 10: psi    psi Euler angle (in degrees) (rotation about z-axis)
%
%   For purposes of generating the phantom, the domains for the x-, y-, and
%   z-axes span [-1,1].  Columns 2 through 7 must be specified in terms
%   of this range.
%
%   [P,E] = PHANTOM3D(...) returns the matrix E used to generate the phantom.
%
%   Class Support
%   -------------
%   All inputs must be of class double.  All outputs are of class double.
%
%   Remarks
%   -------
%   For any given voxel in the output image, the voxel's value is equal to the
%   sum of the additive intensity values of all ellipsoids that the voxel is a
%   part of.  If a voxel is not part of any ellipsoid, its value is 0.
%
%   The additive intensity value A for an ellipsoid can be positive or negative;
%   if it is negative, the ellipsoid will be darker than the surrounding pixels.
%   Note that, depending on the values of A, some voxels may have values outside
%   the range [0,1].
%
%   Example
%   -------
%        ph = phantom3d(128);
%        figure, imshow(squeeze(ph(64,:,:)))
%
%   Copyright 2005 Matthias Christian Schabel (matthias @ stanfordalumni . org)
%   University of Utah Department of Radiology
%   Utah Center for Advanced Imaging Research
%   729 Arapeen Drive
%   Salt Lake City, UT 84108-1218
%
%   This code is released under the Gnu Public License (GPL). For more information,
%   see : http://www.gnu.org/copyleft/gpl.html
%
%   Portions of this code are based on phantom.m, copyrighted by the Mathworks
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Modification May 25, 2015, by Patrick J. Bolan, University of Minnesota
% Added support for anisotropic phantom sizes: the phantom size can now be
% a vector.
%

[ellipse,n] = parse_inputs(varargin{:});

nx = n(1); ny = n(2); nz = n(3);
p = zeros([nx ny nz]);

rngx =  ( (0:nx-1)-(nx-1)/2 ) / ((nx-1)/2);
rngy =  ( (0:ny-1)-(ny-1)/2 ) / ((ny-1)/2);
rngz =  ( (0:nz-1)-(nz-1)/2 ) / ((nz-1)/2);

% PJB: Note the swap of the x and y with meshgrid parameters.
%[x,y,z] = meshgrid(rngx,rngy,rngz);
[x,y,z] = meshgrid(rngy,rngx,rngz);
x=single(x);y=single(y);z=single(z);
coord = [flatten(single(x)); flatten(single(y)); flatten(single(z))];
clear x y z;
p = flatten(p);

for k = 1:size(ellipse,1)
   A = ellipse(k,1);            % Amplitude change for this ellipsoid
   asq = ellipse(k,2)^2;        % a^2
   bsq = ellipse(k,3)^2;        % b^2
   csq = ellipse(k,4)^2;        % c^2
   x0 = ellipse(k,5);           % x offset
   y0 = ellipse(k,6);           % y offset
   z0 = ellipse(k,7);           % z offset
   phi = ellipse(k,8)*pi/180;   % first Euler angle in radians
   theta = ellipse(k,9)*pi/180; % second Euler angle in radians
   psi = ellipse(k,10)*pi/180;  % third Euler angle in radians

   cphi = cos(phi);
   sphi = sin(phi);
   ctheta = cos(theta);
   stheta = sin(theta);
   cpsi = cos(psi);
   spsi = sin(psi);

   % Euler rotation matrix
   alpha = [cpsi*cphi-ctheta*sphi*spsi   cpsi*sphi+ctheta*cphi*spsi  spsi*stheta;
            -spsi*cphi-ctheta*sphi*cpsi  -spsi*sphi+ctheta*cphi*cpsi cpsi*stheta;
            stheta*sphi                  -stheta*cphi                ctheta];

   % rotated ellipsoid coordinates
   coordp = alpha*coord;

   idx = find((coordp(1,:)-x0).^2./asq + (coordp(2,:)-y0).^2./bsq + (coordp(3,:)-z0).^2./csq <= 1);
   p(idx) = p(idx) + A;
end

%p = reshape(p,[nx ny nz]);
p = reshape(p, [nx ny nz]);

return;

function out = flatten(in)

out = reshape(in,[1 numel(in)]);

return;

function [e,n] = parse_inputs(varargin)
%  e is the m-by-10 array which defines ellipsoids
%  n is a 3-vector with the size of the phantom brain image, [nx ny nz]

n = [64 64 64];     % The default size
e = [];
defaults = {'shepp-logan', 'modified shepp-logan', 'yu-ye-wang'};

for i=1:nargin
   if ischar(varargin{i})         % Look for a default phantom
      def = varargin{i};
      idx = strcmpi(def, defaults);
      if isempty(idx)
         eid = sprintf('Images:%s:unknownPhantom',mfilename);
         msg = 'Unknown default phantom selected.';
         error(eid,'%s',msg);
      end
      switch defaults{idx}
      case 'shepp-logan'
         e = shepp_logan;
      case 'modified shepp-logan'
         e = modified_shepp_logan;
      case 'yu-ye-wang'
         e = yu_ye_wang;
      end
   elseif numel(varargin{i})==1
      n = [varargin{i} varargin{i} varargin{i}];   % a scalar is the image size
   elseif numel(varargin{i})==3
      siz = varargin{i};
      n = [siz(1) siz(2) siz(3)]; % 3 integers specify image dimensions
   elseif ndims(varargin{i})==2 && size(varargin{i},2)==10
      e = varargin{i};            % user specified phantom
   else
      eid = sprintf('Images:%s:invalidInputArgs',mfilename);
      msg = 'Invalid input arguments.';
      error(eid,'%s',msg);
   end
end

% ellipse is not yet defined
if isempty(e)
   e = modified_shepp_logan;
end

return;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Default head phantoms:   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function e = shepp_logan

e = modified_shepp_logan;
e(:,1) = [1 -.98 -.02 -.02 .01 .01 .01 .01 .01 .01];

return;

function e = modified_shepp_logan
%
%   This head phantom is the same as the Shepp-Logan except
%   the intensities are changed to yield higher contrast in
%   the image.  Taken from Toft, 199-200.
%
%         A      a     b     c     x0      y0      z0    phi  theta    psi
%        -----------------------------------------------------------------
e =    [  1  .6900  .920  .810      0       0       0      0      0      0
        -.8  .6624  .874  .780      0  -.0184       0      0      0      0
        -.2  .1100  .310  .220    .22       0       0    -18      0     10
        -.2  .1600  .410  .280   -.22       0       0     18      0     10
         .1  .2100  .250  .410      0     .35    -.15      0      0      0
         .1  .0460  .046  .050      0      .1     .25      0      0      0
         .1  .0460  .046  .050      0     -.1     .25      0      0      0
         .1  .0460  .023  .050   -.08   -.605       0      0      0      0
         .1  .0230  .023  .020      0   -.606       0      0      0      0
         .1  .0230  .046  .020    .06   -.605       0      0      0      0 ];

return;

function e = yu_ye_wang
%
%   Yu H, Ye Y, Wang G, Katsevich-Type Algorithms for Variable Radius Spiral Cone-Beam CT
%
%         A      a     b     c     x0      y0      z0    phi  theta    psi
%        -----------------------------------------------------------------
e =    [  1  .6900  .920  .900      0       0       0      0      0      0
        -.8  .6624  .874  .880      0       0       0      0      0      0
        -.2  .4100  .160  .210   -.22       0    -.25    108      0      0
        -.2  .3100  .110  .220    .22       0    -.25     72      0      0
         .2  .2100  .250  .500      0     .35    -.25      0      0      0
         .2  .0460  .046  .046      0      .1    -.25      0      0      0
         .1  .0460  .023  .020   -.08    -.65    -.25      0      0      0
         .1  .0460  .023  .020    .06    -.65    -.25     90      0      0
         .2  .0560  .040  .100    .06   -.105    .625     90      0      0
        -.2  .0560  .056  .100      0    .100    .625      0      0      0 ];

return;


function x = reciprocal_cdf_inv ( cdf, a, b )

%*****************************************************************************80
%
%% RECIPROCAL_CDF_INV inverts the Reciprocal CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real CDF, the value of the CDF.
%
%    Input, real A, B, the parameters of the PDF.
%    0.0 < A <= B.
%
%    Output, real X, the corresponding argument of the CDF.
%
  if ( cdf <= 0.0 )
    x = 0.0;
  elseif ( 0.0 < cdf )
    x = b^cdf / a^( cdf - 1.0 );
  end

  return
end


%WEIGHT_CALC    MATLAB file that performs weight calculations
%   Calls a file of ship data.  Prints an output file called weights.out.
%   For details see Subsection 7.2.2 in the book.
%   Companion file to Biran, A. (2003), Ship Hydrostatics and Stability,
%   Oxford: Butterworth-Heinemann.

!rename weights.out weights.old      % prepare space for new data file
disp('Enter name of data file, then write RETURN and press ENTER ')
keyboard
fname = 'weights.out';
fid = fopen(fname, 'w');
title = [ sname ', weight calculations' ];
fprintf(fid, '%40s\n', title)
fprintf(fid, '---------------------------------------------------------------------------\n')
fprintf(fid, '     Weight item          Mass      vcg      z-Moment    vcg      x-Moment\n')
fprintf(fid, '----------------------------------------------------------------------------\n')

Displ = sum(wdata(:, 1));
vmom  = wdata(:, 1).*wdata(:, 2);
KG    = sum(vmom)/Displ;
lmom  = wdata(:, 1).*wdata(:, 3);
LCG   = sum(lmom)/Displ;

head = '%16s %11.2f %7.2f %12.2f %8.2f %13.2f\n';
[ m, n ] = size(wdata);
for k = 1:m
    name = names(k, :);
    fprintf(fid, head, name, wdata(k, 1), wdata(k, 2), vmom(k), wdata(k, 3), lmom(k))
end
fprintf(fid, '---------------------------------------------------------------------------\n')
subtitle = 'Total           ';
fprintf(fid, head, subtitle, Displ, KG, sum(vmom), LCG, sum(lmom))


function varargout = sample(f, varargin)
%SAMPLE   Values of a CHEBFUN3 object on a tensor product grid.
%   X = SAMPLE(F) returns the tensor of values of F on a tensor product
%   grid.
%
%   [CORE, C, R, T] = SAMPLE(F) returns the low rank representation of the
%   values of F on a tensor product grid so that X = CORE x_1 C x_2 R x_3 T.
%
%   [CORE, C, R, T] = SAMPLE(F, M, N, P) returns the values of F on an
%   M x N x P tensor product grid.
%
% See also CHEBFUN3/FEVAL.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Empty check.
if ( isempty(f) )
    varargout = {[]};
    return
end

if ( nargin == 4 )
     m = varargin{1};
     n = varargin{2};
     p = varargin{3};
else
    [m,n,p] = length(f);
    m = max(m, 51);
    n = max(n, 51);
    p = max(p, 51);
end

% Use Slice-Tucker decomposition so we can keep it in low rank form:
[fCore, fCols, fRows, fTubes] = tucker(f);
Cvals = sample(fCols, m);
Rvals = sample(fRows, n);
Tvals = sample(fTubes, p);

% Evaluate:
if ( nargout <= 1 )
    varargout = {chebfun3.txm(chebfun3.txm(chebfun3.txm(fCore, Cvals, ...
        1), Rvals, 2), Tvals, 3)};
else
    varargout = {fCore, Cvals, Rvals, Tvals};
end

end


function [final_sound] = im2sound(filename, ext, f_sample, f_low, ...
    f_high, amp_mod, sample_t)

%INPUTS:
%'filename' - Name of the image to be encoded (not including extension
%ext' - Extension of the image (not including "." at the beginning).
%'f_sample' - Sampling frequency (Hz)
%'f_low' - Lowest frequency (Hz) (e.g. 40)
%'f_high' - Highest frequency (Hz) (e.g. 6000)
%'amp_mod' - Multiplication factor for the amplitude.  Decrease until
%image is clear. Too high and the waveform clips.  Too low and the image
%is very dark (e.g. 0.00002)
%'sample_t' - Duration of the sample in seconds.  Longer samples have
%better quality (e.g. 10)

%OUTPUTS:
%'final_sound' - the final sound containing the image.  This is
%automatically saved to a .wav file with the original image filename

%INITIALISING VARIABLES:
%The waveform at each time point.  This is reset at the beginning of each
%time point
temp_sound = 0;
%The final waveform
final_sound = 0;

%MAIN BODY
%Loading the sample image and calculating the image size
raw_im = imread(strcat(filename,'.',ext));
size_raw_im = size(raw_im);

%Making a frequency table for the height of the image.  Each row of the
%image is assigned a particular frequency from the corresponding row of
%this table.  The frequencies are linearly distributed between the highest and
%lowest user-definied frequencies.  "f_step" is the increment between each
%adjacent frequency
f_step = (f_high - f_low)/size_raw_im(1,1);
f_table = (f_high:-f_step:f_low);

%The final sound will dwell on each column of the image for a specific
%time.  This time is defined by "t_start" and "t_end".  It depends on how
%long the user determined the sound-clip should be and how wide (how many
%columns) the image is.
t_step = (sample_t/size_raw_im(1,2));

%Initial values for the start and end times.  These will be increased at
%the end of each loop iteration (when the script moves onto the next column
%of the image).
t_start = 0;
t_end = t_step;

%The loop which generates the sound file.  At each iteration it generates a
%segment of the final sound file, which is temporarily saved to
%"temp_sound".  This segment is built up of frequencies from that
%particular column of the image.
for j = 1:size_raw_im(1,2)
    %Initialising the variable (the sound for each frequency (row) is added
    %to the existing sound)
    temp_sound = 0;

    %Setting the time in matrix format
    t = t_start:1/f_sample:(t_end);

    %For each iteration of this loop, the script goes down the current
    %column of the image and generates a waveform of the frequency
    %specified in "f_table".  The amplitude of the waveform is determined
    %by the pixel intensity.  This generated waveform is added to all the
    %previously generated waveforms in that particular column
    for i = size_raw_im(1,1):-1:1
        temp_sound = temp_sound+ sin(2*pi*t*f_table(i))*...
            double(raw_im(i,j))*amp_mod;

    end

    %At the end of each column the segment of sound generated is added to
    %the end of the existing sound file ("final_sound").
    final_sound = cat(2,final_sound,temp_sound);

    %The temporary sound is cleared ready for the start of the next column
    clear temp_sound

    %Moving to the next time frame
    t_start = t_start + t_step;
    t_end = t_end + t_step;

end

%This saves "final_sound" to the '.wav' file of the same name as the input
%file
wavwrite(final_sound, f_sample, strcat(filename, '.wav'));


function [ gx ] = ObsRecGen(x,P,u,in)
% observation function for k-ToM's bet about her opponent's next move
% [ gx ] = ObsRecGen(x,P,u,in)
% Marie Devaine wrote this function in November 2015 (comments: JD).
% A k-ToM learner bases her decision (a=1 or a=0) upon her prediction of
% her opponent's next move, given the game payoff table. When k>1, k-ToM
% maintains more than one such prediction, which depends upon the possible
% level of her opponent. Let P(o=1) be the probability that k-ToM's
% opponent will pick the first alternative option. Then:
% P(o=1) = sum_k P(o=1|k)*P(k)
% where P(o=1|k) is the probability that k-ToM's opponent will pick
% the first alternative option if he was a k-ToM, and P(k) is the
% probability that k-ToM's opponent is a k-ToM.
% IN:
%   - x: hidden states (see indexing in inG.indlev)
%   - P: observation param:
%       P(1) = (log-) temperature
%       P(2) = bias [optional]
%   - u: [useless]
%   - inG: input structure (see prepare_kToM.m)
% OUT:
%   - gx: proba that the agent will pick the first option, i.e. gx=P(y=1).

player = in.player; % 1 or 2: role of the player
ntotPar = in.npara; % only for k-ToM with k>0
level = in.lev; % depth of k-ToM's recursive beliefs
game = in.game; % payoff table
a = 0.36; % for E[s(x)] when x~n(mu,Sig)
indlev = in.indlev; % hidden-states indexing [see defIndlev.m]

% Get the agent's prediction about her opponent's next move, ie P(o=1).
if level==0 % 0-ToM

    mx = x(1); % E[log-odds of P(o=1)]
    Vx = exp(x(2)); % V[log-odds of P(o=1)]
    Po = VBA_sigmoid(mx/(sqrt(1+a*Vx))); % P(o=1)

else

    % Get P(k'). Note: if the agent is k-ToM, then, by definition, she
    % considers that her opponent's sophistication is k' < k. In addition,
    % there is a constraint of normalization, ie sum_k' P(k') = 1. Thus,
    % one only needs to keep track of k'-1 probabilities (the last one is,
    % by construction, 1-sum_k' P(k')).
    Pk = VBA_sigmoid(x(1:(level-1))); % P(k'), with k'=0,...,k-1
    Pk = [Pk;max(0,1-sum(Pk))]; % insert last P(k'=k-1)

    % Get P(o=1|k'). Note: the agent's prediction P(o=1|k') depends upon
    % her estimate of her opponent's parameters (learning rate, tmperature,
    % bias...). Uncertainty Re: these parameters eventually results in
    % blurring her prediction.
    % Note: hidden states encode x(theta), the log-odds of P(o=1|k',theta)
    % evaluated at the agent's estimate of theta (mu). In addition, they
    % encode the gradient of x wrt to theta (dx/dtheta), and V[theta]. This
    % then serves to derive P(o=1|k') as follows:
    % P(o=1|k') = E[sigm(x(theta))]
    %           = sigm(E[x(theta)]/sqrt(1+a*V[x(theta)])
    %           = sigm(E[x(theta)]/sqrt(1+a*V[theta]*(dx/dtheta)^2)
    f = zeros(level,1); % E[x(theta)]
    Vx = zeros(level,1); % V[x(theta)]
    for j=1:level % loop over possible opponent's levels  (k'=j-1)
        f(j) = x(indlev(j).f); % E[x(theta)|k'=j-1]
        df = x(indlev(j).df); % d[x(theta)]/dtheta for k'=j-1
        Sig = exp(x(indlev(j).Par(2:2:2*ntotPar))); % V[theta|k'=j-1]
        Vx(j) = sum(Sig.*df.^2); % V[x(theta)|k'=j-1]
    end
    Es = VBA_sigmoid(f./sqrt(1+a*Vx)); % E[sigm(x(theta))]

    % Get P(o=1) = sum_k P(o=1|k')*P(k')
    Po = Pk'*Es; % k-ToM's belief about her opponent's next move

end

% Make decision based upon P(o=1)
DV = fplayer(Po,exp(P(1)),player,game); % incentive for a=1
if length(P)==1
    gx = VBA_sigmoid(DV); % P(a=1)
else % P(2) = bias
    gx = VBA_sigmoid(DV+P(2)); % P(a=1) with bias
end


function [Model, Info] = linear_sparse_stepwise_vec(X,Y,Model,parm)
%  Estimate linear weight matrix for input-output mapping
%     Automatic Relevance Prior for each input dimension
%     is imposed to get sparse weight matrix
%
% Notice !!
%     Delay embedding is done in this module,
%     then input vector should be original input data without embedding
%
%   [Model, Info] = linear_sparse_stepwise(X,Y,Model,parm)
%     Scalar iteration version
% --- Input
%  Y  : Output data ( N x T x Ntrial )
%  X  : Input data  ( M x (T + (Dtau-1)*Tau)  x Ntrial)
%  N  =  # of output
%  M  =  # of input (original input space dimension)
%  T  =  # of time sample
%
%  Estimate the following model
%    Y(t) = W * [X(:, t + (Dtau-1)*Tau ); ...; X(:, t)]
%
%  Model : Structure for estimated model
%  if Model is empty, initialization is done before training
%  if Model is previous training result, re-training us done
%
%  parm  : Structure for learning parameter
%  parm.Npre_train :  # of VB-update in initial training
%  parm.Ntrain :  # of training
%  parm.Nskip  :  skip # for print
%  parm.a_min  :  Min value for pruning small variance component
%  parm.Prune  :  = 1 : Prune small variance & irrelevant input dimension
%
%  parm.Tau       = Lag time
%  parm.Dtau      = Number of embedding dimension
%    Total input dimension in embedding space is (M * parm.Dtau)
% --- Output
%  Model : Structure for estimated model
%  Model.SY  :  Noise variance         ( 1 x 1 )
%  Model.W   :  Weight matrix          ( N x M*D ) , D = parm.Dtau
%  Model.A   :  Prior weight variance  ( 1 x M*D )
%  Model.ix_act : Active index for W after pruning
%
%  Info  : Structure for learning process history
%  Info.FE  = LP + H : Free energy
%  Info.LP  = Log likelihood
%  Info.H   = - Model entropy
%
% 2009-10-18 Made by M. Sato

MINVAL = 1.0e-15;

fprintf('linear_sparse_stepwise start\n')

% Dimension
[N ,T  ,Ntrial]  = size(Y); % N = # of output
[M ,Tx ,Ntrialx] = size(X); % M = # of input without embedding

Tall = T * Ntrial;

if Ntrial~=Ntrialx, error('# of trial is different in X and Y'); end;

Nskip  = 100;   % skip steps for display info
a_min = 1e-14;	% Minimum value for weight pruning
Fdiff = 1e-12; % Threshold for convergence
Ncheck = 100;   % Minimum number of training iteration
Fstep  = 5;     % Free energy convergence check step
Prune  = 1;     % Prune mode
space_ARD=1;	% if space_ARD=1, ARD is done only for space dimension

if isfield(parm,'Nskip'), Nskip  = parm.Nskip; end;
if isfield(parm,'Fdiff'), Fdiff   = parm.Fdiff; end;
if isfield(parm,'a_min'), a_min   = parm.a_min ; end;
if isfield(parm,'Prune'), Prune = parm.Prune; end;
if isfield(parm,'Ncheck'), Ncheck = parm.Ncheck; end
if isfield(parm,'Fstep'),Fstep  = parm.Fstep ; end
if isfield(parm,'Fstep'),Fstep  = parm.Fstep ; end
if isfield(parm,'space_ARD'),space_ARD  = parm.space_ARD ; end

% # of embedding dimension
if isfield(parm,'Dtau')
	D    = parm.Dtau;
	tau  = parm.Tau;
else
	D    = 1;
	tau  = 1;
end
% Parameters
Ntrain = parm.Ntrain;

if Ntrain < 1, Info = []; return; end;

if isfield(parm,'Npre_train')
	Npre_train = parm.Npre_train;
else
%	Npre_train = Ntrain;
	if Tall >= 2*M*D
		Npre_train = 0;
	elseif Tall >= M*D
		Npre_train = fix(Ntrain/2);
	else
		Npre_train = Ntrain;
	end
end

if Npre_train > Ntrain, Npre_train = Ntrain; end;

if isfield(parm,'Nupdate')
	Nupdate = parm.Nupdate;
else
	Nupdate = 1;
end

fprintf('--- Output Dimension  = %d\n',N)
fprintf('--- Input  Dimension  = %d\n',M)
fprintf('--- Embedding  Dimension  = %d\n',D)
fprintf('--- Number of trials  = %d\n',Ntrial)
fprintf('--- Number of training sample = %d\n',Tall)
fprintf('--- Total update iteration    = %d (%d)\n',Ntrain,Npre_train)

% original input dimension
Xdim  = M;
M = Xdim*D;
M_ALL = M;

%
% --- Initialization
%

% Input/Output variance
sx = mean((X(:) - mean(X(:))).^2);
sy = mean((Y(:) - mean(Y(:))).^2);

SY0 = mean(sy);
A0  = 1./mean(sx);

if isfield(parm,'Ta0') && parm.Ta0 > 0,
	Ta0 = parm.Ta0;
	a0  = parm.a0 * A0;
else
	Ta0 = 0;
	a0  = 1;
end

% Input covariance
XX  = sum(sum(X.^2,3),2)/(Tx*Ntrial);
XX  = repmat(XX', [1 D]);% 1 x M

if isfield(Model,'ix_act')
	SY  = Model.SY;  % 1 x 1

	% Active index
	ix_act = Model.ix_act;
	A = zeros(1,M_ALL);
	W = zeros(N,M_ALL);

	A(ix_act)   = sum(Model.A,1);
	W(:,ix_act) = Model.W;

	if M_ALL ~= Xdim*D,
		fprintf('M_ALL=%d,Xdim=%d,D=%d\n',M_ALL ,Xdim ,D)
		error('M_ALL ~= Xdim*D')
	end

	% Active index in input space without embedding
	IX_dim = find( sum(reshape(A,[Xdim,D]),2) > 0);

	IX_act = repmat( (0:(D-1))* Xdim ,[length(IX_dim) 1]) ...
	       + repmat(IX_dim, [1 D]);
	IX_act = IX_act(:);

	W  = W(:,IX_act) ;  % N x (M*D)
	A  = A(IX_act) ;    % 1 x (M*D)
	X  = X(IX_dim,:,:);
	XX = XX(IX_act);

	M     = length(IX_act);
	Xdim  = length(IX_dim);
else
	A = repmat(A0, [1,M_ALL]);
	W = zeros(N,M_ALL);
	SY = SY0;
	% Save original input for pruning
	IX_act  = 1:M_ALL;
	IX_dim  = 1:Xdim;
end

ix_act  = 1:M;
ix_dim  = 1:Xdim;
M_all   = M;

% Delay embedding index for W
Wid = [(0:(D-1))'* Xdim + 1 , (1:D)'* Xdim];

% Working variable
G_A = zeros(1,M);       % N x M

if size(A,1)==Xdim && size(A,2)==1
	A   = repmat(A',[1 D]);
elseif size(A,1)==1 && size(A,2)==Xdim
	A   = repmat(A,[1 D]);
end

if length(SY)==1, SY= repmat(SY,N,1); end;

SW = 1./(Tall*XX + 1./A );

fprintf('a_min = %g\n', a_min)
fprintf('SY0 = %g\n', SY0)
fprintf('SY  = %g\n', mean(SY))

% Free energy histry
FE  = zeros(Ntrain,1);
LP  = zeros(Ntrain,1);
H   = zeros(Ntrain,1);
Err = zeros(Ntrain,1);
Mhist = zeros(Ntrain,1);

% ARD hyper param. history
if isfield(parm,'Debug') && ~isempty(parm.Debug) && parm.Debug > 0
	Debug = 1;
	A_tmp = zeros(N*M_ALL, ceil(Ntrain/Nskip));
else
	Debug = 0;
end

% recover all component
A_all  = zeros(1,M_all);       % N x M

[dY] = error_delay_time(X,Y,W,tau);

k_save  = 0;

%%%%%% Learning Loop %%%%%%
for k=1:Ntrain
	% Ainv = alpha , A = 1/alpha
	Ainv = 1./A;

	% E = ( (Y-W*X)^2 +  W^2 * Ainv )/SY
    [W]  = weight_update_embed(X, dY, W, Tall*XX, Ainv, tau);
	[dY] = error_delay_time(X, Y, W, tau);

    dYY = sum(dY.^2,2)/(Tall); % N x 1
	WW  = W.^2;

    % Noise variance update
    SY  = dYY + WW * Ainv'/(Tall);
    % Prevent zero variance
    SY  = max( SY, MINVAL);

    % Weight variance
	SW = 1./(Tall*XX + Ainv );

    % Log variance
    SWA     = max( SW .* Ainv , MINVAL);
    log_sw  = N*(sum( log(SWA) - SWA + 1 ));
    log_sy  = sum( log(SY) );
    log_a   = Ta0*sum( log(Ainv) - a0.*Ainv + 1 );

    % Free energy
    LP(k)  = - (0.5) * (log_sy + N*sum(SW.*XX));
    H(k)   = 0.5*( log_sw + log_a  );
    FE(k)  = LP(k) + H(k)/Tall;
    Err(k) = sum(dYY)/(N*SY0);

	% E = ( (Y-W*X)^2 +  W^2 * Ainv )/SY

    % Hyper parameter for weight variance (ARD)
	if mod(k,Nupdate) == 0,
		% Ainv = 1./A;
	    % SW = 1./( Tall*XX + Ainv );

	    % G_A = 1 - (SW)./A;
	    %     = (Tall*XX + Ainv - 1/A) * SW
	    G_A = Tall.* ( SW ) .* XX;
	    G_A = max((G_A), MINVAL);

		% N*A = ( (W.^2) + SW )/SY ;
		if k <= Npre_train,
			% VB update rule (Stable)
			%  ARD for each weight
			A  = ((1./SY)'*WW + N*SW + 2*Ta0*a0)/( N + 2*Ta0 );
		else
			% Accelerated update rule (Unstable)
		    A  = sqrt( A.* ((1./SY)'*WW) ./(G_A * N) );
		    %A  = (WW + 2*Ta0*a0)./(G_A * N + 2*Ta0);
		end

		if space_ARD==1
			Am = mean(reshape(A,[M/D,D]),2);
			A  = repmat(Am', 1,D);
		end

	    % Prune small variance
	    if Prune == 1
		    % Find active input dimension
		    ix_act_old = ix_act;
		    ix_dim_old = ix_dim;

		    % recover all component
			A_all  = zeros(1,M_all);       % 1 x M
		    %  A_all(:,ix_act)  = A;
		    WWW = sum(WW,1);
		    A_all(ix_act) = WWW/max(WWW);

		    % find active input dimension (absolute index)
		    ix_dim = find( sum(reshape(A_all,[Xdim,D]),2) > a_min );
		    ix_act = repmat(ix_dim, [1 D]) ...
		           + repmat([0:D-1]*Xdim, [length(ix_dim) 1]);
		    ix_act = ix_act(:);

		    Mnew   = length(ix_act);  		% # of effective input

		    if Mnew < M,
			    % convert to relative index
			    jx_act = trans_index(ix_act,ix_act_old,M_all);
			    jx_dim = trans_index(ix_dim,ix_dim_old,Xdim);

			    M   = Mnew;
			    A   = A(jx_act) ;    % 1 x M
			    W   = W(:,jx_act) ;  % N x M
			    SW  = SW(:,jx_act);  % N x M

			    X	 = X(jx_dim,:,:);  	 	% Xdim x T
			    XX	 = XX(jx_act);  	 	% 1 x M
			end
	    end
	    % END of if Prune == 1

		A = max(A,MINVAL);

	end
	% END of if mod(k,Nupdate) == 0

	Mhist(k) = M;

    if mod(k, Nskip)==0
        % Save history
		if Debug == 1
        	k_save = k_save + 1;
        	A_tmp(:,k_save) = A_all(:);
		end

        fprintf('Iter = %4d, M = %4d, err = %g, F = %g, H = %g\n', ...
               k, length(ix_act), Err(k), FE(k), - 2*H(k)/log(T));
    end

    % Convergence check
	if k > Ncheck,
		Fdif = (FE(k) - FE(k-Fstep))/(abs(FE(k))+eps);
	else
		Fdif = Fdiff + 1;
	end

	if (Fdiff > abs(Fdif)),
		fprintf('Converged : Free energy change = %g\n',Fdif)
		break;
	end;
end

ix_act = IX_act(ix_act);

% Active index
Model.ix_act = ix_act;
Model.M_all  = M_ALL ;

% Save output variable
Model.A    = A ;
Model.W    = W ;
Model.SY   = SY;
Model.SW   = SW; % = 1./(Tall*XX + diag(Ainv))

Model.method = 'linear_sparse_stepwise';
Model.mode   = 'scalar';
Model.sparse = 'sparse';

% Save history
Info.FE  = FE(1:k);
Info.LP  = LP(1:k);
Info.H   = H(1:k) ;
Info.Err = Err(1:k);
Info.M   = Mhist(1:k);

if exist('A_tmp','var')
	Info.A   = A_tmp(:,1:k_save) ;
end

% recover all component
%W_all   = zeros(N,M_all);
%SW_all  = zeros(N,M_all);
%
%A_all(:,ix_act)  = A;
%W_all(:,ix_act)  = W ;  % N x M
%SW_all(:,ix_act) = SW;  % N x M

%%% ---- Index transformation from old active_index to current active_index
function	jx = trans_index(ix,ix_old,M)

N = length(ix_old);
Itrans = zeros(M,1);
Itrans(ix_old) = 1:N;

jx = Itrans(ix);


function I = mi_model_gd_vec(x, y, Ym, biascorrect, demeaned)
% MI_MODEL_GD_VEC Vectorized MI calculation between multiple Gaussian variables
%         and a common discrete variable in bits based on ANOVA style model comparison.
%   I = mi_model_gd_vec(x,y,Ym) returns the MI between the (possibly multidimensional)
%   Gaussian variables x and the discrete variable y.
%   size(x) = [Ntrl Nvec Ndim]
%   so each output I(i) = mi_model_gd(squeeze(x(:,i,:)), y, Ym);
%
%   For 1D x this is a lower bound to the mutual information.
%   y should contain integer values in the range [0 Ym-1] (inclusive).
%
%   biascorrect : true / false option (default true) which specifies whether
%   bias correction should be applied to the esimtated MI.
%   demeaned : false / true option (default false) which specifies whether the
%   input data already has zero mean (true if it has been copula-normalized)
%   See also: MI_MODEL_GD, MI_MIXTURE_GD_VEC

% ensure samples first axis for vectors
if isvector(x)
    x = x(:);
end
if ndims(x)>3
    error('mi_model_gd: input arrays should be 3d')
end
if isvector(y)
    y = y(:);
else
    error('mi_model_gd: only univariate discrete variable supported');
end

Ntrl = size(x,1);
Nvec = size(x,2);
Xdim = size(x,3);

if size(y,1) ~= Ntrl
    error('mi_model_gd: number of trials do not match');
end

% default option values
if nargin<4
    biascorrect = true;
end

if nargin<5
    demeaned = false;
end

% unconditional demean
if ~demeaned
    x = bsxfun(@minus,x,sum(x,1)/Ntrl);
end

I = zeros(Nvec,1);

% y = y-1;
% for vi=1:Nvec
%   I(vi) = mi_model_gd(squeeze(x(:,vi,:)),y,Ym,true,true);
% end
%
% return

% one-hot encoding of Y
Yhot = indexed2boolean(y);

% remove class means
[Xcen class_means] = removeclassmeans(x, Yhot);

% allocate memory for class-conditional entropies and covariances
Ntrl_y = sum(Yhot);
Hcond  = zeros(Nvec,Ym);
Cm     = zeros(Nvec,Xdim,Xdim,Ym);

% allocate memory for overall entropy and covariance
Hunc = zeros(Nvec,1);
Cx   = zeros(Nvec,Xdim,Xdim);

%  data is class-demeaned, this needs to be accounted for in the
% unconditional entropies
c = diag(sqrt(Ntrl_y))*class_means.';
c = reshape(c, [Ym Nvec Xdim]);

for vi1=1:Xdim
  % all voxels for this dimension
  x1 = Xcen(:,:,vi1);
  c1 = squeeze(c(:,:,vi1));

  Cx(:,vi1,vi1) = sum(x1.^2)+sum(c1.^2);
  for yi=1:Ym
    tmp = x1(Yhot(:,yi),:);
    Cm(:,vi1,vi1,yi) = sum(tmp.^2);
  end

  for vi2=(vi1+1):Xdim
    x2  = Xcen(:,:,vi2);
    c2  = squeeze(c(:,:,vi2));
    tmp = transpose(sum(x1.*x2) + sum(c1.*c2));

    Cx(:,vi1,vi2) = tmp;
    Cx(:,vi2,vi1) = tmp;
    for yi=1:Ym
      tmp = transpose(sum(x1(Yhot(:,yi),:).*x2(Yhot(:,yi),:)));
      Cm(:,vi1,vi2,yi) = tmp;
      Cm(:,vi2,vi1,yi) = tmp;
    end
  end
end

Cx = Cx / (Ntrl-1);
Cx = vecchol(Cx);
for vi=1:Xdim
  Hunc = Hunc + shiftdim(log(Cx(:,vi,vi)));
end

for yi=1:Ym
  Cm(:,:,:,yi) = Cm(:,:,:,yi) / (Ntrl_y(yi) - 1);
  Cm(:,:,:,yi) = vecchol(Cm(:,:,:,yi));
  for vi=1:Xdim
    Hcond(:,yi) = Hcond(:,yi)+shiftdim(log(Cm(:,vi,vi,yi)));
  end
end

% apply bias corrections
ln2 = log(2);
if biascorrect

  vars = 1:Xdim;

  psiterms_unc = psi((Ntrl - vars)/2) / 2;
  dterm_unc    = (ln2 - log(Ntrl-1)) / 2;
  bias_unc     = Xdim'*dterm_unc + sum(psiterms_unc);

  dterm_cond    = (ln2 - log(Ntrl_y-1)) / 2;
  psiterms_cond = zeros(1,Ym);
  for vi=vars
    idx = (Ntrl_y-vi);
    psiterms_cond = psiterms_cond + psi(idx/2);
  end
  bias_cond = Xdim*dterm_cond + (psiterms_cond/2);

  Hunc  = Hunc  - bias_unc;
  Hcond = Hcond - ones(Nvec,1)*bias_cond;
end

% class weights
w = Ntrl_y ./ Ntrl;

% compute mutual information
I = Hunc - Hcond*w';

% convert to bits
I = I / ln2;

function [Xcen, class_means] = removeclassmeans(X, design)
[Ntrl, Nvec, Ndim] = size(X);
Xcen = X(:,:);
class_means = zeros(Nvec*Ndim,size(design,2));
for k = 1:size(design,2)
  sel = design(:,k);
  tmp = Xcen(sel,:);
  class_means(:,k) = mean(tmp,1);
  Xcen(sel,:) = bsxfun(@minus,tmp,class_means(:,k).');
end
Xcen = reshape(Xcen,[Ntrl Nvec Ndim]);

function Y = indexed2boolean(X)
uX = unique(X);
Y  = false(numel(X),numel(uX));
for k = 1:size(Y,2)
  Y(X==uX(k),k) = true;
end


 function sr = de_ftab_fit_sprad(fit, sl)
%function sr = de_ftab_fit_sprad(fit, sl)
% for each s vector compute spectra radius of hessian of LS fit
% in
%	sl	cell{L}	[(Nd),L]
% out
%	sr	[(Nd),M]
% applies only to 'exp' fit

sll = ndgrid_jf('mat', sl{:}); % [(Nd),L]
Nd = size(sll); LL = Nd(end); Nd = Nd(1:end-1);
sll = reshape(sll, [], LL); % [*Nd,L]

if LL ~= 2, error 'only L=2 done', end
MM = fit.MM;

%sr = cell(fit.MM,1);
sr = zeros(prod(Nd), MM);
for mm=1:MM
	alf = fit.coef{mm}; % [ne,1]
	mac = fit.mac{mm}; % [ne,L]
	g0 = mac' * alf; % gradient of fit at s=0 (largest point)
	h0 = mac' * diag(alf) * mac - g0 * g0'
	norm(g0)
	norm(g0 * g0')
	norm(h0)
	for is=1:prod(Nd)
		ss = sll(is,:)'; % [L,1]
		q = alf .* exp(-mac * ss); % [ne,1]
		q = q / sum(q);
%		plot(q), drawnow
		g = mac' * q; % gradient of f_m(s)
		h = mac' * diag(q) * mac - g * g';
		sr(is,mm) = norm(g * g');
%		sr(is,mm) = norm(h);
	end
end
sr = reshape(sr, [Nd MM]);


function [C] = spm_cov2corr(C)
% returns the correlation matrix given the covariance matrix
% FORMAT [R] = spm_cov2corr(C);
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_cov2corr.m 1143 2008-02-07 19:33:33Z spm $

%--------------------------------------------------------------------------
n    = length(C);
D    = sparse(1:n,1:n,sqrt(1./(diag(C) + eps)));
C    = real(D*C);
C    = real(C*D);


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

% Test script for illustration

% base scenario
t=0; T=1;
f = 0.03;
alpha = 0.075;
beta = 0.5;
nu = 0.1;
rho = 0;

sabrdensity = @(x,y) psabr3(t,T,f,x,alpha,y,beta,nu,rho);

lcoeff = 1; ucoeff = 1;
lowerbound = f-lcoeff*f;
upperbound = f + ucoeff*f;

xvals = lowerbound:.0005:upperbound;

zvals1d = density1d(xvals,sabrdensity,0,1);

% change rho
rho = -.9;
sabrdensity = @(x,y) psabr30(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dm = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dm)
rho = .9;
sabrdensity = @(x,y) psabr30(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dp = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dp)
figure('Color',[1 1 1]);
hold on;
plot(xvals,zvals1dm,'r','LineWidth',2); plot(xvals,zvals1d,'b','LineWidth',2); plot(xvals,zvals1dp,'g','LineWidth',2);
title('Plot of SABR densities for different \rho')
xlabel('0\leq x \leq .1')
ylabel('density')
legend('\rho=-0.9','\rho=0','\rho=0.9')
hold off;
rho = 0.0;

% change nu
nu = 0.05;
sabrdensity = @(x,y) psabr30(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dm = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dm)
nu = .15;
sabrdensity = @(x,y) psabr30(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dp = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dp)
figure('Color',[1 1 1]);
hold on;
plot(xvals,zvals1dm,'r','LineWidth',2); plot(xvals,zvals1d,'b','LineWidth',2); plot(xvals,zvals1dp,'g','LineWidth',2);
title('Plot of SABR densities for different \nu')
xlabel('0\leq x \leq .1')
ylabel('density')
legend('\nu=0.05','\nu=0.4','\nu=0.2')
hold off;
nu = 0.1;

% change beta
beta = 0.4;
sabrdensity = @(x,y) psabr30(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dm = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dm)
beta = 0.8;
sabrdensity = @(x,y) psabr30(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dp = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dp)
figure('Color',[1 1 1]);
hold on;
plot(xvals,zvals1dm,'r','LineWidth',2); plot(xvals,zvals1d,'b','LineWidth',2); plot(xvals,zvals1dp,'g','LineWidth',2);
title('Plot of SABR densities for different \beta')
xlabel('0\leq x \leq .1')
ylabel('density')
legend('\beta=0.4','\beta=0.5','\beta=0.6')
hold off;
beta = 0.5;

% change alpha
alpha = 0.05;
sabrdensity = @(x,y) psabr30(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dm = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dm)
alpha = .1;
sabrdensity = @(x,y) psabr30(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dp = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dp)
figure('Color',[1 1 1]);
hold on;
plot(xvals,zvals1dm,'r','LineWidth',2); plot(xvals,zvals1d,'b','LineWidth',2); plot(xvals,zvals1dp,'g','LineWidth',2);
title('Plot of SABR densities for different \alpha')
xlabel('0\leq x \leq .1')
ylabel('density')
legend('\alpha=0.05','\alpha=0.075','\alpha=0.1')
hold off;
alpha = 0.075;


function [Q] = CouetteBC2D(xin, yin, nxin, nyin, mapI, mapO, mapW, mapC, Q, time);

% function [Q] = CouetteBC2D(xin, yin, nxin, nyin, mapI, mapO, mapW, mapC, Q, time);
% Purpose: evaluate solution for Couette flow

% Couette flow (mach .2 at inner cylinder)
gamma = 1.4;

% extract conserved variables
rho = Q(:,:,1); rhou = Q(:,:,2); rhov = Q(:,:,3); Ener = Q(:,:,4);

mapB = [mapI;mapO];

rad = sqrt(xin(mapB).^2 + yin(mapB).^2);
theta = atan2(yin(mapB), xin(mapB));

utheta = (-rad + 16./rad)/75;
p = 1 + (1./(75^2))*( (rad.^2)/2 - 32*log(rad) - 128./rad.^2 );

rho (mapB) = 1;
rhou(mapB) = -sin(theta).*utheta;
rhov(mapB) =  cos(theta).*utheta;
Ener(mapB) = p/(gamma-1) + 0.5*(rhou(mapB).^2 + rhov(mapB).^2)./rho(mapB);

% pack modified conserved variables
Q(:,:,1) = rho; Q(:,:,2) = rhou; Q(:,:,3) = rhov; Q(:,:,4) = Ener;
return


function [E,L,G]=GenPCBasis(S,A)
% this function computes the conditional principal portfolios
% see A. Meucci - "Managing Diversification", Risk Magazine, June 2009
% available at www.ssrn.com

% Code by A. Meucci. This version March 2009.
% Last version available at MATLAB central as "Managing Diversification"

% inputs
% S : covariance matrix
% A : conditioning matrix

% outputs
% E : conditional principal portfolios composition
% L : conditional principal portfolios variances
% G : map weights -> conditional diversification distribution (square root of, not normalized)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if isempty(A)
    N=size(S,1);
    K=0;
    [E_,L_]=eig(S);
    E=E_;
    for n=1:N
        E(:,n)=E_(:,N-n+1);
        L(n)=L_(N-n+1,N-n+1);
    end

else

    [K,N]=size(A);
    E=[];
    B=A;
    for n=1:N-K
        if ~isempty(E)
            B=[A
                E'*S];
        end
        e=GenFirstEigVect(S,B);
        E=[E e];
    end

    for n=N-K+1:N
        B=E'*S;
        e=GenFirstEigVect(S,B);
        E=[E e];
    end

    % swap order
    E=[E(:,N-K+1:N) E(:,1:N-K)];
end

L=diag(E'*S*E);

G=diag(sqrt(L))*inv(E);
G=G(K+1:N,:);


function inoutsig = gaindb(inoutsig,gn,varargin)
%GAINDB  Increase/decrease level of signal
%   Usage:  outsig = gaindb(insig,gn);
%
%   `gaindb(insig,gn)` increases the energy level of the signal by *gn*
%   dB.
%
%   If *gn* is a scalar, the whole input signal is scaled.
%
%   If *gn* is a vector, each column is scaled by the entries in
%   *gn*. The length of *gn* must match the number of columns.
%
%   `gaindb(insig,gn,dim)` scales the signal along dimension *dim*.
%
%   See also: rms

%   AUTHOR: Peter L. Søndergaard, 2009

% ------ Checking of input parameters ---------

if nargin<2
  error('%s: Too few input parameters.',upper(mfilename));
end;

if ~isnumeric(inoutsig)
  error('%s: insig must be numeric.',upper(mfilename));
end;

if ~isnumeric(gn)
  error('%s: gn must be numeric.',upper(mfilename));
end;

definput.keyvals.dim=[];
[flags,kv]=ltfatarghelper({'dim'},definput,varargin);

% ------ Computation --------------------------

if isscalar(gn)
  inoutsig = inoutsig*10^(gn/20);
else
  if isvector(gn)
    M=length(gn);

    [inoutsig,L,Ls,W,dim,permutedsize,order]=...
        assert_sigreshape_pre(inoutsig,[],kv.dim,upper(mfilename));

    if M~=W
      error('%s: Length of gn and signal size must match.',upper(mfilename));
    end;

    for ii=1:W
      inoutsig(:,ii)=inoutsig(:,ii)*10^(gn(ii)/20);
    end;

    inoutsig=assert_sigreshape_post(inoutsig,kv.dim,permutedsize,order);

  else
    if ~isnumeric(gn)
      error('%s: gn must be a scalar or vector.',upper(mfilename));
    end;
  end;
end;


function surfPoints = getSurfacePoints(mask3M)
%"getSurfacePoints"
%   Return the [row,col,slice] of surface points in mask3M, where a surface
%   point is defined as any voxel in the structure that is adjacent to a
%   voxel not in the structure.  Two voxels are considered adjacent when
%   their faces touch, and NOT adjacent if only their corners or edges
%   touch.
%
%   To create the 3D surface analog of mask3M use:
%
%   surfPoints = getSurfacePoints(mask3M);
%   surf3M = repmat(logical(0), size(mask3M));
%   for i=1:size(surfPoints,1)
%        surf3M(surfPoints(i,1),surfPoints(i,2), surfPoints(i,3)) = 1;
%   end
%
%   Constructing a 2nd 3D mask the size of mask3M may pose a memory
%   problem, so substituting mask3M for surf3M is a good idea if
%   mask3M is disposable.
%
% JRA 11/20/03
%     03/23/05 - New algorithm for speed, ~10x faster
%
% Usage: surfPoints = getSurfacePoints(mask3M)

surfPoints = [];

[r,c,s] = find3d(mask3M);

%Find minimum rows, cols, slices.
minR = min(r); maxR = max(r);
minC = min(c); maxC = max(c);
minS = min(s); maxS = max(s);

%Restrict surface calculation to region with structure.
mask3M = mask3M(minR:maxR, minC:maxC, minS:maxS);

%Construct the "allNeighborsOn" matrix, which for any voxel not on the edge
%of maskM, tells whether or not ALL of its neighbors are on.
plusRowShift  = mask3M(3:end, 2:end-1, 2:end-1);
allNeighborsOn = plusRowShift;
clear plusRowShift

minusRowShift = mask3M(1:end-2, 2:end-1, 2:end-1);
allNeighborsOn = allNeighborsOn & minusRowShift;
clear minusRowShift

plusColShift  = mask3M(2:end-1, 3:end, 2:end-1);
allNeighborsOn = allNeighborsOn & plusColShift;
clear plusColShift

minusColShift = mask3M(2:end-1, 1:end-2, 2:end-1);
allNeighborsOn = allNeighborsOn & minusColShift;
clear minusColShift

plusSlcShift  = mask3M(2:end-1, 2:end-1, 3:end);
allNeighborsOn = allNeighborsOn & plusSlcShift;
clear plusSlcShift

minusSlcShift = mask3M(2:end-1, 2:end-1, 1:end-2);
allNeighborsOn = allNeighborsOn & minusSlcShift;
clear minusSlcShift

%Now find all surface points (except those on the edge of maskM), defined
%as those points that are ON in mask3M and don't have ALL their neighbors
%on.
kernal = mask3M(2:end-1, 2:end-1, 2:end-1) & ~allNeighborsOn;

%Finally drop the kernal back into the middle of mask3M.  All points on the
%first/last row, column and slice of mask3M are by definion surface points.
mask3M(2:end-1, 2:end-1, 2:end-1) = kernal;

%Find the location of the surface points.
[r,c,s] = find3d(mask3M);

%Correct for taking a subset of mask3M when we began.
r = r + (minR - 1);
c = c + (minC - 1);
s = s + (minS - 1);

surfPoints = [r;c;s]';


%TROTY Rotation about Y axis
%
% T = TROTY(THETA) is a homogeneous transformation (4x4) representing a rotation
% of THETA radians about the y-axis.
%
% T = TROTY(THETA, 'deg') as above but THETA is in degrees.
%
% Notes::
% - Translational component is zero.
%
% See also ROTY, TROTX, TROTZ, TROT2.

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

function T = troty(t, varargin)
	T =    [roty(t, varargin{:}) [0 0 0]'; 0 0 0 1];


function triangulation_test21 ( )

%*****************************************************************************80
%
%% TEST21 tests TRIANGULATION_ORDER3_PRINT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 August 2006
%
%  Author:
%
%    John Burkardt
%
  node_num = 9;
  triangle_num = 12;

  node_xy(1:2,1:node_num) = [ ...
       0.0, 0.0; ...
       0.0, 1.0; ...
       0.2, 0.5; ...
       0.3, 0.6; ...
       0.4, 0.5; ...
       0.6, 0.4; ...
       0.6, 0.5; ...
       1.0, 0.0; ...
       1.0, 1.0 ]';
  triangle_node(1:3,1:triangle_num) = [ ...
       2, 1, 3; ...
       3, 1, 6; ...
       2, 3, 4; ...
       4, 3, 5; ...
       7, 4, 5; ...
       5, 3, 6; ...
       7, 5, 6; ...
       9, 4, 7; ...
       6, 1, 8; ...
       7, 6, 8; ...
       7, 8, 9; ...
       2, 4, 9 ]';
  triangle_neighbor(1:3,1:triangle_num) = [ ...
       -28,   2,  3; ...
         1,   9,  6; ...
         1,   4, 12; ...
         3,   6,  5; ...
         8,   4,  7; ...
         4,   2,  7; ...
         5,   6, 10; ...
        12,   5, 11; ...
         2, -34, 10; ...
         7,   9, 11; ...
        10, -38,  8; ...
         3,   8, -3 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST21\n' );
  fprintf ( 1, '  TRIANGULATION_ORDER3_PRINT prints out a triangulation.\n' );

  triangulation_order3_print ( node_num, triangle_num, node_xy, ...
    triangle_node, triangle_neighbor );

  return
end


function centerOfMass = dtiRoiGetCenterOfMassCoord(roi)

% function centerOfMass = dtiRoiGetCenterOfMassCoord(roi)
%
% For a given ROI this simple function will compute the center of mass and
% return the coordinate for that center point.
%
% User can then use that point to create a new ROI that is centered on the
% center of mass - useful for ROIs that are functionally defined but oddly
% shapped.
%
%% EXAMPLE USAGE SCRIPT
% roiFile = 'LMT.mat';
% % Read in the roi that we want to take the center of mass for
% roi = dtiReadRoi(roiFile);
%
% % Set the radius for the new ROI sphere
% radius = 5;
%
% % Get the coords and find the center of mass
% centerOfMass = dtiRoiGetCenterOfMassCoord(roi);
%
% % Create a new roi with a X mm radius from the center coord
% coords = dtiBuildSphereCoords(centerOfMass,radius);
%
% % Set the coords for the new ROI in the old roi struct and change the name
% roi.coords = coords;
% roi.name = [roi.name '_sphere_' num2str(radius) 'mm'];
%
% % Write out the new roi.
% cd(mrvDirup(roiFile));
% dtiWriteRoi(roi,roi.name);
%
%
% HISTORY:
% 2011.04.08 LMP Wrote the thing
% 2011.06.20 LMP Added example usage script to comments
% 2011.07.21 LMP Now accepts roi as a mat file or struct

if ~isstruct(roi)
 if exist(roi,'file')
    roi = dtiReadRoi(roi);
 else
  keyboard
 end
end

centerOfMass = round(mean(roi.coords,1)*10)/10;
fprintf('Center of mass coordinate: %.1f, %.1f, %.1f\n',centerOfMass);

return


%% DEMO 7:  Algorithms 02. SART
%
%
% In this demo the usage of the algorithms on the SART family among with
% their options are presented.
%
%
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
%
% Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Ander Biguri
%--------------------------------------------------------------------------
%% Initialize
clear;
close all;

%% Define Geometry
geo=defaultGeometry('nVoxel',[128;128;128]);

%% Load data and generate projections
% see previous demo for explanation
angles=linspace(0,2*pi,100);
head=headPhantom(geo.nVoxel);
projections=Ax(head,geo,angles,'interpolated');
noise_projections=addCTnoise(projections);
%% SART family of algorithms
%
% There are 3 algorithms in this damily included in TIGRE: SART,SIRT and
% OS-SART.
%
% The main difference between them is the update process.
%   SART: Updates the image projection by projection
%   SIRT: Updates the image using the whole set of projections at once
%   OS-SART: middle ground. Updates the image using a subset of the
%            projections
%
%  Of these algorithms, SART is generally the one reaching a better image
%  (less L2 error) for the same amount of iterations, and SIRT is the
%  worst (still relatively similar). However, SART needs increased
%  computational time per iteration, as it needs to update the image very often,
%  while SIRT only updates the emage ones for the whole sets of projections.
%  OS-SART lies in the middle, reaching similar convergence (L2 error per
%  iteration) than SART but with less computational time than SART.
%
%% Usage, with optional parameters.
% In the three algorithms, there are 4 mandatory input arguments:
% Projections, geometry, angles and number of iterations.
%
%
% Optional arguments for all of them
%==========================================================================
% 'lambda': hyperparameter. The update will be multiplied by this number
% every iteration, to make the steps bigger or smaller. Default: 1
%
lambda=1;

% 'lambdared': reduction multiplier for the hyperparameter.
% lambda=lambda*lambdared every iterations, so the steps can be smaller
% the further the update. Default=0.99
lambdared=0.999;

% 'Init' : Initialization method. Possible options are
%          'none' (default). There will be no initialization method, just
%                 the algorithm
%
%          'FDK'  Initialize the image with the result of FDK algorithm
%
%          'multigrid' Initialize using the multigrid method. The image
%                      will be solved in a small scale, and the size of it
%                      will increase until the desired size is reached.
%
%          'image'     Initialzies with a user given image. Not recoomended
%                      unless you really know what you are doing.

initmode='none';

% 'InitImg' : related to init. The image to use for initializing the
% algorithm.

% 'verbose': boolean to make the algorithm display (or not) running state.
%            default true.

verbose=true;
% 'QualMeas'     Asks the algorithm for a set of quality measurement
%                parameters. Input should contain a cell array of desired
%                quality measurement names. Example: {'CC','RMSE','MSSIM'}
%                These will be computed in each iteration.
qualmeas={'RMSE'};

% SIRT and SART both have no extra input parameters.
% =========================================================================
[imgSIRT,errL2SIRT,qualitySIRT]=SIRT(projections,geo,angles,20,...
                            'lambda',lambda,'lambda_red',lambdared,'verbose',verbose,'QualMeas',qualmeas);
[imgSART,errL2SART,qualitySART]=SART(projections,geo,angles,20,...
                            'lambda',lambda,'lambda_red',lambdared,'verbose',verbose,'QualMeas',qualmeas);
% OS-SART
% ========================================================================
% Additionally OS-SART includes a couple of other parameters, related to
% the subsets.
%
%   'BlockSize':   Sets the projection block size used simultaneously. If
%                  BlockSize = 1 OS-SART becomes SART and if  BlockSize = size(angles,2)
%                  then OS-SART becomes SIRT. Default is 20.
blcks=8;
% 'OrderStrategy':  Chooses the subset ordering strategy. Options are
%                  'ordered' :uses them in the input order, but divided
%                  'random'  : orders them randomply
%                  'angularDistance': chooses the next subset with the
%                                     biggest angular distance with the
%                                     ones used.  (default)
order='angularDistance';
[imgOSSART,errL2OSSART,qualityOSSART]=OS_SART(projections,geo,angles,20,...
                            'lambda',lambda,'lambda_red',lambdared,'verbose',verbose,'QualMeas',qualmeas,...
                             'BlockSize',blcks,'OrderStrategy',order);
%% Lets have a brief show of the results
% set(0,'DefaultTextInterpreter', 'latex')

subplot(211)
plot(log10([errL2SIRT;[errL2OSSART nan(1,length(errL2SIRT)-length(errL2OSSART))];[errL2SART nan(1,length(errL2SIRT)-length(errL2SART))]]'));
title('Convergence')
xlabel('Iteration')
ylabel('$ log_{10}(|Ax-b|) $','interpreter','latex')
legend('SIRT','OS-SART','SART')
subplot(212)
plot(log10([qualitySIRT;[qualityOSSART nan(1,length(qualitySIRT)-length(qualityOSSART))];[qualitySART nan(1,length(qualitySIRT)-length(qualitySART))]]'));
title('Evolution of RMSE')
legend('SIRT','OS-SART','SART')
xlabel('Iteration')
ylabel('$ log_{10}(RMSE) $','interpreter','latex')

%% plot the results

% It is clear that SART will get to better results for the same amoutn of
% iterations, however, it takes x7 more time to run.

% SART
% OS-SART
% SIRT

plotImg([imgSIRT;  imgOSSART; imgSART;],'Dim','Z','Savegif','sarts.gif');

% plot error
plotImg(abs([head-imgSIRT; head-imgOSSART; head-imgSART; ]),'Dim','Z');


function stapx = stnddv2(XYdata,numpnts,reps,dis)
%This function approximates the standard deviation by using neighboring
%values

%This provides a limit on the number of total replicates to consider on
% neighboring points.
maxval= 15;
x= reshape([XYdata.x],length(XYdata(1).x),[])';
n = size(x,1);

if ~exist('dis','var') || isempty(dis)
    dis = pdist2(x,x);
end
%Ensure there are enough points available
if numpnts>n, numpnts=n;end
stapx=zeros(n,1);
parfor i=1:n %parfor
    %Finds the closest n points
    [~,pntsloc] = sort(dis(i,:));
    pntsloc=pntsloc(1:numpnts);
    y=[];
    leny = length(y); %this has to be done to remove a warning
    for j=1:numpnts
        %Grab y values
        y=[y XYdata(pntsloc(j)).y];
        leny = length(y);
        %If the sampling has many replicates only use close samples
        if leny>maxval
            %Only give the limit of points
            if j~=1, leny=maxval;y=y(1:maxval);end
            break;
        end
    end
    stsamps = zeros(reps,1);
    for j=1:reps
        %Calculate the standard deviation - std func is not used because
        % the mean has already been removed
        samps = randi(leny,leny,1);
        stsamps(j)=std(y(samps));
    end
    %Find the mean standard deviation
    stapx(i)=mean(stsamps);
end

end


function [cm2] = ft22cm2(ft2)
% Convert area from square feet to square centimeters.
% Chad A. Greene 2012
cm2 = ft2*929.0304;


function strides = compute_strides(bnet)
% COMPUTE_STRIDES For each CPT and each variable in that CPT,
% returns the stride of that variable.  So in future, we can
% quickly extract a slice of the CPT.
%
% The return value is a 2d array, where strides(i,j) contains the
% stride of the jth variable in the ith CPT.  Cell arrays would
% have saved space but they are slower.
%

num_cpts = size(bnet.CPD, 2);
max_cpt_dim = 1 + max(sum(bnet.dag));
strides = zeros(num_cpts, max_cpt_dim);

for i = 1:num_cpts
  c = CPT(bnet, i);
  siz = size(CPT(bnet, i));

  % Deal with the special case of a 1-d array separately
  if siz(2) == 1
    dim = 1;
  else
    dim = size(siz, 2);
  end

  strides(i, 1:dim ) = [1 cumprod(siz(1:dim-1))];
end


function b = mv_st ( m, n, nz_num, row, col, a, x )

%*****************************************************************************80
%
%% MV_ST multiplies a sparse triple matrix times a vector.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, integer NZ_NUM, the number of nonzero values.
%
%    Input, integer ROW(NZ_NUM), COL(NZ_NUM), the row and column indices.
%
%    Input, real A(NZ_NUM), the nonzero values in the M by N matrix.
%
%    Input, real X(N), the vector to be multiplied.
%
%    Output, real B(M), the product A*X.
%
  b = zeros ( m, 1 );

  for k = 1 : nz_num
    b(row(k)) = b(row(k)) + a(k) * x(col(k));
  end

  return
end


function [blockSizes, blockStarts, blockSymbols] = getContigBlocks( ids )
%  Given a sequence "ids" of discrete positive integers
%     find all contiguous blocks within it
%USAGE:
%  [S, L, A] = getContigBlocks( [1 1 1 2 2 2 2 1 3 3] );
%  will return [B,L,S], where
%      S : [3 4 1 2] is vector of block sizes
%      L : [1 4 8 9] is vector of starting locations
%      A : [1 2 1 3] is vector of block labels (original vals in "ids")

blockSizes = [];
blockStarts = [];
blockSymbols = [];
aa = 1;
while aa <= length( ids )

    blockStarts(end+1) = aa;
    blockSymbols(end+1) = ids(aa);

    if aa == length( ids )
        % Reached the end of the input vector
        blockSizes(end+1) = 1;
        break;
    else
        % Search forwards
        didFindMismatch = 0;
        for bb = aa+1:length(ids)
            if ids(bb) ~= ids(aa);
                didFindMismatch = 1;
                bb = bb - 1;
                break;
            end
        end
        if didFindMismatch
            blockSizes(end+1) = bb - aa + 1;
        else
            blockSizes(end+1) = length(ids) - aa + 1;
            break;
        end

    end

    aa = bb+1;

end


function hermite_cubic_test10 ( )

%*****************************************************************************80
%
%% HERMITE_CUBIC_TEST10 tests HERMITE_CUBIC_SPLINE_INTEGRAL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 February 2011
%
%  Author:
%
%    John Burkardt
%
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'HERMITE_CUBIC_TEST10:\n' );
  fprintf ( 1, '  HERMITE_CUBIC_SPLINE_INTEGRAL integrates a Hermite\n' );
  fprintf ( 1, '  cubic spline over the definition interval [X1,XNN].\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  If the subintervals are equally spaced, the derivative\n' );
  fprintf ( 1, '  information has no effect on the result, except for\n' );
  fprintf ( 1, '  the first and last values, DN(1) and DN(NN).\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '                            Exact       Computed\n' );
  fprintf ( 1, '     X1          XNN        Integral    Integral  Comment\n' );
  fprintf ( 1, '\n' );

  for test = 1 : 5
%
%  Equal spacing.
%
    if ( test == 1 )
      nn = 11;
      xn = linspace ( 0.0, pi, nn );
      fn = sin ( xn );
      dn = cos ( xn );
      integral_exact = - cos ( xn(nn) ) + cos ( xn(1) );
      comment = 'Equal spacing, correct DN';
%
%  Equal spacing, reset DN(2:NN-1) to random numbers.
%
    elseif ( test == 2 )
      nn = 11;
      xn = linspace ( 0.0, pi, nn );
      fn = sin ( xn );
      dn = cos ( xn );
      [ dn(2:nn-1), seed ] = r8vec_uniform_01 ( nn - 2, seed );
      dn(2:nn-1) = 1000.0 * dn(2:nn-1);
      integral_exact = - cos ( xn(nn) ) + cos ( xn(1) );
      comment = 'Equal spacing, DN(2:N-1) random';
%
%  Equal spacing, now reset all of DN to random numbers.
%
    elseif ( test == 3 )

      nn = 11;
      xn = linspace ( 0.0, pi, nn );
      fn = sin ( xn );
      [ dn, seed ] = r8vec_uniform_01 ( nn, seed );
      dn(1:nn) = 1000.0 * dn(1:nn);
      integral_exact = - cos ( xn(nn) ) + cos ( xn(1) );
      comment = 'Equal spacing, DN(1:N) random';
%
%  Variable spacing, correct data.
%
    elseif ( test == 4 )
      nn = 11;
      xn = linspace ( 0.0, pi^2, nn );
      xn = sqrt ( xn );
      fn = sin ( xn );
      dn = cos ( xn );
      integral_exact = - cos ( xn(nn) ) + cos ( xn(1) );
      comment = 'Variable spacing, correct DN';
%
%  Variable spacing, change one entry in DN.
%
    elseif ( test == 5 )
      nn = 11;
      xn = linspace ( 0.0, pi^2, nn );
      xn = sqrt ( xn );
      fn = sin ( xn );
      dn = cos ( xn );
      [ r, seed ] = r8_uniform_01 ( seed );
      dn( floor ( nn + 1 ) / 2 ) = 1000.0 * r;
      integral_exact = - cos ( xn(nn) ) + cos ( xn(1) );
      comment = 'Variable spacing, a single internal DN randomized.';
    end

    integral_computed = hermite_cubic_spline_integral ( nn, xn, fn, dn );

    fprintf ( 1, '  %10f  %10f  %10.6g  %10.6g  %s\n', ...
      xn(1), xn(nn), integral_exact, integral_computed, comment );

  end

  return
end