function mix = mygmminit(x,centres,idx,covartype)
%GMMINIT Initialises Gaussian mixture model from data
%
[ndata, xdim] = size(x);
k = rows(centres);
mix = gmm(xdim,k,covartype);
I = eye(k);
post = I(idx,:);
mix.centres = centres;

% Arbitrary width used if variance collapses to zero: make it 'large' so
% that centre is responsible for a reasonable number of points.
GMM_WIDTH = 1.0;

% Set priors depending on number of points in each cluster
cluster_sizes = max(sum(post, 1), 1);  % Make sure that no prior is zero
mix.priors = cluster_sizes/sum(cluster_sizes); % Normalise priors

switch mix.covar_type
case 'spherical'
   if mix.ncentres > 1
      % Determine widths as distance to nearest centre
      % (or a constant if this is zero)
      for i = 1:mix.ncentres
        mix.covars(i) = mean(dist2(x(idx==i,:),mix.centres(i,:)));
      end
   else
      % Just use variance of all data points averaged over all
      % dimensions
      mix.covars = mean(diag(cov(x)));
   end
  case 'diag'
    for j = 1:mix.ncentres
      % Pick out data points belonging to this centre
      c = x(find(post(:, j)),:);
      diffs = c - (ones(size(c, 1), 1) * mix.centres(j, :));
      mix.covars(j, :) = sum((diffs.*diffs), 1)/size(c, 1);
      % Replace small entries by GMM_WIDTH value
      mix.covars(j, :) = mix.covars(j, :) + GMM_WIDTH.*(mix.covars(j, :)<eps);
    end
  case 'full'
    for j = 1:mix.ncentres
      % Pick out data points belonging to this centre
      c = x(find(post(:, j)),:);
      diffs = c - (ones(size(c, 1), 1) * mix.centres(j, :));
      mix.covars(:,:,j) = (diffs'*diffs)/(size(c, 1));
      % Add GMM_WIDTH*Identity to rank-deficient covariance matrices
      if rank(mix.covars(:,:,j)) < mix.nin
	mix.covars(:,:,j) = mix.covars(:,:,j) + GMM_WIDTH.*eye(mix.nin);
      end
    end
  case 'ppca'
    for j = 1:mix.ncentres
      % Pick out data points belonging to this centre
      c = x(find(post(:,j)),:);
      diffs = c - (ones(size(c, 1), 1) * mix.centres(j, :));
      [tempcovars, tempU, templambda] = ...
	ppca((diffs'*diffs)/size(c, 1), mix.ppca_dim);
      if length(templambda) ~= mix.ppca_dim
	error('Unable to extract enough components');
      else
        mix.covars(j) = tempcovars;
        mix.U(:, :, j) = tempU;
        mix.lambda(j, :) = templambda;
      end
    end
  otherwise
    error(['Unknown covariance type ', mix.covar_type]);
end


function linpack_s_test05 ( )

%*****************************************************************************80
%
%% TEST05 tests SGBFA and SGBSL.
%
%  Discussion:
%
%    The problem solved here is a larger version of this one:
%
%    Matrix A is ( 2 -1  0  0  0)    right hand side B is  (1)
%                (-1  2 -1  0  0)                          (0)
%                ( 0 -1  2 -1  0)                          (0)
%                ( 0  0 -1  2 -1)                          (0)
%                ( 0  0  0 -1  2)                          (1)
%
%
%    Solution is   (1)
%                  (1)
%                  (1)
%                  (1)
%                  (1)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 June 2009
%
%  Author:
%
%    John Burkardt
%
%  Local Parameters:
%
%    N is the number of equations.
%
%    ML is the number of subdiagonals,
%    MU the number of superdiagonals.
%
%    LDA is the leading dimension of the array used to store the
%    matrix, which must be at least 2*ML+MU+1.
%
  n = 10;
  ml = 1;
  mu = 1;
  lda = 2*ml+mu+1;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST05\n' );
  fprintf ( 1, '  For a general banded matrix,\n' );
  fprintf ( 1, '  SGBFA factors the matrix,\n' );
  fprintf ( 1, '  SGBSL solves a factored linear system.\n' );
  fprintf ( 1, '  The matrix size is N = %d\n', n );
%
%  Set the right hand side B.
%
  b(1) = 1.0;
  b(2:n-1) = 0.0;
  b(n) = 1.0;
%
%  Force B to be a column vector.
%
  b = b';
%
%  Set the matrix A.
%
  m = ml + mu + 1;
  fprintf ( 1, '  The bandwidth of the matrix is %d\n', m );

  for j = 1 : n
    a(m-1,j) = -1.0;
    a(m,j) = 2.0;
    a(m+1,j) = -1.0;
  end
%
%  Call SGBFA to factor the matrix A.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Factor the matrix.\n' );

  [ a, ipivot, info ] = sgbfa ( a, lda, n, ml, mu );

  if ( info ~= 0 )
    fprintf ( 1, '  Error!  SGBFA returns INFO = %d\n', info );
    return
  end
%
%  Call SGBSL to solve the linear system.  The solution
%  is returned in B, that is, it overwrites the right hand side.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Solve the linear system.\n' );

  job = 0;
  b = sgbsl ( a, lda, n, ml, mu, ipivot, b, job );
%
%  Print the results.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The first and last 5 entries of the solution:\n' );
  fprintf ( 1, '  (All should be 1):\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    if ( i <= 5 | n-5 < i )
      fprintf ( 1, '  %6d  %14f\n', i, b(i) );
    end
    if ( i == 5 )
      fprintf ( 1, '  ......  ..............\n' );
    end
  end

  return
end


function [wts,cfreqs] = gammatone_matrix(nfft, sr, nfilts, width, minfreq, maxfreq, maxlen)
% [wts,cfreqa] = fft2gammatonemx(nfft, sr, nfilts, width, minfreq, maxfreq, maxlen)
%      Generate a matrix of weights to combine FFT bins into
%      Gammatone bins.  nfft defines the source FFT size at
%      sampling rate sr.  Optional nfilts specifies the number of
%      output bands required (default 64), and width is the
%      constant width of each band in Bark (default 1).
%      minfreq, maxfreq specify range covered in Hz (100, sr/2).
%      While wts has nfft columns, the second half are all zero.
%      Hence, aud spectrum is
%      fft2gammatonemx(nfft,sr)*abs(fft(xincols,nfft));
%      maxlen truncates the rows to this many bins.
%      cfreqs returns the actual center frequencies of each
%      gammatone band in Hz.
%
% 2004-09-05  Dan Ellis dpwe@ee.columbia.edu  based on rastamat/audspec.m
% Last updated: $Date: 2009/02/22 02:29:25 $

if nargin < 2;    sr = 16000; end
if nargin < 3;    nfilts = 64; end
if nargin < 4;    width = 0.5; end
if nargin < 5;    minfreq = 50; end
if nargin < 6;    maxfreq = sr/2; end
if nargin < 7;    maxlen = nfft/2; end

wts = zeros(nfilts, nfft);

% after Slaney's MakeERBFilters
EarQ = 9.26449;
minBW = 24.7;
order = 1;

cfreqs = -(EarQ*minBW) + exp((1:nfilts)'*(-log(maxfreq + EarQ*minBW) + ...
                log(minfreq + EarQ*minBW))/nfilts) * (maxfreq + EarQ*minBW);
cfreqs = flipud(cfreqs);

GTord = 4;

ucirc = exp(j*2*pi*[0:(nfft/2)]/nfft);

justpoles = 0;

for i = 1:nfilts
  cf = cfreqs(i);
  ERB = width*((cf/EarQ).^order + minBW^order).^(1/order);
  B = 1.019*2*pi*ERB;
  r = exp(-B/sr);
  theta = 2*pi*cf/sr;
  pole = r*exp(j*theta);

  if justpoles == 1
    % point on unit circle of maximum gain, from differentiating magnitude
    cosomegamax = (1+r*r)/(2*r)*cos(theta);
    if abs(cosomegamax) > 1
      if theta < pi/2;  omegamax = 0;
      else              omegamax = pi;   end
    else
      omegamax = acos(cosomegamax);
    end
    center = exp(j*omegamax);
    gain = abs((pole-center).*(pole'-center)).^GTord;
    wts(i,1:(nfft/2+1)) = gain * (abs((pole-ucirc).*(pole'- ...
                                                     ucirc)).^-GTord);
  else
    % poles and zeros, following Malcolm's MakeERBFilter
    T = 1/sr;
    A11 = -(2*T*cos(2*cf*pi*T)./exp(B*T) + 2*sqrt(3+2^1.5)*T*sin(2* ...
                                                      cf*pi*T)./exp(B*T))/2;
    A12 = -(2*T*cos(2*cf*pi*T)./exp(B*T) - 2*sqrt(3+2^1.5)*T*sin(2* ...
                                                      cf*pi*T)./exp(B*T))/2;
    A13 = -(2*T*cos(2*cf*pi*T)./exp(B*T) + 2*sqrt(3-2^1.5)*T*sin(2* ...
                                                      cf*pi*T)./exp(B*T))/2;
    A14 = -(2*T*cos(2*cf*pi*T)./exp(B*T) - 2*sqrt(3-2^1.5)*T*sin(2* ...
                                                      cf*pi*T)./exp(B*T))/2;
    zros = -[A11 A12 A13 A14]/T;

    gain(i) =  abs((-2*exp(4*j*cf*pi*T)*T + ...
                2*exp(-(B*T) + 2*j*cf*pi*T).*T.* ...
                (cos(2*cf*pi*T) - sqrt(3 - 2^(3/2))* ...
                 sin(2*cf*pi*T))) .* ...
               (-2*exp(4*j*cf*pi*T)*T + ...
                2*exp(-(B*T) + 2*j*cf*pi*T).*T.* ...
                (cos(2*cf*pi*T) + sqrt(3 - 2^(3/2)) * ...
                 sin(2*cf*pi*T))).* ...
               (-2*exp(4*j*cf*pi*T)*T + ...
                2*exp(-(B*T) + 2*j*cf*pi*T).*T.* ...
                (cos(2*cf*pi*T) - ...
                 sqrt(3 + 2^(3/2))*sin(2*cf*pi*T))) .* ...
               (-2*exp(4*j*cf*pi*T)*T + 2*exp(-(B*T) + 2*j*cf*pi*T).*T.* ...
                (cos(2*cf*pi*T) + sqrt(3 + 2^(3/2))*sin(2*cf*pi*T))) ./ ...
               (-2 ./ exp(2*B*T) - 2*exp(4*j*cf*pi*T) +  ...
                2*(1 + exp(4*j*cf*pi*T))./exp(B*T)).^4);
    wts(i,1:(nfft/2+1)) = ((T^4)/gain(i)) ...
        * abs(ucirc-zros(1)).*abs(ucirc-zros(2))...
        .*abs(ucirc-zros(3)).*abs(ucirc-zros(4))...
        .*(abs((pole-ucirc).*(pole'-ucirc)).^-GTord);
  end
end

wts = wts(:,1:maxlen);


%% Potts-denoising, Gaussian noise

%%
% Load signal
original = loadPcwConst('sample1');
% Add Gaussian noise with parameter sigma = 0.1
f = original + 0.1 * randn(size(original));

%%
% Compute $L^2$ Potts estimator
figure(1)
pottsL2 = minL2Potts(f, 0.1);
showPotts(f, pottsL2, original, 'L2-Potts')

%%
% Compute $L^1$ Potts estimator
figure(2)
pottsL1 = minL1Potts(f, 0.4);
showPotts(f, pottsL1, original, 'L1-Potts')


% EX_PLANE_STRAIN_RING: solve the plane-strain problem on one quarter of a cylinder.

% 1) PHYSICAL DATA OF THE PROBLEM
clear problem_data
% Physical domain, defined as NURBS map given in a text file
problem_data.geo_name = 'geo_ring.txt';

% Type of boundary conditions
problem_data.nmnn_sides   = [2];
problem_data.drchlt_sides = [];
problem_data.press_sides  = [1];
problem_data.symm_sides   = [3 4];

% Physical parameters
E  =  1; nu = 0;
problem_data.lambda_lame = @(x, y) ((nu*E)/((1+nu)*(1-2*nu)) * ones (size (x)));
problem_data.mu_lame = @(x, y) (E/(2*(1+nu)) * ones (size (x)));

% Source and boundary terms
P = 1;
problem_data.f = @(x, y) zeros (2, size (x, 1), size (x, 2));
problem_data.g = @(x, y, ind) test_plane_strain_ring_g_nmnn (x, y, P, nu, ind);
problem_data.h = @(x, y, ind) test_plane_strain_ring_uex (x, y, E, nu, P);
problem_data.p = @(x, y, ind) P * ones (size (x));

% Exact solution (optional)
problem_data.uex = @(x, y) test_plane_strain_ring_uex (x, y, E, nu, P);

% 2) CHOICE OF THE DISCRETIZATION PARAMETERS
clear method_data
method_data.degree     = [3 3];     % Degree of the basis functions
method_data.regularity = [2 2];     % Regularity of the basis functions
method_data.nsub       = [9 9];     % Number of subdivisions
method_data.nquad      = [4 4];     % Points for the Gaussian quadrature rule

% 3) CALL TO THE SOLVER
[geometry, msh, space, u] = solve_linear_elasticity (problem_data, method_data);

% 4) POST-PROCESSING.
% 4.1) Export to Paraview
output_file = 'plane_strain_ring_Deg3_Reg2_Sub9';

vtk_pts = {linspace(0, 1, 21), linspace(0, 1, 21)};
fprintf ('results being saved in: %s \n \n', output_file)
sp_to_vtk (u, space, geometry, vtk_pts, output_file, {'displacement', 'stress'}, {'value', 'stress'}, ...
    problem_data.lambda_lame, problem_data.mu_lame)

% 4.2) Plot in Matlab. Comparison with the exact solution.
[eu, F] = sp_eval (u, space, geometry, vtk_pts);
[X, Y]  = deal (squeeze(F(1,:,:)), squeeze(F(2,:,:)));

subplot (1,2,1)
quiver (X, Y, squeeze(eu(1,:,:)), squeeze(eu(2,:,:)))
title ('Numerical solution'), axis equal tight
subplot (1,2,2)
eu2 = problem_data.uex (X, Y);
quiver (X, Y, squeeze(eu2(1,:,:)), squeeze(eu2(2,:,:)))
title ('Exact solution'), axis equal tight

error_l2 = sp_l2_error (space, msh, u, problem_data.uex)

%!demo
%! ex_plane_strain_ring

%!test
%! problem_data.geo_name = 'geo_ring.txt';
%! problem_data.nmnn_sides   = [2];
%! problem_data.drchlt_sides = [];
%! problem_data.press_sides  = [1];
%! problem_data.symm_sides   = [3 4];
%! E  =  1; nu = 0;
%! problem_data.lambda_lame = @(x, y) ((nu*E)/((1+nu)*(1-2*nu)) * ones (size (x)));
%! problem_data.mu_lame = @(x, y) (E/(2*(1+nu)) * ones (size (x)));
%! P = 1;
%! problem_data.f = @(x, y) zeros (2, size (x, 1), size (x, 2));
%! problem_data.g = @(x, y, ind) test_plane_strain_ring_g_nmnn (x, y, P, nu, ind);
%! problem_data.h = @(x, y, ind) test_plane_strain_ring_uex (x, y, E, nu, P);
%! problem_data.p = @(x, y, ind) P * ones (size (x));
%! problem_data.uex = @(x, y) test_plane_strain_ring_uex (x, y, E, nu, P);
%! method_data.degree     = [3 3];     % Degree of the basis functions
%! method_data.regularity = [2 2];     % Regularity of the basis functions
%! method_data.nsub       = [9 9];     % Number of subdivisions
%! method_data.nquad      = [4 4];     % Points for the Gaussian quadrature rule
%! [geometry, msh, space, u] = solve_linear_elasticity (problem_data, method_data);
%! error_l2 = sp_l2_error (space, msh, u, problem_data.uex);
%! assert (msh.nel, 81)
%! assert (space.ndof, 288)
%! assert (error_l2, 1.53640513551293e-06, 1e-16)


%%
% Test for W1 on graph using linprog.

addpath('../toolbox/');
rep = 'results/w1-graph/';
[~,~] = mkdir(rep);

% helpers
normalize = @(x)x/sum(x(:));

% generate a planar graph
N = 80;
X = randn(2,N);
X(2,:) = X(2,:)*.75;
T = delaunay(X(1,:),X(2,:));

I = [T(:,1);T(:,2);T(:,2);T(:,3);T(:,3);T(:,1)];
J = [T(:,2);T(:,1);T(:,3);T(:,2);T(:,1);T(:,3)];
[I,J] = deal(I(I<J), J(I<J));
P = length(I);

% gradient operator
grad = sparse( [1:P 1:P], [I;J]', [ones(P,1) -ones(P,1)], P,N);

clf;
triplot(T,X(1,:),X(2,:));

% edge length
w = sqrt( (X(1,I)-X(1,J)).^2 + (X(2,I)-X(2,J)).^2 );
w = w(:);

% source/sink
[~,s] = min(X(1,:));
d = sqrt( (X(1,s)-X(1,:)).^2 + (X(2,s)-X(2,:)).^2 );
[~,t] = max(d);

% target measures
test = 'diracs';
test = 'spread'; Q = 14; % Q-NN

switch test
    case 'diracs'
        mu = zeros(N,1); mu(s)=1;
        nu = zeros(N,1); nu(t)=1;
    case 'spread'
        v = [s t]; Mu = {};
        for i=1:2
            % distance to target
            x = X(:,v(i));
            d = sqrt( (x(1)-X(1,:)).^2 + (x(2)-X(2,:)).^2 );
            [d,Is] = sort(d, 'ascend'); d = d(1:Q); Is = Is(1:Q);
            Mu{i} = zeros(N,1);
            Mu{i}(Is) = normalize(sqrt( 1-d/d(end) ));
        end
        mu = Mu{1}; nu = Mu{2};
end

%%
% display the measures.

clf; hold on;
triplot(T,X(1,:),X(2,:), 'k');
% vertex according to mass
for i=1:N
    plot(X(1,i),X(2,i), '.', 'MarkerSize', .01 + 50*mu(i)/max(mu), 'color', 'r');
    plot(X(1,i),X(2,i), '.', 'MarkerSize', .01 + 50*nu(i)/max(mu), 'color', 'b');
end
axis equal; axis off;
saveas(gcf, [rep 'inputs.png'], 'png');

%%
% Solve Primal and Dual

% max <u,mu-nu> s.t.  |grad(u)| <= w
cvx_solver sdpt3 % SeDuMi %
cvx_begin quiet % sdp quiet
cvx_precision high;
variable u(N,1); %  real;
% norm( grad*u, Inf ) <= 1;
abs( grad*u ) <= w;
maximize( sum( u .* (mu-nu) ) );
cvx_end

% min |w.*f|_1 s.t.   div(f)=mu-nu
cvx_solver sdpt3 % SeDuMi %
cvx_begin quiet % sdp quiet
cvx_precision high;
variable f(P,1); %  real;
(grad'*f) - (mu-nu)  == 0;
minimize( sum( abs(f).*w ) );
% maximize( sum( abs(f(:)).*w(:) ) );
cvx_end

% plot flow
K = find(abs(f)>1e-3);
clf; hold on;
triplot(T,X(1,:),X(2,:), 'k-');
for k=K'
    plot(X(1,[I(k) J(k)]),X(2,[I(k) J(k)]), 'k', 'LineWidth', 4);
end
% vertex according to mass
for i=1:N
    plot(X(1,i),X(2,i), '.', 'MarkerSize', .01 + 50*mu(i)/max(mu), 'color', 'r');
    plot(X(1,i),X(2,i), '.', 'MarkerSize', .01 + 50*nu(i)/max(mu), 'color', 'b');
end
axis equal; axis off;
saveas(gcf, [rep 'flow.png'], 'png');
saveas(gcf, [rep 'flow.eps'], 'epsc');

% plot distance function
K = find(abs(f)>1e-3);
D = u-min(u(:)); D = D/max(D(:));
h = min(abs( (grad*u) ./ w ), 1);
clf; hold on;
% triplot(T,X(1,:),X(2,:), 'k');
% edge according to saturation index
for k=1:P
    col = h(k) * [0 1 0] + (1-h(k)) * [0 0 0];
    plot(X(1,[I(k) J(k)]),X(2,[I(k) J(k)]), 'color',  col,'LineWidth', 2);
end
% shortest path
%for k=K'
%    plot(X(1,[I(k) J(k)]),X(2,[I(k) J(k)]), 'k:', 'LineWidth', 3);
%end
% vertex according to distance u
for i=1:N
    plot(X(1,i),X(2,i), '.', 'MarkerSize', 30, 'color', [D(i) 0 1-D(i)]);
end
plot(X(1,s),X(2,s), 'r.', 'MarkerSize', 40);
plot(X(1,t),X(2,t), 'b.', 'MarkerSize', 40);
axis equal; axis off;
saveas(gcf, [rep 'potential.png'], 'png');
saveas(gcf, [rep 'potential.epsc'], 'epsc');


function co = convexification(varargin)
%CONVEXIFICATION Compute the convexification of a polygon.
%
%   CO = convexification(H)
%   Creates convexification from support function. Support function is
%   supposed to be uniformly distributed over [0 2pi].
%
%   CO = convexification(POLYGON)
%   Computes support function of the polygon, then the corresponding
%   convexification.
%
%   CO = convexification(POLYGON, N)
%   Uses N points for convexification computation. Note that the number of
%   points of CO can be lower than N.
%
%   CAUTION: The result will be valid only for convex polygons.
%
%   See also
%   polygons2d, supportFunction

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2005-01-12
% Copyright 2005-2022 INRA - Cepia Software Platform

if ~isempty(varargin)>0
    var = varargin{1};
    if size(var, 2)==1
        h = var;
    else
        poly = var;
        N = 128;
        if length(varargin)>1
            N = varargin{2};
        end
        h = supportFunction(poly, N);
    end
else
    error('not enough input arguments');
end

N   = length(h);
u   = (0:2*pi/N:2*pi*(1-1/N))';
v   = [cos(u) sin(u)].*[h h];

i1  = 1:N;
i2  = [2:N 1];
i3  = [3:N 1 2];

circ = zeros(N, 4);
for i=1:N
    circ(i, 1:4) = createDirectedCircle(v(i1(i),:), v(i2(i),:), v(i3(i), :));
end

% remove non direct-oriented circles
circ = circ(circ(:,4)==0, :);

% keep only circles seen several times
dp = diff(circ(:,1:2));
dp = sum(dp.*dp, 2);
ind1 = [1; find(dp<1e-10)+1];
circ = circ(ind1, :);

% keep only one instance of each circle
dp = diff(circ(:,1:2));
dp = sum(dp.*dp, 2);
ind = [1; find(dp>1e-10)+1];
co = 2*circ(ind, 1:2);

% eventually remove the last point if it is the same as the first one
if distancePoints(co(1,:), co(end, :))<1e-10 && size(co, 1)>1
    co = co(1:end-1,:);
end


function pass = test_linearSystem1(pref)
% A linear CHEBOP test. This test tests a system of coupled ODEs, both with and
% without breakpoints in the domain of the CHEBOP. Problems are solved using
% chebcolloc1, chebcolloc2 and ultraS discretizations.

% Test 2x2 system (sin/cos)
% Toby Driscoll (extended by AB)

% Note: This test is taken from chebop_systemsolve1 in V4.

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

tol = 1e-10;

%% Smooth domain, chebcolloc1:
d = [-pi pi];
A = chebop(@(x,u,v) [u - diff(v) ; diff(u) + v],d);
A.lbc = @(u,v) u + 1;
A.rbc = @(u,v) v;
x = chebfun('x',d);

pref.discretization = @chebcolloc1;
u = mldivide(A, 0, pref);
u1 = u{1}; u2 = u{2};

% Want to check BCs as well.
bcFunLeft = A.lbc(u1,u2);
bcFunRight = A.rbc(u1,u2);

pass(1) = norm( u1 - cos(x),inf) < 100*tol;
pass(2) = norm( u2 - sin(x),inf) < 100*tol;
pass(3) = norm(bcFunLeft(d(1))) < tol && norm(bcFunRight(d(end))) < tol;

%% Smooth domain, chebcolloc2
pref.discretization = @chebcolloc2;
u = mldivide(A, 0, pref);
u1 = u{1}; u2 = u{2};

% Want to check BCs as well.
bcFunLeft = A.lbc(u1,u2);
bcFunRight = A.rbc(u1,u2);

pass(4) = norm( u1 - cos(x),inf) < 100*tol;
pass(5) = norm( u2 - sin(x),inf) < 100*tol;
pass(6) = norm(bcFunLeft(d(1))) < tol && norm(bcFunRight(d(end))) < tol;

%% Smooth domain, ultraS
pref.discretization = @ultraS;
u = mldivide(A, 0, pref);
u1 = u{1}; u2 = u{2};

% Want to check BCs as well.
bcFunLeft = A.lbc(u1,u2);
bcFunRight = A.rbc(u1,u2);

pass(7) = norm( u1 - cos(x),inf) < 100*tol;
pass(8) = norm( u2 - sin(x),inf) < 100*tol;
pass(9) = norm(bcFunLeft(d(1))) < tol && norm(bcFunRight(d(end))) < tol;
%% Piecewise, chebcolloc1:
A.domain = [-pi 0 pi];
pref.discretization = @chebcolloc1;
u = mldivide(A, 0, pref);
u1 = u{1}; u2 = u{2};

% Want to check BCs as well.
bcFunLeft = A.lbc(u1,u2);
bcFunRight = A.rbc(u1,u2);
bcValLeft = bcFunLeft(d(1));
bcValRight = bcFunRight(d(2));

% And check that we're continuous over breakpoint
u1jump = jump(u1, 0);
u2jump = jump(u2, 0);

pass(10) = norm( u1 - cos(x),inf) < 2000*tol;
pass(11) = norm( u2 - sin(x),inf) < 2000*tol;
pass(12) = norm(bcValLeft) < tol && norm(bcValRight) < tol;
pass(13) = norm(u1jump) < tol && norm(u2jump) < tol;

%% Piecewise, chebcolloc2:
pref.discretization = @chebcolloc2;
u = mldivide(A, 0, pref);
u1 = u{1}; u2 = u{2};

% Want to check BCs as well.
bcFunLeft = A.lbc(u1,u2);
bcFunRight = A.rbc(u1,u2);
bcValLeft = bcFunLeft(d(1));
bcValRight = bcFunRight(d(2));

% And check that we're continuous over breakpoint
u1jump = jump(u1, 0);
u2jump = jump(u2, 0);

pass(14) = norm( u1 - cos(x),inf) < 2000*tol;
pass(15) = norm( u2 - sin(x),inf) < 2000*tol;
pass(16) = norm(bcValLeft) < tol && norm(bcValRight) < tol;
pass(17) = norm(u1jump) < tol && norm(u2jump) < tol;

%% Piecewise, ultraS:
pref.discretization = @ultraS;
u = mldivide(A, 0, pref);
u1 = u{1}; u2 = u{2};

% Want to check BCs as well.
bcFunLeft = A.lbc(u1,u2);
bcFunRight = A.rbc(u1,u2);
bcValLeft = bcFunLeft(d(1));
bcValRight = bcFunRight(d(2));

% And check that we're continuous over breakpoint
u1jump = jump(u1, 0);
u2jump = jump(u2, 0);

pass(18) = norm( u1 - cos(x),inf) < 2000*tol;
pass(19) = norm( u2 - sin(x),inf) < 2000*tol;
pass(20) = norm(bcValLeft) < tol && norm(bcValRight) < tol;
pass(21) = norm(u1jump) < tol && norm(u2jump) < tol;
end


function [F_N, Y_N] = leastSquaresSHT(N, F, dirs, basisType, weights)
%LEASTSQUARESSHT Spherical harmonic transform of F using least-squares
%
%   N:  maximum order of transform
%   F:  the spherical function evaluated at K directions 'dirs'
%       if F is a KxM matrix then the transform is applied to each column
%       of F, and returns the coefficients at each column of F_N
%       respectively
%   dirs:   [azimuth1 inclination1; ...; azimuthK inclinationK] angles in
%           rads for each evaluation point, where inclination is the polar
%           angle from zenith: inclination = pi/2-elevation
%   basisType:  'complex' or 'real' spherical harmonics
%   weights:    vector of K weights for each direction, for weighted
%               least-squares
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   Archontis Politis, 10/10/2013, updated 7/2/2015
%   archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    % compute the harmonic coefficients
    Y_N = getSH(N, dirs, basisType);
    Npoints = size(dirs,1);

    % non-weighted case for uniform arrangements
    if nargin<5 || ~exist('weights','var') || isempty(weights)

        % perform transform in the least squares sense
        F_N = pinv(Y_N)*F;
    else

        % perform weighted least-squares transform
        F_N = (Y_N'*diag(weights)*Y_N) \ (Y_N'*diag(weights) * F);
    end

end


% test for computation of curvature on meshes

name = 'mushroom';
name = 'fandisk';
name = 'horse';
name = 'nefertiti';
name = 'elephant-50kv';
name = 'bunny';
name = 'skull';
name = 'lion-head';
name = 'vase-lion';
name = 'david50kf';
name = 'david_head';
name = 'armadillo';
name = 'aphro';
name = 'gargoyle';
name = 'tyra';
name = 'screwdriver';
name = 'hand';
options.name = name;

path(path, '../toolbox_graph_data/off/');

rep = ['results/curvature/' name '/'];
if not(exist(rep))
    mkdir(rep);
end

[vertex,face] = read_mesh(name);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% local covariance analysis
options.covariance_smoothing = 15;
[C,U,D] = compute_mesh_local_covariance(vertex,face,vertex,options);

% options for display
tau = 1.2;
options.normal_scaling = 1.5;

options.normal = squeeze(U(:,2,:));
clf;
options.face_vertex_color = perform_saturation( -D(2,:)' - D(3,:)',tau);
plot_mesh(vertex,face, options);
shading interp; camlight; colormap jet(256);
saveas(gcf, [rep name '-covariance.png'], 'png');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% curvature

options.curvature_smoothing = 10;
[Umin,Umax,Cmin,Cmax,Cmean,Cgauss,Normal] = compute_curvature(vertex,face,options);

options.normal = [];
clf;
options.face_vertex_color = perform_saturation(Cmax,tau);
plot_mesh(vertex,face, options);
shading interp; camlight; colormap jet(256);
saveas(gcf, [rep name '-cmax.png'], 'png');
clf;
options.face_vertex_color = perform_saturation(Cmin,tau);
plot_mesh(vertex,face, options);
shading interp; camlight; colormap jet(256);
saveas(gcf, [rep name '-cmin.png'], 'png');
clf;
options.face_vertex_color = perform_saturation(Cmean,tau);
plot_mesh(vertex,face, options);
shading interp; camlight; colormap jet(256);
saveas(gcf, [rep name '-cmean.png'], 'png');
clf;
options.face_vertex_color = perform_saturation(Cgauss,tau);
plot_mesh(vertex,face, options);
shading interp; camlight; colormap jet(256);
saveas(gcf, [rep name '-cgauss.png'], 'png');
clf;
options.face_vertex_color = perform_saturation(abs(Cmin)+abs(Cmax),tau);
plot_mesh(vertex,face, options);
shading interp; camlight; colormap jet(256);
saveas(gcf, [rep name '-cabs.png'], 'png');


function determ = jacobi_determinant ( n )

%*****************************************************************************80
%
%% JACOBI_DETERMINANT returns the determinant of the JACOBI matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real DETERM, the determinant.
%
  if ( mod ( n, 2 ) == 1 )

    determ = 0.0;

  else

    lambda = legendre_zeros ( n );

    determ = prod ( lambda(1:n) );

  end

  return
end


function [X, funVal, ValueL]=LeastR(A, Y, z, opts)
%
%%
% Function LeastR
%      Least Squares Loss with the L1-norm Regularization
%
%% Problem
%
%  min  1/2 || A x - y||^2 + 1/2 rsL2 * ||x||_2^2 + z * ||x||_1
%
%  By default, rsL2=0.
%  When rsL2 is nonzero, this corresponds to the well-know elastic net.
%
%% Input parameters:
%
%  A-         Matrix of size m x n
%                A can be a dense matrix
%                         a sparse matrix
%                         or a DCT matrix
%  y -        Response vector (of size mx1)
%  z -        L_1 norm regularization parameter (z >=0)
%  opts-      Optional inputs (default value: opts=[])
%
%% Output parameters:
%  x-         Solution
%  funVal-    Function value during iterations
%
%% Copyright (C) 2009-2010 Jun Liu, and Jieping Ye
%
% You are suggested to first read the Manual.
%
% For any problem, please contact with Jun Liu via j.liu@asu.edu
%
% Last modified on February 18, 2010.
%
%% Related papers
%
% [1]  Jun Liu and Jieping Ye, Efficient Euclidean Projections
%      in Linear Time, ICML 2009.
%
% [2]  Jun Liu and Jieping Ye, Sparse Learning with Efficient Euclidean
%      Projections onto the L1 Ball, Technical Report ASU, 2008.
%
%% Related functions
%
%  sll_opts, initFactor, pathSolutionLeast
%  LeastC, nnLeastR, nnLeastC,
%
%%

%% Verify and initialize the parameters
%%

% Verify the number of input parameters
if (nargin <3)
    error('\n Inputs: A, y and z should be specified!\n');
elseif (nargin==3)
    opts=[];
end

% Get the size of the matrix A
[m,n]=size(A);% m -- sample size,n -- feature dimension%******************************
k=size(Y,2);
%sigma=diag(diag(rand(k)));
sigma=opts.sigma;
% Verify the length of y
if(size(Y,1) ~=m)%********************************
%if (length(y) ~=m)
    error('\n Check the length of y!\n');
end

% Verify the value of z
if (z<0)
    error('\n z should be nonnegative!\n');
end

% run sll_opts to set default values (flags)
opts=sll_opts(opts);

%% Detailed initialization
%% Normalization

% Please refer to sll_opts for the definitions of mu, nu and nFlag
%
% If .nFlag =1, the input matrix A is normalized to
%                     A= ( A- repmat(mu, m,1) ) * diag(nu)^{-1}
%
% If .nFlag =2, the input matrix A is normalized to
%                     A= diag(nu)^{-1} * ( A- repmat(mu, m,1) )
%
% Such normalization is done implicitly
%     This implicit normalization is suggested for the sparse matrix
%                                    but not for the dense matrix
%

if (opts.nFlag~=0)
    if (isfield(opts,'mu'))
        mu=opts.mu;
        if(size(mu,2)~=n)
            error('\n Check the input .mu');
        end
    else
        mu=mean(A,1);
    end

    if (opts.nFlag==1)
        if (isfield(opts,'nu'))
            nu=opts.nu;
            if(size(nu,1)~=n)
                error('\n Check the input .nu!');
            end
        else
            nu=(sum(A.^2,1)/m).^(0.5); nu=nu';
        end
    else % .nFlag=2
        if (isfield(opts,'nu'))
            nu=opts.nu;
            if(size(nu,1)~=m)
                error('\n Check the input .nu!');
            end
        else
            nu=(sum(A.^2,2)/n).^(0.5);
        end
    end

    ind_zero=find(abs(nu)<= 1e-10);    nu(ind_zero)=1;
    % If some values in nu is typically small, it might be that,
    % the entries in a given row or column in A are all close to zero.
    % For numerical stability, we set the corresponding value to 1.
end

if (~issparse(A)) && (opts.nFlag~=0)
    fprintf('\n -----------------------------------------------------');
    fprintf('\n The data is not sparse or not stored in sparse format');
    fprintf('\n The code still works.');
    fprintf('\n But we suggest you to normalize the data directly,');
    fprintf('\n for achieving better efficiency.');
    fprintf('\n -----------------------------------------------------');
end

%% Starting point initialization

% compute AT y
if (opts.nFlag==0)
    ATY=A'*Y; %A-->mxn;y-->mxk****************************
elseif (opts.nFlag==1)
    ATy=A'*y - sum(y) * mu';  ATy=ATy./nu;
else
    invNu=y./nu;              ATy=A'*invNu-sum(invNu)*mu';
end

% process the regularization parameter

% L2 norm regularization
if isfield(opts,'rsL2')
    rsL2=opts.rsL2;
    if (rsL2<0)
        error('\n opts.rsL2 should be nonnegative!');
    end
else
    rsL2=0;
end

% L1 norm regularization
if (opts.rFlag==0)
    lambda=z;
else % z here is the scaling factor lying in [0,1]
    if (z<0 || z>1)
        error('\n opts.rFlag=1, and z should be in [0,1]');
    end

    lambda_max=max(abs(ATy));
    lambda=z*lambda_max;

    rsL2=rsL2*lambda_max; % the input rsL2 is a ratio of lambda_max
end

% initialize a starting point
if opts.init==2
    X=zeros(n,k);%*************************************
    %x=zeros(n,1);
else
    if isfield(opts,'x0')
        x=opts.x0;
        if (length(x)~=n)
            error('\n Check the input .x0');
        end
    else
        X=ATY;  % if .x0 is not specified, we use ratio*ATy,
        % where ratio is a positive value
    end
end

% compute A x
if (opts.nFlag==0)
    AX=A* X;
elseif (opts.nFlag==1)
    invNu=x./nu; mu_invNu=mu * invNu;
    Ax=A*invNu -repmat(mu_invNu, m, 1);
else
    Ax=A*x-repmat(mu*x, m, 1);     Ax=Ax./nu;
end

if (opts.init==0) % If .init=0, we set x=ratio*x by "initFactor"
    % Please refer to the function initFactor for detail

    x_norm=sum(abs(X(:))); x_2norm=trace(X'*X);%*****************************corresponding to L1norm,Fnorm
    if x_norm>=1e-6
        ratio=initFactorqiao(x_norm, AX, Y, lambda,'LeastR', rsL2, x_2norm);
        X=ratio*X;    AX=ratio*AX;
    end
end

%% The main program

%% The Armijo Goldstein line search scheme + accelearted gradient descent
if (opts.mFlag==0 && opts.lFlag==0)

    bFlag=0; % this flag tests whether the gradient step only changes a little

    L=1 + rsL2;
    % We assume that the maximum eigenvalue of A'A is over 1

    % assign xp with x, and Axp with Ax
    Xp=X; AXp=AX; XXp=zeros(n,k);

    % alphap and alpha are used for computing the weight in forming search point
    alphap=0; alpha=1;

    for iterStep=1:opts.maxIter
        % --------------------------- step 1 ---------------------------
        % compute search point s based on xp and x (with beta)
        beta=(alphap-1)/alpha;    S=X + beta* XXp;

        % --------------------------- step 2 ---------------------------
        % line search for L and compute the new approximate solution x

        % compute the gradient (g) at s
        AS=AX + beta* (AX-AXp);

        % compute AT As
        if (opts.nFlag==0)
            ATAS=A'*AS;
        elseif (opts.nFlag==1)
            ATAs=A'*As - sum(As) * mu';  ATAs=ATAs./nu;
        else
            invNu=As./nu;                ATAs=A'*invNu-sum(invNu)*mu';
        end

        % obtain the gradient g
        G=ATAS-ATY + rsL2 * S*inv(sigma);%************************** +rsL2*s*inv(sigma)

        % copy x and Ax to xp and Axp
        Xp=X;    AXp=AX;

        while (1)
            % let s walk in a step in the antigradient of s to get v
            % and then do the l1-norm regularized projection
            V=S-G/L;

            % L1-norm regularized projection
            X=sign(V).*max(abs(V)-lambda / L,0);

            V=X-S;  % the difference between the new approximate solution x
            % and the search point s

            % compute A x
            if (opts.nFlag==0)
                AX=A* X;
            elseif (opts.nFlag==1)
                invNu=x./nu; mu_invNu=mu * invNu;
                Ax=A*invNu -repmat(mu_invNu, m, 1);
            else
                Ax=A*x-repmat(mu*x, m, 1);     Ax=Ax./nu;
            end

            AV=AX -AS;
            r_sum=trace(V'*V); l_sum=trace(AV'*AV);

            if (r_sum <=1e-20)
                bFlag=1; % this shows that, the gradient step makes little improvement
                break;
            end

            % the condition is ||Av||_2^2 <= (L - rsL2) * ||v||_2^2
            if(l_sum <= r_sum * (L-rsL2))
                break;
            else
                L=max(2*L, l_sum/r_sum + rsL2);
                % fprintf('\n L=%5.6f',L);
            end
        end

        ValueL(iterStep)=L;

        % --------------------------- step 3 ---------------------------
        % update alpha and alphap, and check whether converge
        alphap=alpha; alpha= (1+ sqrt(4*alpha*alpha +1))/2;

        XXp=X-Xp;   AXY=AX-Y;
        funVal(iterStep)=trace(AXY'* AXY)/2 + rsL2/2 * trace(X'*X) + sum(abs(X(:))) * lambda;

        if (bFlag)
            % fprintf('\n The program terminates as the gradient step changes the solution very small.');
            break;
        end

        switch(opts.tFlag)
            case 0
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <= opts.tol)
                        break;
                    end
                end
            case 1
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <=...
                            opts.tol* funVal(iterStep-1))
                        break;
                    end
                end
            case 2
                if ( funVal(iterStep)<= opts.tol)
                    break;
                end
            case 3
                norm_xxp=sqrt(xxp'*xxp);
                if ( norm_xxp <=opts.tol)
                    break;
                end
            case 4
                norm_xp=sqrt(xp'*xp);    norm_xxp=sqrt(xxp'*xxp);
                if ( norm_xxp <=opts.tol * max(norm_xp,1))
                    break;
                end
            case 5
                if iterStep>=opts.maxIter
                    break;
                end
        end
    end
end

%% Reformulated problem + Nemirovski's scheme

% .mFlag=1, and .lFlag=0
%  refomulate the problem as the constrained convex optimization
%  problem, and then apply Armijo Goldstein line search scheme

% Problem:
%    min  1/2 || A x - y||^2 + 1/2 rsL2 * ||x||_2^2 + z * t' * 1
%    s.t.   |x| <= t

if(opts.mFlag==1 && opts.lFlag==0)

    bFlag=0; % this flag tests whether the gradient step only changes a little

    L=1 + rsL2;
    % We assume that the maximum eigenvalue of A'A is over 1

    % assign xp with x, and Axp with Ax
    xp=x; Axp=Ax; xxp=zeros(n,1);
    t=abs(x); tp=t;
    % t is the upper bound of absolute value of x

    % alphap and alpha are used for computing the weight in forming search point
    alphap=0; alpha=1;

    for iterStep=1:opts.maxIter
        % --------------------------- step 1 ---------------------------
        % compute search point s based on xp and x (with beta)
        beta=(alphap-1)/alpha;    s=x + beta* xxp; s_t= t + beta * (t -tp);

        % --------------------------- step 2 ---------------------------
        % line search for L and compute the new approximate solution x

        % compute the gradient (g) at s
        As=Ax + beta* (Ax-Axp);

        % compute AT As
        if (opts.nFlag==0)
            ATAs=A'*As;
        elseif (opts.nFlag==1)
            ATAs=A'*As - sum(As) * mu';  ATAs=ATAs./nu;
        else
            invNu=As./nu;                ATAs=A'*invNu-sum(invNu)*mu';
        end

        % obtain the gradient g
        g=ATAs-ATy + rsL2 * s;

        % copy x and Ax to xp and Axp
        xp=x;    Axp=Ax;
        tp=t;

        while (1)
            % let s walk in a step in the antigradient of s to get v
            % and then do the l1-norm regularized projection

            u=s-g/L;
            v= s_t - lambda / L;

            % projection
            [x, t]=ep1R(u, v, n);

            v=x-s;  % the difference between the new approximate solution x
                       % and the search point s

            v_t=t-s_t;

            % compute A x
            if (opts.nFlag==0)
                Ax=A* x;
            elseif (opts.nFlag==1)
                invNu=x./nu; mu_invNu=mu * invNu;
                Ax=A*invNu -repmat(mu_invNu, m, 1);
            else
                Ax=A*x-repmat(mu*x, m, 1);     Ax=Ax./nu;
            end

            Av=Ax -As;
            r_sum=v'*v + v_t'*v_t; l_sum=Av'*Av + v'*v * rsL2;

            if (r_sum <=1e-20)
                bFlag=1; % this shows that, the gradient step makes little improvement
                break;
            end

            % the condition is ||Av||_2^2 + rsL2 * ||v||_2^2
            %                       <= L * (||v||_2^2 + ||v_t|| _2^2 )
            if(l_sum <= r_sum * L)
                break;
            else
                L=max(2*L, l_sum/r_sum);
                % fprintf('\n L=%5.6f',L);
            end
        end

        ValueL(iterStep)=L;

        % --------------------------- step 3 ---------------------------
        % update alpha and alphap, and check whether converge
        alphap=alpha; alpha= (1+ sqrt(4*alpha*alpha +1))/2;

        xxp=x-xp;   Axy=Ax-y;
        funVal(iterStep)=Axy'* Axy/2 + rsL2/2 * x'*x + sum(t) * lambda;

        if (bFlag)
            % fprintf('\n The program terminates as the gradient step changes the solution very small.');
            break;
        end

        switch(opts.tFlag)
            case 0
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <= opts.tol)
                        break;
                    end
                end
            case 1
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <=...
                            opts.tol* funVal(iterStep-1))
                        break;
                    end
                end
            case 2
                if ( funVal(iterStep)<= opts.tol)
                    break;
                end
            case 3
                norm_xxp=sqrt(xxp'*xxp + norm(t-tp)^2);
                if ( norm_xxp <=opts.tol)
                    break;
                end
            case 4
                norm_xp=sqrt(xp'*xp + tp'*tp);    norm_xxp=sqrt(xxp'*xxp+ norm(t-tp)^2);
                if ( norm_xxp <=opts.tol * max(norm_xp,1))
                    break;
                end
            case 5
                if iterStep>=opts.maxIter
                    break;
                end
        end
    end

end

%% adaptive line search

% .mFlag=1, and .lFlag=1
%  refomulate the problem as the constrained convex optimization
%  problem, and then apply adaptive line search scheme

% Problem:
%    min  1/2 || A x - y||^2 + 1/2 rsL2 * ||x||_2^2 + z * t' * 1
%    s.t.   |x| <= t

if(opts.mFlag==1 && opts.lFlag==1)

    bFlag=0; % this flag tests whether the gradient step only changes a little

    L=1 + rsL2;
    % We assume that the maximum eigenvalue of A'A is over 1

    gamma=1;
    % we shall set the value of gamma = L,
    % where L is appropriate for the starting point

    xp=x; Axp=Ax;
    % store x and Ax
    xxp=zeros(n,1);
    % the difference of x and xp
    t=abs(x); tp=t;
    % t is the upper bound of absolute value of x

    % compute AT Ax
    if (opts.nFlag==0)
        ATAx=A'*Ax;
    elseif (opts.nFlag==1)
        ATAx=A'*Ax - sum(Ax) * mu';  ATAx=ATAx./nu;
    else
        invNu=Ax./nu;                ATAx=A'*invNu-sum(invNu)*mu';
    end

    % We begin the adaptive line search in the following
    %
    % Note that, in the line search, L and beta are changing

    for iterStep=1:opts.maxIter

        ATAxp=ATAx;
        % store ATAx to ATAxp

        if (iterStep~=1)
            % compute AT Ax
            if (opts.nFlag==0)
                ATAx=A'*Ax;
            elseif (opts.nFlag==1)
                ATAx=A'*Ax - sum(Ax) * mu';  ATAx=ATAx./nu;
            else
                invNu=Ax./nu;                ATAx=A'*invNu-sum(invNu)*mu';
            end
        end

        %--------- Line Search for L begins
        while (1)
            if (iterStep~=1)
                alpha= (-gamma+ sqrt(gamma*gamma + 4* L * gamma)) / (2*L);
                beta= (gamma - gamma* alphap) / (alphap * gamma + alphap* L * alpha);
                % beta is the coefficient for generating search point s

                s=x + beta* xxp;   s_t= t + beta * (t -tp);
                As=Ax + beta* (Ax-Axp);
                ATAs=ATAx + beta * (ATAx- ATAxp);
                % compute the search point s, A * s, and A' * A * s
            else
                alpha= (-1+ sqrt(5)) / 2;
                beta=0; s=x; s_t=t; As=Ax; ATAs=ATAx;
            end

            g=ATAs-ATy + rsL2 * s;
            % compute the gradient g

            % let s walk in a step in the antigradient of s
            u=s-g/L;
            v= s_t - lambda / L;

            % projection
            [xnew, tnew]=ep1R(u,v,n);

            v=xnew-s;  % the difference between the new approximate solution x
                            % and the search point s
            v_t=tnew-s_t;

            % compute A xnew
            if (opts.nFlag==0)
                Axnew=A* xnew;
            elseif (opts.nFlag==1)
                invNu=xnew./nu; mu_invNu=mu * invNu;
                Axnew=A*invNu -repmat(mu_invNu, m, 1);
            else
                Axnew=A*xnew-repmat(mu*xnew, m, 1);     Axnew=Axnew./nu;
            end

            Av=Axnew -As;
            r_sum=v'*v + v_t'*v_t; l_sum=Av'*Av + v'*v * rsL2;

            if (r_sum <=1e-20)
                bFlag=1; % this shows that, the gradient step makes little improvement
                break;
            end

            % the condition is ||Av||_2^2 + rsL2 * ||v||_2^2
            %                       <= L * (||v||_2^2 + ||v_t|| _2^2 )
            if(l_sum <= r_sum * L)
                break;
            else
                L=max(2*L, l_sum/r_sum);
                % fprintf('\n L=%5.6f',L);
            end
        end
        %--------- Line Search for L ends

        gamma=L* alpha* alpha;    alphap=alpha;
        % update gamma, and alphap

        ValueL(iterStep)=L;

        tao=L * r_sum / l_sum;
        if (tao >=5)
            L=L*0.8;
        end
        % decrease the value of L

        xp=x;    x=xnew; xxp=x-xp;
        Axp=Ax;  Ax=Axnew;
        % update x and Ax with xnew and Axnew
        tp=t; t=tnew;
        % update tp and t

        Axy=Ax-y;
        funVal(iterStep)=Axy' * Axy/2 + rsL2/2 * x'*x + lambda * sum(t);
        % compute function value

        if (bFlag)
            % fprintf('\n The program terminates as the gradient step changes the solution very small.');
            break;
        end

        switch(opts.tFlag)
            case 0
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <= opts.tol)
                        break;
                    end
                end
            case 1
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <=...
                            opts.tol* funVal(iterStep-1))
                        break;
                    end
                end
            case 2
                if ( funVal(iterStep)<= opts.tol)
                    break;
                end
            case 3
                norm_xxp=sqrt(xxp'*xxp+ norm(t-tp)^2);
                if ( norm_xxp <=opts.tol)
                    break;
                end
            case 4
                norm_xp=sqrt(xp'*xp + tp'*tp);    norm_xxp=sqrt(xxp'*xxp+ norm(t-tp)^2);
                if ( norm_xxp <=opts.tol * max(norm_xp,1))
                    break;
                end
            case 5
                if iterStep>=opts.maxIter
                    break;
                end
        end
    end
end

%%
if(opts.mFlag==0 && opts.lFlag==1)
    error('\n The function does not support opts.mFlag=0 & opts.lFlag=1!');
end


function f = olsmatrix(X,mode)

% function f = olsmatrix(X,mode)
%
% <X> is samples x parameters
% <mode> (optional) is
%   0 means normal operation
%   1 means use inv instead of \ and omit unit-length normalization.
%     the point of this mode is to reduce memory usage (i think).
%   default: 0.
%
% what we want to do is to perform OLS regression using <X>
% and obtain the parameter estimates.  this is accomplished
% by inv(X'*X)*X'*y = f*y where y is the data (samples x cases).
%
% what this function does is to return <f> which has dimensions
% parameters x samples.
%
% to ensure well-conditioning, we unit-length normalize each column
% of <X> before doing the inv.m operation.  also, we actually use
% left-slash (\), which apparently is equivalent to inv.m but faster
% and more accurate (see code for details).  if you pass <mode> as 1,
% we omit the normalization and use the inv method instead of the \ method.
%
% also, in the case that one or more regressors in <X> are all zero, then
% without special handling, then this case will result in warnings and
% NaN results.  what we do is to explicitly ensure that all-zero regressors
% are ignored and that there are all-zero rows for these regressors
% in <f>.  this makes it such that the weights estimated for these
% regressors are simply zero.
%
% see also olsmatrix2.m.
%
% history:
% 2011/06/27 - explicitly ignore 0
% 2011/03/29 - mode 1 now omits unit length normalization.
% 2011/03/28 - add <mode> input.
% 2010/08/05: now, we do not try to detect weird cases with low variance.
%             instead, we just blindly unit-length normalize each column.

% input
if ~exist('mode','var') || isempty(mode)
  mode = 0;
end

% check
assert(all(~isnan(X(:))));

% deal with degenerate regressors
good = ~all(X==0,1);

% initialize result
f = zeros(size(X,2),size(X,1));

% do it
switch mode
case 0
  [X,len] = unitlength(X(:,good),1,[],0);
  temp = diag(1./len)*((X'*X)\X');  % hm, suggested by M-Lint
case 1
  temp = inv(X(:,good)'*X(:,good))*X(:,good)';
end

% return
if any(good)
  f(good,:) = temp;
end


%TROTX SE(3) rotation about X axis
%
% T = TROTX(THETA) is a homogeneous transformation (4x4) representing a rotation
% of THETA radians about the x-axis.
%
% T = TROTX(THETA, 'deg') as above but THETA is in degrees.
%
% Notes::
% - Translational component is zero.
%
% See also rotx, troty, trotz, trot2, SE3.Rx.

%## 3d rotation

% Copyright (C) 1993-2019 Peter I. Corke
%
% This file is part of The Spatial Math Toolbox for MATLAB (SMTB).
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
% of the Software, and to permit persons to whom the Software is furnished to do
% so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
% FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
% COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
% IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
% CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
%
% https://github.com/petercorke/spatial-math

function T = trotx(t, varargin)
	T = [rotx(t, varargin{:}) [0 0 0]'; 0 0 0 1];


function anorm = update_gbound(anorm,alpha,beta,j)
%UPDATE_GBOUND   Update Gerscgorin estimate of 2-norm
%  ANORM = UPDATE_GBOUND(ANORM,ALPHA,BETA,J) updates the Gersgorin bound
%  for the tridiagonal in the Lanczos process after the J'th step.
%  Applies Gerscgorins circles to T_K'*T_k instead of T_k itself
%  since this gives a tighter bound.

if j==1 % Apply Gerscgorin circles to T_k'*T_k to estimate || A ||_2
  i=j;
  % scale to avoid overflow
  scale = max(abs(alpha(i)),abs(beta(i+1)));
  alpha(i) = alpha(i)/scale;
  beta(i) = beta(i)/scale;
  anorm = 1.01*scale*sqrt(alpha(i)^2+beta(i+1)^2 + abs(alpha(i)*beta(i+1)));
elseif j==2
  i=1;
  % scale to avoid overflow
  scale = max(max(abs(alpha(1:2)),max(abs(beta(2:3)))));
  alpha(1:2) = alpha(1:2)/scale;
  beta(2:3) = beta(2:3)/scale;

  anorm = max(anorm, scale*sqrt(alpha(i)^2+beta(i+1)^2 + ...
      abs(alpha(i)*beta(i+1) + alpha(i+1)*beta(i+1)) + ...
      abs(beta(i+1)*beta(i+2))));
  i=2;
  anorm = max(anorm,scale*sqrt(abs(beta(i)*alpha(i-1) + alpha(i)*beta(i)) + ...
      beta(i)^2+alpha(i)^2+beta(i+1)^2 +  ...
      abs(alpha(i)*beta(i+1))) );
elseif j==3
  % scale to avoid overflow
  scale = max(max(abs(alpha(1:3)),max(abs(beta(2:4)))));
  alpha(1:3) = alpha(1:3)/scale;
  beta(2:4) = beta(2:4)/scale;
  i=2;
  anorm = max(anorm,scale*sqrt(abs(beta(i)*alpha(i-1) + alpha(i)*beta(i)) + ...
      beta(i)^2+alpha(i)^2+beta(i+1)^2 +  ...
      abs(alpha(i)*beta(i+1) + alpha(i+1)*beta(i+1)) + ...
      abs(beta(i+1)*beta(i+2))) );
  i=3;
  anorm = max(anorm,scale*sqrt(abs(beta(i)*beta(i-1)) + ...
      abs(beta(i)*alpha(i-1) + alpha(i)*beta(i)) + ...
      beta(i)^2+alpha(i)^2+beta(i+1)^2 +  ...
      abs(alpha(i)*beta(i+1))) );
else
  % scale to avoid overflow
  %  scale = max(max(abs(alpha(j-2:j)),max(abs(beta(j-2:j+1)))));
  %  alpha(j-2:j) = alpha(j-2:j)/scale;
  %  beta(j-2:j+1) = beta(j-2:j+1)/scale;

  % Avoid scaling, which is slow. At j>3 the estimate is usually quite good
  % so just make sure that anorm is not made infinite by overflow.
  i = j-1;
  anorm1 = sqrt(abs(beta(i)*beta(i-1)) + ...
      abs(beta(i)*alpha(i-1) + alpha(i)*beta(i)) + ...
      beta(i)^2+alpha(i)^2+beta(i+1)^2 +  ...
      abs(alpha(i)*beta(i+1) + alpha(i+1)*beta(i+1)) + ...
      abs(beta(i+1)*beta(i+2)));
  if isfinite(anorm1)
    anorm = max(anorm,anorm1);
  end
  i = j;
  anorm1 = sqrt(abs(beta(i)*beta(i-1)) + ...
      abs(beta(i)*alpha(i-1) + alpha(i)*beta(i)) + ...
      beta(i)^2+alpha(i)^2+beta(i+1)^2 +  ...
      abs(alpha(i)*beta(i+1)));
  if isfinite(anorm1)
    anorm = max(anorm,anorm1);
  end
end


%% Simulating Iron Ore Prices
% This example demonstrates calibrating an geomtric brownian motion &
% mean reverting stochastic model from historical data of iron ore prices.
% The model is then used to simulate the prices into the future using
% the Stochastic Differential Equation Simulation engine in Econometrics
% Toolbox.
%
%% Import Historical Data
% Data is saved as a excel file based on historical iron ore prices from
% May 10, 2012 to June 22, 2012
% http://www.indexmundi.com/commodities/?commodity=iron-ore&months=240&currency=aud

% Database Version
importirondb
dates = data.Month;
Date = datenum(dates,'yyyy-mm-dd HH:MM:SS.FFF');
S = data.Price;

% Excel
% [data,txt] = xlsread('ironoremonthly.xlsx');
% dates = txt(3:end,1);
% Date = datenum(dates,'dd/mm/yyyy');
% S = data(:,1);
SR = S(2:end)./S(1:end-1) - 1;
%% The Models
% Return Model (gbm)
% The returns based model used for simulation Iron Orce Prices is geometric
% brownian motion.  It can be described as:
%
% $$ dX_t = \mu(t)X_t dt + D(t,X_t)V(t)dW_t $$
%
% $$ \mu = \textrm{ Instantaneous Rate of Return} $$
%
% $$ \sigma = \textrm { Volatility Rate} $$
%
% Mean Reversion Model (hwv)
% The mean reversion model used for simulating the Iron Ore prices is an Ornstein-Uhlenbeck
% brownian motion with mean reverting drift. This model is fit to the prices
% iron ore prices. The discrete-time equation of this model can be written as,
%
% $$ \Delta x_t = \alpha(\mu - x_t)\Delta t + \sigma dz_t, \textrm{ where } dz_t \sim N(0, \sqrt\Delta_t) $$
%
% $$ \alpha = \textrm{ Mean Reversion Rate}$$
%
% $$ \mu = \textrm{ Mean level} $$
%
% $$ \sigma = \textrm { Volatility } $$

%% Calibrate Parameters

modelsel = 'gbm'; %hwv - mean reverting, %gbm - based on returns
startdate = '2010-08-01';
i = find(datenum(startdate,'yyyy-mm-dd') == Date);
S2 = S(i:end);
Date2 = Date(i:end);
SR2 = SR(i+1:end);

switch modelsel
    case 'gbm'
        dt = 1;
        sigma = std(SR2);
        mu = mean(SR2);
        model = gbm(mu,sigma,'StartState', S(end));
    case 'hwv'
        % hwv - mean reversion model
        % The reversion rate and mean level can be calculated from the coefficients
        % of a linear fit between the prices and their first difference scaled
        % by the time interval parameter. All quantities are specified on an annual
        % scale.
        regressors = [ones(length(S2) - 1, 1) S2(1:end-1)];
        [coefficients, intervals, residuals] = regress(diff(S2), regressors);
        dt    = 1;
        speed = -coefficients(2)/dt;
        level = -coefficients(1)/coefficients(2);
        sigma =  std(residuals)/sqrt(dt);
        model = hwv(speed,level,sigma,  'StartState', S(end));
end

%% Monte-Carlo Simulation
% The model defined above can be simulated with the simulate method of the
% SDE object to generate multiple log price paths. These are exponentiated
% to compute the simulated natural gas prices. The plot below shows 1000
% paths simulated daily, 8 years into the future.

NTrials = 1000;
years = 8;
ann = 12;
NSteps  = years*ann;

tic
Xsim = simulate(model, NSteps, 'NTrials', NTrials, 'DeltaTime', dt);
Xsim = squeeze(Xsim); % Remove redundant dimension
toc

Ssim = Xsim;
SYear = Ssim(13:12:end,:);

% Visualize first 20 prices of 1000 paths
plot(Date, S, Date(end)+(0:30:(years*ann+1)*30-1), Ssim(:,1:100));
datetick; xlabel('Date'); ylabel('Iron Ore Price $US/tonne');
title('Historical & Simulated Iron Ore Prices')
%% Save Model
% The calibrated model is saved in a MAT-file for later use.

save ironoremodel model dt

%% Visual Analysis of Simulated Price Paths
% Instead of plotting a number of paths at once, we can plot longer single paths
% against the observed historical data to visually validate the simulated
% paths. This can serve as a final sanity check.

path = 15;
figure
plot(Date, S, 'b', Date(end)+(0:30:NSteps*30), Ssim(:,path), 'r');
title(['Historical & Simulated Prices, Path ' int2str(path)]);
datetick('x','keeplimits');
xlabel('Date'); ylabel('Iron Ore Price $US/tonne');
%% Histogram results of price paths at the end of each year

figure

for i = 1:years
    subplot(years/2,2,i)
    SsimM = mean(Ssim(i*12+1,:));
    hist(Ssim(i*12,:),20)
    hold on
    hm = plot(SsimM,0,'ro');
    title(['Year: ',num2str(i),' of Iron Ore Prices'])
    xlabel(['Iron Ore Price: Mean =',num2str(round(100*(SsimM))/100)])
    zlabel('Simulation Count')
end


function surfacedata = ellipsoidsurface

a = 9;
b = 3;
c = 1;

a2 = a*a;
a4 = a2*a2;
b2 = b*b;
b4 = b2*b2;
c2 = c*c;
c4 = c2*c2;

surfacedata = struct('phi',@phi,'gradient', @gradient, 'unitoutnormal',...
    @unitoutnormal, 'project', @project, 'initmesh',@initmesh, 'meancurvature', @meancurvature, ...
    'u',@u, 'f',@f,'gra_u',@gra_u);

function [node, elem] = initmesh
M = load('meshdata/ellipsoid3394.mat');
node = M.node;
elem = M.elem;
end

function z = phi(p)
% level set function

x = p(:,1); y = p(:,2); z = p(:,3);
z = x.^2/a2 + y.^2/b2 + z.^2/c2 - 1.0;
end

function n = unitoutnormal(p)
%
n = gradient(p);
l = sqrt(sum(n.^2,2));
n = n./repmat(l,1,3);
end

function n = gradient(p)
% gradient of phi

x = p(:,1); y = p(:,2); z = p(:,3);
n = [2*x/a2, 2*y/b2, 2*z/c2];
end

function [node,dist] = project(p)
% projection function

s = sign(phi(p));

node = p;

normalAtNode = gradient(node);
valueAtNode = phi(node);
node = node - valueAtNode*ones(1,3).*normalAtNode./(dot(normalAtNode,normalAtNode,2)*ones(1,3));

vector = (-s)*ones(1,3).*(node - p);

d = s.*sqrt(dot(vector,vector,2));

normalAtNode = gradient(node);

node = p - d*ones(1,3).*normalAtNode./(sqrt(dot(normalAtNode,normalAtNode,2))*ones(1,3));

valueAtNode = phi(node);

normalAtNode = gradient(node);

vector = (-s)*ones(1,3).*(node - p);

d = s.*sqrt(dot(vector,vector,2));

e1 = normalAtNode./(sqrt(dot(normalAtNode, normalAtNode,2))*ones(1,3))-vector./(sqrt(dot(vector,vector,2))*ones(1,3));
error=sqrt(valueAtNode.^2./(dot(normalAtNode, normalAtNode,2))+dot(e1,e1,2));

k=1;
while max(abs(error)) > 1e-6 && k<200

    k=k+1;

    node = node - valueAtNode*ones(1,3).*normalAtNode./(dot(normalAtNode,normalAtNode,2)*ones(1,3));

    vector = -s*ones(1,3).*(node - p);
    d = s.*sqrt(dot(vector,vector,2));
    normalAtNode = gradient(node);

    node = p - d*ones(1,3).*normalAtNode./(sqrt(dot(normalAtNode,normalAtNode,2))*ones(1,3));

    valueAtNode = phi(node);

    normalAtNode = gradient(node);

    vector = (-s)*ones(1,3).*(node - p);

    d = s.*sqrt(dot(vector,vector,2));
    e1 = normalAtNode./(sqrt(dot(normalAtNode, normalAtNode,2))*ones(1,3))-vector./(sqrt(dot(vector,vector,2))*ones(1,3));
    error=sqrt(valueAtNode.^2./(dot(normalAtNode, normalAtNode,2))+dot(e1,e1,2));
end

if nargout == 2
    dist = d;
end
end

function mc = meancurvature(p)

x = p(:,1); y = p(:,2); z = p(:,3);
x2 = x.^2; y2 = y.^2; z2 = z.^2;

s1 = c4*a2*y2 + b4*a2*z2 + c4*b2*x2 + a4*b2*z2 + b4*c2*x2 + a4*c2*y2;
s2 = b4*c4*x2 + a4*c4*y2 + a4*b4*z2;
s3 = x2/a4+y2/b4+z2/c4;

mc = 0.5* s1./(s2.*sqrt(s3));
end

function z = f(p)
p = project(p);
x = p(:,1); y = p(:,2); z = p(:,3);
x2 = x.^2; y2 = y.^2; z2 = z.^2;
d1 = c4*y2 + b4*z2;
d2 = c2*y2 + b2*z2;
d3 = b4*c2 + c4*b2;
d4 = c4 * b4 * (d2 * a4 + d1 * a2 + x2 * d3);
d5 = (a4 * d1 + b4 * c4 * x2);
z = a2*( d4.*x.*cos(x) +  a2*d1.*d5.*sin(x)) ./ d5.^2;
end

function z = u(p)
% exact solution
p = project(p);
z =sin(p(:,1));
end

function z = gra_u(p)

p = project(p);
x = p(:,1); y = p(:,2); z = p(:,3);
x2 = x.^2; y2 = y.^2; z2 = z.^2;

d1 = x2/a4 + y2/b4 + z2/c4 ;

z1 = (1 - x2 ./(a4*d1));
z2 = -x.*y./(a2*b2*d1);
z3 = -x.*z./(a2*c2*d1);
z = cos(x)*ones(1,3).*[z1,z2,z3];
end

end


function seq = ScanMatch_NumToDoubleStr(num, modulus)
%SCANMATCH_NUMTODOUBLESTR transform an array of numbers into a sequence of
% double characters (each number is represented by a residue of two letter) in
% function of the modulus. The modulus is the length of the
% alphabet used for the second letter of a residue.
% i.e.  seq = ScanMatch_NumToDoubleStr([1 2 27], 26) % modulus = 26 (all the alphabet is used)
%  returns aAaBBa
% i.e.  seq = ScanMatch_NumToDoubleStr([1 2 27], 10) % modulus = 26 (all the alphabet is used)
%  returns aAaBcG
%
% The modulus can be any number between 1 and 26.
% The Input numbers have to be between 1 and 676 (which is the maximum you
% can code with two characters: 676 is xX with a modulus of 26 )
%
%   Part of the ScanMatch toolbox
%   Written by Filipe Cristino
%   $Version: 1.00 $  $Date: 10/09/2009

% ---- Check Modulus ----
if modulus > 26 || modulus < 1
    error('ScanMatch:WrongInput', 'The modulus has to be between 1 and 26!')
end

% ---- Check input numbers ----
max_num = 26 * modulus; % Max number you can convert in function of the modulus
if sum(num <1 | num > max_num)
    error('ScanMatch:WrongInput', 'The input number can not be less than 1 or more than (26 * modulus)')
end

% ---- substract one to the array as the conversion works from zero
num = num - 1;

% ---- Start conversion ----
num_l = length(num);
ind= 1;
for i=1:num_l
    seq(ind) = lower(char((fix(num(i)/modulus))+65));
    seq(ind+1) = char(rem(num(i),modulus)+65);
    ind = ind+2;
end


function err = getL2error3ND2(node,elem,exactE,Eh,markedElem)
%% GETL2ERROR3ND2 L2 norm of the quadratic (1st type) Nedelec edge element.
%
%  error = getL23errorND2(node,elem,exactE,Eh,markedElem);
%
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Sort elem to ascend ordering
elem = sort(elem,2);

%% Construct Data Structure
[Dlambda,volume] = gradbasis3(node,elem);
% elem2dof
[elem2edge,edge] = dof3edge(elem);
[elem2face,face] = dof3face(elem);
NT = size(elem,1);  NE = size(edge,1); NF = size(face,1);
elem2dof = [elem2edge elem2edge+NE elem2face+2*NE elem2face+2*NE+NF];
% local indices
locBasesIdx = [1 2 0; 1 3 0; 1 4 0; 2 3 0; 2 4 0; 3 4 0; ... % phi
               1 2 0; 1 3 0; 1 4 0; 2 3 0; 2 4 0; 3 4 0; ... % psi
               3 2 4; 3 1 4; 2 1 4; 2 1 3; ...
               4 2 3; 4 1 3; 4 1 2; 3 1 2]; % chi
%% Compute square of the L2 error
err = zeros(NT,1);
[lambda,w] = quadpts3(4); % quadrature order is 4
nQuad = size(lambda,1);
for p = 1:nQuad
    % quadrature points in the x-y-z coordinate
    pxy = lambda(p,1)*node(elem(:,1),:) ...
        + lambda(p,2)*node(elem(:,2),:) ...
        + lambda(p,3)*node(elem(:,3),:) ...
        + lambda(p,4)*node(elem(:,4),:);
    Ep = exactE(pxy);
    Ehp = zeros(NT,3);
    % compute Ehp at quadrature points
    for k = 1:20
        k1 = locBasesIdx(k,1);
        k2 = locBasesIdx(k,2);
        k3 = locBasesIdx(k,3);
        % evaluate basis at quadrature points
        if k<=6
        % phi_k = lambda_{k1}Dlambda_{k2} - lambda_{k2}Dlambda_{k1};
            basis_k = (lambda(p,k1)*Dlambda(:,:,k2) ...
                      -lambda(p,k2)*Dlambda(:,:,k1));
        elseif k<=12
        % phi_k = lambda_{k1}Dlambda_{k2} + lambda_{k2}Dlambda_{k1};
            basis_k = (lambda(p,k1)*Dlambda(:,:,k2) ...
                      +lambda(p,k2)*Dlambda(:,:,k1));
        else
        % chi_k = lambda_{k1}phi_{k2,k3};
            basis_k = lambda(p,k1)*(lambda(p,k2)*Dlambda(:,:,k3) ...
                                   -lambda(p,k3)*Dlambda(:,:,k2));
        end
        Ehp = Ehp + repmat(Eh(elem2dof(:,k)),1,3).*basis_k;
    end
    err = err + w(p)*volume.*sum((Ep - Ehp).^2,2);
end
% modify the error
err(isnan(err)) = 0; % remove the singular part
if (nargin == 5) && ~isempty(markedElem)
    err = err(markedElem); % L2 error on some marked region
end
err = sqrt(sum(err));
%% TODO add example in M-lint


%% Ex. 7 More complicated use of loop and index

b = [2 5 7 4 9 8 3];
c = [2 3 5 7];
sum1 = 0;
for k = 1:4
 sum1 = sum1+b(c(k));
end
sum1
%Output:
%       24
% Remark: This program performs the summation of
% sum1 = b(c(1))+b(c(2))+b(c(3))+b(c(4))
%      = b(2)+b(3)+b(5)+b(7)
%       = 5+7+9+3
%       = 24


function pred_value = xval_simple_ols_loo(X, Y, pthreshold)
% Check: a very simple leave-one-out cross-validated regression
% pred_value = xval_simple_ols_loo(X, Y, p-value selection for univariate feature selection)
%
% pred_value: cross-validated predictions of outcome data
% X: n x variables matrix of predictors
% Y: n x 1 vector of outcomes
%
% Tor Wager, June 2010
%
% Go to any LASSO output directory and run this:
%
% maskInfo = iimg_read_img(fullfile(pwd, 'mask.img'), 2);
% dat{1} =  iimg_get_data(maskInfo, anticimages);
% pred_value = xval_simple_ols_loo(X, Y)

[N, k] = size(X);
pred_value = zeros(N, 1);

create_figure('test', 1, 2);

fprintf('Fold: %03d', 0);
for i = 1:N

    fprintf('\b\b\b%3.0f ', i);

    % select training data
    Xi = X; Xi(i, :) = [];
    Yi = Y; Yi(i) = [];

    % select features based on univariate correlations
    [r, p, Tstat] = correlation_fast_series(Xi, Yi);
    wh_features = p <= pthreshold;
    nfeatures(i) = sum(wh_features);
    if nfeatures(i) == 0
        disp('Warning: no features pass threshold');
        wh_features = p <= prctile(p, 10);
    else
        Xi = Xi(:, wh_features);
    end

    Xi = [ones(N-1, 1) Xi]; % add intercept

    % Make prediction
    b = pinv(Xi) * Yi;
    pred_value(i, 1) = [1 X(i, wh_features)] * b;

    create_figure('test', 1, 2, 1); subplot(1, 2, 1); plot(Xi*b, Yi, 'ko');
    plot(pred_value(i, 1), Y(i), 'ro', 'MarkerFaceColor', 'r');
    subplot(1, 2, 2);
   title('Black circles = training set; Red = holdout obs');
    drawnow;

end

cm = colormap(jet(N));
figure; hold on;
for i = 1:N
    plot(pred_value(i), Y(i), 'ko', 'MarkerFaceColor', cm(i, :));
end
xlabel('Predicted outcome (xval)'); ylabel('Outcome');
title('Color = order in data series');

% figure; hold on;
% for i = 1:N
%     plot(intercept_vals(i), pred_value(i), 'ko', 'MarkerFaceColor', cm(i, :));
% end

%
% figure; hold on;
%     plot(pred_value(whorder(1:24)), Y(whorder(1:24)), 'bo', 'MarkerFaceColor', 'b');
% plot(pred_value(whorder(25:end)), Y(whorder(25:end)), 'ro', 'MarkerFaceColor', 'r');


% This script estimates the prior of a hedge fund return and processes extreme views on CVaR
% according to Entropy Pooling
% This script complements the article
%	"Fully Flexible Extreme Views"
%	by A. Meucci, D. Ardia, S. Keel
%	available at www.ssrn.com
% The most recent version of this code is available at
% MATLAB Central - File Exchange

% IMPORTANT - This script is about the methodology, not the input data, which has been modified

clear; clc; close all;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Fetch data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
load pseudodata;

xi = 100 * cell2mat(data(:, 2));
n = length(xi);

% bandwith
bw = kernelbw(xi);

% weights
lambda = log(2) / (n / 2);
wi = exp(-lambda * (n - (n:-1:1)'));
wi = flipud(wi) / sum(wi);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Prior market model
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% kernel density
market = [];
market.mu  = mean(xi);
market.pdf = @(x) kernelpdf(x, xi, bw, wi);
market.cdf = @(x) kernelcdf(x, xi, bw, wi);
market.inv = @(x) kernelinv(x, xi, bw, wi);
market.VaR95 = market.inv(0.05);
market.CVaR95 = quadgk(@(x) (x .* market.pdf(x)), -100, market.VaR95) / 0.05;

% numerical (Gauss-Hermite grid) prior
load ghq1000.mat % load mesh of GH zeros
tmp = (ghqx-min(ghqx))/(max(ghqx)-min(ghqx)); % rescale GH zeros so they belong to [0,1]
epsilon = 1e-10;
Lower = market.inv(epsilon);
Upper = market.inv(1-epsilon);
X  = Lower + tmp * (Upper-Lower); % rescale mesh

p = integrateSubIntervals(X, market.cdf);
p = normalizeProb(p);
J = length(X);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Entropy posterior from extreme view on mean and CVaR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
view = [];
view.mu   = mean(xi) - 1.0;
view.CVaR95 = market.CVaR95 - 1.0;

% Netwton Raphson
s = [];
idx = cumsum(p) <= 0.05 ;
s(1) = sum(idx);
[p_, KLdiv] = optimizeEntropy(p, idx', 0.05, [ones(1, J); X'; (idx .* X)'], [1; view.mu; 0.05 * view.CVaR95]);
%[p_, KLdiv] = optimizeEntropy(p, [idx'; (idx .* X)'], [0.05; 0.05 * view.CVaR95], [ones(1, J); X'], [1; view.mu]);

doStop = 0;
i = 1;
while ~doStop
   i = i + 1;

   idx = [ones(1, s(i-1)), zeros(1, J - s(i-1))]';
   [dummy, KLdiv_s]  = optimizeEntropy(p, idx', 0.05, [ones(1, J); X'; (idx .* X)'], [1; view.mu; 0.05 * view.CVaR95]);
   %[dummy, KLdiv_s]  = optimizeEntropy(p, [idx'; (idx .* X)'], [0.05; 0.05 * view.CVaR95], [ones(1, J); X'], [1; view.mu]);

   idx = [ones(1, s(i-1) + 1), zeros(1, J - s(i-1) - 1)]';
   [dummy, KLdiv_s1]  = optimizeEntropy(p, idx', 0.05, [ones(1, J); X'; (idx .* X)'], [1; view.mu; 0.05 * view.CVaR95]);
   %[dummy, KLdiv_s1] = optimizeEntropy(p, [idx'; (idx .* X)'], [0.05; 0.05 * view.CVaR95], [ones(1, J); X'], [1; view.mu]);

   idx = [ones(1, s(i-1) + 2), zeros(1, J - s(i-1) - 2)]';
   [dummy, KLdiv_s2]  = optimizeEntropy(p, idx', 0.05, [ones(1, J); X'; (idx .* X)'], [1; view.mu; 0.05 * view.CVaR95]);
   %[dummy, KLdiv_s2] = optimizeEntropy(p, [idx'; (idx .* X)'], [0.05; 0.05 * view.CVaR95], [ones(1, J); X'], [1; view.mu]);

   % first difference
   DE  = KLdiv_s1 - KLdiv_s;
   % second difference
   D2E = KLdiv_s2 - 2 * KLdiv_s1 + KLdiv_s;
   % optimal s
   s = [s; round( s(i-1) - (DE / D2E) )];

   tmp = [];
   idx  = [ones(1, s(i)), zeros(1, J - s(i))]';
   [tmp.p_, tmp.KLdiv]  = optimizeEntropy(p, idx', 0.05, [ones(1, J); X'; (idx .* X)'], [1; view.mu; 0.05 * view.CVaR95]);
   %[tmp.p_, tmp.KLdiv] = optimizeEntropy(p, [idx'; (idx .* X)'], [0.05; 0.05 * view.CVaR95], [ones(1, J); X'], [1; view.mu]);
   p_ = [p_, tmp.p_];
   KLdiv = [KLdiv; tmp.KLdiv]; %#ok<*AGROW>

   % if change in KLdiv less than one percent, stop
   if abs((KLdiv(i) - KLdiv(i-1)) / KLdiv(i-1)) < 0.01
      doStop = 1;
   end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Display results
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure;
plot(X, p, 'b', 'LineWidth', 1.5);
hold on;
plot(X, p_(:, end), 'r', 'LineWidth', 1.5);
hold on; plot(market.mu, 0.0, 'o', 'Color', 'Blue', 'MarkerSize', 8, 'MarkerFaceColor', 'Blue');
hold on; plot(market.CVaR95, 0.0, '^', 'Color', 'Blue', 'MarkerSize', 8, 'MarkerFaceColor', 'Blue');
hold on; plot(view.mu, 0.0, 'o', 'Color', 'Red', 'MarkerSize', 8, 'MarkerFaceColor', 'Red');
hold on; plot(view.CVaR95, 0.0, '^', 'Color', 'Red', 'MarkerSize', 8, 'MarkerFaceColor', 'Red');
grid on;
xlabel('Returns [%]');
h = legend('Prior', 'Posterior');
set(h, 'FontSize', 8);
set(gca, 'YTickLabel', '');


function [On,Wr] = reorder_mod(W,M)
%REORDER_MOD         Reorder connectivity matrix by modular structure
%
%   On = reorder_mod(W,M);
%   [On Wr] = reorder_mod(W,M);
%
%   This function reorders the connectivity matrix by modular structure and
%   may consequently be useful in visualization of modular structure.
%
%   Inputs:
%       W,      connectivity matrix (binary/weighted undirected/directed)
%       M,      module affiliation vector
%
%   Outputs:
%       On,     new node order
%       Wr,     reordered connectivity matrix
%
%
%   Used in: Rubinov and Sporns (2011) NeuroImage; Zingg et al. (2014) Cell.
%
%
%   2011, Mika Rubinov, UNSW/U Cambridge

%   Modification History:
%   Mar 2011: Original
%	Jan 2015: Improved behavior for directed networks

%#ok<*ASGLU>
%#ok<*AGROW>

W = W+eps;
[u,dum,M] = unique(M);                                      %make consecutive;
n = numel(M);                                               %number of nodes
m = numel(u);                                               %number of modules

Nm=zeros(1,m);                                              %number of nodes in modules
Knm_o=zeros(n,m);                                           %node-to-module out-degree
Knm_i=zeros(n,m);                                           %node-to-module in-degree
for i=1:m
    Nm(i)=nnz(M==i);
    Knm_o(:,i)=sum(W(:,M==i),2);
    Knm_i(:,i)=sum(W(M==i,:),1);
end
Knm=(Knm_o+Knm_i)/2;

Wm=zeros(m);
for u=1:m
    for v=1:m
        Wm(u,v)=sum(sum(W(M==u,M==v)));
    end
end
Bm=(Wm+Wm.')./(2*(Nm.'*Nm));
% Km_o=sum(Wm,2);
% Km_i=sum(Wm,1);
% Bm_oi=Wm-Km_o*Km_i/sum(sum(Wm));
% Bm=(Bm_oi+Bm_oi.')/2;

%1. Arrange densely connected modules together
[I,J,bv]=find(tril(Bm,-1));                             	%symmetrized intermodular connectivity values
[~,ord]=sort(bv,'descend');                                 %sort by greatest relative connectivity
I=I(ord);
J=J(ord);
Om=[I(1) J(1)];                                             %new module order

Si=true(size(I));
Sj=true(size(J));
Si(I==I(1) | I==J(1))=0;
Sj(J==I(1) | J==J(1))=0;

while length(Om)<m                                          %while not all modules ordered
    for u=1:numel(I)
        if Si(u)     && any(J(u)==Om([1 end]))
            old=J(u);
            new=I(u);
            break
        elseif Sj(u) && any(I(u)==Om([1 end]))
            old=I(u);
            new=J(u);
            break
        end
    end
    if old==Om(1)
        Om=[new Om];
    elseif old==Om(end)
        Om=[Om new];
    end
    Si(I==new)=false;
    Sj(J==new)=false;
end

%2. Reorder nodes within modules
On = zeros(n,1,'uint64');                                   %node order array
for i=1:m
    u = Om(i);
    ind = find(M==u);                                       %indices

    mod_imp=[Om; sign((1:m)-i); abs((1:m)-i); Bm(u,Om)].';
    mod_imp=sortrows(mod_imp,[3 -4]);
    mod_imp=prod(mod_imp(2:end,[1 2]),2);

    [dum,ord]=sortrows(Knm(ind,:),mod_imp);                 %sort nodes by number of links to close modules
    On(ind(ord))=1e6*i+(1:Nm(u));                           %assign node order (assumes <1e6 nodes in a module)
end

[dum,On]=sort(On);                                          %reorder nodes
Wr=W(On,On);                                                %reorder matrix


function [varargout]=dihedralAngles(varargin)

% function [A]=dihedralAngles(E,V,elementType)
% -----------------------------------------------------------------------
% This function computes the dihedral angles A for the input elements
% defined by E, the element nodal connectivity matrix, and V, the node or
% vertex coordinate matrix. The output array A contains the same number of
% rows as E (a row for each element), and contains m columns where m is the
% number of edges for this type of element (e.g. 12 for a hexahedral element
% or 6 for a tetrahedral element).
%
%
% 2021/03/24 Created KMM
% -----------------------------------------------------------------------

%% PARSE INPUT

switch nargin
    case 2
        E=varargin{1};
        V=varargin{2};
        elementType=[];
    case 3
        E=varargin{1};
        V=varargin{2};
        elementType=varargin{3};
    otherwise
        error('Wrong number of inputs');
end

numNodes=size(E,2);

if isempty(elementType) %have to assume defaults
    switch numNodes
        case 8 %Hexahedral elements
            elementType='hex8';
        case 20 %Hexahedral elements
            elementType='hex20';
            numFaces=6;
        case 4 %Linear tets
            elementType='tet4';
        case 10 %Quadratic tets
            elementType='tet10';
            numFaces=4;
        case 6 %Quadratic triangles
            elementType='tri6';
        case 3
            elementType='tri3';
        case 14
            elementType='rhomdo14';
        otherwise
            elementType='unknown';
    end
end

switch elementType
    case 'tet4'
        numFaces=4;
        c  = [1 3; 3 2; 2 1;...
              1 4; 2 4; 3 4];

        ee = [3 2; 4 3; 1 3;...
              1 2; 4 1; 2 4];
    case 'hex8'
        numFaces=6;
        c=[1 3; 1 4; 1 5; 1 6; ...
           2 3; 2 4; 2 5; 2 6; ...
           3 5; 3 6; 4 5; 4 6; ...
           ];

        ee=[2 1; 4 3; 3 2; 1 4;...
            5 6; 7 8; 6 7; 8 5;...
            2 6; 5 1; 7 3; 4 8;...
            ];
end

%%

[F,~]=element2patch(E,[],elementType);
N=patchNormal(F,V);

FE=reshape(1:1:size(F,1),size(E,1),numFaces);
A=zeros(size(E,1),size(c,1));
for q=1:1:size(c,1)
    a=acos(dot(N(FE(:,c(q,1)),:),N(FE(:,c(q,2)),:),2));
    p1=V(E(:,ee(q,1)),:);
    p2=V(E(:,ee(q,2)),:);
    n=vecnormalize(p2-p1);
    x=cross(N(FE(:,c(q,1)),:),N(FE(:,c(q,2)),:),2);
    logic1=dot(n,x,2)>0;
    logicAngle=(a<pi & logic1);
    a(logicAngle)=pi-a(logicAngle);
    A(:,q)=a;
end

%% Collect output

varargout{1}=A;
if nargout>1
    %Create edge array
    ee=ee';
    ee=ee(:);
    EE=reshape(E(:,ee)',2,numel(ee)*size(E,1)/2)';
    varargout{2}=EE;
end
if nargout>2
    %Create angles for edge array
    AE=reshape(A',1,numel(A))';
    varargout{3}=AE;
end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function [ error_per_image ] = compute_diag_error( ground_truth_all, detected_points_all )
%compute_error
%   compute the average point-to-point Euclidean error normalized by the
%   diag bounding box distance (measured as the Euclidean distance between the
%   outer corners of the eyes)
%
%   Inputs:
%          grounth_truth_all, size: num_of_points x 2 x num_of_images
%          detected_points_all, size: num_of_points x 2 x num_of_images
%   Output:
%          error_per_image, size: num_of_images x 1

num_of_images = size(ground_truth_all,3);
num_of_points = size(ground_truth_all,1);

error_per_image = zeros(num_of_images,1);

for i =1:num_of_images
    detected_points      = detected_points_all(:,:,i);
    ground_truth_points  = ground_truth_all(:,:,i);

    diag_distance = norm([min(ground_truth_points(:,1)) min(ground_truth_points(:,2))]-[max(ground_truth_points(:,1)) max(ground_truth_points(:,2))]);

    sum=0;
    for j=1:num_of_points
        sum = sum+norm(detected_points(j,:)-ground_truth_points(j,:));
    end
    error_per_image(i) = sum/(num_of_points*diag_distance);
end

end


function[c]=materncfun(al,ga)
%MATERNCFUN Returns the normalization or C-function for a Matern process.
%
%   MATERNCFUN is a low-level function used in the JMATERN package.
%
%   MATERNCFUN(ALPHA) returns the Matern C-function defined in Eqn. (44) of
%   Lilly et al. (2017).
%
%   MATERNCFUN(ALPHA,GAMMA) returns C-function for the two-parameter
%   generalized Matern process.  See MATERNSPEC for details.
%
%   Usage: c=materncfun(alpha);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2015--2020 J.M. Lilly --- type 'help jlab_license' for details

%c=frac(1,2*pi)*frac(gamma(1/2)*gamma(alpha-1/2),gamma(alpha));
if nargin==1
    if al>1/2
        c=frac(1,2*sqrt(pi))*frac(gamma(al-1/2),gamma(al));
    else
        c=frac(2.^(abs(al-1/2)-abs(al)),sqrt(2*pi)).*...
            frac(gamma(abs(al-1/2)),gamma(abs(al)));
    end
else
    c=frac(1,2*pi*ga).*beta(frac(1,2*ga),frac(1,ga).*(al-1/2));
end
%Verified that these two forms are the same for al>1/2
%al1=[1/2:.01:10]';
%gamma1=[0:.01:10]';
%[al,gamma]=meshgrid(al1,gamma1);
%c=materncfun(al,gamma);
%jpcolor(al1,gamma1,log10(c)),caxis([-1 0])


% The vector corresponding to the second
% smallest eigenvalue of the Laplacian matrix
%
% INPUTs: adjacency matrix, nxn
% OUTPUTs: fiedler vector, nx1
%
% Other routines used: laplacianMatrix.m
% GB: last updated, Oct 10 2012

function fv=fiedlerVector(adj)

[V,D]=eig(laplacianMatrix(adj));
[~,Y]=sort(diag(D));
fv=V(:,Y(2));


% p(y_n) = N(y_n | C*x_n, R)
% p(x_n | x_(n-1)) = N(x_n | A*x_(n-1), Q)
%
% [x, Covx, logp] = kalman_filter_step(x, Covx, y, A, Q, C, R, 1)
%
% [x, Covx, logp] = kalman_filter_step(x, Covx, y, A, Q, C, inv(chol(R,'lower')), 2)

% Last modified 2011-10-19
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@aalto.fi)

function [x, Covx, logp] = kalman_filter_step(x, Covx, y, A, Q, C, R, method)

% Prediction step
x = A*x;
Covx = A*Covx*A' + Q;

% Update step
switch method
 case 1

  % Standard update step (if length(y) <= length(x))
  y = y - C*x;
  S = C*Covx*C' + R;
  L = chol(S, 'lower');
  K = linsolve_tril(L, C*Covx); %(Covx * C') / S;
  v = linsolve_tril(L, y);
  if nargout >= 3
    logp = gaussian_logpdf(v'*v, ...
                           0, ...
                           0, ...
                           logdet_chol(L), ...
                           length(y));
  end
  x = x + K'*v;
  Covx = Covx - K'*K; % K*C*Covx;

 case 2
  % Update step in lower-dimensional space of X

  invL_R = R;
  Lx = chol(Covx, 'lower');
  Z = invL_R * C * Lx;
  L = chol(speye(size(Lx)) + Z'*Z, 'lower');
  v_y = y - C*x;
  v_x = C'*(invL_R'*(invL_R*y)) + linsolve_lchol(Lx,x);
  Z = linsolve_tril(L, Lx');
  Covx = Z'*Z;
  x = Covx * v_x;

  if nargout >= 3
    v = invL_R' * (invL_R*v_y); % R \ (y - C*x)
    logp = gaussian_logpdf(v'*v_y - v'*C*Covx*C'*v, ...
                           0, ...
                           0, ...
                           logdet_chol(L) - logdet_chol(invL_R), ...
                           length(y));
  end

% $$$   invR = R;
% $$$   S = Covx * C';
% $$$   K = Covx + S*invR*S';
% $$$   L = chol(K, 'lower');
% $$$   Z = L \ Covx;
% $$$   if nargout >= 3
% $$$     % Compute marginal likelihood term
% $$$   end
% $$$   x = Z' * linsolve_tril(L, S*(invR*y) + x);
% $$$   Covx = Z'*Z;

end


% Description:
%
%     Enumerate N choose 2 combinations.
%
% Syntax:
%
%     c = mNC2(n)
%
% Inputs:
%
%     n - [ 1 x 1 ] (int)
%
% Outputs:
%
%     c - [ N x 2 ] (int)
%
% Details:
%
% Examples:
%
% Notes:
%
% Author(s):
%
%     William Gruner (williamgruner@gmail.com)
%
% References:
%
% Acknowledgements:
%
%     Many thanks to Dr. Erik Erhardt and Dr. Elena Allen of the Mind Research
%     Network (www.mrn.org) for their continued collaboration.
%
% Version:
%
%     $Author: williamgruner $
%     $Date: 2010-04-01 11:39:09 -0600 (Thu, 01 Apr 2010) $
%     $Revision: 482 $

function c = mNC2(n)

    c = [];

    for i = 1 : n
        for j = i + 1 : n
            c(end + 1, :) = [ i j ];
        end
    end


classdef logistic_regression
% This file defines logistic regression (binary classifier) problem class
%
% Inputs:
%       x_train     train data matrix of x of size dxn.
%       y_train     train data vector of y of size 1xn.
%       x_test      test data matrix of x of size dxn.
%       y_test      test data vector of y of size 1xn.
%       lambda      l2-regularized parameter.
% Output:
%       Problem     problem instance.
%
%
% The problem of interest is defined as
%
%           min f(w) = 1/n * sum_i^n f_i(w),
%           where
%           f_i(w) = log(1 + exp(-y_i' .* (w'*x_i))) + lambda/2 * w^2.
%
% "w" is the model parameter of size d vector.
%
%
% This file is part of GDLibrary and SGDLibrary.
%
% Created by H.Kasai on Feb. 17, 2016
% Modified by H.Kasai on March 23, 2018

    properties
        name;
        dim;
        samples;
        lambda;
        classes;
        hessain_w_independent;
        d;
        n_train;
        n_test;
        x_train;
        y_train;
        x_test;
        y_test;
        x_norm;
        x;
    end

    methods
        function obj = logistic_regression(x_train, y_train, x_test, y_test, varargin)

            obj.x_train = x_train;
            obj.y_train = y_train;
            obj.x_test = x_test;
            obj.y_test = y_test;

            if nargin < 5
                obj.lambda = 0.1;
            else
                obj.lambda = varargin{1};
            end

            obj.d = size(obj.x_train, 1);
            obj.n_train = length(y_train);
            obj.n_test = length(y_test);
            obj.name = 'logistic_regression';
            obj.dim = obj.d;
            obj.samples = obj.n_train;
            obj.lambda = obj.lambda;
            obj.classes = 2;
            obj.hessain_w_independent = false;
            obj.x_norm = sum(obj.x_train.^2,1);
            obj.x = obj.x_train;
        end

        function f = cost(obj, w)
            %f_old = sum(log(1+exp(-y_train.*(w'*x_train)))/n_train,2)+ lambda * (w'*w) / 2; %is replaced below
            % becasuse log(sigmoid(a)) = log(1/(1+exp(-a))) = log1 - log(1+exp(-a)) = -log(1+exp(-a)).
            %f = -sum(log(sigmoid(y_train.*(w'*x_train)))/n_train,2) + lambda * (w'*w) / 2;

    %         Commented out below due to avoid '-Inf' values of g by HK on 2017/12/5
    %         f = -sum(log(sigmoid(y_train.*(w'*x_train))),2)/n_train + lambda * (w'*w) / 2;

    %         The above is replaced with below by HK on 2017/12/5
            sigmod_result = sigmoid(obj.y_train.*(w'*obj.x_train));
            sigmod_result = sigmod_result + (sigmod_result<eps).*eps;
            f = -sum(log(sigmod_result),2)/obj.n_train + obj.lambda * (w'*w) / 2;
        end

        function f = cost_batch(obj, w, indices)

            f = -sum(log(sigmoid(obj.y_train(indices).*(w'*obj.x_train(:,indices))))/obj.n_train,2) + obj.lambda * (w'*w) / 2;

        end

        function g = grad(obj, w, indices)

            % org code
            %g = -sum(((ones(obj.d,1) * (sigmoid(-obj.y_train(indices).*(w'*obj.x_train(:,indices))) .* obj.y_train(indices))) .* obj.x_train(:,indices))/length(indices),2) + obj.lambda * w;
            %
            % (log(1+exp(-y_train.*(w'*x_train)))' = -y_train.*x_train * (exp(-y_train.*(w'*x_train))/(1+exp(-y_train.*(w'*x_train))))
            %   = -y_train.*x_train * (1 - sigmoid(y_train.*(w'*x_train)))
            % or
            % (log(sigmoid(y_train.*(w'*x_train)))' = y_train.*x_train * 1/sigmoid(y_train.*(w'*x_train)) * (sigmoid(y_train.*(w'*x_train)))'
            %   = y_train.*x_train * 1/sigmoid(y_train.*(w'*x_train)) * (- sigmoid(y_train.*(w'*x_train)) * (1 - sigmoid(y_train.*(w'*x_train))))
            %   = -y_train.*x_train * (1 - sigmoid(y_train.*(w'*x_train)))

    %         Replace the commented-out lines with below (although it is a bit slower) due to avoid 'NAN' values of g by HK on 2017/12/5
            g = -sum(ones(obj.d,1) * obj.y_train(indices).*obj.x_train(:,indices) * (ones(1,length(indices))-sigmoid(obj.y_train(indices).*(w'*obj.x_train(:,indices))))',2)/length(indices)+ obj.lambda * w;

    %         Commented out below due to avoid 'NAN' values of g by HK on 2017/12/5
%             e = exp(-1*obj.y_train(indices)'.*(obj.x_train(:,indices)'*w));
%             s = e./(1+e);
%             g = -(1/length(indices))*((s.*obj.y_train(indices)')'*obj.x_train(:,indices)')';
%             g = full(g) + obj.lambda * w;

        end

        function g = full_grad(obj, w)

            %g = -sum(ones(d,1) * y_train.*x_train * (ones(1,n_train)-sigmoid(y_train.*(w'*x_train)))',2)/n_train+ lambda * w;
            g = grad(obj, w, 1:obj.n_train);
        end

        function g = ind_grad(obj, w, indices)

            g = -ones(obj.d,1) * obj.y_train(indices).*obj.x_train(:,indices) * diag(ones(1,length(indices))-sigmoid(obj.y_train(indices).*(w'*obj.x_train(:,indices))))+ obj.lambda* repmat(w, [1 length(indices)]);

        end

        function h = hess(obj, w, indices)

            %org code
            %temp = exp(-1*(y_train(indices)').*(x_train(:,indices)'*w));
            %b = temp ./ (1+temp);
            %h = 1/length(indices)*x_train(:,indices)*(diag(b-b.^2)*(x_train(:,indices)'))+lambda*eye(d);

            sigm_val = sigmoid(obj.y_train(indices).*(w'*obj.x_train(:,indices)));
            c = sigm_val .* (ones(1,length(indices))-sigm_val);
            h = 1/length(indices)* obj.x_train(:,indices) * diag(obj.y_train(indices).^2 .* c) * obj.x_train(:,indices)'+obj.lambda*eye(obj.d);

        end

        function h = full_hess(obj, w)

            h = hess(obj, w, 1:obj.n_train);

        end

        function hv = hess_vec(obj, w, v, indices)

            sigm_val = sigmoid(obj.y_train(indices).*(w'*obj.x_train(:,indices)));
            c = sigm_val .* (ones(1,length(indices))-sigm_val);
            hv = 1/length(indices)* obj.x_train(:,indices) * diag(obj.y_train(indices).^2 .* c) * (obj.x_train(:,indices)' * v) +obj.lambda*v;

        end

        function p = prediction(obj, w)

            p = sigmoid(w' * obj.x_test);

            class1_idx = p>0.5;
            class2_idx = p<=0.5;
            p(class1_idx) = 1;
            p(class2_idx) = -1;

        end

        function a = accuracy(obj, y_pred)

            a = sum(y_pred == obj.y_test) / obj.n_test;

        end

        function w_opt = calc_solution(obj, maxiter, method)

            if nargin < 3
                method = 'lbfgs';
            end

            options.max_iter = maxiter;
            options.verbose = true;
            options.tol_optgap = 1.0e-24;
            options.tol_gnorm = 1.0e-16;
            options.step_alg = 'backtracking';

            if strcmp(method, 'sd')
                [w_opt,~] = sd(obj, options);
            elseif strcmp(method, 'cg')
                [w_opt,~] = ncg(obj, options);
            elseif strcmp(method, 'newton')
                options.sub_mode = 'INEXACT';
                options.step_alg = 'non-backtracking';
                [w_opt,~] = newton(obj, options);
            else
                options.step_alg = 'backtracking';
                options.mem_size = 5;
                [w_opt,~] = lbfgs(obj, options);
            end
        end

        %% for NIM
        function [labels, samples] = get_partial_samples(obj, indices)
            samples = obj.x_train(:,indices);
            labels  = obj.y_train(indices);
        end

        function [s] = phi_prime(obj, w, indices)
            e = exp(-1.0 * obj.y_train(indices)' .* (obj.x_train(:,indices)'*w));
            s = e ./ (1.0+e);
        end

        function [ss] = phi_double_prime(obj, w, indices)
            e = exp(-1.0 * obj.y_train(indices)' .* (obj.x_train(:,indices)'*w));
            s = e ./ (1.0+e);
            ss = s .* (1.0 - s);
        end

        %% for Sub-sampled Newton
        function h = diag_based_hess(obj, w, indices, square_hess_diag)
            X = obj.x_train(:,indices)';
            h = X' * diag(square_hess_diag) * X / length(indices) + obj.lambda * eye(obj.d);
        end

        function square_hess_diag = calc_square_hess_diag(obj, w, indices)
            %hess_diag = 1./(1+exp(Y.*(X*w)))./(1+exp(-Y.*(X*w)));
            Xw = obj.x_train(:,indices)'*w;
            y = obj.y_train(indices)';
            yXw = y .* Xw;
            square_hess_diag = 1./(1+exp(yXw))./(1+exp(-yXw));
        end

    end
end


% This function computes the entropy of a label set.

% Copyright (C) 2012 Quan Wang <wangq10@rpi.edu>,
% Signal Analysis and Machine Perception Laboratory,
% Department of Electrical, Computer, and Systems Engineering,
% Rensselaer Polytechnic Institute, Troy, NY 12180, USA
%
% You are free to use this software for academic purposes if you cite our paper:
% Q. Wang, Y. Ou, A.A. Julius, K.L. Boyer, M.J. Kim,
% Tracking tetrahymena pyriformis cells using decision trees,
% in: 2012 International Conference on Pattern Recognition, Tsukuba Science City, Japan.
%
% For commercial use, please contact the authors.

function label_ent=getEntropy(label)

% label: one label vector
% label_ent: resulting entropy

n1=sum(1-label);
n2=sum(label);
if n1+n2==0
    label_ent=0;
    return;
end
p1=n1/(n1+n2);
p2=n2/(n1+n2);
if p1==0
    label_ent=-p2*log(p2);
    return;
elseif p2==0
    label_ent=-p1*log(p1);
    return;
end
label_ent=-p1*log(p1)-p2*log(p2);


function x=v_randvec(n,m,c,w,mode)
%V_RANDVEC  Generate real or complex GMM/lognormal random vectors X=(N,M,C,W,MODE)
% generates a random matrix of size (|n|,p) where p is the maximum
% dimension of M or C (see note below about row versus column vectors)
%  Inputs:  N        is the number of points to generate
%           M(K,P)   is the mean vectors (one row per mixture)
%           C(K,P)   are diagonal covariances (one row per mixture)
%        or C(P,P,K) are full covariance matrices (one per mixture)
%           W(K)     are the mixture weights (or omit if all mixtures have equal weight)
%           MODE     character string specifying options:
%                       g = real gaussian [default]
%                       c = complex gaussian
%                       l = lognormal
%
% Outputs:  X(N,P) is the output data
%
% Note this routine generates row vectors such that E((x-m)'*(x-m)) = C = cov(x). If
% Alternatively x=(n,m',c,w,mode)' will generate column vectors satisfying E((x-m)*(x-m)') = C = cov(x').

% Bugs/suggestions
%    (1)  New mode 'x' to approximate chi squared

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

% first sort out the input arguments

sm=size(m);
if nargin<3
    c=ones(sm);         % default to unit variance
end
sc=size(c);
p=max(sm(2),sc(2));     % data dimension
k=sm(1);                % number of mixtures
fullc=(length(sc)>2) || (sc(1)>k);
if nargin<4
    mode='g';   % default to gaussian
    w=ones(k,1);
else
    if ischar(w)
        mode = w;       % w argument has been omitted
        w=ones(k,1);
    elseif nargin<5
        mode='g';
    end
end
ty=mode(1);   % ignore all but first character for type
x=zeros(n,p);   % initialize output array
if sm(2)~=p
    m=repmat(m,1,p);    % if m is given as a scalar
end
if sc(2) ~=p
    c=repmat(c,1,p);    % if c is given as a scalar
end
q=sqrt(0.5);
if k>1
    kx=v_randiscr(w,n);
else
    kx=ones(n,1);
end
for kk=1:k
    nx=find(kx==kk);
    nn=length(nx);
    if nn       % check if we need to generate any from mixture kk

        % extract the mean and cov for this mixture

        mm=m(kk,:);     % mean vector
        if fullc        % full covariance matrix
            cc=c(:,:,kk);
            if ty=='l'      % lognormal distribution - convert mean and covariance
                cc=log(1+cc./(mm.'*mm));
                mm=log(mm)-0.5*diag(cc).';
            end
        else
            cc=c(kk,:);
            if ty=='l'      % lognormal distribution - convert mean and covariance
                cc=log(1+cc(:).'./mm.^2);
                mm=log(mm)-0.5*cc;
            end
        end

        % now generate nn complex or real values

        if ty=='c'  % generate complex values
            xx=q*randn(nn,p)+1i*q*randn(nn,p); % complex-valued unit variance values
        else
            xx=randn(nn,p);   % real-valued unit variance values
        end;

        % scale by the square root of the covariance matrix

        if fullc   % full covariance covariance
            [v,d]=eig((cc+cc')/2);   % force covariance matrix to be hermitian
            xx=(xx.*repmat(sqrt(abs(diag(d))).',nn,1))*v'+repmat(mm,nn,1); % and also positive definite
        else
            xx=xx.*repmat(sqrt(abs(cc)),nn,1)+repmat(mm,nn,1); % different mean/cov for each column
        end
        x(nx,:)=xx;
    end
end
if ty=='l'  % lognormal distribution
    x=exp(x);
end
if ~nargout
    if p==1
        if ty=='c'
            plot(real(x), imag(x),'+');
            xlabel('Real');
            ylabel('Imag');
        else
            nbin=max(min(floor(sqrt(n)),50),5);
            hist(x,nbin);
            xlabel('X');
            ylabel('Frequency');
        end
    else
        [vv,iv]=sort(var(x,0,1));
        iv=sort(iv(end-1:end));
        plot(real(x(:,iv(1))), real(x(:,iv(2))),'+');
        if ty=='c'
            xylab='Real[ x(%d) ]';
        else
            xylab='x(%d)';
        end
        xlabel(sprintf(xylab,iv(1)));
        ylabel(sprintf(xylab,iv(2)));
    end
end


%%
f = imread('Fig0208(a).tif');
imshow(f);
figure,imhist(f);  %  显示直方图
ylim('auto')
%%
g = histeq(f,256); %  直方图均衡
figure,imshow(g);
figure,imhist(g);
ylim('auto')