% test for data interpolation using geodesic distances

rep  = 'data/';
rep  = '';

name = 'mm';
name = 'cavern';
name = 'disk';
name = 'toto';
name = 'boat';
name = 'cartoon';

clear options;
options.null = 0;

%% compute a binary shape
n = 300;
if strcmp(name, 'toto')
    M0 = load_image([rep name],n);
    M = sum(M0,3);
    M = 1-(M==M(1));
    % compute the constraint map
    CM = zeros(n) + Inf; CM(M==0) = -Inf;
    options.constraint_map = CM; % constraint the propagation inside the shape
    W = ones(n);
    mode = 'interpolation';
else
    n0 = [];
    if strcmp(name, 'disk')
        n0 = n;
    end
    if strcmp(name, 'cartoon')
        n0 = 400;
    end
    M = load_image([rep name],n0);
    M = rescale(crop(M,n)); M0 = M;
    M = rescale(sum(M,3));
    W = compute_edge_energy(M, 1.2);
    W = 1./rescale(W,0.001,1);
    mode = 'colorization';
end

% RGB2COL matrix
CM = rand(3); CM(:,1) = 1;
[CM,R] = qr(CM); CM(:,1) = 1/3; CM = CM';

%% pick some points
b = 1;
points = [];
while b==1
    clf;
    hold on;
    imagesc(rescale(M)); axis image; axis off;
    if not(isempty(points))
        plot_scattered(points(end:-1:1,:));
    end
    colormap gray(256); axis ij;
    hold off;
    [y,x,b] = ginput(1);
    if b==1
        points(:,end+1) = [x;y];
    end
end
npoints = size(points,2);
% random colors (sqrt to make it more colorfull
f = rand(3,npoints);
f = ( f ./ repmat(sum(f,1),[3, 1]) ).^(1);
% use fixed palette
fc = [1 0 0; 0 1 0; 0 0 1; 1 1 0; 1 0 1; 0 1 1; 0.2 0.7 1; 1 0.2 0.7; 0.7 1 0.2; 0.2 1 0.7]';
sel =  floor(rand(npoints,1)*6)+1;
f = fc(:,sel);

if strcmp(mode, 'colorization')
    % random CbCr
    f = CM*f; f = f(2:end,:);
end

%% perform interpolation
if strcmp(mode, 'colorization')
    sigma_list = 0.001/n;
    alpha_list = 3;
else
    % the smaller, the more interpolation is performed
    % the higher, the more diffusion it is
    sigma_list = [25 25 25 0.01 0.01 0.01]/n;
    % the higher, the more "voronoi"-like it is
    alpha_list = [1 3 50 1 3 50];
end

nsigma = length(sigma_list);
nrows = 1;
if nsigma>3
    nrows = 2;
end

clf;
for i=1:nsigma
    options.sigma = sigma_list(i);
    options.alpha = alpha_list(i);
    [A,G] = perform_geodesic_interpolation(W,points,f,options);

    if strcmp(mode, 'colorization')
        % cat the luminosity component
        A = cat(3, M,A);
        A = reshape(A, [n^2 3])';
        A = ( (CM^(-1)) * A )';
        A = reshape( A, [n n 3] );
    end
    A = clamp(A);
    subplot(nrows,ceil(nsigma/nrows), i);
    hold on;
    imagesc(rescale(A)); axis image; axis off; axis ij;
    plot_scattered(points(end:-1:1,:));
    hold off;
    title(['\alpha=' num2str(options.alpha) ', \sigma=' num2str(options.sigma)]);
end


function test_subplot_2D
%TEST_SUBPLOT_2D Test subplot 2D with DRAGZOOM

figure('Units', 'pixels');
x = -pi*2:0.1:pi*2;
y1 = sin(x);
y2 = cos(x);
y3 = sin(x).^3 - cos(x).^2;

hax1 = subplot(3, 1, 1);
plot(hax1, x, y1, '.-r')

hax2 = subplot(3, 1, 2);
plot(hax2, x, y2, 'o-b')

hax3 = subplot(3, 1, 3);
plot(hax3, x, y3, '*-g')
legend('test')

% dragzoom([hax1; hax3]); % manage only axes 1 and axes 3
dragzoom()


function determ = conex2_determinant ( alpha )

%*****************************************************************************80
%
%% CONEX2_DETERMINANT returns the determinant of the CONEX2 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ALPHA, the scalar defining A.
%
%    Output, real DETERM, the determinant.
%
  determ = 1.0 / alpha;

  return
end


function test_suite = test_daubcqf
initTestSuite;

function test_daubcqf_min
  [a, b] = daubcqf(4);
  ax = [0.482962913144534   0.836516303737808   0.224143868042013  -0.129409522551260];
  bx = [0.129409522551260   0.224143868042013  -0.836516303737808   0.482962913144534];
assertVectorsAlmostEqual(a, ax, 'relative', 0.001);
assertVectorsAlmostEqual(b, bx, 'relative', 0.001);

function test_daubcqf_max
  [a, b] = daubcqf(4, 'max');
  ax = [-0.129409522551260   0.224143868042013   0.836516303737808   0.482962913144534];
  bx = [-0.482962913144534   0.836516303737808  -0.224143868042013  -0.129409522551260];
assertVectorsAlmostEqual(a, ax, 'relative', 0.001);
assertVectorsAlmostEqual(b, bx, 'relative', 0.001);

function test_daubcqf_mid_even_k
  [a, b] = daubcqf(4, 'mid');
  ax = [0.482962913144534   0.836516303737808   0.224143868042013  -0.129409522551260];
  bx = [0.129409522551260   0.224143868042013  -0.836516303737808   0.482962913144534];
assertVectorsAlmostEqual(a, ax, 'relative', 0.001);
assertVectorsAlmostEqual(b, bx, 'relative', 0.001);

function test_daubcqf_mid_odd_k
  [a, b] = daubcqf(6, 'mid');
  ax = [0.332670552950083   0.806891509311093   0.459877502118491  -0.135011020010255  -0.085441273882027   0.035226291885710];
  bx = [-0.035226291885710  -0.085441273882027   0.135011020010255   0.459877502118491 -0.806891509311093   0.332670552950083];
assertVectorsAlmostEqual(a, ax, 'relative', 0.001);
assertVectorsAlmostEqual(b, bx, 'relative', 0.001);

function test_daubcqf_odd
  handle = @() daubcqf(9);
assertExceptionThrown(handle, '');


function s = ymdf_to_s_roman ( y, m, d, f )

%*****************************************************************************80
%
%% YMDF_TO_S_ROMAN writes a Roman YMDF date into a string.
%
%  Example:
%
%     Y  M   D  F    S
%    --  -  --  ---  -----------------------------------
%    56  4   1  0.1  Kalends Aprilis DVI AUC
%    56  4   2  0.2  Ante diem iv Nones Aprilis DVI AUC
%    56  4   3  0.3  Ante diem iii Nones Aprilis DVI AUC
%    56  4   4  0.4  Pridie Nones Aprilis DVI AUC
%    56  4   5  0.5  Nones Aprilis DVI AUC
%    56  4   6  0.6  Ante diem viii Ides Aprilis DVI AUC
%    56  4   7  0.7  Ante diem vii Ides Aprilis DVI AUC
%    56  4   8  0.8  Ante diem vi Ides Aprilis DVI AUC
%    56  4   9  0.9  Ante diem v Ides Aprilis DVI AUC
%    56  4  10  0.0  Ante diem iv Ides Aprilis DVI AUC
%    56  4  11  0.0  Ante diem iii Ides Aprilis DVI AUC
%    56  4  12  0.0  Pridie Ides Aprilis DVI AUC
%    56  4  13  0.0  Ides Aprilis DVI AUC
%    56  4  14  0.0  Ante diem xvii Kalends Maius DVI AUC
%    56  4  15  0.0  Ante diem xvi Kalends Maius DVI AUC
%    ...
%    56  4  28  0.0  Ante diem iv Kalends Maius DVI AUC
%    56  4  29  0.0  Ante diem iii Kalends Maius DVI AUC
%    56  4  30  0.0  Pridie Kalends Maius DVI AUC
%
%  Discussion:
%
%    "AUC" means "ab urbe condita", or "from the founding of the city".
%
%    At the moment, we ignore F.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, M, D, real F, the YMDF date.
%
%    Output, string S, a string representing the date.
%

%
%  Check the input.
%
  [ y2, m2, d2, ierror ] = ymd_check_roman ( y, m, d );

  if ( ierror ~= 0 )
    s = '?';
    return
  end

  s_month = month_to_month_name_roman ( m2 );
%
%  Get the next month's name.
%
  m3 = i4_wrap ( m2 + 1, 1, 12 );
  s_month_next = month_to_month_name_roman ( m3 );

  nones = month_to_nones_roman ( m2 );
  ides = month_to_ides_roman ( m2 );
  last = month_length_roman ( y2, m2 );

  if ( d2 == 1 )
    s = [ 'Kalends ', s_month ];
  elseif ( d2 < nones - 1 )
    jday = nones + 1 - d2;
    s_day = i4_to_roman ( jday );
    s = [ 'Ante diem ', s_day, ' Nones ', s_month ];
  elseif ( d2 == nones - 1 )
    s = [ 'Pridie Nones ', s_month ];
  elseif ( d2 == nones )
    s = [ 'Nones ', s_month ];
  elseif ( d2 < ides - 1 )
    jday = ides + 1 - d2;
    s_day = i4_to_roman ( jday );
    s = [ 'Ante diem ', s_day, ' Ides ', s_month ];
  elseif ( d2 == ides - 1 )
    s = [ 'Pridie Ides ', s_month ];
  elseif ( d2 == ides )
    s = [ 'Ides ', s_month ];
  elseif ( m2 == 2 && year_is_leap_roman ( y2 ) )

    if ( d2 < 25 )
      jday = last + 1 - d2;
      s_day = i4_to_roman ( jday );
      s = [ 'Ante diem ', s_day, ' Kalends ', s_month_next ];
    elseif ( d2 == 25 )
      jday = last + 2 - d2;
      s_day = i4_to_roman ( jday );
      s = [ 'Ante diem Bis ', s_day, ' Kalends ', s_month_next ];
    elseif ( d2 < last )
      jday = last + 2 - d2;
      s_day = i4_to_roman ( jday );
      s = [ 'Ante diem ', s_day, ' Kalends ', s_month_next ];
    else
      s = [ 'Pridie Kalends ', s_month_next ];
    end

  elseif ( d2 < last )
    jday = last + 2 - d2;
    s_day = i4_to_roman ( jday );
    s = [ 'Ante diem ', s_day, ' Kalends ', s_month_next ];
  else
    s = [ 'Pridie Kalends ', s_month_next ];
  end

  s_year = i4_to_roman ( y2 );
  s = [ s, ' ', s_year, ' AUC' ];

  s = s_blanks_delete ( s );

  return
end


function varargout = norm(varargin)
%NORM (overloaded)
%
% t = NORM(x,P)
%
% The variable t can only be used in convexity preserving
% operations such as t<0, max(t,y)<1, minimize t etc.
%
%    For matrices...
%      NORM(X) models the largest singular value of X, max(svd(X)).
%      NORM(X,2) is the same as NORM(X).
%      NORM(X,1) models the 1-norm of X, the largest column sum, max(sum(abs(X))).
%      NORM(X,inf) models the infinity norm of X, the largest row sum, max(sum(abs(X'))).
%      NORM(X,'fro') models the Frobenius norm, sqrt(sum(diag(X'*X))).
%    For vectors...
%      NORM(V) = norm(V,2) = standard Euclidean norm.
%      NORM(V,inf) = max(abs(V)).
%      NORM(V,1) = sum(abs(V))
%
% SEE ALSO SUMK, SUMABSK

%% ***************************************************
% This file defines a nonlinear operator for YALMIP
%
% It can take three different inputs
% For DOUBLE inputs, it returns standard double values
% For SDPVAR inputs, it generates an internal variable
%
% When first input is 'model' it returns the graph
% in the first output and structure describing some
% properties of the operator.

%% ***************************************************
switch class(varargin{1})

    case 'double' % What is the numerical value of this argument (needed for displays etc)
        % SHOULD NEVER HAPPEN, THIS SHOULD BE CAUGHT BY BUILT-IN
        error('Overloaded SDPVAR/NORM CALLED WITH DOUBLE. Report error')

    case 'sdpvar' % Overloaded operator for SDPVAR objects. Pass on args and save them.
        if nargin == 1
            varargout{1} = yalmip('addextendedvariable',mfilename,varargin{1},2);
        else
            switch varargin{2}
                case {1,2,inf,'inf','fro'}
                    varargout{1} = yalmip('addextendedvariable',mfilename,varargin{:});
                otherwise
                    error('norm(x,P) only supported for P = 1, 2, inf and ''fro''');
            end
        end

    case 'char' % YALMIP sends 'model' when it wants the epigraph or hypograph
        switch varargin{1}
            case 'graph'
                t = varargin{2};
                X = varargin{3};
                p = varargin{4};

                % Code below complicated by two things
                % 1: Absolute value for complex data -> cone constraints on
                %    elements
                % 2: SUBSREF does not call SDPVAR subsref -> use extsubsref.m

                % FIX : Exploit symmetry to create smaller problem
                switch p
                    case 1
                        z = sdpvar(size(X,1),size(X,2),'full');
                        if min(size(X))>1
                            if isreal(X)
                                F = (-z <= X <= z);
                            else
                                F = ([]);
                                for i = 1:size(X,1)
                                    for j = 1:size(X,2)
                                        xi = extsubsref(X,i,j);
                                        zi = extsubsref(z,i,j);
                                        F = F + (cone([real(xi);imag(xi)],zi));
                                    end
                                end
                            end
                            F = F + (sum(z,1) <= t);
                        else
                            if isreal(X)
                                F = (-z <= X <= z) + (sum(z) <= t);
                                [M,m] = derivebounds(X);
                                bounds(z,0,max(abs([M -m]),[],2));
                                bounds(t,0,sum(max(abs([M -m]),[],2)));
                            else
                                F = ([]);
                                for i = 1:length(X)
                                    xi = extsubsref(X,i);
                                    zi = extsubsref(z,i);
                                    F = F + (cone([real(xi);imag(xi)],zi));
                                end
                                F = F + (sum(z) <= t);
                            end
                        end
                    case 2
                        z = sdpvar(size(X,1),size(X,2));
                        if min(size(X))>1
                            F = ([t*eye(size(X,1)) X;X' t*eye(size(X,2))])>=0;
                        else
                            F = (cone(X(:),t));
                        end
                    case {inf,'inf'}
                        if min(size(X))>1
                            z = sdpvar(size(X,1),size(X,2),'full');
                            if isreal(X)
                                F = (-z <= X <= z);
                            else
                                F = ([]);
                                for i = 1:size(X,1)
                                    for j = 1:size(X,2)
                                        xi = extsubsref(X,i,j);
                                        zi = extsubsref(z,i,j);
                                        F = F + (cone([real(xi);imag(xi)],zi));
                                    end
                                end
                            end
                            F = F + (sum(z,2) <= t);
                        else
                            if isreal(X)
                                F = (-t <= X <= t);
                                [M,m,infbound] = derivebounds(X);
                                if ~infbound
                                    F = F + (0<=t<=max(M));
                                end
                            else
                                F = ([]);
                                for i = 1:length(X)
                                    xi = extsubsref(X,i);
                                    F = F + (cone([real(xi);imag(xi)],t));
                                end
                            end
                        end
                    case 'fro'
                        X.dim(1)=X.dim(1)*X.dim(2);
                        X.dim(2)=1;
                        F = (cone(X,t));
                    otherwise
                end
                varargout{1} = F;
                varargout{2} = struct('convexity','convex','monotonicity','none','definiteness','positive');
                varargout{3} = X;
            case 'milp'

                t = varargin{2};
                X = varargin{3};
                p = varargin{4};
                if ~isreal(X) | isequal(p,2) | isequal(p,'fro') | min(size(X))>1 % Complex valued data, matrices and 2-norm not supported
                    varargout{1} = [];
                    varargout{2} = [];
                    varargout{3} = [];
                else
                    if p==1
                        X     = reshape(X,length(X),1);
                        absX  = sdpvar(length(X),1);
                        d     = binvar(length(X),1);
                        [M,m] = derivebounds(X);
                         F = ([]);
                         positive = find(m >= 0);
                         negative = find(M <= 0);

                         % d(find(positive)) = 1;
                         % d(find(negative)) = 0;
                         if ~isempty(positive)
                             d = subsasgn(d,struct('type','()','subs',{{positive}}),1);
                         end
                         if ~isempty(negative)
                             d = subsasgn(d,struct('type','()','subs',{{negative}}),0);
                         end

                         F = F + (X <= M.*d)     + (2*m.*d     <= absX+X <= 2*M.*d);
                         F = F + (X >= m.*(1-d)) + (2*m.*(1-d) <= absX-X <= 2*M.*(1-d));
                         F = F + (t - sum(absX) == 0);

                    else

                        if 0

                            %2^n cases
                            %e.g in 2d,
                            %norm([x;y],inf) = y, y>0, y>x.y>-x
                            %                = x, x>0, x>y.x>-y

                            n = length(X);
                            X     = reshape(X,n,1);
                            absX  = sdpvar(n,1);
                            d     = binvar(n,1);
                            [M,m] = derivebounds(X);

                            F = (sum(d)==0);

                            top = 1;
                            for i = 1:n
                                xi = extsubsref(X,i);
                                y = extsubsref(X,setdiff(1:n,i));
                                for sign_abs_largest_variable = -1:2:1
                                    di = extsubsref(d,top);
                                    for j = setdiff(1:n,i)
                                        y = extsubsref(X,j);
                                        for sign_other = -1:2:1
                                            F = F + (xi*sign_abs_largest_variable >= sign_other*y);
                                            F = F + (-M*100*(1-di) <= xi*sign_abs_largest_variable-t <= t+M*100*(1-di));
                                        end
                                    end
                                    top = top + 1;
                                end
                            end

                        else
                            % OLD
                            n = length(X);
                            X     = reshape(X,n,1);
                            absX  = sdpvar(n,1);
                            d     = binvar(n,1);
                            [M,m] = derivebounds(X);
                            F = ([]);
                            F = F + (X <= M.*d)     + (2*m.*d     <= absX+X <= 2*M.*d);
                            F = F + (X >= m.*(1-d)) + (2*m.*(1-d) <= absX-X <= 2*M.*(1-d));
                            M = max(M,-m);
                            d = binvar(n,1);
                            F = F + (sum(d)==1);
                            F = F + (absX <= t <= absX + M.*(1-d));

                            kk = [];
                            ii = [];
                            for i = 1:n
                                k = [1:1:i-1 i+1:1:n]';
                                ii = [ii;repmat(i,n-1,1)];
                                kk = [kk;k];
                                Mm = M(k);
                            end
                            xii = extsubsref(absX,ii);
                            dii = extsubsref(d,ii);
                            xkk = extsubsref(absX,kk);
                            F = F + (xkk <= xii+(M(kk)-m(ii)).*(1-dii));
                        end

                        %   for i = 1:n
                        %       xi = extsubsref(absX,i);
                        %       di = extsubsref(d,i);
                        %       for k = [1:1:i-1 i+1:1:n]
                        %           xk = extsubsref(absX,k);
                        %           F = F + (xk <= xi+M(k)*(1-di));
                        %       end
                        %   end

                    end
                    varargout{1} = F;
                    varargout{2} = struct('convexity','milp','monotonicity','milp','definiteness','positive');
                    varargout{3} = X;
                end
            otherwise
                error('SDPVAR/NORM called with CHAR argument?');
        end
    otherwise
        error('Strange type on first argument in SDPVAR/NORM');
end


%GETSIZE Dataset size and number of classes
%
%  [M,K,C] = GETSIZE(A,DIM)
%
% INPUT
%   A    Dataset
%   DIM  1,2 or 3 : the number of the output argument to be returned
%
% OUTPUT
%   M    Number of objects
%   K    Number of features
%   C    Number of classes
%
% DESCRIPTION
% Returns size of the dataset A and the number of classes. C is determined
% from the number of labels stored in A.LABLIST. If DIM = 1,2 or 3, just
% one of these numbers is returned, e.g. C = GETSIZE(A,3).


function calpak_test013 ( )

%*****************************************************************************80
%
%% CALPAK_TEST013 tests JED_TO_YMDF_ARMENIAN and YMDF_TO_JED_ARMENIAN.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 March 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CALPAK_TEST013\n' );
  fprintf ( 1, '  For the Armenian calendar:\n' );
  fprintf ( 1, '  JED_TO_YMDF_ARMENIAN: JED -> YMDF.\n' );
  fprintf ( 1, '  YMDF_TO_JED_ARMENIAN: YMDF -> JED.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  JED (in)    YMDF               JED (out)\n' );
  fprintf ( 1, '\n' );

  jed_epoch = epoch_to_jed_armenian ( );

  i = 0;

  while ( 1 )

    i = i + 1;
    jed1 = jed_test ( i );

    if ( jed1 < 0.0 )
      break
    end

    if ( jed_epoch <= jed1 )

      [ y2, m2, d2, f2 ] = jed_to_ymdf_armenian ( jed1 );

      s2 = ymdf_to_s_numeric ( y2, m2, d2, f2 );

      jed3 = ymdf_to_jed_armenian ( y2, m2, d2, f2 );

      fprintf ( 1, '  %11.2f  %20s  %11.2f\n', jed1, s2, jed3 );

    end

  end

  return
end


function code = tree_rooted_code ( nnode, parent )

%*****************************************************************************80
%
%% TREE_ROOTED_CODE returns the code of a rooted tree.
%
%  Discussion:
%
%    This code for a rooted tree depends on the node ordering, so it's actually
%    the code for a labeled rooted tree.  To eliminate the effects of node
%    labeling, one could choose as the code for a tree the maximum of all
%    the codes associated with the different possible labelings of the tree.
%    There are more effective ways of arriving at this code than simply
%    generating all possible codes and comparing them.
%
%    For a tree with NNODES, the code is a list of 2*NNODE 0's and 1's,
%    describing a traversal of the tree starting at an imaginary node 0,
%    moving "down" to the root (a code entry of 1), and then moving
%    "down" (1) or "up" (0) as the tree is traversed in a depth first
%    manner.  The final move must be from the root up to the imaginary
%    node 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 June 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NNODE, the number of nodes.
%
%    Input, integer PARENT(NNODE), is the parent node of each node.
%    The node with parent 0 is the root.
%
%    Output, integer CODE(2*NNODE), the code for the tree.
%

%
%  Find the root.
%
  father = 0;
  for i = 1 : nnode
    if ( parent(i) == 0 )
      k = 1;
      code(1) = 1;
      father = i;
      break
    end
  end

  if ( father == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TREE_ROOTED_CODE - Fatal error!\n' );
    fprintf ( 1, '  Could not find the root.\n' );
    error ( 'TREE_ROOTED_CODE - Fatal error!' );
  end

  while ( father ~= 0 )

    k = k + 1;
    code(k) = 0;

    for son = 1 : nnode
      if ( parent(son) == father )
        code(k) = 1;
        father = son;
        break
      end
    end

    if ( code(k) == 0 )
      parent(father) = - parent(father);
      father = - parent(father);
    end

  end

  parent(1:nnode) = - parent(1:nnode);

  return
end


function [ less, equal, more ] = r8r8vec_index_search ( n, x, y, indx, ...
  xval, yval )

%*****************************************************************************80
%
%% R8R8VEC_INDEX_SEARCH searches for an R8R8 value in an indexed sorted list.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the size of the current list.
%
%    Input, real X(N), Y(N), the list.
%
%    Input, integer INDX(N), the sort index of the list.
%
%    Input, real XVAL, YVAL, the value to be sought.
%
%    Output, integer LESS, EQUAL, MORE, the indexes in INDX of the
%    entries of X that are just less than, equal to, and just greater
%    than XVAL.  If XVAL does not occur in X, then EQUAL is zero.
%    If XVAL is the minimum entry of X, then LESS is 0.  If XVAL
%    is the greatest entry of X, then MORE is N+1.
%
  if ( n <= 0 )
    less = 0;
    equal = 0;
    more = 0;
    return
  end

  lo = 1;
  hi = n;

  xlo = x(indx(lo));
  ylo = y(indx(lo));

  xhi = x(indx(hi));
  yhi = y(indx(hi));

  compare = r8r8_compare ( xval, yval, xlo, ylo );

  if ( compare == -1 )
    less = 0;
    equal = 0;
    more = 1;
    return
  elseif ( compare == 0 )
    less = 0;
    equal = 1;
    more = 2;
    return
  end

  compare = r8r8_compare ( xval, yval, xhi, yhi );

  if ( compare == 1 )
    less = n;
    equal = 0;
    more = n + 1;
    return
  elseif ( compare == 0 )
    less = n - 1;
    equal = n;
    more = n + 1;
    return
  end

  while ( 1 )

    if ( lo + 1 == hi )
      less = lo;
      equal = 0;
      more = hi;
      return
    end

    mid = round ( ( lo + hi ) / 2 );
    xmid = x(indx(mid));
    ymid = y(indx(mid));

    compare = r8r8_compare ( xval, yval, xmid, ymid );

    if ( compare == 0 )
      equal = mid;
      less = equal - 1;
      more = equal + 1;
      return
    elseif ( compare == -1 )
      hi = mid;
    elseif ( compare == +1 )
      lo = mid;
    end

  end

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%	SurfBox-MATLAB (c)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%%	Yue M. Lu and Minh N. Do
%%
%%	Department of Electrical and Computer Engineering
%%	Coordinated Science Laboratory
%%	University of Illinois at Urbana-Champaign
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%%	surf_vec2coeff.m
%%
%%	First created: 04-06-07
%%	Last modified: 04-06-07
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Y = surf_coeff2vec(c, size_info)

%% Convert the surfacelet coefficient stored in a linear vector to a nested
%% cell array.
%%
%% Input:
%%
%% c: a linear vector storing all the coefficients.
%%
%% size_info: a book keeping cell array storing the dimension information
%% of Y. This is useful when we want to convert c back to Y. See
%% surf_vec2coeff.m for details.
%%
%% Output:
%%
%% Y: an L+1 by 1 cell array containing the surfacelet coefficients. See
%% Surfdec.m for details
%%

%% we can inherit the nested structure
Y = size_info;

%% number of multiscale levels
L = length(Y) - 1;

Ncoeff = 1;

for n = 1 : L
    for m = 1 : length(Y{n})
        for k = 1 : length(Y{n}{m})
            sz = size_info{n}{m}{k};
            d = prod(sz);
            Y{n}{m}{k} = reshape(c(Ncoeff : Ncoeff + d - 1), sz);
            Ncoeff = Ncoeff + d;
        end
    end
end

%% the lowpass subband
sz = size_info{end};
Y{end} = reshape(c(Ncoeff : end), sz);


function plotProjectedAmps(view, scanList,projectionPhase)
%
% plotProjectedAmplitudes(view, [scanList])
%
% Bar plot of the amplitudes for each scan, averaging across
% all pixels (in all slices) in the current ROI.  The bar heights
% and a coarse SEM can be obtained from get(gca,'UserData').
%
% gmb  5/25/98
% bw   2/19/99  Added seY field to the UserData field.
%	    seY is an estimate of the variability in the
%      amplitudes.  It is the SEM of the in the complex
%      (amp*exp(-i*ph)) representation.  The values are
%      computed in vectorMean.m
% rfd  7/18/02 Added optional scanList.

if(~exist('scanList','var'))
    scanList = [1:numScans(view)];
end
if (~exist('projectionPhase','var'))
    computeProjPhase=1;
else
    computeProjPhase=0;
end
% Compute means across scans, for all pixels in the
% currently selected ROI.  The seZ value is the mean
% distance from the mean.
[meanAmps,meanPhs,seZ] = vectorMeans(view);

%Reference scan is the current scan
refScan = getCurScan(view);

% Compute the amplitude projected onto the reference phase
if (computeProjPhase)
    projectionPhase=meanPhs(refScan);
end
meanProjectedAmps = meanAmps.*cos(meanPhs-projectionPhase);

selectGraphWin;

% Header
ROIname = view.ROIs(view.selectedROI).name;
headerStr = ['Mean of projected amplitudes, ROI ',ROIname];
set(gcf,'Name',headerStr);

%plot the bar graph
clf
fontSize = 14;
h = mybar(meanProjectedAmps(scanList), zeros(size(scanList)), num2str(scanList'));
xlabel('Scan','FontSize',fontSize);
ylabel('Mean Projected Amplitude','FontSize',fontSize);
ylim =get(gca,'YLim');
set(gca,'YLim',ylim*1.1);
set(gca,'FontSize',fontSize);

foo = cell2struct(h,'bar');
hbar = foo(find(scanList==refScan)).bar;
set(hbar,'FaceColor','r')

%Save the data in gca('UserData')
data.y = meanProjectedAmps;
data.refScan = refScan;
data.seY = seZ;
set(gca,'UserData',data);

return;


function [ r, s1, s2, s3 ] = r8_random ( s1, s2, s3 )

%*****************************************************************************80
%
%% R8_RANDOM returns a pseudorandom number between 0 and 1.
%
%  Discussion:
%
%    This function returns a pseudo-random number rectangularly distributed
%    between 0 and 1.   The cycle length is 6.95E+12.  (See page 123
%    of Applied Statistics (1984) volume 33), not as claimed in the
%    original article.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 July 2008
%
%  Author:
%
%    Original FORTRAN77 version by Brian Wichman, David Hill.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Brian Wichman, David Hill,
%    Algorithm AS 183: An Efficient and Portable Pseudo-Random
%    Number Generator,
%    Applied Statistics,
%    Volume 31, Number 2, 1982, pages 188-190.
%
%  Parameters:
%
%    Input, integer S1, S2, S3, three values used as the
%    seed for the sequence.  These values should be positive
%    integers between 1 and 30,000.
%
%    Output, real R, the next value in the sequence.
%
%    Output, integer S1, S2, S3, updated seed values.
%
  s1 = mod ( 171 * s1, 30269 );
  s2 = mod ( 172 * s2, 30307 );
  s3 = mod ( 170 * s3, 30323 );

  r = mod ( s1 / 30269.0 ...
          + s2 / 30307.0 ...
          + s3 / 30323.0, 1.0 );

  return
end


function points = mergeClosePoints(points, varargin)
%MERGECLOSEPOINTS Merge points that are closer than a given distance.
%
%   PTS2 = mergeClosePoints(PTS, DIST)
%   Remove points in the array PTS such that no points closer than the
%   distance DIST remain in the array.
%
%   PTS2 = mergeClosePoints(PTS)
%   If the distance is not specified, the default value 1e-14 is used.
%
%
%   Example
%     pts = rand(200, 2);
%     pts2 = mergeClosePoints(pts, .1);
%     figure; drawPoint(pts, '.');
%     hold on; drawPoint(pts2, 'mo');
%
%   See also
%     points2d, removeMultipleVertices

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2013-10-04, using Matlab 7.9.0.529 (R2009b)
% Copyright 2013-2022 INRA - Cepia Software Platform

% default values
minDist = 1e-14;
if ~isempty(varargin)
    minDist = varargin{1};
end

i = 1;
while i < size(points, 1)
    dist = distancePoints(points(i,:), points);
    inds = dist < minDist;
    inds(i) = 0;

    points(inds, :) = [];

    % switch to next point
    i = i + 1;
end


function [V, S, varexp, w, Yhat] = plssquash(X, Y, varargin)
% Decomposes data X into K components that are ordered in their
% covariance with Y, designed to predict orthogonal parts of Y
%
% :Usage:
% ::
%
%     [V, S, varexp, w, Yhat] = plssquash(X, Y, varargin)
%
% :Optional Inputs:
%   - case {'noplot'}, turn off plotting
%   - case 'ndims', save only first ndims (K) vectors
%
% :Outputs:
%
%   **V:**
%        'eigenvetors', or weights, on data (columns)
%
%   **S:**
%        score matrix, N x K
%
%   **varexp:**
%        sqrt(r-square) with first k components predicting Y
%
%   **w:**
%        V*b, integrated weights.  for predicting new data, pred = X*w
%
%   **Yhat:**
%        X*V*b, or S*b
%
% ..
%    Tor Wager, 9/12/09
% ..

doplot = 1;
ndims = length(Y) - 1;

for i = 1:length(varargin)
    if ischar(varargin{i})
        switch varargin{i}
            case {'noplot'}, doplot = 0;
            case 'ndims', ndims = varargin{i + 1};

            otherwise, warning(['Unknown input string option:' varargin{i}]);
        end
    end
end

% univariate covariance-based weights
% imperfect prediction
X = X - repmat(mean(X), size(X, 1), 1); %scale(X);
Y = scale(Y);

%
% neither V nor S appears to have orthogonal columns, though many are
% later columns, as df in Y is approached, appear to be highly colinear
% this may be because all variance in Y is basically explained...

clear V  % V is voxel weights for each component 1:K
clear S
rY = Y; % Y values to successively predict; intialize to Y

for i = 1:ndims  %ceil(length(Y)./2)

    % V = voxel weights, based on univariate relationship with rY
    V(:,i) = (X' * rY); %.^ .5;
    V(:,i) = V(:,i) ./ norm(V(:,i));

    % score matrix, N x K
    % these will ultimately be predictors for Y
    % chosen to maximize predictive power with few orthogonal components
    S = X*V;

    rY = Y - S * pinv(S) * Y;

    b = pinv(S) * Y;
    Yhat = S * b;

    rsq(i) = 1 - var(rY) ./ var(Y);

    if doplot
        create_figure('Fit'); plot(Yhat, Y, 'kx'); refline;

        rsq_adj(i) = 1 - (1-rsq(i)) * ((size(Y,1)-1) ./ (size(Y,1)-i-1));

        title(sprintf('cum. r-squared = %3.1f, adj = %3.1f', 100*rsq(i), 100*rsq_adj(i)));
        drawnow; pause(.3)
    end

end

varexp = sqrt(rsq);

b = pinv(S) * Y;
Yhat = S * b;
w = V * b;  % final weight vector

%prediction = mean(training) + data*S*b
%Yhat = X * V * b;
%figure; plot(Yhat, Y, 'kx')

end


function [Pgood,Pbad] = NDS(Population,K)
% Sort the population based on non-dominated sorting and crowding distance

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    FrontNo  = NDSort(Population.objs,inf);
    CrowdDis = CrowdingDistance(Population.objs,FrontNo);
    [~,rank] = sortrows([FrontNo;-CrowdDis]');
    Pgood    = Population(rank(1:K));
    Pbad     = Population(rank(end-K+1:end));
end


function [ t, rank ] = subset_colex_successor ( n, t, rank )

%*****************************************************************************80
%
%% SUBSET_COLEX_SUCCESSOR computes the subset colexicographic successor.
%
%  Discussion:
%
%    In the original code, there is a last element with no successor.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 August 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer N, the number of elements in the master set.
%    N must be positive.
%
%    Input/output, integer T(N), describes a subset.  T(I) is 0 if
%    the I-th element of the master set is not in the subset, and is
%    1 if the I-th element is part of the subset.
%    On input, T describes a subset.
%    On output, T describes the next subset in the ordering.
%    If the input T was the last in the ordering, then the output T
%    will be the first.
%
%    Input/output, integer RANK, the rank.
%    If RANK = -1 on input, then the routine understands that this is
%    the first call, and that the user wishes the routine to supply
%    the first element in the ordering, which has RANK = 0.
%    In general, the input value of RANK is increased by 1 for output,
%    unless the very last element of the ordering was input, in which
%    case the output value of RANK is 0.
%

%
%  Return the first element.
%
  if ( rank == -1 )
    t(1:n) = 0;
    rank = 0;
    return
  end
%
%  Check.
%
  subset_check ( n, t );

  for i = 1 : n

    if ( t(i) == 0 )
      t(i) = 1;
      rank = rank + 1;
      return
    else
      t(i) = 0;
    end

  end

  rank = 0;

  return
end


function c = tapas_hgf_ar1_binary_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuration for the Hierarchical Gaussian Filter (HGF) for AR(1) processes
% for binary inputs in the absence of perceptual uncertainty.
%
% The HGF is the model introduced in
%
% Mathys C, Daunizeau J, Friston, KJ, and Stephan KE. (2011). A Bayesian foundation
% for individual learning under uncertainty. Frontiers in Human Neuroscience, 5:39.
%
% The binary HGF model has since been augmented with a positive factor kappa1 which
% scales the second level with respect to the first, i.e., the relation between the
% first and second level is
%
% p(x1=1|x2) = s(kappa1*x2), where s(.) is the logistic sigmoid.
%
% By default, kappa1 is fixed to 1, leading (apart from the AR(1) process) to the
% model introduced in Mathys et al. (2011).
%
% This file refers to BINARY inputs (Eqs 1-3 in Mathys et al., (2011));
% for continuous inputs, refer to tapas_hgf_config.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The HGF configuration consists of the priors of parameters and initial values. All priors are
% Gaussian in the space where the quantity they refer to is estimated. They are specified by their
% sufficient statistics: mean and variance (NOT standard deviation).
%
% Quantities are estimated in their native space if they are unbounded (e.g., the omegas). They are
% estimated in log-space if they have a natural lower bound at zero (e.g., the sigmas).
%
% The phis are estimated in 'logit space' because they are confined to the interval from 0 to 1.
% 'Logit-space' is a logistic sigmoid transformation of native space with a variable upper bound
% a>0:
%
% tapas_logit(x) = ln(x/(a-x)); x = a/(1+exp(-tapas_logit(x)))
%
% Parameters can be fixed (i.e., set to a fixed value) by setting the variance of their prior to
% zero. Aside from being useful for model comparison, the need for this arises whenever the scale
% and origin at the j-th level are arbitrary. This is the case if the observation model does not
% contain the representations mu_j and sigma_j. A choice of scale and origin is then implied by
% fixing the initial value mu_j_0 of mu_j and either kappa_j-1 or omega_j-1.
%
% Fitted trajectories can be plotted by using the command
%
% >> tapas_hgf_binary_plotTraj(est)
%
% where est is the stucture returned by tapas_fitModel. This structure contains the estimated
% perceptual parameters in est.p_prc and the estimated trajectories of the agent's
% representations (cf. Mathys et al., 2011). Their meanings are:
%
%         est.p_prc.mu_0       row vector of initial values of mu (in ascending order of levels)
%         est.p_prc.sa_0       row vector of initial values of sigma (in ascending order of levels)
%         est.p_prc.phi        row vector of phis (representing reversion slope to attractor; in ascending order of levels)
%         est.p_prc.m        row vector of ms (representing attractors; in ascending order of levels)
%         est.p_prc.ka         row vector of kappas (in ascending order of levels)
%         est.p_prc.om         row vector of omegas (in ascending order of levels)
%
% Note that the first entry in all of the row vectors will be NaN because, at the first level,
% these parameters are either determined by the second level (mu_0 and sa_0) or undefined (rho,
% kappa, and omega).
%
%         est.traj.mu          mu (rows: trials, columns: levels)
%         est.traj.sa          sigma (rows: trials, columns: levels)
%         est.traj.muhat       prediction of mu (rows: trials, columns: levels)
%         est.traj.sahat       precisions of predictions (rows: trials, columns: levels)
%         est.traj.v           inferred variance of random walk (rows: trials, columns: levels)
%         est.traj.w           weighting factors (rows: trials, columns: levels)
%         est.traj.da          volatility prediction errors  (rows: trials, columns: levels)
%         est.traj.ud          updates with respect to prediction  (rows: trials, columns: levels)
%         est.traj.psi         precision weights on prediction errors  (rows: trials, columns: levels)
%         est.traj.epsi        precision-weighted prediction errors  (rows: trials, columns: levels)
%         est.traj.wt          full weights on prediction errors (at the first level,
%                                  this is the learning rate) (rows: trials, columns: levels)
%
% Note that in the absence of sensory uncertainty (which is the assumption here), the first
% column of mu, corresponding to the first level, will be equal to the inputs. Likewise, the
% first column of sa will be 0 always.
%
% Tips:
% - When analyzing a new dataset, take your inputs u and responses y and use
%
%   >> est = tapas_fitModel(y, u, 'tapas_hgf_binary_config', 'tapas_bayes_optimal_binary_config');
%
%   to determine the Bayes optimal perceptual parameters (given your current priors as defined in
%   this file here, so choose them wide and loose to let the inputs influence the result). You can
%   then use the optimal parameters as your new prior means for the perceptual parameters.
%
% - If you get an error saying that the prior means are in a region where model assumptions are
%   violated, lower the prior means of the omegas, starting with the highest level and proceeding
%   downwards.
%
% - Alternatives are lowering the prior means of the kappas, if they are not fixed, or adjusting
%   the values of the kappas or omegas, if any of them are fixed.
%
% - If the log-model evidence cannot be calculated because the Hessian poses problems, look at
%   est.optim.H and fix the parameters that lead to NaNs.
%
% - Your guide to all these adjustments is the log-model evidence (LME). Whenever the LME increases
%   by at least 3 across datasets, the adjustment was a good idea and can be justified by just this:
%   the LME increased, so you had a better model.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2017 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Config structure
c = struct;

% Model name
c.model = 'tapas_hgf_ar1_binary';

% Number of levels (minimum: 3)
c.n_levels = 3;

% Input intervals
% If input intervals are irregular, the last column of the input
% matrix u has to contain the interval between inputs k-1 and k
% in the k-th row, and this flag has to be set to true
c.irregular_intervals = false;

% Sufficient statistics of Gaussian parameter priors

% Initial mus and sigmas
% Format: row vectors of length n_levels
% For all but the first two levels, this is usually best
% kept fixed to 1 (determines origin on x_i-scale). The
% first level is NaN because it is determined by the second,
% and the second implies neutrality between outcomes when it
% is centered at 0.
c.mu_0mu = [NaN, 0, 1];
c.mu_0sa = [NaN, 0, 0];

c.logsa_0mu = [NaN, log(0.006), log(4)];
c.logsa_0sa = [NaN,          0,      0];

% Phis
% Format: row vector of length n_levels.
% Undefined (therefore NaN) at the first level.
% Fix this to zero (-Inf in logit space) to set to zero.
c.logitphimu = [NaN, -Inf, tapas_logit(0.1,1)];
c.logitphisa = [NaN,    0,                  2];

% ms
% Format: row vector of length n_levels.
% This should be fixed for all levels where the omega of
% the next lowest level is not fixed because that offers
% an alternative parametrization of the same model.
c.mmu = [NaN, c.mu_0mu(2), c.mu_0mu(3)];
c.msa = [NaN,           0,           1];

% Kappas
% Format: row vector of length n_levels-1.
% Fixing log(kappa1) to log(1) leads to the original HGF model.
% Higher log(kappas) should be fixed (preferably to log(1)) if the
% observation model does not use mu_i+1 (kappa then determines the
% scaling of x_i+1).
c.logkamu = [log(1), log(1)];
c.logkasa = [     0,      0];

% Omegas
% Format: row vector of length n_levels.
% Undefined (therefore NaN) at the first level.
c.ommu = [NaN,  -2,  -6];
c.omsa = [NaN, 4^2, 4^2];

% Gather prior settings in vectors
c.priormus = [
    c.mu_0mu,...
    c.logsa_0mu,...
    c.logitphimu,...
    c.mmu,...
    c.logkamu,...
    c.ommu,...
         ];

c.priorsas = [
    c.mu_0sa,...
    c.logsa_0sa,...
    c.logitphisa,...
    c.msa,...
    c.logkasa,...
    c.omsa,...
         ];

% Check whether we have the right number of priors
expectedLength = 5*c.n_levels+(c.n_levels-1);
if length([c.priormus, c.priorsas]) ~= 2*expectedLength;
    error('tapas:hgf:PriorDefNotMatchingLevels', 'Prior definition does not match number of levels.')
end

% Model function handle
c.prc_fun = @tapas_hgf_ar1_binary;

% Handle to function that transforms perceptual parameters to their native space
% from the space they are estimated in
c.transp_prc_fun = @tapas_hgf_ar1_binary_transp;

return;


function [prob,sol,fmin] = nls_prob(varargin)
%NLS_PROB  Return an OPTI NLS
%
%   prob = nls_prob(no) return a pre-built optiprob of a saved NLS.
%
%   [prob,sol,fmin] = NLS_prob(no) returns the optimum solution and function
%   eval at the optimum
%
%   no = nls_prob() returns the number of problems available for testing.

%   (C) 2012 Jonathan Currie (IPL)

% Functions are taken from:
% More, J. J., B. S. Garbow, and K. E. Hillstrom. "Testing Unconstrained
% Optimization Software." ACM Transactions on Mathematical Software 7,
% no. 1 (1981): 17-41.

%Check if just returning no problems
if(nargin < 1)
    prob = 20; sol = []; fmin = [];
    return;
else
    no = varargin{1};
end

%Big switch yard
switch(no)
    case 1
        fun = @(x) [10*(x(2)-x(1)^2); 1 - x(1)];
        ydata = zeros(2,1);
        x0 = [-1.2;1];
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [1,1];
        fmin = 0;

    case 2
        fun = @(x) [-13 + x(1) + ((5 - x(2))*x(2) - 2)*x(2);
                    -29 + x(1) + ((x(2) + 1)*x(2) - 14)*x(2)];
        ydata = zeros(2,1);
        x0 = [0.5;-2];
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [5,4];
        fmin = 0;

    case 3
        fun = @(x) [1e4*x(1)*x(2) - 1;
                    exp(-x(1)) + exp(-x(2)) - 1.0001];
        ydata = zeros(2,1);
        x0 = [0;1];
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [1.098e-5;9.106];
        fmin = 0;

    case 4
        fun = @(x) [x(1) - 1e6; x(2) - 2e-6; x(1)*x(2) - 2];
        ydata = zeros(3,1);
        x0 = [1;1];
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [1e6,2e-6];
        fmin = 0;

    case 5
        fun = @(x) [1.5 - x(1)*(1 - x(2)); 2.25 - x(1)*(1 - x(2)^2); 2.625 - x(1)*(1 - x(2)^3) ];
        ydata = zeros(3,1);
        x0 = [1;1];
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [3;0.5];
        fmin = 0;

    case 6
        i = (1:10)';
        fun = @(x) 2 + 2*i - (exp(x(1)*i) + exp(x(2)*i));
        ydata = zeros(10,1);
        x0 = [0.3;0.4];
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [0.2578;0.2578];
        fmin = 124.36226865;

    case 7
        fun = @more7;
        ydata = zeros(3,1);
        x0 = [-1;0;0];
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [1;0;0];
        fmin = 0;

    case 8
        u = (1:15)';
        v = 16 - u;
        w = zeros(15,1);
        for i = 1:15
            w(i) = min(u(i),v(i));
        end
        y = [0.14;0.18;0.22;0.25;0.29;0.32;0.35;0.39;0.37;0.58;0.73;0.96;1.34;2.10;4.39];
        fun = @(x) (x(1) + u ./ (v*x(2) + w*x(3)));
        x0 = [1;1;1];
        prob = optiprob('fun',fun,'ydata',y,'x0',x0);
        sol = [];
        fmin = 8.21487e-3;

    case 9
        i = (1:15)';
        y = [0.0009;0.0044;0.0175;0.0540;0.1295;0.2420;0.3521;0.3989;0.3521;0.2420;0.1295;0.0540;0.0175;0.0044;0.0009];
        t = (8 - i)./2;
        fun = @(x) x(1)*exp((-x(2)*(t - x(3)).^2)./2);
        x0 = [0.4;1;0];
        prob = optiprob('fun',fun,'ydata',y,'x0',x0);
        sol = [];
        fmin = 1.12793e-8;

    case {10,11}
        i = (1:16)';
        y = [34780;28610;23650;19630;16370;13720;11540;9744;8261;7030;6005;5147;4427;3820;3307;2872];
        t = 45 + 5*i;
        fun = @(x) x(1)*exp(x(2)./(t + x(3)));
        x0 = [0.02;4000;250];
        prob = optiprob('fun',fun,'ydata',y,'x0',x0);
        sol = [];
        fmin = 87.9458;

%     case 11 %not working??
%         m = 100;
%         i = [1:m]';
%         t = i./100;
%         y = 25 + (-50 * log(t)).^(2/3);
%         fun = @(x) exp(-(abs(y*100.*i*x(2)).^x(3))./x(1)) - t;
%         x0 = [5;2.5;0.15];
%         ydata = zeros(m,1);
%         prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
%         sol = [50;25;1.5];
%         fmin = 0;

    case 12
        m = 3;
        i = (1:m)';
        t = 0.1*i;
        fun = @(x) exp(-t*x(1)) - exp(-t*x(2)) - x(3)*exp(-t) - exp(-10*t);
        x0 = [0;10;20];
        ydata = zeros(m,1);
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [1;10;1];
        fmin = 0;

    case 13
        fun = @(x) [x(1) + 10*x(2);
                    sqrt(5)*(x(3) - x(4));
                    (x(3) - 2*x(3))^2;
                    sqrt(10)*(x(1)-x(4))^2];
        x0 = [3;-1;0;1];
        ydata = zeros(4,1);
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [0;0;0;0];
        fmin = 0;

    case 14
        fun = @(x) [10*(x(2)-x(1)^2);
                    1 - x(1);
                    sqrt(90)*(x(4)-x(3)^2);
                    1 - x(3);
                    sqrt(10)*(x(2) + x(4) - 2);
                    10^(-0.5)*(x(2)-x(4))];
        x0 = [3;-1;-3;-1];
        ydata = zeros(6,1);
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [1;1;1;1];
        fmin = 0;

    case 15
        y = [0.1957;0.1947;0.1735;0.16;0.0844;0.0627;0.0456;0.0342;0.0323;0.0235;0.0246];
        u = [4;2;1;0.5;0.25;0.167;0.125;0.1;0.0833;0.0714;0.0625];
        fun = @(x) (x(1)*(u.^2 + u*x(2))) ./ (u.^2 + u*x(3) + x(4));
        x0 = [0.25;0.39;0.415;0.39];
        prob = optiprob('fun',fun,'ydata',-y,'x0',x0);
        sol = [];
        fmin = 3.07505e-4;

    case 16
        m = 20;
        i = (1:m)';
        t = i/5;
        fun = @(x) (x(1) + t*x(2) - exp(t)).^2 + (x(3) + x(4)*sin(t) - cos(t)).^2;
        x0 = [25;5;-5;-1];
        ydata = zeros(m,1);
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [];
        fmin = 85822.2016;

    case 17
        m = 33;
        i = (1:m)';
        t = 10*(i - 1);
        y = [0.844;0.908;0.932;0.936;0.925;0.908;0.881;0.850;0.818;0.784;0.751;0.718;0.685;0.658;0.628;0.603;...
             0.58;0.558;0.538;0.522;0.506;0.490;0.478;0.467;0.457;0.448;0.438;0.431;0.424;0.42;0.414;0.411;0.406];
        fun = @(x) (x(1) + x(2)*exp(-t*x(4)) + x(3)*exp(-t*x(5)));
        x0 = [0.5;1.5;-1;0.01;0.02];
        prob = optiprob('fun',fun,'ydata',-y,'x0',x0);
        sol = [];
        fmin = 5.46489e-5;

    case 18
        m = 13;
        i = (1:m)';
        t = 0.1*i;
        y = exp(-t) - 5*exp(-10*t) + 3*exp(-4*t);
        fun = @(x) x(3)*exp(-t*x(1)) - x(4)*exp(-t*x(2)) + x(6)*exp(-t*x(5));
        x0 = [1;2;1;1;1;1];
        prob = optiprob('fun',fun,'ydata',y,'x0',x0);
        sol = [];
        fmin = 0; %5.65565e-3; seems wrong?

    case 19
        m = 65;
        i = (1:m)';
        t = (i-1)/10;
        y = [1.366;1.191;1.112;1.013;0.991;0.885;0.831;0.847;0.786;0.725;0.746;0.679;0.608;0.655;0.616;0.606;0.602;0.626;0.651;0.724;0.649;0.649;...
            0.694;0.644;0.624;0.661;0.612;0.558;0.533;0.495;0.5;0.423;0.395;0.375;0.372;0.391;0.396;0.405;0.428;0.429;0.523;0.562;0.607;0.653;...
            0.672;0.708;0.633;0.668;0.645;0.632;0.591;0.559;0.597;0.625;0.739;0.710;0.729;0.720;0.636;0.581;0.428;0.292;0.162;0.098;0.054];
        fun = @(x) (x(1)*exp(-t*x(5)) + x(2)*exp(-(t-x(9)).^2*x(6)) + x(3)*exp(-(t-x(10)).^2*x(7)) + x(4)*exp(-(t-x(11)).^2*x(8)));
        x0 = [1.3;0.65;0.65;0.7;0.6;3;5;7;2;4.5;5.5];
        prob = optiprob('fun',fun,'ydata',y,'x0',x0);
        sol = [];
        fmin = 4.01377e-2;

    case 20
        m = 31; n = 12;
        fun = @more20;
        x0 = zeros(n,1);
        ydata = zeros(m,1);
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [];
        fmin = 4.72238e-10;

    otherwise
        error('Problem not available or not implemented yet');
end

function j = more7(x)

if(x(1) > 0)
    theta = 1/(2*pi)*atan(x(2)/x(1));
else
    theta = 1/(2*pi)*atan(x(2)/x(1)) + 0.5;
end

j(1,1) = 10*(x(3) - 10*theta);
j(2,1) = 10*(x(1)^2 + x(2)^2)^0.5 - 1;
j(3,1) = x(3);

function f = more20(x)
n = length(x);
j = (2:n)';
j2 = (1:n)';
t = 0.1*(1:29)';
f = zeros(31,1);
for k = 1:29
    f(k) = sum( (j-1).*x(2:end).*t(k).^(j-2) ) - sum( x.*t(k).^(j2-1) ) - 1;
end
f(30) = x(1);
f(31) = x(2) - x(1)^2 - 1;


function [blockVectorX,lambda,varargout] = ...
    lobpcg(blockVectorX,operatorA,varargin)
%LOBPCG solves Hermitian partial eigenproblems using preconditioning
%
% [blockVectorX,lambda]=lobpcg(blockVectorX,operatorA)
%
% outputs the array of algebraic smallest eigenvalues lambda and
% corresponding matrix of orthonormalized eigenvectors blockVectorX of the
% Hermitian (full or sparse) operator operatorA using input matrix
% blockVectorX as an initial guess, without preconditioning, somewhat
% similar to
%
% opts.issym=1;opts.isreal=1;K=size(blockVectorX,2);
% [blockVectorX,lambda]=eigs(operatorA,K,'SR',opts);
%
% for real symmetric operator operatorA, or
%
% K=size(blockVectorX,2);[blockVectorX,lambda]=eigs(operatorA,K,'SR');
% for Hermitian operator operatorA.
%
% [blockVectorX,lambda,failureFlag]=lobpcg(blockVectorX,operatorA)
% also returns a convergence flag.
% If failureFlag is 0 then all the eigenvalues converged; otherwise not all
% converged.
%
% [blockVectorX,lambda,failureFlag,lambdaHistory,residualNormsHistory]=...
% lobpcg(blockVectorX,'operatorA','operatorB','operatorT',blockVectorY,...
% residualTolerance,maxIterations,verbosityLevel);
%
% computes smallest eigenvalues lambda and corresponding eigenvectors
% blockVectorX of the generalized eigenproblem Ax=lambda Bx, where
% Hermitian operators operatorA and operatorB are given as functions, as
% well as a preconditioner, operatorT. The operators operatorB and
% operatorT must be in addition POSITIVE DEFINITE. To compute the largest
% eigenpairs of operatorA, simply apply the code to operatorA multiplied by
% -1. The code does not involve ANY matrix factorizations of operratorA and
% operatorB, thus, e.g., it preserves the sparsity and the structure of
% operatorA and operatorB.
%
% residualTolerance and maxIterations control tolerance and max number of
% steps, and verbosityLevel = 0, 1, or 2 controls the amount of printed
% info. lambdaHistory is a matrix with all iterative lambdas, and
% residualNormsHistory are matrices of the history of 2-norms of residuals
%
% Required input:
%  * blockVectorX (class numeric) - initial approximation to eigenvectors,
%    full or sparse matrix n-by-blockSize. blockVectorX must be full rank.
%  * operatorA (class numeric, char, or function_handle) - the main operator
%    of the eigenproblem, can be a matrix, a function name, or handle
%
% Optional function input:
%   * operatorB (class numeric, char, or function_handle) - the second
%     operator, if solving a generalized eigenproblem, can be a matrix,
%      a function name, or handle; by default if empty, operatorB=I.
%   * operatorT  (class char or function_handle) - the preconditioner,
%     by default operatorT(blockVectorX)=blockVectorX.
%
% Optional constraints input:
%   blockVectorY (class numeric) - a full or sparse n-by-sizeY matrix of
%   constraints, where sizeY < n. blockVectorY must be full rank.
%   The iterations will be performed in the (operatorB-)
%   orthogonal complement of the column-space of blockVectorY.
%
% Optional scalar input parameters:
%   residualTolerance (class numeric) - tolerance, by default,
%   residualTolerance=n*sqrt(eps) maxIterations - max number of iterations,
%   by default, maxIterations = min(n,20) verbosityLevel - either 0 (no
%   info), 1, or 2 (with pictures); by default, verbosityLevel = 0.
%
% Required output: blockVectorX and lambda (both class numeric) are
% computed blockSize eigenpairs, where blockSize=size(blockVectorX,2)
% for the initial guess blockVectorX if it is full rank.
%
% Optional output: failureFlag (class integer), lambdaHistory (class numeric)
% and residualNormsHistory (class numeric) are described above.
%
% Functions operatorA(blockVectorX), operatorB(blockVectorX) and
% operatorT(blockVectorX) must support blockVectorX being a matrix, not
% just a column vector.
%
% Every iteration involves one application of operatorA and operatorB, and
% one of operatorT.
%
% Main memory requirements: 6 (9 if isempty(operatorB)=0) matrices of the
% same size as blockVectorX, 2 matrices of the same size as blockVectorY
% (if present), and two square matrices of the size 3*blockSize.
%
% In all examples below, we use the Laplacian operator in a 20x20 square
% with the mesh size 1 which can be generated in MATLAB by running
% A = delsq(numgrid('S',21)); n=size(A,1);
% or in MATLAB and Octave by
% [~,~,A] = laplacian([19,19]); n=size(A,1);
% see http://www.mathworks.com/matlabcentral/fileexchange/27279
%
% The following Example:
%
% [blockVectorX,lambda,failureFlag]=lobpcg(randn(n,8),A,1e-5,50,2);
%
% attempts to compute 8 first eigenpairs without preconditioning,
% but not all eigenpairs converge after 50 steps, so failureFlag=1.
%
% The next Example:
%
% blockVectorY=[];lambda_all=[];
% for j=1:4
%   [blockVectorX,lambda]=...
%                    lobpcg(randn(n,2),A,blockVectorY,1e-5,200,2);
%   blockVectorY=[blockVectorY,blockVectorX];
%   lambda_all=[lambda_all' lambda']'; pause;
% end
%
% attemps to compute the same 8 eigenpairs by calling the code 4 times
% with blockSize=2 using orthogonalization to the previously founded
% eigenvectors.
%
% The following Example:
%
% R=ichol(A,struct('michol','on')); precfun = @(x)R\(R'\x);
% [blockVectorX,lambda,failureFlag]=lobpcg(randn(n,8),A,[],@(x)precfun(x),1e-5,60,2);
%
% computes the same eigenpairs in less then 25 steps, so that failureFlag=0
% using the preconditioner function "precfun", defined inline. If "precfun"
% is defined as a MATLAB function in a file, the function handle
% @(x)precfun(x) can be equivalently replaced by the function name 'precfun'
% Running
%
% [blockVectorX,lambda,failureFlag]=...
%          lobpcg(randn(n,8),A,speye(n),@(x)precfun(x),1e-5,50,2);
%
% produces similar answers, but is somewhat slower and needs more memory as
% technically a generalized eigenproblem with B=I is solved here.
%
% The following Example for a mostly diagonally dominant sparse matrix A
% demonstrates different types of preconditioning, compared to the standard
% use of the main diagonal of A:
%
% clear all; close all;
% n = 1000; M = spdiags([1:n]',0,n,n); precfun=@(x)M\x;
% A=M+sprandsym(n,.1); Xini=randn(n,5); maxiter=15; tol=1e-5;
% [~,~,~,~,rnp]=lobpcg(Xini,A,tol,maxiter,1);
% [~,~,~,~,r]=lobpcg(Xini,A,[],@(x)precfun(x),tol,maxiter,1);
% subplot(2,2,1), semilogy(r'); hold on; semilogy(rnp',':>');
% title('No preconditioning (top)'); axis tight;
% M(1,2) = 2; precfun=@(x)M\x; % M is no longer symmetric
% [~,~,~,~,rns]=lobpcg(Xini,A,[],@(x)precfun(x),tol,maxiter,1);
% subplot(2,2,2),  semilogy(r'); hold on; semilogy(rns','--s');
% title('Nonsymmetric preconditioning (square)'); axis tight;
% M(1,2) = 0; precfun=@(x)M\(x+10*sin(x)); % nonlinear preconditioning
% [~,~,~,~,rnl]=lobpcg(Xini,A,[],@(x)precfun(x),tol,maxiter,1);
% subplot(2,2,3),  semilogy(r'); hold on; semilogy(rnl','-.*');
% title('Nonlinear preconditioning (star)'); axis tight;
% M=abs(M-3.5*speye(n,n)); precfun=@(x)M\x;
% [~,~,~,~,rs]=lobpcg(Xini,A,[],@(x)precfun(x),tol,maxiter,1);
% subplot(2,2,4),  semilogy(r'); hold on; semilogy(rs','-d');
% title('Selective preconditioning (diamond)'); axis tight;
%
% Revision 4.16 adds support for distributed or codistributed arrays
% available in MATLAB BigData toolbox, e.g.,
%
% A = codistributed(diag(1:100)); B = codistributed(diag(101:200));
% [blockVectorX,lambda]=lobpcg(randn(100,2),A,1e-5,5,2)
%
% Revision 4.17 adds support for single precision, e.g.,
% A = diag(1:100); B = single(diag(101:200));
% [blockVectorX,lambda]=lobpcg(randn(100,2),A,1e-5,15,2);
% A = diag(1:100); B = diag(101:200);
% [blockVectorX,lambda]=lobpcg(randn(100,2,'single'),A,1e-5,15,2);
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% This main function LOBPCG is a version of
% the preconditioned conjugate gradient method (Algorithm 5.1) described in
% A. V. Knyazev, Toward the Optimal Preconditioned Eigensolver:
% Locally Optimal Block Preconditioned Conjugate Gradient Method,
% SIAM Journal on Scientific Computing 23 (2001), no. 2, pp. 517-541.
% http://dx.doi.org/10.1137/S1064827500366124
%
% Known bugs/features:
%
% - an excessively small requested tolerance may result in often restarts
% and instability. The code is not written to produce an eps-level
% accuracy! Use common sense.
%
% - the code may be very sensitive to the number of eigenpairs computed,
% if there is a cluster of eigenvalues not completely included, cf.
%
% operatorA=diag([1 1.99 2:99]);
% [blockVectorX,lambda]=lobpcg(randn(100,1),operatorA,1e-10,80,2);
% [blockVectorX,lambda]=lobpcg(randn(100,2),operatorA,1e-10,80,2);
% [blockVectorX,lambda]=lobpcg(randn(100,3),operatorA,1e-10,80,2);
%
% - using a nonsymmetric preconditioner is possible, but may be unstable
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The main distribution site:
% https://github.com/lobpcg/blopex
%
% A C-version of this code is a part of the
% https://github.com/lobpcg/blopex
% package and is directly available, e.g., in SLEPc and HYPRE.
%
% A python version of this code is in
% https://docs.scipy.org/doc/scipy/reference/generated/scipy.sparse.linalg.lobpcg.html
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   License:  MIT / Apache-2.0
%   Copyright (c) 2000-2019 A.V. Knyazev, Andrew.Knyazev@ucdenver.edu
%   $Revision: 1.2 $  $Date: 13-June-2019
%   This revision is tested in 9.6.0.1114505 (R2019a) Update 2, but is
%   expected to work on any >R2007b MATLAB.
%   Revision 4.13 tested in MATLAB 6.5-7.13.
%   Revision 4.13 tested and available in Octave 3.2.3-3.4.2, see
%   https://octave.sourceforge.io/linear-algebra/function/lobpcg.html
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Begin
% Function gather defined to be identity if nonexistent, before 2016a
if exist("gather", "file") == 2
    mygather=@(x)gather(x);
else
    mygather=@(x)x;
end
% constants
CONVENTIONAL_CONSTRAINTS = 1;
SYMMETRIC_CONSTRAINTS = 2;
%Initial settings
failureFlag = 1;
if nargin < 2
    error('BLOPEX:lobpcg:NotEnoughInputs',...
        strcat('There must be at least 2 input agruments: ',...
        'blockVectorX and operatorA'));
end
if nargin > 8
    warning('BLOPEX:lobpcg:TooManyInputs',...
        strcat('There must be at most 8 input agruments ',...
        'unless arguments are passed to a function'));
end
if ~isnumeric(blockVectorX)
    error('BLOPEX:lobpcg:FirstInputNotNumeric',...
        'The first input argument blockVectorX must be numeric');
end
[n,blockSize]=size(blockVectorX);
if blockSize > n
    error('BLOPEX:lobpcg:FirstInputFat',...
        'The first input argument blockVectorX must be tall, not fat');
end
if n < 6
    error('BLOPEX:lobpcg:MatrixTooSmall',...
        'The code does not work for matrices of small sizes');
end
if isa(operatorA,'numeric')
    nA = size(operatorA,1);
    if any(size(operatorA) ~= nA)
        error('BLOPEX:lobpcg:MatrixNotSquare',...
            'operatorA must be a square matrix or a string');
    end
    if size(operatorA) ~= n
        error('BLOPEX:lobpcg:MatrixWrongSize',...
            ['The size ' int2str(size(operatorA))...
            ' of operatorA is not the same as ' int2str(n)...
            ' - the number of rows of blockVectorX']);
    end
end
count_string = 0;
operatorT = [];
operatorB = [];
residualTolerance = [];
maxIterations = [];
verbosityLevel = [];
blockVectorY = []; sizeY = 0;
for j = 1:nargin-2
    if isequal(size(varargin{j}),[n,n])
        if isempty(operatorB)
            operatorB = varargin{j};
        else
            error('BLOPEX:lobpcg:TooManyMatrixInputs',...
                strcat('Too many matrix input arguments. ',...
                'Preconditioner operatorT must be an M-function'));
        end
    elseif isequal(size(varargin{j},1),n) && size(varargin{j},2) < n
        if isempty(blockVectorY)
            blockVectorY = varargin{j};
            sizeY=size(blockVectorY,2);
        else
            error('BLOPEX:lobpcg:WrongConstraintsFormat',...
                'Something wrong with blockVectorY input argument');
        end
    elseif ischar(varargin{j}) || isa(varargin{j},'function_handle')
        if count_string == 0
            if isempty(operatorB)
                operatorB = varargin{j};
                count_string = count_string + 1;
            else
                operatorT = varargin{j};
            end
        elseif count_string == 1
            operatorT = varargin{j};
        else
            warning('BLOPEX:lobpcg:TooManyStringFunctionHandleInputs',...
                'Too many string or FunctionHandle input arguments');
        end
    elseif isequal(size(varargin{j}),[n,n])
        error('BLOPEX:lobpcg:WrongPreconditionerFormat',...
            'Preconditioner operatorT must be an M-function');
    elseif max(size(varargin{j})) == 1
        if isempty(residualTolerance)
            residualTolerance = varargin{j};
        elseif isempty(maxIterations)
            maxIterations = varargin{j};
        elseif isempty(verbosityLevel)
            verbosityLevel = varargin{j};
        else
            warning('BLOPEX:lobpcg:TooManyScalarInputs',...
                'Too many scalar parameters, need only three');
        end
    elseif isempty(varargin{j})
        if isempty(operatorB)
            count_string = count_string + 1;
        elseif ~isempty(operatorT)
            count_string = count_string + 1;
        elseif ~isempty(blockVectorY)
            error('BLOPEX:lobpcg:UnrecognizedEmptyInput',...
                ['Unrecognized empty input argument number ' int2str(j+2)]);
        end
    else
        error('BLOPEX:lobpcg:UnrecognizedInput',...
            ['Input argument number ' int2str(j+2) ' not recognized.']);
    end
end
if verbosityLevel
    if issparse(blockVectorX)
        fprintf(['The sparse initial guess with %i colunms '...
            'and %i raws is detected  \n'],n,blockSize);
    else
        fprintf(['The full initial guess with %i colunms '...
            'and %i raws is detected  \n'],n,blockSize);
    end
    if ischar(operatorA)
        fprintf('The main operator is detected as an M-function %s \n',...
            operatorA);
    elseif isa(operatorA,'function_handle')
        fprintf('The main operator is detected as an M-function %s \n',...
            func2str(operatorA));
    elseif issparse(operatorA)
        fprintf('The main operator is detected as a sparse matrix \n');
    else
        fprintf('The main operator is detected as a full matrix \n');
    end
    if isempty(operatorB)
        fprintf('Solving standard eigenvalue problem, not generalized \n');
    elseif ischar(operatorB)
        fprintf(['The second operator of the generalized eigenproblem \n'...
            'is detected as an M-function %s \n'],operatorB);
    elseif isa(operatorB,'function_handle')
        fprintf(['The second operator of the generalized eigenproblem \n'...
            'is detected as an M-function %s \n'],func2str(operatorB));
    elseif issparse(operatorB)
        fprintf(strcat('The second operator of the generalized',...
            'eigenproblem \n is detected as a sparse matrix \n'));
    else
        fprintf(strcat('The second operator of the generalized',...
            'eigenproblem \n is detected as a full matrix \n'));
    end
    if isempty(operatorT)
        fprintf('No preconditioner is detected \n');
    elseif ischar(operatorT)
        fprintf('The preconditioner is detected as an M-function %s \n',...
            operatorT);
    elseif isa(operatorT,'function_handle')
        fprintf('The preconditioner is detected as an M-function %s \n',...
            func2str(operatorT));
    end
    if isempty(blockVectorY)
        fprintf('No matrix of constraints is detected \n')
    elseif issparse(blockVectorY)
        fprintf('The sparse matrix of %i constraints is detected \n',sizeY);
    else
        fprintf('The full matrix of %i constraints is detected \n',sizeY);
    end
    if issparse(blockVectorY) ~= issparse(blockVectorX)
        warning('BLOPEX:lobpcg:SparsityInconsistent',...
            strcat('The sparsity formats of the initial guess and ',...
            'the constraints are inconsistent'));
    end
end
% Set defaults
if isempty(residualTolerance)
    residualTolerance = sqrt(eps)*n;
end
if isempty(maxIterations)
    maxIterations = min(n,20);
end
if isempty(verbosityLevel)
    verbosityLevel = 0;
end
if verbosityLevel
    fprintf('Tolerance %e and maximum number of iterations %i \n',...
        residualTolerance,maxIterations)
end
%constraints preprocessing
if isempty(blockVectorY)
    constraintStyle = 0;
else
    %    constraintStyle = SYMMETRIC_CONSTRAINTS; % more accurate?
    constraintStyle = CONVENTIONAL_CONSTRAINTS;
end
if constraintStyle == CONVENTIONAL_CONSTRAINTS

    if isempty(operatorB)
        gramY = blockVectorY'*blockVectorY;
    else
        if isnumeric(operatorB)
            blockVectorBY = operatorB*blockVectorY;
        else
            blockVectorBY = feval(operatorB,blockVectorY);
        end
        gramY=blockVectorY'*blockVectorBY;
    end
    gramY=(gramY'+gramY)*0.5;
    if isempty(operatorB)
        blockVectorX = blockVectorX - ...
            blockVectorY*(gramY\(blockVectorY'*blockVectorX));
    else
        blockVectorX =blockVectorX - ...
            blockVectorY*(gramY\(blockVectorBY'*blockVectorX));
    end

elseif constraintStyle == SYMMETRIC_CONSTRAINTS

    if ~isempty(operatorB)
        if isnumeric(operatorB)
            blockVectorY = operatorB*blockVectorY;
        else
            blockVectorY = feval(operatorB,blockVectorY);
        end
    end
    if isempty(operatorT)
        gramY = blockVectorY'*blockVectorY;
    else
        blockVectorTY = feval(operatorT,blockVectorY);
        gramY = blockVectorY'*blockVectorTY;
    end
    gramY=(gramY'+gramY)*0.5;
    if isempty(operatorT)
        blockVectorX = blockVectorX - ...
            blockVectorY*(gramY\(blockVectorY'*blockVectorX));
    else
        blockVectorX = blockVectorX - ...
            blockVectorTY*(gramY\(blockVectorY'*blockVectorX));
    end

end
%Making the initial vectors (operatorB-) orthonormal
if isempty(operatorB)
    %[blockVectorX,gramXBX] = qr(blockVectorX,0);
    gramXBX=mygather(blockVectorX'*blockVectorX);
    if ~isreal(gramXBX)
        gramXBX=(gramXBX+gramXBX')*0.5;
    end
    [gramXBX,cholFlag]=chol(gramXBX);
    if  cholFlag ~= 0
        error('BLOPEX:lobpcg:ConstraintsTooTight',...
            'The initial approximation after constraints is not full rank');
    end
    blockVectorX = blockVectorX/gramXBX;
else
    %[blockVectorX,blockVectorBX] = orth(operatorB,blockVectorX);
    if isnumeric(operatorB)
        blockVectorBX = operatorB*blockVectorX;
    else
        blockVectorBX = feval(operatorB,blockVectorX);
    end
    gramXBX=blockVectorX'*blockVectorBX;
    if ~isreal(gramXBX)
        gramXBX=(gramXBX+gramXBX')*0.5;
    end
    [gramXBX,cholFlag]=chol(gramXBX);
    if  cholFlag ~= 0
        error('BLOPEX:lobpcg:InitialNotFullRank',...
            '%s\n%s', ...
            'The initial approximation after constraints is not ',...
            'full rank or/and operatorB is not positive definite');
    end
    blockVectorX = blockVectorX/gramXBX;
    blockVectorBX = blockVectorBX/gramXBX;
end
% Checking if the problem is big enough for the algorithm,
% i.e. n-sizeY > 5*blockSize
% Theoretically, the algorithm should be able to run if
% n-sizeY > 3*blockSize,
% but the extreme cases might be unstable, so we use 5 instead of 3 here.
if n-sizeY < 5*blockSize
    error('BLOPEX:lobpcg:MatrixTooSmall','%s\n%s', ...
        'The problem size is too small, relative to the block size.',...
        'Try using eig() or eigs() instead.');
end
% Preallocation
residualNormsHistory=zeros(blockSize,maxIterations);
lambdaHistory=zeros(blockSize,maxIterations+1);
condestGhistory=zeros(1,maxIterations+1);
blockVectorAR=zeros(n,blockSize, 'like', blockVectorX);
blockVectorP=zeros(n,blockSize, 'like', blockVectorX);
blockVectorAP=zeros(n,blockSize, 'like', blockVectorX);
if ~isempty(operatorB)
    blockVectorBR=zeros(n,blockSize, 'like', blockVectorX);
    blockVectorBP=zeros(n,blockSize, 'like', blockVectorX);
end
%Initial settings for the loop
if isnumeric(operatorA)
    blockVectorAX = operatorA*blockVectorX;
else
    blockVectorAX = feval(operatorA,blockVectorX);
end
gramXAX = full(blockVectorX'*blockVectorAX);
gramXAX = (gramXAX + gramXAX')*0.5;
% eig(...,'chol') uses only the diagonal and upper triangle -
% not true in MATLAB
% Octave v3.2.3-4, eig() does not support inputting 'chol'
[coordX,gramXAX]=eig(gramXAX,eye(blockSize));
lambda=diag(gramXAX); %eig returns non-ordered eigenvalues on the diagonal
if issparse(blockVectorX)
    coordX=sparse(coordX);
end
blockVectorX  =  blockVectorX*coordX;
blockVectorAX = blockVectorAX*coordX;
if ~isempty(operatorB)
    blockVectorBX = blockVectorBX*coordX;
end
clear coordX
condestGhistory(1)=-log10(eps)/2;  %if too small cause unnecessary restarts
lambdaHistory(1:blockSize,1) = mygather(lambda);
activeMask = true(blockSize,1);
% currentBlockSize = blockSize; %iterate all
%
% restart=1;%steepest descent
%The main part of the method is the loop of the CG method: begin
for iterationNumber=1:maxIterations

    %     %Computing the active residuals
    %     if isempty(operatorB)
    %%         if currentBlockSize > 1
    %%             blockVectorR(:,activeMask)=blockVectorAX(:,activeMask) - ...
    %%                 blockVectorX(:,activeMask)*spdiags(lambda(activeMask),0,currentBlockSize,currentBlockSize);
    %%         else
    %%             blockVectorR(:,activeMask)=blockVectorAX(:,activeMask) - ...
    %%                 blockVectorX(:,activeMask)*lambda(activeMask);
    %%         end
    %         blockVectorR(:,activeMask)=blockVectorAX(:,activeMask) - ...
    %         bsxfun(@times,blockVectorX(:,activeMask),lambda(activeMask)');
    %     else
    %%         if currentBlockSize > 1
    %%             blockVectorR(:,activeMask)=blockVectorAX(:,activeMask) - ...
    %%                 blockVectorBX(:,activeMask)*spdiags(lambda(activeMask),0,currentBlockSize,currentBlockSize);
    %%         else
    %%             blockVectorR(:,activeMask)=blockVectorAX(:,activeMask) - ...
    %%                 blockVectorBX(:,activeMask)*lambda(activeMask);
    %%         end
    %         blockVectorR(:,activeMask)=blockVectorAX(:,activeMask) - ...
    %         bsxfun(@times,blockVectorBX(:,activeMask),lambda(activeMask)');
    %     end

    %Computing all residuals
    if isempty(operatorB)
        %         if blockSize > 1
        %             blockVectorR = blockVectorAX - ...
        %                 blockVectorX*spdiags(lambda,0,blockSize,blockSize);
        %         else
        %             blockVectorR = blockVectorAX - blockVectorX*lambda;
        %             %to make blockVectorR full when lambda is just a scalar
        %         end
        blockVectorR = blockVectorAX - ...
            bsxfun(@times,blockVectorX,lambda');
    else
        %        if blockSize > 1
        %             blockVectorR = blockVectorAX - ...
        %                 blockVectorBX*spdiags(lambda,0,blockSize,blockSize);
        %        else
        %             blockVectorR = blockVectorAX - blockVectorBX*lambda;
        %        end
        blockVectorR = blockVectorAX - ...
            bsxfun(@times,blockVectorBX,lambda');
    end

    %Satisfying the constraints for the active residulas
    if constraintStyle == SYMMETRIC_CONSTRAINTS
        if isempty(operatorT)
            blockVectorR(:,activeMask) = blockVectorR(:,activeMask) - ...
                blockVectorY*(gramY\(blockVectorY'*...
                blockVectorR(:,activeMask)));
        else
            blockVectorR(:,activeMask) = blockVectorR(:,activeMask) - ...
                blockVectorY*(gramY\(blockVectorTY'*...
                blockVectorR(:,activeMask)));
        end
    end

    residualNorms = full(sqrt(sum(conj(blockVectorR).*blockVectorR)'));
    residualNormsHistory(1:blockSize,iterationNumber) = ...
        mygather(residualNorms);

    %index antifreeze
    activeMask = full(residualNorms > residualTolerance) & activeMask;
    %activeMask = full(residualNorms > residualTolerance);
    %above allows vectors back into active, which causes problems with frosen Ps
    %activeMask = full(residualNorms > 0);      %iterate all, ignore freeze

    currentBlockSize = mygather(sum(activeMask));
    if  currentBlockSize == 0
        failureFlag=0; %all eigenpairs converged
        break
    end

    %Applying the preconditioner operatorT to the active residulas
    if ~isempty(operatorT)
        blockVectorR(:,activeMask) = ...
            feval(operatorT,blockVectorR(:,activeMask));
    end

    if constraintStyle == CONVENTIONAL_CONSTRAINTS
        if isempty(operatorB)
            blockVectorR(:,activeMask) = blockVectorR(:,activeMask) - ...
                blockVectorY*(gramY\(blockVectorY'*...
                blockVectorR(:,activeMask)));
        else
            blockVectorR(:,activeMask) = blockVectorR(:,activeMask) - ...
                blockVectorY*(gramY\(blockVectorBY'*...
                blockVectorR(:,activeMask)));
        end
    end

    %Making active (preconditioned) residuals orthogonal to blockVectorX
    if isempty(operatorB)
        blockVectorR(:,activeMask) = blockVectorR(:,activeMask) - ...
            blockVectorX*(blockVectorX'*blockVectorR(:,activeMask));
    else
        blockVectorR(:,activeMask) = blockVectorR(:,activeMask) - ...
            blockVectorX*(blockVectorBX'*blockVectorR(:,activeMask));
    end

    %Making active residuals orthonormal
    if isempty(operatorB)
        %[blockVectorR(:,activeMask),gramRBR]=...
        %qr(blockVectorR(:,activeMask),0); %to increase stability
        gramRBR=blockVectorR(:,activeMask)'*blockVectorR(:,activeMask);
        if ~isreal(gramRBR)
            gramRBR=(gramRBR+gramRBR')*0.5;
        end
        [gramRBR,cholFlag]=chol(gramRBR);
        if  cholFlag == 0
            blockVectorR(:,activeMask) = blockVectorR(:,activeMask)/gramRBR;
        else
            warning('BLOPEX:lobpcg:ResidualNotFullRank',...
                'The residual is not full rank.');
            break
        end
    else
        if isnumeric(operatorB)
            blockVectorBR(:,activeMask) = ...
                operatorB*blockVectorR(:,activeMask);
        else
            blockVectorBR(:,activeMask) = ...
                feval(operatorB,blockVectorR(:,activeMask));
        end
        gramRBR=blockVectorR(:,activeMask)'*blockVectorBR(:,activeMask);
        if ~isreal(gramRBR)
            gramRBR=(gramRBR+gramRBR')*0.5;
        end
        [gramRBR,cholFlag]=chol(gramRBR);
        if  cholFlag == 0
            blockVectorR(:,activeMask) = ...
                blockVectorR(:,activeMask)/gramRBR;
            blockVectorBR(:,activeMask) = ...
                blockVectorBR(:,activeMask)/gramRBR;
        else
            warning('BLOPEX:lobpcg:ResidualNotFullRankOrElse',...
                strcat('The residual is not full rank or/and operatorB ',...
                'is not positive definite.'));
            break
        end
    end
    clear gramRBR;

    if isnumeric(operatorA)
        blockVectorAR(:,activeMask) = ...
            mygather(operatorA*blockVectorR(:,activeMask));
    else
        blockVectorAR(:,activeMask) = ...
            feval(operatorA,blockVectorR(:,activeMask));
    end

    condestGmean = mean(condestGhistory(max(1,iterationNumber-10-...
        round(log(currentBlockSize))):iterationNumber));

    %  restart=1;

    % The Raileight-Ritz method for [blockVectorX blockVectorR blockVectorP]
    if isa(blockVectorAR, 'single')    % single initial
        myeps = 1; % play safe
    elseif isa(blockVectorR, 'single') %single somethings else
        myeps = eps(single(1));
    else                               % double everything
        myeps = eps;
    end
    if  mygather(residualNorms) > myeps^0.6
        explicitGramFlag = 0;
    else
        explicitGramFlag = 1;  %suggested by Garrett Moran, private
    end

    activeRSize=size(blockVectorR(:,activeMask),2);
    if iterationNumber == 1
        activePSize=0;
        restart=1;
    else
        activePSize=size(blockVectorP(:,activeMask),2);
        restart=0;
    end

    gramXAR=full(blockVectorAX'*blockVectorR(:,activeMask));
    gramRAR=full(blockVectorAR(:,activeMask)'*blockVectorR(:,activeMask));
    gramRAR=(gramRAR'+gramRAR)*0.5;

    if explicitGramFlag
        gramXAX=full(blockVectorAX'*blockVectorX);
        gramXAX=(gramXAX'+gramXAX)*0.5;
        if isempty(operatorB)
            gramXBX=full(blockVectorX'*blockVectorX);
            gramRBR=full(blockVectorR(:,activeMask)'*...
                blockVectorR(:,activeMask));
            gramXBR=full(blockVectorX'*blockVectorR(:,activeMask));
        else
            gramXBX=full(blockVectorBX'*blockVectorX);
            gramRBR=full(blockVectorBR(:,activeMask)'*...
                blockVectorR(:,activeMask));
            gramXBR=full(blockVectorBX'*blockVectorR(:,activeMask));
        end
        gramXBX=(gramXBX'+gramXBX)*0.5;
        gramRBR=(gramRBR'+gramRBR)*0.5;

    end

    if iterationNumber > 1
        %Making active conjugate directions orthonormal
        if isempty(operatorB)
            %[blockVectorP(:,activeMask),gramPBP] = qr(blockVectorP(:,activeMask),0);
            gramPBP=blockVectorP(:,activeMask)'*blockVectorP(:,activeMask);
            if ~isreal(gramPBP)
                gramPBP=(gramPBP+gramPBP')*0.5;
            end
            [gramPBP,cholFlag]=chol(gramPBP);
            if  cholFlag == 0
                blockVectorP(:,activeMask) = ...
                    blockVectorP(:,activeMask)/gramPBP;
                blockVectorAP(:,activeMask) = ...
                    blockVectorAP(:,activeMask)/gramPBP;
                restart = 0;
            else
                warning('BLOPEX:lobpcg:DirectionNotFullRank',...
                    'The direction matrix is not full rank.');
                restart = 1;
            end
        else
            gramPBP=blockVectorP(:,activeMask)'*blockVectorBP(:,activeMask);
            if ~isreal(gramPBP)
                gramPBP=(gramPBP+gramPBP')*0.5;
            end
            [gramPBP,cholFlag]=chol(gramPBP);
            if  cholFlag == 0
                blockVectorP(:,activeMask) = ...
                    blockVectorP(:,activeMask)/gramPBP;
                blockVectorAP(:,activeMask) = ...
                    blockVectorAP(:,activeMask)/gramPBP;
                blockVectorBP(:,activeMask) = ...
                    blockVectorBP(:,activeMask)/gramPBP;
                restart = 0;
            else
                warning('BLOPEX:lobpcg:DirectionNotFullRank',...
                    strcat('The direction matrix is not full rank ',...
                    'or/and operatorB is not positive definite.'));
                restart = 1;
            end
        end
        clear gramPBP
    end

    for cond_try=1:2           %cond_try == 2 when restart

        if ~restart
            gramXAP=full(blockVectorAX'*blockVectorP(:,activeMask));
            gramRAP=full(blockVectorAR(:,activeMask)'*...
                blockVectorP(:,activeMask));
            gramPAP=full(blockVectorAP(:,activeMask)'*...
                blockVectorP(:,activeMask));
            gramPAP=(gramPAP'+gramPAP)*0.5;

            if explicitGramFlag
                gramA = [ gramXAX     gramXAR     gramXAP
                    gramXAR'    gramRAR     gramRAP
                    gramXAP'     gramRAP'    gramPAP ];
            else
                gramA = [ diag(lambda)  gramXAR  gramXAP
                    gramXAR'      gramRAR  gramRAP
                    gramXAP'      gramRAP'  gramPAP ];
            end

            clear gramXAP  gramRAP gramPAP

            if isempty(operatorB)
                gramXBP=full(blockVectorX'*blockVectorP(:,activeMask));
                gramRBP=full(blockVectorR(:,activeMask)'*...
                    blockVectorP(:,activeMask));
            else
                gramXBP=full(blockVectorBX'*blockVectorP(:,activeMask));
                gramRBP=full(blockVectorBR(:,activeMask)'*...
                    blockVectorP(:,activeMask));
                %or blockVectorR(:,activeMask)'*blockVectorBP(:,activeMask);
            end

            if explicitGramFlag
                if isempty(operatorB)
                    gramPBP=full(blockVectorP(:,activeMask)'*...
                        blockVectorP(:,activeMask));
                else
                    gramPBP=full(blockVectorBP(:,activeMask)'*...
                        blockVectorP(:,activeMask));
                end
                gramPBP=(gramPBP'+gramPBP)*0.5;
                gramB = [ gramXBX  gramXBR  gramXBP
                    gramXBR' gramRBR  gramRBP
                    gramXBP' gramRBP' gramPBP ];
                clear   gramPBP
            else
                gramB=[eye(blockSize) zeros(blockSize,activeRSize) gramXBP
                    zeros(blockSize,activeRSize)' eye(activeRSize) gramRBP
                    gramXBP' gramRBP' eye(activePSize) ];
            end

            clear gramXBP  gramRBP;

        else

            if explicitGramFlag
                gramA = [ gramXAX   gramXAR
                    gramXAR'    gramRAR  ];
                gramB = [ gramXBX  gramXBR
                    gramXBR' eye(activeRSize)  ];
                clear gramXAX gramXBX gramXBR
            else
                gramA = [ diag(lambda)  gramXAR
                    gramXAR'        gramRAR  ];
                gramB = eye(blockSize+activeRSize);
            end

            clear gramXAR gramRAR;

        end

        condestG = log10(cond(gramB))+1;
        if (condestG/condestGmean > 2 && condestG > 2 )|| condestG > 8
            %black magic - need to guess the restart
            if verbosityLevel
                fprintf('Restart on step %i as condestG %5.4e \n',...
                    iterationNumber,condestG);
            end
            if cond_try == 1 && ~restart
                restart=1; %steepest descent restart for stability
            else
                warning('BLOPEX:lobpcg:IllConditioning',...
                    'Gramm matrix ill-conditioned: results unpredictable');
            end
        else
            break
        end

    end

    [gramA,gramB]=eig(gramA,gramB);
    lambda=diag(gramB(1:blockSize,1:blockSize));
    coordX=gramA(:,1:blockSize);

    clear gramA gramB

    if issparse(blockVectorX)
        coordX=sparse(coordX);
    end

    if ~restart
        blockVectorP =  blockVectorR(:,activeMask)*...
            coordX(blockSize+1:blockSize+activeRSize,:) + ...
            blockVectorP(:,activeMask)*...
            coordX(blockSize+activeRSize+1:blockSize + ...
            activeRSize+activePSize,:);
        blockVectorAP = blockVectorAR(:,activeMask)*...
            coordX(blockSize+1:blockSize+activeRSize,:) + ...
            blockVectorAP(:,activeMask)*...
            coordX(blockSize+activeRSize+1:blockSize + ...
            activeRSize+activePSize,:);
        if ~isempty(operatorB)
            blockVectorBP = blockVectorBR(:,activeMask)*...
                coordX(blockSize+1:blockSize+activeRSize,:) + ...
                blockVectorBP(:,activeMask)*...
                coordX(blockSize+activeRSize+1:blockSize+activeRSize+activePSize,:);
        end
    else %use block steepest descent
        blockVectorP =   blockVectorR(:,activeMask)*...
            coordX(blockSize+1:blockSize+activeRSize,:);
        blockVectorAP = blockVectorAR(:,activeMask)*...
            coordX(blockSize+1:blockSize+activeRSize,:);
        if ~isempty(operatorB)
            blockVectorBP = blockVectorBR(:,activeMask)*...
                coordX(blockSize+1:blockSize+activeRSize,:);
        end
    end

    blockVectorX = blockVectorX*coordX(1:blockSize,:) + blockVectorP;
    blockVectorAX = blockVectorAX*coordX(1:blockSize,:) + blockVectorAP;
    if ~isempty(operatorB)
        blockVectorBX = blockVectorBX*coordX(1:blockSize,:) + blockVectorBP;
    end
    clear coordX
    %%end RR

    lambdaHistory(1:blockSize,iterationNumber+1) = mygather(lambda);
    condestGhistory(iterationNumber+1) = mygather(condestG);

    if verbosityLevel
        fprintf('Iteration %i current block size %i \n',...
            iterationNumber,currentBlockSize);
        fprintf('Eigenvalues lambda %17.16e \n',mygather(lambda));
        fprintf('Residual Norms %e \n',mygather(residualNorms'));
    end
end
%The main step of the method was the CG cycle: end
%Postprocessing
%Making sure blockVectorX's "exactly" satisfy the blockVectorY constrains??
%Making sure blockVectorX's are "exactly" othonormalized by final "exact" RR
if isempty(operatorB)
    gramXBX=full(blockVectorX'*blockVectorX);
else
    if isnumeric(operatorB)
        blockVectorBX = operatorB*blockVectorX;
    else
        blockVectorBX = feval(operatorB,blockVectorX);
    end
    gramXBX = full(blockVectorX'*blockVectorBX);
end
gramXBX=(gramXBX'+gramXBX)*0.5;
if isnumeric(operatorA)
    blockVectorAX = operatorA*blockVectorX;
else
    blockVectorAX = feval(operatorA,blockVectorX);
end
gramXAX = full(blockVectorX'*blockVectorAX);
gramXAX = (gramXAX + gramXAX')*0.5;
%Raileigh-Ritz for blockVectorX, which is already operatorB-orthonormal
[coordX,gramXBX] = eig(gramXAX,gramXBX);
lambda=diag(gramXBX);
if issparse(blockVectorX)
    coordX=sparse(coordX);
end
blockVectorX  =   blockVectorX*coordX;
blockVectorAX  =  blockVectorAX*coordX;
if ~isempty(operatorB)
    blockVectorBX  =  blockVectorBX*coordX;
end
%Computing all residuals
if isempty(operatorB)
    %     if blockSize > 1
    %         blockVectorR = blockVectorAX - ...
    %             blockVectorX*spdiags(lambda,0,blockSize,blockSize);
    %     else
    %         blockVectorR = blockVectorAX - blockVectorX*lambda;
    %     end
    blockVectorR = blockVectorAX - ...
        bsxfun(@times,blockVectorX,lambda');
else
    %     if blockSize > 1
    %         blockVectorR=blockVectorAX - ...
    %             blockVectorBX*spdiags(lambda,0,blockSize,blockSize);
    %     else
    %         blockVectorR = blockVectorAX - blockVectorBX*lambda;
    %     end
    blockVectorR = blockVectorAX - ...
        bsxfun(@times,blockVectorBX,lambda');
end
residualNorms=full(sqrt(sum(conj(blockVectorR).*blockVectorR)'));
residualNormsHistory(1:blockSize,iterationNumber) = ...
    mygather(residualNorms);
if verbosityLevel
    fprintf('Final Eigenvalues lambda %17.16e \n',mygather(lambda));
    fprintf('Final Residual Norms %e \n',mygather(residualNorms'));
end
if verbosityLevel == 2
    whos
    figure(491)
    semilogy((abs(residualNormsHistory(1:blockSize,1:iterationNumber-1)))');
    title('Residuals for Different Eigenpairs','fontsize',16);
    ylabel('Eucledian norm of residuals','fontsize',16);
    xlabel('Iteration number','fontsize',16);
    %axis tight;
    %axis([0 maxIterations+1 1e-15 1e3])
    set(gca,'FontSize',14);
    figure(492);
    semilogy(abs((lambdaHistory(1:blockSize,1:iterationNumber)-...
        repmat(mygather(lambda),1,iterationNumber)))');
    title('Eigenvalue errors for Different Eigenpairs','fontsize',16);
    ylabel('Estimated eigenvalue errors','fontsize',16);
    xlabel('Iteration number','fontsize',16);
    %axis tight;
    %axis([0 maxIterations+1 1e-15 1e3])
    set(gca,'FontSize',14);
    drawnow;
end
varargout(1)={failureFlag};
varargout(2)={lambdaHistory(1:blockSize,1:iterationNumber)};
varargout(3)={residualNormsHistory(1:blockSize,1:iterationNumber-1)};
end


function [dyn] = N2dyn(N)
% Convert force from newtons to dyne.
% Chad A. Greene 2012
dyn = N*100000;


function [locs pks]=peakseek(x,minpeakdist,minpeakh)
% Alternative to the findpeaks function.  This thing runs much much faster.
% It really leaves findpeaks in the dust.  It also can handle ties between
% peaks.  Findpeaks just erases both in a tie.  Shame on findpeaks.
%
% x is a vector input (generally a timecourse)
% minpeakdist is the minimum desired distance between peaks (optional, defaults to 1)
% minpeakh is the minimum height of a peak (optional)
%
% (c) 2010
% Peter O'Connor
% peter<dot>ed<dot>oconnor .AT. gmail<dot>com

if size(x,2)==1, x=x'; end

% Find all maxima and ties
locs=find(x(2:end-1)>=x(1:end-2) & x(2:end-1)>=x(3:end))+1;

if nargin<2, minpeakdist=1; end % If no minpeakdist specified, default to 1.

if nargin>2 % If there's a minpeakheight
    locs(x(locs)<=minpeakh)=[];
end

if minpeakdist>1
    while 1

        del=diff(locs)<minpeakdist;

        if ~any(del), break; end

        pks=x(locs);

        [garb mins]=min([pks(del) ; pks([false del])]); %#ok<ASGLU>

        deln=find(del);

        deln=[deln(mins==1) deln(mins==2)+1];

        locs(deln)=[];

    end
end

if nargout>1,
    pks=x(locs);
end

end


function [ y2, j2, f2 ] = ymdf_to_yjf_islamic ( y1, m1, d1, f1 )

%*****************************************************************************80
%
%% YMDF_TO_YJF_ISLAMIC converts from YMDF to YJF form in the Islamic calendar.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y1, M1, D1, real F1,
%    the YMDF date.
%
%    Output, integer Y2, J2, real F2, the YJF date.
%

%
%  Check the date.
%
  [ y1, m1, d1, ierror ] = ymd_check_islamic ( y1, m1, d1 );

  if ( ierror ~= 0 )
    y2 = 0;
    j2 = 0;
    f2 = 0.0;
    return
  end

  y2 = y1;
  j2 = d1;
  f2 = f1;
%
%  Add in the days of the elapsed months.
%
  for m = 1 : m1 - 1
    j2 = j2 + month_length_islamic ( y2, m );
  end

  return
end


function tQuantUnif

FPDF = PDFFn('Laplace');

Sym = true;
%Sym = false;
NlevA = [4 8 16 32 64 128 256];
for (Nlev = NlevA)
  [Yq, Xq, MSE, Entropy, SNRdB, sdV] = QuantUnif(Nlev, FPDF, Sym);
%  Xq
%  Yq
  fprintf('Nlev:%4d, SNR =%5.1f dB, s/V =%6.3f, Entropy =%5.2f bits\n', ...
      Nlev, SNRdB, sdV, Entropy);
end

return


function ierror = part_rsf_check ( n, npart, a )

%*****************************************************************************80
%
%% PART_RSF_CHECK checks a reverse standard form partition of an integer.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer N, the integer to be partitioned.
%    N must be positive.
%
%    Input, integer NPART, the number of parts of the partition.
%    1 <= NPART <= N.
%
%    Input, integer A(NPART), contains the partition.
%    A(1) through A(NPART) contain the nonzero integers which
%    sum to N.  The entries must be in ASCENDING order.
%
%    Output, integer IERROR, error flag.
%    0, no error.
%    -1, N is illegal.
%    -2, NPART is illegal.
%    -3, the entries do not add up to N.
%    I, the I-th entry of A is illegal.
%
  ierror = 0;

  if ( n < 1 )
    ierror = -1;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PART_RSF_CHECK - Fatal error!\n' );
    fprintf ( 1, '  N < 1.\n' );
    error ( 'PART_RSF_CHECK - Fatal error!\n' );
  end

  if ( npart < 1 || n < npart )
    ierror = -2;
    return
  end
%
%  Every entry must lie between 1 and N.
%
  for i = 1 : npart
    if ( a(i) < 1 || n < a(i) )
      ierror = i;
      return
    end
  end
%
%  The entries must be in ascending order.
%
  for i = 2 : npart
    if ( a(i) < a(i-1) )
      ierror = i;
      return
    end
  end
%
%  The entries must add up to N.
%
  if ( sum ( a(1:npart) ) ~= n )
    ierror = -3;
  end

  return
end


function pass = test_fevalm( pref )
% Test spherefun/fevalm

if ( nargin == 0)
    pref = chebfunpref;
end

tol = 100*pref.cheb2Prefs.chebfun2eps;
rng(2016);

% Check empty spherefun:
f = spherefun;
s = pi*(2*rand(5,1) - 1);
t = pi/2*rand(5,1);
B = fevalm(f, s, t);
pass(1) = isempty( B );

% Check rank 1 spherefun:
f = chebfun2(@(lam,th) cos(lam).*sin(th));
s = pi*(2*rand(5,1) - 1);
t = pi/2*rand(5,1);
[ss, tt] = meshgrid( s, t);
A = feval(f, ss, tt);
B = fevalm(f, s, t);
pass(2) = norm( A - B ) < tol;

% Check essentially one dimensional function:
f = spherefun(@(lam,th) exp(-(cos(th)-1).^2));
s = pi*(2*rand(5,1) - 1);
t = pi/2*rand(5,1);
[ss, tt] = meshgrid( s, t);
A = feval(f, ss, tt);
B = fevalm(f, s, t);
pass(3) = norm( A - B ) < tol;

end