close all;
clear all;
clc;

% Create the directory for storing results
spx.fs.ensure_dir('bin');

% set number
r = 1;
h = spx.data.motion.Hopkins155;
% pre-load all examples
h.load_all_examples();
examples = h.get_2_3_motions();
example = examples{r};
solver = @ssc_mc_omp;

Y = example.X;
% homogenize
Y = spx.la.affine.homogenize(Y);
Y = spx.norm.normalize_l2(Y);
Y = Y;

if true
    fprintf('\n\n\n Statistics for the pair of motions:\n\n');
    [M, S]  = size(Y);
    sizes = example.counts;
    angle_result = spx.cluster.subspace.nearest_same_subspace_neighbors_by_inner_product(Y, sizes);
    spx.cluster.subspace.print_nearest_neighbor_result(angle_result);

end

mf = spx.graphics.Figures;
if true
    % Ambient space dimension and number of data points
    [trial.M, trial.S] = size(Y);
    % Number of subspaces
    trial.K = example.num_motions;
    % maximum dimension for each subspace
    trial.D = 3;
    trial.cluster_sizes = example.counts;
    % Solve the sparse subspace clustering problem
    tstart = tic;
    try
        clustering_result = solver(Y, trial.D, trial.K);
    catch ME
        % we will move on to next one
        fprintf('Problem in processing this example. %s: %s\n', ME.identifier, ME.message);
        % move on to next example
        error('cannot continue.');
    end
    trial.elapsed_time = toc (tstart);
    trial.singular_values = clustering_result.singular_values;
    % graph connectivity
    trial.connectivity = clustering_result.connectivity;
    % estimated number of clusters
    trial.estimated_num_subspaces = clustering_result.num_clusters;
    % Time to compare the clustering
    cluster_labels = clustering_result.labels;
    true_labels = example.labels;
    comparsion_result = spx.cluster.clustering_error(cluster_labels, true_labels, trial.K);
    trial.clustering_error_perc = comparsion_result.error_perc;
    trial.clustering_acc_perc = 100 - comparsion_result.error_perc;
    % Compute the statistics related to subspace preservation
    spr_stats = spx.cluster.subspace.subspace_preservation_stats(clustering_result.Z, trial.cluster_sizes);
    trial.spr_error = spr_stats.spr_error;
    trial.spr_flag = spr_stats.spr_flag;
    trial.spr_perc = spr_stats.spr_perc;
    fprintf('\nclustering error: %0.2f %% , clustering accuracy: %0.2f %%,\n mean spr error: %0.2f preserving : %0.2f %%,\n connectivity: %0.2f, elapsed time: %0.2f sec', trial.clustering_error_perc, trial.clustering_acc_perc, spr_stats.spr_error, spr_stats.spr_perc, trial.connectivity, trial.elapsed_time);
    fprintf('\n\n');
    Z = abs(clustering_result.Z);
    s = spx.stats.format_descriptive_statistics(Z(:));
    fprintf(s);
    fprintf('\n');
    mf.new_figure('Representations');
    imshow(Z);
end


function pass = test_equiOption( pref )
% Test funqi in 2D

if ( nargin < 1 )
    pref = chebfunpref;
end
tol = 100*pref.cheb2Prefs.chebfun2eps;

% Canonical domain:
dom = [-1 1 -1 1];
f = @(x,y) cos(x+y);
x = linspace(dom(1),dom(2),100);
y = linspace(dom(3),dom(4),100);
[xx, yy] = meshgrid( x, y );
A = f(xx, yy) ;
g = chebfun2( A , dom, 'equi' );
h = chebfun2( f, dom);
pass(1) = norm( h - g ) < tol ;

% Rectangular domain:
dom = [-1 2 -2 1];
f = @(x,y) cos(x+y);
x = linspace(dom(1),dom(2),100);
y = linspace(dom(3),dom(4),100);
[xx, yy] = meshgrid( x, y );
A = f(xx, yy) ;
g = chebfun2( A , dom, 'equi' );
h = chebfun2( f, dom);
pass(2) = norm( h - g ) < tol ;

% Nonsymmetric function:
dom = [-1 2 -2 1];
f = @(x,y) cos(x+2*y);
x = linspace(dom(1),dom(2),100);
y = linspace(dom(3),dom(4),100);
[xx, yy] = meshgrid( x, y );
A = f(xx, yy) ;
g = chebfun2( A , dom, 'equi' );
h = chebfun2( f, dom);
pass(3) = norm( h - g ) < tol ;

% Small domain;
h = 1e-3;
dom = [1-h 1+h 1-2*h 1+2*h];
f = @(x,y) cos(x+2*y);
x = linspace(dom(1),dom(2),100);
y = linspace(dom(3),dom(4),100);
[xx, yy] = meshgrid( x, y );
A = f(xx, yy) ;
g = chebfun2( A , dom, 'equi' );
h = chebfun2( f, dom);
pass(4) = norm( h - g ) < tol ;

end


function [V,F,bi,bo] = annulus(s,r,varargin)
  % ANNULUS Construct a triangle mesh of a unit annulus.
  %
  % [V,F] = annulus(s,r)
  % [V,F] = annulus(s,r,'ParameterName',ParameterValue, ...)
  %
  % Inputs:
  %   s  number of samples on the inner ring
  %   r  radius of the inner ring
  %   Optional:
  %     'Flags'  followed by flags to pass to Triangle for meshing.
  %        {'-q30 -aX'} where X is squared boundary edge length
  %     'R'  followed by outer ring radius {1}
  % Outputs:
  %   V  #V by 2 list of mesh vertex positions
  %   F  #F by 3 list of triangle mesh indices
  %   bi  list of vertices on inner boundary
  %   bo  list of vertices on outer boundary
  %

  flags = nan;
  R = 1;
  params_to_variables = containers.Map( ...
    {'Flags','R'},{'flags','R'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end
  if isnan(flags)
    flags = sprintf('-q30 -a%0.17f',(2*pi*r/s)^2);
  end

  theta = linspace(0,2*pi,s+1)';theta = theta(1:end-1);
  Vr = r*[cos(theta) sin(theta)];
  Er = fliplr([1:size(Vr,1);2:size(Vr,1) 1]');
  theta = linspace(0,2*pi,ceil(R/r)*s+1)';theta = theta(1:end-1);
  VR = R*[cos(theta) sin(theta)];
  ER = [1:size(VR,1);2:size(VR,1) 1]';

  [V,F] = triangulate([Vr;VR],[Er;size(Vr,1)+ER],'Holes',[0 0],'Flags',flags);
  b = unique(outline(F));
  bi = intersect(find(normrow(V)< 0.5*(R+r)),b);
  bo = intersect(find(normrow(V)> 0.5*(R+r)),b);
end


function [S,t,f,Serr]=mtspecgramc(data,movingwin,params)
% Multi-taper time-frequency spectrum - continuous process
%
% Usage:
% [S,t,f,Serr]=mtspecgramc(data,movingwin,params)
% Input:
% Note units have to be consistent. Thus, if movingwin is in seconds, Fs
% has to be in Hz. see chronux.m for more information.
%       data        (in form samples x channels/trials) -- required
%       movingwin         (in the form [window winstep] i.e length of moving
%                                                 window and step size)
%                                                 Note that units here have
%                                                 to be consistent with
%                                                 units of Fs - required
%       params: structure with fields tapers, pad, Fs, fpass, err, trialave
%       - optional
%           tapers : precalculated tapers from dpss or in the one of the following
%                    forms:
%                   (1) A numeric vector [TW K] where TW is the
%                       time-bandwidth product and K is the number of
%                       tapers to be used (less than or equal to
%                       2TW-1).
%                   (2) A numeric vector [W T p] where W is the
%                       bandwidth, T is the duration of the data and p
%                       is an integer such that 2TW-p tapers are used. In
%                       this form there is no default i.e. to specify
%                       the bandwidth, you have to specify T and p as
%                       well. Note that the units of W and T have to be
%                       consistent: if W is in Hz, T must be in seconds
%                       and vice versa. Note that these units must also
%                       be consistent with the units of params.Fs: W can
%                       be in Hz if and only if params.Fs is in Hz.
%                       The default is to use form 1 with TW=3 and K=5
%                   Note that T has to be equal to movingwin(1).
%
%	        pad		    (padding factor for the FFT) - optional. Defaults to 0.
%			      	 e.g. For N = 500, if PAD = 0, we pad the FFT
%			      	 to 512 points; if PAD = 2, we pad the FFT
%			      	 to 2048 points, etc.
%           Fs   (sampling frequency) - optional. Default 1.
%           fpass    (frequency band to be used in the calculation in the form
%                                   [fmin fmax])- optional.
%                                   Default all frequencies between 0 and Fs/2
%           err  (error calculation [1 p] - Theoretical error bars; [2 p] - Jackknife error bars
%                                   [0 p] or 0 - no error bars) - optional. Default 0.
%           trialave (average over trials/channels when 1, don't average when 0) - optional. Default 0
% Output:
%       S       (spectrum in form time x frequency x channels/trials if trialave=0; in the form time x frequency if trialave=1)
%       t       (times)
%       f       (frequencies)
%       Serr    (error bars) only for err(1)>=1

if nargin < 2; error('Need data and window parameters'); end;
if nargin < 3; params=[]; end;

if length(params.tapers)==3 & movingwin(1)~=params.tapers(2);
    error('Duration of data in params.tapers is inconsistent with movingwin(1), modify params.tapers(2) to proceed')
end

[tapers,pad,Fs,fpass,err,trialave,params]=getparams(params);
if nargout > 3 && err(1)==0;
%   Cannot compute error bars with err(1)=0. change params and run again.
    error('When Serr is desired, err(1) has to be non-zero.');
end;

N=size(data,1);
Nwin=round(Fs*movingwin(1)); % number of samples in window
Nstep=round(movingwin(2)*Fs); % number of samples to step through
nfft=2^(nextpow2(Nwin)+pad);

[f,findx]=getfgrid(Fs,nfft,fpass);
tapers=dpsschk(tapers,Nwin,Fs); % check tapers
[NC C]=size(data); % size of data
[NK K]=size(tapers); % size of tapers

tapers=tapers(:,:,ones(1,C)); % add channel indices to tapers

if NK~=Nwin ; error('length of tapers is incompatible with length of data'); end;

winstart=1:Nstep:N-Nwin+1;
nw=length(winstart);

S = zeros(nw,length(f),C);

for n=1:nw;
    J=fft((permute(data(winstart(n):winstart(n)+Nwin-1,:,ones(1,K)),[1 3 2]) .* tapers)  ,nfft)/Fs;   % fft of projected data
    J=J(findx,:,:);
    S(n,:,:)=squeeze(mean(conj(J).*J,2));
end

if nargout==4;Serr=squeeze(Serr);end;
winmid=winstart+round(Nwin/2);
t=winmid/Fs;


function [F,stdF,results] = nips2011_experiment_mohsst5(model, testset, seed)

% Seed for the random number generator
if nargin < 3
  seed = 10;
end
fprintf('Seed: %g\n', seed);
rand('state', seed);
randn('state', seed);

% Load the data
data = mohsst5_loaddata();
sea = sum(~isnan(data.observations),2) > 0;
Y = data.observations(sea,:);
[M,N] = size(Y);

switch testset
 case 1 % randomly (uniformly) selected
  disp('Test set: Uniform');

  I = rand(size(Y)) < 0.2;
  Itest = I & ~isnan(Y);
  Itrain = ~I & ~isnan(Y);
  Ytest = Y;
  Ytrain = Y;
  Ytest(~Itest) = nan;
  Ytrain(~Itrain) = nan;

  test_percent = sum(Itest(:)) / sum(~isnan(Y(:)));
  fprintf('Size of the test set: %.1f percent\n', 100*test_percent);

% $$$   figure
% $$$   spy(Itest)
% $$$   figure
% $$$   spy(Itrain)
% $$$   figure
% $$$   spy(~isnan(Y));
% $$$   figure
% $$$   spy(Itrain&Itest)
% $$$   figure
% $$$   spy((Itrain|Itest)==~isnan(Y));
% $$$   return

  testset_string = 'uniform';

 case 2 % pattern from earliest years used for latest years
  disp('Test set: Pattern');

  Imv = isnan(Y);
  Imv(:,(20*12+1):end) = false;
  Itest = Imv(:,end:-1:1) & ~isnan(Y);
  Itrain = ~Imv(:,end:-1:1) & ~isnan(Y);

  Ytest = Y;
  Ytrain = Y;
  Ytest(~Itest) = nan;
  Ytrain(~Itrain) = nan;

  test_percent = sum(Itest(:)) / sum(~isnan(Y(:)));
  fprintf('Size of the test set: %.1f percent\n', 100*test_percent);

% $$$   figure
% $$$   spy(Itest)
% $$$   figure
% $$$   spy(Itrain)
% $$$   figure
% $$$   spy(~isnan(Y));
% $$$   figure
% $$$   spy(Itrain&Itest)
% $$$   figure
% $$$   spy((Itrain|Itest)==~isnan(Y));
% $$$   return

  testset_string = 'pattern';

 otherwise
  error('Unknown test set requested');
end

switch model

 case 1

  %
  % Local GP
  %

  %disp('Model: Local GP');
  disp('Model: Local GP with two covfuncs per domain');

  % Temporal covariance function
  d = abs(1-(1:length(data.time)));
% $$$   covfunc1 = gp_cov_toeplitz(gp_cov_pp(d,1));
  covfunc1 = gp_cov_toeplitz(gp_cov_sum(gp_cov_pp(d,1), ...
                                        gp_cov_scale(gp_cov_pp(d,1))));
% $$$   theta_temporal = [7];   % length scale
  theta_temporal = [7;   % length scale 1
                    1.0; % magnitude 2
                    3];  % length scale 2

  % Spatial covariance function
  [LON,LAT] = meshgrid(data.longitude,data.latitude);
  X = geographic_to_euclidean([LON(:)';LAT(:)']);

  % Use block-Toeplitz structure for the covariance function
  [lat,lon0] = meshgrid(data.latitude,data.longitude(1));
  X0 = geographic_to_euclidean([lon0(:)';lat(:)']);
  D = sqrt(sq_dist(X0,X));
% $$$   covfunc2 = gp_cov_toeplitz_block(gp_cov_pp(D,3));
  covfunc2 = gp_cov_toeplitz_block(gp_cov_sum(gp_cov_pp(D,3), ...
                                              gp_cov_scale(gp_cov_pp(D,3))));

  % Select sea areas
  covfunc2 = gp_cov_select(covfunc2, sea);

% $$$   theta_spatial = [3000];  % length scale
  theta_spatial = [3000;  % length scale 1
                   1.0;   % magnitude 2
                   2000]; % length scale 2

  % Initial guess for covariance parameters
  theta_init = [0.5; ...               % total magnitude
                theta_temporal(:); ... % temporal parameters
                theta_spatial(:); ...  % spatial parameters
                0.5]';                 % noise magnitude

  % Prior for the hyperparameters
  a = 1e-3;
  b = 1e-3;
  logprior_theta = @(theta) sum(gamma_logpdf(theta, a, b));
  dlogprior_theta = @(theta) gamma_dlogpdf(theta, a, b);

  % Noise scaling
  w = 1./sqrt(cosd(LAT));
  noise_scale = repmat(w(sea), [1,size(Y,2)]);

  % Inference
  N_samples = 1000;
  burnin = floor(N_samples/2);
  res = gp_kron(Ytrain, covfunc1, covfunc2, N_samples, theta_init, ...
                logprior_theta, dlogprior_theta, burnin, 'noise_scale', ...
                noise_scale);

  F = res.F;
  stdF = sqrt(res.FF - res.F.^2);
  results = res;

  %model_string = 'gpkron';
  model_string = 'gpkron-cov2';

 case 2

  %
  % VB PCA
  %

  disp('Model: VB PCA');

  D = 150;

  % PCA module for X (one constant component for modeling bias)
  prior.mu = [1; zeros(D-1,1)];
  prior.CovX = diag([1e-6; ones(D-1,1)]);
  X_module = factor_module_iid(D, N, 'prior', prior);

  % ARD module for W
  W_module = factor_module_ard(D, M);

  % Isotropic noise (precisions weighted proportionally to grid size)
  [LON,LAT] = meshgrid(data.longitude, data.latitude);
  weights = cosd(LAT(:));
  weights = weights(sea); %metoffice_remove_bins(weights,maskfile);
  weights = repmat(weights, [1, N]);
  noise_module = noise_module_isotropic(M, N, 1e-3, 1e-3, ...
                                        'init', 10, ...
                                        'weights', weights);

  % Run VB PCA
  Q = vbfa(D, Ytrain, W_module, X_module, noise_module, ...
           'maxiter', 500, ...
           'rotate', true);
% $$$            'autosavefile', filename, ...
% $$$            'autosave', [1 20:20:2000]);

  % Results
  F = Q.W'*Q.X;
  varF = reshape(Q.CovW, [D*D, M])' * reshape(Q.CovX, [D*D,N]);
  for m=1:M
    for n=1:N
      varF(m,n) = varF(m,n) + ...
          Q.W(:,m)'*Q.CovX(:,:,n)*Q.W(:,m) + ...
          Q.X(:,n)'*Q.CovW(:,:,m)*Q.X(:,n);
    end
  end
  stdF = sqrt(varF);
  results = Q;

  model_string = 'vbpca';

 otherwise
  error('Unknown model requested');
end

% Error measures
[LON,LAT] = meshgrid(data.longitude, data.latitude);
weights = cosd(LAT(:));
weights = weights(sea);
weights = repmat(weights, [1, N]);
rmse_train = rmsew(Y(Itrain)-F(Itrain),weights(Itrain));
rmse_test = rmsew(Y(Itest)-F(Itest),weights(Itest));
rmse_zero = rmsew(Y(Itest),weights(Itest));
fprintf('Training WRMSE=%.4f and testing WRMSE=%.4f\n', rmse_train, rmse_test);
fprintf('Testing WRMSE=%.4f with zero predictions\n', rmse_zero);

filename = sprintf(['/home/jluttine/matlab/publications/nips2011/' ...
                    'results_nips2011_mohsst5_%s_%s_%s'], model_string, ...
                   testset_string, datestr(now,'yyyymmdd'));

save(filename, 'F', 'stdF', 'results', 'Ytest', 'Ytrain');
fprintf('Saved results to %s\n', filename);

if nargout < 1
  clear F
  clear stdF
  clear results
end


function [A,b,Aeq,beq,lb,ub,report] = separateBounds( A,b,Aeq,beq,tol )
%separateBounds - given a quadruplet {A, b, Aeq, beq} of linear constraint
%matrices and vectors corresponding to the region A*x<=b, Aeq*x=beq, the routine
%will convert to a sextuplet {A, b, Aeq, beq, lb, ub} of the kind used
%in the Optimization Toolbox.
%
%      [A,b,Aeq,beq,lb,ub,report] = separateBounds( A,b,Aeq,beq )
%
%In other words, the code will look through the rows of A and Aeq for
%constraints corresponding to simple upper and lower bounds. It will then
%separate these constraints from the rest, expressing them instead using
%vectors lb, ub. The "report" output argument is a structure containing
%some relevnt stats,
%
%    report.infeasibleBounds: indices, i, of any lb(i)>ub(i)
%    report.inequalitiesRemoved: indices of rows removed from A,b
%    report.equalitiesRemoved: indices of rows removed from Aeq,beq
%
%EXAMPLE:
%
%     A =  [1     1     1
%          -1     0     0
%           0    -1     0
%           1     0     0
%           0     1     0] ;
%
%      b=[1 0 0 1 1].';
%
%     Aeq=[0 0 1];
%     beq=1;
%
%     >>[A,b,Aeq,beq,lb,ub] = separateBounds( A,b,Aeq,beq )
%
%         A =
%
%              1     1     1
%
%
%         b =
%
%              1
%
%
%         Aeq =
%
%            Empty matrix: 0-by-3
%
%
%         beq =
%
%            Empty matrix: 1-by-0
%
%
%         lb =
%
%              0
%              0
%              1
%
%
%         ub =
%
%              1
%              1
%              1
%
%By default, the code will consider a constraint A(i,:)<=b(i) to correspond
%to a pure bound if a row A(i,:) or similarly Aeq(i,:) contains no more
%than one non-zero element. In certain situations, however, the matrices A
%and Aeq are the output of floating point calculations and such rows will
%contain non-zero elements representing numerical noise. In such instances,
%one can call the code with a tolerance parameter
%
%      [A,b,Aeq,beq,lb,ub,report] = separateBounds( A,b,Aeq,beq,tol )
%
%where 0<=tol<=1. The criterion for deciding whether a constraint is a pure
%bound is then,
%
%        max(abs(A(:,i)))>=tol*sum(abs(A(i,:)))
%
%The default beavior corresponds to tol=1.

  %%%%begin parsing

          if ~exist('Aeq','var'), Aeq=[]; end
          if ~exist('beq','var'),  beq=[]; end
          if ~exist('tol','var')||isempty(tol),
              tol=1;
          elseif tol<0 || tol>1
              error 'Tolerance parameter must satisfy 0<=tol<=1';
          end

          Na=nan; Nb=nan;
          if xor(isempty(A),isempty(b))
            error 'If A is empty then b must also be empty and vice versa'
          elseif ~isempty(A)
              Na=size(A,2);
          elseif isempty(A)
              A=[];b=[];
          end

          if xor(isempty(Aeq),isempty(beq))
              error 'If Aeq is empty then beq must also be empty and vice versa'
          elseif ~isempty(Aeq)
              Nb=size(Aeq,2);
          elseif isempty(Aeq)
                Aeq=[]; beq=[];
          end

          if ~isnan(Na) && ~isnan(Nb)

              if Na~=Nb
               error 'If both A and Aeq are both non-empty, they must have same number of columns'
              end

          elseif isnan(Na) && isnan(Nb)

              lb=[]; ub=[];
              return

          end

          N=max(Na,Nb);

          b=b(:);  beq=beq(:); %henceforth we are sure b-data are column vectors

           if size(A,1)~=length(b)
               error 'Incompatible inequality matrix data sizes: size(A,1) ~= length(b)'
           end

           if size(Aeq,1)~=length(beq)
               error 'Incompatible equality matrix data sizes: size(Aeq,1) ~= length(beq)'
           end

  %%%%end parsing

   [ls1,lv1,us1,uv1,A1,b1,rows1] = extract(A,b,tol);
   [ls2,lv2,us2,uv2,A2,b2,rows2] = extract(Aeq,beq,tol);
   [ls3,lv3,us3,uv3] = deal(us2,uv2,ls2,lv2);

   A=A1;
   b=b1;
   Aeq=A2;
   beq=b2;
   lb=accumarray([ls1;ls2;ls3]  ,  [lv1;lv2;lv3]  , [N,1], @max , -inf);
   ub=accumarray([us1;us2;us3]  ,  [uv1;uv2;uv3]  , [N,1], @min , +inf);

   report.infeasibleBounds=find(ub<lb).';
   report.inequalitiesRemoved=rows1.';
   report.equalitiesRemoved=rows2.';

   if nargout<7 &&  ~isempty(report.infeasibleBounds)

       warnstr=[sprintf('Infeasible combinations of bounds ( LB(i)>UB(i) ) were detected.\n\n'),...
               sprintf( 'To disable this warning, call with 7 output argument syntax:\n\n'),...
               sprintf( '          [A,b,Aeq,beq,lb,ub,report] = separateBounds(...)')];

       warning('separateBounds:infeasible', warnstr);

   end

 function [lsubs,lvals,usubs,uvals,A,b,rows] = extract(A,b,tol)

      lsubs=[]; usubs=[];
      lvals=[];  uvals=[];
      rows=[];

     if isempty(A), return; end

    b=b(:);

    absA=abs(A);

    [maxA,idx]=max(absA,[],2);
    sumA=sum(absA,2);

    rows =find(  maxA>=tol*sumA & sumA>0 );

    subs=idx(rows);

      ind=sub2ind(size(A),rows, subs);
      num=b(rows);
      den=A(ind);

    vals=num./den;

    lidx=(den<0);
    uidx=(den>0);

    lsubs=subs(lidx);
    lvals=vals(lidx);

    usubs=subs(uidx);
    uvals=vals(uidx);

   zrows=(sumA==0);
   rowsInfeas=find(zrows & b<0);
   rowsFeas=find(zrows & b>=0);

   if ~isempty(rowsInfeas) %row of all zeros

       N=size(A,2);
       lsubs=(1:N).';
       lvals=inf(N,1);
       usubs=lsubs;
       uvals=-lvals;
       rows=[rows;rowsInfeas];
   end

   if ~isempty(rowsFeas) %row of all zeros
       rows=[rows;rowsFeas];
   end

    A(rows,:)=[];
    b(rows)=[];


function [params, s_new] = denoise_fica_tanh(params, s, state)
% FastICA tanh nonlinearity as DSS denoising function
%   [params, s_new] = denoise_fica_tanh(params, s, state)
%     params  Function specific modifiable parameters
%       a     Scaling constant (default: 1)
%     state   DSS algorithm state
%     s       Source signal estimate, matrix of row vector signals
%     s_new   Denoised signal estimate

% Copyright (C) 2004, 2005 DSS MATLAB package team (dss@cis.hut.fi).
% Distributed by Laboratory of Computer and Information Science,
% Helsinki University of Technology. http://www.cis.hut.fi/projects/dss/.
% $Id$

if nargin<3 | ~isstruct(state)
    params.name = 'FastICA tanh nonlinearity';
    params.description = '';
    params.param = {'a'};
    params.param_value ={1};
    params.param_type = {'scalar'};
    params.param_desc = {'Steepness modifier'};
    params.approach = {'defl','symm'};
    params.alpha = {};
    params.beta = {};
    return;
end

if ~isfield(params, 'a')
  params.a = 1;
end
a = params.a;

hypTan = tanh(a * s);
s_new = hypTan - a * repmat(mean(1 - hypTan .^ 2, 2), 1, length(s)) .* s;


function halton_test0125 ( )

%*****************************************************************************80
%
%% TEST0125 tests I4_TO_HALTON.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 January 2009
%
%  Author:
%
%    John Burkardt
%
  dim_max = 3;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST0125\n' );
  fprintf ( 1, '  I4_TO_HALTON computes a Halton sequence.\n' );
  fprintf ( 1, '  The user specifies all data explicitly.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  In this test, we call I4_TO_HALTON repeatedly.\n' );
  fprintf ( 1, '  We use distinct primes as bases.\n' );

  for dim_num = 1 : dim_max

    n = 11;
    step = 0;
    seed(1:dim_num) = 0;
    leap(1:dim_num) = 1;
    for j = 1 : dim_num
      base(j) = prime(j);
    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  DIM_NUM = %12d\n', dim_num );
    fprintf ( 1, '  N =    %12d\n', n );
    fprintf ( 1, '  STEP = %12d\n', step );
    i4vec_transpose_print ( dim_num, seed, '  SEED = ' );
    i4vec_transpose_print ( dim_num, leap, '  LEAP = ' );
    i4vec_transpose_print ( dim_num, base, '  BASE = ' );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  STEP      Halton\n' );
    fprintf ( 1, '\n' );
    for j = 1 : n
      step = j - 1;
      r = i4_to_halton ( dim_num, step, seed, leap, base );
      fprintf ( 1, '  %6d  ', step );
      for i = 1 : dim_num
        fprintf ( 1, '%8f  ', r(i) );
      end
      fprintf ( 1, '\n' );
    end

  end

  return
end


function [x,rec] = bsearch(fun,x0,opt,varargin)
% BSEARCH Finds the minimum of a combinatorial function using backward search
%
%   X = BSEARCH(FUN, X0) attempts to find a combination of elements
%   of X0 which locally minimize the function FUN using backward
%   search (backward elimination). FUN accepts input X and returns
%   scalar function value F evaluated at X. X0 must be a vector of
%   initial indexes. Returned X contains indexes locally minimizing
%   the function FUN.
%
%   X = BSEARCH(FUN, X0, OPTIONS) allows use of optional
%   search parameters. See BSEARCH_OPT for details.
%
%   X = BSEARCH(FUN, X0, OPTIONS, P1, ..., Pn) P1,...,Pn are
%   additional parameters passed to the function FUN.
%
%   [X,REC] = BSEARCH(FUN, X0, ...) returns record of search as a
%   array of structs.
%
%   In backward search the elements are removed one at the time.
%   The element ehich removal minimizes the function value is
%   removed at each round. In a case of tie choose randomly.
%
% Copyright (c) Aki Vehtari (2007)

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

% 2007-02-23  Aki Vehtari  <Aki.Vehtari@hut.fi>
%             Use same parameter order as in fminunc etc.
%             Minimize instead of maximize
%             Rewrite.

% Columns
nx = size(x0,2);  % number of elements
nremoved = 0;     % number of removed elements
% Options
opt = bsearch_opt(opt);

% Base value
value = feval(fun,x0,varargin{:});
if (opt.display >= 1)
  fprintf(' Base value: %.4g\n Elements: %s\n', value, num2str(x0));
end
minvalue=value;
rec(nremoved+1).chosen = x0;
rec(nremoved+1).candidates = [];
rec(nremoved+1).values = value;

% The loop
while (nx > opt.nsel)
  values=zeros(nx,1);
  for i1=1:nx
    values(i1) = feval(fun,setdiff(x0,x0(i1)),varargin{:});
    if (opt.display >= 2)
      fprintf('%d %.5g\n', x0(i1),  values(i1));
    end
  end
  % Let's get the index of the best column
  % In a case of tie, choose randomly
  value=min(values);
  mini=randpick(find(values==value));
  if (opt.stop && value>=minvalue)
    if opt.display
      fprintf('Stopping because value not decreasing\n');
    end
    break
  end
  nremoved = nremoved + 1;
  rec(nremoved+1).candidates = x0;
  rec(nremoved+1).values = values;
  x0 = setdiff(x0,x0(mini));       % Remove the element from x0
  nx=nx-1;
  rec(nremoved+1).chosen = x0;
  if value<minvalue
    minvalue=value;
    minvaluei=nremoved+1;
  end

  if (opt.display >= 1)
    fprintf(' Value: %.4g\n Chosen: %s\n', value, num2str(x0));
  end

  if (minvalue<opt.stopvalue)
    if opt.display
      fprintf('Objective function value smaller than specified goal\n');
    end
    break
  end
end
x=rec(minvaluei).chosen;


function SO3VF = approximation(nodes, values, varargin)
%
% Syntax
%   SO3VF = SO3VectorFieldHarmonic.approximation(nodes, values)
%   SO3VF = SO3VectorFieldHarmonic.approximation(nodes, values, 'bandwidth', bw)
%
% Input
%   nodes - @rotation
%   values - @vector3d
%
% Output
%   SO3VF - @SO3VectorFieldHarmonic
%
% Options
%   bandwidth - maximal degree of the Wigner-D functions (default: 128)
%

% TODO: This method uses the very expensive approximation method

SO3F = SO3FunHarmonic.approximation(nodes(:),values.xyz,varargin{:});

SO3VF = SO3VectorFieldHarmonic(SO3F);

end


function p=internalpoint(v,aloop)
%
% p=internalpoint(v,aloop)
%
% imperical function to find an internal point
% of a planar polygon
%
% author: Qianqian Fang (fangq<at> nmr.mgh.harvard.edu)
% date: 2008/04/08
%
% input:
%    v:     x,y,z coordinates of each node of the mesh
%    aloop:  input, a single vector separated by NaN, each segment
%             is a close-polygon consisted by node IDs
% output:
%    p:   output, [x y z] of an internal point of aloop
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

p=[];
nd=v(aloop,:);
boxfacet=find(sum(abs(diff(nd)))<1e-2); % find a flat loop
if(length(boxfacet))   % if the loop is flat along x/y/z dir
     bf=boxfacet(1);    % no degeneracy allowed
     idx=setdiff([1 2 3],bf);

     p0=(nd(1,:)+nd(2,:))/2;
     pvec=complex(p0(idx(1)),p0(idx(2)));
     vec=nd(2,:)-nd(1,:);
     vec=complex(vec(idx(1)),vec(idx(2)))*exp(i*pi/2)*(1e-5)/sqrt(sum(vec.*vec));
     testpt=[real(pvec+vec) imag(pvec+vec);real(pvec-vec) imag(pvec-vec)];
     in=inpolygon(testpt(:,1),testpt(:,2), nd(:,idx(1)),nd(:,idx(2)));
     p=testpt(find(in>0),:);
     p([bf,idx(1),idx(2)])=[nd(1,bf),p];
end

if(length(p)==0|length(p)==2)
    error('fail to find an internal point of curve');
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   demo script for surface repairing using surf2vol and remeshsurf
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% preparation
% user must add the path of iso2mesh to matlab path list
% addpath('../');

% user need to add the full path to .../iso2mesh/bin directory
% to windows/Linux/Unix PATH environment variable

%% load the sample data
load rat_head.mat

% volimage is a volumetric image such as an X-ray or MRI image
% A,b are registration matrix and vector, respectively
%% perform mesh generation

[node,face]=v2s(volimage,0.5,2,'cgalmesh');

node=node(:,1:3);
face=face(:,1:3);

plotmesh(node,face);
axis equal

[newno,newfc]=remeshsurf(node,face,1);

newno=sms(newno,newfc(:,1:3),3,0.5);

figure;
plotmesh(newno,newfc(:,1:3));
axis equal


function r = sqrt(a)
%SQRT         Hessian (elementwise) square root
%

% written  04/04/04     S.M. Rump
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  K = prod(size(a.x));
  if K==1                   % scalar hessian

    r.x = sqrt(a.x);
    rx1 = 0.5 / r.x;
    r.dx = a.dx * rx1;
    r.hx = ( a.hx - reshape( ((0.25/a.x)*a.dx) * a.dx.',size(a.hx)) ) * rx1;

  else                      % matrix hessian

    N = getappdata(0,'INTLAB_HESSIAN_NUMVAR');
    N2 = N^2;

    r.x = sqrt(a.x);
    if issparse(a.hx)               % input sparse

      ax = 0.5./full(r.x(:));
      sizeax = length(ax);
      [ia,ja,sa] = find(a.dx);
      % check for emptyness: cures Matlab bug
      % a=sparse([],[],[],2,1), [i,j,s]=find(a), s(i).*s(:)  yields error
      if isempty(ia)
        r.dx = sparse([],[],[],N,sizeax);
        r.hx = sparse([],[],[],N2,sizeax);
      else
        rdx = ax(ja).*sa(:);
        adx1 = (-0.25) ./ a.x(ja);
        if isa(a.x,'intval')          % sparse intval
          adx1 = times(adx1(:),rdx,0);
          if isreal(rdx)
            r.dx = intval( sparse(ia,ja,rdx.inf,N,sizeax) , sparse(ia,ja,rdx.sup,N,sizeax) , 'infsup' );
          else
            r.dx = intval( sparse(ia,ja,rdx.mid,N,sizeax) , sparse(ia,ja,rdx.rad,N,sizeax) , 'midrad' );
          end
          if adx1.complex
            adx1 = intval( sparse(ia,ja,adx1.mid,N,sizeax) , sparse(ia,ja,adx1.rad,N,sizeax) , 'midrad' );
          else
            adx1 = intval( sparse(ia,ja,adx1.inf,N,sizeax) , sparse(ia,ja,adx1.sup,N,sizeax) , 'infsup' );
          end
        else                          % sparse point
          r.dx = sparse(ia,ja,rdx,N,sizeax);
          adx1 = sparse(ia,ja,adx1(:).*rdx,N,sizeax);
        end
        r.hx = adx2rhx(N,sizeax,adx1,a.dx);
      end
      [ia,ja,sa] = find(a.hx);      % sparse point or intval
      % check for emptyness: cures Matlab bug
      % a=sparse([],[],[],2,1), [i,j,s]=find(a), s(i).*s(:)  yields error
      if ~isempty(ia)
        if isa(a.x,'intval')
          rhx = times(ax(ja),sa(:),0);
          if rhx.complex
            r.hx = r.hx + intval( sparse(ia,ja,rhx.mid,N2,sizeax) , sparse(ia,ja,rhx.rad,N2,sizeax) , 'midrad' );
          else
            r.hx = r.hx + intval( sparse(ia,ja,rhx.inf,N2,sizeax) , sparse(ia,ja,rhx.sup,N2,sizeax) , 'infsup' );
          end
        else
          r.hx = r.hx + sparse(ia,ja,ax(ja).*sa(:),N2,sizeax);
        end
      end

    else                            % input full

      r.x = sqrt(a.x);
      rx1 = 0.5 ./ ( r.x(:).' );
      rx1 = rx1(ones(N*N,1),:);
      r.dx = a.dx .* rx1(1:N,:);
      adx = repmat(0.25./(a.x(:).'),N,1) .* a.dx;
      r.hx = ( a.hx - adx(repmat(1:N,N,1),:) .* a.dx(repmat(1:N,1,N),:) ) .* rx1;

    end

  end

  r = class(r,'hessian');

  if rndold
    setround(rndold)
  end


function axisenlarge(f,h)
%AXISENLARGE - enlarge the axes of a figure (f,h)
%
% Usage:  (1) axisenlarge(1.05)    % enlarge axes by 5% in each direction
%         (2) axisenlarge(-1.05)   % shrink to fit content before enlarging
%
% Inputs:
%    f      enlarge axis by a factor f relative to current size or
%           by -f relative to the graph content. For separate factors
%           in each direction use [fx fy fz] or [fleft fright fbottom ftop fdown fup]
%    h      axis handle [default = gca]

%	   Copyright (C) Mike Brookes 2012
%      Version: $Id: axisenlarge.m 10428 2018-03-08 07:33:39Z 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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fpt=[1 1 1 1 1 1; 1 1 2 2 2 2; 1 1 2 2 3 3; 1 2 3 4 3 4; 1 2 3 4 5 5; 1 2 3 4 5 6];
if nargin<2 || ~numel(h)
    h=gca;
end
if nargin<1 || ~numel(f)
    f=-1.02;
end
nf=min(numel(f),6);
f=f(fpt(nf,:));  % expand f to dimension 4
if any(f>=0)
    ax0=[get(h,'XLim') get(h,'YLim') get(h,'ZLim')];
else
    ax0=zeros(1,6);
end
if any(f<0)
    axis(h,'tight');
    ax1=[get(h,'XLim') get(h,'YLim')  get(h,'ZLim')];
    ax0(f<0)=ax1(f<0);
    f=abs(f);
end
ax1=ax0.*f+ax0([2 1 4 3 6 5]).*(1-f);
set(h,'XLim',ax1(1:2),'YLim',ax1(3:4),'ZLim',ax1(5:6));


function newquiver(X,Y,U,V,varargin)
% NEWQUIVER - Plot velocity vectors as arrows
%
% Syntax:
%   NEWQUIVER(X,Y,U,V,options)
%
% In:
%   X       - Grid of x coordinates
%   Y       - Grid of y coordinates
%   U       - Vector direction (x component)
%   V       - Vector direction (y component)
%   options - name-value pairs (see below)
%
% Out:
%   (none)
%
% Description:
%   NEWQUIVER(X,Y,U,V) plots velocity vectors as arrows with components
%   (u,v) at the points (x,y).  The matrices X,Y,U,V must all be the same
%   size and contain corresponding position and velocity components
%   (X and Y can also be vectors to specify a uniform grid).
%   NEWQUIVER automatically scales the arrows to fit within the grid.
%
%   Additional option-value pairs can be given.
%   Accepted options/values are:
%    * scale     : A two-element vector for scaling the shape
%    * color     : Fill color (rgbkcy...) (default 'k': black)
%    * EdgeColor : Edge color of shape  (default 'k': black)
%    * LineWidth : Line width (default 0.5)
%    * X         : Arrow shape as a two-column vector (default: arrow)
%
% See also:
%    quiver
%
% Copyright:
%   2012-2018 - Simo Särkkä and Arno Solin
%
% License:
%   This software is provided under the MIT License. See the accompanying
%   LICENSE file for details.

%% Define arrow shape

  % Define arrow shape
  arrow.X = [8.7185878,  4.0337352 % Upper right corner
            -2.2072895,  0         % Arrow tip
             8.7185878, -4.0337352 % Lower right corner
             6.9831476, -1.6157441 % Inner lower corner
             6.9831476, -0.8       % Inner lower shaft start
             28,        -0.8       % Lower shaft end
             28,         0.8       % Upper shaft end
             6.9831476,  0.8       % Inner upper shaft start
             6.9831476,  1.6157441 % Inner upper corner
             8.7185878,  4.0337352]; % Upper right corner

   % Flip arrow left to right
   arrow.X(:,1) = -arrow.X(:,1);

   % Normalize length to one and center
   arrow.X(:,1) = arrow.X(:,1)-min(arrow.X(:,1));
   arrow.X = arrow.X/max(arrow.X(:,1));
   arrow.X(:,1) = arrow.X(:,1)-.5;

%% Set default options

  % Default options structure
  defaultopt.scale       = .5*[min(X(X(:)>min(X(:))))-min(X(:)) ...
                               min(Y(Y(:)>min(Y(:))))-min(Y(:))];
  defaultopt.color       = 'k';
  defaultopt.EdgeColor   = 'k';
  defaultopt.LineWidth   = .5;
  defaultopt.X           = arrow.X;

%% Get options

  % If there are options submitted
  if length(varargin) > 1

    % Options
    options = cell2struct(varargin(2:2:end),varargin(1:2:end),2);

    % Merge the default and submitted options
    fnames = fieldnames(defaultopt);
    for i=1:length(fnames)
      %val = getfield(options,fnames{i});
      if ~isfield(options,fnames{i})
        options=setfield(options,fnames{i},getfield(defaultopt,fnames{i}));
      end
    end
  else
    options = defaultopt;
  end

  % Make sure there is a scaling factor in both directions
  if numel(options.scale)~=2, options.scale = [1 1]*options.scale(1); end

%% Get arrow directions

  % Calculate arrow direction
  TH = atan2(V,U);

  hold on

  for i=1:numel(X)

    % Roatate
    Z = [cos(TH(i)) -sin(TH(i)); sin(TH(i)) cos(TH(i))]*options.X';

    % Scale
    Z = bsxfun(@times,Z,options.scale(:));

    % Move
    Z = bsxfun(@plus,Z,[X(i); Y(i)]);

    % Plot
    patch(Z(1,:),Z(2,:),1,'FaceColor',options.color, ...
        'EdgeColor',options.EdgeColor, ...
        'LineWidth',options.LineWidth,'LineStyle','-')

  end


function [ x, more ] = sgmga_vcn_naive ( dim_num, level_weight, x_max, x, ...
  q_min, q_max, more )

%*****************************************************************************80
%
%% SGMGA_VCN_NAIVE returns the next constrained vector.
%
%  Discussion:
%
%    This function uses a naive algorithm, which quickly becomes unsuitable
%    for higher dimensions.  The function SGMGA_VCN is an attempt at a more
%    efficient calculation of the same quantities.
%
%    We consider vectors X of dimension DIM_NUM satisfying:
%
%      0 <= X(1:DIM_NUM) <= X_MAX(1:DIM_NUM).
%
%    and define
%
%      Q = sum ( 1 <= I <= DIM_NUM ) LEVEL_WEIGHT(I) * X(I)
%
%    and seek X satisfying the constraint:
%
%      Q_MIN < Q <= Q_MAX
%
%    For sparse grid applications, we compute
%
%      LEVEL_WEIGHT_MIN_POS = minimum positive entry in LEVEL_WEIGHT
%
%    and assume there is an underlying LEVEL used to index the sets of
%    constrained vectors, and that
%
%      Q_MAX = LEVEL * LEVEL_WEIGHT_MIN_POS
%      Q_MIN = LEVEL - LEVEL_WEIGHT_MIN_POS * sum ( LEVEL_WEIGHT(:) )
%      X_MAX(I) = LEVEL * LEVEL_WEIGHT_MIN_POS / LEVEL_WEIGHT(I)
%
%    This routine returns, one at a time exactly those X which satisfy
%    the constraint.  No attempt is made to return the X values in
%    any particular order as far as Q goes.
%
%  Example:
%
%    LEVEL_WEIGHT:          1.000000        1.000000
%
%    Q_MIN:        0.000000
%    Q_MAX:        2.000000
%    X_MAX:                         2         2
%
%         1        1.000000         1         0
%         2        2.000000         2         0
%         3        1.000000         0         1
%         4        2.000000         1         1
%         5        2.000000         0         2
%
%    LEVEL_WEIGHT:          1.000000        2.000000
%
%    Q_MIN:       -1.000000
%    Q_MAX:        2.000000
%    X_MAX:                         2         1
%
%         1        0.000000         0         0
%         2        1.000000         1         0
%         3        2.000000         2         0
%         4        2.000000         0         1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    An Anisotropic Sparse Grid Stochastic Collocation Method for Partial
%    Differential Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2411-2442.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the number of components in the vector.
%
%    Input, real LEVEL_WEIGHT(DIM_NUM), the anisotropic weights.
%
%    Input, integer X_MAX(DIM_NUM), the maximum
%    values allowed in each component of X.
%
%    Input, integer X(DIM_NUM).  On first call, with
%    MORE = FALSE, the input value of X is not important.  On subsequent calls,
%    the input value of X should be the output value from the previous call.
%
%    Input, real Q_MIN, Q_MAX, the lower and upper limits on the sum.
%
%    Input, logical MORE.  On input, if the user has set MORE
%    FALSE, the user is requesting the initiation of a new sequence
%    of values.  If MORE is TRUE, then the user is requesting "more"
%    values in the current sequence.
%
%    Output, integer X(DIM_NUM).
%    On output, (with MORE = TRUE), the value of X will be the "next"
%    vector in the reverse lexicographical list of vectors that satisfy
%    the condition.  However, on output with MORE = FALSE, the vector
%    X is meaningless, because there are no more vectors in the list.
%
%    Output, logical MORE.  If MORE is TRUE on output,
%    then another value was found and returned in X, but if MORE is
%    FALSE, then there are no more values in the sequence, and X is
%    NOT the next value.
%
  if ( ~more )

    more = 1;

    x = zeros ( dim_num, 1 );

    q = level_weight(1:dim_num)' * x(1:dim_num);

    if ( q_min < q && q <= q_max )
      return
    end

  end

  while ( 1 )

    i = 1;

    while ( 1 )

      if ( x(i) < x_max(i) )
        break
      end

      if ( dim_num <= i )
        more = 0;
        return
      end

      i = i + 1;

    end

    x(i) = x(i) + 1;
    x(1:i-1) = 0;

    q = level_weight(1:dim_num)' * x(1:dim_num);

    if ( q_min < q && q <= q_max )
      break
    end

  end

  return
end


function pde = HodgeLaplacian3Edata1
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

pde.f = @f;
pde.exactu = @exactu;
pde.exactsigma = @exactsigma;
pde.gu = @exactu;
pde.gsigma = @exactsigma;
pde.gun = @gun;
pde.curlu = [0 0 0];
pde.gcurlu = 0;

    function s = f(p)
    x = p(:,1); y = p(:,2);
    s = [cos(x), -sin(y), zeros(size(p,1),1)];
    end

    function s = exactu(p)
    x = p(:,1); y = p(:,2); z = p(:,3);
    s = [cos(x), -sin(y), 2*z+1];
    end

    function s = exactsigma(p)
    x = p(:,1); y = p(:,2);
    s = sin(x)+cos(y)-2;
    end

    function f = gun(p) % for unit cube [0,1]^3
    f = zeros(size(p,1),1);
    x = p(:,1); y = p(:,2); z = p(:,3);
    u = exactu(p);
    leftbd = (abs(x)<eps);    % n = (-1,0,0)
    f(leftbd) = - u(leftbd,1);
    rightbd = (abs(x-1)<eps); % n = (1,0,0)
    f(rightbd) = u(rightbd,1);
    frontbd = (abs(y)<eps);   % n = (0,-1,0)
    f(frontbd) = -u(frontbd,2);
    backbd = (abs(y-1)<eps);  % n = (0,1,0)
    f(backbd) = u(backbd,2);
    topbd = (abs(z-1)<eps);   % n = (0,0,1)
    f(topbd) = u(topbd,3);
    bottombd = (abs(z)<eps);  % n = (0,0,-1)
    f(bottombd) = - u(bottombd,3);
    end

end


function [lx, ly, lz, varargin] = parseGridArgs3d(varargin)
%PARSEGRIDARGS3D  Extract or compute position vectors for meshgrid
%
%   [LX, LY] = parseGridArgs3d(LX, LY)
%   simply returns LX and LY.
%
%   [LX, LY] = parseGridArgs3d([NX NY])
%   assumes LX = 1:NX and LY = 1:NY
%
%   [LX, LY] = parseGridArgs3d([NX NY])
%   assumes LX = 1:NX and LY = 1:NY
%
%
%   Example
%   [lx, ly, lz] = parseGridArgs3d([100 100 100]);
%   [lx, ly, lz] = parseGridArgs3d(1:2:50, 1:4:100, 1:25);
%   [lx, ly, lz, varargin] = parseGridArgs3d(varargin{:});
%   for usage within another function.
%
%   See also
%     parseGridArgs

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2009-05-29,    using Matlab 7.7.0.471 (R2008b)
% Copyright 2009 INRA - Cepia Software Platform.

% If empty arguments, return default values
if isempty(varargin)
    lx = 1:100;
    ly = 1:100;
    lz = 1:100;
    return
end

var = varargin{1};

% case of a 2x3 matrix with starting position, increment, end position for
% each coordinate
if all(size(var) > [2 2])
    lx = var(1,1):var(1,2):var(1,3);
    ly = var(2,1):var(2,2):var(2,3);
    lz = var(3,1):var(3,2):var(3,3);
    varargin(1) = [];
    return;
end

% first argument contains maximal position for each coordinate
if all(size(var) == [1 3])
    lx = 1:var(1);
    ly = 1:var(2);
    lz = 1:var(3);
    varargin(1) = [];
    return;
end

% first and second arguments contain vector for each coordinate
% respectively
if length(varargin) > 2
    lx = varargin{1};
    ly = varargin{2};
    lz = varargin{3};
    varargin(1:3) = [];
    return
end

% otherwise, throws error
error('Error in parsing grid arguments');


%u2F    estimates fundamental matrix using ortogonal LS regression
%	F = u2F(u) estimates F from u using NORMU
%  F = u2F(u,'nonorm') disables normalization
%	see also NORMU, U2FA

function F = u2F (u, str)

if (nargin > 1) & strcmp(str, 'nonorm')
   donorm = 0;
else
   donorm = 1;
end

ptNum = size(u,2);

if donorm
   A1    = normu(u(1:3,:));
   A2    = normu(u(4:6,:));

   u1   = A1*u(1:3,:);
   u2   = A2*u(4:6,:);
end

for i = 1:ptNum
   Z(i,:)   = reshape(u1(:,i)*u2(:,i)',1,9);
end

M       = Z'*Z;
V       = seig(M);
F = reshape(V(:,1),3,3);

[uu,us,uv] = svd(F);
[y,i]      = min (abs(diag(us)));
us(i,i)    = 0;
F          = uu*us*uv';

if donorm
   F = A1'*F*A2;
end

F = F /norm(F,2);


function [c] = APGLASSOup(b,A,para)

%% Object: c = argmin (1/2)\|y-Dx\|_2^2+lambda\|x\|_1+\mu\|x_I\|_2^2
%%                s.t. x_T >0
%% input arguments:
%%         b -------  D'*y transformed object vector
%%         A -------  D'*D transformed dictionary
%%         para ------ Lambda: Sparsity Level
%%                     (lambda1: template set; lambda2:trivial template; lambda3:\mu)
%%                     Lip: Lipschitz Constant for F(x)
%%                     Maxit: Maximal Iteration number
%%                     nT: number of templates
%% output arguments:
%%         c ------  output Coefficient vetor

%  Initialization
ColDim = size(A,1);
xPrev = zeros(ColDim,1);
x = zeros(ColDim,1);
tPrev = 1;
t = 1;
lambda = para.Lambda;
Lip = para.Lip;
maxit = para.Maxit;
nT = para.nT;

temp_lambda = zeros(ColDim,1);
temp_lambda(1:nT) = lambda(1);

%% main loop
for iter =1:maxit
    tem_t = (tPrev-1)/t;
    tem_y = (1+tem_t)*x - tem_t*xPrev;
    temp_lambda(nT+1:end) = lambda(3)*tem_y(nT+1:end);
    tem_y = tem_y - (A*tem_y-b+temp_lambda)/Lip; % update residual
    xPrev = x;
    x(1:nT) = max(tem_y(1:nT),0);
    x(nT+1:end) = softthres(tem_y(nT+1:end),lambda(2)/Lip);
    tPrev = t;
    t = (1+sqrt(1+4*t^2))/2;
end
c = x;

%% soft thresholding operator
function y = softthres(x,lambda)
y = max(x-lambda,0)-max(-x-lambda,0);


function [xs, ys, values]=centers2edges(xs, ys, values)
    % CENTERS2EDGES converts matrices of measurement centers to cells with measurements at center
    %
    % edges=CENTERS2EDGES(centers) returns the edges of each bin, for a vector CENTERS containing
    % the center of each bin.  for centers of length N, edges will be of length N+1. edges for the
    % first and last bin, are symmetrical about each bin's center.
    %
    % [xs, ys,values]=CENTERS2EDGES(xs, ys, values) 2-dimensional version. xs and ys are
    % matrices of the same size
    %
    % Pcolor typically uses the points as edges, and ignores the last values.
    %
    % warning, shading interp might look pretty, but it will shift items and is inaccurate.
    % instead, use 2interp
    %
    % ex.
    %  >> centers2edges([0 2 4 8])
    %  ans =
    %      -1     1     3     6    10
    %
    % see also EDGES2CENTERS, HISTCOUNTS, HISTOGRAM

    if nargout ~= nargin
        error('Each input should have a corresponding output');
    end

    if nargin==1 && isvector(xs)
        if isempty(xs)
            % nothing to do. return the empty value
        else
            if numel(xs) <2
                error('need at least 2 centers to determine bin size')
            end
            half_dx = diff(xs) ./ 2;
            if any(half_dx <= 0)
                error('values should be in ascending order');
            end
            half_dx = [-half_dx(1); half_dx(:); half_dx(end)];
            xs(2:end+1)=xs;
            xs = xs + reshape(half_dx,size(xs));
        end
    else
        if ~isequal(size(xs), size(ys))
            error('X and Y matrices should be same size');
        end
        if min(size(xs) ==1)
            error('X and Y must be matrices, not vectors');
        end
        % expand grid in each direction so that xs and ys are in the centers
        half_dx = diff(xs,[],2) ./ 2;
        if any(half_dx <= 0)
            error('X vector should be in ascending order')
        end
        half_dx = [-half_dx(:,1) , half_dx , half_dx(:,end)];
        xs=[xs(:,1), xs] + half_dx;
        xs(end+1,:)=xs(end,:);

        half_dy = diff(ys,[],1) ./ 2;

        if any(half_dx <= 0)
            error('Y vector should be in ascending order');
        end
        half_dy = [-half_dy(1,:) ; half_dy ; half_dy(end,:)];
        ys=[ys(1,:); ys] + half_dy;
        ys(:,end+1)=ys(:,end);

        if exist('values','var')
            assert(isequal(size(xs)-1,size(values)),'size of VALUES doesn''t match X and Y matrices');
            values(end+1,:)=nan;
            values(:,end+1)=nan;
        elseif nargout >=3
            error('Values will only be returned if provided');
        end
    end
end


function [varargout]=cunique(varargin)

% function [A_uni,ind1,ind2,Ac]=cunique(A)
%-------------------------------------------------------------------------
%This function is similar to MATLAB's unique function. There are three
%differences: 1) An additional 4th optional output is available providing
%the count, or number of occurances, for each element in the input array.
%2) The 2nd output mathes the size of the first input, 3) The 3rd output
%is reshaped to be the size of the input variable.
%
% See also: unique
%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
%
% Change log:
% 2018/03/21 Created
% 2019/07/02 Adding varargin handling (pass unique options) including
% 'rows' support.
% 2020/01/10 Fixed bug in accumarray array size specification
%-------------------------------------------------------------------------

%%

A=varargin{1};

[A_uni,ind1,ind2]=unique(varargin{:});

if any(strcmp(varargin,'rows'))
    typeOpt=2;
else
    typeOpt=1;
end

switch typeOpt
    case 1
        varargout{1}=A_uni;
        varargout{2}=reshape(ind1,size(A_uni));
        varargout{3}=reshape(ind2,size(A));
    case 2
        varargout{1}=A_uni;
        varargout{2}=reshape(ind1,[size(A_uni,1) 1]);
        varargout{3}=reshape(ind2,[size(A,1) 1]);
end

if nargout==4
    [subInd] = ind2subn(size(A_uni),ind2);
%     Ac=accumarray(subInd,ones(numel(ind2),1),[size(A_uni,1),1]);
    Ac=accumarray(subInd,ones(numel(ind2),1),size(A_uni));
    switch typeOpt
        case 1
            Ac=reshape(Ac(ind2),size(A));
        case 2
            Ac=reshape(Ac(ind2),[size(A,1) 1]);
    end
    varargout{4}=Ac;
end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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 ival = r8mat_is_antipersymm ( m, n, a )

%*****************************************************************************80
%
%% R8MAT_IS_ANTIPERSYMM checks for antipersymmetry.
%
%  Discussion:
%
%    A is antipersymmetric if A(I,J) = -A(N+1-J,N+1-I).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the row and column dimensions of
%    the matrix.  M and N must be positive.
%
%    Input, real A(M,N), the matrix.
%
%    Output, integer IVAL:
%    -1, the matrix is not antipersymmetric.
%    1, the matrix is antipersymmetric.
%
  ival = 1

  for i = 1 : min ( m, n )
    for j = n : -1 : max ( 1, n - m + 1 )
      if ( a(i,j) ~= -a(n+1-j,n+1-i) )
        ival = -1;
        return
      end
    end
  end

  return
end


function pde = Kelloggdata
%%  KELLOGGDATA data of Kellogg Problem
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

pde = struct('f',0,'exactu',@exactu,'g_D',@exactu,'Du',@Du,'d',@d);

    function K =  d(p)  % Diffusion constant
    idx = (p(:,1).*p(:,2) >0);
    K  = ones(size(p,1),1);
    K(idx) = 161.4476387975881;
    end

    function u =  exactu(p) % exact solution
    gamma = 0.1;
    sigma = -14.9225565104455152;
    rho = pi/4;
    r = sqrt(sum(p.^2,2));
    theta = atan2(p(:,2),p(:,1));
    theta = (theta>= 0).*theta + (theta<0).*(theta+2*pi);
    mu = (theta>= 0 & theta<pi/2).*cos((pi/2-sigma)*gamma).*cos((theta-pi/2+rho)*gamma)...
        +(theta>= pi/2 & theta<pi).*cos(rho*gamma).*cos((theta-pi+sigma)*gamma)...
        +(theta>= pi & theta<1.5*pi).*cos(sigma*gamma).*cos((theta-pi-rho)*gamma)...
        +(theta>= 1.5*pi & theta<2*pi).*cos((pi/2-rho)*gamma).*cos((theta-1.5*pi-sigma)*gamma);
    u =  r.^gamma.*mu;
    end

    function uprime =  Du(p)
    % the gradient of exact solution
    gamma = 0.1;
    sigma = -14.92256510455152;
    rho = pi/4;
    theta = atan2(p(:,2),p(:,1));  % jiaodu
    theta = (theta>= 0).*theta +(theta<0).*(theta+2*pi);
    t = 1+(p(:,2).^2)./(p(:,1).^2);
    r = sqrt(sum(p.^2,2));
    rg = r.^gamma;

    ux1 = (p(:,1)>= 0.0 & p(:,2)>= 0.0).*(rg.*gamma./r.*cos((pi/2-sigma)*gamma)./r.*p(:,1)...
        .*cos((theta-pi/2+rho)*gamma)...
        +rg.*cos((pi/2-sigma)*gamma).*sin((theta-pi/2+rho)*gamma)...
        *gamma.*p(:,2)./(p(:,1).^2)./t);

    uy1 = (p(:,1)>= 0.0 & p(:,2)>= 0.0).*(rg*gamma./r.*cos((pi/2-sigma).*gamma)...
        .*cos((theta-pi/2+rho).*gamma)./r.*p(:,2)...
        -rg.*cos((pi/2-sigma).*gamma).*sin((theta-pi/2+rho)*gamma)...
        *gamma./p(:,1)./t);

    ux2 = (p(:,1)<= 0.0 & p(:,2)>= 0.0).*(r.^(-1.9).*p(:,1)*gamma.*cos(rho.*gamma)...
        .*cos((theta-pi+sigma).*gamma)...
        +rg.*cos(rho*gamma).*sin((theta-pi+sigma).*gamma)*gamma...
        .*p(:,2)./(p(:,1).^2)./t);
    uy2 = (p(:,1)<= 0.0 & p(:,2)>= 0.0).*( r.^(-1.9).*p(:,2)*gamma.*cos(rho.*gamma)...
        .*cos((theta-pi+sigma)*gamma)...
        -rg.*cos(rho*gamma).*sin((theta-pi+sigma).*gamma)*gamma./p(:,1)./t);

    ux3 = (p(:,1)<= 0.0 & p(:,2)<= 0.0).*(r.^(-1.9).*p(:,1).*gamma.*cos(sigma.*gamma)...
        .*cos((theta-pi-rho).*gamma) ...
        +rg.*cos(sigma.*gamma).*sin((theta-pi-rho).*gamma).*gamma...
        .*p(:,2)./(p(:,1).^2)./t);
    uy3 = (p(:,1)<= 0.0 & p(:,2)<= 0.0).*(r.^(-1.9).*p(:,2)*gamma.*cos(sigma*gamma)...
        .*cos((theta-pi-rho).*gamma) ...
        -rg.*cos(sigma*gamma).*sin((theta-pi-rho).*gamma)*gamma./p(:,1)./t);

    ux4 = (p(:,1)>= 0.0& p(:,2)<= 0.0).*(r.^(-1.9).*p(:,1).*gamma.*cos((pi/2-rho)*gamma)...
        .*cos((theta-3*pi/2-sigma)*gamma) ...
         +rg.*cos((pi/2-rho).*gamma).*sin((theta-3*pi/2-sigma)*gamma)...
         *gamma.*p(:,2)./(p(:,1).^2)./t);

    uy4 = (p(:,1)>= 0.0 & p(:,2)<= 0.0).*(r.^(-1.9).*p(:,2)*gamma.*cos((pi/2-rho)*gamma)...
        .*cos((theta-3*pi/2-sigma)*gamma)...
        -rg.*cos((pi/2-rho)*gamma).*sin((theta-3*pi/2-sigma)*gamma)...
        *gamma./p(:,1)./t);

    uprime(:,1) =  ux1+ux2+ux3+ux4;
    uprime(:,2) =  uy1+uy2+uy3+uy4;
    end
end


function u = cosh(a)
%COSH         Slope hyperbolic cosine cosh(a)
%

% written  12/06/98     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  u = a;

  u.r = cosh(a.r);
  u.s = slopeconvexconcave('cosh','sinh(%)',a,1);

  if rndold
    setround(rndold)
  end


function [ X ] = X_Solver_first(D,rho)
  [U,S,V] = svd(D);
  S0 = diag(S);
  r = length(S0);
  P = [ones(r,1), 1-S0, 1/2/rho-S0];
  rt = zeros(r,1);
  for t = 1:r
    p = P(t,:);
    Delta = p(2)^2-4*p(1)*p(3);
    if Delta <= 0
      rt(t) = 0;
    else
      rts = roots(p);
      rts = sort(rts);
      if rts(1)*rts(2)<=0
        rt(t) = rts(2);
      elseif rts(2)<0
        rt(t) = 0;
      else
        funval = log(1+rts(2))+rho*(rts(2)-S0(t)).^2;
        if funval>log(1+0)+rho*(0-S0(t)).^2;
          rt(t) = 0;
        end
      end
    end
  end

  SSS = diag(rt);
  [m,n] = size(D);
  sig = zeros(m,n);
  sig(1:min(m,n),1:min(m,n)) = SSS;

  X = U*sig*V';
end


function [ J ] = vec2jacl( vec )

tolerance = 1e-12;

if size(vec,1) == 3

    phi = vec;

    ph = norm(phi);
    if ph < tolerance
        % If the angle is small, fall back on the series representation
        J = vec2jaclSeries(phi,10);
    else
        axis = phi/ph;

        cph = (1 - cos(ph))/ph;
        sph = sin(ph)/ph;

        J = sph * eye(3) + (1 - sph) * axis * axis' + cph * hat(axis);
    end

elseif size(vec,1) == 6

    phi = vec(1:3);
    rho = vec(4:6);

    ph = norm(phi);
    if ph < tolerance
        % If the angle is small, fall back on the series representation
        J = vec2jaclSeries(phi,10);
    else
        Jsmall = vec2jacl( phi );
        Q = vec2Ql( vec );
        J = [ Jsmall Q; zeros(3) Jsmall ];
    end
end
end


function [t,x_new,f_new,g_new,funEvals,H] = ArmijoBacktrack(...
    x,t,d,f,fr,g,gtd,c1,LS_interp,LS_multi,progTol,debug,doPlot,saveHessianComp,funObj,varargin)
% [t,x_new,f_new,g_new,funEvals,H] = ArmijoBacktrack(...
%    x,t,d,f,fr,g,gtd,c1,LS_interp,LS_multi,progTol,debug,doPlot,saveHessianComp,funObj,varargin)
%
% Backtracking linesearch to satisfy Armijo condition
%
% Inputs:
%   x: starting location
%   t: initial step size
%   d: descent direction
%   f: function value at starting location
%   fr: reference function value (usually funObj(x))
%   gtd: directional derivative at starting location
%   c1: sufficient decrease parameter
%   debug: display debugging information
%   LS_interp: type of interpolation
%   progTol: minimum allowable step length
%   doPlot: do a graphical display of interpolation
%   funObj: objective function
%   varargin: parameters of objective function
%
% Outputs:
%   t: step length
%   f_new: function value at x+t*d
%   g_new: gradient value at x+t*d
%   funEvals: number function evaluations performed by line search
%   H: Hessian at initial guess (only computed if requested)
%
% recet change: LS changed to LS_interp and LS_multi

% Evaluate the Objective and Gradient at the Initial Step
if nargout == 6
    [f_new,g_new,H] = funObj(x + t*d,varargin{:});
else
    [f_new,g_new] = funObj(x+t*d,varargin{:});
end
funEvals = 1;

while f_new > fr + c1*t*gtd || ~isLegal(f_new)
    temp = t;

    if LS_interp == 0 || ~isLegal(f_new)
        % Ignore value of new point
        if debug
            fprintf('Fixed BT\n');
        end
        t = 0.5*t;
    elseif LS_interp == 1 || ~isLegal(g_new)
        % Use function value at new point, but not its derivative
        if funEvals < 2 || LS_multi == 0 || ~isLegal(f_prev)
            % Backtracking w/ quadratic interpolation based on two points
            if debug
                fprintf('Quad BT\n');
            end
            t = polyinterp([0 f gtd; t f_new sqrt(-1)],doPlot,0,t);
        else
            % Backtracking w/ cubic interpolation based on three points
            if debug
                fprintf('Cubic BT\n');
            end
            t = polyinterp([0 f gtd; t f_new sqrt(-1); t_prev f_prev sqrt(-1)],doPlot,0,t);
        end
    else
        % Use function value and derivative at new point

        if funEvals < 2 || LS_multi == 0 || ~isLegal(f_prev)
            % Backtracking w/ cubic interpolation w/ derivative
            if debug
                fprintf('Grad-Cubic BT\n');
            end
            t = polyinterp([0 f gtd; t f_new g_new'*d],doPlot,0,t);
        elseif ~isLegal(g_prev)
            % Backtracking w/ quartic interpolation 3 points and derivative
            % of two
            if debug
                fprintf('Grad-Quartic BT\n');
            end
            t = polyinterp([0 f gtd; t f_new g_new'*d; t_prev f_prev sqrt(-1)],doPlot,0,t);
        else
            % Backtracking w/ quintic interpolation of 3 points and derivative
            % of two
            if debug
                fprintf('Grad-Quintic BT\n');
            end
            t = polyinterp([0 f gtd; t f_new g_new'*d; t_prev f_prev g_prev'*d],doPlot,0,t);
         end
    end

    % Adjust if change in t is too small/large
    if t < temp*1e-3
        if debug
            fprintf('Interpolated Value Too Small, Adjusting\n');
        end
        t = temp*1e-3;
    elseif t > temp*0.6
        if debug
            fprintf('Interpolated Value Too Large, Adjusting\n');
        end
        t = temp*0.6;
    end

    % Store old point if doing three-point interpolation
    if LS_multi
        f_prev = f_new;
        t_prev = temp;
        if LS_interp == 2
            g_prev = g_new;
        end
    end

    if ~saveHessianComp && nargout == 6
        [f_new,g_new,H] = funObj(x + t*d,varargin{:});
    else
        [f_new,g_new] = funObj(x + t*d,varargin{:});
    end
    funEvals = funEvals+1;

    % Check whether step size has become too small
    if max(abs(t*d)) <= progTol
        if debug
            fprintf('Backtracking Line Search Failed\n');
        end
        t = 0;
        f_new = f;
        g_new = g;
        break;
    end
end

% Evaluate Hessian at new point
if nargout == 6 && funEvals > 1 && saveHessianComp
    [f_new,g_new,H] = funObj(x + t*d,varargin{:});
    funEvals = funEvals+1;
end

x_new = x + t*d;

end


function newp = eleminterpolate(p,tree,weight)
%% ELEMINTERPOLATE interpolate a piecewise constant function.
%
% newp = eleminterpolate(p,tree) interpolate a piecewise constant function
% p from a coarse grid to a fine grid or the other way around. tree(:,1:3)
% stores the binary tree of the coarsening. tree(:,1) is the index of
% parent element in the coarsened mesh and tree(:,2:3) are two children
% indices in the original mesh.
%
% For uniform refinement, the prolongation from the coarse grid to the fine
% grid is simply |newp = repmat(p,4,1)| and from the fine grid to the
% coarse one is |newp = p(1:NT/4)|.
%
% Example
%
%   [node,elem] = squaremesh([0 1 0 1],1/2);
%   p = 1:size(elem,1);
%   figure(1);
%   subplot(1,4,1); showsolution(node,elem,p,'EdgeColor','k'); view(2);
%   [node,elem,~,~,tree] = bisect(node,elem,[1 2]);
%   p = eleminterpolate(p,tree);
%   subplot(1,4,2); showsolution(node,elem,p,'EdgeColor','k'); view(2);
%   [node,elem,~,~,tree] = bisect(node,elem,[1 2]);
%   p = eleminterpolate(p,tree);
%   subplot(1,4,3); showsolution(node,elem,p,'EdgeColor','k'); view(2);
%   [node,elem,~,~,tree] = coarsen(node,elem,'all');
%   p = eleminterpolate(p,tree);
%   subplot(1,4,4); showsolution(node,elem,p,'EdgeColor','k'); view(2);
%
% See also bisect, coarsen, nodeinterpolate
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('weight','var'), weight = 1; end
%%
newp = p;
if (size(tree,1)==0), return; end % no change
NTin = length(p);
NTf = max(tree(:,3));
if NTin < NTf 		   % coarse grid to fine grid
    NTc = NTin;
    newp = zeros(NTf,1);
    isNew = tree(:,3); % Right child is a new element
    inCoarse = true(NTf,1);
    inCoarse(isNew) = false;
    % Case 0: still in coarse element
    newp(inCoarse) = p;
    % Case 1: The parent element is in the coarse mesh. A triangle t could
    % be bisected twice and a child could be a parent
    idx = (tree(:,1) <= NTc);
    if weight == 1
        newp(tree(idx,2)) = p(tree(idx,1));
        newp(tree(idx,3)) = p(tree(idx,1));
    else
        newp(tree(idx,2)) = weight*p(tree(idx,1));
        newp(tree(idx,3)) = weight*p(tree(idx,1));
    end
    % Case 2: The parent is in the intermediate mesh
    idx = (tree(:,1) > NTc);
    if weight == 1
        newp(tree(idx,2)) = newp(tree(idx,1));
        newp(tree(idx,3)) = newp(tree(idx,1));
    else
        newp(tree(idx,2)) = weight*newp(tree(idx,1));
        newp(tree(idx,3)) = weight*newp(tree(idx,1));
    end
else      % fine grid to coarse grid
    newp = p;
    newp(tree(:,3)) = [];
end


%% patchFeatureDetect
% Below is a demonstration of the features of the |patchFeatureDetect| function

%%
clear; close all; clc;

%% Syntax
% |[G]=patchFeatureDetect(F,V,a);|

%% Description
% This function detects surface features or groups for the input patch data
% defined by the faces F and the vertices V. Patch elements for which the
% dihedral angle is lower than a are grouped together.
% This function is useful for detecting sets of faces from for instance
% importance CAD geometry, e.g. top faces, side faces etc can be
% automatically detected provided that their boundaries have a significant
% enough angle change.

%%
% Plot settings
fontSize=15;

%%
% Create example data

testCase=4;
switch testCase
    case 1
        boxDim=[4 5 6]; %Width in each direction
        pointSpacing=1; %Desired point spacing
        [F,V]=triBox(boxDim,pointSpacing);
    case 2
        boxDim=[4 5 6]; %Width in each direction
        boxEl=[4 5 6]; %Desired number of elements
        [F,V]=quadBox(boxDim,boxEl);
    case 3
        defaultFolder = fileparts(fileparts(mfilename('fullpath')));
        pathName=fullfile(defaultFolder,'data','libSurf');
        dataStruct=load(fullfile(pathName,'enginePart_p1.mat'));
        F=dataStruct.F;
        V=dataStruct.V;
    case 4
        defaultFolder = fileparts(fileparts(mfilename('fullpath')));
        pathName=fullfile(defaultFolder,'data','libSurf');
        dataStruct=load(fullfile(pathName,'sprocket.mat'));
        F=dataStruct.F;
        V=dataStruct.V;
end

%%
% Use |patchFeatureDetect| to detect features in patch

%Angular threshold in radians
a=(45/180)*pi;

%Detect surface features
G=patchFeatureDetect(F,V,a);

%%

% Plotting model
cFigure;
subplot(1,2,1); hold on;
title('Input surface','FontSize',fontSize);
gpatch(F,V,'w','k',1);
axisGeom(gca,fontSize); camlight headlight;

subplot(1,2,2); hold on;
title('Surface feature labels','FontSize',fontSize);
gpatch(F,V,G,'k',1);
axisGeom(gca,fontSize); camlight headlight;
colormap gjet; icolorbar;
gdrawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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/>.


% comp_log_Mel receives a time domain speech signal and produces its
% log Mel filterbank coefficients. It is the Matlab counterpart of the
% feature extraction program provided with AURORA2 database.
% Author: Xiao Xiong
% Created: 18 Jul, 2005
% Last Modified: 28 Jul, 2005
% Inputs:
%       x   1-D time domain signal
% Outputs:
%       hist_log_mel    the log Mel filterbank coefficients
%       hist_mel        the Mel filterbank coefficients
%       hist_abs        the spectral magnitude
%       logE            the log energy item needed in computing MFCC

function [X_mel] = abs2Mel(X_abs, Fs, N_mel)

if nargin < 2
    Fs = 8000;
end
if nargin < 3
    N_mel = 23;             % for AURORA2 database, 23 mel filterbank are used
end

[N_vec,n] = size(X_abs);
FFT_length = 2*n;

% generate the mel window
mel_win = mel_window_FE(N_mel, FFT_length/2, Fs);
% X_mel = zeros(N_vec,N_mel);
% for i=1:N_vec
%     X_mel(i,:) = X_abs(i,:)*mel_win;
% end
% faster implementation
X_mel = X_abs*mel_win;


function [NTWDseries TWDseries] = SquaredDiffComputation_beatbybeat(Complexes)
% [NTWDseries TWDseries] = SquaredDiffComputation_beatbybeat(Complexes)
%   OVERVIEW:   Computed squared difference between consecutive QRS
%               complexes with and without dynamic time warping.
%
%   INPUT:      MANDATORY:
%               Complexes       : 2D array of dimension number of complexes by median length of complexes
%
%
%
%
%   OUTPUT:
%               NTWDseries      : Squared difference between consecutive QRS complexes without time warping
%
%               TWDseries       : Squared difference between consecutive QRS complexes with time warping
%
%	REPO:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%   ORIGINAL SOURCE AND AUTHORS:
%       Written by Ismail Sadiq
%	COPYRIGHT (C) 2019
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information. The license may be found in
%       the Documents folder of the Physionet-Cardiovascular-Signal-Toolbox.
%

noofconsecutivecomplexes = size(Complexes,1);

% time warped squared diff
TWDseries(1:noofconsecutivecomplexes) = 0;
% non time warped squared diff
NTWDseries(1:noofconsecutivecomplexes) = 0;

for index = 1:noofconsecutivecomplexes-1

    complex1 = Complexes(index,:);
    complex2 = Complexes(index+1,:);

    % compute squared difference between each complex and median complex w/o
    % dtw
    SD = sum((complex1 - complex2).^2);
    SD = SD ./ length(complex1);
    NTWDseries(index) = SD;

    % compute squared difference between each complex and median complex with dtw
    [dist,ix,iy] = dtw(complex1, complex2);
    templt1 = complex1(ix);
    templt2 = complex2(iy);

    MD = sum((templt1 - templt2).^2);
    MD = MD ./ length(templt1);

    TWDseries(index) = MD;

end

end


%TRIDEIG Eigenvalues of symmetric tridiagonal matrix.
%   E = TRIDEIG(A,B) is a vector containing the eigenvalues of a square
%   tridiagonal matrix with diagonal elements A and subdiagonal elements B.
%   A and B should be real vectors of length N and N-1, respectively, where
%   N is the size of the matrix. E is a real vector of length N.

%   P.-O. Persson <persson@mit.edu>
%   Department of Mathematics, MIT
%   October 21, 2002

error('No mex-file found for this platform.')


 function ftab = de_ftab(xrs, mas, varargin)
%function ftab = de_ftab(xrs, mas, [options])
%|
%| For multiple-kVp X-ray imaging, we must evaluate functions f_m(s_1, ..., s_L)
%| for m=1,..,M, where M is the number of kVp settings,
%| and L is the number of material components.
%| f_m(s1, s2) = -log( \int exp(- (m1(E) s1 + m2(E) s2)) dI_m(E) / I_m(E) )
%| (For "dual-energy" imaging, M = L = 2.)
%|
%| This routine builds tables/models of f_m, its inverse, and its derivatives.
%| These are needed for (dual-energy) X-ray CT reconstruction.
%|
%| in
%|	xrs	strum	X-ray spectra; see xray_read_spectra.m
%|	mas	strum	mass attenuation coefficients; see xray_read_mac.m
%|
%| option
%|	'sls'	strum	see de_ftab_sls.m
%|	'ftype'	char	fit type for de_ftab_fit() (default: '')
%|	'fit_args' {}	options for de_ftab_fit() (default: {})
%|			e.g.: {'wt', num2cell(ones(M,1))} weighting for fitting
%|	'ctype'	char	curv type for de_ftab_curv() (default: '')
%|	'show'	0|1	visualize
%|
%| out
%|	ftab	strum
%|		data:
%|			.xrs,.mas,.sls,.mac	based on input
%|			.fit	strum with fm approximation methods
%|				see de_ftab_fit.m
%|			.inv1	strum to invert 1 component BH, e.g., water
%|				see de_ftab_inv1.m
%|			.inv2	strum to invert BH by polynomials,
%|				see de_ftab_inv1.m
%|		methods:
%|			.fm_fun(s1, s2, ...) (probably not needed by user)
%|			.plot_fm()
%|			.plot_mac()
%|			.plot_jac()
%|			.plot_inv()
%|
%| Copyright 2008-6-15, Jeff Fessler, University of Michigan

if nargin == 1 && streq(xrs, 'test'), de_ftab_test, return, end
if nargin < 2, ir_usage, end

ftab.show = false;
ftab.sls = struct([]);
ftab.ftype = ''; % defer to de_ftab_fit() default
ftab.fit_args = {}; % defer to de_ftab_fit() default(s)
ftab.ctype = ''; % defer to de_ftab_curv() default ('newt' is fastest)
ftab = vararg_pair(ftab, varargin);

% X-ray spectra
if isempty(xrs)
%	xrs = {'mono,60,100'}; % monoenergetic for testing
	xrs = {'ps1'};
end
if iscell(xrs)
	xrs = xray_read_spectra(xrs{:});
end
if ftab.show
	clf, xrs.plot
prompt
end
ftab.xrs = xrs;

ftab.MM = size(xrs.sp, 2); % # of spectra

% mas: mass attenuation coefficient (strum), at tabulated energies
if isempty(mas)
	mas = {'soft', 'bone'};	% default materials
end
if iscell(mas)
	mas = xray_read_mac(mas);
end
if ftab.show
	clf, mas.plot('kev', xrs.en)
prompt
end
ftab.mas = mas;

% mac: mass attenuation coefficient, at spectrum energies
ftab.mac = xray_make_mac(xrs, mas);
ftab.LL = ncol(ftab.mac.mac);
if ftab.show
	pr ftab.mac.bar % 'bmassml'
%	pr cond(ftab.mac.bar) % examine condition number
	pr sqrt(cond(ftab.mac.bar' * ftab.mac.bar)) % examine condition number
	de_ftab_plot_mac(ftab);
prompt
end

% material integral sampling:
% s_l is samples of the integral of the density of the lth material type.
if isempty(ftab.sls)
	ftab.sls = de_ftab_sls;
end

% build tables of f_m(s1, s2)
fm_fun = @de_ftab_make_fm;
ftab.fm = fm_fun(ftab);
if ftab.show % figure showing f1, f2.  it looks linear, but it not quite!
	de_ftab_plot_fm(ftab, 'down', 4);
prompt
end

% parametric fit to each fm to form a continuous function!
printm 'fit'
if streq(ftab.ftype, 'exp3')
	ftab.fit = de_ftab_fit_exp3(ftab.xrs, ftab.mac.mac, ftab.fit_args{:});
else
	ftab.fit = de_ftab_fit(ftab.sls.sl, ftab.fm, 'type', ftab.ftype, ...
		'mtype', mas.type, ...
		'kev', xrs.en, 'mac', ftab.mac.mac, ... % needed for exp
		'macbar', ftab.mac.bar, ftab.fit_args{:});
% todo: may be bad (and slow) to use finely sampled energies (kev) for exp fit
%		'type', 'exp');
%ftab.fit = de_ftab_fit(ftab.sl, ftab.fm, 'type', 'poly', 'order', 3, 'dc', 1);
end

ftab.fit = de_ftab_curv(ftab.fit, 'ctype', ftab.ctype);

% Jacobian transformation
ftab.T = pinv(ftab.mac.bar); % [L M] transformation, fs = T f approx s !

% Build 1D inverse of 1st material component (usually water)
% for conventional "water only" beam-hardening correction.
ftab.inv1 = de_ftab_inv1(ftab.fit, ftab.sls.sl{1});

% build polynomial inverse approximation
ftab.inv2 = de_ftab_inv2(ftab.fit, ftab.sls.sl, 'T', ftab.T); % todo: options

%ftab.inv.eval = @(ftab,fhat) de_ftab_invert(ftab,fhat);
%ftab.inv = de_ftab_inv_setup(ftab);

meth = {'fm_fun', fm_fun, '(cell: sl)';
	'show_fm', @de_ftab_plot_fm, '()'; % backward compat
	'plot_fm', @de_ftab_plot_fm, '()';
	'plot_mac', @de_ftab_plot_mac, '()';
	'plot_jac', @de_ftab_plot_jac, '()';
	};

ftab = strum(ftab, meth);

ftab = de_ftab_inv_setup(ftab);

if ftab.show % compare fit to sampled fm, etc.
	ftab.inv1.plot(ftab.fit);
	if ~isempty(ftab.inv2)
		ftab.inv2.plot(ftab.fit, ftab.sls.sl);
	end
%	ftab.fit.show_fm(ftab.sls.sl, ftab.fm)
%	ftab.fit.show_err(ftab.sls.sl, ftab.fm)
	ftab.plot_jac;
	ftab.plot_inv;
%keyboard
end

% for safefty, remove table, forcing user to use ftab.fm_fun or ftab.fit.fmfun !
% ftab = rmfield(ftab, 'fm');

end % de_ftab()

%
% de_ftab_inv_setup()
% add methods related to inverse to ftab
%
function ftab = de_ftab_inv_setup(ftab)

printm 'inv: f -> s'

arg.inv = struct; % place holder
fun = @(ftab, fhat) de_ftab_invert(ftab, fhat);
meth = {'inv_fun', fun, '(fhat)';
	'plot_inv', @de_ftab_plot_inv, '()'};
ftab = strum(arg, meth, 'base', ftab);

end % de_ftab_inv_setup()

%
% de_ftab_plot_inv()
% plot inverse, testing with a different set of samples to see errors
%
function dummy = de_ftab_plot_inv(ftab)
dummy = [];
LL = ftab.LL;
MM = ftab.MM;
if LL ~= 2, warn 'L=2 done only', return, end
if MM < LL, warn('plot_inv with MM=%d < LL=%d skipped', MM, LL), return, end

if 1
	sls = ftab.sls;
	s1 = linspace(0, sls.max(1)+1, length(sls.sl{1}) + 8);
	s2 = linspace(0, sls.max(2)+1, length(sls.sl{2}) + 8);
	ss = ndgrid_jf('mat', s1, s2);
	ftmp = ftab.fit.fmfun(ss);
	stmp = ftab.inv_fun(ftmp);

	err = stmp - ss;
	err = abs(err);
	for ll=1:LL
		printm('worst inverse error l=%d: %g of %g', ll, ...
			max(col(stackpick(err,ll))), max(col(stackpick(ss,ll))))
	end
end

if im && usejava('jvm')
	ss1 = ss(:,:,1);
	ss2 = ss(:,:,2);
	clf, pl = @(n) subplot(220 + n);
	pl(1), mesh(s1, s2, ss1')
	axis tight, xtick, ytick, zwhite, grid
	hold on, plot3(ss1, ss2, stmp(:,:,1), 'y.'), hold off
	xlabel 's1', ylabel 's2', title 's1'

	pl(2), mesh(s1, s2, ss2')
	axis tight, xtick, ytick, zwhite, grid
	hold on, plot3(ss1, ss2, stmp(:,:,2), 'y.'), hold off
	xlabel 's1', ylabel 's2', title 's2'

	pl(3), mesh(s1, s2, err(:,:,1)')
	colormap hsv, caxis([0 max(err(:))]), cbar
	axis tight, xtick, ytick, zwhite, grid
	xlabel 's1', ylabel 's2', title 'error 1'

	pl(4), mesh(s1, s2, err(:,:,2)')
	axis tight, xtick, ytick, zwhite, grid
	xlabel 's1', ylabel 's2', title 'error 2'
prompt
end

end % de_ftab_plot_inv()

%
% de_ftab_plot_mac()
%
function dummy = de_ftab_plot_mac(ftab, varargin)
dummy = [];
xrs = ftab.xrs;
mas = ftab.mas;
mac = ftab.mac;

ltype = {'c:', 'y:', 'g:', 'r:'};
mtype = {'g+', 'r^', 'm*', 'yo'};
arg = {};
for ll=1:ftab.LL
	arg = {arg{:}, xrs.en, mac.mac(:,ll), ltype{ll}};
end
for mm=1:ftab.MM
	arg = {arg{:}, xrs.eff(mm), mac.bar(mm,:), mtype{mm}};
end

plot(arg{:})
axisy(0.1, max(0.7, 1.1*max(mac.bar(:))))
xlabel 'E', ylabel 'mac(E)'
mtype = mas.type;
legend(mtype{:})

prompt
end % de_ftab_plot_mac()

%
% de_ftab_make_fm()
% build tables of f_m(s1, s2, ...)
%
function fm = de_ftab_make_fm(ftab, varargin)
if length(varargin) == 0
	varargin = ftab.sls.sl; % default
end
if length(varargin) == 1 && iscell(varargin{1})
	sll = varargin{1};
else
	sll = ndgrid_jf('cell', varargin{:});
end
fm = de_ftab_fm(sll, ftab.mac.mac, ftab.xrs.Ide);

end % de_ftab_make_fm()

%
% de_ftab_plot_fm()
%
function dummy = de_ftab_plot_fm(ftab, varargin)
dummy = [];
arg.down = 1;
arg = vararg_pair(arg, varargin);

fm = ftab.fm;
sl = ftab.sls.sl;

if ftab.LL == 1
	plot(sl{1}, fm, '-', sl{1}, sl{1} * ftab.mac.bar(:)', ':')
	xlabel 's1'
	ylabel 'f1(s1)'
	name = ftab.xrs.name;
	legend(name{:}, 2)

elseif ftab.LL == 2

	fmax = 1.01 * max(fm(:));
	fmax = ceil(fmax);
	s_max(1) = max(sl{1}(:));
	s_max(2) = max(sl{2}(:));

	i1 = 1:arg.down:length(sl{1});
	i2 = 1:arg.down:length(sl{2});
	s1 = sl{1}(i1);
	s2 = sl{2}(i2);

	pl = @(mm) subplot(1,ftab.MM,mm);

	clf
	for mm=1:ftab.MM
		f1 = fm(i1,i2,mm);
		pl(mm)
		plot(s1, f1', '.-')
		axisx([0 s_max(1)]), xlabel 's_1'
		titlef('f_%d(s)', mm)
		text(-60, 10, '[cm^2/g]')
	end

	if usejava('jvm')
		prompt
	else
		warn 'need jvm for plot_fm mesh'
	return
	end

	clf
	for mm=1:ftab.MM
		f1 = fm(i1,i2,mm);
		pl(mm)
		mesh(s1, s2, f1')
		colormap hsv, caxis([0 fmax]), cbar
		axis([0 s_max(1) 0 s_max(2) 0 fmax])
		xtick, ytick, ztick, zwhite, xlabel s_1, ylabel s_2
		titlef('f_%d(s)', mm)
		text(-60, 10, '[cm^2/g]')
	end
end

end % de_ftab_plot_fm()

%
% de_ftab_test
%
function de_ftab_test
xrs = xray_read_spectra('ps1'); % M=2
%xrs = xray_read_spectra('poly1,60,90,120'); % M=3 test
%xrs = xray_read_spectra('poly1,160', 'filters', {{'copper', 0.1}}); % M < L test
xrs.plot
prompt
mas = xray_read_mac({'water', 'bone'});
if 0
	mas = xray_read_mac({'soft', 'iodine'});
	tmp = mas.mac_raw;
	tmp{2}(:) = tmp{2}(:) / 10; % todo: explore scaling to improve cond #
	mas.mac_raw = tmp;
end
ftab = de_ftab(xrs, mas, 'show', im, ...
	 'ftype','exp', 'fit_args', {'kev', 10:5:160, 'mac', []});
if im
	ftab.plot_fm;
	prompt
	ftab.inv1.plot(ftab.fit);
	if ~isempty(ftab.inv2)
		ftab.inv2.plot(ftab.fit, ftab.sls.sl);
	end
	clf, ftab.plot_mac;
	ftab.plot_jac;
	ftab.plot_inv;
end
end % de_ftab_test


function cdot(x,y,z,c,dot_size,label,range);
% CDOT plots colored dots over the range of data
%
% Usage:  cdot(x,y,z,c,dot_size,label,range);
%
%   x=vector of x locations
%   y = vector of x locations
%   z = vector of z values
%   c = colormap
%   dot_size = Marker size for dots (e.g 8)
%   label = 1 to label with value, 0 otherwise
%   range = range of data for colormap
%
%  example
%    [x,y,z]=peaks(20);cdot(x(:),y(:),z(:),jet,20,1,[-8 8])
%
 [m,n]=size(c);
 ndots=length(z);
 zsave=z;
 if(exist('range')==1),
  zmin=range(1);
  zmax=range(length(range))+eps;
  iz=find(z>=zmax);
  if(~isempty(iz)),
    z(iz)=(zmax-eps)*ones(size(iz));
  end
  iz=find(z<zmin);
  if(~isempty(iz)),
    z(iz)=zmin*ones(size(iz));
  end
 else
  zmin=min(z(:));
  zmax=max(z(:))+eps;
 end
 zinc=(zmax-zmin)/m;
% set(gca,'xlim',[min(x) max(x)],'ylim',[min(y) max(y)]);
 for i=1:m;
   z1=zmin+zinc*(i-1);
   z2=zmin+zinc*i;
   ind=find(z>=z1& z<=z2);
   line(x(ind),y(ind),'linestyle','none','marker','.','markersize',...
      dot_size,'color',c(i,:));
 end
 if(label==1);
     for i=1:ndots
%      text(x(i),y(i),sprintf(' %d',zsave(i)),'fontsize',14,...
    text(double(x(i)),double(y(i)),sprintf('%5.2f',zsave(i)),...
     'HorizontalAlignment','left','VerticalAlignment','bottom')
%      bgtext(x(i),y(i),sprintf(' %d',zsave(i)),[1 1 .9],'fontsize',10,...
%       bgtext(x(i),y(i),sprintf(' %d',zsave(i)),[1 1 .9]);
    end
 end


        % Deschiderea unei ferestre de figuri
figure(1)
t = -10*pi:pi/250:10*pi;
        % Desenarea cometei tridimensionale
comet3((cos(2*t).^2).*sin(t),(sin(2*t).^2).*cos(t),t);


% Crude approximations for the W function (used by bisect)
%
% Syntax:
% crude(xx,nb)
%
% xx is the argument, W(xx)
%
% nb is the branch of the W function needed:
% nb = 0 - upper branch, Wp
% nb <> 0 - lower branch, Wm
%
function c=crude(xx,nb)
%
% D. A. Barry, 23 June 2003 (d.a.barry@ed.ac.uk)
%
% Various constants
%
em=-exp(-1);
em9=-exp(-9);
c13=1/3;
em2=2/em;
s2=sqrt(2);
s21=2*s2-3;
s22=4-3*s2;
s23=s2-2;
if nb == 0
%
%  Calculations for crude Wp
%
   if xx <= 20
      reta=s2*sqrt(1-xx/em);
      an2=4.612634277343749*sqrt(sqrt(reta+1.09556884765625));
      c=reta/(1+reta/(3+(s21*an2+s22)*reta/(s23*(an2+reta))))-1;
   else
      zl=log(xx);
      c=log(xx/log(xx/zl^exp(-1.124491989777808/(.4225028202459761+zl))));
   end
else
%
%  Calculations for crude Wm
%
   if xx <= em9
      zl=log(-xx);
      t=-1-zl;
      ts=sqrt(t);
      c=zl-(2*ts)/(s2+(c13-t/(270+ts*127.0471381349219))*ts);
   else
      zl=log(-xx);
      eta=2-em2*xx;
      c=log(xx/log(-xx/((1-.5043921323068457*(zl+1))*(sqrt(eta)+eta/3)+1)));
   end
end


% computes bootstrap bias corrected accelerated bootstrap confidence
% interval and returns if it includes 0 as a logical vector sig
% mu_est - estimated parameter from complete sample
% bs_est - bootstrap estimates
% jk_est - jacknife estimates
% alpha - desired FPR
%
% Written by Bogdan Petre, 2020
function [sig, CI] = BCa(mu_est, bs_est, jk_est, alpha)
    fprintf('Estimating BCa CIs...\n')
    bias = sum(bs_est < mu_est)./length(bs_est);
    z0 = icdf('norm', bias, 0, 1);

    jk_mean = mean(jk_est);
    num = sum((jk_mean - jk_est).^3);
    den = sum((jk_mean - jk_est).^2);
    a = num ./ (6*den.^(3/2));

    zL = z0 + icdf('norm',alpha/2,0,1);
    alpha1 = normcdf(z0 + zL./(1-a.*zL));
    zU = z0 + icdf('norm',1-alpha/2,0,1);
    alpha2 = normcdf(z0 + zU./(1-a.*zL));

    CI = quantile(bs_est, [alpha1, alpha2]);

    sig = prod(sign(CI),2) > 0;
end


%STUMPC Decision stump classifier
%
%   W = STUMPC(A,CRIT,N)
%   W = A*STUMPC([],CRIT,N)
%   W = A*STUMPC(CRIT,N)
%
% Computation of a decision tree classifier out of a dataset A using
% a binary splitting criterion CRIT:
%   INFCRIT  -  information gain
%   MAXCRIT  -  purity (default)
%   FISHCRIT -  Fisher criterion
% Just N (default N=1) nodes are computed.
%
% see also DATASETS, MAPPINGS, TREEC, TREE_MAP

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

% $Id: stumpc.m,v 1.2 2009/07/10 11:19:20 duin Exp $

function w = stumpc(varargin)

	mapname = 'Decision Stump';
  argin = shiftargin(varargin,{'char','integer'});
  argin = shiftargin(argin,'integer',2);
  argin = setdefaults(argin,[],'maxcrit',1);

  if mapping_task(argin,'definition')
    w = define_mapping(argin,'untrained',mapname);

  elseif mapping_task(argin,'training')			% Train a mapping.

    [a,crit,n] = deal(argin{:});
    islabtype(a,'crisp');
    isvaldset(a,1,2); % at least 1 object per class, 2 classes

    % First get some useful parameters:
    [m,k,c] = getsize(a);
    nlab = getnlab(a);
    tree = maketree(+a,nlab,c,crit,n);

    % Store the results:
    w = prmapping('tree_map','trained',{tree,1},getlablist(a),k,c);
    w = setname(w,mapname);
    w = setcost(w,a);

  end

	return

%MAKETREE General tree building algorithm
%
% 	tree = maketree(A,nlab,c,crit,stop)
%
% Constructs a binary decision tree using the criterion function
% specified in the string crit ('maxcrit', 'fishcrit' or 'infcrit'
% (default)) for a set of objects A. stop is a counter for the number of
% branches that are allowed below the present.
%
% Definition of the resulting tree:
%
% 	tree(n,1) - feature number to be used in node n
% 	tree(n,2) - threshold t to be used
% 	tree(n,3) - node to be processed if value <= t
% 	tree(n,4) - node to be processed if value > t
% 	tree(n,5:4+c) - aposteriori probabilities for all classes in
% 			node n
%
% If tree(n,3) == 0, stop, class in tree(n,1)
%
% This is a low-level routine called by treec.
%
% See also infstop, infcrit, maxcrit, fishcrit and mapt.

% Authors: Guido te Brake, TWI/SSOR, Delft University of Technology
%     R.P.W. Duin, TN/PH, Delft University of Technology
% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Physics, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

function tree = maketree(a,nlab,c,crit,n)
		[m,k] = size(a);

	% Construct the tree:

	% When all objects have the same label, create an end-node:
	if all([nlab == nlab(1)])
		% Avoid giving 0-1 probabilities, but 'regularize' them a bit using
		% a 'uniform' Bayesian prior:
		p = ones(1,c)/(m+c); p(nlab(1)) = (m+1)/(m+c);
		tree = [nlab(1),0,0,0,p];
	else
		% now the tree is recursively constructed further:
		[f,j,t] = feval(crit,+a,nlab); % use desired split criterion

		p = sum(expandd(nlab),1);
		if length(p) < c, p = [p,zeros(1,c-length(p))]; end
		% When the stop criterion is not reached yet, we recursively split
		% further:

		if n >= 1
			% Make the left branch:
			J = find(a(:,j) <= t);
      tl = maketree(+a(J,:),nlab(J),c,crit,n-1);

      % Make the right branch:
			K = find(a(:,j) > t);
			tr = maketree(+a(K,:),nlab(K),c,crit,n-1);

      % Fix the node labelings before the branches can be 'glued'
			% together to a big tree:
			[t1,t2] = size(tl);
      tl = tl + [zeros(t1,2) tl(:,[3 4])>0 zeros(t1,c)];
			[t3,t4] = size(tr);
			tr = tr + (t1+1)*[zeros(t3,2) tr(:,[3 4])>0 zeros(t3,c)];

      % Make the complete tree: the split-node and the branches:
			tree= [[j,t,2,t1+2,(p+1)/(m+c)]; tl; tr];
		else
			% We reached the stop criterion, so make an end-node:
			[mt,cmax] = max(p);
			tree = [cmax,0,0,0,(p+1)/(m+c)];
		end

	end
	return

%MAXCRIT Maximum entropy criterion for best feature split.
%
% 	[f,j,t] = maxcrit(A,nlabels)
%
% Computes the value of the maximum purity f for all features over
% the data set A given its numeric labels. j is the optimum feature,
% t its threshold. This is a low level routine called for constructing
% decision trees.
%
% [1] L. Breiman, J.H. Friedman, R.A. Olshen, and C.J. Stone,
% Classification and regression trees, Wadsworth, California, 1984.

% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Physics, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

function [f,j,t] = maxcrit(a,nlab)
		[m,k] = size(a);
	c = max(nlab);
	% -variable T is an (2c)x k matrix containing:
	%      minimum feature values class 1
	%      maximum feature values class 1
	%      minimum feature values class 2
	%      maximum feature values class 2
	%            etc.
	% -variable R (same size) contains:
	%      fraction of objects which is < min. class 1.
	%      fraction of objects which is > max. class 1.
	%      fraction of objects which is < min. class 2.
	%      fraction of objects which is > max. class 2.
	%            etc.
	% These values are collected and computed in the next loop:
	T = zeros(2*c,k); R = zeros(2*c,k);
	for j = 1:c
		L = (nlab == j);
		if sum(L) == 0
			T([2*j-1:2*j],:) = zeros(2,k);
			R([2*j-1:2*j],:) = zeros(2,k);
		else
			T(2*j-1,:) = min(a(L,:),[],1);
			R(2*j-1,:) = sum(a < ones(m,1)*T(2*j-1,:),1);
			T(2*j,:) = max(a(L,:),[],1);
			R(2*j,:) = sum(a > ones(m,1)*T(2*j,:),1);
		end
	end
	% From R the purity index for all features is computed:
	G = R .* (m-R);
	% and the best feature is found:
	[gmax,tmax] = max(G,[],1);
	[f,j] = max(gmax);
	Tmax = tmax(j);
	if Tmax ~= 2*floor(Tmax/2)
		t = (T(Tmax,j) + max(a(find(a(:,j) < T(Tmax,j)),j)))/2;
	else
		t = (T(Tmax,j) + min(a(find(a(:,j) > T(Tmax,j)),j)))/2;
	end
	return

%INFCRIT The information gain and its the best feature split.
%
% 	[f,j,t] = infcrit(A,nlabels)
%
% Computes over all features the information gain f for its best
% threshold from the dataset A and its numeric labels. For f=1:
% perfect discrimination, f=0: complete mixture. j is the optimum
% feature, t its threshold. This is a lowlevel routine called for
% constructing decision trees.

% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Physics, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

function [g,j,t] = infcrit(a,nlab)
		[m,k] = size(a);
	c = max(nlab);
	mininfo = ones(k,2);
	% determine feature domains of interest
	[sn,ln] = min(a,[],1);
	[sx,lx] = max(a,[],1);
	JN = (nlab(:,ones(1,k)) == ones(m,1)*nlab(ln)') * realmax;
	JX = -(nlab(:,ones(1,k)) == ones(m,1)*nlab(lx)') * realmax;
	S = sort([sn; min(a+JN,[],1); max(a+JX,[],1); sx]);
	% S(2,:) to S(3,:) are interesting feature domains
	P = sort(a);
	Q = (P >= ones(m,1)*S(2,:)) & (P <= ones(m,1)*S(3,:));
	% these are the feature values in those domains
	for f=1:k,		% repeat for all features
		af = a(:,f);
		JQ = find(Q(:,f));
		SET = P(JQ,f)';
		if JQ(1) ~= 1
			SET = [P(JQ(1)-1,f), SET];
		end
		n = length(JQ);
		if JQ(n) ~= m
			SET = [SET, P(JQ(n)+1,f)];
		end
		n = length(SET) -1;
		T = (SET(1:n) + SET(2:n+1))/2; % all possible thresholds
		L = zeros(c,n); R = L;     % left and right node object counts per class
		for j = 1:c
			J = find(nlab==j); mj = length(J);
			if mj == 0
				L(j,:) = realmin*ones(1,n); R(j,:) = L(j,:);
			else
				L(j,:) = sum(repmat(af(J),1,n) <= repmat(T,mj,1)) + realmin;
				R(j,:) = sum(repmat(af(J),1,n) > repmat(T,mj,1)) + realmin;
			end
		end
		infomeas =  - (sum(L .* log10(L./(ones(c,1)*sum(L)))) ...
			       + sum(R .* log10(R./(ones(c,1)*sum(R))))) ...
		    ./ (log10(2)*(sum(L)+sum(R))); % criterion value for all thresholds
		[mininfo(f,1),j] = min(infomeas);     % finds the best
		mininfo(f,2) = T(j);     % and its threshold
	end
	g = 1-mininfo(:,1)';
	[finfo,j] = min(mininfo(:,1));		% best over all features
	t = mininfo(j,2);			% and its threshold
	return

%FISHCRIT Fisher's Criterion and its best feature split
%
% 	[f,j,t] = fishcrit(A,nlabels)
%
% Computes the value of the Fisher's criterion f for all features
% over the dataset A with given numeric labels. Two classes only. j
% is the optimum feature, t its threshold. This is a lowlevel
% routine called for constructing decision trees.

% Copyright R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Physics, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

function [f,j,t] = fishcrit(a,nlab)
		[m,k] = size(a);
	c = max(nlab);
	if c > 2
		error('Not more than 2 classes allowed for Fisher Criterion')
	end
	% Get the mean and variances of both the classes:
	J1 = find(nlab==1);
	J2 = find(nlab==2);
	u = (mean(a(J1,:),1) - mean(a(J2,:),1)).^2;
	s = std(a(J1,:),0,1).^2 + std(a(J2,:),0,1).^2 + realmin;
	% The Fisher ratio becomes:
	f = u ./ s;
	% Find then the best feature:
	[ff,j] = max(f);
	% Given the feature, compute the threshold:
	m1 = mean(a(J1,j),1);
	m2 = mean(a(J2,j),1);
	w1 = m1 - m2; w2 = (m1*m1-m2*m2)/2;
	if abs(w1) < eps % the means are equal, so the Fisher
			 % criterion (should) become 0. Let us set the thresold
			 % halfway the domain
			 t = (max(a(J1,j),[],1) + minc(a(J2,j),[],1)) / 2;
	else
		t = w2/w1;
	end
	return

%INFSTOP Quinlan's Chi-square test for early stopping
%
% 	crt = infstop(A,nlabels,j,t)
%
% Computes the Chi-square test described by Quinlan [1] to be used
% in maketree for forward pruning (early stopping) using dataset A
% and its numeric labels. j is the feature used for splitting and t
% the threshold.
%
% [1] J.R. Quinlan, Simplifying Decision Trees,
% Int. J. Man - Machine Studies, vol. 27, 1987, pp. 221-234.
%
% See maketree, treec, classt, prune

% Guido te Brake, TWI/SSOR, TU Delft.
% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Physics, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

function crt = infstop(a,nlab,j,t)
		[m,k] = size(a);
	c = max(nlab);
	aj = a(:,j);
	ELAB = expandd(nlab);
	L = sum(ELAB(aj <= t,:),1) + 0.001;
	R = sum(ELAB(aj > t,:),1) + 0.001;
	LL = (L+R) * sum(L) / m;
	RR = (L+R) * sum(R) / m;
	crt = sum(((L-LL).^2)./LL + ((R-RR).^2)./RR);
	return

%PRUNEP Pessimistic pruning of a decision tree
%
% 	tree = prunep(tree,a,nlab,num)
%
% Must be called by giving a tree and the training set a. num is the
% starting node, if omitted pruning starts at the root. Pessimistic
% pruning is defined by Quinlan.
%
% See also maketree, treec, mapt

% Guido te Brake, TWI/SSOR, TU Delft.
% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Physics, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

function tree = prunep(tree,a,nlab,num)
		if nargin < 4, num = 1; end;
	[N,k] = size(a);
	c = size(tree,2)-4;
	if tree(num,3) == 0, return, end;
	w = prmapping('treec','trained',{tree,num},[1:c]',k,c);
	ttt=tree_map(prdataset(a,nlab),w);
	J = testc(ttt)*N;
	EA = J + nleaves(tree,num)./2;   % expected number of errors in tree
	P = sum(expandd(nlab,c),1);     % distribution of classes
					%disp([length(P) c])
					[pm,cm] = max(P);     % most frequent class
					E = N - pm;     % errors if substituted by leave
					SD = sqrt((EA * (N - EA))/N);
					if (E + 0.5) < (EA + SD)	     % clean tree while removing nodes
						[mt,kt] = size(tree);
						nodes = zeros(mt,1); nodes(num) = 1; n = 0;
						while sum(nodes) > n;	     % find all nodes to be removed
							n = sum(nodes);
							J = find(tree(:,3)>0 & nodes==1);
							nodes(tree(J,3)) = ones(length(J),1);
							nodes(tree(J,4)) = ones(length(J),1);
						end
						tree(num,:) = [cm 0 0 0 P/N];
						nodes(num) = 0; nc = cumsum(nodes);
						J = find(tree(:,3)>0);% update internal references
						tree(J,[3 4]) = tree(J,[3 4]) - reshape(nc(tree(J,[3 4])),length(J),2);
						tree = tree(~nodes,:);% remove obsolete nodes
					else
						K1 = find(a(:,tree(num,1)) <= tree(num,2));
						K2 = find(a(:,tree(num,1)) >  tree(num,2));

						tree = prunep(tree,a(K1,:),nlab(K1),tree(num,3));
						tree = prunep(tree,a(K2,:),nlab(K2),tree(num,4));
					end
					return

%PRUNET Prune tree by testset
%
% 	tree = prunet(tree,a)
%
% The test set a is used to prune a decision tree.

% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Physics, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

function tree = prunet(tree,a)
		[m,k] = size(a);
	[n,s] = size(tree);
	c = s-4;
	erre = zeros(1,n);
	deln = zeros(1,n);
	w = prmapping('treec','trained',{tree,1},[1:c]',k,c);
	[f,lab,nn] = tree_map(a,w);  % bug, this works only if a is dataset, labels ???
	[fmax,cmax] = max(tree(:,[5:4+c]),[],2);
	nngood = nn([1:n]'+(cmax-1)*n);
	errn = sum(nn,2) - nngood;% errors in each node
	sd = 1;
	while sd > 0
		erre = zeros(n,1);
		deln = zeros(1,n);
		endn = find(tree(:,3) == 0)';	% endnodes
		pendl = max(tree(:,3*ones(1,length(endn)))' == endn(ones(n,1),:)');
		pendr = max(tree(:,4*ones(1,length(endn)))' == endn(ones(n,1),:)');
		pend = find(pendl & pendr);		% parents of two endnodes
		erre(pend) = errn(tree(pend,3)) + errn(tree(pend,4));
		deln = pend(find(erre(pend) >= errn(pend))); % nodes to be leaved
		sd = length(deln);
		if sd > 0
			tree(tree(deln,3),:) = -1*ones(sd,s);
			tree(tree(deln,4),:) = -1*ones(sd,s);
			tree(deln,[1,2,3,4]) = [cmax(deln),zeros(sd,3)];
		end
	end
	return

%NLEAVES Computes the number of leaves in a decision tree
%
% 	number = nleaves(tree,num)
%
% This procedure counts the number of leaves in a (sub)tree of the
% tree by using num. If num is omitted, the root is taken (num = 1).
%
% This is a utility used by maketree.

% Guido te Brake, TWI/SSOR, TU Delft
% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Physics, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

function number = nleaves(tree,num)
		if nargin < 2, num = 1; end
	if tree(num,3) == 0
		number = 1 ;
	else
		number = nleaves(tree,tree(num,3)) + nleaves(tree,tree(num,4));
	end
	return