function h=error_ellipse(varargin)
% ERROR_ELLIPSE - plot an error ellipse, or ellipsoid, defining confidence region
%    ERROR_ELLIPSE(C22) - Given a 2x2 covariance matrix, plot the
%    associated error ellipse, at the origin. It returns a graphics handle
%    of the ellipse that was drawn.
%
%    ERROR_ELLIPSE(C33) - Given a 3x3 covariance matrix, plot the
%    associated error ellipsoid, at the origin, as well as its projections
%    onto the three axes. Returns a vector of 4 graphics handles, for the
%    three ellipses (in the X-Y, Y-Z, and Z-X planes, respectively) and for
%    the ellipsoid.
%
%    ERROR_ELLIPSE(C,MU) - Plot the ellipse, or ellipsoid, centered at MU,
%    a vector whose length should match that of C (which is 2x2 or 3x3).
%
%    ERROR_ELLIPSE(...,'Property1',Value1,'Name2',Value2,...) sets the
%    values of specified properties, including:
%      'C' - Alternate method of specifying the covariance matrix
%      'mu' - Alternate method of specifying the ellipse (-oid) center
%      'conf' - A value betwen 0 and 1 specifying the confidence interval.
%        the default is 0.5 which is the 50% error ellipse.
%      'scale' - Allow the plot the be scaled to difference units.
%      'style' - A plotting style used to format ellipses.
%      'clip' - specifies a clipping radius. Portions of the ellipse, -oid,
%        outside the radius will not be shown.
%
%    NOTES: C must be positive definite for this function to work properly.

default_properties = struct(...
  'C', [], ... % The covaraince matrix (required)
  'mu', [], ... % Center of ellipse (optional)
  'conf', 0.5, ... % Percent confidence/100
  'scale', 1, ... % Scale factor, e.g. 1e-3 to plot m as km
  'style', '', ...  % Plot style
  'clip', inf); % Clipping radius

if length(varargin) >= 1 & isnumeric(varargin{1})
  default_properties.C = varargin{1};
  varargin(1) = [];
end

if length(varargin) >= 1 & isnumeric(varargin{1})
  default_properties.mu = varargin{1};
  varargin(1) = [];
end

if length(varargin) >= 1 & isnumeric(varargin{1})
  default_properties.conf = varargin{1};
  varargin(1) = [];
end

if length(varargin) >= 1 & isnumeric(varargin{1})
  default_properties.scale = varargin{1};
  varargin(1) = [];
end

if length(varargin) >= 1 & ~ischar(varargin{1})
  error('Invalid parameter/value pair arguments.')
end

prop = getopt(default_properties, varargin{:});
C = prop.C;

if isempty(prop.mu)
  mu = zeros(length(C),1);
else
  mu = prop.mu;
end

conf = prop.conf;
scale = prop.scale;
style = prop.style;

if conf <= 0 | conf >= 1
  error('conf parameter must be in range 0 to 1, exclusive')
end

[r,c] = size(C);
if r ~= c | (r ~= 2 & r ~= 3)
  error(['Don''t know what to do with ',num2str(r),'x',num2str(c),' matrix'])
end

x0=mu(1);
y0=mu(2);

% Compute quantile for the desired percentile
k = sqrt(qchisq(conf,r)); % r is the number of dimensions (degrees of freedom)

hold_state = get(gca,'nextplot');

if r==3 & c==3
  z0=mu(3);

  % Make the matrix has positive eigenvalues - else it's not a valid covariance matrix!
  if any(eig(C) <=0)
    error('The covariance matrix must be positive definite (it has non-positive eigenvalues)')
  end

  % C is 3x3; extract the 2x2 matricies, and plot the associated error
  % ellipses. They are drawn in space, around the ellipsoid; it may be
  % preferable to draw them on the axes.
  Cxy = C(1:2,1:2);
  Cyz = C(2:3,2:3);
  Czx = C([3 1],[3 1]);

  [x,y,z] = getpoints(Cxy,prop.clip);
  h1=plot3(x0+k*x,y0+k*y,z0+k*z,prop.style);hold on
  [y,z,x] = getpoints(Cyz,prop.clip);
  h2=plot3(x0+k*x,y0+k*y,z0+k*z,prop.style);hold on
  [z,x,y] = getpoints(Czx,prop.clip);
  h3=plot3(x0+k*x,y0+k*y,z0+k*z,prop.style);hold on

  [eigvec,eigval] = eig(C);

  [X,Y,Z] = ellipsoid(0,0,0,1,1,1);
  XYZ = [X(:),Y(:),Z(:)]*sqrt(eigval)*eigvec';

  X(:) = scale*(k*XYZ(:,1)+x0);
  Y(:) = scale*(k*XYZ(:,2)+y0);
  Z(:) = scale*(k*XYZ(:,3)+z0);
  h4=surf(X,Y,Z);
  colormap gray
  alpha(0.3)
  camlight
  if nargout
    h=[h1 h2 h3 h4];
  end
elseif r==2 & c==2
  % Make the matrix has positive eigenvalues - else it's not a valid covariance matrix!
  if any(eig(C) <=0)
    error('The covariance matrix must be positive definite (it has non-positive eigenvalues)')
  end

  [x,y,z] = getpoints(C,prop.clip);
  h1=plot(scale*(x0+k*x),scale*(y0+k*y),prop.style);
  set(h1,'zdata',z+1)
  if nargout
    h=h1;
  end
else
  error('C (covaraince matrix) must be specified as a 2x2 or 3x3 matrix)')
end
%axis equal

set(gca,'nextplot',hold_state);

%---------------------------------------------------------------
% getpoints - Generate x and y points that define an ellipse, given a 2x2
%   covariance matrix, C. z, if requested, is all zeros with same shape as
%   x and y.
function [x,y,z] = getpoints(C,clipping_radius)

n=100; % Number of points around ellipse
p=0:pi/n:2*pi; % angles around a circle

[eigvec,eigval] = eig(C); % Compute eigen-stuff
xy = [cos(p'),sin(p')] * sqrt(eigval) * eigvec'; % Transformation
x = xy(:,1);
y = xy(:,2);
z = zeros(size(x));

% Clip data to a bounding radius
if nargin >= 2
  r = sqrt(sum(xy.^2,2)); % Euclidian distance (distance from center)
  x(r > clipping_radius) = nan;
  y(r > clipping_radius) = nan;
  z(r > clipping_radius) = nan;
end

%---------------------------------------------------------------
function x=qchisq(P,n)
% QCHISQ(P,N) - quantile of the chi-square distribution.
if nargin<2
  n=1;
end

s0 = P==0;
s1 = P==1;
s = P>0 & P<1;
x = 0.5*ones(size(P));
x(s0) = -inf;
x(s1) = inf;
x(~(s0|s1|s))=nan;

for ii=1:14
  dx = -(pchisq(x(s),n)-P(s))./dchisq(x(s),n);
  x(s) = x(s)+dx;
  if all(abs(dx) < 1e-6)
    break;
  end
end

%---------------------------------------------------------------
function F=pchisq(x,n)
% PCHISQ(X,N) - Probability function of the chi-square distribution.
if nargin<2
  n=1;
end
F=zeros(size(x));

if rem(n,2) == 0
  s = x>0;
  k = 0;
  for jj = 0:n/2-1;
    k = k + (x(s)/2).^jj/factorial(jj);
  end
  F(s) = 1-exp(-x(s)/2).*k;
else
  for ii=1:numel(x)
    if x(ii) > 0
      F(ii) = quadl(@dchisq,0,x(ii),1e-6,0,n);
    else
      F(ii) = 0;
    end
  end
end

%---------------------------------------------------------------
function f=dchisq(x,n)
% DCHISQ(X,N) - Density function of the chi-square distribution.
if nargin<2
  n=1;
end
f=zeros(size(x));
s = x>=0;
f(s) = x(s).^(n/2-1).*exp(-x(s)/2)./(2^(n/2)*gamma(n/2));

%---------------------------------------------------------------
function properties = getopt(properties,varargin)
%GETOPT - Process paired optional arguments as 'prop1',val1,'prop2',val2,...
%
%   getopt(properties,varargin) returns a modified properties structure,
%   given an initial properties structure, and a list of paired arguments.
%   Each argumnet pair should be of the form property_name,val where
%   property_name is the name of one of the field in properties, and val is
%   the value to be assigned to that structure field.
%
%   No validation of the values is performed.
%
% EXAMPLE:
%   properties = struct('zoom',1.0,'aspect',1.0,'gamma',1.0,'file',[],'bg',[]);
%   properties = getopt(properties,'aspect',0.76,'file','mydata.dat')
% would return:
%   properties =
%         zoom: 1
%       aspect: 0.7600
%        gamma: 1
%         file: 'mydata.dat'
%           bg: []
%
% Typical usage in a function:
%   properties = getopt(properties,varargin{:})

% Process the properties (optional input arguments)
prop_names = fieldnames(properties);
TargetField = [];
for ii=1:length(varargin)
  arg = varargin{ii};
  if isempty(TargetField)
    if ~ischar(arg)
      error('Propery names must be character strings');
    end
    f = find(strcmp(prop_names, arg));
    if length(f) == 0
      error('%s ',['invalid property ''',arg,'''; must be one of:'],prop_names{:});
    end
    TargetField = arg;
  else
    % properties.(TargetField) = arg; % Ver 6.5 and later only
    properties = setfield(properties, TargetField, arg); % Ver 6.1 friendly
    TargetField = '';
  end
end
if ~isempty(TargetField)
  error('Property names and values must be specified in pairs.');
end


function point_merge_test05 ( m, n, n_unique, tol, seed )

%*****************************************************************************80
%
%% POINT_MERGE_TEST05 times uniqueness indexing with a tolerance.
%
%  Discussion:
%
%    POINT_RADIAL_TOL_UNIQUE_COUNT uses an algorithm that should be,
%      in general, O(N);
%    POINT_TOL_UNIQUE_COUNT uses an O(N^2) algorithm.
%
%    For this test, we just want to make sure the algorithms agree
%    in the counting.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 July 2010
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'POINT_MERGE_TEST05\n' );
  fprintf ( 1, '  We time the computations in TEST04, calling\n' );
  fprintf ( 1, '  POINT_RADIAL_TOL_UNIQUE_COUNT, (with random center)\n' );
  fprintf ( 1, '  POINT_TOL_UNIQUE_COUNT, (with zero tolerance)\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  M = %d\n', m );
  fprintf ( 1, '  N = %d\n', n );
  fprintf ( 1, '  TOL = %f\n', tol );
  fprintf ( 1, '  SEED = %d\n', seed );

  [ a, seed ] = r8col_duplicates ( m, n, n_unique, seed );
%
%  The form of the tolerance test means that if two vectors are initially
%  equal, they remain "tolerably equal" after the addition of random
%  perturbation vectors whose 2-norm is no greater than TOL/2.
%
  for j = 1 : n
    [ r, seed ] = r8vec_uniform_01 ( m, seed );
    r(1:m) = r(1:m) / sqrt ( sum ( r(1:m).^2 ) );
    a(1:m,j) = a(1:m,j) + 0.5 * tol * r(1:m,1);
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  N_UNIQUE =                      %d\n', n_unique );

  tic;
  [ unique_num, undx, xdnu, seed ] = point_radial_tol_unique_index ( m, n, a, ...
    tol, seed );
  wtime = toc;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  POINT_RADIAL_TOL_UNIQUE_INDEX\n' );
  fprintf ( 1, '  Unique_num = %d\n', unique_num );
  fprintf ( 1, '  Time = %f\n', wtime );

  tic;
  [ unique_num, xdnu ] = point_tol_unique_index ( m, n, a, tol );
  wtime = toc;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  POINT_TOL_UNIQUE_INDEX\n' );
  fprintf ( 1, '  Unique_num = %d\n', unique_num );
  fprintf ( 1, '  Time = %f\n', wtime );

  return
end


function value = pei_condition ( alpha, n )

%*****************************************************************************80
%
%% PEI_CONDITION returns the L1 condition of the PEI matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 January 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ALPHA, the scalar that defines the Pei matrix.  A
%    typical value of ALPHA is 1.0.
%
%    Input, integer N, the order of A.
%
%    Output, real VALUE, the L1 condition.
%
  a_norm = abs ( alpha + 1.0 ) + n - 1;
  b_norm = ( abs ( alpha + n - 1.0 ) + n - 1.0 ) ...
    / abs ( alpha * ( alpha + n ) );
  value = a_norm * b_norm;

  return
end


function sparse_grid_test01 ( dim_min, dim_max, level_max_min, ...
  level_max_max )

%*****************************************************************************80
%
%% TEST01 tests SPARSE_GRID_OFN_SIZE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 December 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_MIN, the minimum spatial dimension to consider.
%
%    Input, integer DIM_MAX, the maximum spatial dimension to consider.
%
%    Input, integer LEVEL_MAX_MIN, the minimum value of LEVEL_MAX to consider.
%
%    Input, integer LEVEL_MAX_MAX, the maximum value of LEVEL_MAX to consider.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST01\n' );
  fprintf ( 1, '  SPARSE_GRID_OFN_SIZE returns the number of\n' );
  fprintf ( 1, '  distinct points in a sparse grid made up of all \n' );
  fprintf ( 1, '  product grids formed from completely nested open \n' );
  fprintf ( 1, '  quadrature rules.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The sparse grid is the sum of all product grids\n' );
  fprintf ( 1, '  of order LEVEL, with\n' );
  fprintf ( 1, '    0 <= LEVEL <= LEVEL_MAX.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  LEVEL is the sum of the levels of the 1D rules,\n' );
  fprintf ( 1, '  the order of the 1D rule is 2^(LEVEL+1) - 1,\n' );
  fprintf ( 1, '  the region is [-1,1]^DIM_NUM.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  For this kind of rule, there is complete nesting,\n' );
  fprintf ( 1, '  that is, a sparse grid of a given level includes\n' );
  fprintf ( 1, '  ALL the points on grids of lower levels.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   DIM: ' );

  for dim_num = dim_min : dim_max
    fprintf ( 1, '  %10d', dim_num );
  end

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

  for level_max = level_max_min : level_max_max
    fprintf ( 1, '    %4d', level_max );
    for dim_num = dim_min : dim_max
      point_num = sparse_grid_ofn_size ( dim_num, level_max );
      fprintf ( 1, '  %10d', point_num );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function value = i4vec_odd_all ( n, a )

%*****************************************************************************80
%
%% I4VEC_ODD_ALL is TRUE if all entries of an I4VEC are odd.
%
%  Discussion:
%
%    An I4VEC is a vector of I4's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, integer A(N), the vector.
%
%    Output, logical VALUE, TRUE if all entries are odd.
%
  value = all ( mod ( a(1:n), 2 ) == 1 );

  return
end


function mckinnon_test ( )

%*****************************************************************************80
%
%% MCKINNON_TEST works with the McKinnon function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 January 2012
%
%  Author:
%
%    John Burkardt
%
  global phi
  global tau
  global theta

  fprintf ( 1, '\n' );
  fprintf ( 1, 'MCKINNON_TEST:\n' );
  fprintf ( 1, '  Test COMPASS_SEARCH with the McKinnon function.\n' );
  m = 2;
  delta_tol = 0.00001;
  delta = 0.3;
  k_max = 20000;
%
%  Test 1
%
  a = ( 1.0 + sqrt ( 33.0 ) ) / 8.0;
  b = ( 1.0 - sqrt ( 33.0 ) ) / 8.0;

  phi = 10.0;
  tau = 1.0;
  theta = 15.0;

  x = [ a, b ];
  r8vec_print ( m, x, '  Initial point X0:' );
  fprintf ( 1, '  PHI = %f, TAU = %f, THETA = %f\n', phi, tau, theta );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X0) = %g\n', mckinnon ( m, x ) );

  [ x, fx, k ] = compass_search ( @mckinnon, m, x, delta_tol, delta, k_max );
  r8vec_print ( m, x, '  Estimated minimizer X1:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X1) = %g, number of steps = %d\n', fx, k );

  x = [ 0.0, -0.5 ];
  r8vec_print ( m, x, '  Correct minimizer X*:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X*) = %g\n', mckinnon ( m, x ) );
%
%  Test 2
%
  a = ( 1.0 + sqrt ( 33.0 ) ) / 8.0;
  b = ( 1.0 - sqrt ( 33.0 ) ) / 8.0;

  phi = 60.0;
  tau = 2.0;
  theta = 6.0;

  x = [ a, b ];
  r8vec_print ( m, x, '  Initial point X0:' );
  fprintf ( 1, '  PHI = %f, TAU = %f, THETA = %f\n', phi, tau, theta );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X0) = %g\n', mckinnon ( m, x ) );

  [ x, fx, k ] = compass_search ( @mckinnon, m, x, delta_tol, delta, k_max );
  r8vec_print ( m, x, '  Estimated minimizer X1:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X1) = %g, number of steps = %d\n', fx, k );

  x = [ 0.0, -0.5 ];
  r8vec_print ( m, x, '  Correct minimizer X*:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X*) = %g\n', mckinnon ( m, x ) );
%
%  Test 3
%
  a = ( 1.0 + sqrt ( 33.0 ) ) / 8.0;
  b = ( 1.0 - sqrt ( 33.0 ) ) / 8.0;

  phi = 4000.0;
  tau = 3.0;
  theta = 6.0;

  x = [ a, b ];
  r8vec_print ( m, x, '  Initial point X0:' );
  fprintf ( 1, '  PHI = %f, TAU = %f, THETA = %f\n', phi, tau, theta );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X0) = %g\n', mckinnon ( m, x ) );

  [ x, fx, k ] = compass_search ( @mckinnon, m, x, delta_tol, delta, k_max );
  r8vec_print ( m, x, '  Estimated minimizer X1:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X1) = %g, number of steps = %d\n', fx, k );

  x = [ 0.0, -0.5 ];
  r8vec_print ( m, x, '  Correct minimizer X*:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X*) = %g\n', mckinnon ( m, x ) );

  return
end


function element_num = grid_q8_element_num ( nelemx, nelemy )

%*****************************************************************************80
%
%% GRID_Q8_ELEMENT_NUM counts the elements in a grid of 8 node quadrilaterals.
%
%  Example:
%
%    Input:
%
%      NELEMX = 3, NELEMY = 2
%
%    Output:
%
%      ELEMENT_NUM = NELEMX * NELEMY = 6
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 June 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NELEMX, NELEMY, the number of elements along the
%    X and Y directions.  The number of elements generated will be
%    NELEMX * NELEMY.
%
%    Output, integer ELEMENT_NUM, the number of elements in the grid.
%
  element_num = nelemx * nelemy;

  return
end


function y = daub16_transform ( n, x )

%*****************************************************************************80
%
%% DAUB16_TRANSFORM computes the DAUB16 transform of a vector.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 July 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the dimension of the vector.
%    N must be a power of 2 and at least 4.
%
%    Input, real X(N), the vector to be transformed.
%
%    Output, real Y(N), the transformed vector.
%
  c = [ ...
     5.441584224310400E-02; ...
     3.128715909142999E-01; ...
     6.756307362972898E-01; ...
     5.853546836542067E-01; ...
    -1.582910525634930E-02; ...
    -2.840155429615469E-01; ...
     4.724845739132827E-04; ...
     1.287474266204784E-01; ...
    -1.736930100180754E-02; ...
    -4.408825393079475E-02; ...
     1.398102791739828E-02; ...
     8.746094047405776E-03; ...
    -4.870352993451574E-03; ...
    -3.917403733769470E-04; ...
     6.754494064505693E-04; ...
    -1.174767841247695E-04 ];
  p = 15;
  y(1:n,1) = x(1:n);
  m = n;
  q = floor ( ( p - 1 ) / 2 );

  while ( 4 <= m )

    i = 1;
    z(1:m,1) = 0.0;

    for j = 1 : 2 : m - 1

      mh = floor ( m / 2 );
      for k = 0 : 2 : p - 1
        j0 = i4_wrap ( j + k,     1, m );
        j1 = i4_wrap ( j + k + 1, 1, m );
        z(i,1)    = z(i,1)    + c(  k+1) * y(j0) + c(  k+2) * y(j1);
        z(i+mh,1) = z(i+mh,1) + c(p-k+1) * y(j0) - c(p-k  ) * y(j1);
      end

      i = i + 1;

    end

    y(1:m,1) = z(1:m);

    m = floor ( m / 2 );

  end

  return
end


function cadj=ref_spreadadj_1(coef)
%REF_SPREADADJ_1  Symbol of adjoint spreading function.
%   Usage: cadj=ref_spreadadj_1(c,number);
%
%   Development version by FJ for comparison of different implementations
%   cadj=SPREADADJ(c) will compute the symbol cadj of the spreading
%   operator that is the adjoint of the spreading operator with symbol c.
%
%   This is the implementation previously in the toolbox,
%   with the addition of an initialisation of cadj before the loop
%   leading to a huge speed improvement :

L=size(coef,1);

% Matlab cannot handle an FFT of a sparse matrix.
coef=ifft(full(coef));

cadj = zeros(L);
for ii=0:L-1
  for jj=0:L-1
    cadj(ii+1,jj+1)=conj(coef(mod(ii-jj,L)+1,mod(-jj,L)+1));
  end;
end;

cadj=fft(full(cadj));


%% GSA_FAST_GetSi: calculate the FAST sensitivity indices
% Ref: Cukier, R.I., C.M. Fortuin, K.E. Shuler, A.G. Petschek and J.H.
% Schaibly (1973). Study of the sensitivity of coupled reaction systems to uncertainties in rate coefficients. I Theory. Journal of Chemical Physics
%
% Max number of input variables: 50
%
% Usage:
%   Si = GSA_FAST_GetSi(pro)
%
% Inputs:
%    pro                project structure
%
% Output:
%    Si                 vecotr of sensitivity coefficients
%
% ------------------------------------------------------------------------
% See also
%
% Author : Flavio Cannavo'
% e-mail: flavio(dot)cannavo(at)gmail(dot)com
% Release: 1.0
% Date   : 01-05-2011
%
% History:
% 1.0  01-05-2011  First release.
%%

function Si = GSA_FAST_GetSi(pro)

k = length(pro.Inputs.pdfs);

M = 4;

W = fnc_FAST_getFreqs(k);

Wmax = W(k);
N = 2*M*Wmax+1;
q = (N-1)/2;

S = pi/2*(2*(1:N)-N-1)/N;
alpha = W'*S;

NormedX = 0.5 + asin(sin(alpha'))/pi;

X = fnc_FAST_getInputs(pro, NormedX);

Y = nan(N,1);

for j=1:N
    Y(j) = pro.Model.handle(X(j,:));
end

A = zeros(N,1);
B = zeros(N,1);
N0 = q+1;

for j=2:2:N
    A(j) = 1/N*(Y(N0)+(Y(N0+(1:q))+Y(N0-(1:q)))'* ...
        cos(pi*j*(1:q)/N)');
end

for j=1:2:N
    B(j) = 1/N*(Y(N0+(1:q))-Y(N0-(1:q)))'* ...
        sin(pi*j*(1:q)/N)';
end

V = 2*(A'*A+B'*B);

for i=1:k
    Vi=0;
    for j=1:M
        Vi = Vi+A(j*W(i))^2+B(j*W(i))^2;
    end
    Vi = 2*Vi;
    Si(i) = Vi/V;
end


function Brockett_Wen_bfgs_test()

    n = 500;
    p = 4;
    B = randn(n, n); B = B + B';
    D = sparse(diag(p : -1 : 1));

    M = stiefelfactory(n, p);

    problem.M = M;
    problem.cost  = @cost;
    problem.egrad = @egrad;

    % Cost function
    function f = cost(X)
        f = trace(X'*(B*X*D));
    end

    % Euclidean gradient of the cost function
    function g = egrad(X)
        g = 2*B*X*D;
    end

    problem.precon = preconBFGS(problem);
    problem.linesearch = @(x, xdot, storedb, key) 1;
    options.beta_type ='steep';
    options.tolgradnorm = 1e-5;
    conjugategradient(problem, [], options);

    pause;

    % For comparison, run RTR-FD
    warning('off', 'manopt:getHessian:approx');
    problem = rmfield(problem, 'precon');
    trustregions(problem);

end


function S = getcovariance(mix,inds,centers)

if ~exist('inds','var'),
  inds = 1:mix.nin;
end;
if ~exist('centers','var'),
  centers = 1:mix.ncentres;
end;

inds = inds(:)';
ninds = length(inds);
centers = centers(:)';
ncenters = length(centers);
S = zeros(ninds,ninds,ncenters);

switch mix.covar_type,

 case 'spherical',
  for i = 1:ncenters,
    j = centers(i);
    S(:,:,i) = eye(ninds)*mix.covars(j);
  end;

 case 'diag',
  for i = 1:ncenters,
    j = centers(i);
    S(:,:,i) = diag(mix.covars(j,:));
  end;

 case {'full','sparse'},
  S = mix.covars(inds,inds,centers);

 case 'ppca',
  for i = 1:ncenters,
    j = centers(i);
    covars = mix.covars(j) * eye(mix.nin) + ...
	     mix.U(:, :, j)*(diag(mix.lambda(j, :))-diag(mix.covars(j)))* ...
	     (mix.U(:, :, j)');
    S(:,:,i) = covars(inds,inds);
  end;

 case 'zero',
  S(:) = 0;

 otherwise,
  error(['Unknown covar_type ',mix.covar_type]);
end;


function [GLRLM] = getGLRLM(ROIonly,levels)
% -------------------------------------------------------------------------
% function [GLRLM] = getGLRLM(ROIonly,levels)
% -------------------------------------------------------------------------
% DESCRIPTION:
% This function computes the Gray-Level Run-Length Matrix (GLRLM) of the
% region of interest (ROI) of an input volume. The input volume is assumed
% to be isotropically resampled. Only one GLRLM is computed per scan,
% simultaneously adding up all possible run-lengths in the 13 directions of
% the 3D space. To account for discretization length differences, runs
% constructed from voxels separated by a distance of sqrt(3) increment the
% GLRLM by a value of sqrt(3), runs constructed from voxels separated by a
% distance of sqrt(2) increment the GLRLM by a value of sqrt(2), and runs
% constructed from voxels separated by a distance of 1 increment the GLRLM
% by a value of 1. This function uses other functions from Wei's GLRLM
% toolbox [2].
%
% --> This function is compatible with 2D analysis (language not adapted in the text)
% -------------------------------------------------------------------------
% REFERENCES:
% [1] Galloway, M. M. (1975). Texture analysis using gray level run lengths.
%     Computer Graphics and Image Processing, 4(2), 172–179.
% [2] Wei's GLRLM toolbox: Xunkai Wei, Gray Level Run Length Matrix Toolbox
%     v1.0, Software,Beijing Aeronautical Technology Research Center, 2007.
%     <http://www.mathworks.com/matlabcentral/fileexchange/17482-gray-level-run-length-matrix-toolbox>
% -------------------------------------------------------------------------
% INPUTS:
% - ROIonly: Smallest box containing the ROI, with the imaging data ready
%            for texture analysis computations. Voxels outside the ROI are
%            set to NaNs.
% - levels: Vector containing the quantized gray-levels in the tumor region
%           (or reconstruction levels of quantization).
%
% ** 'ROIonly' and 'levels' should be outputs from 'prepareVolume.m' **
% -------------------------------------------------------------------------
% OUTPUTS:
% - GLRLM: Gray-Level Run-Length Matrix of 'ROIOnly'.
% -------------------------------------------------------------------------
% AUTHOR(S):
% - Martin Vallieres <mart.vallieres@gmail.com>
% - Xunkai Wei <xunkai.wei@gmail.com>
% -------------------------------------------------------------------------
% HISTORY:
% - Creation: January 2013
% - Revision: May 2015
% -------------------------------------------------------------------------
% STATEMENT:
% This file is part of <https://github.com/mvallieres/radiomics/>,
% a package providing MATLAB programming tools for radiomics analysis.
% --> Copyright (C) 2015  Martin Vallieres
%
%    This package is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    This package 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 package.  If not, see <http://www.gnu.org/licenses/>.
%
%    _______________________________________________________________
%
% --> Copyright (c) 2007-2012, Xunkai Wei
%     All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in
%       the documentation and/or other materials provided with the distribution
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.
% -------------------------------------------------------------------------

% PRELIMINARY
nLevel = length(levels);
if nLevel > 100
    adjust = 10000;
else
    adjust = 1000;
end
levelTemp = max(levels)+1;
ROIonly(isnan(ROIonly)) = levelTemp; % Last row needs to be taken out of the GLRLM
levels = [levels,levelTemp];

% QUANTIZATION EFFECTS CORRECTION
% In case (for example) we initially wanted to have 64 levels, but due to
% quantization, only 60 resulted.
uniqueVol = round(levels*adjust)/adjust;
ROIonly=round(ROIonly*adjust)/adjust;
NL = length(levels) - 1;

%INITIALIZATION
sizeV = size(ROIonly);
numInit = ceil(max(sizeV)*sqrt(3)); % Max run length
GLRLM = zeros(NL+1,numInit);

% START COMPUTATION
% Directions [1,0,0], [0 1 0], [1 1 0] and [-1 1 0] : 2D directions
% (x:right-left, y:top-bottom, z:3rd dimension)
if numel(size(ROIonly)) == 3
    nComp = sizeV(3); % We can add-up the GLRLMs taken separately in every image in the x-y plane
else
    nComp = 1;
end
for i = 1:nComp
    image = ROIonly(:,:,i);
    uniqueIm = unique(image);
    NLtemp = length(uniqueIm);
    indexRow = zeros(NLtemp,1);
    temp = image;
    for j = 1:NLtemp
        indexRow(j) = find(uniqueIm(j)==uniqueVol);
        image(temp==uniqueIm(j)) = j;
    end

    % [1,0,0]
    GLRLMtemp = rle_0(image,NLtemp);
    nRun = size(GLRLMtemp,2);
    GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun); % Cumulative addition into the GLRLM

    % [0 1 0]
    GLRLMtemp = rle_0(image',NLtemp);
    nRun = size(GLRLMtemp,2);
    GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun); % Cumulative addition into the GLRLM

    % [1 1 0]
    seq = zigzag(image);
    GLRLMtemp = rle_45(seq,NLtemp);
    nRun = size(GLRLMtemp,2);
    GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun)*sqrt(2); % Cumulative addition into the GLRLM

    % [-1 1 0]
    seq = zigzag(fliplr(image));
    GLRLMtemp = rle_45(seq,NLtemp);
    nRun = size(GLRLMtemp,2);
    GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun)*sqrt(2); % Cumulative addition into the GLRLM
end

if numel(size(ROIonly)) == 3 % 3D DIRECTIONS
    % Directions [0,0,1], [1 0 1] and [-1 0 1]
    % (x:right-left, y:top-bottom, z:3rd dimension)
    nComp = sizeV(1); % We can add-up the GLRLMs taken separately in every image in the x-z plane
    image = zeros(sizeV(3),sizeV(2));
    for i = 1:nComp
        for j = 1:sizeV(3)
            image(j,1:end) = ROIonly(i,1:end,j);
        end
        uniqueIm = unique(image);
        NLtemp = length(uniqueIm);
        indexRow = zeros(NLtemp,1);
        temp = image;
        for j=1:NLtemp
            indexRow(j) = find(uniqueIm(j)==uniqueVol);
            image(temp==uniqueIm(j)) = j;
        end

        % [0,0,1]
        GLRLMtemp = rle_0(image',NLtemp);
        nRun = size(GLRLMtemp,2);
        GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun); % Cumulative addition into the GLRLM

        % [1 0 1]
        seq = zigzag(image);
        GLRLMtemp = rle_45(seq,NLtemp);
        nRun = size(GLRLMtemp,2);
        GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun)*sqrt(2); % Cumulative addition into the GLRLM

        % [-1 0 1]
        seq = zigzag(fliplr(image));
        GLRLMtemp = rle_45(seq,NLtemp);
        nRun = size(GLRLMtemp,2);
        GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun)*sqrt(2); % Cumulative addition into the GLRLM
    end

    % Directions [0,1,1] and [0 -1 1]
    % (x:right-left, y:top-bottom, z:3rd dimension)
    nComp = sizeV(2); % We can add-up the GLRLMs taken separately in every image in the y-z plane
    image = zeros(sizeV(1),sizeV(3));
    for i = 1:nComp
        for j = 1:sizeV(3)
            image(1:end,j) = ROIonly(1:end,i,j);
        end
        uniqueIm = unique(image);
        NLtemp = length(uniqueIm);
        indexRow = zeros(NLtemp,1);
        temp = image;
        for j = 1:NLtemp
            indexRow(j) = find(uniqueIm(j)==uniqueVol);
            image(temp==uniqueIm(j)) = j;
        end

        % [0,1,1]
        seq = zigzag(image);
        GLRLMtemp = rle_45(seq,NLtemp);
        nRun = size(GLRLMtemp,2);
        GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun)*sqrt(2); % Cumulative addition into the GLRLM

        % [0 -1 1]
        seq = zigzag(fliplr(image));
        GLRLMtemp = rle_45(seq,NLtemp);
        nRun = size(GLRLMtemp,2);
        GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun)*sqrt(2); % Cumulative addition into the GLRLM
    end

    % Four corners: [1,1,1], [-1,1,1], [-1,1,-1], [1,1,-1]
    % (x:right-left, y:top-bottom, z:3rd dimension)
    image = zeros(sizeV(3),sizeV(2));
    temp = rand(sizeV(3),sizeV(2));
    diagTemp = spdiags(temp);
    szDiag = size(diagTemp);
    diagMat1 = zeros(szDiag(1),szDiag(2),sizeV(1));
    diagMat2 = zeros(size(diagTemp,1),size(diagTemp,2),sizeV(1));
    for i = 1:sizeV(1)
        for j = 1:sizeV(3)
            image(j,1:end) = ROIonly(i,1:end,j);
        end
        try
            diagMat1(:,:,i)=spdiags(image);
        catch
            % Add a column at the beginning to prevent errors
            temp=spdiags(image);
            numberDiff=abs(size(temp,2)-size(diagMat1,2));
            if mod(numberDiff,2) % Odd difference number
                temp=padarray(temp,[0,(numberDiff+1)/2,0],0);
                diagMat1(:,:,i)=temp(:,1:end-1);
            else
                diagMat1(:,:,i)=padarray(temp,[0,numberDiff/2,0],0);
            end
        end
        try
            diagMat2(:,:,i)=spdiags(fliplr(image));
        catch
            % Add a column at the beginning to prevent errors
            temp = spdiags(fliplr(image));
            numberDiff = abs(size(temp,2)-size(diagMat2,2));
            if mod(numberDiff,2) % Odd difference number
                temp = padarray(temp,[0,(numberDiff+1)/2,0],0);
                diagMat2(:,:,i) = temp(:,1:end-1);
            else
                diagMat2(:,:,i) = padarray(temp,[0,numberDiff/2,0],0);
            end
        end
    end
    for j = 1:szDiag(2)
        index = (diagMat1(:,j,1)~=0);
        nTemp = sum(index);
        image1 = zeros(sizeV(1),nTemp);
        image2 = zeros(sizeV(1),nTemp);
        for k = 1:sizeV(1)
            image1(k,1:nTemp) = diagMat1(index(1:end),j,k)';
            image2(k,1:nTemp) = diagMat2(index(1:end),j,k)';
        end

        % 2 first corners
        uniqueIm = unique(image1);
        NLtemp = length(uniqueIm);
        indexRow = zeros(NLtemp,1);
        temp = image1;
        for i = 1:NLtemp
            indexRow(i) = find(uniqueIm(i)==uniqueVol);
            image1(temp==uniqueIm(i)) = i;
        end
        seq = zigzag(image1);
        GLRLMtemp = rle_45(seq,NLtemp);
        nRun = size(GLRLMtemp,2);
        GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun)*sqrt(3); % Cumulative addition into the GLRLM
        seq = zigzag(fliplr(image1));
        GLRLMtemp = rle_45(seq,NLtemp);
        nRun = size(GLRLMtemp,2);
        GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun)*sqrt(3); % Cumulative addition into the GLRLM

        % 2 last corners
        uniqueIm = unique(image2);
        NLtemp = length(uniqueIm);
        indexRow = zeros(NLtemp,1);
        temp = image2;
        for i = 1:NLtemp
            indexRow(i) = find(uniqueIm(i)==uniqueVol);
            image2(temp==uniqueIm(i)) = i;
        end
        seq = zigzag(image2);
        GLRLMtemp = rle_45(seq,NLtemp);
        nRun = size(GLRLMtemp,2);
        GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun)*sqrt(3); % Cumulative addition into the GLRLM
        seq = zigzag(fliplr(image2));
        GLRLMtemp = rle_45(seq,NLtemp);
        nRun = size(GLRLMtemp,2);
        GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun)*sqrt(3); % Cumulative addition into the GLRLM
    end
end

% REMOVE UNECESSARY COLUMNS
GLRLM(end,:) = [];
stop = find(sum(GLRLM),1,'last');
GLRLM(:,(stop+1):end) = [];

end


function y = vl_nnspnorm(x, param, dzdy)
%VL_NNSPNORM CNN spatial normalization.
%   Y = VL_NNSPNORM(X, PARAM) computes the spatial normalization of
%   the data X with parameters PARAM = [PH PW ALPHA BETA]. Here PH and
%   PW define the size of the spatial neighbourhood used for
%   nomalization.
%
%   For each feature channel, the function computes the sum of squares
%   of X inside each rectangle, N2(i,j). It then divides each element
%   of X as follows:
%
%      Y(i,j) = X(i,j) / (1 + ALPHA * N2(i,j))^BETA.
%
%   DZDX = VL_NNSPNORM(X, PARAM, DZDY) computes the derivative of the
%   block projected onto DZDY. DZDX and DZDY have the same dimensions
%   as X and Y respectively.

% Copyright (C) 2015 Karel Lenc and Andrea Vedaldi.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

pad = floor((param(1:2)-1)/2) ;
pad = [pad ; param(1:2)-1-pad] ;

n2 = vl_nnpool(x.*x, param(1:2), 'method', 'avg', 'pad', pad) ;
f = 1 + param(3) * n2 ;

if nargin <= 2 || isempty(dzdy)
  y = f.^(-param(4)) .* x ;
else
  t = vl_nnpool(x.*x, param(1:2), f.^(-param(4)-1) .* dzdy .* x, 'method', 'avg', 'pad', pad) ;
  y = f.^(-param(4)) .* dzdy - 2 * param(3)*param(4) * x .* t ;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% hessian.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function h = mcs_hessian(i,k,x,x0,f,f0,g,G)
% computes the element G(i,k) of the Hessian of the local quadratic
% model
% Input:
% i, k		indices (k < i)
% x(1:n)	'neighbor' of x0 used for computing G(i,k); differs from
%		x0 only in the ith and kth component
% x0(1:n)	point around which the quadratic model is computed
% f		function value at x
% f0		function value at x0
% g(1:i)	components of the gradient of the local quadratic model %		that have already been computed
% G		components of the Hessian of the local quadratic model
%		that have already been computed
% Output:
% h = G(i,k)	newly computed nondiagonal element of the Hessian

function h = mcs_hessian(i,k,x,x0,f,f0,g,G)
h = f-f0-g(i)*(x(i)-x0(i))-g(k)*(x(k)-x0(k))-0.5*G(i,i)*(x(i)-x0(i))^2-0.5*G(k,k)*(x(k)-x0(k))^2;
h = h/(x(i)-x0(i))/(x(k)-x0(k));


function [gm, gc, pm, pc] = termmarg(dB,degrees,w)
%
% Utility Function: TERMMARG
%
% The purpose of this function is to compute the gain and phase margins
% from the frequency response data

% Author: Craig Borghesani
% Date: 9/19/94
% Revised: 10/27/94
% Copyright (c) 1999, Prentice-Hall

%
% DETERMINING GAIN MARGIN
%
% find where the phase is less than -180 degrees.  this is done by adding
% +180 degrees and looking for where the sign changes from +1 to -1

shift_degrees = degrees + 180;
sign_deg = sign(shift_degrees);
diff_sign = abs([0,diff(sign_deg)]);
cross_180 = find(diff_sign == 2);

% what if phase vector begins at -180 (or very close to)
if shift_degrees(1) < 1 & shift_degrees(1) > -1,
 cross_180 = [2,cross_180];
end

% determine frequencies that saddle the -180 degree point
if length(cross_180),
 loc1 = cross_180 - 1;
 loc2 = cross_180;

% eliminate ghost crossings
 loc1(abs(shift_degrees(loc1)) > 20) = [];
 loc2(abs(shift_degrees(loc2)) > 20) = [];

 if length(loc1),
  degrees1 = degrees(loc1);
  degrees2 = degrees(loc2);
  w1 = w(loc1);
  w2 = w(loc2);

  dB1      = dB(loc1);
  dB2      = dB(loc2);

% compute slope of line between the two points
  deg_slope = (degrees2 - degrees1)./(log10(w2) - log10(w1));
  mag_slope = (dB2 - dB1)./(log10(w2) - log10(w1));

% compute y-intercept
  deg_int = degrees1 - deg_slope.*log10(w1);
  mag_int = dB1 - mag_slope.*log10(w1);

% use phase line equation to interpolate crossover frequency at -180 degrees
  gc_vec = 10 .^((-180 - deg_int)./deg_slope);

% use magnitude line equation to interpolate gain margin in dB
  gm_vec = -(mag_slope.*log10(gc_vec) + mag_int);

% return smallest gain margin value in arithmetic
  gm_vec = 10 .^(gm_vec/20);
  [min_gm,loc_min] = min(gm_vec);
  gm = min_gm;
  gc = gc_vec(loc_min);

 else

  gc = NaN;
  gm = inf;

 end
else

 gc = NaN;
 gm = inf;

end

%
% DETERMINING PHASE MARGIN
%
% find where the magnitude is greater than 0 dB.  this is done by seeing
% where the sign changes from +1 to -1
sign_dB = sign(dB);
diff_sign = abs([0,diff(sign_dB)]);
cross_0 = find(diff_sign==2);

% determine frequencies that saddle the 0 dB point
if length(cross_0),

 loc1 = cross_0 - 1;
 loc2 = cross_0;

 w1 = w(loc1);
 w2 = w(loc2);

 dB1      = dB(loc1);
 dB2      = dB(loc2);
 degrees1 = degrees(loc1);
 degrees2 = degrees(loc2);

% compute slope of line between the two points
 mag_slope = (dB2 - dB1)./(log10(w2) - log10(w1));
 deg_slope = (degrees2 - degrees1)./(log10(w2) - log10(w1));

% compute y-intercept
 mag_int = dB1 - mag_slope.*log10(w1);
 deg_int = degrees1 - deg_slope.*log10(w1);

% use magnitude line equation to interpolate crossover frequency at 0 dB
 pc_vec = 10 .^((0 - mag_int)./mag_slope);

% use phase line equation to interpolate phase margin
 pm_vec = 180 + (deg_slope.*log10(pc_vec) + deg_int);

% determine smallest phase margin
 [min_pm,loc_min] = min(pm_vec);
 pm = min_pm;
 pc = pc_vec(loc_min);

else

 pc = NaN;
 pm = inf;

end


function z = entropy(x)
% Compute entropy H(x) of a discrete variable x.
% Written by Mo Chen (mochen80@gmail.com).
    n = numel(x);
    x = reshape(x,1,n);
    [u,~,label] = unique(x);
    p = full(mean(sparse(1:n,label,1,n,numel(u),n),1));
    z = -dot(p,log2(p+eps));
endfunction


function out = DN_pleft(y,th)
% DN_pleft  Distance from the mean at which a given proportion of data are
%                   more distant.
%
% Measures the maximum distance from the mean at which a given fixed proportion,
% p, of the time-series data points are further.
% Normalizes by the standard deviation of the time series
% (could generalize to separate positive and negative deviations in future)
% Uses the quantile function from Matlab's Statistics Toolbox
%
%---INPUTS:
% y, the input data vector
% th, the proportion of data further than p from the mean
%           (output p, normalized by standard deviation)

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

if nargin < 2 || isempty(th)
    th = 0.1; % default
end

p = quantile(abs(y-mean(y)),1-th);

% A proportion, th, of the data lie further than p from the mean
out = p/std(y);

end


function op = linop_vec( sz )
%LINOP_VEC Matrix to vector reshape operator
%OP = LINOP_VEC( SZ )
%    Constructs a TFOCS-compatible linear operator that reduces a matrix
%    variable to a vector version using column-major order.
%    This is equivalent to X(:)
%    The transpose operator will reshape a vector into a matrix.
%
%    The input SZ should of the form [M,N] where [M,N] describe the
%    size of the matrix variable.  The ouput vector will be of
%    length M*N.  If SZ is a single entry, then M = N is assumed.
%    For advanced usage with multidimensional arrays,
%    use the more general linop_reshape function instead.
%
%   To do the reverse operation (from vector to matrix),
%   use this function together with linop_adjoint.
%
%   See also linop_reshape

if numel(sz) > 2, error('must supply a 2-entry vector'); end
if numel(sz) == 1, sz = [sz(1),sz(1)]; end

% Switch conventions of the size variable:
sz = { [sz(1),sz(2)], [sz(1)*sz(2),1] };

op = @(x,mode)linop_handles_vec( sz, x, mode );

function y = linop_handles_vec(sz, x, mode )
switch mode,
    case 0, y = sz;
    case 1, y = x(:);
    case 2,
        MN = sz{1}; M = MN(1); N = MN(2);
        y = reshape( x, M, N );

end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function y=wrap(x,sm,lg)

% y=wrap(x,sm,lg)
%
% Wrap the values of x so that they are >=sm and <lg

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

error(nargchk(3,3,nargin));
error(chk_param(x,'x','numeric','real'));
error(chk_param(sm,'sm','scalar','real'));
error(chk_param(sm,'lg','scalar','real'));
if (lg<=sm)
  error('lg must be > sm');
end

y=mod(x-sm,lg-sm)+sm;


function [rhsHx, rhsHy, rhsEz] = MaxwellCurvedRHS2D(cinfo, Hx,Hy,Ez)

% function [rhsHx, rhsHy, rhsEz] = MaxwellCurvedRHS2D(cinfo, Hx,Hy,Ez)
% Purpose  : Evaluate RHS flux in 2D Maxwell TM form

Globals2D;

[rhsHx,rhsHy,rhsEz] = MaxwellRHS2D(Hx, Hy, Ez);

% correct residuals at each curved element
Ncinfo = length(cinfo);
for n=1:Ncinfo

  % for each curved element computed L2 derivatives via cubature
  cur = cinfo(n); k1 = cur.elmt; cDx = cur.Dx; cDy = cur.Dy;

  rhsHx(:,k1) = -cDy*Ez(:,k1);
  rhsHy(:,k1) =  cDx*Ez(:,k1);
  rhsEz(:,k1) =  cDx*Hy(:,k1) - cDy*Hx(:,k1);

  % for each face of each curved element use Gauss quadrature based lifts
  for f1=1:Nfaces
    k2 = EToE(k1,f1);
    gnx = cur.gnx(:,f1); gny = cur.gny(:,f1);
    gVM = cur.gVM(:,:,f1); gVP = cur.gVP(:,:,f1);
    glift = cur.glift(:,:,f1);

    % compute difference of solution traces at Gauss nodes
    gdHx = gVM*Hx(:,k1) - gVP*Hx(:,k2);
    gdHy = gVM*Hy(:,k1) - gVP*Hy(:,k2);
    gdEz = gVM*Ez(:,k1) - gVP*Ez(:,k2);

    % correct jump at Gauss nodes on domain boundary faces
    if(k1==k2)
      gdHx = 0*gdHx; gdHy = 0*gdHy; gdEz = 2*gVM*Ez(:,k1);
    end

    % perform upwinding
    gndotdH =  gnx.*gdHx+gny.*gdHy;
    fluxHx =  gny.*gdEz + gndotdH.*gnx-gdHx;
    fluxHy = -gnx.*gdEz + gndotdH.*gny-gdHy;
    fluxEz = -gnx.*gdHy + gny.*gdHx   -gdEz;

    % lift flux terms using Gauss based lift operator
    rhsHx(:,k1) = rhsHx(:,k1) + glift*fluxHx/2;
    rhsHy(:,k1) = rhsHy(:,k1) + glift*fluxHy/2;
    rhsEz(:,k1) = rhsEz(:,k1) + glift*fluxEz/2;
  end
end
return;


function failed = runtest()
%RUNTEST Test Bayesian Adaptive Direct Search (BADS).
%  RUNTEST executes a few runs of the BADS optimization algorithm to
%  check that it is installed correctly, and returns the number of failed
%  tests.
%
%  See also BADS, BADS_EXAMPLES.

nvars = 3;                              % Number of dimensions
x0 = 4*ones(1,nvars);                  % Initial point
tolerr = [0.1 0.1 1 1];                   % Error tolerance

txt{1} = 'Test with deterministic function (ellipsoid)';
fprintf('%s.\n', txt{1});
fun = @(x) sum((x./(1:numel(x)).^2).^2);     % Objective function
[exitflag(1),err(1)] = testblock(fun,[],[],x0,0,0);

txt{2} = 'Test with deterministic function (sphere) and non-bound constraints';
fprintf('%s.\n', txt{2});
fun = @(x) sum(x.^2,2);
nonbcon = @(x) x(:,1) + x(:,2) < sqrt(2);     % Non-bound constraints
[exitflag(2),err(2)] = testblock(fun,[],nonbcon,x0,1,0);

txt{3} = 'Test with noisy function (noisy sphere)';
fprintf('%s.\n', txt{3});
fun = @(x) sum(x.^2) + randn();             % Noisy objective function
truefun = @(x) sum(x.^2);
[exitflag(3),err(3)] = testblock(fun,truefun,[],x0,0,0);

txt{4} = 'Test with heteroskedastic, specified noisy function (noisy sphere)';
fprintf('%s.\n', txt{4});
fun = @henoisysphere;           % Heteroskedastic noisy objective function
truefun = @(x) sum(x.^2);
[exitflag(4),err(4)] = testblock(fun,truefun,[],x0,0,1);

failed = 0;
fprintf('===========================================================================\n');
for i = 1:4
    fprintf('%s:', txt{i});
    if exitflag(i) >= 0 && err(i) < tolerr(i)
        fprintf('\tPASSED\n');
    else
        fprintf('\tFAILED\n');
        failed = failed + 1;
    end
end
fprintf('===========================================================================\n');
fprintf('\n');

if failed == 0
    display('BADS is working correctly. See bads_examples.m for usage examples; check out the BADS website;');
    display('consult the online FAQ; or digit ''help bads'' for more information. Enjoy!');
else
    display('BADS is not working correctly. Please check the online FAQ for more information.');
end

% %% Noisy sphere (FMINSEARCH)
%
% display('Comparison with FMINSEARCH (noisy sphere). Press any key to continue.')
% fun = @(x) sum(x.^2) + randn();             % Noisy objective function
% options = optimset('Display','iter');
% pause;
% [x,fval,exitflag,output] = fminsearch(fun,x0,options);
% display(['Final value (not-noisy): ' num2str(sum(x.^2),'%.3f') ' (true value: 0.0) with ' num2str(output.funcCount) ' fun evals.']);
%
% %% Noisy sphere (GA)
%
% if exist('ga.m','file')
%     display('Comparison with GA (noisy sphere). Press any key to continue.')
%     fun = @(x) sum(x.^2) + randn();             % Noisy objective function
%     options = gaoptimset('Display','iter','Generations',19);
%     pause;
%     rng(0);
%     [x,fval,exitFlag,output] = ga(fun,nvars,[],[],[],[],LB,UB,[],[],options);
%     display(['Final value (not-noisy): ' num2str(sum(x.^2),'%.3f')  ' (true value: 0.0) with ' num2str(output.funccount) ' fun evals.']);
% end

end

%--------------------------------------------------------------------------
function [exitflag,err] = testblock(fun,truefun,nonbcon,x0,fmin,henoise_flag)

nvars = numel(x0);
LB = -100*ones(1,nvars);                % Lower bound
UB = 100*ones(1,nvars);                 % Upper bound
PLB = -8*ones(1,nvars);                 % Plausible lower bound
PUB = 12*ones(1,nvars);                 % Plausible upper bound

options = bads('defaults');             % Default options
options.Debug = true;

if henoise_flag
    options.SpecifyTargetNoise = true;
    options.MaxFunEvals = 200;
else
    options.MaxFunEvals = 100;
end
[x,fval,exitflag,output] = bads(fun,x0,LB,UB,PLB,PUB,nonbcon,options);

if isempty(truefun)
    display(['Final value: ' num2str(fval,'%.3f') ' (true value: ' num2str(fmin) '), with ' num2str(output.funccount) ' fun evals.']);
    err = abs(fval - fmin);
else
    fval_true = truefun(x);
    display(['Final value (not-noisy): ' num2str(fval_true,'%.3f') ' (true value: ' num2str(fmin) ') with ' num2str(output.funccount) ' fun evals.']);
    err = abs(fval_true - fmin);
end
fprintf('\n');
end

%--------------------------------------------------------------------------
function [y,s] = henoisysphere(x)

y = sum(x.^2);
s = 2 + 1*sqrt(y);
y = y + s*randn();

end


% transform radians to degrees

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

deg=rad*180/pi;


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

%%begin
% In the field of robotics there are many possible ways of representing
% positions and orientations, but the homogeneous transformation is well
% matched to MATLABs powerful tools for matrix manipulation.
%
% Homogeneous transformations describe the relationships between Cartesian
% coordinate frames in terms of translation and orientation.

%  A pure translation of 0.5m in the X direction is represented by

transl(0.5, 0.0, 0.0)

% a rotation of 90degrees about the Y axis by

troty(pi/2)

% and a rotation of -90degrees about the Z axis by

trotz(-pi/2)

%  these may be concatenated by multiplication

t = transl(0.5, 0.0, 0.0) * troty(pi/2) * trotz(-pi/2)

%
% If this transformation represented the origin of a new coordinate frame with respect
% to the world frame origin (0, 0, 0), that new origin would be given by

t * [0 0 0 1]'

% the orientation of the new coordinate frame may be expressed in terms of
% Euler angles

tr2eul(t)

% or roll/pitch/yaw angles

tr2rpy(t)

% It is important to note that tranform multiplication is in general not
% commutative as shown by the following example

trotx(pi/2) * trotz(-pi/8)
trotz(-pi/8) * trotx(pi/2)


function m = medianw( x, w, dim )
% Fast weighted median.
%
% Computes the weighted median of a set of samples.
%  http://en.wikipedia.org/wiki/Weighted_median
% "A weighted median of a sample is the 50% weighted percentile."
% For matrices computes median along each column (or dimension dim).
% If all weights are equal to 1 gives identical results to median.
%
% USAGE
%  m = medianw( x, w, [dim] )
%
% INPUTS
%  x      - vector or array of samples
%  w      - vector or array of weights
%  dim    - dimension along which to compute median
%
% OUTPUTS
%  m      - weighted median value of x
%
% EXAMPLE - simple toy example
%  x=[1 2 3]; w=[1 1 5]; medianw(x,w)
%
% EXAMPLE - comparison to median
%  n=randi(100); m=randi(100);
%  x=rand(n,m); w=ones(n,m);
%  m1=median(x); m2=medianw(x,w);
%  assert(isequal(m1,m2))
%
% See also median
%
% Piotr's Computer Vision Matlab Toolbox      Version 3.24
% Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

if(nargin<3), dim=find(size(x)~=1,1); end
d=dim; nd=ndims(x); n=numel(x);

if( n==1 || size(x,d)==1 )
  m=x;
elseif( length(x)==n )
  [x,o]=sort(x); w=w(o); w=cumsum(w);
  w=w/w(end); [~,j]=min(w<=.5);
  if(j==1 || w(j-1)~=.5), m=x(j);
  else m=(x(j-1)+x(j))/2; end
else
  if(d>1), p=[d 1:d-1 d+1:nd]; x=permute(x,p); w=permute(w,p); end
  [x,o]=sort(x); w=w(o); w=cumsum(w); is={':'}; is=is(ones(1,nd-1));
  w=bsxfun(@rdivide,w,w(end,is{:})); [~,j]=min(w<=.5);
  s=size(x); s=reshape(((1:n/s(1))-1)*s(1),size(j));
  j0=max(1,j-1); j0=j0+s; j=j+s;
  same=w(j0)~=.5; j0(same)=j(same); m=(x(j0)+x(j))/2;
  if(d>1), p=[2:d 1 d+1:nd]; m=permute(m,p); end
end

end


function [x,y,X,Y] = simplex(func, x, kernel, opts, varargin)
%
% SIMPLEX - multidimensional unconstrained non-linear optimsiation
%
%    X = SIMPLEX(FUNC,X) finds a local minumum of a function, via a function
%    handle FUNC, starting from an initial point X.  The local minimum is
%    located via the Nelder-Mead simplex algorithm [1], which does not require
%    any gradient information.
%
%    [X,Y] = SIMPLEX(FUNC,X) also returns the value of the function, Y, at
%    the local minimum, X.
%
%    X = SIMPLEX(FUNC,X,OPTS) allows the optimisation parameters to be
%    specified via a structure, OPTS, with members
%
%       opts.Chi         - Parameter governing expansion steps
%       opts.Delta       - Parameter governing size of initial simplex.
%       opts.Gamma       - Parameter governing contraction steps.
%       opts.Rho         - Parameter governing reflection steps.
%       opts.Sigma       - Parameter governing shrinkage steps.
%       opts.MaxIter     - Maximum number of optimisation steps.
%       opts.MaxFunEvals - Maximum number of function evaluations.
%       opts.TolFun      - Stopping criterion based on the relative change in
%                          value of the function in each step.
%       opts.TolX        - Stopping criterion based on the change in the
%                          minimiser in each step.
%
%    OPTS = SIMPLEX() returns a structure containing the default optimisation
%    parameters, with the following values:
%
%       opts.Chi         = 2
%       opts.Delta       = 0.01
%       opts.Gamma       = 0.5
%       opts.Rho         = 1
%       opts.Sigma       = 0.5
%       opts.MaxIter     = 200
%       opts.MaxFunEvals = 1000
%       opts.TolFun      = 1e-6
%       opts.TolX        = 1e-6
%
%    X = SIMPLEX(FUNC,X,OPTS, P1, P2, ...) allows addinal parameters to be
%    passed to the function to be minimised.
%
%    [X,Y,XX,YY] = SIMPLEX(FUNC, X) also returns in XX all of the values of
%    X evaluated during the optimisation process and in YY the corresponding
%    values of the function.
%
%    References:
%
%       [1] J. A. Nelder and R. Mead, "A simplex method for function
%           minimization", Computer Journal, 7:308-313, 1965.
%
% Description : Simple implementation of the Nelder-Mead simplex optimisation
%               algorithm [1].  Similar to the fminsearch routine from the
%               MATLAB optimisation toolbox.
%
% References  : [1] J. A. Nelder and R. Mead, "A simplex method for function
%                   minimization", Computer Journal, 7:308-313, 1965.
%
% Copyright (c) 2011,  KULeuven-ESAT-SCD, License & help @ http://www.esat.kuleuven.be/sista/lssvmlab

% use default optimisation parameters if none given

if nargin < 4

    opts.Chi         = 2;
    opts.Delta       = 1.2;%0.01;
    opts.Gamma       = 0.5;
    opts.Rho         = 1;
    opts.Sigma       = 0.5;
    opts.MaxIter     = 15;%200;
    opts.MaxFunEvals = 25;
    opts.TolFun      = 1e-6;
    opts.TolX        = 1e-6;

end

% return structure containing default optimisation parameters

if nargin == 0

    x = opts;

    return

end

% get initial parameters

x = x(:);
n = length(x);
x = repmat(x', n+1, 1);
y = zeros(n+1, 1);

% form initial simplex

for i=1:n

    x(i,i) = x(i,i) + opts.Delta;
    y(i)   = func(x(i,:), varargin{:});

end

y(n+1) = func(x(n+1,:), varargin{:});
X      = x;
Y      = y;
count  = n+1;

[~, idx] = min(y);
xx        = x(idx,:);

if strcmp(kernel,'RBF_kernel') || strcmp(kernel,'wav_kernel')|| strcmp(kernel,'RBF4_kernel')
    k = 1;
    format = '  % 4d        % 4d     %e     %.4f        %.4f      %s \n';
    fprintf(1, '\n Iteration   Func-count    min f(x)    log(gamma)    log(sig2)    Procedure\n\n');
    fprintf(1, format, 1, count, min(y),xx(1),xx(2),'initial');
elseif strcmp(kernel,'lin_kernel')
    k = 2;
    format = '  % 4d        % 4d     %e     %.4f           %s \n';
    fprintf(1, '\n Iteration   Func-count    min f(x)    log(gamma)      Procedure\n\n');
    fprintf(1, format, 1, count, min(y),xx(1),'initial');
elseif strcmp(kernel,'poly_kernel')
    k = 3;
    format = '  % 4d        % 4d     %e     %.4f         %.4f         %s   \n';
    fprintf(1, '\n Iteration   Func-count    min f(x)    log(gamma)      log(t)        Procedure\n\n');
    fprintf(1, format, 1, count, min(y),xx(1),xx(2),'initial');
end

% iterative improvement

for i=2:opts.MaxIter

    % order

    [y,idx] = sort(y);
    x       = x(idx,:);

    % reflect

    centroid = mean(x(1:end-1,:));
    x_r      = centroid + opts.Rho*(centroid - x(end,:));
    y_r      = func(x_r, varargin{:});
    count    = count + 1;
    X        = [X ; x_r];
    Y        = [Y ; y_r];

    if y_r >= y(1) && y_r < y(end-1)

        % accept reflection point

        x(end,:) = x_r;
        y(end)   = y_r;
        [~, idx] = min(y);
        xx = x(idx,:);
        if k==1 || k==3
            fprintf(1, format, i, count, min(y),xx(1),xx(2),'reflect');
        elseif k==2
            fprintf(1, format, i, count, min(y),xx(1),'reflect');
        end

    else

        if y_r < y(1)

            % expand

            x_e   = centroid + opts.Chi*(x_r - centroid);
            y_e   = func(x_e, varargin{:});
            count = count + 1;
            X     = [X ; x_e];
            Y     = [Y ; y_e];

            if y_e < y_r

                % accept expansion point

                x(end,:) = x_e;
                y(end)   = y_e;
                [~, idx] = min(y);
                xx = x(idx,:);
                if k==1 || k==3
                   fprintf(1, format, i, count, min(y),xx(1),xx(2), 'expand');
                elseif k==2
                   fprintf(1, format, i, count, min(y),xx(1), 'expand');
                end

            else

                % accept reflection point

                x(end,:) = x_r;
                y(end)   = y_r;
                [~, idx] = min(y);
                xx = x(idx,:);
                if k==1 || k==3
                   fprintf(1, format, i, count, min(y),xx(1),xx(2), 'reflect');
                elseif k==2
                   fprintf(1, format, i, count, min(y),xx(1), 'reflect');
                end

            end

        else

            % contract

            shrink = 0;

            if y(end-1) <= y_r && y_r < y(end)

                % contract outside

                x_c   = centroid + opts.Gamma*(x_r - centroid);
                y_c   = func(x_c, varargin{:});
                count = count + 1;
                X     = [X ; x_c];
                Y     = [Y ; y_c];

                if y_c <= y_r

                    % accept contraction point

                    x(end,:) = x_c;
                    y(end)   = y_c;
                    [~, idx] = min(y);
                    xx = x(idx,:);
                    if k==1 || k==3
                        fprintf(1, format, i, count, min(y),xx(1),xx(2),'contract outside');
                    elseif k==2
                        fprintf(1, format, i, count, min(y),xx(1),'contract outside');
                    end

                else

                    shrink = 1;

                end

            else

                % contract inside

                x_c   = centroid + opts.Gamma*(centroid - x(end,:));
                y_c   = func(x_c, varargin{:});
                count = count + 1;
                X     = [X ; x_c];
                Y     = [Y ; y_c];

                if y_c <= y(end)

                    % accept contraction point

                    x(end,:) = x_c;
                    y(end)   = y_c;
                    [~, idx] = min(y);
                    xx = x(idx,:);
                    if k==1 || k==3
                        fprintf(1, format, i, count, min(y),xx(1),xx(2),'contract inside');
                    elseif k==2
                        fprintf(1, format, i, count, min(y),xx(1),'contract inside');
                    end

                else

                    shrink = 1;

                end

            end

            if shrink

                % shrink

                for j=2:n+1

                    x(j,:) = x(1,:) + opts.Sigma*(x(j,:) - x(1,:));
                    y(j)   = func(x(j,:), varargin{:});
                    count  = count + 1;
                    X      = [X ; x(j,:)];
                    Y      = [Y ; y(j)];

                end
                [~, idx] = min(y);
                xx = x(idx,:);
                if k==1 || k==3
                    fprintf(1, format, i, count, min(y),xx(1),xx(2),'shrink');
                elseif k==2
                    fprintf(1, format, i, count, min(y),xx(1),'shrink');
                end

            end

        end

    end

    % evaluate stopping criterion

    if max(abs(min(x) - max(x))) < opts.TolX

        fprintf(1, 'optimisation terminated sucessfully (TolX criterion)\n');

        break;

    end

    if abs(max(y) - min(y))/max(abs(y))  < opts.TolFun

        fprintf(1, 'optimisation terminated sucessfully (TolFun criterion)\n\n');

        break;

    end
    if count  > opts.MaxFunEvals

        fprintf(1, 'optimisation terminated sucessfully (MaxFunEvals criterion)\n\n');

        break;

    end

end

if i == opts.MaxIter

    fprintf(1, 'Warning : maximim number of iterations exceeded\n\n');

end

% update model structure

[y, idx] = min(y);
x        = x(idx,:);

% bye bye...


function pass = test_chebcoeffs3(pref)
% Test chebcoeffs3 and coeffs3.

if ( nargin == 0)
    pref = chebfunpref;
end
tol = 1000*pref.cheb3Prefs.chebfun3eps;

% Create a rank-(3,3,3) function:
m = 8;
n = 10;
p = 5;
Tm = chebpoly(m);
Tn = chebpoly(n);
Tp = chebpoly(p);
f = outerProd(Tm, Tm, Tm) + outerProd(Tn, Tn, Tn) + outerProd(Tp, Tp, Tp);

% Create expected tensor of coefficients for f:
fCoeffs = chebcoeffs3(f);
coeffsExact = zeros(m+1, n+1, p+1);
coeffsExact(m+1, m+1, m+1) = 1;
coeffsExact(n+1, n+1, n+1) = 1;
coeffsExact(p+1, p+1, p+1) = 1;
pass(1) = norm(fCoeffs(:) - coeffsExact(:)) < tol;

% Check the same with coeffs3 instead of chebcoeffs3:
fCoeffs = coeffs3(f);
coeffsExact = zeros(m+1, n+1, p+1);
coeffsExact(m+1, m+1, m+1) = 1;
coeffsExact(n+1, n+1, n+1) = 1;
coeffsExact(p+1, p+1, p+1) = 1;
pass(2) = norm(fCoeffs(:) - coeffsExact(:)) < tol;

% check the reverse:
C = chebfun3.vals2coeffs(chebpolyval3(f));
pass(3) = norm(C(:) - coeffsExact(:)) < tol;

% Check that it works for multiple outputs also:
[core, cols_coeffs, rows_coeffs, tubes_coeffs] = chebcoeffs3(f);
fCoeffs = chebfun3.txm(chebfun3.txm(chebfun3.txm(core, ...
        cols_coeffs, 1), rows_coeffs, 2), tubes_coeffs, 3);
coeffsExact = zeros(m+1, n+1, p+1);
coeffsExact(m+1, m+1, m+1) = 1;
coeffsExact(n+1, n+1, n+1) = 1;
coeffsExact(p+1, p+1, p+1) = 1;
pass(4) = norm(fCoeffs(:) - coeffsExact(:)) < tol;

% Check that coeffs3 also works for multiple outputs also:
[core, cols_coeffs, rows_coeffs, tubes_coeffs] = coeffs3(f);
fCoeffs = chebfun3.txm(chebfun3.txm(chebfun3.txm(core, ...
        cols_coeffs, 1), rows_coeffs, 2), tubes_coeffs, 3);
coeffsExact = zeros(m+1, n+1, p+1);
coeffsExact(m+1, m+1, m+1) = 1;
coeffsExact(n+1, n+1, n+1) = 1;
coeffsExact(p+1, p+1, p+1) = 1;
pass(5) = norm(fCoeffs(:) - coeffsExact(:)) < tol;

end


% Copyright (C) Daphne Koller, Stanford University, 2012

function F = NormalizeFactorValues( F )

  for i=1:length(F)
    ThisFactor = F(i);
    ThisFactor.val = ThisFactor.val / sum(ThisFactor.val);
    F(i) = ThisFactor;
  end


function [AF,BF]=wpfbtbounds(wt,varargin)
%WPFBTBOUNDS Frame bounds of WPFBT
%   Usage: fcond=wpfbtbounds(wt,L);
%          [A,B]=wpfbtbounds(wt,L);
%          [...]=wpfbtbounds(wt);
%
%   `wpfbtbounds(wt,L)` calculates the ratio $B/A$ of the frame bounds
%   of the wavelet packet filterbank specified by *wt* for a system of length
%   *L*. The ratio is a measure of the stability of the system.
%
%   `wpfbtbounds(wt)` does the same, except *L* is chosen to be the next
%   compatible length bigger than the longest filter from the identical
%   filterbank.
%
%   `[A,B]=wpfbtbounds(...)` returns the lower and upper frame bounds
%   explicitly.
%
%   See |wfbt| for explanation of parameter *wt*.
%
%   Additionally, the function accepts the following flags:
%
%   `'intsqrt'`(default),`'intnoscale'`, `'intscale'`
%       The filters in the filterbank tree are scaled to reflect the
%       behavior of |wpfbt| and |iwpfbt| with the same flags.
%
%   `'scaling_notset'`(default),`'noscale'`,`'scale'`,`'sqrt'`
%     Support for scaling flags as described in |uwpfbt|. By default,
%     the bounds are caltulated for |wpfbt|, passing any of the non-default
%     flags results in bounds for |uwpfbt|.
%
%   See also: wpfbt, filterbankbounds

% AUTHOR: Zdenek Prusa

complainif_notenoughargs(nargin,1,'WPFBTBOUNDS');

definput.keyvals.L = [];
definput.flags.interscaling = {'intsqrt', 'intscale', 'intnoscale'};
definput.import = {'uwfbtcommon'};
definput.importdefaults = {'scaling_notset'};
[flags,~,L]=ltfatarghelper({'L'},definput,varargin);

wt = wfbtinit({'strict',wt},'nat');

if ~isempty(L) && flags.do_scaling_notset
   if L~=wfbtlength(L,wt)
       error(['%s: Specified length L is incompatible with the length of ' ...
              'the time shifts.'],upper(mfilename));
   end;
end

for ii=1:numel(wt.nodes)
   a = wt.nodes{ii}.a;
   assert(all(a==a(1)),sprintf(['%s: One of the basic wavelet ',...
                                'filterbanks is not uniform.'],...
                                upper(mfilename)));
end

% Do the equivalent filterbank using multirate identity property
[gmultid,amultid] = wpfbt2filterbank(wt,flags.interscaling,flags.scaling);

if isempty(L)
   L = wfbtlength(max(cellfun(@(gEl) numel(gEl.h),gmultid)),wt);
end

% Do the equivalent uniform filterbank
if any(amultid~=amultid(1))
   [gu,au] = nonu2ufilterbank(gmultid,amultid);
else
   [gu,au] = deal(gmultid,amultid);
end

if nargout<2
   AF = filterbankbounds(gu,au,L);
elseif nargout == 2
   [AF, BF] = filterbankbounds(gu,au,L);
end


function [myhandle, x_circle, y_circle] = circles(radii,centers,varargin)
% Plot multiple circles as a single line object...very fast, very efficient!
%
% circles(radii,centers) plots circles of radius values specified by RADII,
%     at centers specified by CENTERS.
% circles(radii,centers,'PropertyName',propertyvalue,...)
%         Will accept as inputs any valid properties
%         accepted by PLOT.
% [h,x_circle,y_circle] = circles(...)
%         Also provides handle to (single-plot) circle
%         object, plotted x-values, and plotted y-values.
%
% NOTE: x- and y- values for each circle will be terminated
%       by NaNs in the output x_circle, y_circle vectors!
%
% INPUTS:
%     r        A vector of n radii
%     centers  An n x 2 array of x,y center coordinates
%
%     Optional Parameter-Value Pairs:
%        Resolution: Stepsize for plotting from theta = 0 to
%                    theta = 360 (Default = 1)
%        Any valid PV-pair accepted by PLOT
%
% EXAMPLES
%
%%% Example 1: Plot a single circle:
%
% r = 10; c = [3,5];
% circles(r,c,'color','g','linewidth',2)
%
%
%%% Example 2: Plot 200 random-sized and centered circles
%%% (Note: this is MUCH faster than drawing them one-by-one,
%%% but the speed gain is at the cost of getting a single
%%% line-object handle rather than a vector of them.)
%
% tic;
% n = 1000;
% r = rand(n,1)*100;
% c = rand(n,2)*300;
% [h,xs,ys] = circles(r,c,...
%         'color','r','linewidth',1,'resolution',1);
% axis equal
% t_allAtOnce = toc
%
%
%%% Example 3: (FOR COMPARISON ONLY!!!)
%%% (Using the r,c generated above:)
%
% tic;
% for ii = 1:numel(r)
%   circles(r(ii),c(ii,:),'color','r','linewidth',1,'resolution',1);
% end
% t_oneAtATime = toc
% fprintf('Plotting %d circles was %0.2f x faster as a single object!\n',n,t_oneAtATime/t_allAtOnce)
%
% Written by Brett Shoelson, PhD 8/3/98.
% brett.shoelson@mathworks.com
%
% MODIFICATIONS:
% Vectorized 11/20/01
% Arg for resolution 03/08/08
% 12/06/11 Modified as CIRCLES to accept multiple radii and centers,
%   and to vectorize all as a single line object.
%
% Copyright 2012 The MathWorks, Inc.

if nargin < 2
    error('CIRCLES requires a minimum of 2 input arguments.');
end

if rem(nargin,2) ~= 0
    % Odd number of inputs!
    error('CIRCLES: Parameter-Values must be entered in valid pairs')
end

[PVs,resolution] = parsePVs(varargin);

theta=0:resolution:360;

x_circle = bsxfun(@times,radii,cos(theta*pi/180));
x_circle = bsxfun(@plus,x_circle,centers(:,1));
x_circle = cat(2,x_circle,nan(size(x_circle,1),1));
x_circle =  x_circle';
x_circle = x_circle(:);

y_circle = bsxfun(@times,radii,sin(theta*pi/180));
y_circle = bsxfun(@plus,y_circle,centers(:,2));
y_circle = cat(2,y_circle,nan(size(y_circle,1),1));
y_circle =  y_circle';
y_circle = y_circle(:);

oldhold = ishold;
hold on;
myhandle = plot(x_circle,y_circle);

% Apply PV pairs
for ii = 1:2:numel(PVs)
    set(myhandle,PVs{ii},PVs{ii+1})
end

if ~oldhold,hold off;end
if nargout < 3
    clear y_circle
end
if nargout < 2
    clear x_circle
end
if nargout < 1
    clear myhandle
end

function [PVs,resolution] = parsePVs(PVs)
resolution = 1;
for ii = 1:2:numel(PVs)
    if strcmpi(PVs{ii},'resolution')
        resolution = PVs{ii+1};
        PVs = PVs(setdiff(1:numel(PVs),[ii,ii+1]));
        return
    end
end


function M=convertPF(P,Pp,isProj)
% Canonical case. Converts from P to F
% I should remove that function
%
% Vincent's Structure From Motion Toolbox      Version 1.1
% Copyright (C) 2008-2011 Vincent Rabaud.  [vrabaud-at-cs.ucsd.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the GPL [see external/gpl.txt]

F=Pp; Pp=P;

if isProj
  if isempty(F)
    % Reference: HZ2, p246, Table 9.1
    ep=Pp(:,4);
    M = skew(ep)*Pp(:,1:3);
    return
  else
    % Reference: HZ2, p256, Result 9.14
    [U,S,V] = svd(F); %#ok<NASGU>
    ep = U(:,end);
    M = [ skew(ep)*F ep ];
    return
  end
else    % F has form 14.1, HZ2, p345 :  [ 0 0 a; 0 0 b; c d e ]
  if isempty(F)
    % Reference: HZ2, p348, table 14.1
    M=[ 0 0 Pp(2,3); 0 0 -Pp(1,3); Pp(1,3)*Pp(2,1)-Pp(1,1)*Pp(2,3) ...
      Pp(1,3)*Pp(2,2)-Pp(1,2)*Pp(2,3) Pp(1,3)*Pp(2,4)-Pp(2,3)*P(1,4) ];
    return
  else
    % Reference: HZ2, p348, table 14.1
    M=zeros(3,4); M(3,4)=1;
    M(1,3)=-F(2,3);
    M(2,3)=F(1,3);
    M(1,1)=-(F(3,1)/M(1,3)*M(2,3)/M(1,3)-1)/(1+(M(2,3)/M(1,3))^2);
    M(2,1)=(F(3,1)+M(1,1)*M(2,3))/M(1,3);

    M(1,2)=-((F(3,2)/M(1,3)-1)*M(2,3)/M(1,3))/(1+(M(2,3)/M(1,3))^2);
    M(2,2)=(F(3,2)+M(1,2)*M(2,3))/M(1,3);
    M(1,4)=1; M(2,4)=(F(3,3)+M(2,3)*M(1,4))/M(1,3);
    return
  end
end
error('Bad input');


% demo for Elogsig

gridM = -10:10;
gridV = 0:10:100;
Nmc = 1e4;
for i=1:length(gridM)
    m = gridM(i);
    for j=1:length(gridV)
        V = gridV(j);
        X = m + sqrt(V).*randn(Nmc,1);
        lsx1 = log(VBA_sigmoid(X));
        lsx2 = log(1-VBA_sigmoid(X));
        Els1(i,j) = mean(lsx1);
        Els2(i,j) = mean(lsx2);
        Els01(i,j) = VBA_Elogsig(m,V);
        Els02(i,j) = VBA_Elogsig(-m,V);
    end
end

figure,plot(VBA_vec(exp(Els1)),VBA_vec(exp(Els01)),'.')
figure,plot(VBA_vec(exp(Els2)),VBA_vec(exp(Els02)),'.')


load('data/bal2.mat');
[L,Det]=lmnn(xTr,yTr,'quiet',1);
enerr=energyclassify(L,xTr,yTr,xTe,yTe,3);
knnerrL=knnclassify(L,xTr,yTr,xTe,yTe,3);
knnerrI=knnclassify(eye(size(L)),xTr,yTr,xTe,yTe,3);

clc;
fprintf('Bal data set:\n');
fprintf('3-NN Euclidean training error: %2.2f\n',knnerrI(1)*100);
fprintf('3-NN Euclidean testing error: %2.2f\n',knnerrI(2)*100);
fprintf('3-NN Malhalanobis training error: %2.2f\n',knnerrL(1)*100);
fprintf('3-NN Malhalanobis testing error: %2.2f\n',knnerrL(2)*100);
fprintf('Energy classification error: %2.2f\n',enerr*100);
fprintf('Training time: %2.2fs\n (As a reality check: My desktop needs 20.53s)\n\n',Det.time);


function Spath = Simulate_SLV_func( N_sim, M, T, S_0, r, q, model, params)
% Simulates Paths of Stochastic Local Volatility Models (basic Euler Scheme for most cases)
% N_sim = # paths
% M = #time steps on [0,T], ie dt =T/M
% Note: returns paths of dimension (N_sim,M+1), since they include S_0
%
% SVLModel:                         (parameters)
%        1 = Heston:                alpha, v0, rho, theta, eta
%        2 = SABR:                  alpha, v0, rho, beta
%        3 = Shifted SABR:          alpha, v0, rho, beta, shift
%        4 = Quadratic SLV:         alpha, v0, rho, theta, eta, a, b, c
%        5 = TanHyp-Heston:         alpha, v0, rho, theta, eta, beta
%        6 = Heston-SABR:           alpha, v0, rho, theta, eta, beta
%
%==============================
% Initialize Common Params/Vectors
%==============================
alpha  = params.alpha;   % (vol of vol)
v0     = params.v0;  % initial vol/var
rho    = params.rho; % covar bewteen asset and vol innovations

Sigmav = alpha; %NOTE: alpha is same as Sigmav (vol of vol)

if model == 1 || model == 4 || model == 5 || model == 6
    theta = params.theta;
    eta   = params.eta;
end

Spath = zeros(N_sim,M+1);
Spath(:,1) = S_0;

dt = T/M;
sqdt = sqrt(dt);
sqdtrho1 = sqdt*rho;
sqdtrho2 = sqdt*sqrt(1-rho^2);

Zeta = r - q;
%==============================
% Simulate Based on Specific StochVol Model
%==============================

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if model == 1 %HESTON MODEL

    expEta = exp(-eta*dt);
    driftv = theta*(1 - expEta);  %analytical drift for variance process
    vOld   = v0*ones(N_sim,1); %used to store variance process
    sqvOld = sqrt(v0)*ones(N_sim,1);

    for m = 1:M
        W1 = randn(N_sim,1); W2 = randn(N_sim,1);  %Generate two Brownian motions
        Spath(:,m+1) = Spath(:,m).*exp((Zeta - .5*vOld)*dt + sqdt*sqvOld.*W1);  %log scheme
        %Spath(:,m+1) = Spath(:,m)*( (1+r)*dt + sqrt(vOld)*(sqrho1*W1 + sqrho2*W2));  %level scheme
        vNew = driftv + vOld*expEta + Sigmav*sqvOld.*(sqdtrho1*W1 + sqdtrho2*W2);
        %vNew = (sqvOld +.5*Sigmav*(sqdtrho1*W1 + sqdtrho2*W2)).^2 + eta*(theta - vOld)*dt -.25*Sigmav*dt; %Milstein
        vOld = abs(vNew); %Always stays positive, the "reflection" scheme
        %vOld = max(0,vNew); %least biased scheme
        sqvOld = sqrt(vOld);
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
elseif model == 2  % SABR
    beta  = params.beta;
    cons1 = -.5*(alpha)^2*dt;
    vOld  = v0*ones(N_sim,1); %used to store variance process
    for m = 1:M
        W1 = randn(N_sim,1); W2 = randn(N_sim,1);  %Generate two Brownian motions
        Spath(:,m+1) = max(0, Spath(:,m) + Spath(:,m).^beta.* vOld .*sqdt.*W1);  %level scheme
        vOld = vOld.*exp(cons1 + alpha*(sqdtrho1*W1 + sqdtrho2*W2));
        %vOld = vOld + alpha*vOld.*(sqdtrho1*W1 + sqdtrho2*W2);
    end

%     %%% LOG SCHEME
%     cons2 = 2*(beta - 1);
%     for m = 1:M
%         W1 = randn(N_sim,1); W2 = randn(N_sim,1);  %Generate two Brownian motions
%         Spath(:,m+1) = Spath(:,m).*exp(-.5*vOld.^2.*Spath(:,m).^cons2*dt + Spath(:,m).^(beta-1).* vOld .*sqdt.*W1);  %level scheme
%         vOld = vOld.*exp(cons1 + alpha*(sqdtrho1*W1 + sqdtrho2*W2));
%     end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
elseif model == 3  % SHIFTED SABR
    beta  = params.beta;
    shift = params.shift;
    cons1 = -.5*(alpha)^2*dt;
    vOld  = v0*ones(N_sim,1); %used to store variance process
    for m = 1:M
        W1 = randn(N_sim,1); W2 = randn(N_sim,1);  %Generate two Brownian motions
        %%% Note: because of shift, -shift <= S_t
        Spath(:,m+1) = max(-shift, Spath(:,m) + (Spath(:,m) + shift).^beta.* vOld .*sqdt.*W1);  %level scheme
        vOld = vOld.*exp(cons1 + alpha*(sqdtrho1*W1 + sqdtrho2*W2));
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
elseif model == 4  % QUADRATIC SLV
    a = params.a; b = params.b; c = params.c;

    expEta = exp(-eta*dt);
    driftv = theta*(1 - expEta);  %analytical drift for variance process
    vOld   = v0*ones(N_sim,1); %used to store variance process
    sqvOld = sqrt(v0)*ones(N_sim,1);

    drift = 1+ Zeta*dt;
    for m = 1:M
        W1 = randn(N_sim,1); W2 = randn(N_sim,1);  %Generate two Brownian motions

        Spath(:,m+1) = max(0,Spath(:,m)*drift + sqdt*sqvOld.*W1.*(Spath(:,m).*(a*Spath(:,m) +b) + c));

        %Spath(:,m+1) = Spath(:,m)*( (1+r)*dt + sqrt(vOld)*(sqrho1*W1 + sqrho2*W2));  %level scheme
        vNew = driftv + vOld*expEta + Sigmav*sqvOld.*(sqdtrho1*W1 + sqdtrho2*W2);
        %vNew = (sqvOld +.5*Sigmav*(sqdtrho1*W1 + sqdtrho2*W2)).^2 + eta*(theta - vOld)*dt -.25*Sigmav*dt; %Milstein
        %vOld = abs(vNew); %Always stays positive, the "reflection" scheme
        vOld = max(0,vNew); %least biased scheme
        sqvOld = sqrt(vOld);
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
elseif model == 5  % TAN-HYP-HESTON
    beta = params.beta;

    expEta = exp(-eta*dt);
    driftv = theta*(1 - expEta);  %analytical drift for variance process
    vOld   = v0*ones(N_sim,1); %used to store variance process
    sqvOld = sqrt(v0)*ones(N_sim,1);

    drift = 1+ Zeta*dt;
    for m = 1:M
        W1 = randn(N_sim,1); W2 = randn(N_sim,1);  %Generate two Brownian motions

        Spath(:,m+1) = max(0,Spath(:,m)*drift + sqdt*sqvOld.*W1.*tanh(beta*Spath(:,m)));
        vNew = driftv + vOld*expEta + Sigmav*sqvOld.*(sqdtrho1*W1 + sqdtrho2*W2);
        %vNew = (sqvOld +.5*Sigmav*(sqdtrho1*W1 + sqdtrho2*W2)).^2 + eta*(theta - vOld)*dt -.25*Sigmav*dt; %Milstein
        %vOld = abs(vNew); %Always stays positive, the "reflection" scheme
        vOld = max(0,vNew); %least biased scheme
        sqvOld = sqrt(vOld);
    end

elseif model == 6   % HESTON-SABR
    drift = 1+ Zeta*dt;
    expEta = exp(-eta*dt);
    driftv = theta*(1 - expEta);  %analytical drift for variance process
    vOld   = v0*ones(N_sim,1); %used to store variance process
    sqvOld = sqrt(v0)*ones(N_sim,1);

    beta  = params.beta;

    for m = 1:M
        W1 = randn(N_sim,1); W2 = randn(N_sim,1);  %Generate two Brownian motions

        %%%% LEVEL scheme
        Spath(:,m+1) = max(0, Spath(:,m)*drift + Spath(:,m).^beta.* sqvOld .*sqdt.*W1);

%         %%%% LOG scheme
%         Spath(:,m+1) = Spath(:,m).*exp(-.5*vOld.^2.*Spath(:,m).^cons2*dt + Spath(:,m).^(beta-1).* sqvOld .*sqdt.*W1);

        vNew = driftv + vOld*expEta + Sigmav*sqvOld.*(sqdtrho1*W1 + sqdtrho2*W2);
        vOld = abs(vNew); %Always stays positive, the "reflection" scheme
        %vOld = max(0,vNew); %least biased scheme
        sqvOld = sqrt(vOld);
    end

%     beta  = params.beta;
%     cons1 = -.5*(alpha)^2*dt;
%     vOld  = v0*ones(N_sim,1); %used to store variance process
%     for m = 1:M
%         W1 = randn(N_sim,1); W2 = randn(N_sim,1);  %Generate two Brownian motions
%         Spath(:,m+1) = max(0, Spath(:,m) + Spath(:,m).^beta.* vOld .*sqdt.*W1);  %level scheme
%         vOld = vOld.*exp(cons1 + alpha*(sqdtrho1*W1 + sqdtrho2*W2));
%         %vOld = vOld + alpha*vOld.*(sqdtrho1*W1 + sqdtrho2*W2);
%     end
%
%     expEta = exp(-eta*dt);
%     driftv = theta*(1 - expEta);  %analytical drift for variance process
%     vOld   = v0*ones(N_sim,1); %used to store variance process
%     sqvOld = sqrt(v0)*ones(N_sim,1);
%
%     for m = 1:M
%         W1 = randn(N_sim,1); W2 = randn(N_sim,1);  %Generate two Brownian motions
%         Spath(:,m+1) = Spath(:,m).*exp((Zeta - .5*vOld)*dt + sqdt*sqvOld.*W1);  %log scheme
%         %Spath(:,m+1) = Spath(:,m)*( (1+r)*dt + sqrt(vOld)*(sqrho1*W1 + sqrho2*W2));  %level scheme
%         vNew = driftv + vOld*expEta + Sigmav*sqvOld.*(sqdtrho1*W1 + sqdtrho2*W2);
%         %vNew = (sqvOld +.5*Sigmav*(sqdtrho1*W1 + sqdtrho2*W2)).^2 + eta*(theta - vOld)*dt -.25*Sigmav*dt; %Milstein
%         vOld = abs(vNew); %Always stays positive, the "reflection" scheme
%         %vOld = max(0,vNew); %least biased scheme
%         sqvOld = sqrt(vOld);
%     end

end

% %%%% REFORMULATED VERSION
% etahat = eta*theta; thetahat = (eta + Sigmav^2)/etahat; Sigmavhat = -Sigmav; v0hat = 1/v0;
% vOld = v0hat*ones(N_sim,1); %used to store variance process
% sqvOld = sqrt(v0hat)*ones(N_sim,1);
%
% expkt = exp(-etahat*dt);
% driftv = thetahat*(1 - expkt);  %analytical drift for variance process
% sqtSv = sqdt*Sigmavhat;  %analytical drift for variance process
%
% for m = 1:M
%     W1 = randn(N_sim,1); W2 = randn(N_sim,1);  %Generate two Brownian motions
%
%     Spath(:,m+1) = Spath(:,m).*exp((r - .5*vOld)*dt + (sqrho1*W1 + sqrho2*W2)./sqvOld);  %log scheme
%     %Spath(:,m+1) = Spath(:,m)*( (1+r)*dt + sqrt(vOld)*(sqrho1*W1 + sqrho2*W2));  %level scheme
%
%     vNew = driftv + vOld*expkt + sqtSv*sqvOld.*W1;
%
%     vOld = abs(vNew); %Always stays positive, the "reflection" scheme
%     sqvOld = sqrt(vOld);
% end


function M = getmassmat3(elem2dof,volume,Dlambda,elemType,K)
%% GETMASSMAT Get mass matrix of the finite element space
% M = getmassmat(Dlambda,elem2dof,elemType,K) get mass matrix of the finite element
% space specified by elemType.
%
% The elemType can be:
%
% - "RT0": The lowest order Raviart-Thomas element
% - "ND1': The lowest order Nedelec element
%

if ~exist('K','var'), K = []; end
NT = size(elem2dof,1);

%% ND1: the lowest order edge element
if strcmp(elemType,'ND1')
    NE = double(max(elem2dof(:)));
    DiDj = zeros(NT,4,4);
    for i = 1:4
        for j = i:4
            DiDj(:,i,j) = dot(Dlambda(:,:,i),Dlambda(:,:,j),2);
            DiDj(:,j,i) = DiDj(:,i,j);
        end
    end
    locEdge = [1 2; 1 3; 1 4; 2 3; 2 4; 3 4];
    ii = zeros(21*NT,1); jj = zeros(21*NT,1); sM = zeros(21*NT,1);
    index = 0;
    for i = 1:6
        for j = i:6
            ii(index+1:index+NT) = double(elem2dof(:,i));
            jj(index+1:index+NT) = double(elem2dof(:,j));
            % mass matrix
            % locEdge = [1 2; 1 3; 1 4; 2 3; 2 4; 3 4];
            i1 = locEdge(i,1); i2 = locEdge(i,2);
            j1 = locEdge(j,1); j2 = locEdge(j,2);
            Mij = 1/20*volume.*( (1+(i1==j1))*DiDj(:,i2,j2) ...
                               - (1+(i1==j2))*DiDj(:,i2,j1) ...
                               - (1+(i2==j1))*DiDj(:,i1,j2) ...
                               + (1+(i2==j2))*DiDj(:,i1,j1));
            sM(index+1:index+NT) = Mij;
            index = index + NT;
        end
    end
    diagIdx = (ii == jj);   upperIdx = ~diagIdx;
    M = sparse(ii(diagIdx),jj(diagIdx),sM(diagIdx),NE,NE);
    MU = sparse(ii(upperIdx),jj(upperIdx),sM(upperIdx),NE,NE);
    M = M + MU + MU';
end

%% RT0: the lowest order face element
if strcmp(elemType,'RT0')
    NF = double(max(elem2dof(:)));
    localFace = [2 3 4; 1 3 4; 1 2 4; 1 2 3]; % normal vector is not oriented
    M = sparse(NF,NF);
    for i = 1:4
        for j = i:4
            % local to global index map
            ii = double(elem2dof(:,i));
            jj = double(elem2dof(:,j));
            i1 = localFace(i,1); i2 = localFace(i,2); i3 = localFace(i,3);
            j1 = localFace(j,1); j2 = localFace(j,2); j3 = localFace(j,3);
            % computation of mass matrix --- (phi_i, phi_j)
            Mij = 1/20*volume*4.*( ...
                  (1+(i1==j1))*dot(mycross(Dlambda(:,:,i2),Dlambda(:,:,i3),2), ...
                                   mycross(Dlambda(:,:,j2),Dlambda(:,:,j3),2),2)...
                 +(1+(i1==j2))*dot(mycross(Dlambda(:,:,i2),Dlambda(:,:,i3),2), ...
                                   mycross(Dlambda(:,:,j3),Dlambda(:,:,j1),2),2)...
                 +(1+(i1==j3))*dot(mycross(Dlambda(:,:,i2),Dlambda(:,:,i3),2), ...
                                   mycross(Dlambda(:,:,j1),Dlambda(:,:,j2),2),2)...
                 +(1+(i2==j1))*dot(mycross(Dlambda(:,:,i3),Dlambda(:,:,i1),2), ...
                                   mycross(Dlambda(:,:,j2),Dlambda(:,:,j3),2),2)...
                 +(1+(i2==j2))*dot(mycross(Dlambda(:,:,i3),Dlambda(:,:,i1),2), ...
                                   mycross(Dlambda(:,:,j3),Dlambda(:,:,j1),2),2)...
                 +(1+(i2==j3))*dot(mycross(Dlambda(:,:,i3),Dlambda(:,:,i1),2), ...
                                   mycross(Dlambda(:,:,j1),Dlambda(:,:,j2),2),2)...
                 +(1+(i3==j1))*dot(mycross(Dlambda(:,:,i1),Dlambda(:,:,i2),2), ...
                                   mycross(Dlambda(:,:,j2),Dlambda(:,:,j3),2),2)...
                 +(1+(i3==j2))*dot(mycross(Dlambda(:,:,i1),Dlambda(:,:,i2),2), ...
                                   mycross(Dlambda(:,:,j3),Dlambda(:,:,j1),2),2)...
                 +(1+(i3==j3))*dot(mycross(Dlambda(:,:,i1),Dlambda(:,:,i2),2), ...
                                   mycross(Dlambda(:,:,j1),Dlambda(:,:,j2),2),2));
            if ~isempty(K)
                Mij = Mij./K;
            end
            if (j==i)
                M = M + sparse(ii,jj,Mij,NF,NF);
            else
                M = M + sparse([ii;jj],[jj;ii],[Mij; Mij],NF,NF);
            end
        end
    end
end


%HOUGHCIRCLES  Finds circles in a grayscale image using the Hough transform
%
%     circles = cv.HoughCircles(image)
%     circles = cv.HoughCircles(..., 'OptionName', optionValue, ...)
%
% ## Input
% * __image__ 8-bit, single-channel, grayscale input image.
%
% ## Output
% * __circles__ Output vector of found circles. A cell-array of 3-element
%   floating-point vectors `{[x, y, radius], ...}`.
%
% ## Options
% * __Method__ Detection method. Currently, the only implemented method is
%   'Gradient' (default). One of:
%   * __Standard__ classical or standard Hough transform. Every line is
%     represented by two floating-point numbers `(rho,theta)`, where `rho` is
%     a distance between `(0,0)` point and the line, and `theta` is the angle
%     between x-axis and the normal to the line. Thus, the matrix must be (the
%     created sequence will be) of `single` type with 2-channels.
%   * __Probabilistic__ probabilistic Hough transform (more efficient in case
%     if the picture contains a few long linear segments). It returns line
%     segments rather than the whole line. Each segment is represented by
%     starting and ending points, and the matrix must be (the created sequence
%     will be) of the `int32` type with 4-channels.
%   * __MultiScale__ multi-scale variant of the classical Hough transform. The
%     lines are encoded the same way as 'Standard'.
%   * __Gradient__ basically 21HT, described in [Yuen90].
% * __DP__ Inverse ratio of the accumulator resolution to the image resolution.
%   For example, if `DP=1`, the accumulator has the same resolution as the
%   input image. If `DP=2`, the accumulator has half as big width and height.
%   default 1.
% * __MinDist__ Minimum distance between the centers of the detected circles.
%   If the parameter is too small, multiple neighbor circles may be falsely
%   detected in addition to a true one. If it is too large, some circles may
%   be missed. default `size(image,1)/8`.
% * __Param1__ First method-specific parameter. In case of 'Gradient', it is
%   the higher threshold of the two passed to the cv.Canny edge detector
%   (the lower one is twice smaller). default 100.
% * __Param2__ Second method-specific parameter. In case of 'Gradient', it is
%   the accumulator threshold for the circle centers at the detection stage.
%   The smaller it is, the more false circles may be detected. Circles,
%   corresponding to the larger accumulator values, will be returned first.
%   default 100.
% * __MinRadius__ Minimum circle radius. default 0.
% * __MaxRadius__ Maximum circle radius. If <= 0, uses the maximum image
%   dimension. If < 0, returns centers without finding the radius. default 0.
%
% The function finds circles in a grayscale image using a modification of the
% Hough transform.
%
% ### Note
% Usually the function detects the centers of circles well. However, it may
% fail to find correct radii. You can assist to the function by specifying the
% radius range (`MinRadius` and `MaxRadius`) if you know it. Or, you may set
% `MaxRadius` to a negative number to return centers only without radius
% search, and find the correct radius using an additional procedure.
%
% ## References
% [Yuen90]:
% > HK Yuen, John Princen, John Illingworth, and Josef Kittler.
% > "Comparative study of hough transform methods for circle finding".
% > Image and Vision Computing, 8(1):71-77, 1990.
%
% See also: cv.fitEllipse, cv.minEnclosingCircle, imfindcircles
%