function [x, info] = lewisCenter(f, x, opts)
% x = LewisCenter(f, opts, x)
% compute the analytic center for the domain {Ax=b} intersect the domain of f
%
% Input:
%    f - a ConvexProgram
%    x - a feasible initial point
%    opts - a structure for options with the following properties (optional)
%       MaxIter - maximum number of iterations
%       Output - maximum number of iterations
%       CentralityTol, FeasibilityTol - stop the following are satisfied
%           ||(A' * lambda - grad f(x)) / sqrt(hess)||_inf < CentralityTol
%           ||A x - b||_inf < FeasibilityTol
%       p - the parameter for lp Lewis weight
%       JLDim - the number of dimensions used in estimating leverage score
%
% Output:
%  x - It outputs the analytic center of f
%  info - a structure containing centrality, feasibility and iter.

defaultOpts = struct('MaxIter', 100, 'Output', @disp, 'CentralityTol', 0.1, 'FeasibilityTol', 1e-12, 'JLDim', 10, 'p', 4);
if nargin >= 3
   opts = setField(defaultOpts, opts);
else
   opts = defaultOpts;
end
x = ddouble(x);

%% prepare the printout
output = TableDisplay('Iter', '5i', 'PredStep', '13.2e', 'CorrStep', '13.2e', 'Centrality', '13.2e', 'Feasibility', '13.2e');
output.output = opts.Output;
output.header();

%% initial parameters
assert(nargin >= 2 && all(f.distance(x) > 0), 'a feasible inital point is required.');

A = f.A; At = A'; b = f.b;
y = 0*b;
lastProgress = 0; % record the last iteration making progress
bestCentrality = 1e32;
d = size(f.A, 1); n = size(f.A, 2);
w = (n-d)/n * ones(n, 1);

%% find the central path
[rs, rx, h] = updateResidual(x, y, w);

for iter = 1:opts.MaxIter
   % Compute the cholesky decomposition
   cholErr = f.solver.factorize(1./h);
   w_new = max(double(1-f.solver.leverageScore(opts.JLDim)), 0);
   w = (w + w_new)/2;

   % Compute the direction
   if (cholErr < f.solver.cholTol)
      v = f.solver.solve([A*(rs./h) rx]); %rs = A^T y - g; %rx = Ax - b;
      Atv = At * v;
      y = y - v(:,1);

      % y = y - (R\(R'\(A*(rs./hess))))
      % dx = (rs + At * (R\(R'\(rx - A*(rs./hess)))))./hess;

      dx = (rs - Atv(:,1))./h - Atv(:,2)./h;
      t = stepSize(x, dx, 0.95); % 0.95*(distance from x to the closest boundart in direction dx)
      x = x + t * dx; % move in direction dx with distance of t (measured in dx)

      [rs, rx, h] = updateResidual(x, y, w);

      centrality = max(abs(rs./sqrt(h))); %||(A^T y - grad \phi)/sqrt(h)||_Inf
      feasibility = max(abs(rx)); %||Ax-b||_Inf
      if isempty(feasibility), feasibility = 0; end % Fix the case rx is []

      % Output the error
      o = struct('Iter', iter, 'PredStep', t, 'CorrStep', t, 'Centrality', centrality, 'Feasibility', feasibility);
      output.row(o);

      if (centrality < 0.9 * bestCentrality)
         bestCentrality = centrality;
         lastProgress = iter;
      end

      % Check stop criteria
      if centrality < opts.CentralityTol && feasibility < opts.FeasibilityTol
         break;
      end
   end

   if (iter > lastProgress + 10)
      break;
   end

   if (cholErr >= f.solver.cholTol)
      opts.Output('Failed due to numerical issue.');
      f.feasible = false;
      x = []; info = [];
      return;
   end
end

info = struct('centrality', centrality, 'feasibility', feasibility, 'iter', iter, 'hess', h);

function t = stepSize(x, dx, factor)
   t = min(double(factor*min(f.barrier.distance(x, dx))),1); % min(Nan,1) = 1 for double
end

function [rs, rx, h, wp] = updateResidual(x, y, w)
   wp = w.^(1-2/opts.p);

   [gb, hb] = f.barrier.derivatives(x); % obtain gradient and hessian of log-barrier ftn \phi(x) = -\sum log(x-l) -\sum log(u-x)
   % hp = diagonals of hessian matrix of log-barrier

   gb = gb.*wp;
   hb = hb.*wp;

   gc = zeros(size(gb), class(gb));
   hc = zeros(size(hb), class(hb));

   % Below if-end is skipped while running f.normalize() in sample.m (due to c=[])
   if ~isempty(f.c)
      gc = gc + f.c;
   end

   % Below if-end is skipped while running f.normalize() in sample.m (due to df=[])
   if ~isempty(f.df)
      gc_ = f.df(f.export(x));
      gc = gc + f.scale .* gc_(f.idx);

      hc_ = f.ddf(f.export(x));
      hc = hc + f.scale2 .* hc_(f.idx);
   end

   % Thus, g=gb and h=hb in f.normalize()
   g = gb + gc; h = hb + hc;

   rs = At * y - g;
   rx = A * x - b;
end

end


function SuperEllipsePlaneRhoTxi
mxMax = 0.99;
myMax = 0.99;
mxMin = 0;
myMin = 0;
c = SuperEllipseParamsRelator.c);
qV = [2 10^6];
color = {'b','r'};
outFile = '/home/alex/Dropbox/PaperStress/AddmissibleSpace/';

% for iq = 1:length(qV)
%     q = qV(iq);
%     rhoMin = 1 - c(q)*mxMax*myMax;
%     rho(:,1) = linspace(rhoMin,1, 100);
%     txiUB(:,1) = atan(mxMax^2*c(q)./(1-rho));
%     txiLB(:,1) = atan((1-rho)/(myMax^2*c(q)));
%     plot(rho,[txiLB txiUB],['-',color{iq}])
%     set(gca,'ytick',[0:pi/8:pi/2]) % where to set the tick marks
%     set(gca,'yticklabels',{'0','\pi/8','\pi/4','3\pi/8','\pi/2'})
%     xlabel('\rho')
% end

myCase = 'A';

switch myCase
    case 'A'

        %figure(1)
        for iq = 1:length(qV)
            q = qV(iq);
            txi = linspace(0,pi/2,1000);
            s.q = q;
            s.txi = txi;
            s.mxMin = mxMin;
            s.myMin = myMin;
            s.mxMax = mxMax;
            s.myMax = myMax;
            rhoBounds = SuperEllipseRhoBoundsComputer(s);
            [rhoMin,rhoMax] = rhoBounds.compute();

            figure()
            h1 = plot(txi,rhoMin,['-',color{iq}]);
            set(h1,'LineWidth',2);
            hold on
            h2 = plot(txi,rhoMax,['-',color{iq}]);
            set(h2,'LineWidth',2);
            set(gca,'xtick',[0:pi/8:pi/2]) % where to set the tick marks
            set(gca,'xticklabels',{'0','\pi/8','\pi/4','3\pi/8','\pi/2'})
            xlabel('\xi')
            ylabel('\rho')
            set(gca,'FontSize',18)
            axis([0 pi/2 0 1])
            print([outFile,'AdmissibleSpaceForQ',num2str(q)],'-dpdf')
        end
    case 'B'
        figure(1)
        hold on
        for iq = 1:length(qV)
            q = qV(iq);
            txi = linspace(0,pi/2,1000);
            rhoMinMx = 1- mxMax^2*c(q)./tan(txi);
            rhoMinMy = 1- myMax^2*c(q)*tan(txi);
            rhoMin = max(rhoMinMx,rhoMinMy);
            h1 = plot(txi,rhoMin,['-',color{iq}]);
            set(h1,'LineWidth',2);
            set(gca,'xtick',[0:pi/8:pi/2]) % where to set the tick marks
            set(gca,'xticklabels',{'0','\pi/8','\pi/4','3\pi/8','\pi/2'})
            xlabel('\xi')
            ylabel('\rho')
            set(gca,'FontSize',18)
            axis([0 pi/2 0 1])
            print([outFile,'AdmissibleSpace',num2str(q)],'-dpdf')
        end
end

end


function triangulation_display ( prefix, node_show, element_show, ...
  neighbor_show )

%*****************************************************************************80
%
%% TRIANGULATION_DISPLAY displays a triangulation.
%
%  Discussion:
%
%    TRIANGULATION_DISPLAY displays a triangulated set of nodes on
%    the MATLAB graphics screen.
%
%  Usage:
%
%    triangulation_display ( 'prefix', node_show, element_show, (neighbor_show) )
%
%    where:
%
%    'prefix' is the common prefix for the node and element files:
%
%    * prefix_nodes.txt,             the node coordinates.
%    * prefix_elements.txt,          the nodes that make up each element.
%    * prefix_element_neighbors.txt, (optional), the neighbors of each element.
%
%    'node_show' indicates the node visibility:
%
%    0: do not show the nodes;
%    1:        show the nodes;
%    2:        show the nodes, and label them.
%
%    'element_show' indicates the element visibility:
%
%    0: do not show the element;
%    1:        show the element;
%    2:        show the element, and color them.
%    3:        show the element, and color them, and label them.
%
%    'neighbor_show' indicates the element neighbor visibility,
%    if the element neighbor file exists:
%
%    0: do not show the element neighbor labels;
%    1:        show the element neighbor labels.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 April 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  timestamp ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRIANGULATION_DISPLAY\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read a node dataset of NODE_NUM points in 2 dimensions.\n' );
  fprintf ( 1, '  Read an associated triangulation dataset of ELEMENT_NUM\n' );
  fprintf ( 1, '  elements using 3, 4 or 6 nodes.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Display an image of the data on the MATLAB graphics screen.\n' );
%
%  First argument is the file prefix.
%
  if ( nargin < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRIANGULATION_DISPLAY:\n' );
    prefix = input ( '  Enter the file prefix:  ' );
  end
%
%  Create the file names.
%
  node_filename = strcat ( prefix, '_nodes.txt' );
  element_filename = strcat ( prefix, '_elements.txt' );
  element_neighbor_filename = strcat ( prefix, '_element_neighbors.txt' );
%
%  Second argument is node visibility.
%
  if ( nargin < 2 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Options for node visibility:\n' );
    fprintf ( 1, '  0: do not show the nodes;\n' );
    fprintf ( 1, '  1:        show the nodes;\n' );
    fprintf ( 1, '  2:        show the nodes, and label them.\n' );
    node_show = input ( '  Enter the node visibility option:  ' );
  end
%
%  Third argument is element visibility.
%
  if ( nargin < 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Options for element visibility:\n' );
    fprintf ( 1, '  0: do not show the elements;\n' );
    fprintf ( 1, '  1:        show the elements;\n' );
    fprintf ( 1, '  2:        show the elements, and color them.\n' );
    fprintf ( 1, '  3:        show the elements, and color them, and label them.\n' );
    element_show = input ( '  Enter the element visibility option:  ' );
  end
%
%  Fourth argument is neighbor visibility.
%
  if ( nargin < 4 && file_exist ( element_neighbor_filename ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Options for element neighbor visibility:\n' );
    fprintf ( 1, '  0: do not show the element neighbor labels;\n' );
    fprintf ( 1, '  1:        show the element neighbor labels;\n' );
    neighbor_show = input ( '  Enter the element neighbor visibility option:  ' );
  end
%
%  Read the node data.
%
  [ dim_num, node_num ] = r8mat_header_read ( node_filename );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the header of "%s".\n', node_filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Spatial dimension DIM_NUM = %d\n', dim_num );
  fprintf ( 1, '  Number of nodes NODE_NUM  = %d\n', node_num );

  node_xy = r8mat_data_read ( node_filename, dim_num, node_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the data in "%s".\n', node_filename );

  r8mat_transpose_print_some ( dim_num, node_num, node_xy, 1, 1, 2, 5, ...
    '  Initial portion of NODE_XY:' );
%
%  Read the element data.
%
  [ element_order, element_num ] = i4mat_header_read ( element_filename );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the header of "%s".\n', element_filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Element order      = %d\n', element_order );
  fprintf ( 1, '  Number of elements = %d\n', element_num );

  element_node = i4mat_data_read ( element_filename, ...
    element_order, element_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the data in "%s".\n', element_filename );

  i4mat_transpose_print_some ( element_order, element_num, element_node, ...
    1, 1, element_order, 5, '  Initial portion of ELEMENT_NODE:' );
%
%  If the neighbor file exists, read the neighbor data.
%
  if ( file_exist ( element_neighbor_filename ) && 0 < neighbor_show )

    [ three, element_num ] = i4mat_header_read ( element_neighbor_filename );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Read the header of "%s".\n', element_neighbor_filename );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Element order      = %d\n', element_order );
    fprintf ( 1, '  Number of elements = %d\n', element_num );

    element_neighbor = i4mat_data_read ( element_neighbor_filename, ...
      3, element_num );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Read the data in "%s".\n', element_neighbor_filename );

    i4mat_transpose_print_some ( 3, element_num, element_neighbor, ...
      1, 1, element_order, 5, '  Initial portion of ELEMENT_NEIGHBOR:' );

  else

    neighbor_show = 0;
    element_neighbor = [];

  end
%
%  Detect and correct zero-based node indexing.
%
  element_node = mesh_base_one ( node_num, element_order, element_num, ...
    element_node );
%
%  Display a plot.
%
  title_string = s_escape_tex ( prefix );

  triangulation_display_matlab ( title_string, node_num, node_xy, element_order, ...
    element_num, element_node, element_neighbor, node_show, element_show, ...
    neighbor_show );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRIANGULATION_DISPLAY\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function column_num = file_column_count ( input_file_name )

%*****************************************************************************80
%
%% FILE_COLUMN_COUNT counts the columns in the first line of a file.
%
%  Discussion:
%
%    The file is assumed to be a simple text file.
%
%    Most lines of the file are presumed to consist of COLUMN_NUM words,
%    separated by spaces.  There may also be some blank lines, and some
%    comment lines, which have a "#" in column 1.
%
%    The routine tries to find the first non-comment non-blank line and
%    counts the number of words in that line.
%
%    If all lines are blanks or comments, it goes back and tries to analyze
%    a comment line.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILE_NAME, the name of the file.
%
%    Output, integer COLUMN_NUM, the number of columns in the file.
%
  FALSE = 0;
  TRUE = 1;
%
%  Open the file.
%
  input_unit = fopen ( input_file_name );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_COLUMN_COUNT - Error!\n' );
    fprintf ( 1, '  Could not open the file "%s".\n', input_file_name );
    error ( 'FILE_COLUMN_COUNT - Error!' );
  end
%
%  Read one line, but skip blank lines and comment lines.
%  Use FGETL so we drop the newline character!
%
  got_one = FALSE;

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

    if ( s_len_trim ( line ) == 0 )

    elseif ( line(1) == '#' )

    else
      got_one = TRUE;
      break;
    end

  end

  fclose ( input_unit );

  if ( got_one == FALSE )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_COLUMN_COUNT - Warning!\n' );
    fprintf ( 1, '  The file does not seem to contain any data.\n' );
    column_num = -1;
    return;
  end

  column_num = s_word_count ( line );

  return
end
function value = file_exist ( file_name )

%*****************************************************************************80
%
%% FILE_EXIST reports whether a file exists.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, character FILE_NAME, the name of the file.
%
%    Output, logical FILE_EXIST, is TRUE if the file exists.
%
  fid = fopen ( file_name );

  if ( fid == -1 )
    value = 0;
  else
    fclose ( fid );
    value = 1;
  end

  return
end
function row_num = file_row_count ( input_file_name )

%*****************************************************************************80
%
%% FILE_ROW_COUNT counts the number of row records in a file.
%
%  Discussion:
%
%    Each input line is a "RECORD".
%
%    The records are divided into three groups:
%
%    * BLANK LINES (nothing but blanks)
%    * COMMENT LINES (begin with a '#')
%    * DATA RECORDS (anything else)
%
%    The value returned by the function is the number of data records.
%
%    By the way, if the MATLAB routine FGETS is used, instead of
%    FGETL, then the variable LINE will include line termination
%    characters, which means that a blank line would not actually
%    have zero characters.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 December 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILE_NAME, the name of the input file.
%
%    Output, integer ROW_NUM, the number of rows found.
%
  input_unit = fopen ( input_file_name );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_ROW_COUNT - Error!\n' );
    fprintf ( 1, '  Could not open the file "%s".\n', input_file_name );
    error ( 'FILE_ROW_COUNT - Error!' );
  end

  blank_num = 0;
  comment_num = 0;
  row_num = 0;

  record_num = 0;

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

    record_num = record_num + 1;
    record_length = s_len_trim ( line );

    if ( record_length <= 0 )
      blank_num = blank_num + 1;
    elseif ( line(1) == '#' )
      comment_num = comment_num + 1;
    else
      row_num = row_num + 1;
    end

  end

  fclose ( input_unit );

  return
end
function value = i4_modp ( i, j )

%*****************************************************************************80
%
%% I4_MODP returns the nonnegative remainder of I4 division.
%
%  Discussion:
%
%    If
%      NREM = I4_MODP ( I, J )
%      NMULT = ( I - NREM ) / J
%    then
%      I = J * NMULT + NREM
%    where NREM is always nonnegative.
%
%    The MOD function computes a result with the same sign as the
%    quantity being divided.  Thus, suppose you had an angle A,
%    and you wanted to ensure that it was between 0 and 360.
%    Then mod(A,360) would do, if A was positive, but if A
%    was negative, your result would be between -360 and 0.
%
%    On the other hand, I4_MODP(A,360) is between 0 and 360, always.
%
%  Example:
%
%        I     J     MOD  I4_MODP    Factorization
%
%      107    50       7       7    107 =  2 *  50 + 7
%      107   -50       7       7    107 = -2 * -50 + 7
%     -107    50      -7      43   -107 = -3 *  50 + 43
%     -107   -50      -7      43   -107 =  3 * -50 + 43
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 1999
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I, the number to be divided.
%
%    Input, integer J, the number that divides I.
%
%    Output, integer VALUE, the nonnegative remainder when I is
%    divided by J.
%
  if ( j == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4_MODP - Fatal error!\n' );
    fprintf ( 1, '  Illegal divisor J = %d\n', j );
    error ( 'I4_MODP - Fatal error!' );
  end

  value = mod ( i, j );

  if ( value < 0 )
    value = value + abs ( j );
  end

  return
end
function value = i4_wrap ( ival, ilo, ihi )

%*****************************************************************************80
%
%% I4_WRAP forces an integer to lie between given limits by wrapping.
%
%  Example:
%
%    ILO = 4, IHI = 8
%
%    I   Value
%
%    -2     8
%    -1     4
%     0     5
%     1     6
%     2     7
%     3     8
%     4     4
%     5     5
%     6     6
%     7     7
%     8     8
%     9     4
%    10     5
%    11     6
%    12     7
%    13     8
%    14     4
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 October 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer IVAL, an integer value.
%
%    Input, integer ILO, IHI, the desired bounds for the integer value.
%
%    Output, integer I4_WRAP, a "wrapped" version of IVAL.
%
  jlo = min ( ilo, ihi );
  jhi = max ( ilo, ihi );

  wide = jhi - jlo + 1;

  if ( wide == 1 )
    value = jlo;
  else
    value = jlo + i4_modp ( ival - jlo, wide );
  end

  return
end
function table = i4mat_data_read ( input_filename, m, n )

%*****************************************************************************80
%
%% I4MAT_DATA_READ reads data from an I4MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Input, integer M, N, the number of rows and columns in the data.
%
%    Output, integer TABLE(M,N), the point coordinates.
%
  table = zeros ( m, n );
%
%  Build up the format string for reading M real numbers.
%
  string = ' ';

  for i = 0 : m
    string = strcat ( string, ' %d' );
  end

  input_unit = fopen ( input_filename );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_DATA_READ - Error!\n' );
    fprintf ( 1, '  Could not open the input file.\n' );
    error ( 'I4MAT_DATA_READ - Error!' );
  end

  i = 0;

  while ( i < n )

    line = fgets ( input_unit );

    if ( line == -1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'I4MAT_DATA_READ - Error!\n' );
      fprintf ( 1, '  End of input while reading data.\n' );
      error ( 'I4MAT_DATA_READ - Error!' );
    end

    if ( line(1) == '#' )

    elseif ( s_len_trim ( line ) == 0 )

    else

      [ x, count ] = sscanf ( line, string );

      if ( count == m )
        i = i + 1;
        table(1:m,i) = x(1:m);
      end

    end

  end

  fclose ( input_unit );

  return
end
function [ m, n ] = i4mat_header_read ( input_filename )

%*****************************************************************************80
%
%% I4MAT_HEADER_READ reads the header from an I4MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Output, integer M, the spatial dimension.
%
%    Output, integer N, the number of points.
%
  m = file_column_count ( input_filename );

  if ( m <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data columns in\n' );
    fprintf ( 1, '  the file %s.\n', input_filename );
  end

  n = file_row_count ( input_filename );

  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data rows in\n' );
    fprintf ( 1, '  the file %s\n', input_filename );
  end

  return
end
function i4mat_transpose_print_some ( m, n, a, ilo, jlo, ihi, jhi, title )

%*****************************************************************************80
%
%% I4MAT_TRANSPOSE_PRINT_SOME prints some of an I4MAT, transposed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 June 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, integer A(M,N), an M by N matrix to be printed.
%
%    Input, integer ILO, JLO, the first row and column to print.
%
%    Input, integer IHI, JHI, the last row and column to print.
%
%    Input, string TITLE, a title.
%
  incx = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, '%s\n', title );

  for i2lo = max ( ilo, 1 ) : incx : min ( ihi, m )

    i2hi = i2lo + incx - 1;
    i2hi = min ( i2hi, m );
    i2hi = min ( i2hi, ihi );

    inc = i2hi + 1 - i2lo;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Row: ' );
    for i = i2lo : i2hi
      fprintf ( 1, '%7d  ', i );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Col\n' );
    fprintf ( 1, '\n' );

    j2lo = max ( jlo, 1 );
    j2hi = min ( jhi, n );

    for j = j2lo : j2hi

      fprintf ( 1, '%5d  ', j );
      for i2 = 1 : inc
        i = i2lo - 1 + i2;
        fprintf ( 1, '%7d  ', a(i,j) );
      end
      fprintf ( 1, '\n' );

    end

  end

  return
end
function element_node = mesh_base_one ( node_num, element_order, ...
  element_num, element_node )

%*****************************************************************************80
%
%% MESH_BASE_ONE ensures that the element definition is one-based.
%
%  Discussion:
%
%    The ELEMENT_NODE array contains nodes indices that form elements.
%    The convention for node indexing might start at 0 or at 1.
%    Since a MATLAB program will naturally assume a 1-based indexing, it is
%    necessary to check a given element definition and, if it is actually
%    0-based, to convert it.
%
%    This function attempts to detect 0-based node indexing and correct it.
%
%    Thanks to Feifei Xu for pointing out that I was subtracting 1 when I
%    should have been adding 1!  29 November 2012.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 November 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, integer ELEMENT_ORDER, the order of the elements.
%
%    Input, integer ELEMENT_NUM, the number of elements.
%
%    Input/output, integer ELEMENT_NODE(ELEMENT_ORDER,ELEMENT_NUM), the element
%    definitions.
%
  node_min = min ( min ( element_node(1:element_order,1:element_num) ) );
  node_max = max ( max ( element_node(1:element_order,1:element_num) ) );

  if ( node_min == 0 && node_max == node_num - 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MESH_BASE_ONE:\n' );
    fprintf ( 1, '  The element indexing appears to be 0-based!\n' );
    fprintf ( 1, '  This will be converted to 1-based.\n' );
    element_node(1:element_order,1:element_num) = ...
      element_node(1:element_order,1:element_num) + 1;
  elseif ( node_min == 1 && node_max == node_num )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MESH_BASE_ONE:\n' );
    fprintf ( 1, '  The element indexing appears to be 1-based!\n' );
    fprintf ( 1, '  No conversion is necessary.\n' );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MESH_BASE_ONE - Warning!\n' );
    fprintf ( 1, '  The element indexing is not of a recognized type.\n' );
  end

  return
end
function table = r8mat_data_read ( input_filename, m, n )

%*****************************************************************************80
%
%% R8MAT_DATA_READ reads data from an R8MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Input, integer M, N, the number of rows and columns of data.
%
%    Output, real TABLE(M,N), the point coordinates.
%
  table = zeros ( m, n );
%
%  Build up the format string for reading M real numbers.
%
  string = ' ';

  for i = 0 : m
    string = strcat ( string, ' %f' );
  end

  input_unit = fopen ( input_filename );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_DATA_READ - Error!\n' );
    fprintf ( 1, '  Could not open the file.\n' );
    error ( 'R8MAT_DATA_READ - Error!' );
  end

  i = 0;

  while ( i < n )

    line = fgets ( input_unit );

    if ( line == -1 )
      break;
    end

    if ( line(1) == '#' )

    elseif ( s_len_trim ( line ) == 0 )

    else

      [ x, count ] = sscanf ( line, string );

      if ( count == m )
        i = i + 1;
        table(1:m,i) = x(1:m);
      end

    end

  end

  fclose ( input_unit );

  return
end
function [ m, n ] = r8mat_header_read ( input_filename )

%*****************************************************************************80
%
%% R8MAT_HEADER_READ reads the header from an R8MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Output, integer M, the spatial dimension.
%
%    Output, integer N, the number of points.
%
  m = file_column_count ( input_filename );

  if ( m <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data columns in\n' );
    fprintf ( 1, '  the file %s.\n', input_filename );
  end

  n = file_row_count ( input_filename );

  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data rows in\n' );
    fprintf ( 1, '  the file %s\n', input_filename );
  end

  return
end
function r8mat_transpose_print_some ( m, n, a, ilo, jlo, ihi, jhi, title )

%*****************************************************************************80
%
%% R8MAT_TRANSPOSE_PRINT_SOME prints some of an R8MAT, transposed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, real A(M,N), an M by N matrix to be printed.
%
%    Input, integer ILO, JLO, the first row and column to print.
%
%    Input, integer IHI, JHI, the last row and column to print.
%
%    Input, string TITLE, a title.
%
  incx = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, '%s\n', title );

  for i2lo = max ( ilo, 1 ) : incx : min ( ihi, m )

    i2hi = i2lo + incx - 1;
    i2hi = min ( i2hi, m );
    i2hi = min ( i2hi, ihi );

    inc = i2hi + 1 - i2lo;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Row: ' );
    for i = i2lo : i2hi
      fprintf ( 1, '%7d       ', i );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Col\n' );

    j2lo = max ( jlo, 1 );
    j2hi = min ( jhi, n );

    for j = j2lo : j2hi

      fprintf ( 1, '%5d ', j );
      for i2 = 1 : inc
        i = i2lo - 1 + i2;
        fprintf ( 1, '%12f', a(i,j) );
      end
      fprintf ( 1, '\n' );

    end

  end

  return
end
function s2 = s_escape_tex ( s1 )

%*****************************************************************************80
%
%% S_ESCAPE_TEX de-escapes TeX escape sequences.
%
%  Discussion:
%
%    In particular, every occurrence of the characters '\', '_',
%    '^', '{' and '}' will be replaced by '\\', '\_', '\^',
%    '\{' and '\}'.  A TeX interpreter, on seeing these character
%    strings, is then likely to return the original characters.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S1, the string to be de-escaped.
%
%    Output, string S2, a copy of the string, modified to avoid TeX escapes.
%
  s1_length = length ( s1 );

  s1_pos = 0;
  s2_pos = 0;
  s2 = [];

  while ( s1_pos < s1_length )

    s1_pos = s1_pos + 1;

    if ( s1(s1_pos) == '\' || ...
         s1(s1_pos) == '_' || ...
         s1(s1_pos) == '^' || ...
         s1(s1_pos) == '{' || ...
         s1(s1_pos) == '}' )
      s2_pos = s2_pos + 1;
      s2 = strcat ( s2, '\' );
    end

    s2_pos = s2_pos + 1;
    s2 = strcat ( s2, s1(s1_pos) );

  end

  return
end
function len = s_len_trim ( s )

%*****************************************************************************80
%
%% S_LEN_TRIM returns the length of a character string to the last nonblank.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 June 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string to be measured.
%
%    Output, integer LEN, the length of the string up to the last nonblank.
%
  len = length ( s );

  while ( 0 < len )
    if ( s(len) ~= ' ' )
      return
    end
    len = len - 1;
  end

  return
end
function word_num = s_word_count ( s )

%*****************************************************************************80
%
%% S_WORD_COUNT counts the number of "words" in a string.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string to be examined.
%
%    Output, integer WORD_NUM, the number of "words" in the string.
%    Words are presumed to be separated by one or more blanks.
%
  FALSE = 0;
  TRUE = 1;

  word_num = 0;
  s_length = length ( s );

  if ( s_length <= 0 )
    return;
  end

  blank = TRUE;

  for i = 1 : s_length

    if ( s(i) == ' ' )
      blank = TRUE;
    elseif ( blank == TRUE )
      word_num = word_num + 1;
      blank = FALSE;
    end

  end

  return
end
function timestamp ( )

%*****************************************************************************80
%
%% TIMESTAMP prints the current YMDHMS date as a timestamp.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  t = now;
  c = datevec ( t );
  s = datestr ( c, 0 );
  fprintf ( 1, '%s\n', s );

  return
end
function triangulation_display_matlab ( title_string, node_num, node_xy, ...
  element_order, element_num, element_node, element_neighbor, node_show, ...
  element_show, neighbor_show )

%*****************************************************************************80
%
%% TRIANGULATION_DISPLAY_MATLAB displays a triangulation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 April 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string TITLE_STRING, a title for the plot.
%
%    Input, integer NODE_NUM, the number of points.
%
%    Input, real NODE_XY(2,NODE_NUM), the nodes.
%
%    Input, integer ELEMENT_ORDER, the order of the elements.
%
%    Input, integer ELEMENT_NUM, the number of elements.
%
%    Input, integer ELEMENT_NODE(ELEMENT_ORDER,ELEMENT_NUM), lists,
%    for each element, the indices of the points that form the vertices
%    of the element.
%
%    Input, integer ELEMENT_NEIGHBOR(3,ELEMENT_NUM).  ELEMENT_NEIGHBOR(I,J)
%    is the neighbor adjacent to the size opposite node ELEMENT_NODE(I,J),
%    or -1 if no such neighbor.
%
%    Input, logical NODE_SHOW:
%    0, do not show the nodes.
%    1, show the nodes.
%    2, show the nodes, and label them.
%
%    Input, logical ELEMENT_SHOW,
%    0, do not show the elements.
%    1, show the elements.
%    2, show the elements, and color them.
%    3, show the elements, and color them, and label them.
%
%    Input, logical NEIGHBOR_SHOW,
%    0, do not show the element neighbor labels.
%    1, show the element neighbor labels.
%

%
%  Clear the graphics screen.
%
  clf;
%
%  Get the scale.
%
  xy_min(1) = min ( node_xy(1,:) );
  xy_max(1) = max ( node_xy(1,:) );

  xy_min(2) = min ( node_xy(2,:) );
  xy_max(2) = max ( node_xy(2,:) );

  xy_range(1:2) = xy_max(1:2) - xy_min(1:2);

  margin = 0.025 * max ( xy_range(1), xy_range(2) );

  x_min = xy_min(1) - margin;
  x_max = xy_max(1) + margin;
  y_min = xy_min(2) - margin;
  y_max = xy_max(2) + margin;
%
%  Draw the element faces.
%
  if ( 2 <= element_show )

    face_color = 'g';

    for element = 1 : element_num
      p = element_node(1:3,element);
      patch ( node_xy(1,p), node_xy(2,p), face_color );
    end

  end
%
%  Draw the element lines.
%
  if ( 1 <= element_show )

    line_color = 'r';

    for element = 1 : element_num
      p = [ element_node(1:3,element)', element_node(1,element) ];
      line ( 'XData', node_xy(1,p), 'YData', node_xy(2,p), ...
        'Color', line_color, 'LineWidth', 2 );
    end

  end
%
%  Label the elements
%
  if ( 3 <= element_show )

    for element = 1 : element_num

      ave_x = 0.0;
      ave_y = 0.0;

      for i = 1 : element_order
        node = element_node(i,element);
        ave_x = ave_x + node_xy(1,node);
        ave_y = ave_y + node_xy(2,node);
      end

      ave_x = ave_x / element_order;
      ave_y = ave_y / element_order;

      text ( ave_x, ave_y, num2str ( element ) );

    end

  end
%
%  Draw the nodes.
%
  hold on

  if ( 1 <= node_show )
    plot ( node_xy(1,:), node_xy(2,:), 'o', 'markersize', 4, ...
      'markerfacecolor', 'black' );
  end
%
%  Label the nodes.
%
  if ( 2 <= node_show )
    if ( node_num <= 100 )
      abit = margin;
    else
      abit = 0.0;
    end
    for j = 1 : node_num
      text ( node_xy(1,j) + abit, node_xy(2,j) + abit, num2str(j) );
    end
  end
%
%  Label the element neighbors.
%
  if ( 1 <= neighbor_show )
    for j = 1 : element_num
      for i = 1 : 3
        np0 = element_node ( i, j );
        ip1 = i4_wrap ( i + 1, 1, 3 );
        np1 = element_node ( ip1, j );
        ip2 = i4_wrap ( i + 2, 1, 3 );
        np2 = element_node ( ip2, j );
        x = 0.40 * node_xy(1,np1) + 0.40 * node_xy(1,np2) + 0.20 * node_xy(1,np0);
        y = 0.40 * node_xy(2,np1) + 0.40 * node_xy(2,np2) + 0.20 * node_xy(2,np0);
        text ( x, y, num2str ( element_neighbor(i,j) ) );
      end
    end
  end
%
%  Label the plot.
%
  xlabel ( '--X axis--' )
  ylabel ( '--Y axis--' )
  title ( title_string )

  axis ( [ x_min, x_max, y_min, y_max ] );
  axis equal

  hold off

  return
end


function [Iq,in_avg,ext_avg,in_min,ext_max]=icassoStability(sR,L,graphmode)
%function Iq=icassoStability(sR,L,graphmode)
%
%PURPOSE
%
%To compute and/or plot the stability (quality) indices of the ICA
%estimate-clusters.
%
% Iq=avg(intra-cluster similarity) - avg(extra-cluster similarity)
%
%See publication Himberg et al. (2004), "Validating the independent
%components of neuroimaging time-series via clustering and
%visualization". NeuroImage, 22:3(1214-1222). Ideally, each ICA
%estimate-cluster should have Iq=1. The smaller the value is, the
%less stable (compact and isolated) the estimate-cluster is.
%
%EXAMPLES OF BASIC USAGE
%
%   Iq=icassoStability(sR);
%
%returns the stability index for each estimate (centrotype) using
%the default number of estimate-clusters.
%
%   Iq=icassoStability(sR,13,'plotindex');
%
%...same as previous but also plots the values (in rank order) and
%uses 13 estimate clusters instead of default.
%
%INPUT
%
%[An argument in brackets is optional. If it isn't  given or it's
% an empty matrix/string, the function will use a default value.]
%
% sR          (struct) Icasso result data structure; clustering
%              must be readily computed.
% [L]         (scalar) number of clusters, default: (reduced) data
%              dimension
% [graphmode] (string) 'none' (default) | 'plotindex' | 'plotstat'
%
%OUTPUTS
%
% Iq  (Lx1 matrix) Iq(C) contains the index value for estimate C
% (actually estimate-cluster C)
%
%DETAILS
%
%The intra-cluster similarities for cluster C mean the mutual
%similarities between the estimates in C, and the extra-cluster
%similarities for C are the similarities between the estimates in C
%and the estimates not in C. The index is the average intra-cluster
%similarity subtracted by the average intra-cluster similarity.
%
%In default graph mode 'none'
% The function computes the index and returns it. No graphical
% output. Iq(C) contains the index for cluster C (That is, for
% estimates sR.cluster.partition(L,:)==C.
%
%In graph mode 'plotindex'
% The function computes the quality index as in mode 'none'
% and plots the index in the active axis (gca) as follows:
% The X-axis shows the value of the index for clusters. The clusters
% are ranked on the Y-axis in descending order according to the
% index. The Y-axis legend shows the cluster number (the same as in
% sR.cluster.partition(L,:)). The values are indicated by black
% dots connected into each other by a dotted line.
%
%In graph mode 'plotstat'
% As previous one but instead of the index,
% some cluster statistics are shown:
%  Light red  indicates the average intra-cluster similarity for a cluster
%  Light blue               average extra-cluster
%  Clear red                minimum intra-cluster
%  Clear blue               maximum extra-cluster
%
%SEE ALSO
% clusterquality
% icassoViz
% icassoShow

%COPYRIGHT NOTICE
%This function is a part of Icasso software library
%Copyright (C) 2003-2005 Johan Himberg
%
%This program is free software; you can redistribute it and/or
%modify it under the terms of the GNU General Public License
%as published by the Free Software Foundation; either version 2
%of the License, or any later version.
%
%This program is distributed in the hope that it will be useful,
%but WITHOUT ANY WARRANTY; without even the implied warranty of
%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%GNU General Public License for more details.
%
%You should have received a copy of the GNU General Public License
%along with this program; if not, write to the Free Software
%Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

% ver 1.2 100501 johan

if nargin<2|isempty(L),
  L=icassoGet(sR,'rdim');
  disp([sprintf('\n') 'Number of estimate-clusters not given: using (reduced)' ...
	   ' data dimension.']);
end

if nargin<3 | isempty(graphmode),
  graphmode='none';
end

clusternumber=1:L;
partition=sR.cluster.partition(L,:);

for i=1:L,
  NofEstimates(i)=sum(partition==i);
end

% compute cluster quality index

[Iq,in_avg,ext_avg]=clusterquality('mean',sR.cluster.similarity,partition);
[tmp,in_min,ext_max]=clusterquality('minmax',sR.cluster.similarity,partition);

% Sort entries according to score

[tmp,order]=sort(-Iq);
score=Iq(order);
in_avg=in_avg(order);
ext_avg=ext_avg(order);
in_min=in_min(order);
ext_max=ext_max(order);
clusternumber=clusternumber(order);
NofEstimates=NofEstimates(order);

% make tick mark labels
for i=1:L,
  ticklabel{i}=num2str(clusternumber(i));
end

% Plot
switch lower(graphmode)
 case 'none'
  return;
 case 'plotindex'
  cla reset;
  h_score=plot(score,1:L,'o:');
  set(h_score,'color','k','markersize',8,'markerfacecolor','k');
  set(gca,'ytick',1:L,'yticklabel',ticklabel,'ydir','reve');
  ax=axis;

  if ax(1)<0,
    axis([ax(1) 1  0.5 L+.5]);
  else
    axis([0 1  0.5 L+.5]);
  end

  grid on;
  xlabel('I_q=avg(S(i)_{int})-avg(S(i)_{ext})');
  title('Stability index (I_q) for ICA estimate-clusters');
 case 'plotstat'
  cla reset;

  h_in_avg=barh(in_avg,0.7); hold on
  h_ext_max=barh(-ext_max,0.35);
  h_in_min=barh(in_min,0.35);
  h_ext_avg=barh(-ext_avg,0.7);

  axis on;
  set(h_in_avg,'facecolor',[1 0.7 0.7]); hold on;
  set(h_in_min,'facecolor',[1 0 0]);
  set(h_ext_avg,'facecolor',[0.7 0.7 1]);
  set(h_ext_max,'facecolor',[0 0 1]);
  axis([-1 1 0.5 L+.5]);

  set(gca,'ytick',1:L,'yticklabel',ticklabel,...
	  'ydir','reve','xtick',[-1 -.5 0 .5 1],...
	  'xgrid','on',...
	  'xticklabel',{'1' '0.5 ' '0' '0.5' '1'});

  title('Statistics on within- and between-cluster similarities');
  xlabel('clusters are ordered according to I_q=avg(S(i)_{int})-avg(S(i)_{ext})');
  legend([h_in_avg(1),h_ext_avg(1),h_in_min(1),h_ext_max(1)],...
	 {'mean S_{in}' 'mean S_{ex}' 'min S_{in}' 'max S_{ex}'},-1);
 otherwise
  error('Graphmode must be ''none'',''plotindex'' or ''plotstat''.');
end

ylabel('Label')


function varargout = subsref(f, index)
%SUBSREF   DELTAFUN subsref.
% ( )
%   F(x) tries to evaluate the distribution F at the points given in x.
%   Mathematically, this doesn't make sense when x coincides with a point having
%   a non-trivial delta function.
%
%   F(f) [TODO]: tries to compose the distribution F with the linear chebfun f.
%
% .
%   F.PROP returns the property PROP of F as defined by GET(F, 'PROP').
%
% {}
%   F{S1, S2} restricts F to the subdomain [S1, S2]. See DELTAFUN/RESTRICT for
%   further details. Note that F{[S1, S2]} is not supported due to the behaviour
%   of the MATLAB subsref() command.
%
% See also FEVAL, COMPOSE, GET.

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

idx = index(1).subs;
switch index(1).type

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%% FEVAL / COMPOSE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    case '()'

        % Where to evaluate:
        x = idx{1};
        varin = {};

        % Deal with additional arguments:
        if ( length(idx) == 2 )
            varin = {idx(2)};
        elseif ( length(idx) > 2 )
            error('CHEBFUN:DELTAFUN:subsref:dimensions', ...
                'Index exceeds chebfun dimensions.')
        end

        % Compute the output:
        if ( isnumeric(x) )
            % Call FEVAL():
            out = feval(f, x, varin{:});

        elseif ( isa(x, 'chebfun') )
            % Call COMPOSE():
            % TODO: write compose and check for linearity of f?
            out = compose(x, f);
        else
            error('CHEBFUN:DELTAFUN:subsref:nonnumeric', ...
              'Cannot evaluate chebfun for non-numeric type.')
        end

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% GET %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    case '.'

        % Call GET() for .PROP access.
        out = get(f, idx);
        if ( numel(index) > 1 )
            % Recurse on SUBSREF():
            index(1) = [];
            out = subsref(out, index);
        end

%% %%%%%%%%%%%%%%%%%%%%%%%%% ACTION of a DISTRIBUTION %%%%%%%%%%%%%%%%%%%%%%%%
    case '{}'
        if ( length(idx) == 1 )
            if ( isequal(idx{1}, ':') )
                % F{:} returns F:
                out = f;
            else
                error('CHEBFUN:DELTAFUN:subsref:badDomain', 'Invalid domain syntax.')
            end

        elseif ( size(idx, 1) == 1 )
            % F{s1,s2,...,sk} returns RESTRICT(F, [s1,s2,...,sk]):
            x = cat(2, idx{:});
            out = restrict(f, x);
        else
            error('CHEBFUN:DELTAFUN:subsref:dimensions', ...
                'Index exceeds chebfun dimensions.')
        end

    otherwise

        error('CHEBFUN:DELTAFUN:subsref:unexpectedType',...
            ['??? Unexpected index.type of ', index(1).type]);
end

% Convert to a cell:
varargout = {out};

end


%TEST_TIMING_DGT_FAC  Test timing factorization DGTs
%
%   This script test the timing SPREADADJs by comparing the results to
%   spreadadj in the main toolbox. Therefore, the correctness of
%   spreadadj must be verified first.

routinemax=6;

spfraction=.1;

test_failed=0;

disp('--- Used subroutines ---');

for rtype=1:2

  if rtype==1
    rname='REAL ';
  else
    rname='CMPLX';
  end;

  for sptype=1:2

    if sptype==1
      spname='FULL  ';
    else
      spname='SPARSE';
    end;

    for L=12:13

      if rtype==1
        if sptype==1
          coef=rand(L,L);
        else
          coef=sprand(L,L,spfraction);
        end;
      else
        if sptype==1
          coef=crand(L,L);
        else
          coef=spcrand(L,L,spfraction);
        end;
      end;

      cadj=spreadadj(coef);

      for rout=1:routinemax
        cadj2=feval(['ref_spreadadj_',num2str(rout)],coef);

        rdiff=cadj-cadj2;

        res=norm(rdiff(:));

        fail='';
        if res>10e-10
          fail='FAILED';
          test_failed=test_failed+1;
        end;

        s=sprintf('ADJ %s %s %i L:%3i %0.5g %s',rname,spname,rout,L,res,fail);
        disp(s)
      end;

    end;

  end;

end;

test_failed


function [obXM,obYM,lp] = mrSpreadInplanes(numofanats,obXM,obYM,lp)
%NAME:   [obXM,obYM,lp] = mrSpreadInplanes(numofanats,obXM,obYM,lp)
%AUTHOR:  Poirson
%DATE:	  08.04.96
%PURPOSE: One of a set of routines that allows user
%         to set and select a set of oblique planes in saggital slice.
%	  The routines are mrTransInplanes.m, mrRotInplanes.m,
%	  mrClipInplanes.m, mrSelInPlane.m, mrSetupInplanes.m,
%         mrSpreadInplanes.m
%HISTORY: Started with mrGetOblPlane from G. Boynton 4/6/96
%NOTES:
%BUGS:
%  Loses the cropping information when changing inplane spread.

INCDELTA = 0.25;

% One more pair of points for the perpendicular line
nPtPairs = numofanats + 1;

% See if the user has already been working on some inPlanes
if size(obXM,1) == 0
	disp('You must first create a candidate set of Inplanes');
	disp('Choose Set_Up_Inplanes first');
	return
end

% Use X and Y travel of perpendicular line
% to find incX and incY between lines.
incX = (obXM(nPtPairs,2)- obXM(nPtPairs,1))/(numofanats-1);
incY = (obYM(nPtPairs,2)- obYM(nPtPairs,1))/(numofanats-1);

% percent of change in X and Y direction;
fractionX = incX/(incX+incY);
fractionY = incY/(incX+incY);

disp('');
disp('--Setting Distance Between Inplanes--');
disp('  Left Button=Decrease, Middle Button=Increase, Right Button=Quit');
button = 0;
while(button~=3)
 [tempx,tempy,button]=mrGinput(1,'cross');
 if (button~=3)
  if (button == 1)
	delta = -1*INCDELTA;
  end
  if (button == 2)
	delta = INCDELTA;
  end

  deltaX = fractionX * delta;
  deltaY = fractionY * delta;

  incX = incX + deltaX;
  incY = incY + deltaY;

  for i=1:numofanats
	obXM(i,:) = obXM(1,:) + (i-1)*incX;
	obYM(i,:) = obYM(1,:) + (i-1)*incY;
  end

  % the perpendicular line, (starting point is the same)
  obXM(nPtPairs,2) = obXM(nPtPairs,1)+(numofanats-1)*incX;
  obYM(nPtPairs,2) = obYM(nPtPairs,1)+(numofanats-1)*incY;

  % draw the new lines on the screen
  for i=1:nPtPairs
     delete(lp(i));
     lp(i)=line(obXM(i,:),obYM(i,:),'Color','w');
  end
 end
end

disp('--Done Setting Distance Between Inplanes--');
disp('');

return;


function  p2 = project_3D_to_2D(p3)
d = distmatrix(p3);
p2_0 = p3(:,1:2);
fun = @(x)myfunc(x,d);

warnid = {'optim:fsolve:NonSquareSystem'};
for ii = 1:length(warnid)
    warning('OFF', warnid{ii});
end

p2 = fsolve(fun, p2_0, optimset('Display','off'));

for ii = 1:length(warnid)
    warning('ON', warnid{ii});
end

% -------------------------------------------------------
function  F = myfunc(p, d)
F = [];
kk = 1;
c = 1.2;
for ii = 1:size(d,1)
    for jj = 1:size(d,2)
        if d(ii,jj) == 0
            continue
        end
        F(kk) = sqrt((p(ii,1) - p(jj,1))^2 + (p(ii,2) - p(jj,2))^2) - d(ii,jj)*c;
        kk = kk+1;
    end
end

% -------------------------------------------------------------------
function d = distmatrix(varargin)

d=[];

v1 = double(varargin{1});
if size(v1,2) == 2
    v1 = [v1, zeros(size(v1,1),1)];
end

if length(varargin)==1

    n = size(v1,1);
    d = zeros(n);
    for i = 1:n
        for j = i+1:n
            d(i,j) = ((v1(i,1) - v1(j,1))^2 + ...
                      (v1(i,2) - v1(j,2))^2 + ...
                      (v1(i,3) - v1(j,3))^2)^0.5;
        end
    end

elseif length(varargin)==2

    v2 = double(varargin{2});
    m = size(v1,1);
    n = size(v2,1);
    d = zeros(m,n);
    for i = 1:m
        for j=1:n
            d(i,j) = ((v1(i,1) - v2(j,1))^2 + ...
                      (v1(i,2) - v2(j,2))^2 + ...
                      (v1(i,3) - v2(j,3))^2)^0.5;
        end
    end

end


function [mask,numClusters,XYZ] = clusterSizeMask(sizeThresh,height_mask)
% :Usage:
% ::
%
%     function [mask,numClusters,XYZ] = clusterSizeMask(sizeThresh,height_mask)
%
% ..
%    Tor Wager, 10/27/01
% ..

mask = []; numClusters = 0;, XYZ = [];

% Get point list of activated voxels
% ------------------------------------------------------
voxels = mask2voxel(height_mask);	% returns n x 3
voxels = voxels';					% put xyz in a single column

if isempty(voxels)
	disp('No voxels meet height threshold.')
	mask = zeros(size(height_mask));
	return
end

% Get cluster indices of voxels
% ------------------------------------------------------
[cl_index] = spm_clusters(voxels);

% Find index of voxels of sufficient size
% ------------------------------------------------------
if ~isempty(sizeThresh) & sizeThresh > 0
    for i = 1:max(cl_index)
	    a(cl_index == i) = sum(cl_index == i);
    end
else
    sizeThresh = 0;
    a = ones(size(cl_index));
end

which_vox = (a >= sizeThresh);
numClusters = sum(length(unique(cl_index(find(a >= sizeThresh)))));

if numClusters == 0
	disp('No clusters meet extent threshold.')
	mask = zeros(size(height_mask));
    XYZ = [];
	return
end

voxels = voxels(:,which_vox);

% Voxel point list output - 3 vector
% ------------------------------------------------------
XYZ = voxels;

% Convert back to mask for mask output
% ------------------------------------------------------
voxels = voxels';					% convert back to row vectors
mask = voxel2mask(voxels,size(height_mask));

return


function [ stats ] = ml_get_cv_grid_states_regression(test,train)
%ML_GET_CV_GRID_STATES_REGRESSION
%
%   input -----------------------------------------------------------------
%
%       o test  : cell array (M x 1), M is the number of parameters for
%                                     which K-fold CV has been evaluated on.
%
%           test{i}.mse  : (1 x K), K number of
%
%   output ----------------------------------------------------------------
%
%       o stats : struct , returns statistics on train and test sets
%
%           stats.train  : (statistics on the training data set)
%
%               - stats.train.acc : (statistics on the accuracy for the training set)
%
%                   - stats.train.mse.mean
%                   - stats.train.mse.std
%
%       the structure is the same for the test statistics
%
%

%% Check if grid search was done on > 1 parameters

N = size(test);

if length(N) > 1
    test = test(:);
    train = train(:);
end

%% Prepare train statistics struct
M                    = length(test);

%% Get statistics

for m=1:M

    %% Get Mean for [Train]
    stats.train.mse.mean(m)     = mean(train{m}.mse);
    stats.train.nmse.mean(m)    = mean(train{m}.nmse);
    stats.train.rmse.mean(m)    = mean(train{m}.rmse);
    stats.train.nrmse.mean(m)   = mean(train{m}.nrmse);
    stats.train.mae.mean(m)     = mean(train{m}.mae);
    stats.train.mare.mean(m)    = mean(train{m}.mare);
    stats.train.r.mean(m)       = mean(train{m}.r);
    stats.train.d.mean(m)       = mean(train{m}.d);
    stats.train.e.mean(m)       = mean(train{m}.e);
    stats.train.me.mean(m)      = mean(train{m}.me);
    stats.train.mre.mean(m)     = mean(train{m}.mre);

    %% Get Std for [Train]

    stats.train.mse.std(m)     = std(train{m}.mse);
    stats.train.nmse.std(m)    = std(train{m}.nmse);
    stats.train.rmse.std(m)    = std(train{m}.rmse);
    stats.train.nrmse.std(m)   = std(train{m}.nrmse);
    stats.train.mae.std(m)     = std(train{m}.mae);
    stats.train.mare.std(m)    = std(train{m}.mare);
    stats.train.r.std(m)       = std(train{m}.r);
    stats.train.d.std(m)       = std(train{m}.d);
    stats.train.e.std(m)       = std(train{m}.e);
    stats.train.me.std(m)      = std(train{m}.me);
    stats.train.mre.std(m)     = std(train{m}.mre);

    %% Get Mean for [Test]
    stats.test.mse.mean(m)     = mean(test{m}.mse);
    stats.test.nmse.mean(m)    = mean(test{m}.nmse);
    stats.test.rmse.mean(m)    = mean(test{m}.rmse);
    stats.test.nrmse.mean(m)   = mean(test{m}.nrmse);
    stats.test.mae.mean(m)     = mean(test{m}.mae);
    stats.test.mare.mean(m)    = mean(test{m}.mare);
    stats.test.r.mean(m)       = mean(test{m}.r);
    stats.test.d.mean(m)       = mean(test{m}.d);
    stats.test.e.mean(m)       = mean(test{m}.e);
    stats.test.me.mean(m)      = mean(test{m}.me);
    stats.test.mre.mean(m)     = mean(test{m}.mre);

    %% Get Std for [Test]

    stats.test.mse.std(m)     = std(test{m}.mse);
    stats.test.nmse.std(m)    = std(test{m}.nmse);
    stats.test.rmse.std(m)    = std(test{m}.rmse);
    stats.test.nrmse.std(m)   = std(test{m}.nrmse);
    stats.test.mae.std(m)     = std(test{m}.mae);
    stats.test.mare.std(m)    = std(test{m}.mare);
    stats.test.r.std(m)       = std(test{m}.r);
    stats.test.d.std(m)       = std(test{m}.d);
    stats.test.e.std(m)       = std(test{m}.e);
    stats.test.me.std(m)      = std(test{m}.me);
    stats.test.mre.std(m)     = std(test{m}.mre);

end

if length(N) > 1

    stats.train.mse.mean     = reshape(stats.train.mse.mean,N);
    stats.train.nmse.mean    = reshape(stats.train.nmse.mean,N);
    stats.train.rmse.mean    = reshape(stats.train.rmse.mean,N);
    stats.train.nrmse.mean   = reshape(stats.train.nrmse.mean,N);
    stats.train.mae.mean     = reshape(stats.train.mae.mean,N);
    stats.train.mare.mean    = reshape(stats.train.mare.mean,N);
    stats.train.r.mean       = reshape(stats.train.r.mean,N);
    stats.train.d.mean       = reshape(stats.train.d.mean,N);
    stats.train.e.mean       = reshape(stats.train.e.mean,N);
    stats.train.me.mean      = reshape(stats.train.me.mean,N);
    stats.train.mre.mean     = reshape(stats.train.mre.mean,N);

    stats.train.mse.std     = reshape(stats.train.mse.std,N);
    stats.train.nmse.std    = reshape(stats.train.nmse.std,N);
    stats.train.rmse.std    = reshape(stats.train.rmse.std,N);
    stats.train.nrmse.std   = reshape(stats.train.nrmse.std,N);
    stats.train.mae.std     = reshape(stats.train.mae.std,N);
    stats.train.mare.std    = reshape(stats.train.mare.std,N);
    stats.train.r.std       = reshape(stats.train.r.std,N);
    stats.train.d.std       = reshape(stats.train.d.std,N);
    stats.train.e.std       = reshape(stats.train.e.std,N);
    stats.train.me.std      = reshape(stats.train.me.std,N);
    stats.train.mre.std     = reshape(stats.train.mre.std,N);

    stats.test.mse.mean     = reshape(stats.test.mse.mean,N);
    stats.test.nmse.mean    = reshape(stats.test.nmse.mean,N);
    stats.test.rmse.mean    = reshape(stats.test.rmse.mean,N);
    stats.test.nrmse.mean   = reshape(stats.test.nrmse.mean,N);
    stats.test.mae.mean     = reshape(stats.test.mae.mean,N);
    stats.test.mare.mean    = reshape(stats.test.mare.mean,N);
    stats.test.r.mean       = reshape(stats.test.r.mean,N);
    stats.test.d.mean       = reshape(stats.test.d.mean,N);
    stats.test.e.mean       = reshape(stats.test.e.mean,N);
    stats.test.me.mean      = reshape(stats.test.me.mean,N);
    stats.test.mre.mean     = reshape(stats.test.mre.mean,N);

    stats.test.mse.std     = reshape(stats.test.mse.std,N);
    stats.test.nmse.std    = reshape(stats.test.nmse.std,N);
    stats.test.rmse.std    = reshape(stats.test.rmse.std,N);
    stats.test.nrmse.std   = reshape(stats.test.nrmse.std,N);
    stats.test.mae.std     = reshape(stats.test.mae.std,N);
    stats.test.mare.std    = reshape(stats.test.mare.std,N);
    stats.test.r.std       = reshape(stats.test.r.std,N);
    stats.test.d.std       = reshape(stats.test.d.std,N);
    stats.test.e.std       = reshape(stats.test.e.std,N);
    stats.test.me.std      = reshape(stats.test.me.std,N);
    stats.test.mre.std     = reshape(stats.test.mre.std,N);

end

end


function stsa_mis(filename,outfile)

%
%  Implements the Bayesian estimator based on the modified Itakura-Saito
%  distortion measure [1, Eq. 43].
%
%  Usage:  stsa_mis(noisyFile, outputFile)
%
%         infile - noisy speech file in .wav format
%         outputFile - enhanced output file in .wav format
%
%
%  Example call:  stsa_mis('sp04_babble_sn10.wav','out_mis.wav');
%
%  References:
%   [1] Loizou, P. (2005). Speech enhancement based on perceptually motivated
%       Bayesian estimators of the speech magnitude spectrum. IEEE Trans. on Speech
%       and Audio Processing, 13(5), 857-869.
%
% Author: Philipos C. Loizou
%
% Copyright (c) 2006 by Philipos C. Loizou
% $Revision: 0.0 $  $Date: 10/09/2006 $
%-------------------------------------------------------------------------

if nargin<2
   fprintf('Usage: stsa_mis inFile outFile.wav \n\n');
   return;
end

[x, Srate, bits]= wavread( filename);

% =============== Initialize variables ===============
%

len=floor(20*Srate/1000); % Frame size in samples
if rem(len,2)==1, len=len+1; end;
PERC=50; % window overlap in percent of frame size
len1=floor(len*PERC/100);
len2=len-len1;

win=hanning(len);  % define window
win = win*len2/sum(win);  % normalize window for equal level output

% Noise magnitude calculations - assuming that the first 6 frames is noise/silence
%
nFFT=len;
nFFT2=len/2;
noise_mean=zeros(nFFT,1);
j=1;
for k=1:5
   noise_mean=noise_mean+abs(fft(win.*x(j:j+len-1),nFFT));
   j=j+len;
end
noise_mu=noise_mean/5;
noise_mu2=noise_mu.^2;

%--- allocate memory and initialize various variables

img=sqrt(-1);
x_old=zeros(len1,1);
Nframes=floor(length(x)/len2)-1;
xfinal=zeros(Nframes*len2,1);

%===============================  Start Processing =======================================================
%
k=1;
aa=0.98;
fprintf('\nThis might take some time ...\n');
for n=1:Nframes

   insign=win.*x(k:k+len-1);

   %--- Take fourier transform of  frame ----

   spec=fft(insign,nFFT);
   sig=abs(spec); % compute the magnitude
   sig2=sig.^2;

       gammak=min(sig2./noise_mu2,40);  % post SNR. Limit it to avoid overflows
       if n==1
           ksi=aa+(1-aa)*max(gammak-1,0);
       else
           ksi=aa*Xk_prev./noise_mu2 + (1-aa)*max(gammak-1,0);     % a priori SNR
       end

       vk=ksi.*gammak./(1+ksi);

       sig_hat=log(comp_int(vk,gammak,sig)); % Eq. 41

       Xk_prev=sig_hat.^2;

       xi_w= ifft( sig_hat.* exp(img*angle(spec)));
	   xi_w= real( xi_w);

	% --- Overlap and add ---------------
    %
    xfinal(k:k+ len2-1)= x_old+ xi_w(1:len1);
	x_old= xi_w(len1+ 1: len);

    if rem(n,20)==0, fprintf('Frame: %d Percent completed:%4.2f\n',n,n*100/Nframes); end;

 k=k+len2;
end
%========================================================================================

wavwrite(xfinal,Srate,16,outfile);

%------------------------------E N D  -----------------------------------
function xhat=comp_int(vk,gammak,Yk)

% -- Evaluates Eq. 43 in [1]
%

Yk2=Yk.*Yk;
G2=gammak.^2;
EV=exp(-vk);

N=40; % number of terms to keep in infinite sum (Eq. 43)
L=length(vk)/2+1;
J1=zeros(L,1);
J2=zeros(L,1);

for j=1:L
  sum=0;  sum_b=0;
  for m=0:N
     F=factorial(m);
     d1=(vk(j))^m;
     d2=hyperg(-m,-m,0.5,Yk2(j)/(4*G2(j)),10);
     d2_b=hyperg(-m,-m,1.5,Yk2(j)/(4*G2(j)),10);
     sum=sum+d1*d2/F;
     sum_b=sum_b+gamma(m+1.5)*d1*d2_b/(F*gamma(m+1));
end
 J1(j)=sum;
 J2(j)=sum_b;
end

J1=J1.*EV(1:L);
J2=J2.*EV(1:L).*sqrt(vk(1:L)).*Yk(1:L)./gammak(1:L);

xhat2=max(real(J1+J2),0.00001);
xhat = [xhat2; flipud(xhat2(2:L-1))];


function varargout = xexpintinv(varargin)
%XEXPINTINV EXPINT(1/Z)/Z

switch class(varargin{1})

    case 'double'
        z = varargin{1};
        varargout{1} = (1./z).*expint(1./z);

    case 'sdpvar'
        varargout{1} = InstantiateElementWise(mfilename,varargin{:});

    case 'char'

        operator = CreateBasicOperator('positive','callback');
        operator.derivative = @derivative;
        operator.range = [0 1];
        operator.domain = [0 inf];

        varargout{1} = [];
        varargout{2} = operator;
        varargout{3} = varargin{3};

    otherwise
        error([upper(mfilename) ' called with weird argument']);
end

function d = derivative(z);
d = (-1./z.^2).*expint(1./z)+(1./z).*(-z.*exp(-1./z)).*(-1./z.^2);


function [x,fval,exitflag,info] = opti_levmar(fun,grad,x0,ydata,lb,ub,A,b,Aeq,beq,opts)
%OPTI_LEVMAR Solve a NLS using LEVMAR (Levenberg-Marquardt by Manolis Lourakis)
%
%   min sum[ (F(x) - ydata)^2 ]       subject to:   A*x <= b
%    x                                              Aeq*x = beq
%                                                   lb <= x <= ub
%
%   x = opti_levmar(fun,grad,x0,ydata) solves a NLS where fun is the
%   fitting function. grad is an optional gradient of the fitting function
%   and x0 is a starting guess. ydata is the data to fit the function to.
%
%   x = opti_levmar(fun,grad,x0,ydata,lb,ub) solves subject to decision
%   variables bounds lb <= x <= ub. Avoid Infinite bounds.
%
%   x = opti_levmar(fun,...,ub,A,b) solves subject to the linear
%   inequalities Ax <= b.
%
%   x = opti_levmar(fun,...,b,Aeq,beq) solves subject to the linear
%   equalities Aeqx = beq.
%
%   x = opti_levmar(fun,...,beq,opts) uses opts to pass optiset options to
%   the solver.
%
%   [x,fval,exitflag,info] = opti_levmar(...) returns the objective value
%   at the solution, together with the solver exitflag, and an information
%   structure.
%
%   THIS IS A WRAPPER FOR LEVMAR
%   See referenced GNU Public License

%   Copyright (C) 2012 Jonathan Currie (I2C2)

if(nargin < 11), opts = optiset; end
if(nargin < 10), beq = []; end
if(nargin < 9), Aeq = []; end
if(nargin < 8), b = []; end
if(nargin < 7), A = []; end
if(nargin < 6), ub = []; end
if(nargin < 5), lb = []; end
if(nargin < 4), error('LEVMAR requires at least 4 arguments'); end

%Setup display level
opts.display = dispLevel(opts.display);

%Check we have a valid x0
if(isempty(x0) || any(isnan(x0)))
    error('LEVMAR requires an initial guess, x0!');
end

t = tic;
% Run LEVMAR
[x, fval, exitflag, iter, feval] = levmar(fun,grad,x0,ydata,lb,ub,A',b,Aeq',beq,opts);

%Collect Results
info.Iterations = iter;
info.FuncEvals = feval;
info.Time = toc(t);
info.Algorithm = 'LEVMAR: Levenberg-Marquardt in C/C++';

switch(exitflag)
    case 1
        info.Status = 'Optimal';
    case 0
        info.Status = 'Exceeded Iterations';
    case -1
        info.Status = 'Infeasible / Could not Converge';
    case -2
        info.Status = 'LEVMAR Error';
    otherwise
        info.Status = 'LEVMAR Error';
end


function [ a, l, r ] = r8vec_part_quick_a ( n, a )

%*****************************************************************************80
%
%% R8VEC_PART_QUICK_A reorders an R8VEC as part of a quick sort.
%
%  Discussion:
%
%    The routine reorders the entries of A.  Using A(1) as a key,
%    all entries of A that are less than or equal to the key will
%    precede the key which precedes all entries that are greater than the key.
%
%  Example:
%
%    Input:
%
%      N = 8
%
%      A = ( 6, 7, 3, 1, 6, 8, 2, 9 )
%
%    Output:
%
%      L = 3, R = 6
%
%      A = ( 3, 1, 2, 6, 6, 8, 9, 7 )
%            -------        -------
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 October 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries of A.
%
%    Input, realA(N), the array to be checked.
%
%    Output, real A(N), has been reordered as described above.
%
%    Output, integer L, R, the indices of A that define the three segments.
%    Let KEY = the input value of A(1).  Then
%    I <= L                 A(I) < KEY;
%         L < I < R         A(I) = KEY;
%                 R <= I    KEY < A(I).
%
  if ( n < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8VEC_PART_QUICK_A - Fatal error!\n' );
    fprintf ( 1, '  N < 1.\n' );
    error ( 'R8VEC_PART_QUICK_A - Fatal error!' );
  elseif ( n == 1 )
    l = 0;
    r = 2;
    return;
  end

  key = a(1);
  m = 1;
%
%  The elements of unknown size have indices between L+1 and R-1.
%
  l = 1;
  r = n + 1;

  for i = 2 : n

    if ( key < a(l+1) )
      r = r - 1;
      [ a(r), a(l+1) ] = r8_swap ( a(r), a(l+1) );
    elseif ( a(l+1) == key )
      m = m + 1;
      [ a(m), a(l+1) ] = r8_swap ( a(m), a(l+1) );
      l = l + 1;
    elseif ( a(l+1) < key )
      l = l + 1;
    end

  end
%
%  Now shift small elements to the left, and KEY elements to center.
%
  for i = 1 : l - m
    a(i) = a(i+m);
  end
%
%  Out of bounds here, occasionally.
%
  l = l - m;

  a(l+1:l+m) = key;

  return
end


function [r, pol, res, zer, zj, fj, wj, errvec, wt] = aaa(F, varargin)
%AAA   AAA and AAA-Lawson (near-minimax) real or complex rational approximation.
%   R = AAA(F, Z) computes the AAA rational approximant R (function handle) to
%   data F on the set of sample points Z.  F may be given by its values at Z,
%   or as a function handle or chebfun.  R = AAA(F, Z, 'degree', N) attempts to
%   compute the minimax approximation of degree N (i.e., rational type (N,N)).
%
%   [R, POL, RES, ZER] = AAA(F, Z) returns vectors of poles, residues, and zeros
%   of R.
%
%   [R, POL, RES, ZER, ZJ, FJ, WJ] = AAA(F, Z) also returns the vectors of
%   support points ZJ, approximation values FJ = r(ZJ), and weights WJ of the
%   barycentric representation of R.
%
%   [R, POL, RES, ZER, ZJ, FJ, WJ, ERRVEC] = AAA(F, Z) also returns the vector
%   of errors ||f-r||_infty in successive iterative steps of AAA.  Note that the
%   rational degrees are not 1,...,length(ERRVEC) but 0,...,length(ERRVEC)-1.
%
%   R = AAA(F, Z, NAME, VALUE) sets the following parameters:
%   - 'tol', TOL: relative tolerance (default TOL = 1e-13),
%   - 'degree', N: maximal degree (default N = 99).
%       The output rational approximant will be of degree at most N.  Like
%       'mmax', N+1, except that Lawson is turned on by default: see below.
%   - 'mmax', MMAX: maximal number of terms in the barycentric representation
%       (default MMAX = 100). R will be of degree at most MMAX-1.  Like
%       'degree', MMAX-1, except that Lawson is not turned on by default.
%   - 'dom', DOM: domain (default DOM = [-1, 1]).  No effect if Z is provided.
%   - 'cleanup', 'off' or 0: turns off automatic removal of Froissart doublets.
%   - 'cleanuptol', CLEANUPTOL: cleanup tolerance (default CLEANUPTOL = TOL).
%       Poles with residues less than this number times the geometric mean size
%       of F times the minimum distance to Z are deemed spurious by the cleanup
%       procedure.  If TOL = 0, then CLEANUPTOL defaults to 1e-13.
%   - 'lawson', NLAWSON: take NLAWSON iteratively reweighted least-squares steps
%       to bring approximation closer to minimax.  Specifying NLAWSON = 0
%       ensures there is no Lawson iteration.  See next paragraph.
%
%   If 'degree' is specified and 'lawson' is not, AAA attempts to find a minimax
%   approximant of degree N by AAA-Lawson iteration.  This will generally be
%   successful only if the minimax error is well above machine precision, and
%   is more reliable for complex problems than real ones.  If 'degree' and
%   'lawson' are both specified, then exactly NLAWSON Lawson steps are taken
%   (so NLAWSON = 0 corresponds to AAA approximation with no Lawson iteration).
%   The final weight vector WT of the Lawson iteration is available with
%   [R, POL, RES, ZER, ZJ, FJ, WJ, ERRVEC, WT] = AAA(F, Z).
%
%   Note that R may have fewer than N poles and zeros.  This may happen, for
%   example, if N is too large, or if F is even and N is odd, or if F is odd
%   and N is even.
%
%   One can also execute R = AAA(F), with no specification of a set Z.  If F is
%   a vector, this is equivalent to R = AAA(F, Z) with
%   Z = LINSPACE(-1, 1, LENGTH(F)).  If F is a function handle, AAA attempts
%   to resolve F on [-1,1] by default.
%
%   This standalone code works in GNU Octave as well as MATLAB.
%
% Examples:
%   r = aaa(@exp); xx = linspace(-1,1); plot(xx,r(xx)-exp(xx))
%
%   r = aaa(@exp,'degree',4); xx = linspace(-1,1); plot(xx,r(xx)-exp(xx))
%
%   X = linspace(-1,1,30); r = aaa(gamma(X),X);
%   fplot(r,[-5,5]), axis([-5 5 -15 15]), grid on
%
%   Z = exp(2i*pi*linspace(0,1,500));
%   [r,pol,res] = aaa(@tan,Z); disp([pol res])
%
%   X = linspace(-1,1,1000); F = tanh(20*X);
%   subplot(1,2,1)
%   r = aaa(F,X,'degree',15,'lawson',0); plot(X,F-r(X)), hold on
%   r = aaa(F,X,'degree',15); plot(X,F-r(X)), hold off
%
%   Z = exp(1i*pi*linspace(-1,1,1000)); G = exp(Z);
%   subplot(1,2,2)
%   r = aaa(G,Z,'degree',3,'lawson',0); plot(G-r(Z)), axis equal, hold on
%   r = aaa(G,Z,'degree',3); plot(G-r(Z)), axis equal, hold off
%
%   References on AAA and AAA-Lawson, respectively:
%
%   [1] Y. Nakatsukasa, O. Sete, and L. N. Trefethen, "The AAA algorithm
%   for rational approximation", SIAM J. Sci. Comp. 40 (2018), A1494-A1522.
%
%   [2] Y. Nakatsukasa and L. N. Trefethen, An algorithm for real and
%   complex rational minimax approximation, SIAM J. Sci. Comp. 42 (2020),
%   A3157-A3179.
%
% See also AAATRIG, CF, CHEBPADE, MINIMAX, PADEAPPROX, RATINTERP.

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

% Parse inputs:
[F, Z, M, dom, tol, mmax, cleanup_flag, cleanup_tol, needZ, mmax_flag, ...
    nlawson, degree_flag, degree] = parseInputs(F, varargin{:});

if ( needZ )
    % Z was not provided.  Try to resolve F on its domain.
    [r, pol, res, zer, zj, fj, wj, errvec] = ...
        aaa_autoZ(F, dom, tol, mmax, cleanup_flag, cleanup_tol, mmax_flag, ...
            nlawson, degree_flag, degree);
    return
end

% Remove infinite or NaN function values and repeated entries:
toKeep = ~isinf(F) & ~isnan(F);
F = F(toKeep); Z = Z(toKeep);
[Z, uni] = unique(Z,'stable'); F = F(uni);

% Initialization for AAA iteration:
M = length(Z);
abstol = tol*norm(F, inf);                 % Absolute tolerance
J = (1:M)';
zj = []; fj = []; C = []; A = [];
errvec = [];
R = mean(F)*ones(size(J));

% AAA iteration:
for m = 1:mmax
    % Introduce next support point:
    [~, jj] = max(abs(F(J) - R(J)));       % Select next support point
    zj = [zj; Z(J(jj))];                   % Update support points
    fj = [fj; F(J(jj))];                   % Update data values
    C = [C 1./(Z - Z(J(jj)))];             % Next column of Cauchy matrix
    J(jj) = [];                            % Update index vector
    A = [A, (F-fj(end)).*C(:,end)];        % Update Loewner matrix

    % Compute weights:
    if ( length(J) >= m )                  % The usual tall-skinny case
        [~, S, V] = svd(A(J,:), 0);        % Reduced SVD
        s = diag(S);
        mm = find( s == min(s) );          % Treat case of multiple min sing val
        nm = length(mm);
        wj = V(:,mm)*ones(nm,1)/sqrt(nm);  % Aim for non-sparse wt vector
    elseif ( length(J) >= 1 )
        V = null(A(J,:));                  % Fewer rows than columns
        nm = size(V,2);
        wj = V*ones(nm,1)/sqrt(nm);        % Aim for non-sparse wt vector
    else
        wj = ones(m,1)/sqrt(m);            % No rows at all (needed for Octave)
    end

    % Compute rational approximant:
    i0 = find(wj~=0);                      % Omit columns with wj = 0
    N = C(:,i0)*(wj(i0).*fj(i0));          % Numerator
    D = C(:,i0)*wj(i0);                    % Denominator
    R = N./D;
    Dinf = isinf(D);
    R(Dinf) = F(Dinf);                     % Interpolate at supp pts with wj~=0

    % Check if converged:
    maxerr = norm(F - R, inf);
    errvec = [errvec; maxerr];
    if ( maxerr <= abstol )
        break
    end
end
maxerrAAA = maxerr;                        % Error at end of AAA

% We now enter Lawson iteration: barycentric IRLS = iteratively reweighted
% least-squares if 'lawson' is specified with NLAWSON > 0 or 'mmax' is
% specified and 'lawson' is not.  In the latter case the number of steps
% is chosen adaptively.  Note that the Lawson iteration is unlikely to be
% successful when the errors are close to machine precision.

wj0 = wj; fj0 = fj;                        % Save params in case Lawson fails
wt = NaN(M,1); wt_new = ones(M,1);
if ( nlawson > 0 )                         % Lawson iteration

    maxerrold = maxerrAAA;
    maxerr = maxerrold;
    nj = length(zj);
    A = [];
    for j = 1:nj                           % Cauchy/Loewner matrix
        A = [A 1./(Z-zj(j)) F./(Z-zj(j))];
    end
    for j = 1:nj
        [i,~] = find(Z==zj(j));            % support pt rows are special
        A(i,:) = 0;
        A(i,2*j-1) = 1;
        A(i,2*j) = F(i);
    end
    stepno = 0;
    while ( (nlawson < inf) && (stepno < nlawson) ) || ...
          ( (nlawson == inf) && (stepno < 20) ) || ...
          ( (nlawson == inf) && (maxerr/maxerrold < .999) && (stepno < 1000) )
        stepno = stepno + 1;
        wt = wt_new;
        W = spdiags(sqrt(wt),0,M,M);
        [~,~,V] = svd(W*A,0);
        c = V(:,end);
        denom = zeros(M,1); num = zeros(M,1);
        for j = 1:nj
            denom = denom + c(2*j)./(Z-zj(j));
            num = num - c(2*j-1)./(Z-zj(j));
        end
        R = num./denom;
        for j = 1:nj
            [i,~] = find(Z==zj(j));        % support pt rows are special
            R(i) = -c(2*j-1)/c(2*j);
        end
        err = F - R; abserr = abs(err);
        wt_new = wt.*abserr; wt_new = wt_new/norm(wt_new,inf);
        maxerrold = maxerr;
        maxerr = max(abserr);
    end
    wj = c(2:2:end);
    fj = -c(1:2:end)./wj;
    % If Lawson has not reduced the error, return to pre-Lawson values.
    if ( (maxerr > maxerrAAA) && (nlawson == Inf) )
        wj = wj0; fj = fj0;
    end
end

% Remove support points with zero weight:
I = find(wj == 0);
zj(I) = []; wj(I) = []; fj(I) = [];

% Construct function handle and compute poles, residues and zeros:
r = @(zz) reval(zz, zj, fj, wj);
[pol, res, zer] = prz(zj, fj, wj);

if ( cleanup_flag == 1 && nlawson == 0 )      % Remove Froissart doublets
    [r, pol, res, zer, zj, fj, wj] = ...
        cleanup(r, pol, res, zer, zj, fj, wj, Z, F, cleanup_tol);
elseif ( cleanup_flag == 2 && nlawson == 0 )  % Alternative cleanup.  Currently
    a.zj = zj; a.fj = fj; a.wj = wj;          % an undocumented feature,
    a.Z = Z; a.F = F;                         % pending further investigation.
    a.cleanup_tol = max(cleanup_tol, eps);
    c = cleanup2(a);
    zj = c.zj; fj = c.fj; wj = c.wj;
    r = @(zz) reval(zz, zj, fj, wj);
    [pol, res, zer] = prz(zj, fj, wj);
end

end % of AAA()

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   PARSEINPUTS   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [F, Z, M, dom, tol, mmax, cleanup_flag, cleanup_tol, ...
    needZ, mmax_flag, nlawson, degree_flag, degree] = parseInputs(F, varargin)

% Check if F is empty:
if ( isempty(F) )
    error('AAA:emptyF', 'No function given.')
elseif ( isa(F, 'chebfun') )
    if ( size(F, 2) ~= 1 )
        error('AAA:nColF', 'Input chebfun must have one column.')
    end
end

% Sample points:
if ( ~isempty(varargin) && isfloat(varargin{1}) )
    % Z is given.
    Z = varargin{1};
    if ( isempty(Z) )
        error('AAA:emptyZ', ...
            'If sample set is provided, it must be nonempty.')
    end
    varargin(1) = [];
end

% Set defaults for other parameters:
tol = 1e-13;                   % Relative tolerance
mmax = 100;                    % Maximum number of terms
degree = NaN;                  % Specified degree
cleanup_tol = 1e-13;           % Cleanup tolerance
nlawson = Inf;                 % Number of Lawson steps (Inf means adaptive)
% Domain:
if ( isa(F, 'chebfun') )
    dom = F.domain([1, end]);
else
    dom = [-1, 1];
end
cleanup_flag = 1;              % Cleanup on
mmax_flag = 0;                 % Checks if mmax manually specified
degree_flag = 0;               % Checks if degree specified
cleanup_set = 0;               % Checks if cleanup_tol manually specified
while ( ~isempty(varargin) )   % Check if parameters have been provided
    if ( strncmpi(varargin{1}, 'tol', 3) )
        if ( isfloat(varargin{2}) && isequal(size(varargin{2}), [1, 1]) )
            tol = varargin{2};
            if ( ~cleanup_set && tol > 0 ) % If not set, set cleanup_tol to tol
              cleanup_tol = tol;
            end
        end
        varargin([1, 2]) = [];

    elseif ( strncmpi(varargin{1}, 'degree', 6) )
        if ( isfloat(varargin{2}) && isequal(size(varargin{2}), [1, 1]) )
            if ( mmax_flag == 1 ) && ( mmax ~= varargin{2}+1 )
                error('AAA:degmmaxmismatch', ' mmax must equal degree+1.')
            end
            degree = varargin{2};
            mmax = degree + 1;
            mmax_flag = 1;
            degree_flag = 1;
        end
        varargin([1, 2]) = [];

    elseif ( strncmpi(varargin{1}, 'mmax', 4) )
        if ( isfloat(varargin{2}) && isequal(size(varargin{2}), [1, 1]) )
            if ( mmax_flag == 1 ) && ( mmax ~= varargin{2})
                error('AAA:degmmaxmismatch', ' mmax must equal degree+1.')
            end
            mmax = varargin{2};
            mmax_flag = 1;
        end
        varargin([1, 2]) = [];

    elseif ( strncmpi(varargin{1}, 'lawson', 6) )
        if ( isfloat(varargin{2}) && isequal(size(varargin{2}), [1, 1]) )
            nlawson = varargin{2};
        end
        varargin([1, 2]) = [];

    elseif ( strncmpi(varargin{1}, 'dom', 3) )
        if ( isfloat(varargin{2}) && isequal(size(varargin{2}), [1, 2]) )
            dom = varargin{2};
        end
        varargin([1, 2]) = [];
        if ( isa(F, 'chebfun') )
            if ( ~isequal(dom, F.domain([1, end])) )
                warning('AAA:dom', ...
                    ['Given domain does not match that of the chebfun.\n', ...
                    'Results may be inaccurate.'])
            end
        end

    elseif ( strncmpi(varargin{1}, 'cleanuptol', 10) )
        if ( isfloat(varargin{2}) && isequal(size(varargin{2}), [1, 1]) )
          cleanup_tol = varargin{2};
          cleanup_set = 1;
        end
        varargin([1, 2]) = [];

    elseif ( strncmpi(varargin{1}, 'cleanup', 7) )
        if ( strncmpi(varargin{2}, 'off', 3) || ( varargin{2} == 0 ) )
            cleanup_flag = 0;
        elseif ( varargin{2} == 2 )     % Alternative cleanup
            cleanup_flag = 2;
        end
        varargin([1, 2]) = [];

    else
        error('AAA:UnknownArg', 'Argument unknown.')
    end
end

% Deal with Z and F:
if ( ~exist('Z', 'var') && isfloat(F) )
    % F is given as data values, pick same number of sample points:
    Z = linspace(dom(1), dom(2), length(F)).';
end

if ( exist('Z', 'var') )
    % Z is given:
    needZ = 0;

    % Work with column vector:
    Z = Z(:);
    M = length(Z);

    % Function values:
    if ( isa(F, 'function_handle') || isa(F, 'chebfun') )
        % Sample F on Z:
        F = F(Z);
    elseif ( isnumeric(F) )
        % Work with column vector and check that it has correct length.
        F = F(:);
        if ( length(F) ~= M )
            error('AAA:lengthFZ', ...
                'Inputs F and Z must have the same length.')
        end
    elseif ( ischar(F) )
        % F is given as a string input. Convert it to a function handle.
        F = inline(vectorize(F));
        F = F(Z);
    else
        error('AAA:UnknownF', 'Input for F not recognized.')
    end

else
    % Z was not given.  Set flag that Z needs to be determined.
    % Also set Z and M since they are needed as output.
    needZ = 1;
    Z = [];
    M = length(Z);
end

if ( ~degree_flag && (nlawson == Inf) )
    nlawson = 0;
end

end % End of PARSEINPUTS.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   CLEANUP   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [r, pol, res, zer, z, f, w] = ...
    cleanup(r, pol, res, zer, z, f, w, Z, F, cleanup_tol)
% Remove spurious pole-zero pairs.

% Find negligible residues:
if any(F)
    geometric_mean_of_absF = exp(mean(log(abs(F(F~=0)))));
else
    geometric_mean_of_absF = 0;
end
Zdistances = NaN(size(pol));
for j = 1:length(Zdistances)
    Zdistances(j) = min(abs(pol(j)-Z));
end
ii = find(abs(res)./Zdistances < cleanup_tol * geometric_mean_of_absF);
ni = length(ii);
if ( ni == 0 )
    return
elseif ( ni == 1 )
    warning('AAA:Froissart','1 Froissart doublet');
else
    warning('AAA:Froissart',[int2str(ni) ' Froissart doublets']);
end

% For each spurious pole find and remove closest support point:
for j = 1:ni
    azp = abs(z-pol(ii(j)));
    jj = find(azp == min(azp),1);

    % Remove support point(s):
    z(jj) = []; f(jj) = [];
end

% Remove support points z from sample set:
for jj = 1:length(z)
    F(Z == z(jj)) = [];
    Z(Z == z(jj)) = [];
end
m = length(z);
M = length(Z);

% Build Loewner matrix:
SF = spdiags(F, 0, M, M);
Sf = diag(f);
C = 1./(Z-z.');               % Cauchy matrix.
A = SF*C - C*Sf;              % Loewner matrix.

% Solve least-squares problem to obtain weights:
[~, ~, V] = svd(A, 0);
w = V(:,m);

% Build function handle and compute poles, residues and zeros:
r = @(zz) reval(zz, z, f, w);
[pol, res, zer] = prz(z, f, w);

end % End of CLEANUP.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   CLEANUP2   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function c = cleanup2(a)
%% Alternative cleanup procedure to remove spurious pole-zero pairs.
% This considers pole-zero distances.  Stefano Costa, August 2022.

z = a.zj; f = a.fj; w = a.wj;
[pol, res, zer] = prz(z, f, w);

cleanup_tol = a.cleanup_tol;

niter = 0;
while(true)
    niter = niter+1;
    Z = a.Z; F = a.F;
    ii = [];
    for jj = 1:length(pol)
        dz = min(abs(zer-pol(jj))); if isempty(dz), dz = 1e100; end
        dS = abs(Z-pol(jj));
        ds = min(dS);
        if any(F)
            q = 4*pi*abs(F).*dS;
            Q = mean(q);                % Arithmetic mean
        else
            Q = 0;
        end
        R = 8*cleanup_tol*Q/(4*pi);    % Equivalent residue value

        % Conditions to expunge poles
        % Expunge if either minimum distance is zero
        if (ds==0) || (dz==0)
            ii = [ii; jj];
        % Expunge if Z is a real interval
        elseif isreal(Z) && (abs(imag(pol(jj)))<eps) && ...
            (real(pol(jj))>=min(Z)) && (real(pol(jj))<=max(Z))
            ii = [ii; jj];
        % Expunge if Z is the unit disk
        elseif all(abs(Z)==1) && (abs(abs(pol(jj))-1)<eps)
            ii = [ii; jj];
        % Expunge if distance to closest zero is undetectable
        elseif (dz/ds<1) && (dz<max(cleanup_tol^2,eps))
            ii = [ii; jj];
        % Expunge if a nearby zero exists and residue is below the
        % equivalent value R. Two choices for real and complex F
        elseif ((dz/ds)<sqrt(cleanup_tol))
            if ( ~any(imag(F)) && (abs(real(res(jj))) < R) )
                ii = [ii; jj];
            elseif (abs(res(jj)) < R)
                ii = [ii; jj];
            end
        end
    end
    ii = unique(ii);

    ni = length(ii);
    if ( ni == 0 )
        % Nothing to do.
        break;
    elseif ( ni == 1 )
        warning('AAA:Froissart',...
            ['1 Froissart doublet, niter = ', int2str(niter)]);
    else
        warning('AAA:Froissart',...
            [int2str(ni) ' Froissart doublets, niter = ' int2str(niter)]);
    end

    % For each spurious pole find and remove closest support point:
    for j = 1:ni
        azp = abs(z-pol(ii(j)));
        jj = find(azp == min(azp),1);

        % Remove support point(s):
        z(jj) = [];
        f(jj) = [];
    end

    % Remove support points z from sample set:
    for jj = 1:length(z)
        F(Z == z(jj)) = [];
        Z(Z == z(jj)) = [];
    end
    m = length(z);
    M = length(Z);

    % Build Loewner matrix:
    SF = spdiags(F, 0, M, M);
    Sf = diag(f);
    C = 1./(Z-z.');             % Cauchy matrix.
    A = SF*C - C*Sf;            % Loewner matrix.

    % Solve least-squares problem to obtain weights:
    [~, ~, V] = svd(A, 0);
    w = V(:,m);

    % Compute poles, residues and zeros:
    [pol, res, zer] = prz(z, f, w);
end % End of while loop

c.zj = z; c.fj = f; c.wj = w;

end  % End of CLEANUP2.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   AAA_AUTOZ   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [r, pol, res, zer, zj, fj, wj, errvec] = ...
    aaa_autoZ(F, dom, tol, mmax, cleanup_flag, cleanup_tol, mmax_flag, ...
               nlawson, degree_flag, degree)
% Automated choice of sample set

% Flag if function has been resolved:
isResolved = 0;

% Main loop:
for n = 5:14
    % Sample points:
    % Next line enables us to do pretty well near poles
    Z = linspace(dom(1)+1.37e-8*diff(dom), dom(2)-3.08e-9*diff(dom), 1 + 2^n).';
    if degree_flag
       [r, pol, res, zer, zj, fj, wj, errvec] = aaa(F, Z, 'tol', tol, ...
          'mmax', mmax, 'cleanup', cleanup_flag, 'cleanuptol', cleanup_tol, ...
          'lawson', nlawson, 'degree', degree);
    else
       [r, pol, res, zer, zj, fj, wj, errvec] = aaa(F, Z, 'tol', tol, ...
          'mmax', mmax, 'cleanup', cleanup_flag, 'cleanuptol', cleanup_tol, ...
          'lawson', nlawson);
    end
    % Test if rational approximant is accurate:
    abstol = tol * norm(F(Z), inf);

    % On Z(n):
    err(1,1) = norm(F(Z) - r(Z), inf);

    Zrefined = linspace(dom(1)+1.37e-8*diff(dom), dom(2)-3.08e-9*diff(dom), ...
        round(1.5 * (1 + 2^(n+1)))).';
    err(2,1) = norm(F(Zrefined) - r(Zrefined), inf);
    if ( all(err < abstol) )
        % Final check that the function is resolved, inspired by sampleTest().
        % Pseudo random sample points in [-1, 1]:
        xeval = [-0.357998918959666; 0.036785641195074];
        % Scale to dom:
        xeval = (dom(2) - dom(1))/2 * xeval + (dom(2) + dom(1))/2;

        if ( norm(F(xeval) - r(xeval), inf) < abstol )
            isResolved = 1;
            break
        end
    end
end

if ( ( isResolved == 0 ) && ~mmax_flag )
    warning('AAA:notResolved', ...
        'Function not resolved using %d pts.', length(Z))
end

end % End of AAA_AUTOZ.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   PRZ   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [pol, res, zer] = prz(zj, fj, wj)
%   Compute poles, residues, and zeros of rational fun in barycentric form.

% Compute poles via generalized eigenvalue problem:
m = length(wj);
B = eye(m+1);
B(1,1) = 0;
E = [0 wj.'; ones(m, 1) diag(zj)];
pol = eig(E, B);
pol = pol(~isinf(pol));

% Compute residues via formula for res of quotient of analytic functions:
N = @(t) (1./(t-zj.')) * (fj.*wj);
Ddiff = @(t) -((1./(t-zj.')).^2) * wj;
res = N(pol)./Ddiff(pol);

% Compute zeros via generalized eigenvalue problem:
E = [0 (wj.*fj).'; ones(m, 1) diag(zj)];
zer = eig(E, B);
zer = zer(~isinf(zer));

end % End of PRZ.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   REVAL   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function r = reval(zz, zj, fj, wj)
%   Construct function handle to evaluate rational function in barycentric form.

zv = zz(:);                         % vectorize zz if necessary
CC = 1./(zv-zj.');                  % Cauchy matrix
r = (CC*(wj.*fj))./(CC*wj);         % vector of values

% Deal with input inf: r(inf) = lim r(zz) = sum(w.*f) / sum(w):
r(isinf(zv)) = sum(wj.*fj)./sum(wj);

% Deal with NaN:
ii = find(isnan(r));
for jj = 1:length(ii)
    if ( isnan(zv(ii(jj))) || ~any(zv(ii(jj)) == zj) )
        % r(NaN) = NaN is fine.
        % The second case may happen if r(zv(ii)) = 0/0 at some point.
    else
        % Clean up values NaN = inf/inf at support points.
        % Find the corresponding node and set entry to correct value:
        r(ii(jj)) = fj(zv(ii(jj)) == zj);
    end
end

% Reshape to input format:
r = reshape(r, size(zz));

end % End of REVAL.


function [traces, rel, cSize] = calcTraces(grains, clusterId, varargin)
% traces of subsets of grains
%
% Syntax
%
%   clusterId = [grainId,variantId];
%   [traces, rel, cSize] = calcTraces(grains, clusterId,'shape')
%
% Input
%  grains - @grain2d
%  cId  - cluster Id
%
% Output
%  traces - @vector3d, size max(clusterId(:,1)) x max(clusterId(:,2)) ....
%  rel    - relyability index, same size as traces
%  cSize  - cluster size, , same size as traces
%
% Options
%  minClusterSize - minimum grainSize required for trace computation (default: 100)
%  shape - characteristic shape based algorithm
%  calliper - use shortest calliper instead of eigenvectors
%  hist  - circular histogram based algorithm
%
% References
%
% * <https://arxiv.org/abs/2201.02103 Determination of child phase habit
% planes from two-dimensional reconstructed parent phase orientation maps>,
% arXiv, 2022
%

% generate clusters
if nargin == 1 || ~isnumeric(clusterId), clusterId = ones(length(grains),1); end
notZero = all(clusterId>0,2);
cSize = accumarray(clusterId(notZero,:),grains.grainSize(notZero));
ic = find(cSize > get_option(varargin,'minClusterSize',100));

% prepare output
sz = [max(clusterId),1];
omega = nan(sz);
rel = zeros(sz);

% extract grain geometry
V = grains.boundary.V;
F = grains.boundary.F;
I_BG = grains.boundary.I_FG;
grainId = grains.id;

% get method to use
useHist = check_option(varargin,'hist');
useCalliper = check_option(varargin,'calliper');

% loop over all relevant clusters
for i = 1:length(ic)

  % combine grains according to clusterIndex
  [sub{1:size(clusterId,2)}]  = ind2sub(sz,ic(i));
  ind = all(clusterId == [sub{:}],2);

  % the boundary segments
  lF = F(any(I_BG(:,grainId(ind)),2),:);

  % shifted into the origin
  lS = V(lF(:,1),:) - V(lF(:,2),:);

  if isempty(lF), continue; end

  if useHist % density function method

    rho = atan2(lS(:,2),lS(:,1));

    %fun = calcDensity(rho, 'weights',sqrt(sum(lS.^2,2)),'periodic','sigma',10*degree);
    fun = calcDensity(rho, 'weights',vecnorm(lS,2,2),'periodic','sigma',10*degree);
    fun.antipodal = true;

    phi = linspace(0,2*pi,360);
    [m,ind] = max(real(fun.eval(phi)));

    omega(ic(i)) = phi(ind);
    rel(ic(i)) = (m-1)/m;

  else % characteristic shape method

    %cS = shape2d.byFV(F(any(I_BG(:,ind),2),:),V,'noSimplify');
    %[omega(ic(i)),a,b] = principalComponents(cS);
    %traces(ic(i)) = cS.caliper('shortestPerp'); % this is a bit more precise but slower

    % the following lines are from shape2d.byFV
    % just consider one direction
    fcond = lS(:,2)<0;
    lS(fcond,:)=lS(fcond,:).*-1;
    dxy = [lS; -lS];

    % sort segments according to angle
    [~,id]= sort(atan2(dxy(:,2),dxy(:,1)));
    dxy = dxy(id,:);

    % sum up
    xyn = cumsum(dxy);

    % shift again
    xyn = [xyn(:,1) - mean(xyn(:,1)) xyn(:,2) - mean(xyn(:,2))];

    if useCalliper

      mid = round(size(xyn,1)/2);
      dxyn = xyn - [xyn(1+mid:end,:);xyn(1:mid,:)];
      delta = vecnorm(dxyn,2,2);

      % minimum and maximum Ferret diameter
      a = max(delta); [b,ib] = min(delta);

      % use vector perpendicular to short axis
      omega(ic(i)) = pi/2 + atan2(dxyn(ib,2),dxyn(ib,1));

    else

      % the following lines are taken from grain2d/principleComponent
      % compute length of line segments
      dist = sqrt(sum((xyn(1:end-1,:) - xyn(2:end,:)).^2,2));
      dist = 0.5*(dist(1:end) + [dist(end);dist(1:end-1)]);

      % weight vertices according to half the length of the adjacent faces
      xyn = xyn(1:end-1,:) .* [dist,dist] .* sum(xyn(1:end-1,:).^2,2).^(0.25);

      % compute eigen values and vectors
      [ew, omega(ic(i))] = eig2(xyn' * xyn);

      % halfaxes are square roots of the eigenvalues
      b = sqrt(ew(1)); a = sqrt(ew(2));
    end
    rel(ic(i)) = (a-b)./a;
  end

end

traces = vector3d.byPolar(pi/2,omega,'antipodal');


function [I] = spm_voice_onsets(Y,FS,C,U)
% Identify intervals containing acoustic energy and post onset minima
% FORMAT [I] = spm_voice_onsets(Y,FS,C,U)
%
% Y    - timeseries
% FS   - sampling frequency
% C    - Convolution kernel [Default: 1/16 sec]
% U    - crossing threshold [Default: 1/8  a.u]

%
% I{i} - cell array of intervals (time bins) containing spectral energy
%
% This routine identifies epochs constaining spectral energy of the power
% envelope, defined as the root mean square (RMS) power. The onset and
% offset of words is evaluated in terms of threshold crossings before and
% after the midpoint of a one second epoch. These are supplemented with
% internal minima (after the spectral peak).
%
% see also: spm_voice_onset.m
%__________________________________________________________________________
% Copyright (C) 2019 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_voice_onsets.m 7750 2019-12-05 17:54:29Z spm $

% find the interval that contains spectral energy
%==========================================================================
global VOX

if nargin < 2, FS = VOX.FS; end                  % onset  threshold
if nargin < 4, U  = 1/8;    end                  % height threshold
if nargin < 3, C  = 1/16;   end                  % smoothing

% identify threshold crossings in power
%--------------------------------------------------------------------------
[G,Y] = spm_voice_check(Y,FS,C);                 % smooth envelope
G     = G/mean(G);                               % normalise to mean
n     = length(G);                               % number of bins

% find zero crossings (of U), for isolated words
%--------------------------------------------------------------------------
j0    = find(G(1:end - 1) < U & G(2:end) > U);
jT    = find(G(1:end - 1) > U & G(2:end) < U);

% find minima
%--------------------------------------------------------------------------
j     = find(diff(G(1:end - 1)) < 0 & diff(G(2:end)) > 0);
if numel(j)
    j = j([FS; diff(j)] > FS/32);                % remove minima < 1/32
    j = j(G(j) < 2);                             % remove minima > 2*mean
end

% find onsets preceded by silence
%--------------------------------------------------------------------------
k0    = [];
for k = 1:numel(j0)
    i = (j0(k) - FS/8):(j0(k) - 1);
    i = fix(i(i < n & i > 1));
    if all(G(i) < U) && j0(k) < FS/2
        k0(end + 1) = j0(k);
    end
end

% find the last onset preceded by silence
%--------------------------------------------------------------------------
if isempty(k0), j0 = []; else, j0 = k0(end); end

% find offsets followed by silence
%--------------------------------------------------------------------------
kT    = [];
for k = 1:numel(jT)
    i = (jT(k) + 1):(jT(k) + FS/8);
    i = fix(i(i < n & i > 1));
    if all(G(i) < U) && jT(k) > FS/2
        kT(end + 1) = jT(k);
    end
end

% find the first offset followed by silence
%--------------------------------------------------------------------------
if isempty(kT), jT = []; else, jT = kT(1); end

% use the first minima in the absence of zero crossings
%--------------------------------------------------------------------------
if isempty(j0)
    try
        j0 = j(1);
    catch
        j0 = 1;
    end
end

% use the last minima in the absence of zero crossings
%--------------------------------------------------------------------------
if isempty(jT)
    try
        jT = j(end);
    catch
        jT = n;
    end
end

% use the last sample if offset precedes onset
%--------------------------------------------------------------------------
if (jT - j0) < 1, jT = n; end

% add internal minima
%--------------------------------------------------------------------------
i   = j(j < jT(end) & j > (j0 + FS/16));
jT  = sort(unique([jT; i]));

% indices of interval containing spectral energy
%--------------------------------------------------------------------------
I     = {};
for i = 1:numel(j0)
    for j = 1:numel(jT)

        % k-th interval
        %------------------------------------------------------------------
        k  = j0(i):jT(j);
        ni = numel(k);

        % retain intervals of plausible length
        %------------------------------------------------------------------
        if ni > FS/16 && ni < FS
            I{end + 1} = k;
        end
    end
end

% sort lengths (longest last), with 3 minima or less
%--------------------------------------------------------------------------
for i = 1:numel(I)
    ni(i) = numel(I{i});
end
[d,j] = sort(ni,'ascend');
I     = I(j(1:min(3,end)));

% graphics(if requested)
%==========================================================================
if ~VOX.onsets
    return
else
    spm_figure('GetWin','onsets'); clf;
end

% timeseries
%--------------------------------------------------------------------------
pst   = (1:n)/FS;
subplot(2,1,1)
plot(pst,Y/max(Y), 'b'),     hold on
plot(pst,G/max(G),':b'),     hold on
plot([1 1]/2,[-1 1],'b'),    hold on
for i = 1:numel(I)
    x = [I{i}(1),I{i}(end),I{i}(end),I{i}(1)]/FS;
    y = [-1,-1,1,1];
    c = spm_softmax(rand(3,1))';
    h = fill(x,y,c);
    set(h,'Facealpha',1/8,'EdgeAlpha',1/8);
end
title('Onsets and offsets','FontSize',16)
xlabel('peristimulus time (seconds)'), spm_axis tight, hold off

% envelope and threshold crossings
%--------------------------------------------------------------------------
subplot(2,1,2)
plot(pst,G,'r'), hold on
plot(pst,0*pst + U,'-.'),         hold on
plot([1 1]/2,[0 max(G)],'b'),     hold on
for i = 1:numel(j0), plot(pst(j0(i)),G(j0(i)),'og'), end
for i = 1:numel(jT), plot(pst(jT(i)),G(jT(i)),'or'), end
title('Spectral envelope','FontSize',16)
xlabel('peristimulus time (secs)'), spm_axis tight, hold off
drawnow, pause(1/4)

% uncomment to play interval
%--------------------------------------------------------------------------
% sound(Y(i),FS)