function f=midi2frq(n,s)
%MIDI2FRQ	Convert musical note numbers to frequencies F=(N,S)
%		s is:	'e' equal tempered (default)
%			'p' pythagorean scale
%			'j' just intonation
%
% notes are numbered in semitones with middle C being 60
% On the equal tempered scale, note 69 (the A above middle C)
% has a frequency of 440 Hz.
%
% see FRQ2NOTE for the inverse transform

% Pythagorean
%     sharps 1 2187/2048 9/8 19683/16384 81/64 4/3 729/512  3/2 6561/4096 27/16 59049/32768 243/128 2
%     flats  1 256/243   9/8 32/27       81/64 4/3 1024/729 3/2 128/81    27/16 16/9        243/128 2
%
% Just Intonation
%     sharps 1 25/24 9/8 75/64 5/4 4/3 45/32  3/2 25/16 5/3 225/128 15/8 2
%     flats  1 16/15 9/8 6/5   5/4 4/3 108/75 3/2 8/5   5/3 18/10   15/8 2

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: midi2frq.m 713 2011-10-16 14:45:43Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin > 1
  if s(1)=='p'
    r=[256/243 9/8 32/27 81/64 4/3 729/512 3/2 128/81 27/16 16/9 243/128];
  elseif s(1)=='j'
    r=[16/15 9/8 6/5 5/4 4/3 36/25 3/2 8/5 5/3 9/5 15/8];
  else
    r=0;
  end
  if r(1)
    c=[0 0 12*log(r)/log(2)-(1:11) 0];
    nm=mod(n,12);
    na=floor(nm);
    nb=nm-na;
    f=440*exp((n+c(na+2).*(1-nb)+c(na+3).*nb-69)*log(2)/12);
  else
    f=440*exp((n-69)*log(2)/12);
  end
else
  f=440*exp((n-69)*log(2)/12);
end


x=[1 2 3 4 5 6 7 8 9]
pause
find(x<6)
pause
y=[1 2 3 4; 5 6 7 8; 9 10 11 12; 13 14 15 16]
pause
find(y<6)'
pause
[l,c,v]=find(y<6);
pause
l'
pause
c'
pause
v'


function varargout = frame2oell(varargin)
% VL_FRAMES2OELL   Convert a geometric frame to an oriented ellipse
%   EFRAME = VL_FRAME2OELL(FRAME) converts the generic FRAME to an
%   oriented ellipses EFRAME. FRAME and EFRAME can be matrices, with
%   one frame per column.
%
%   A frame is either a point, a disc, an oriented disc, an ellipse,
%   or an oriented ellipse. These are represented respectively by 2,
%   3, 4, 5 and 6 parameters each, as described in VL_PLOTFRAME().  An
%   oriented ellipse is the most general geometric frame; hence, there
%   is no loss of information in this conversion.
%
%   If FRAME is an oriented disc or ellipse, then the conversion is
%   immediate. If, however, FRAME is not oriented (it is either a
%   point or an unoriented disc or ellipse), then an orientation must
%   be assigned. The orientation is chosen in such a way that the
%   affine transformation that maps the standard oriented frame into
%   the output EFRAME does not rotate the Y axis. If frames represent
%   detected visual features, this convention corresponds to assume
%   that features are upright.
%
%   If FRAME is a point, then the output is an ellipse with null area.
%
%   See: feature frames,
%   VL_PLOTFRAME(), VL_HELP().
[varargout{1:nargout}] = vl_frame2oell(varargin{:});


%% Projection Based Shape Parameters
%
%%
% In this section we discuss shape parameters grains that depend on one
% dimensional projections, i.e.,
%
% || <grain2d.caliper.html |caliper|>  || caliper or Feret diameter in $\mu m$ || <grain2d.diameter.html |diameter|>  || diameter in $\mu m$ ||
%
% In order to demonstrate these parameters we first import a small sample
% data set.

% load sample EBSD data set
mtexdata forsterite silent

% reconstruct grains, discard boudnary grains and smooth them
[grains, ebsd.grainId] = calcGrains(ebsd('indexed'),'angle',5*degree);
ebsd(grains(grains.grainSize<5)) = [];
[grains, ebsd.grainId] = calcGrains(ebsd('indexed'),'angle',5*degree);
grains(grains.isBoundary) = [];

grains = smooth(grains('indexed'),10,'moveTriplePoints');

% plot all grains and highlight a specific one
plot(grains)

ind = 654;
hold on
plot(grains(ind).boundary,'lineWidth',5,'linecolor','blue')
hold off

%%
% The most well known projection based parameter is the
% <grain2d.diamter.html |diameter|> which refers to the longest distance
% between any two boundary points and is given in $\mu m$.

grains(ind).diameter

%%
% The diameter is a special case of the <grain2d.caliper.html |caliper|> or
% Feret diameter of a grain. By definition the caliper is the length of a
% grain when projected onto a line. We may trace the caliper with respect
% to projection direction

close all
omega = linspace(0,180);
plot(omega,grains(ind).caliper(omega*degree),'LineWidth',2)
ylabel('length in $\mu$m','Interpreter','latex')
xlabel('angle of the projection line in degree')
xlim([0,180])

%%
% We observe that that maximum caliper is about 7000 while the minimum
% caliper is about 2000. We may compute the exact direction and length of
% the maximum or minimum by passing the options |'longest'| or |'shortest'|
% to the function <grain2d.caliper.html |caliper|>. In this case the the
% output is of type @vector3d indicating the direction. The
% <vector3d.norm.html |norm|> of the vector coincides with the caliper for
% this projection direction. Hence, the |norm(grains.caliper('longest'))|
% coincides with the diameter.

plot(grains(ind),'micronbar','off')
legend('off')

norm(grains(ind).caliper('longest'))
norm(grains(ind).caliper('shortest'))

hold on
quiver(grains(ind),grains(ind).caliper('longest'),'noScaling')
quiver(grains(ind),grains(ind).caliper('shortest'),'noScaling')
hold off

%%
% The difference between the longest and the shortest caliper can be taken
% as a measure how round a grain is.

cMin = grains.caliper('shortest');
cMax = grains.caliper('longest');

plot(grains,(norm(cMax) - norm(cMin))./norm(cMax),'micronbar','off')
mtexColorbar('title','TODO')

%%
% This longest and shortest caliper are comparable to
% <grain2d.longAxis.html |grains.longAxis|> and <grain2d.shortAxis.html
% |grains.shortAxis|> computed from an ellipse fitted to the grain. In the
% case of rectangular particles, one might not primarily be interested in
% the longest caliper of a grain but rather in the direction normal to the
% shortest caliper. This is computed when specifying the option
% |'shortestPerp'|. If we imagine a very strong alignment of the long axes
% of orthorhombic particles, the maximum diameter may show a bimodal
% distribution (the two, roughly equally distributed diagonals of the
% particle).

% load some test grains
testgrains = mtexdata('testgrains');
testgrains = smooth(testgrains([6 8]),10);

% compute the longest caliper and the caliper perpendicular to the shortest
cMax = testgrains.caliper('longest');
cMinPerp = testgrains.caliper('shortestPerp');

% plot the grains and visualize the different long axes
plot(testgrains,'micronbar','off','lineWidth',2)
hold on
quiver(testgrains,cMax,'DisplayName','longest calliper','LineWidth',3)
quiver(testgrains,testgrains.longAxis,'DisplayName','long axis','LineWidth',3)
quiver(testgrains,cMinPerp,'DisplayName','perp to shortest','LineWidth',3)
hold off
legend('Location','east')

%% PAROR and SURFOR
%
% Another way of quantifying shape farbics is by making use of the
% cumulative projection function of the grains or the grain boundary
% segments. These methods are heavily inspired by
% <https://en.wikipedia.org/wiki/Flatland Edwin A. Abbotts 'Flatland - A
% romance of many dimensions' (1884)> and based on Panozzo, R., 1983,
% "Two-dimensional analysis of shape fabric using projections of digitized
% lines in a plane". Tectonophysics 95, 279-294. and Panozzo, R., 1984,
% "Two-dimensional strain from the orientation of lines in a plane." J.
% Struct. Geol. 6, 215-221. implemented in Mtex as <grain2d.paror.html
% |grains.paror|> and <grainBoundary.surfor.html |grainBoudnary.surfor|>
%
% As mentioned above the function <grain2d.caliper.html |caliper|> can be
% called with a list of angles and returns the projection length of all
% grains with respect to all angles.

% projection angle
omega = linspace(0,360*degree,361);
c = grains('Fo').caliper(omega);

subplot(1,2,1)
polarplot(omega,c,'LineWidth',2,'color',[0 0.25 0.5 0.25])
title('Forsterite')

% take the average
hold on
polarplot(omega,5*mean(c),'LineWidth',3,'color','k');
hold off

subplot(1,2,2)
c = grains('Enstatite').caliper(omega);

polarplot(omega,c,'LineWidth',2,'color',[0 0.25 0.5 0.25])
title('Enstatite')

% take the average
hold on
polarplot(omega,5*mean(c),'LineWidth',3,'color','k');
hold off

%%
% The above averaged caliper can be computed more directly by the function
% <grain2d.paror.html |grains.paror|> which returns the cumulative particle
% projection function normalized to 1. The projection angles can be
% regarded as the rotation angle of the particle (counterclockwise) while
% projecting from the y-axis onto the x-axis.

close all
cumplF = paror(grains('fo'),omega);
cumplE = paror(grains('en'),omega);

subplot(1,2,1)
polarplot(omega,cumplF,'LineWidth',3,'color','k')

subplot(1,2,2)
polarplot(omega,cumplE,'LineWidth',3,'color','k')

%%
% We can interpret the results in the following way. The minimum of the
% curve is a measure of the amplitude of the projection function and can be
% compared to an averaged axial ratio $b/a$ of the entire  fabric;
% isotropic fabrics would have a $b/a$ close to 1 while highly anisotropic
% fabrics can be identified by small $b/a$ values.

min(cumplF), min(cumplE)

%%
% The position of the maxima and minima of the projection function derived
% from <grain2d.paror.html |paror|> can be interpreted in the following
% way: the maximum position represents the preferred axis parallel to the
% longest projection and the normal the minimum position represents the
% preferred axis related to the normal to the shortest projection function.

% for the Forsterite
[~, id_max] = max(cumplF);
[~, id_min] = min(cumplF);

mod(omega(id_max)./degree,180)
mod(omega(id_min)./degree-90,180)

% for the Enstatite
[~, id_max] = max(cumplE);
[~, id_min] = min(cumplE);

mod(omega(id_max)./degree,180)
mod(omega(id_min)./degree-90,180)

%%
% The smaller the difference between these values, the closer the fabric is
% to an orthorhombic symmetry.
%
% Similarly to using the entire particle (the convex hull in case of the
% projection functions), we can use a distribution of lines
% <grainBoundary.surfor.html |grainBoudnary.surfor|>. This can be useful
% for the quantification of the grain boundary anisotropy or in general
% might be needed if we look at boundaries which do not form closed
% outlines, e.g. a list of subgrain or twin boundaries or the contact
% between certain phases.
%
% Let's compare the boundaries between the different unlike phases and
% between forsterite-forsterite in our sample:

close all
pairs = [1 1; nchoosek(1:3,2)];
phase = {'Fo' 'En' 'Di'};
for i=1:length(pairs)

  gB = grains.boundary(phase{pairs(i,:)});
  polarplot(omega, surfor(gB,omega), 'linewidth',2, ...
    'DisplayName',[phase{pairs(i,1)} '-' phase{pairs(i,2)}]);
  hold on

end
hold off
legend('Location','southoutside','Orientation','horizontal')

%%
% We can see that Forsterite-Forsterite boundaries form a fabric slightly
% more inclined with respect to the other phase boundariesand that the
% phase boundaries between the two pyroxenes (Enstatite and Diopside) show
% the lowest anisotropy.
%
%% Characteristic Shape
%
% The characteristic shape results from the cummulative sum of all grain
% boundary segements ordered by the angle of the segment direction. It can
% be regarded as to represent the average grain shape, however without the
% need to use closed areas such as it would be required when working with
% grains.
%
% Here we can compare the shape defined by Forterite-Forsterite,
% Enstatite-Enstatite and Forsterite-Enstatite boundaries

plotopts = {'normalize','linewidth',2, 'plain'};

shapeF = characteristicShape(grains.boundary('Fo','Fo'))
plot(shapeF,plotopts{:},'DisplayName','Fo-Fo')

hold on
shapeE = characteristicShape(grains.boundary('En','En'));
plot(shapeE,plotopts{:},'DisplayName','En-En')

hold on
shapeEF = characteristicShape(grains.boundary('En','Fo'));
plot(shapeEF,plotopts{:},'DisplayName','En-Fo')
hold off

legend('Location','southoutside','Orientation','horizontal')

%%
% The output of the command <grainBoundary.characteristicShape.html
% |characteristicShape|> is a <shape2d.shape2d.html |shape2d|> object which
% behaves very similar to a <grain2d.grain2d.html |grain2d|> object. Hence
% it is easy to derive things such as a long axis or e.g. the angle between
% the longest and the shortest caliper which can be regarded as a measure
% of asymmetry.

angle(shapeF.caliper('longest'),shapeF.caliper('shortest'))/degree
angle(shapeE.caliper('longest'),shapeE.caliper('shortest'))/degree
angle(shapeEF.caliper('longest'),shapeF.caliper('shortest'))/degree


function B = BoundMirrorShrink(A)
% Shrink the matrix to remove the padded mirror boundaries
%
% for example
%
% A = [
%     5  4  5  6  12  6
%     2  1  2  3  11  3
%     5  4  5  6  12  6
%     8  7  8  9  13  9
%     5  4  5  6  12  6
%     ]
%
% B = BoundMirrorShrink(A) will yield
%
%     1  2  3  11
%     4  5  6  12
%     7  8  9  13

% Chenyang Xu and Jerry L. Prince, 9/9/1999
% http://iacl.ece.jhu.edu/projects/gvf

[m,n] = size(A);
yi = 2:m-1;
xi = 2:n-1;
B = A(yi,xi);


function [XTrain XTest yTrain yTest] = splitData(X, y)

% Splits the data into training and testing

% Useful Variables
totalEgs = size(X,1);
numTrain = 0.8*totalEgs; % 80% data reserved for training
numTest = 0.2*totalEgs; % 20% reserved for testing
numLabels = unique(y); % how many unique labels present
TrainPerLable = numTrain/length(numLabels); % how many egs per lable in training
TestPerLable = numTest/length(numLabels);  % how many egs per lable in test

for i=1:length(numLabels)
    temp = (y==numLabels(i,1)); % find out where each label is present
    idx = find(temp==1); % get the index of that row where the lable is present
    Xtemp = X(idx(:,1),:); % get values of X stored at that particular index
    idxTemp = (randperm(size(Xtemp,1)))'; % randomly choose rows to put into train
    XTrainTemp = Xtemp(idxTemp(1:TrainPerLable,1),:);
    yTrainTemp = (ones(TrainPerLable,1))*i;
    XTestTemp = Xtemp(idxTemp(TrainPerLable+1:end),:); % select remaining rows for testing
    yTestTemp = (ones(TestPerLable,1))*i;

    if i==1 % set up first input to train and test
        XTrain = XTrainTemp;
        yTrain = yTrainTemp;
        XTest = XTestTemp;
        yTest = yTestTemp;
    else % keep adding new egs to the training and testing sets
        XTrain = [XTrain; XTrainTemp];
        yTrain = [yTrain; yTrainTemp];
        XTest = [XTest; XTestTemp];
        yTest = [yTest; yTestTemp];
    end
end


function r = get_consensus_set_rank(CS, E, mode, T_noise_squared)

% r = get_consensus_set_rank(CS, E, mode, T_noise_squared)
%
% DESC:
% get the rank of a consensus set
%
% AUTHOR
% Marco Zuliani - marco.zuliani@gmail.com
%
% VERSION:
% 1.0.2
%
% INPUT:
% CS                = logical array identifyng the CS
% E                 = error associated to each data element
% mode              = specify the type of ranking
%                     0 -> RANSAC (cardinality of the CS)
%                     1 -> MSAC
% T_noise_squared   = noise threshold to discriminate inliers vs outliers
%
% OUTPUT:
% r                 = consensus set rank

% HISTORY:
%
% 1.0.0             - 01/13/08 - Initial version
% 1.0.1             - 01/17/08 - Added different M-estimators
% 1.0.2             - 02/22/08 - Removed the M-estimators
%                                Added different ranking modes
% 1.0.3             - 05/26/14 - Removed MLESAC

N = length(CS);

switch mode

    case 'RANSAC'

        r = sum(CS);

    case 'MSAC'

        rho = E;
        rho(rho >= T_noise_squared) = T_noise_squared;

        % set to negative sothat also this rank is to be
        % maximized
        r = -sum(rho)/N;

    otherwise

        error('RANSACToolbox:optionError', 'Unknown ranking mode');

end

return


function outsig=frsyn(F,insig);
%FRSYN  Frame synthesis operator
%   Usage: f=frsyn(F,c);
%
%   `f=frsyn(F,c)` constructs a signal *f* from the frame coefficients *c*
%   using the frame *F*. The frame object *F* must have been created using
%   |frame|.
%
%   Examples:
%   ---------
%
%   In the following example a signal *f* is constructed through the frame
%   synthesis operator using a Gabor frame. The coefficients associated with
%   this Gabor expansion are contained in an identity matrix. The identity
%   matrix corresponds to a diagonal in the time-frequency plane, that is,
%   one atom at each time position with increasing frequency.:::
%
%      a = 10;
%      M = 40;
%
%      F = frame('dgt', 'gauss', a, M);
%
%      c = framenative2coef(F, eye(40));
%
%      f = frsyn(F, c);
%
%   See also: frame, frana, plotframe

complainif_notenoughargs(nargin,2,'FRSYN');
complainif_notvalidframeobj(F,'FRSYN');

L=framelengthcoef(F,size(insig,1));

F=frameaccel(F,L);

outsig=F.frsyn(insig);


function [alpha,xp] = StepSizeSW(f,x,d,alpha,params)
% Line search algorithm satisfying strong Wolfe conditions.
% Algorithms 3.5 on pages 60-61 in Nocedal and Wright.
% Requires x.p, x.f and x.g to be initialized.

alpha0 = params.stpmin;
c1 = params.ftol;
c2 = params.gtol;
% alpha is alpha_i
gxd = x.g'*d;
% alphap is alpha_{i-1}
alphap = alpha0;
fxp = x.f;
i=1;
while norm(alphap-alpha) > params.xtol
  xp.p = x.p + alpha*d;
  xp.f = feval(f,xp.p,1);
  if (xp.f > x.f + c1*alpha*gxd) | ((i > 1) & (xp.f >= fxp)),
    [alpha,xp] = zoom(f,x,d,alphap,alpha,fxp,c1,c2);
    return;
  end
  xp.g = feval(f,xp.p,2); gxpd = xp.g'*d;
  if abs(gxpd) <= -c2*gxd,
    return;
  end
  if gxpd >= 0,
    [alpha,xp] = zoom(f,x,d,alpha,alphap,xp.f,c1,c2);
    return;
  end
  alphap = alpha;
  fxp = xp.f;
  %  alpha = alpha + (params.stpmax-alpha)*rand(1);
  alpha = alpha + (params.stpmax-alpha)*0.5;
  i = i+1;
end
alpha,
error('No stepsize found');

function [alpha,xp] = zoom(f,x,d,alphal,alphah,fxl,c1,c2)
% function [alpha,xp] = zoom(f,x,d,alphal,alphah,fxl)
% Algorithm 3.6 on page 61 in Nocedal and Wright

gxd = x.g'*d;

while 1
   alpha = 1/2*(alphal+alphah);
   xp.p = x.p + alpha*d;
   xp.f = feval(f,xp.p,1);
   if ((xp.f > x.f + c1*alpha*gxd) | (xp.f >= fxl)),
      alphah = alpha;
   else
      xp.g = feval(f,xp.p,2); gxpd = xp.g'*d;
      if abs(gxpd) <= -c2*gxd,
        return;
      end
      if gxpd*(alphah-alphal) >= 0,
        alphah = alphal;
      end
      alphal = alpha;
      fxl = xp.f;
   end
end


function [x, infos] = admm_seq_conv_nmf(V, rank, t, in_options)
% ADMM based convolutive non-negative matrix factorization (ADMM-Conv-NMF).
%
% The problem of interest is defined as
%
%
% Given a non-negative matrix V, factorized non-negative matrices {W, H} are calculated.
%
%
% Inputs:
%       matrix      V
%       rank        rank
%
% Output:
%       w           solution of w
%       infos       information
%
% References:
%
%
% This file is part of NMFLibrary
%
% This file has been ported from
% convNMF_MM1.m and convNMF_MM2.m at https://github.com/lyn202206/ADMM-Convolutive-NMF
% by Yinan Li.
%
% Ported by H.Kasai on June 29, 2022
%
% Change log:
%

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

    % set local options
    local_options = [];
    local_options.metric_type = 'beta-div';
    local_options.d_beta = 2;
    local_options.rho = 1;
    local_options.flag = 1;

    % 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;
    if ~isfield(options, 'x_init')
        W = zeros(m, rank, t);
        for i = 1 : t
            [init_factors, ~] = generate_init_factors(V, rank, init_options);
            W(:, :, i) = init_factors.W;
        end
        H = init_factors.H;
    else
        W = init_options.x_init.W;
        H = init_options.x_init.H;
    end

    % initialize
    epoch = 0;
    grad_calc_count = 0;

    options = check_divergence(options);
    sub_mode = sprintf('beta=%.1f', options.d_beta);
    if ~strcmp(options.metric_type, 'beta-div')
        sub_mode = options.metric_type;
    end
    method_name = sprintf('ADMM-Seq-Conv (%s)', sub_mode);

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

    % initialize for this algorithm
    X = zeros(m, n, t);
    for i=0:t-1
        tW = W(:, :, i+1);
        tH = shift_t(H, i);
        % initial X
        X(:, :, i+1) = tW * tH;
    end

    % some variable for test
    Xplus = X;
    U = zeros(m, n,t);

    alphaX = zeros(m, n,t);
    alphaH = zeros(rank,n);
    alphaW = zeros(m,rank,t);

    Wplus = W;
    Hplus = H;

    % store initial info
    clear infos;

    [Wcon, Hcon] = reconstruct_wh(W, H, t);
    [infos, f_val, optgap] = store_nmf_info(V, Wcon, Hcon, [], options, [], epoch, grad_calc_count, 0);

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

    % set start time
    start_time = tic();

    % main 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

        % update H by accumulation
        P1 = zeros(rank,rank);
        P2 = zeros(rank,n);
        P3 = zeros(rank,n);
        for i = 0 : t-1
            tW = W(:, :, i+1);
            tX = shift_t(X(:, :, i+1), -i);
            talphaX = shift_t(alphaX(:, :, i+1), -i);
            P1 = P1 + tW'*tW;
            P2 = P2 + tW'*tX;
            P3 = P3 + tW'*talphaX;
        end
        H = (P1 + eye(rank)) \ (P2 + Hplus + 1/options.rho * (P3-alphaH));

        % update Hplus
        Hplus = max(H + 1/options.rho * alphaH, 0);

        % update alphaH
        alphaH = alphaH + options.rho * (H - Hplus);

        % update parameters sequentially
        for i = 0 : t-1
            % update Xplus in each time slice
            tW = Wplus(:, :, i+1);
            tH = shift_t(Hplus, i);
            % calculate Xplus
            Xplus(:, :, i+1) = tW * tH;

            % splite V in each time slice which result in U
            U(:, :, i+1) = (tW * tH + eps).*V./(sum(Xplus, 3) + t*eps);

            % update X in each time slice
            if options.d_beta ==2

                % update for Euclidean distance
                tW = W(:, :, i+1);
                tH = shift_t(H, i);
                tV = U(:, :, i+1);

                % update X
                X(:, :, i+1) = (options.rho*tW * tH + tV - alphaX(:, :, i+1)) / (1 + options.rho);

            elseif options.d_beta == 1

                % update for Kullback-Leibler divergence
                tW = W(:, :, i+1);
                tH = shift_t(H, i);
                tV = U(:, :, i+1);

                % update X
                b = options.rho * tW * tH - alphaX(:, :, i+1) - 1;
                X(:, :, i+1) = (b + sqrt(b.^2 + 4 * options.rho * tV)) / (2 * options.rho);

            elseif options.d_beta == 0

                % update for Itakura-Saito divergence
                tW = W(:, :, i+1);
                tH = shift_t(H, i);
                tV = U(:, :, i+1);

                % parameters for update
                A = alphaX(:, :, i+1) / options.rho - tW * tH;
                B = 1/(3 * options.rho) - A.^2/9;
                C = - A.^3/27 + A/(6 * options.rho) + tV / (2*options.rho);
                D = B.^3 + C.^2;

                % update X
                tX = X(:, :, i+1);
                tX(D>=0) = nthroot(C(D>=0)+sqrt(D(D>=0)),3) + ...
                nthroot(C(D>=0)-sqrt(D(D>=0)),3) - ...
                A(D>=0)/3;

                phi = acos(C(D<0) ./ ((-B(D<0)).^1.5));
                tX(D<0) = 2*sqrt(-B(D<0)).*cos(phi/3) - A(D<0)/3;
                X(:, :, i+1) = tX;
            else
                error('The options.d_beta you specified is not currently supported.')
            end

            % update alphaX
            alphaX(:, :, i+1) = alphaX(:, :, i+1) +  options.rho * (X(:, :, i+1) - tW * tH);

            % update W
            tH = shift_t(H, i);
            tX = X(:, :, i+1);
            P = tH * tH' + eye(rank);
            Q = tH * tX' + Wplus(:, :, i+1)' + 1/options.rho*(tH * alphaX(:, :, i+1)' - alphaW(:, :, i+1)');
            W(:, :, i+1) = (P \ Q)';

            % update Wplus
            Wplus(:, :, i+1) = max(W(:, :, i+1) + 1/options.rho * alphaW(:, :, i+1), 0);

            % udpate alphaW
            alphaW(:, :, i+1) = alphaW(:, :, i+1) + options.rho * (W(:, :, i+1) - Wplus(:, :, i+1));
        end

        %  if to update X before the update of U
        if options.flag == 1

            %  update Xplus in each time slice
            for i=0:t-1
                tW = Wplus(:, :, i+1);
                tH = shift_t(Hplus, i);
                % initial X
                Xplus(:, :, i+1) = tW * tH;
            end

            % splite V in each time slice which result in U
            for i=0:t-1
                tW = Wplus(:, :, i+1);
                tH = shift_t(Hplus, i);
                U(:, :, i+1) = (tW * tH + eps) .* V ./ (sum(Xplus, 3) + t*eps);
            end

            % update X in each time slice after the update of the whole W
            if options.d_beta == 2

                % update for Euclidean distance
                for i=0:t-1
                    tW = W(:, :, i+1);
                    tH = shift_t(H, i);
                    tV = U(:, :, i+1);
                    % update X
                    X(:, :, i+1) = (options.rho*tW * tH + tV-alphaX(:, :, i+1)) / (1 + options.rho);
                end

            elseif options.d_beta == 1

                % update for Kullback-Leibler divergence
                for i=0:t-1
                    tW = W(:, :, i+1);
                    tH = shift_t(H, i);
                    tV = U(:, :, i+1);
                    % update X
                    b = options.rho * tW * tH - alphaX(:, :, i+1) - 1;
                    X(:, :, i+1) = (b + sqrt(b.^2 + 4 * options.rho * tV)) / (2 * options.rho);
                end

            elseif options.d_beta == 0

                % update for Itakura-Saito divergence
                for i=0:t-1
                    tW = W(:, :, i+1);
                    tH = shift_t(H, i);
                    tV = U(:, :, i+1);

                    % parameters for update
                    A = alphaX(:, :, i+1) / options.rho - tW * tH;
                    B = 1 / (3 * options.rho) - A.^2/9;
                    C = - A.^3/27 + A / (6 * options.rho) + tV/(2 * options.rho);
                    D = B.^3 + C.^2;

                    % update X
                    tX = X(:, :, i+1);
                    tX(D>=0) = nthroot(C(D>=0)+sqrt(D(D>=0)),3) + ...
                    nthroot(C(D>=0)-sqrt(D(D>=0)),3) - ...
                    A(D>=0)/3;

                    phi = acos(C(D<0) ./ ((-B(D<0)).^1.5));
                    tX(D<0) = 2*sqrt(-B(D<0)).*cos(phi/3) - A(D<0)/3;
                    X(:, :, i+1) = tX;
                end
            else
                error('The options.d_beta you specified is not currently supported.')
            end

           % update for dual variables
            for i=0:t-1
                tW = W(:, :, i+1);
                tH = shift_t(H, i);
                % update alphaX
                alphaX(:, :, i+1) = alphaX(:, :, i+1) +  options.rho*(X(:, :, i+1)-tW * tH);
            end
        end

        % measure gradient calc count
        grad_calc_count = grad_calc_count + m*n;

        % measure elapsed time
        elapsed_time = toc(start_time);

        % update epoch
        epoch = epoch + 1;

        % store info
        [Wcon, Hcon] = reconstruct_wh(W, H, t);
        infos = store_nmf_info(V, Wcon, Hcon, [], options, infos, epoch, grad_calc_count, elapsed_time);

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

    end

    W = Wplus;
    H = Hplus;
    [W, H] = renormalize_convNMF(W, H);

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

end

function [W_concat, H_concat] = reconstruct_wh(W, H, t)

    W_concat = [];
    H_concat = [];
    for j = 1 : t
        W_concat = [W_concat W(:, :,j)];
        H_concat = [H_concat; shift_t(H, j-1)];
    end
end


function test_bug2338

% MEM 2gb
% WALLTIME 00:20:00
% DEPENDENCY ft_prepare_bemmodel ft_prepare_headmodel ft_prepare_leadfield ft_compute_leadfield ft_headmodel_openmeeg

% 4 Layers
r = [85 88 92 100];
c = [1 1/20 1/80 1];

order = [1 2 3 4];

% Description of the spherical mesh
[pnt, tri] = mesh_sphere(42);

% Create a set of electrodes on the outer surface
elec.elecpos = max(r) * pnt;
elec.label = {};
nelec = size(elec.elecpos,1);
for ii=1:nelec
  elec.label{ii} = sprintf('vertex%03d', ii);
end

% Create one triangulated mesh for each boundary, the first boundary is inside
mesh = [];
for ii=1:length(r)
  mesh.bnd(ii).pnt = pnt * r(ii);
  mesh.bnd(ii).tri = tri;
end

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

% Compute the BEM model using the old code
cfg = [];
cfg.method = 'openmeeg';

cfg.conductivity = c(order);
mesh1 = mesh;
mesh1.bnd = mesh1.bnd(order);
vol1 = ft_prepare_bemmodel(cfg, mesh1);

% flip the geometry and conductivity around
order = [4 3 2 1];

cfg.conductivity = c(order);
mesh2 = mesh;
mesh2.bnd = mesh2.bnd(order);
vol2 = ft_prepare_bemmodel(cfg, mesh2);

cfg = [];
cfg.sourcemodel.pos = [0 0 70];
cfg.elec = elec;
cfg.headmodel = vol1;
lf1 = ft_prepare_leadfield(cfg);
cfg.headmodel = vol2;
lf2 = ft_prepare_leadfield(cfg);

assert(isalmostequal(lf1.leadfield{1}, lf2.leadfield{1}, 'reltol', 1e-6));

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

% Compute the BEM model using the new code
cfg = [];
cfg.method = 'openmeeg';

cfg.conductivity = c(order);
mesh1 = mesh;
mesh1.bnd = mesh1.bnd(order);
vol1 = ft_prepare_headmodel(cfg, mesh1);

% flip the geometry and conductivity around
order = [4 3 2 1];

cfg.conductivity = c(order);
mesh2 = mesh;
mesh2.bnd = mesh2.bnd(order);
vol2 = ft_prepare_headmodel(cfg, mesh2);

cfg = [];
cfg.sourcemodel.pos = [0 0 70];
cfg.elec = elec;
cfg.headmodel = vol1;
lf1 = ft_prepare_leadfield(cfg);
cfg.headmodel = vol2;
lf2 = ft_prepare_leadfield(cfg);

assert(isalmostequal(lf1.leadfield{1}, lf2.leadfield{1}, 'reltol', 1e-6));


%
% Bayesian Optimization of Combinatorial Structures
%
% Copyright (C) 2018 R. Baptista & M. Poloczek
%
% BOCS 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.
%
% BOCS 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 BOCS.  If not, see <http://www.gnu.org/licenses/>.
%
% Copyright (C) 2018 MIT & University of Arizona
% Authors: Ricardo Baptista & Matthias Poloczek
% E-mails: rsb@mit.edu & poloczek@email.arizona.edu
%

% Script runs discrete optimization algorithms for the
% contamination control simulation problem. The results are
% compared for different values of the \lambda tuning
% parameter.

clear; close all; clc
addpath(genpath('../algorithms'))
addpath(genpath('../stat_model'))
addpath(genpath('../test_problems/ContStudy'))
addpath(genpath('../tools'))

%% Setup parameters

% Setup fixed parameters
n_vars  = 30;
n_proc  = 1;
test_name = 'contamination';

% Number of runs and optimization iterations
n_func     = 10;
n_runs     = 10;
n_init     = 20;
evalBudget = 270;

% problem parameters (Monte Carlo samples)
mcSamps = 1e2;

% Variance prior parameters (Inverse Gamma)
aPr    = 2;
bPr    = 1;

% Regularization parameters
lambda_vals = [0, 1e-4, 1e-2, 1];
lambda_str  = {'0', '1em4', '1em2', '1'};

% Set additive regularization function
reg_term = @(x) sum(x,2);

%% Generate Test Cases

% setup objective functions
inputs_all = cell(n_func, n_runs);

for t1=1:n_func

    fprintf('Setting up test function %d\n', t1);

    % Generate random case study
    seed = randi(10000,1);

    for t2=1:n_runs

        % Set inputs struct for each problem
        inputs_all{t1,t2} = struct;
        inputs_all{t1,t2}.n_vars      = n_vars;

        % Save other definitions
        inputs_all{t1,t2}.evalBudget  = evalBudget;
        inputs_all{t1,t2}.n_runs      = n_runs;
        inputs_all{t1,t2}.n_init      = n_init;
        inputs_all{t1,t2}.lambda_vals = lambda_vals;

        % Set priors for estimator
        inputs_all{t1,t2}.aPr         = aPr;
        inputs_all{t1,t2}.bPr         = bPr;

        % Save objective function and regularization term
        inputs_all{t1,t2}.model = @(x) contamination_prob(x, mcSamps, seed);
        inputs_all{t1,t2}.reg_term = @(x) reg_term(x);

        % Generate initial samples for statistical models
        inputs_all{t1,t2}.x_vals = sample_models(n_init, n_vars);
        inputs_all{t1,t2}.y_vals = inputs_all{t1,t2}.model(inputs_all{t1,t2}.x_vals);

    end
end

inputs_all = reshape(inputs_all, n_func*n_runs, 1);

% Make folder
mkdir(['../results/' test_name])

% Save test cases
save(['../results/' test_name '/all_tests'])

% Run cases
run_cases(inputs_all, lambda_vals, test_name, n_proc);

% -- END OF FILE --


function kern = diagKernParamInit(kern)

% DIAGKERNPARAMINIT DIAG kernel parameter initialisation.
% The diag covariance function takes a one dimensional input and outputs a diagonal noise that is provided by an exponentiated and scaled version of the input.
%
% k(x_i, x_j) = delta_ij sigma2 exp(x_i)
%
% The only parameter is sigma2, the process variance (kern.variance).
%
% SEEALSO : whiteKernParamInit
%
% FORMAT
% DESC initialises the diagonal noise covariance function
%  kernel structure with some default parameters.
% ARG kern : the kernel structure which requires initialisation.
% RETURN kern : the kernel structure with the default parameters placed in.
%
% SEEALSO : kernCreate, kernParamInit
%
% COPYRIGHT : Neil D. Lawrence, 2011

% KERN

kern.variance = exp(-2);
kern.nParams = 1;

kern.transforms.index = 1;
kern.transforms.type = optimiDefaultConstraint('positive');
kern.trans = optimiDefaultConstraint('positive');

kern.isStationary = false;


function DSO3 = refine(DSO3)

% step 1: compute center of any two orientation connected by an edge

% all combinations of vertices of all tetrahegons
v1 = DSO3.tetra(:,[1 1 1 2 2 3]).';
v2 = DSO3.tetra(:,[2 3 4 3 4 4]).';

% unique edges
vv = sort([v1(:),v2(:)],2);
[v,~,iCenterEdges] = unique(vv,'rows');
iCenterEdges = length(DSO3) + reshape(iCenterEdges,6,[]).';

% step 2: set up corner tetrahegons
% bases
base = iCenterEdges(:,[1 2 3  1 4 5  2 4 6  3 5 6]);
base = reshape(base.',3,[]);
corner = reshape(DSO3.tetra.',1,[]);

% split center octaeder into four tetrahegons
octaeder = iCenterEdges(:,[1 3 4 5  3 4 5 6  1 2 3 4  2 3 4 6]);
octaeder = reshape(octaeder.',4,[]);

% set up tetraeder
newTetra = [[base;corner],octaeder].';

% new vertices as mean of all edges
centerEdges = mean2(DSO3.subSet(v(:,1)),DSO3.subSet(v(:,2)));

% set up refined grid
DSO3.a = [DSO3.a(:);centerEdges.a];
DSO3.b = [DSO3.b(:);centerEdges.b];
DSO3.c = [DSO3.c(:);centerEdges.c];
DSO3.d = [DSO3.d(:);centerEdges.d];
DSO3.i = zeros(size(DSO3.d));

% set up new tetrahegons
DSO3.tetra = sort(newTetra,2);

% compute neighbouring list
DSO3.tetraNeighbour = calcNeighbour(DSO3.tetra);

% the neighbours of the four corner tetrahegons
%1 2 3 4
%cornerNeighbour =
%DSO3.tetraNeighbour

% set up new lookup
%DSO3.lookup = DSO3.lookup*8;
%DSO3.lookup = calcLookUp(DSO3,5*degree);
DSO3.lookup = [];
res = 40*degree;
for i = 1:3
  res = res / 2;
  DSO3.lookup = calcLookUp(DSO3,res);
end

end

% -----------------------------------------------------------------
function v = mean2(v1,v2)
% compute the mean of two orientations

dv = inv(v1) .* v2;
dv.SS = specimenSymmetry;

[dv,omega] = dv.project2FundamentalRegion;

% half angle
dv.a = dv.a .* cos(omega./4) ./ cos(omega./2);
dv.b = dv.b .* sin(omega./4) ./ sin(omega./2);
dv.c = dv.c .* sin(omega./4) ./ sin(omega./2);
dv.d = dv.d .* sin(omega./4) ./ sin(omega./2);

% new vertices
v = v1 .* dv;

end


function ppg_filt(up)
%PPG_FILT extracts respiratory signals using various filtering techniques
% from the PPG signal as specified in PC's literature review.
%	            ppg_filt(option, up)
%
%	Inputs:
%		option          the option which has led to this function being used
%       up              universal parameters structure
%
%	Outputs:
%       ...
%

fprintf('\n--- Extracting Respiratory Signals from PPG using Filtering Techniques ');
log_int_respSig = 0;             % Has value 1 unless this is a final respiratory signal

for subj = up.paramSet.subj_list

    %% Cycle through each ppg signal
    for sig_no = 1 : length(up.paramSet.ppg_sigs)

        %% Cycle through each method
        for filt_no = 1 : length(up.al.options.ppg_filt)

            %% Skip if this processing has been done previously
            eval(['save_name = ''' up.paramSet.ppg_sigs{sig_no}, up.paths.filenames.filt '_' up.al.options.ppg_filt{filt_no} ''';']);
            iden_resp_sig_file_ending
            savepath = [up.paths.data_save_folder, num2str(subj), ending];
            exist_log = check_exists(savepath, save_name);
            if exist_log
                continue
            end

            %% Load relevant data
            if ~exist('data', 'var')
                load([up.paths.data_load_folder, up.paths.data_load_filename]);
            end
            % Extract PPG data
            eval(['rel_data = data(subj).' up.paramSet.ppg_sigs{sig_no} ';']);

            %% Filter the raw signal using this method
            respWave = feval(up.al.options.ppg_filt{filt_no}, rel_data, up);

            %% Band-pass filter
            filtered_data = bpf_signal_to_remove_non_resp_freqs(respWave, respWave.fs, up);
            eval([save_name ' = filtered_data;']);

            %% Save processed data
            save_or_append_data

        end

    end

end

end


function b = dppsl ( ap, n, b )

%*****************************************************************************80
%
%% DPPSL solves a real symmetric positive definite system factored by DPPCO or DPPFA.
%
%  Discussion:
%
%    To compute inverse(A) * C where C is a matrix with P columns
%
%      call dppco ( ap, n, rcond, z, info )
%
%      if ( rcond is too small .or. info /= 0 ) then
%        exit
%      end if
%
%      do j = 1, p
%        call dppsl ( ap, n, c(1,j) )
%      end do
%
%    A division by zero will occur if the input factor contains
%    a zero on the diagonal.  Technically this indicates
%    singularity but it is usually caused by improper subroutine
%    arguments.  It will not occur if the subroutines are called
%    correctly and INFO == 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real AP(N*(N+1)/2), the output from DPPCO or DPPFA.
%
%    Input, integer N, the order of the matrix.
%
%    Input, real B(N), the right hand side.
%
%    Output, real B(N), the solution.
%
  kk = 0;

  for k = 1 : n
    t = ddot ( k-1, ap(kk+1:kk+k-1), 1, b(1:k-1), 1 );
    kk = kk + k;
    b(k) = ( b(k) - t ) / ap(kk);
  end

  for k = n : -1 : 1
    b(k) = b(k) / ap(kk);
    kk = kk - k;
    t = -b(k);
    b(1:k-1) = daxpy ( k-1, t, ap(kk+1:kk+k-1), 1, b(1:k-1), 1 );
  end

  return
end


function M = productmanifold(elements)
% Returns a structure describing a product manifold M = M1 x M2 x ... x Mn.
%
% function M = productmanifold(elements)
%
% Input: an elements structure such that each field contains a manifold
% structure.
%
% Output: a manifold structure M representing the manifold obtained by
% taking the Cartesian product of the manifolds described in the elements
% structure, with the metric obtainded by element-wise extension. Points
% and vectors are stored as structures with the same fieldnames as in
% elements.
%
% Example:
% M = productmanifold(struct('X', spherefactory(3), 'Y', spherefactory(4)))
% disp(M.name());
% x = M.rand()
%
% Points of M = S^2 x S^3 are represented as structures with two fields, X
% and Y. The values associated to X are points of S^2, and likewise points
% of S^3 for the field Y. Tangent vectors are also represented as
% structures with two corresponding fields X and Y.
%
% See also: powermanifold

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:
%
%   July  4, 2013 (NB):
%       Added support for vec, mat, tangent.
%       Added support for egrad2rgrad and ehess2rhess.
%       Modified hash function to make hash strings shorter.
%
%   Dec. 17, 2018 (NB):
%       Added check all_elements_provide() to many functions, so that if,
%       for example, one of the elements does not provide exp(), then the
%       product manifold also won't provide exp(). This makes it easier for
%       tools such as, for example, checkgradient, to determine whether exp
%       is available or not.
%
%   Feb. 10, 2020 (NB):
%       Added warnings about calling egrad2rgrad and ehess2rhess without
%       storedb and key, even if some base manifolds allow them.
%
%   Jan. 4, 2021 (NB):
%       Changes for compatibility with Octave 6.1.0: by introducing a
%       "helper" function, we separate out the pre-computations. This way,
%       all pre-computed quantities are passed as input to the helper
%       function. This makes them available to nested subfunctions.
%       The extra step is not necessary in Matlab.

    elems = fieldnames(elements);
    nelems = numel(elems);

    assert(nelems >= 1, ...
           'elements must be a structure with at least one field.');

    % Below are some precomputations for the mat/vec pair.
    %
    % Gather the length of the column vector representations of tangent
    % vectors for each of the manifolds. Raise a flag if any of the base
    % manifolds has no vec function available.
    vec_available = true;
    vec_lens = zeros(nelems, 1);
    for ii = 1 : nelems
        Mi = elements.(elems{ii});
        if isfield(Mi, 'vec')
            rand_x = Mi.rand();
            zero_u = Mi.zerovec(rand_x);
            vec_lens(ii) = length(Mi.vec(rand_x, zero_u));
        else
            vec_available = false;
            break;
        end
    end
    vec_pos = cumsum([1 ; vec_lens]);
    %
    vecmatareisometries = vec_available;
    for ii = 1 : nelems
        if ~isfield(elements.(elems{ii}), 'vecmatareisometries') || ...
           ~elements.(elems{ii}).vecmatareisometries()
            vecmatareisometries = false;
            break;
        end
    end
    %
    % Above are some precomputations for the mat/vec pair.

    % The helper function is the actual factory.
    M = productmanifoldhelper(elements, elems, nelems, vec_available, ...
                              vec_pos, vecmatareisometries);

end

function M = productmanifoldhelper(elements, elems, nelems, ...
                                   vec_available, vec_pos, ...
                                   vecmatareisometries)

    % Handy function to check if all elements provide the necessary methods
    function answer = all_elements_provide(method_name)
        answer = false;
        for i = 1 : nelems
            if ~isfield(elements.(elems{i}), method_name)
                return;
            end
        end
        answer = true;
    end

    M.name = @name;
    function str = name()
        str = 'Product manifold: ';
        str = [str sprintf('[%s: %s]', ...
                           elems{1}, elements.(elems{1}).name())];
        for i = 2 : nelems
            str = [str sprintf(' x [%s: %s]', ...
                   elems{i}, elements.(elems{i}).name())]; %#ok<AGROW>
        end
    end

    M.dim = @dim;
    function d = dim()
        d = 0;
        for i = 1 : nelems
            d = d + elements.(elems{i}).dim();
        end
    end

    M.inner = @inner;
    function val = inner(x, u, v)
        val = 0;
        for i = 1 : nelems
            val = val + elements.(elems{i}).inner(x.(elems{i}), ...
                                               u.(elems{i}), v.(elems{i}));
        end
    end

    M.norm = @(x, d) sqrt(M.inner(x, d, d));

    if all_elements_provide('dist')
        M.dist = @dist;
    end
    function d = dist(x, y)
        sqd = 0;
        for i = 1 : nelems
            sqd = sqd + elements.(elems{i}).dist(x.(elems{i}), ...
                                                 y.(elems{i}))^2;
        end
        d = sqrt(sqd);
    end

    if all_elements_provide('typicaldist')
        M.typicaldist = @typicaldist;
    end
    function d = typicaldist
        sqd = 0;
        for i = 1 : nelems
            sqd = sqd + elements.(elems{i}).typicaldist()^2;
        end
        d = sqrt(sqd);
    end

    M.proj = @proj;
    function v = proj(x, u)
        for i = 1 : nelems
            v.(elems{i}) = elements.(elems{i}).proj(x.(elems{i}), ...
                                                    u.(elems{i}));
        end
    end

    M.tangent = @tangent;
    function v = tangent(x, u)
        for i = 1 : nelems
            v.(elems{i}) = elements.(elems{i}).tangent(x.(elems{i}), ...
                                                       u.(elems{i}));
        end
    end

    % True by default, false if any false encountered
    M.tangent2ambient_is_identity = true;
    for k = 1 : nelems
        if isfield(elements.(elems{k}), 'tangent2ambient_is_identity')
            if ~elements.(elems{k}).tangent2ambient_is_identity
                M.tangent2ambient_is_identity = false;
                break;
            end
        end
    end

    M.tangent2ambient = @tangent2ambient;
    function v = tangent2ambient(x, u)
        for i = 1 : nelems
            if isfield(elements.(elems{i}), 'tangent2ambient')
                v.(elems{i}) = ...
                    elements.(elems{i}).tangent2ambient( ...
                                               x.(elems{i}), u.(elems{i}));
            else
                v.(elems{i}) = u.(elems{i});
            end
        end
    end

    M.egrad2rgrad = @egrad2rgrad;
    function g = egrad2rgrad(x, g)
        for i = 1 : nelems
            g.(elems{i}) = elements.(elems{i}).egrad2rgrad(...
                                               x.(elems{i}), g.(elems{i}));
        end
    end
    for ii = 1 : nelems
        if nargin(elements.(elems{ii}).egrad2rgrad) > 2
            warning('manopt:productmanifold:egrad2rgrad', ...
                   ['Product manifolds call M.egrad2rgrad with only two ', ...
                    'inputs:\nstoredb and key won''t be available.']);
            break;
        end
    end

    M.ehess2rhess = @ehess2rhess;
    function h = ehess2rhess(x, eg, eh, h)
        for i = 1 : nelems
            h.(elems{i}) = elements.(elems{i}).ehess2rhess(...
                 x.(elems{i}), eg.(elems{i}), eh.(elems{i}), h.(elems{i}));
        end
    end
    for ii = 1 : nelems
        if nargin(elements.(elems{ii}).ehess2rhess) > 4
            warning('manopt:productmanifold:ehess2rhess', ...
                   ['Product manifolds call M.ehess2rhess with only two ', ...
                    'inputs:\nstoredb and key won''t be available.']);
            break;
        end
    end

    if all_elements_provide('exp')
        M.exp = @exp;
    end
    function y = exp(x, u, t)
        if nargin < 3
            t = 1.0;
        end
        for i = 1 : nelems
            y.(elems{i}) = elements.(elems{i}).exp(x.(elems{i}), ...
                                                   u.(elems{i}), t);
        end
    end

    M.retr = @retr;
    function y = retr(x, u, t)
        if nargin < 3
            t = 1.0;
        end
        for i = 1 : nelems
            y.(elems{i}) = elements.(elems{i}).retr(x.(elems{i}), ...
                                                    u.(elems{i}), t);
        end
    end

    if all_elements_provide('log')
        M.log = @log;
    end
    function u = log(x1, x2)
        for i = 1 : nelems
            u.(elems{i}) = elements.(elems{i}).log(x1.(elems{i}), ...
                                                   x2.(elems{i}));
        end
    end

    M.hash = @hash;
    function str = hash(x)
        str = '';
        for i = 1 : nelems
            str = [str elements.(elems{i}).hash(x.(elems{i}))]; %#ok<AGROW>
        end
        str = ['z' hashmd5(str)];
    end

    M.lincomb = @lincomb;
    function v = lincomb(x, a1, u1, a2, u2)
        if nargin == 3
            for i = 1 : nelems
                v.(elems{i}) = elements.(elems{i}).lincomb(x.(elems{i}), ...
                                                        a1, u1.(elems{i}));
            end
        elseif nargin == 5
            for i = 1 : nelems
                v.(elems{i}) = elements.(elems{i}).lincomb(x.(elems{i}), ...
                                     a1, u1.(elems{i}), a2, u2.(elems{i}));
            end
        else
            error('Bad usage of productmanifold.lincomb');
        end
    end

    M.rand = @rand;
    function x = rand()
        for i = 1 : nelems
            x.(elems{i}) = elements.(elems{i}).rand();
        end
    end

    M.randvec = @randvec;
    function u = randvec(x)
        for i = 1 : nelems
            u.(elems{i}) = elements.(elems{i}).randvec(x.(elems{i}));
        end
        u = M.lincomb(x, 1/sqrt(nelems), u);
    end

    M.zerovec = @zerovec;
    function u = zerovec(x)
        for i = 1 : nelems
            u.(elems{i}) = elements.(elems{i}).zerovec(x.(elems{i}));
        end
    end

    if all_elements_provide('transp')
        M.transp = @transp;
    end
    function v = transp(x1, x2, u)
        for i = 1 : nelems
            v.(elems{i}) = elements.(elems{i}).transp(x1.(elems{i}), ...
                                              x2.(elems{i}), u.(elems{i}));
        end
    end

    if all_elements_provide('pairmean')
        M.pairmean = @pairmean;
    end
    function y = pairmean(x1, x2)
        for i = 1 : nelems
            y.(elems{i}) = elements.(elems{i}).pairmean(x1.(elems{i}), ...
                                                        x2.(elems{i}));
        end
    end

    if vec_available
        M.vec = @vec;
        M.mat = @mat;
    end

    function u_vec = vec(x, u_mat)
        u_vec = zeros(vec_pos(end)-1, 1);
        for i = 1 : nelems
            range = vec_pos(i) : (vec_pos(i+1)-1);
            u_vec(range) = elements.(elems{i}).vec(x.(elems{i}), ...
                                                   u_mat.(elems{i}));
        end
    end

    function u_mat = mat(x, u_vec)
        u_mat = struct();
        for i = 1 : nelems
            range = vec_pos(i) : (vec_pos(i+1)-1);
            u_mat.(elems{i}) = elements.(elems{i}).mat(x.(elems{i}), ...
                                                       u_vec(range));
        end
    end

    M.vecmatareisometries = @() vecmatareisometries;

    if all_elements_provide('lie_identity')
        M.lie_identity = @lie_identity;
    end

    function I = lie_identity()
        I = struct();
        for i = 1 : nelems
            Mi = elements.(elems{i});
            Ii = Mi.lie_identity();
            I.(elems{i}) = Ii;
        end
    end
end


function testBranin(varargin)

run_high_dim = 0;
embedding = 1;
embedding_consecutive = 0;
test_percentage = 0;

total_iter = 500;
high_dim = 25;
rotate = 0;

%% Test the percentage of success
if test_percentage
    dim = 2;                                         % Intrinsic Dimensionality.
    used_dim = 2;                                              % Dimension used.

    maximizers = trueMaximizer();
    scale = max(1.5*log(used_dim));
    test_bounds = stardardBounds(dim)*scale;                  % Standard Bounds.

    prct = success_prctg(high_dim, 1000, dim, used_dim, test_bounds, ...
        maximizers);                         % Success Percentage by simulation.

    fprintf('Success Percentage by using %d dimensions is approximately: \n%f.\n', ...
        used_dim, prct);
end

%% Random embedding with true intrisic dimensions
%  True maximum is ensured to fall in to the bounds
if embedding
    dim = 2;
    model = rembo(total_iter, dim, high_dim, rotate, 1);
    ditance_log_plot(total_iter, model.f);
end

%% Random embedding with true intrisic dimensions
%  True maximum is NOT ensured to fall in to the bounds
%  Runs are repeated to ensure covering of the true maximum
if embedding_consecutive
    dim = 2;
    rotate = 0;
    run_iter = 125; num_trial = 4;
    f_values = zeros(run_iter*num_trial,1);

    for i = 1:num_trial
        model = rembo(run_iter, dim, high_dim, rotate);
        f_values((i-1)*run_iter+1:i*run_iter) = model.f;
    end

    ditance_log_plot(run_iter*num_trial, f_values);
end

%% High Dim
if run_high_dim
    rotate = 0;
    model = rembo(total_iter, high_dim, high_dim, rotate, 0, 0);

    ditance_log_plot(total_iter, model.f);
end

%% Run rembo.
    function model = rembo(total_iter, dim, high_dim, rotate, force_in_bounds, ...
        embed)
        % total_iter: total number of iterations.
        % dim: embedding dimension.
        % high_dim: ambient dimension.
        % roate: whether to randomly rotate the objective function.
        % force_in_bounds: to force an optimizer in bound by repeatly drawing
        %                  random embedding matrices.
        % embed: Whether to use a randome embedding matrix. (If not
        %        then we effectively use regular BO.)

        if nargin < 5
            force_in_bounds = 0;       % Whether to force an optimizer in bound.
        end

        if nargin < 6
            embed = 1;                        % Whether to use embedding or not.
        end

        if rotate
            % Rotate the objective function.
            [rm, ~] = qr(randn(high_dim, high_dim), 0);
        else
            % Do not rotate the objective function.
            rm = eye(high_dim);
        end

        if embed
            % Generate random projection matrix A.
            [in_bounds, A] = test_fall_in_bound(dim, rm);
            while ~in_bounds && force_in_bounds
                % Ensure that at least one maximizer fall in bound by
                % generating as many random projection matrix A as needed.
                [in_bounds, A] = test_fall_in_bound(dim, rm);
            end
        else
            % By setting A to be identity we do not use embedding here.
            A = eye(high_dim, dim);
        end

        scale = max(1.5*log(dim), 1);
        bounds = stardardBounds(dim)*scale;                 % Initialize bounds.
        obj_fct = @(x)-branin((A*x')');     % Initialize the objective function.

        init_pt = zeros(1, dim);                                % Initial point.
        init_f = obj_fct(init_pt);                     % Evaluate initial point.

        hyp = [ones(dim, 1)*0.1 ; 1];          % Setup initial hyper-parameters.
        hyp = log(hyp);

        % Initialize model.
        model = init_model(dim, bounds, init_pt, init_f, hyp, 1e-10, 'ard');
        % Do optimization.
        model = sparse_opt(obj_fct, total_iter-1, model);
    end

%% Helper functions.
    function ditance_log_plot(total_iter, fvalues)
        maximazer = trueMaximizer();
        figure;
        dis = zeros(total_iter,1);
        for i =1:total_iter
            dis(i) = -branin(maximazer(:, 1)') - max(fvalues(1:i));
        end
        loglog(1:total_iter, dis);
    end

    function [in_bounds, A] = test_fall_in_bound(used_dim, rm)
        maximizers = trueMaximizer();
        test_bounds = stardardBounds(2);
        scale = max(1.5*log(used_dim));
        test_bounds = test_bounds*scale;

        [prct, A] = success_prctg(high_dim, 1, 2, used_dim, test_bounds,...
            maximizers, rm);
        in_bounds =  prct;

        if in_bounds
            fprintf('At least one maximizer in bounds.\n');
        else
            fprintf('NO maximizer in bounds.\n');
        end
    end

    function [maximizers] = trueMaximizer()
        bounds_branin = [-5,10; 0, 15];
        maximizers = [pi, -pi, 9.42478; 2.275, 12.275, 2.475];
        maximizers = bsxfun(@minus, maximizers, bounds_branin(:, 1));
        maximizers = bsxfun(@rdivide, maximizers, bounds_branin(:, 2) - ...
            bounds_branin(:, 1))*2-1;
    end

    function [prct, A] = success_prctg(high_dim, num_trial, dim,...
        used_dim, bounds, maximizers, rm)

        total = 0;
        cmbnts = combntns(1:used_dim,dim);
        num_maximizers = size(maximizers, 2);

        for i = 1:num_trial
            indices = 1:high_dim;
            A = randn(high_dim, used_dim);

            if nargin > 6
                A = rm*A;
            end
            fail = 1;

            for j = 1:size(cmbnts, 1)
                for k = 1:num_maximizers
                    true_maximizer = inv(A(indices(1:dim), cmbnts(j, :))) * ...
                        maximizers(:, k);
                    if ~(sum(true_maximizer <= bounds(:,2)) < dim || ...
                        sum(true_maximizer >= bounds(:,1)) < dim)
                        fail = 0;
                    end
                end
            end
            total = total + fail;
        end
        prct = 1 - total/num_trial;
    end
end


% SP_EVALUATE_ELEMENT_LIST_PARAM: compute the basis functions, in the parametric domain, in a given list of elements.
%
%     sp = sp_evaluate_element_list_param (space, msh_elems, 'option1', value1, ...)
%
% INPUTS:
%
%    space:   object defining the space of discrete functions (see sp_vector)
%    msh_elems: msh structure containing the information of quadrature or
%               visualization points, for a given list of elements
%               (see msh_cartesian/msh_evaluate_element_list)
%   'option', value: additional optional parameters, currently available options are:
%
%              Name     |   Default value |  Meaning
%           ------------+-----------------+----------------------------------
%            value      |      true       |  compute shape_functions
%            gradient   |      false      |  compute shape_function_gradients
%            divergence |      false      |  compute shape_function_divs
%            curl       |      false      |  compute shape_function_curls
%            hessian    |      false      |  compute shape_function_hessians
%
% OUTPUT:
%
%    sp: struct representing the discrete function space, with the following fields:
%              (see the article for a detailed description)
%
%    FIELD_NAME      (SIZE)                                     DESCRIPTION
%    ncomp           (scalar)                                   number of components of the functions of the space
%    ndof            (scalar)                                   total number of degrees of freedom
%    ndof_dir        (ncomp_param x ndim matrix)                for each component, number of degrees of freedom along each direction
%    nsh_max         (scalar)                                   maximum number of shape functions per element
%    nsh             (1 x msh_col.nel vector)                   actual number of shape functions per each element
%    connectivity    (nsh_max x msh_col.nel vector)             indices of basis functions that do not vanish in each element
%    shape_functions (ncomp_param x msh_col.nqn x nsh_max x msh_col.nel)  basis functions evaluated at each quadrature node in each element
%    shape_function_gradients
%       (ncomp_param x ndim x msh_col.nqn x nsh_max x msh_col.nel) basis function gradients evaluated at each quadrature node in each element
%    shape_function_hessians
%       (ncomp_param x ndim x ndim x msh_col.nqn x nsh_max x msh_col.nel) basis function hessians evaluated at each quadrature node in each element
%    shape_function_divs (msh_col.nqn x nsh_max x msh_col.nel)     basis function divergence evaluated at each quadrature node in each element
%    shape_function_curls
%         2D:  (msh_col.nqn x nsh_max x msh_col.nel)               basis function curl evaluated at each quadrature node in each element
%         3D:  (3 x msh_col.nqn x nsh_max x msh_col.nel)
%
% Copyright (C) 2009, 2010, 2011 Carlo de Falco
% Copyright (C) 2011, 2015, 2019 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function sp = sp_evaluate_element_list_param (space, msh, varargin)

value = true;
gradient = false;
divergence = false;
curl = false;
hessian = false;
if (~isempty (varargin))
  if (~rem (length (varargin), 2) == 0)
    error ('sp_evaluate_element_list_param: options must be passed in the [option, value] format');
  end
  for ii=1:2:length(varargin)-1
    if (strcmpi (varargin {ii}, 'value'))
      value = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'gradient'))
      gradient = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'curl'))
      curl = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'divergence'))
      divergence = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'hessian'))
      hessian = varargin {ii+1};
    else
      warning ('Ignoring unknown option %s', varargin {ii});
    end
  end
end

first_der = gradient || divergence || curl;
for icomp = 1:space.ncomp_param
  sp_col_scalar(icomp) = sp_evaluate_element_list_param (space.scalar_spaces{icomp}, msh, 'value', value, 'gradient', first_der, 'hessian', hessian);
end

ndof_scalar = [sp_col_scalar.ndof];

ndof = sum (ndof_scalar);
nsh  = zeros (1, msh.nel);
connectivity = [];
aux = 0;
for icomp = 1:space.ncomp_param
  ndof_dir(icomp,:) = sp_col_scalar(icomp).ndof_dir;
  nsh = nsh + sp_col_scalar(icomp).nsh(:)';

  inds = find (sp_col_scalar(icomp).connectivity);
  sp_col_scalar(icomp).connectivity(inds) = sp_col_scalar(icomp).connectivity(inds) + aux;
  connectivity = [connectivity; sp_col_scalar(icomp).connectivity];
  aux = aux + ndof_scalar(icomp);
end

sp = struct('nsh_max', space.nsh_max, 'nsh', nsh, 'ndof', ndof,  ...
            'ndof_dir', ndof_dir, 'connectivity', connectivity, ...
            'ncomp', space.ncomp, 'ncomp_param', space.ncomp_param);

if (value)
  sp.shape_functions = zeros (sp.ncomp_param, msh.nqn, sp.nsh_max, msh.nel);
  for icomp = 1:space.ncomp_param
    indices = space.cumsum_nsh(icomp)+(1:sp_col_scalar(icomp).nsh_max);
    sp.shape_functions(icomp,:,indices,:) = sp_col_scalar(icomp).shape_functions;
  end
end

if (gradient || curl || divergence)
  shape_fun_grads = zeros (space.ncomp_param, msh.ndim, msh.nqn, sp.nsh_max, msh.nel);

  for icomp = 1:space.ncomp_param
    indices = space.cumsum_nsh(icomp)+(1:sp_col_scalar(icomp).nsh_max);
    shape_fun_grads(icomp,:,:,indices,:) = sp_col_scalar(icomp).shape_function_gradients;
  end

  if (gradient)
    sp.shape_function_gradients = shape_fun_grads;
  end

  if (divergence)
    sp.shape_function_divs = zeros (msh.nqn, sp.nsh_max, msh.nel);
    for icomp = 1:space.ncomp_param
      sp.shape_function_divs = sp.shape_function_divs + ...
        reshape (shape_fun_grads(icomp,icomp,:,:,:), msh.nqn, sp.nsh_max, msh.nel);
    end
  end

  if (curl)
    if (space.ncomp_param == 2)
      sp.shape_function_curls = reshape (shape_fun_grads(2,1,:,:,:) - ...
	 	       shape_fun_grads(1,2,:,:,:), msh.nqn, sp.nsh_max, msh.nel);
    elseif (space.ncomp_param == 3)
      shape_fun_curls = zeros (space.ncomp, msh.nqn, sp.nsh_max, msh.nel);
      for icomp = 1:space.ncomp
        ind1 = mod(icomp,3) + 1;
        ind2 = mod(ind1, 3) + 1;
        shape_fun_curls(icomp,:,:,:) = reshape (shape_fun_grads(ind2,ind1,:,:,:) - ...
                   shape_fun_grads(ind1,ind2,:,:,:), 1, msh.nqn, sp.nsh_max, msh.nel);
      end
      sp.shape_function_curls = shape_fun_curls;
    end
  end
end

if (hessian)
  sp.shape_function_hessians = zeros (space.ncomp, msh.ndim, msh.ndim, msh.nqn, sp.nsh_max, msh.nel);
  for icomp = 1:space.ncomp_param
    indices = space.cumsum_nsh(icomp)+(1:sp_col_scalar(icomp).nsh_max);
    sp.shape_function_hessians(icomp,:,:,:,indices,:) = sp_col_scalar(icomp).shape_function_hessians;
  end
end

end


function G = Grid2GlinearROI(Grid,Grid_dec,npxdtc,ctrIdx,block,interp,nblk,vmsk,nnzmsk)
% Grid2Glinear
% input Grid/s generated in fbkp_geometry
% -Grid is a matrix of integers that are coordinate of the detector pixel
% -Grid_dec is [] is the 'nearest neighbour' interp option is used
% -block (integer & scalar) sets the projection undergoing backprojection
% -nblk number of blocks (i.e. projections) used to make up the (rows) sparse matrix
% i.e. the the large block goes from block (i.e. projection) to block + nblk
% -vmsk is the mask as a vector (logical)
% -npxdtc number of detector pixels ( nb in Glinear)
% output one sparse matrix G as by Glinear (see Fessler code)
%
% Gianni Schena March  2003

G=[];
block
nargin

Nx=size(Grid(:,:,1),1); Ny=size(Grid(:,:,1),2); % get the size of the problem
nm=Nx*Ny
%

if nargin < 8
	vmsk=logical(ones(1,nm)); nnzmsk=nm;
	% define default mask - if the mask is not an input parameter
end

if nargin < 7, nblk=1, end

%SPALLOC(M,N,NZMAX) creates an M-by-N all zero sparse matrix with room to eventually hold NZMAX nonzeros.

if strcmp(interp, 'nearest neighbor')
	G = spalloc(nblk*npxdtc,nnzmsk, nblk*nnzmsk);
else
	G = spalloc(nblk*npxdtc,nnzmsk, 2*nblk*nnzmsk); % pre-allocation
end

jc=[1:1:nnzmsk]; % index of columns in the matrix G

for ib=1:nblk % for nblk projections and starting from block

	Gv=Grid(:,:,block+ib-1) ; Gv=Gv(:) ; % starts from block
	ir = double(Gv) + ctrIdx  ; % the coordinate (saved as 'round') + centre
	ir=[ir+npxdtc*(ib-1)]; ir=ir(vmsk)';

	% S = SPARSE(i,j,s,m,n,nzmax) uses the rows of [i,j,s] to generate an
	% m-by-n sparse matrix with space allocated for nzmax nonzeros.

	if strcmp(interp, 'nearest neighbor')
		v=1.;
		T=sparse(ir,jc,double(v),npxdtc*nblk,nnzmsk);

	else % i.e. interp == linear
		dec=double(Grid_dec(:,:,block+ib-1))/100; % use also the decimal saved as integer
		vc = dec(:); % weight for (floor +1) == weight of 'ceil' term
		vc=vc(vmsk);
		T= [sparse(ir,jc,(1-vc),npxdtc*(nblk),nnzmsk) + sparse(ir+1,jc,vc,npxdtc*(nblk),nnzmsk)];
		%T= [sparse(ir,jc',(1-vc)) + sparse([ir+1],jc',vc)];
	end

	G=[G+T];
end % end ib


function A = makehatch(hatch)
%MAKEHATCH Predefined hatch patterns
%  MAKEHATCH(HATCH) returns a matrix with the hatch pattern for HATCH
%   according to the following table:
%      HATCH        pattern
%     -------      ---------
%        /          right-slanted lines
%        \          left-slanted lines
%        |          vertical lines
%        -          horizontal lines
%        +          crossing vertical and horizontal lines
%        x          criss-crossing lines
%        .          single dots
%
%  See also: APPLYHATCH

%  By Ben Hinkle, bhinkle@mathworks.com
%  This code is in the public domain.

n = 6;
A=zeros(n);
switch (hatch)
case '/'
  A = fliplr(eye(n));
case '\'
  A = eye(n);
case '|'
  A(:,1) = 1;
case '-'
  A(1,:) = 1;
case '+'
  A(:,1) = 1;
  A(1,:) = 1;
case 'x'
  A = eye(n) | fliplr(diag(ones(n-1,1),-1));
case '.'
  A(1:2,1:2)=1;
otherwise
  error(['Undefined hatch pattern "' hatch '".']);
end


function results = vl_test_binsearch(varargin)
% VL_TEST_BINSEARCH
vl_test_init ;

function test_inf_bins()
x = [-inf -1 0 1 +inf] ;
vl_assert_equal(vl_binsearch([],          x), [0 0 0 0 0]) ;
vl_assert_equal(vl_binsearch([-inf 0],    x), [1 1 2 2 2]) ;
vl_assert_equal(vl_binsearch([-inf],      x), [1 1 1 1 1]) ;
vl_assert_equal(vl_binsearch([-inf +inf], x), [1 1 1 1 2]) ;

function test_empty()
vl_assert_equal(vl_binsearch([], []), []) ;

function test_bnd()
vl_assert_equal(vl_binsearch([], [1]),    [0]) ;
vl_assert_equal(vl_binsearch([], [-inf]), [0]) ;
vl_assert_equal(vl_binsearch([], [+inf]), [0]) ;

vl_assert_equal(vl_binsearch([1], [.9]),   [0]) ;
vl_assert_equal(vl_binsearch([1], [1]),    [1]) ;
vl_assert_equal(vl_binsearch([1], [-inf]), [0]) ;
vl_assert_equal(vl_binsearch([1], [+inf]), [1]) ;

function test_basic()
vl_assert_equal(vl_binsearch(-10:10, -10:10), 1:21) ;
vl_assert_equal(vl_binsearch(-10:10, -11:10), 0:21) ;
vl_assert_equal(vl_binsearch(-10:10, [-inf, -11:10, +inf]), [0 0:21 21]) ;

function test_frac()
vl_assert_equal(vl_binsearch(1:10, 1:.5:10), floor(1:.5:10))
vl_assert_equal(vl_binsearch(1:10, fliplr(1:.5:10)), ...
                fliplr(floor(1:.5:10))) ;

function test_array()
a = reshape(1:100,10,10) ;
b = reshape(1:.5:100.5, 2, []) ;
c = floor(b) ;
vl_assert_equal(vl_binsearch(a,b), c) ;


function obox = orientedBox(points)
%ORIENTEDBOX Minimum-width oriented bounding box of a set of points
%
%   OBOX = orientedBox(PTS)
%   Computes the oriented bounding box of a set of points. Oriented box is
%   defined by a center, two dimensions (the length and the width), and the
%   orientation of the length axis. Orientation is counted in degrees,
%   counter-clockwise.
%
%   Example
%     % Draw oriented bounding box of an ellipse
%     elli = [30 40 40 20 30];
%     pts = ellipseToPolygon(elli, 120);
%     obox = orientedBox(pts);
%     figure; hold on;
%     drawEllipse(elli);
%     drawOrientedBox(obox, 'm');
%
%   See also
%   drawOrientedBox, orientedBoxToPolygon
%

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

%% initialisations

% first, compute convex hull of the polygon
inds = convhull(points(:,1), points(:,2));
hull = points(inds, :);

% if first and last points are the same, remove the last one
if inds(1) == inds(end)
    hull = hull(1:end-1, :);
end

% compute convex hull centroid, that corresponds to approximate
% location of rectangle center
center = mean(hull, 1);
hull = bsxfun(@minus, hull, center);

% number of hull vertices
nV = size(hull, 1);

% default values
rotatedAngle = 0;
minWidth = inf;
minAngle = 0;

% avoid degenerated cases
if nV < 3
    return;
end

% indices of vertices in extreme y directions
[tmp, indA] = min(hull(:, 2)); %#ok<ASGLU>
[tmp, indB] = max(hull(:, 2)); %#ok<ASGLU>

caliperA = [ 1 0];    % Caliper A points along the positive x-axis
caliperB = [-1 0];    % Caliper B points along the negative x-axis

%% Find the direction with minimum width (rotating caliper algorithm)

while rotatedAngle < pi
    % compute the direction vectors corresponding to each edge
    indA2 = mod(indA, nV) + 1;
    vectorA = hull(indA2, :) - hull(indA, :);

    indB2 = mod(indB, nV) + 1;
    vectorB = hull(indB2, :) - hull(indB, :);

    % Determine the angle between each caliper and the next adjacent edge
    % in the polygon
    angleA = vectorAngle(caliperA, vectorA);
    angleB = vectorAngle(caliperB, vectorB);

    % Determine the smallest of these angles
    angleIncrement = min(angleA, angleB);

    % Rotate the calipers by the smallest angle
    caliperA = rotateVector(caliperA, angleIncrement);
    caliperB = rotateVector(caliperB, angleIncrement);

    rotatedAngle = rotatedAngle + angleIncrement;

    % compute current width, and update opposite vertex
    if angleA < angleB
        line = createLine(hull(indA, :), hull(indA2, :));
        width = distancePointLine(hull(indB, :), line);
        indA = mod(indA, nV) + 1;

    else
        line = createLine(hull(indB, :), hull(indB2, :));
        width = distancePointLine(hull(indA, :), line);
        indB = mod(indB, nV) + 1;

    end

    % update minimum width and corresponding angle if needed
    if width < minWidth
        minWidth = width;
        minAngle = rotatedAngle;
    end
end

%% Compute box dimensions

% orientation of the main axis
theta = rad2deg(minAngle);

% pre-compute trigonometric functions
cot = cos(minAngle);
sit = sin(minAngle);

% elongation in direction of rectangle length
x = hull(:,1);
y = hull(:,2);
x2  =   x * cot + y * sit;
y2  = - x * sit + y * cot;

% compute extension along main directions
xmin = min(x2);    xmax = max(x2);
ymin = min(y2);    ymax = max(y2);

% position of the center with respect to the centroid compute before
dl = (xmax + xmin)/2;
dw = (ymax + ymin)/2;

% change  coordinate from rectangle to user-space
dx  = dl * cot - dw * sit;
dy  = dl * sit + dw * cot;

% coordinates of oriented box center
center = center + [dx dy];

% size of the rectangle
rectLength  = xmax - xmin;
rectWidth   = ymax - ymin;

% concatenate rectangle data
obox = [center rectLength rectWidth theta];


function test04 ( dim_num, n, z )

%*****************************************************************************80
%
%% TEST04 tests GAMMA_MEASURE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST04\n' );
  fprintf ( 1, '  GAMMA_MEASURE computes the GAMMA measure of quality.\n' );
  fprintf ( 1, '  The mesh ratio               Gamma = %14f\n', ...
    gamma_measure ( dim_num, n, z ) );

  return
end


function view = computeProbMap(view,scanList)
%
% view = computeProbMap(view,[scanList])
%
% Cycles through tSeries, computing the -log10 P values
% for each voxel. This is a first pass at a P map - not taking into account
% the non-gaussian properties of the spatio-temporal noise distribution.
%
% scanList:
%   0 - do all scans
%   number or list of numbers - do only those scans
%   default - prompt user via selectScans dialog
%
% Based on computeMeanMap
% and  Bantettini el al, 1993,
% Processing Strategies for Time Course Data Sets
% MRM 30:161-173 (1993) pp 171
%

nScans = numScans(view);
logProb=cell(1,nScans);
if strcmp(view.mapName,'logProbMap')
    % If exists, initialize to existing map
    map=view.map;
else
    % Otherwise, initialize to empty cell array
    map = cell(1,nScans);
end

% (Re-)set scanList
if ~exist('scanList','var')
    scanList = selectScans(view);
elseif scanList == 0
    scanList = 1:nScans;
end
if isempty(scanList)
    error('Analysis aborted');
end

% Compute it
waitHandle = mrvWaitbar(0,'Computing log10 P values from the tSeries.  Please wait...');
ncScans = length(scanList);
for iScan = 1:ncScans
    scan = scanList(iScan);
    nFrames=numFrames(view,scan);

    logProb{scanList(iScan)}=-(log10(computeCoherenceSignificance(view.co{scanList(iScan)},nFrames)));

    mrvWaitbar(scan/ncScans)
end
close(waitHandle);

% Set parameter map
view = setParameterMap(view,logProb,'log10Prob');

% Save file
saveParameterMap(view);


randinit
clf
hold on
g = PGraph(3);

sim_time = 2;

xinit = [0, 0, 0];
plot(xinit(1), xinit(2), 'go');
rho = 1;

g.add_node(xinit);
t =2;

for k=1:500
    % Step 3
    % find random state x,y in [-10, 10], th in [0 2pi]
    x = (rand-0.5)*20;
    y = (rand-0.5)*20;
    theta = rand*2*pi;
    xrand = [x, y, theta]';
    xrand'

    % Step 4
    % find the existing node closest in state space
    dmin = Inf;
    for v=1:g.n
        xv = g.coord(v);
        d = sum( (xv(1:2)-xrand(1:2)).^2 ) + t*angdiff(xv(3)-xrand(3))^2;
        if d < dmin
            xnear = xv;
            vnear = v;
            dmin = d;
        end
    end

    % Step 5
    % figure how to drive the robot from xnear to xrand
    x0 = xnear';
    xg = xrand';
    %set_param('sl_movepoint2/Bicycle', 'x0', sprintf('[%f,%f,%f]', xnear) );
    %set_param('sl_movepoint2/Goal pose', 'Value', sprintf('trans2([%f,%f,0])', xrand(1:2)) );

    r = sim('sl_drivepose', sim_time);
    t = r.find('tout');
    y = r.find('yout');
    plot2(y)
    drawnow
    xnew = y(end,:)';
    plot2(xnew', 'go');

    % ensure that the path is collision free

    % Step 7,8
    % add xnew to the graph, with an edge to xnear
    g.add_node(xnew, vnear);
end

grid on
xyzlabel
zlabel('\theta');
axis([-10 10 -10 10])
iprint('rrt_paths');
view(0, 0);
iprint('rrt_paths_xt');


function LnX = coefficient(x,y,L)
% Lagrange form for the polynomial interpolation
% this function produces elements of the form y_k*L_k(x)
n = length(x);
Num = '';
Den = '';
syms X

for i = 1:n
    if i ~= L
    TempNum = strcat('(X-x(',num2str(i),')',')','*');
    Num = strcat(Num,TempNum);
    TempDen = strcat('(x(',num2str(L),')','-','x(',num2str(i),')',')','*');
    Den = strcat(Den,TempDen);
    end
end

Num(end) = []; Den(end) = [];
LnX = (eval(strcat('(',Num,')','/','(',Den,')')))*y(L);


function [au] = km2au(km)
% Convert length from kilometers to astronomical units.
% Chad A. Greene 2012
au = km*6.684587122671e-9;


function [dpixc, dveccr, dpixc_ind, blinkmat, N] = generatedata2(sizevec, psf, separ, maxphotvec, offset, Nt)
% generate 2 points with different shift

nx = sizevec(2)-sizevec(1);
ny = sizevec(4)-sizevec(3);
N = 2;

if size(maxphotvec,2)>size(maxphotvec,1); maxphotvec = maxphotvec'; end
maxphotmat = repmat(maxphotvec, 1,Nt);

blinkmat_equal = rand(N, Nt);
blinkmat = blinkmat_equal .* maxphotmat; %different intensities...
center = round([nx ny]/2);
centerim = pixelize(center, 1, sizevec, nx, ny, [],0);
dpixc_ind = newimar(2);
dpixc_ind{1} = clip(dip_image(conv2(centerim,psf,'same')), 0, Inf);
dpixc_ind{2} = clip(shift(dpixc_ind{1}, separ), 0, Inf);

dpixc_nonoise = array2im(dpixc_ind'*blinkmat + offset);

dpixc_dip = noise(dpixc_nonoise,'poisson');
dpixc = double(dpixc_dip);
dveccr = double(squeeze(reshape(dpixc, nx*ny, 1, Nt))); % vectors of resized images


function [A,B]= useconvhulln(Z2)

% This file is part of SOSTOOLS - Sum of Squares Toolbox ver 3.00.
%
% Copyright (C)2002, 2004, 2013  A. Papachristodoulou (1), J. Anderson (1),
%                                G. Valmorbida (1), S. Prajna (2),
%                                P. Seiler (3), P. A. Parrilo (4)
% (1) Department of Engineering Science, University of Oxford, Oxford, U.K.
% (2) Control and Dynamical Systems - California Institute of Technology,
%     Pasadena, CA 91125, USA.
% (3) Aerospace and Engineering Mechanics Department, University of
%     Minnesota, Minneapolis, MN 55455-0153, USA.
% (4) Laboratory for Information and Decision Systems, M.I.T.,
%     Massachusetts, MA 02139-4307
%
% Send bug reports and feedback to: sostools@cds.caltech.edu
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program. If not, see <http://www.gnu.org/licenses/>.

% AP Apr 03 02
% AP Feb 01 03

% First, find the convex hull of Z2

convh = convhulln(Z2);
facets = size(convh,1);
[nZ1,nZ2] = size(Z2);

% Form the hyperplanes
% A hyperplane of the form [x1 ... xn -ones][A';b'] = 0
% [A';b'] is in the nullspace of the matrix [x1,...,xn -ones].
% Ignore it if it has dimension greater than 1
% Normalise if possible.

for i = 1:facets
    vert = [Z2(convh(i,:),:) -ones(nZ2,1)];
    nullvert = null(vert,'r');
    if size(nullvert,2) == 1
        if nullvert(end) ~=0
            nullvert = nullvert./nullvert(end);
        end
        coeff(:,i) = nullvert(1:end-1);
        cold(i,1) = nullvert(end);
    end
end

coeff = coeff';

% Condition the matrix a bit

coeffcold = [coeff cold];
coeffcold = round(coeffcold*100000)/100000;

% Discard same hyperplanes (due to convhulln result)
[coeff2cold,Ix,Iy] = unique(coeffcold,'rows');

%Remove a possible zero row

yind = find(sum(coeff2cold.^2,2) == 0);
if ~isempty(yind)
    Ix = [Ix([1:yind-1],1);Ix(yind+1:end,1)];
end

coeff2 = coeff(Ix,:);
convhnew = convh(Ix,:);
cnew = cold(Ix);
facetsnew = size(convhnew,1);

% Make inequalities out of them by testing a point not on the hyperplane
% Notation: convex hull is now Ax-b<=0
for fac = 1:facetsnew
    for ind = 1:nZ1
        matr = find(convhnew(fac,:) - ind*ones(1,nZ2) == 0);
        tests(fac) = coeff2(fac,:)*Z2(ind,:)'-cnew(fac);
        if isempty(matr) & abs(tests(fac)) > 1e-8
            break
        end
    end
    if tests(fac)>0
        coeff2(fac,:)=-coeff2(fac,:);
        cnew(fac)=-cnew(fac);
    end
end

A = coeff2;
B = cnew;


% Implementation of the MEM-EKF* algorithm based on the article
%
% "Tracking the Orientation and Axes Lengths of an Elliptical Extended Object"
% Shishan Yang and Marcus Baum
% arXiv preprint, 2018,
% https://arxiv.org/abs/1805.03276
%
% Further information:
% http://www.fusion.informatik.uni-goettingen.de
% https://github.com/Fusion-Goettingen
%
% Source code written by Shishan Yang
% =============================

function [r,p,Cr,Cp]= time_update(r,p,Cr,Cp,Ar, Ap,Cwr, Cwp)
r = Ar*r;
Cr = Ar*Cr*Ar'+Cwr;

p = Ap*p;
Cp = Ap*Cp*Ap'+Cwp;
end


function [M, RHS, Mx, My, Mz, RHSx, RHSy, RHSz] = ...
    convectionTvdTermCylindrical3D(u, phi, FL)
% This function uses the TVD scheme to discretize a 3D
% convection term in the form \grad (u \phi) where u is a face vactor
% It also returns the x and y parts of the matrix of coefficient.
%
% SYNOPSIS:
%
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% extract data from the mesh structure
Nr = u.domain.dims(1);
Ntetta = u.domain.dims(2);
Nz = u.domain.dims(3);
G=reshape((1:(Nr+2)*(Ntetta+2)*(Nz+2)), Nr+2, Ntetta+2, Nz+2);
DRp = repmat(u.domain.cellsize.x(2:end-1), 1, Ntetta, Nz);
DTHETAp = repmat(u.domain.cellsize.y(2:end-1)', Nr, 1, Nz);
DZ = zeros(1,1,Nz+2);
DZ(1,1,:) = u.domain.cellsize.z;
DZp=repmat(DZ(1,1,2:end-1), Nr, Ntetta, 1);
rp = repmat(u.domain.cellcenters.x, 1, Ntetta, Nz);
rf = repmat(u.domain.facecenters.x, 1, Ntetta, Nz);
dx=repmat(0.5*(u.domain.cellsize.x(1:end-1)+u.domain.cellsize.x(2:end)), 1, Ntetta, Nz);
dy=repmat(0.5*(u.domain.cellsize.y(1:end-1)+u.domain.cellsize.y(2:end))', Nr, 1, Nz);
dz=zeros(1, 1, Nz+1);
dz(1,1,:)=0.5*(u.domain.cellsize.z(1:end-1)+u.domain.cellsize.z(2:end));
dz=repmat(dz, Nr, Ntetta, 1);
psiX_p = zeros(Nr+1,Ntetta,Nz);
psiX_m = zeros(Nr+1,Ntetta,Nz);
psiY_p = zeros(Nr,Ntetta+1,Nz);
psiY_m = zeros(Nr,Ntetta+1,Nz);
psiZ_p = zeros(Nr,Ntetta,Nz+1);
psiZ_m = zeros(Nr,Ntetta,Nz+1);

% define the vectors to stores the sparse matrix data
iix = zeros(3*(Nr+2)*(Ntetta+2)*(Nz+2),1);
jjx = zeros(3*(Nr+2)*(Ntetta+2)*(Nz+2),1);
sx = zeros(3*(Nr+2)*(Ntetta+2)*(Nz+2),1);
iiy = zeros(3*(Nr+2)*(Ntetta+2)*(Nz+2),1);
jjy = zeros(3*(Nr+2)*(Ntetta+2)*(Nz+2),1);
sy = zeros(3*(Nr+2)*(Ntetta+2)*(Nz+2),1);
iiz = zeros(3*(Nr+2)*(Ntetta+2)*(Nz+2),1);
jjz = zeros(3*(Nr+2)*(Ntetta+2)*(Nz+2),1);
sz = zeros(3*(Nr+2)*(Ntetta+2)*(Nz+2),1);
mnx = Nr*Ntetta*Nz;	mny = Nr*Ntetta*Nz;   mnz = Nr*Ntetta*Nz;

% extract the velocity data
% note: size(ux) = [1:m+1, 1:n] and size(uy) = [1:m, 1:n+1]
ux = u.xvalue;
uy = u.yvalue;
uz = u.zvalue;

% calculate the upstream to downstream gradient ratios for u>0 (+ ratio)
% x direction
dphiX_p = (phi.value(2:Nr+2, 2:Ntetta+1, 2:Nz+1)-phi.value(1:Nr+1, 2:Ntetta+1, 2:Nz+1))./dx;
rX_p = dphiX_p(1:end-1,:,:)./fsign(dphiX_p(2:end,:,:));
psiX_p(2:Nr+1,:,:) = 0.5*FL(rX_p).* ...
    (phi.value(3:Nr+2,2:Ntetta+1,2:Nz+1)-phi.value(2:Nr+1,2:Ntetta+1,2:Nz+1));
psiX_p(1,:,:) = 0; % left boundary
% y direction
dphiY_p = (phi.value(2:Nr+1, 2:Ntetta+2, 2:Nz+1)-phi.value(2:Nr+1, 1:Ntetta+1, 2:Nz+1))./dy;
rY_p = dphiY_p(:,1:end-1,:)./fsign(dphiY_p(:,2:end,:));
psiY_p(:,2:Ntetta+1,:) = 0.5*FL(rY_p).* ...
    (phi.value(2:Nr+1,3:Ntetta+2,2:Nz+1)-phi.value(2:Nr+1, 2:Ntetta+1,2:Nz+1));
psiY_p(:,1,:) = 0; % Bottom boundary
% z direction
dphiZ_p = (phi.value(2:Nr+1, 2:Ntetta+1, 2:Nz+2)-phi.value(2:Nr+1, 2:Ntetta+1, 1:Nz+1))./dz;
rZ_p = dphiZ_p(:,:,1:end-1)./fsign(dphiZ_p(:,:,2:end));
psiZ_p(:,:,2:Nz+1) = 0.5*FL(rZ_p).* ...
    (phi.value(2:Nr+1,2:Ntetta+1,3:Nz+2)-phi.value(2:Nr+1,2:Ntetta+1,2:Nz+1));
psiZ_p(:,:,1) = 0; % Back boundary

% calculate the upstream to downstream gradient ratios for u<0 (- ratio)
% x direction
rX_m = dphiX_p(2:end,:,:)./fsign(dphiX_p(1:end-1,:,:));
psiX_m(1:Nr,:,:) = 0.5*FL(rX_m).* ...
    (phi.value(1:Nr, 2:Ntetta+1, 2:Nz+1)-phi.value(2:Nr+1, 2:Ntetta+1, 2:Nz+1));
psiX_m(Nr+1,:,:) = 0; % right boundary
% y direction
rY_m = dphiY_p(:,2:end,:)./fsign(dphiY_p(:,1:end-1,:));
psiY_m(:,1:Ntetta,:) = 0.5*FL(rY_m).* ...
    (phi.value(2:Nr+1,1:Ntetta,2:Nz+1)-phi.value(2:Nr+1,2:Ntetta+1,2:Nz+1));
psiY_m(:,Ntetta+1,:) = 0; % top boundary
% z direction
rZ_m = dphiZ_p(:,:,2:end)./fsign(dphiZ_p(:,:,1:end-1));
psiZ_m(:,:,1:Nz) = 0.5*FL(rZ_m).* ...
    (phi.value(2:Nr+1,2:Ntetta+1,1:Nz)-phi.value(2:Nr+1,2:Ntetta+1,2:Nz+1));
psiZ_m(:,:,Nz+1) = 0; % front boundary
% reassign the east, west, north, and south velocity vectors for the
% code readability
ue = ux(2:Nr+1,:,:);		uw = ux(1:Nr,:,:);
vn = uy(:,2:Ntetta+1,:);     vs = uy(:,1:Ntetta,:);
wf = uz(:,:,2:Nz+1);     wb = uz(:,:,1:Nz);
re = rf(2:Nr+1,:,:);         rw = rf(1:Nr,:,:);

% find the velocity direction for the upwind scheme
ue_min = min(ue,0);	ue_max = max(ue,0);
uw_min = min(uw,0);	uw_max = max(uw,0);
vn_min = min(vn,0);	vn_max = max(vn,0);
vs_min = min(vs,0);	vs_max = max(vs,0);
wf_min = min(wf,0);	wf_max = max(wf,0);
wb_min = min(wb,0);	wb_max = max(wb,0);

% calculate the coefficients for the internal cells
AE = re.*ue_min./(DRp.*rp);
AW = -rw.*uw_max./(DRp.*rp);
AN = vn_min./(DTHETAp.*rp);
AS = -vs_max./(DTHETAp.*rp);
AF = wf_min./DZp;
AB = -wb_max./DZp;
APx = (re.*ue_max-rw.*uw_min)./(DRp.*rp);
APy = (vn_max-vs_min)./(DTHETAp.*rp);
APz = (wf_max-wb_min)./DZp;

% Also correct for the boundary cells (not the ghost cells)
% Left boundary:
APx(1,:,:) = APx(1,:,:)-rw(1,:,:).*uw_max(1,:,:)./(2*rp(1,:,:)*DRp(1));   AW(1,:,:) = AW(1,:,:)/2;
% Right boundary:
AE(end,:,:) = AE(end,:,:)/2;    APx(end,:,:) = APx(end,:,:)+re(end,:,:).*ue_min(end,:,:)./(2*DRp(end)*rp(end,:,:));
% Bottom boundary:
APy(:,1,:) = APy(:,1,:)-vs_max(:,1,:)./(2*DTHETAp(1)*rp(:,1,:));   AS(:,1,:) = AS(:,1,:)/2;
% Top boundary:
AN(:,end,:) = AN(:,end,:)/2;    APy(:,end,:) = APy(:,end,:)+vn_min(:,end,:)./(2*DTHETAp(end)*rp(:,end,:));
% Back boundary:
APz(:,:,1) = APz(:,:,1)-wb_max(:,:,1)/(2*DZp(1));   AB(:,:,1) = AB(:,:,1)/2;
% Front boundary:
AF(:,:,end) = AF(:,:,end)/2;    APz(:,:,end) = APz(:,:,end) + wf_min(:,:,end)/(2*DZp(end));

AE = reshape(AE,mnx,1);
AW = reshape(AW,mnx,1);
AN = reshape(AN,mny,1);
AS = reshape(AS,mny,1);
AF = reshape(AF,mnz,1);
AB = reshape(AB,mnz,1);
APx = reshape(APx,mnx,1);
APy = reshape(APy,mny,1);
APz = reshape(APz,mnz,1);

% build the sparse matrix based on the numbering system
rowx_index = reshape(G(2:Nr+1,2:Ntetta+1,2:Nz+1),mnx,1); % main diagonal x
iix(1:3*mnx) = repmat(rowx_index,3,1);
rowy_index = reshape(G(2:Nr+1,2:Ntetta+1,2:Nz+1),mny,1); % main diagonal y
iiy(1:3*mny) = repmat(rowy_index,3,1);
rowz_index = reshape(G(2:Nr+1,2:Ntetta+1,2:Nz+1),mnz,1); % main diagonal z
iiz(1:3*mnz) = repmat(rowz_index,3,1);
jjx(1:3*mnx) = [reshape(G(1:Nr,2:Ntetta+1,2:Nz+1),mnx,1); reshape(G(2:Nr+1,2:Ntetta+1,2:Nz+1),mnx,1); reshape(G(3:Nr+2,2:Ntetta+1,2:Nz+1),mnx,1)];
jjy(1:3*mny) = [reshape(G(2:Nr+1,1:Ntetta,2:Nz+1),mny,1); reshape(G(2:Nr+1,2:Ntetta+1,2:Nz+1),mny,1); reshape(G(2:Nr+1,3:Ntetta+2,2:Nz+1),mny,1)];
jjz(1:3*mnz) = [reshape(G(2:Nr+1,2:Ntetta+1,1:Nz),mnz,1); reshape(G(2:Nr+1,2:Ntetta+1,2:Nz+1),mnz,1); reshape(G(2:Nr+1,2:Ntetta+1,3:Nz+2),mnz,1)];
sx(1:3*mnx) = [AW; APx; AE];
sy(1:3*mny) = [AS; APy; AN];
sz(1:3*mnz) = [AB; APz; AF];

% calculate the TVD correction term
div_x = -(1./(DRp.*rp)).*(re.*(ue_max.*psiX_p(2:Nr+1,:,:)+ue_min.*psiX_m(2:Nr+1,:,:))- ...
              rw.*(uw_max.*psiX_p(1:Nr,:,:)+uw_min.*psiX_m(1:Nr,:,:)));
div_y = -(1./(DTHETAp.*rp)).*((vn_max.*psiY_p(:,2:Ntetta+1,:)+vn_min.*psiY_m(:,2:Ntetta+1,:))- ...
              (vs_max.*psiY_p(:,1:Ntetta,:)+vs_min.*psiY_m(:,1:Ntetta,:)));
div_z = -(1./DZp).*((wf_max.*psiZ_p(:,:,2:Nz+1)+wf_min.*psiZ_m(:,:,2:Nz+1))- ...
              (wb_max.*psiZ_p(:,:,1:Nz)+wb_min.*psiZ_m(:,:,1:Nz)));

% define the RHS Vector
RHS = zeros((Nr+2)*(Ntetta+2)*(Nz+2),1);
RHSx = zeros((Nr+2)*(Ntetta+2)*(Nz+2),1);
RHSy = zeros((Nr+2)*(Ntetta+2)*(Nz+2),1);
RHSz = zeros((Nr+2)*(Ntetta+2)*(Nz+2),1);

% assign the values of the RHS vector
row_index = rowx_index;
RHS(row_index) = reshape(div_x+div_y+div_z,Nr*Ntetta*Nz,1);
RHSx(rowx_index) = reshape(div_x,Nr*Ntetta*Nz,1);
RHSy(rowy_index) = reshape(div_y,Nr*Ntetta*Nz,1);
RHSz(rowz_index) = reshape(div_z,Nr*Ntetta*Nz,1);

% build the sparse matrix
kx = 3*mnx;
ky = 3*mny;
kz = 3*mnz;
Mx = sparse(iix(1:kx), jjx(1:kx), sx(1:kx), (Nr+2)*(Ntetta+2)*(Nz+2), (Nr+2)*(Ntetta+2)*(Nz+2));
My = sparse(iiy(1:ky), jjy(1:ky), sy(1:ky), (Nr+2)*(Ntetta+2)*(Nz+2), (Nr+2)*(Ntetta+2)*(Nz+2));
Mz = sparse(iiz(1:kz), jjz(1:kz), sz(1:kz), (Nr+2)*(Ntetta+2)*(Nz+2), (Nr+2)*(Ntetta+2)*(Nz+2));
M = Mx + My + Mz;

end

function phi_out = fsign(phi_in)
% This function checks the value of phi_in and assigns an eps value to the
% elements that are less than or equal to zero, while keeping the signs of
% the nonzero elements
    phi_out = (abs(phi_in)>=eps).*phi_in+eps*(phi_in==0)+eps*(abs(phi_in)<eps).*sign(phi_in);
end


function p=lin2pcmu(x,s)
%LIN2PCMU Convert linear to Mu-law PCM P=(X,S)
%	pcmu = lin2pcmu(lin) where lin contains a vector
%	or matrix of signal values within a range determined by
%	the scale factor s (see table below).
%	Values outside this range will be clipped.
%	The input values will be converted to integer
%	Mu-law pcm vlues in the range 0 to 255.
%
%	Input values are multiplied by the scale factor s:
%
%		   s		Input Range
%
%		   1		+-8159
%		4004.189931	+-2.03761563 (default)
%		8159		+-1
%
%	The default input scaling factor 4004.189931 is equal to
%	sqrt((2207^2 + 5215^2)/2) and follows ITU standard G.711.
%	The sine wave with PCM-Mu values [158 139 139 158 30 11 11 30]
%	has a mean square value of unity corresponding to 0 dBm0.

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: lin2pcmu.m,v 1.4 2007/05/04 07:01:38 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin < 2 s=4004.189931; end
y=x*s;
y=(abs(y+8031)-abs(y-8031))/2;
q=floor((y+8032)/8032);
[m,e]=log2(abs(y)+33);
p=175+128*q-8*(e+abs(e-6))-floor(32*m-16);


function rule_num = triangle_ncc_rule_num ( )

%*****************************************************************************80
%
%% TRIANGLE_NCC_RULE_NUM returns the number of NCC rules available.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Peter Silvester,
%    Symmetric Quadrature Formulae for Simplexes,
%    Mathematics of Computation,
%    Volume 24, Number 109, January 1970, pages 95-100.
%
%  Parameters:
%
%    Output, integer RULE_NUM, the number of rules available.
%
  rule_num = 9;

  return
end


function [f,g] = call_log_reg(v, y, X, lambda)
% Primal objective function (Omega = inv(Sigma))
% f = 0.5*(logdet(V) - logdet(Sigma) - tr(V*SigmaInv) - (m-mu)'*SigmaInv*(m-mu)
%     + L) - sum_d fb(mbar_d, vbar_d)
% where mbar = X*m, vbar = diag(X*V*X')
%
% Written by Emtiyaz,
% Modified by Wu Lin
  global m_cache;
  global V_cache;
  global iter_counter;
  global is_cache;
  global times_cache;
  iter_counter = iter_counter + 1;

  [D L] = size(X);

  Omega = diag(lambda);
  %Extract mean, Cholesky and bias
  m = v(1:L);
  idx = L + [1:L*(L+1)/2];
  U = triu(unpackcovariance(v(idx),L));

  % compute V
  V = U'*U;

  if is_cache==1
      m_cache(:,iter_counter) = m;
      V_cache(:,:,iter_counter) = V;
  end

  % compute kl and its gradient
  kl = 0.5*(2*sum(log(diag(U))) + sum(log(lambda(2:end))) - trace(V*Omega) -m'*(Omega*m) + L);

  % contribution from the bound
  mbar = X*m;
  vbar = sum(X.*(V*X')',2); % diag(X*V*X') efficient
  [fb, gmb, gvb] = E_log_p('bernoulli_logit', y, mbar, vbar, []);
  fb = -fb;
  gm_lvb = X'*(-gmb);
  gV_lvb = X'*bsxfun(@times, -gvb, X); %efficient X'*diag(gllp/2)*X;

  % final
  f = kl - sum(fb);
  gm = - Omega*m - gm_lvb;
  gU = diag(1./diag(U)) - triu(U*Omega) - triu(2*U*gV_lvb);

  g=[gm(:); gU(triu(ones(L))==1)];

  % return
  g=-g;
  f=-f;

  if iter_counter+1<=length(times_cache)
      times_cache(iter_counter+1) = toc;
  else
      times_cache(end) = toc;
  end


clear all, close all, clc
addpath('./utils');
load allFaces.mat
X = faces(:,1:nfaces(1));
[L,S] = RPCA(X);

%%
inds = [3 4 14 15 17 18 19 20 21 32 43];
for k=[3 4 14 15 17 18 19 20 21 32 43]
    k
    subplot(2,2,1)
    imagesc(reshape(X(:,k),192,168)), colormap gray
    subplot(2,2,3)
    imagesc(reshape(L(:,k),192,168)), colormap gray
    subplot(2,2,4)
    imagesc(reshape(S(:,k),192,168)), colormap gray
    pause
end


function [Rlat,Rlon,alt] = LatLongCalcSingle_ADI(msg, inputLat, inputLong)
% Calculate latitude, longitude and altitude from message bits
% Copyright 2010, The MathWorks, Inc.

persistent NL
persistent latzones
persistent Dlat0
persistent Dlat1
persistent latOffset0
persistent latOffset1

if isempty(NL)
    Dlat0 = 360/(4*15-0);
    latOffset0 = floor(inputLat/Dlat0);
    Dlat1 = 360/(4*15-1);
    latOffset1 = floor(inputLat/Dlat1);
    NL=2:59;
    latzones = [(180/pi)*acos(sqrt((1-cos(pi/2/15))./(1-cos(2*pi./NL)))) 0];
end

% Altitude calculation
q = msg(48);
if q == 0
    af = 100;
else
    af = 25;
end
altBits=[msg(41:47);msg(49:52)]';
alt = altBits*[1024;512;256;128;64;32;16;8;4;2;1]*af - 1000;

evenOdd1 = msg(54);
latBits = msg(55:71)';
longBits = msg(72:88)';
la1 = latBits*[65536;32768;16384;8192;4096;2048;1024;512;256;128;64;32;16;8;4;2;1];
lo1 = longBits*[65536;32768;16384;8192;4096;2048;1024;512;256;128;64;32;16;8;4;2;1];

% Technically you need both even and odd messages to calculate lat/long
% unambiguously. For this code, use a single message and then check to see
% if the lat/long values are reasonable. If not, change the lat/long base
% factors (LL.a1, LL.a2, etc.) and recompute.

% Latitude calculation
if evenOdd1 == 0
    Rlat = Dlat0*(latOffset0 + la1/131072);
else
    Rlat = Dlat1*(latOffset1 + la1/131072);
end

% Compare latitude to known location. If it's off by more than two degrees,
% use new base factors.
if Rlat > inputLat+2
    if strcmp(evenOdd1,'Even')
        Rlat = Dlat0*(latOffset0 - 1 + la1/131072);
    else
        Rlat = Dlat1*(latOffset1 - 1 + la1/131072);
    end
elseif Rlat < inputLat-2
    if strcmp(evenOdd1,'Even')
        Rlat = Dlat0*(latOffset0 + 1 + la1/131072);
    else
        Rlat = Dlat1*(latOffset1 + 1 + la1/131072);
    end
end

% Based on latitude, calculate longitude
NL0 = find(latzones<Rlat,1,'first');
ni0 = NL0;
ni1 = NL0 - 1;

Dlon0 = 360/ni0;
longOffset0 = floor(inputLong/Dlon0);
Dlon1 = 360/ni1;
longOffset1 = floor(inputLong/Dlon1);

if evenOdd1 == 0
    Rlon = Dlon0*(longOffset0 + lo1/131072);
else
    Rlon = Dlon1*(longOffset1 + lo1/131072);
end

% Compare longitude to known location. If it's off by more than two
% degrees, use new base factors.
if Rlon > inputLong+2
    if strcmp(evenOdd1,'Even')
        Rlon = Dlon0*(longOffset0 - 1 + lo1/131072);
    else
        Rlon = Dlon1*(longOffset1 - 1 + lo1/131072);
    end
elseif Rlon < inputLong-2
    if strcmp(evenOdd1,'Even')
        Rlon = Dlon0*(longOffset0 + 1 + lo1/131072);
    else
        Rlon = Dlon1*(longOffset1 + 1 + lo1/131072);
    end
end

% disp(sprintf('Plane is at altitude %d\nLatitude value: %d\nLongitude value: %d', alt, la1, lo1));

% GoogleMap(aircraftID, alt1, Rlat, Rlon)