function [suspicious_index suspicious_score u] = OD_onlinePCA(A, beta)
%
% Outlier detection via over-sampling online PCA
%
% This function is used for outlier detection. The main idea is using
% the variation of the first principal direction detect the outlierness
% of each instance(event) in the leave one out procedure. Here the
% over-sampling on target instance is also used for enlarge the
% outlierness
%
%
% input
%   A: the data matrix, each row represents an instance
%   beta: forgetting factor
%         For example, beta=0.9 means we decrease the influence of previous
%         data by a factor of 0.9
% output
%   suspicious_score: the suspicious score for each instance
%   suspicious_index: the ranking of instances according to their
%                     suspicious score
%                     For example, suspicious_index(i)=j means the ith
%                     instance is in jth position in the ranking.
%
% References
% Anomaly Detection via Over-Sampling Principal Component Analysis
% Send your comment and inquiry to Dr. Yi-Ren Yeh (yirenyeh@gmail.com)
%

[n p]= size(A);

A_m = mean(A);
d = 0.0001;
u = ones(p,1);
for i = 1:n
    [u d] = Track_w(A(i,:)-A_m, u, d, 1);
end
u = u/norm(u);

sim_pool = zeros(n,1);
ratio = 1/(n*beta);
for i = 1:n
    temp_mu = (A_m+ratio*A(i,:))/(1+ratio);
    x =A(i,:)-temp_mu;
    [w] = Track_w(x, u, d, beta);
    w = w/norm(w);
    sim_pool(i,:) = abs(diag(u'*w));
    if (~mod(i,10000))
        display(['Iteration ',num2str(i)])
    end
end

[non,suspicious_index]=sort(sim_pool);
suspicious_score = 1-sim_pool;

%==========================================================================
function [w d] = Track_w(x, w, d, beta)
y = x*w;
d = beta*d+y^2;
e = x'-w*y;
w = w + (y*e)/d;
% function [w d] = Track_w(x, w, d, n, ratio)
% y = x*w;
% d = (1/n)*d+ratio*y^2;
% e = x'-w*y;
% w = w + (y*e)/d;


function value = r8_cosd ( degrees )

%*****************************************************************************80
%
%% R8_COSD returns the cosine of an angle given in degrees.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 July 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real DEGREES, the angle in degrees.
%
%    Output, real VALUE, the cosine of the angle.
%
  radians = pi * ( degrees / 180.0 );

  value = cos ( radians );

  return
end


function area = polygonArea3d(poly, varargin)
%POLYGONAREA3D Area of a 3D polygon.
%
%   AREA = polygonArea3d(POLY)
%   POLY is given as a N-by-3 array of vertex coordinates. The resulting
%   area is positive.
%   Works also for polygons given as a cell array of polygons.
%
%   Example
%     % area of a simple 3D square
%     poly = [10 30 20;20 30 20;20 40 20;10 40 20];
%     polygonArea3d(poly)
%     ans =
%        100
%
%     % Area of a 3D mesh
%     [v f] = createCubeOctahedron;
%     polygons = meshFacePolygons(v, f);
%     areas = polygonArea3d(polygons);
%     sum(areas)
%     ans =
%         18.9282
%
%   See also
%     polygons3d, triangleArea3d, polygonArea, polygonCentroid3d

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2012-02-24,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2012 INRA - Cepia Software Platform.

%   HISTORY
%   2013-08-20 add support for multiple polygons

% Check multiple polygons
if iscell(poly) || sum(sum(isnan(poly))) > 0
    % split the polygons into a cell array
    polygons = splitPolygons3d(poly);
    nPolys = length(polygons);

    % compute area of each polygon
    area = zeros(nPolys, 1);
    for  i = 1:nPolys
        area(i) = polygonArea3d(polygons{i});
    end

    return;
end

% put the first vertex at origin (reducing computation errors for polygons
% far from origin)
v0 = poly(1, :);
poly = bsxfun(@minus, poly, v0);

% indices of next vertices
N = size(poly, 1);
iNext = [2:N 1];

% compute cross-product of each elementary triangle formed by origin and
% two consecutive vertices
cp = cross(poly, poly(iNext,:), 2);

% choose one of the triangles as reference for the normal direction
vn = vectorNorm3d(cp);
[tmp, ind] = max(vn); %#ok<ASGLU>
cpRef = cp(ind,:);

% compute the sign of the area of each triangle
% (need to compute the sign explicitely, as the norm of the cross product
% does not keep orientation within supporting plane)
sign_i = sign(dot(cp, repmat(cpRef, N, 1), 2));

% compute area of each triangle, using sign correction
area_i = vectorNorm3d(cp) .* sign_i;

% sum up individual triangles area
area = sum(area_i) / 2;


%TRNORM Normalize an SO(3) or SE(3) matrix
%
% TRNORM(R) is guaranteed to be a proper orthogonal matrix rotation
% matrix (3x3) which is "close" to the input matrix R (3x3). If R
% = [N,O,A] the O and A vectors are made unit length and the normal vector
% is formed from N = O x A, and then we ensure that O and A are orthogonal
% by O = A x N.
%
% TRNORM(T) as above but the rotational submatrix of the homogeneous
% transformation T (4x4) is normalised while the translational part is
% unchanged.
%
% If R (3x3xK) or T (4x4xK) representing a sequence then the normalisation
% is performed on each of the K planes.
%
% Notes::
% - Only the direction of A (the z-axis) is unchanged.
% - Used to prevent finite word length arithmetic causing transforms to
%   become `unnormalized'.
% - There is no Toolbox function for SO(2) or SE(2).
%
% See also OA2TR, SO3.trnorm, SE3.trnorm.

%## 3d homogeneous

% 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 TR = trnorm(T)

    assert(ishomog(T) || isrot(T), 'SMTB:trnorm:badarg', 'expecting 3x3xN or 4x4xN hom xform');

    if ndims(T) == 3
        % recurse for transform sequence
        n = size(T);
        TR = zeros(n);
        for i=1:n(3)
            TR(:,:,i) = trnorm(T(:,:,i));
        end
        return
    end

    o = T(1:3,2); a = T(1:3,3);
    n = cross(o, a);         % N = O x A
    o = cross(a, n);         % O = A x N
    R = [unit(n) unit(o) unit(a)];

    if ishomog(T)
        TR = rt2tr( R, T(1:3,4) );
    elseif isrot(T)
        TR = R;
    end


% RMSE - Evaluates root mean squared error.
%
% E = RMSE(Y)
%
% E = RMSE(Y1,Y2)

% Last modified 2010-10-21
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function e = rmse(Y, Yh)

if nargin < 2
  e = sqrt(Y(:)'*Y(:)/numel(Y));
else
  z = Y(:)-Yh(:);
  e = sqrt(z'*z/numel(z));
end


function line = fitLine3d(points)
%FITLINE3D Fit a 3D line to a set of points.
%
%   LINE = fitLine3d(PTS)
%
%   Example
%     pts = randn(300, 3);
%     pts = transformPoint3d(pts, createScaling3d([6 4 2]));
%     pts = transformPoint3d(pts, createRotationOx(pi/6));
%     pts = transformPoint3d(pts, createRotationOy(pi/4));
%     pts = transformPoint3d(pts, createRotationOz(pi/3));
%     pts = transformPoint3d(pts, createTranslation3d([5 4 3]));
%     elli = inertiaEllipsoid(pts);
%     figure; drawPoint3d(pts); axis equal;
%     hold on; drawEllipsoid(elli, ...
%         'drawEllipses', true, 'EllipseColor', 'b', 'EllipseWidth', 3);
%     line = fitLine3d(pts);
%     drawLine3d(line, 'color', 'm', 'LineWidth', 4);
%
%   See also
%     lines3d, inertiaEllipsoid, fitPlane
%

% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2012-11-11,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2012 INRA - Cepia Software Platform.

% number of points
n = size(points, 1);

% compute centroid
center = mean(points);

% compute the covariance matrix
covPts = cov(points)/n;

% perform a principal component analysis with 2 variables,
% to extract inertia axes
[U, S] = svd(covPts);

% sort axes from greater to lower
[dummy, ind] = sort(diag(S), 'descend'); %#ok<ASGLU>

% format U to ensure first axis points to positive x direction
U = U(ind, :);
if U(1,1) < 0
    U = -U;
    % keep matrix determinant positive
    U(:,3) = -U(:,3);
end

line = [center U(:,1)'];


function qoi_quad ( )

%*****************************************************************************80
%
%% QOI_QUAD seeks the expected value of a quantity of interest.
%
%  Discussion:
%
%    Our base value for DELTA_BASE is 0.01.
%
%    Our quantity of interest Q is the time at which the solution achieves
%    the value 0.99.
%
%    Our parameter U is uniformly distributed in [-1,+1], and determines
%    our actual value of DELTA by
%
%      DELTA = 2^U * DELTA_BASE.
%
%    We seek the expected value of Q, which we define as:
%
%      E(Q) = Integral ( -1 <= U <= +1 ) Q(Y(DELTA(U))) rho(U) dU
%
%    where
%
%      rho(U) = 1/2;
%      DELTA(U) = 2^U * DELTA_BASE;
%      Y(DELTA()) is the solution of the combustion ODE for an initial
%        condition of DELTA;
%      Q(Y) is the (first) time T* at which the solution Y(T*) = 0.99.
%
%    We approximate the integral using a Clenshaw-Curtis quadrature rule.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 February 2013
%
%  Author:
%
%    John Burkardt
%
  delta_base = 0.01;
  n_max = 12;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'QOI_QUAD\n' );
  fprintf ( 1, '  Use quadrature to estimate the expected value of our\n' );
  fprintf ( 1, '  quantity of interest Q, the time at which the\n' );
  fprintf ( 1, '  combustion solution reaches 0.99.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Using Clenshaw-Curtis quadrature of orders 1 through %d\n', n_max );
%
%  Set up an option to have ODE45 return when the solution reached 0.99.
%
  options = odeset ( 'EVENTS', @event_function );

  fprintf ( 1, '\n' );
  fprintf ( 1, '   N      Estimated Q\n' );
  fprintf ( 1, '\n' );

  for n = 1 : n_max

    [ x, w ] = clenshaw_curtis_compute ( n );

    f = zeros ( n, 1 );

    for j = 1 : n
%
%  Uniformly select
%    U, the logarithm base 2, between -1 and +1, of
%    F, a factor between 1/2 and 2, which multiplies
%    DELTA_BASE, the base value, which gives us
%    DELTA, the initial size of the flame.
%
      u = x(j);
      factor = 2^u;
      delta = factor * delta_base;
%
%  Set the starting point.
%
      t_start = 0.0;
      y_start(1,1) = delta;
%
%  Get the stopping point.
%
      t_stop = 250.0;
%
%  Call ODE45 to solve the problem, stopping immediately if the event
%  (y=0.99) is observed.
%
      t_span = [ t_start, t_stop ];

      [ t, y ] = ode45 ( @flame_fun, t_span, y_start, options );
%
%  Our function must be weighted by rho(u), which is uniformly 1/2.
%
      f(j) = 0.5 * t(end);

    end
%
%  Compute the integral estimate.
%
    q = w' * f;

    fprintf ( 1, '  %2d  %12g\n', n, q );

  end
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'QOI_QUAD\n' );
  fprintf ( 1, '  Normal end of execution\n' );

  return
end


%% RATE OF CONVERGENCE OF MIXED FINITE ELEMENT METHOD (RT0-P0) FOR POISSON EQUATION
%
% This example is to show the rate of convergence of RT0-P0 mixed finite
% element approximation of the Poisson equation on the unit square with the
% following boundary conditions:
%
% - pure Dirichlet boundary condition.
% - Pure Neumann boundary condition.
% - mxied boundary condition.
%
% The basis, data structure and numerical test is summarized in <a
% href="matlab:ifem Poisson3RT0femrate">Poisson3RT0femrate</a>.
%
% The optimal rates of convergence for u and sigma are observed, namely,
% 1st order for L2 norm of u, L2 norm of sigma and H(div) norm of sigma.
% The 2nd order convergent rates between two discrete functions ||uI-uh||
% and ||sigmaI-sigmah|| are known as superconvergence.
%
% Triangular preconditioned GMRES (default solver) and Uzawa preconditioned
% CG converges uniformly in all cases. Traingular preconditioner is two
% times faster than PCG although GMRES is used.
% See also PoissonBDM1mfemrate, Poissonfemrate, Poissonafemrate
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Setting
[node,elem] = cubemesh([0,1,0,1,0,1],0.5);
mesh = struct('node',node,'elem',elem);
option.L0 = 1;
option.maxIt = 3;
option.printlevel = 1;
option.elemType = 'RT0';
% pde = mixedPossiondata;

%% Dirichelt for u and Neumann boundary condition for sigma
option.solver = 'uzawapcg';
pde = sincosdata3;
mesh.bdFlag = setboundary3(node,elem,'Dirichlet');
mfemPoisson3(mesh,pde,option);

%% Neumann for u and Dirichlet boundary condition for sigma.
option.solver = 'tri';
pde = sincosdata3;
mesh.bdFlag = setboundary3(node,elem,'Neumann');
mfemPoisson3(mesh,pde,option);


function [x,N] = makesig(SigName,N)
% [x,N] = makesig(SigName,N) Creates artificial test signal identical to the
%     standard test signals proposed and used by D. Donoho and I. Johnstone
%     in WaveLab (- a matlab toolbox developed by Donoho et al. the statistics
%     department at Stanford University).
%
%    Input:  SigName - Name of the desired signal (Default 'all')
%                        'AllSig' (Returns a matrix with all the signals)
%                        'HeaviSine'
%                        'Bumps'
%                        'Blocks'
%                        'Doppler'
%                        'Ramp'
%                        'Cusp'
%                        'Sing'
%                        'HiSine'
%                        'LoSine'
%                        'LinChirp'
%                        'TwoChirp'
%                        'QuadChirp'
%                        'MishMash'
%                        'WernerSorrows' (Heisenberg)
%                        'Leopold' (Kronecker)
%            N       - Length in samples of the desired signal (Default 512)
%
%    Output: x   - vector/matrix of test signals
%            N   - length of signal returned
%
%    See also:
%
%    References:
%            WaveLab can be accessed at
%            www_url: http://playfair.stanford.edu/~wavelab/
%            Also see various articles by D.L. Donoho et al. at
%            web_url: http://playfair.stanford.edu/
%
%Author: Jan Erik Odegard  <odegard@ece.rice.edu>
%This m-file is a copy of the  code provided with WaveLab
%customized to be consistent with RWT.

if(nargin < 1)
  SigName = 'AllSig';
  N = 512;
elseif(nargin == 1)
  N = 512;
end;
t = (1:N) ./N;
x = [];
y = [];
if(strcmp(SigName,'HeaviSine') | strcmp(SigName,'AllSig')),
  y = 4.*sin(4*pi.*t);
  y = y - sign(t - .3) - sign(.72 - t);
end;
x = [x;y];
y = [];
if(strcmp(SigName,'Bumps') | strcmp(SigName,'AllSig')),
  pos = [ .1 .13 .15 .23 .25 .40 .44 .65  .76 .78 .81];
  hgt = [ 4  5   3   4  5  4.2 2.1 4.3  3.1 5.1 4.2];
  wth = [.005 .005 .006 .01 .01 .03 .01 .01  .005 .008 .005];
  y = zeros(size(t));
  for j =1:length(pos)
    y = y + hgt(j)./( 1 + abs((t - pos(j))./wth(j))).^4;
  end
end;
x = [x;y];
y = [];
if(strcmp(SigName,'Blocks') | strcmp(SigName,'AllSig')),
  pos = [ .1 .13 .15 .23 .25 .40 .44 .65  .76 .78 .81];
  hgt = [4 (-5) 3 (-4) 5 (-4.2) 2.1 4.3  (-3.1) 2.1 (-4.2)];
  y = zeros(size(t));
  for j=1:length(pos)
    y = y + (1 + sign(t-pos(j))).*(hgt(j)/2) ;
  end
end;
x = [x;y];
y = [];
if(strcmp(SigName,'Doppler') | strcmp(SigName,'AllSig')),
  y = sqrt(t.*(1-t)).*sin((2*pi*1.05) ./(t+.05));
end;
x = [x;y];
y = [];
if(strcmp(SigName,'Ramp') | strcmp(SigName,'AllSig')),
  y = t - (t >= .37);
end;
x = [x;y];
y = [];
if(strcmp(SigName,'Cusp') | strcmp(SigName,'AllSig')),
  y = sqrt(abs(t - .37));
end;
x = [x;y];
y = [];
if(strcmp(SigName,'Sing') | strcmp(SigName,'AllSig')),
  k = floor(N * .37);
  y = 1 ./abs(t - (k+.5)/N);
end;
x = [x;y];
y = [];
if(strcmp(SigName,'HiSine') | strcmp(SigName,'AllSig')),
  y = sin( pi * (N * .6902) .* t);
end;
x = [x;y];
y = [];
if(strcmp(SigName,'LoSine') | strcmp(SigName,'AllSig')),
  y = sin( pi * (N * .3333) .* t);
end;
x = [x;y];
y = [];
if(strcmp(SigName,'LinChirp') | strcmp(SigName,'AllSig')),
  y = sin(pi .* t .* ((N .* .125) .* t));
end;
x = [x;y];
y = [];
if(strcmp(SigName,'TwoChirp') | strcmp(SigName,'AllSig')),
  y = sin(pi .* t .* (N .* t)) + sin((pi/3) .* t .* (N .* t));
end;
x = [x;y];
y = [];
if(strcmp(SigName,'QuadChirp') | strcmp(SigName,'AllSig')),
  y = sin( (pi/3) .* t .* (N .* t.^2));
end;
x = [x;y];
y = [];
if(strcmp(SigName,'MishMash') | strcmp(SigName,'AllSig')),
  % QuadChirp + LinChirp + HiSine
  y = sin( (pi/3) .* t .* (N .* t.^2)) ;
  y = y +  sin( pi * (N * .6902) .* t);
  y = y +  sin(pi .* t .* (N .* .125 .* t));
end;
x = [x;y];
y = [];
if(strcmp(SigName,'WernerSorrows') | strcmp(SigName,'AllSig')),
  y = sin( pi .* t .* (N/2 .* t.^2)) ;
  y = y +  sin( pi * (N * .6902) .* t);
  y = y +  sin(pi .* t .* (N .* t));
  pos = [ .1 .13 .15 .23 .25 .40 .44 .65  .76 .78 .81];
  hgt = [ 4  5   3   4  5  4.2 2.1 4.3  3.1 5.1 4.2];
  wth = [.005 .005 .006 .01 .01 .03 .01 .01  .005 .008 .005];
  for j =1:length(pos)
    y = y + hgt(j)./( 1 + abs((t - pos(j))./wth(j))).^4;
  end
end;
x = [x;y];
y = [];
if(strcmp(SigName,'Leopold') | strcmp(SigName,'AllSig')),
  y = (t == floor(.37 * N)/N); 		% Kronecker
end;
x = [x;y];
y = [];

%  disp(sprintf('MakeSignal: I don*t recognize << %s>>',SigName))
%  disp('Allowable SigNames are:')
%  disp('AllSig'),
%  disp('HeaviSine'),
%  disp('Bumps'),
%  disp('Blocks'),
%  disp('Doppler'),
%  disp('Ramp'),
%  disp('Cusp'),
%  disp('Crease'),
%  disp('Sing'),
%  disp('HiSine'),
%  disp('LoSine'),
%  disp('LinChirp'),
%  disp('TwoChirp'),
%  disp('QuadChirp'),
%  disp('MishMash'),
%  disp('WernerSorrows'),
%  disp('Leopold'),
%end


function [ SimilarityMatrix ] = similarity_neighbor_grid_further( x, side, types, dist)
%similarity_neighbor_grid_further Summary of this function goes here

    % this assumes that the patch is laid out with first column, then second
    % column, ... final column (column major)

%     dist = 2;
    SimilarityMatrix = eye(side*side);

    % types - 1 - horizontal, 2 - vertical, 3 - diagonal (bl-tr), 4 -
    % diagonal (br - tl)
    for t=1:numel(types)

        if(types(t) == 1)

            % for horizontal we want to link both neighbours
            % (which are offset from the points by height)
            i = 1:(side*side-side*dist);
            % create the neighboring links for i
            SimilarityMatrix(sub2ind([side^2, side^2], i, i+side*dist)) = 1;
            SimilarityMatrix(sub2ind([side^2, side^2], i+side*dist, i)) = 1;

        end
        if(types(t) == 2)

            % for vertical we want to link both neighbours except at edge
            % cases which are mod(y_loc,side) = 0 as they are at the edges
            i = 1:side*side;
            i_to_rem =[];
            for s=1:dist
                i_to_rem = union(i_to_rem, i(mod(i+s-1, side) == 0));
            end
            i_both = setdiff(i, i_to_rem);
            % create the neighboring links for i
            SimilarityMatrix(sub2ind([side^2, side^2], i_both+dist, i_both)) = 1;
            SimilarityMatrix(sub2ind([side^2, side^2], i_both, i_both+dist)) = 1;

        end
        if(types(t) == 3)

            % for diagonal to top right, and bottom left don't use right most column
            i = 1:(side^2)-dist * side;
            i_to_rem = [];
            for s=1:dist
                i_to_rem = union(i_to_rem,i(mod(i-s, side) == 0));
            end
            i_both = setdiff(i, i_to_rem);
            % create the neighboring links for i
            SimilarityMatrix(sub2ind([side^2, side^2], i_both+dist*side-dist, i_both)) = 1;
            SimilarityMatrix(sub2ind([side^2, side^2], i_both, i_both+dist*side-dist)) = 1;
        end
        if(types(t) == 4)

            % for diagonal to top left, and bottom right don't use right most column
            i = 1:(side^2)-dist*side;
            i_to_rem = [];
            for s=1:dist
                i_to_rem = union(i_to_rem, i(mod(i+s-1, side) == 0));
            end
            i_both = setdiff(i, i_to_rem);
            % create the neighboring links for i
            SimilarityMatrix(sub2ind([side^2, side^2], i_both+dist*side+dist, i_both)) = 1;
            SimilarityMatrix(sub2ind([side^2, side^2], i_both, i_both+dist*side+ dist)) = 1;

        end

    end
    assert(isequal(SimilarityMatrix, SimilarityMatrix'));
end


function [node,face,elem]=meshunitsphere(tsize,maxvol)
%
% [node,face,elem]=meshunitsphere(tsize,maxvol)
%
% create the surface and/or volumetric mesh of a unit sphere
% centered at [0 0 0] and radius 1
%
% author: Qianqian Fang, <q.fang at neu.edu>
%
% input:
%   tsize: maximum size of the surface triangles (from 0 to 1)
%   maxvol: maximum volume of the tetrahedron; if one wants to return
%           elem without specifying maxvol, maxvol=tsize^3
%
% output:
%   node: node coordinates, 3 columns for x, y and z respectively
%   face: integer array with dimensions of NB x 3, each row represents
%         a surface mesh face element
%   elem: integer array with dimensions of NE x 4, each row represents
%         a tetrahedron. If ignored, this function only produces the surface
%
% example:
%   [node,face]=meshunitsphere(0.05);
%   [node,face,elem]=meshunitsphere(0.05,0.01);
%   plotmesh(node,elem,'x>0'); axis equal;
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

dim=60;
esize=tsize*dim;
thresh=dim/2-1;
[xi,yi,zi]=meshgrid(0:0.5:dim,0:0.5:dim,0:0.5:dim);
dist=thresh-sqrt((xi-30).^2+(yi-30).^2+(zi-30).^2);
dist(dist<0)=0;
clear xi yi zi;

% extract a level-set at v=thresh, being a sphere with R=thresh
% the maximum element size of the surface triangles is tsize*dim

[node,face]=vol2restrictedtri(dist,1,[dim dim dim],dim*dim*dim,30,esize,esize,40000);
node=(node-0.5)*0.5;

[node,face]=removeisolatednode(node,face);

node=(node-30)/28;
r0=sqrt(sum((node.*node)'));
node=node.*repmat(1./r0(:),1,3);

if(nargout==3)
   if(nargin==1)
       maxvol=tsize*tsize*tsize;
   end
   [node,elem,face]=surf2mesh(node,face,[-1 -1 -1]*1.1,[1 1 1]*1.1,1,maxvol,[],[]);
   elem=elem(:,1:4);
end

face=face(:,1:3);


function prime_plot ( n_max )

%*****************************************************************************80
%
%% PRIME_PLOT makes a box plot of primes.
%
%  Usage:
%
%    prime_plot ( n_max )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N_MAX, the maximum number to be considered.
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PRIME_PLOT\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Display primes and composite numbers.\n' );
  fprintf ( 1, '  If N is composite, Box (N,D) is blue if N is divisible by D.\n' );
  fprintf ( 1, '  If N is prime, Column (N,:) is red.\n' );
%
%  First argument is N_MAX.
%
  if ( nargin < 1 )
    fprintf ( 1, '\n' );
    n_max = input ( '  Enter N, the maximum number to check: ' );
  end

  clf
%
%  Every box starts out WHITE.
%
  for n = 1 : n_max

    for d = 1 : n_max

      x1 = n - 0.44;
      x2 = n + 0.44;
      y1 = d - 0.44;
      y2 = d + 0.44;

      patch ( [ x1, x2, x2, x1 ], [ y1, y1, y2, y2 ], 'w', 'EdgeColor', 'none' );

    end

  end
%
%  By convention, 1 is NOT a prime!
%
  n = 1;
  d = 1;

  x1 = n - 0.44;
  x2 = n + 0.44;
  y1 = d - 0.44;
  y2 = d + 0.44;
  patch ( [ x1, x2, x2, x1 ], [ y1, y1, y2, y2 ], 'b', 'EdgeColor', 'none' );

  for n = 2 : n_max

    prime = 1;

    d = 1;

    for d = 2 : min ( n_max, n - 1 )

      if ( mod ( n, d ) == 0 )
        x1 = n - 0.44;
        x2 = n + 0.44;
        y1 = d - 0.44;
        y2 = d + 0.44;
        patch ( [ x1, x2, x2, x1 ], [ y1, y1, y2, y2 ], 'b', 'EdgeColor', 'none' );
        prime = 0;
      end

    end

    d = n;

    if ( prime )
      x1 = n - 0.44;
      x2 = n + 0.44;
      y1 = 1 - 0.44;
      y2 = d + 0.44;
      patch ( [ x1, x2, x2, x1 ], [ y1, y1, y2, y2 ], 'r', 'EdgeColor', 'none' );
    else
      x1 = n - 0.44;
      x2 = n + 0.44;
      y1 = d - 0.44;
      y2 = d + 0.44;
      patch ( [ x1, x2, x2, x1 ], [ y1, y1, y2, y2 ], 'b', 'EdgeColor', 'none' );
      x1 = n - 0.44;
      x2 = n + 0.44;
      y1 = 1 - 0.44;
      y2 = 1 + 0.44;
      patch ( [ x1, x2, x2, x1 ], [ y1, y1, y2, y2 ], 'b', 'EdgeColor', 'none' );
    end

  end

  xlabel ( '<---N--->' )
  ylabel ( '<--Divisors-->' )
  title ( 'Primes are red columns; blue squares are factors.' )

  axis ( [ 0.5, n + 0.5, 0.5, n + 0.5 ] );
  axis equal
  axis square
  axis tight

  filename = 'prime_plot.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Plot saved in file "%s".\n', filename );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PRIME_PLOT\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function timestamp ( )

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

  return
end


function [f,str] = constructpolynomialmatrix2d(matrixsize,locs,degree)

% function [f,str] = constructpolynomialmatrix2d(matrixsize,locs,degree)
%
% <matrixsize> is a 2D matrix size like [100 50]
% <locs> is a row or column vector of indices into that matrix size
% <degree> is the maximum polynomial degree desired.
%   should be no greater than 10.
%
% return <f>, a matrix of dimensions length(<locs>) x N
% with polynomial basis functions evaluated at <locs> in
% the columns.  the polynomial basis functions are evaluated
% over the range [-1,1] which is presumed to correspond to
% the beginning and ending element along each of the two dimensions.
% (if a dimension has only one element, the values are all set to 1.)
% also, return <str>, the algebraic expression that corresponds to
% the columns of <f>.  'x' refers to the first matrix dimension; 'y'
% refers to the second matrix dimension.
%
% note that there may be various gain factors on the basis functions
% (e.g. don't assume that they are all unit-length).
%
% also, beware of numerical precision issues for high degrees...
%
% see also constructpolynomialmatrix3d.m.
%
% history:
% - 2013/05/23 - hard code the basis expansion to ensure consistent results across platforms.
%                this changes previous behavior.
%
% example:
% [f,str] = constructpolynomialmatrix2d([30 30],find(ones(30,30)),2);
% str
% f = reshape(f,30,30,[]);
% figure; imagesc(makeimagestack(f));

% prep
x = sym('x');
y = sym('y');

% do the algebra
%str = char(expand((x+y+1)^degree));
switch degree
case 0
  str = '1';
case 1
  str = 'x+y+1';
case 2
  str = 'x^2+2*x*y+2*x+y^2+2*y+1';
case 3
  str = 'x^3+3*x^2*y+3*x^2+3*x*y^2+6*x*y+3*x+y^3+3*y^2+3*y+1';
case 4
  str = '1+4*x+4*y+6*x^2+12*x*y+6*y^2+4*x^3*y+6*x^2*y^2+12*x^2*y+4*x*y^3+12*x*y^2+x^4+4*x^3+y^4+4*y^3';
case 5
  str = '1+5*x+5*y+10*x^2+20*x*y+10*y^2+20*x^3*y+30*x^2*y^2+30*x^2*y+20*x*y^3+30*x*y^2+5*x^4+10*x^3+5*y^4+10*y^3+y^5+5*x*y^4+10*x^2*y^3+10*x^3*y^2+5*x^4*y+x^5';
case 6
  str = '1+6*x+6*y+15*x^2+30*x*y+15*y^2+60*x^3*y+90*x^2*y^2+60*x^2*y+60*x*y^3+60*x*y^2+15*x^4+20*x^3+15*y^4+20*y^3+6*x*y^5+y^6+6*y^5+30*x*y^4+15*x^2*y^4+60*x^2*y^3+60*x^3*y^2+20*x^3*y^3+15*x^4*y^2+30*x^4*y+6*x^5+x^6+6*x^5*y';
case 7
  str = '1+7*x+7*y+21*x^2+42*x*y+21*y^2+140*x^3*y+210*x^2*y^2+105*x^2*y+140*x*y^3+105*x*y^2+35*x^4+35*x^3+35*y^4+35*y^3+7*x*y^6+42*x*y^5+7*y^6+21*y^5+y^7+105*x*y^4+105*x^2*y^4+210*x^2*y^3+21*x^2*y^5+210*x^3*y^2+35*x^3*y^4+140*x^3*y^3+105*x^4*y^2+105*x^4*y+21*x^5+7*x^6+x^7+35*x^4*y^3+42*x^5*y+21*x^5*y^2+7*x^6*y';
case 8
  str = '1+8*x+8*y+28*x^2+56*x*y+28*y^2+280*x^3*y+420*x^2*y^2+168*x^2*y+280*x*y^3+168*x*y^2+70*x^4+56*x^3+70*y^4+56*y^3+56*x*y^6+168*x*y^5+8*x*y^7+28*y^6+56*y^5+8*y^7+y^8+280*x*y^4+28*x^2*y^6+420*x^2*y^4+560*x^2*y^3+168*x^2*y^5+560*x^3*y^2+280*x^3*y^4+560*x^3*y^3+56*x^3*y^5+420*x^4*y^2+280*x^4*y+56*x^5+28*x^6+8*x^7+x^8+70*x^4*y^4+280*x^4*y^3+168*x^5*y+168*x^5*y^2+56*x^5*y^3+56*x^6*y+28*x^6*y^2+8*x^7*y';
case 9
  str = '1+9*x+9*y+36*x^2+72*x*y+36*y^2+504*x^3*y+756*x^2*y^2+252*x^2*y+504*x*y^3+252*x*y^2+126*x^4+84*x^3+126*y^4+84*y^3+252*x*y^6+504*x*y^5+72*x*y^7+84*y^6+126*y^5+36*y^7+9*y^8+630*x*y^4+252*x^2*y^6+1260*x^2*y^4+1260*x^2*y^3+756*x^2*y^5+1260*x^3*y^2+1260*x^3*y^4+1680*x^3*y^3+504*x^3*y^5+1260*x^4*y^2+630*x^4*y+126*x^5+84*x^6+36*x^7+9*x^8+630*x^4*y^4+1260*x^4*y^3+504*x^5*y+756*x^5*y^2+504*x^5*y^3+252*x^6*y+252*x^6*y^2+72*x^7*y+x^9+y^9+9*x^8*y+36*x^7*y^2+84*x^6*y^3+126*x^5*y^4+126*x^4*y^5+84*x^3*y^6+36*x^2*y^7+9*x*y^8';
case 10
  str = '1+10*x+10*y+45*x^2+90*x*y+45*y^2+840*x^3*y+1260*x^2*y^2+360*x^2*y+840*x*y^3+360*x*y^2+210*x^4+120*x^3+210*y^4+120*y^3+840*x*y^6+1260*x*y^5+360*x*y^7+210*y^6+252*y^5+120*y^7+45*y^8+1260*x*y^4+1260*x^2*y^6+3150*x^2*y^4+2520*x^2*y^3+2520*x^2*y^5+2520*x^3*y^2+4200*x^3*y^4+4200*x^3*y^3+2520*x^3*y^5+3150*x^4*y^2+1260*x^4*y+252*x^5+210*x^6+120*x^7+45*x^8+3150*x^4*y^4+4200*x^4*y^3+1260*x^5*y+2520*x^5*y^2+2520*x^5*y^3+840*x^6*y+1260*x^6*y^2+360*x^7*y+10*x^9+10*y^9+x^10+90*x^8*y+360*x^7*y^2+840*x^6*y^3+1260*x^5*y^4+1260*x^4*y^5+840*x^3*y^6+360*x^2*y^7+90*x*y^8+10*x^9*y+45*x^8*y^2+y^10+120*x^7*y^3+210*x^6*y^4+252*x^5*y^5+210*x^4*y^6+120*x^3*y^7+45*x^2*y^8+10*x*y^9';
otherwise
  die;
end

%REMOVE since hard-coded now
%str = sort(strsplit(str,'+'));%% sort the stuff in between + signs to try to ensure consistent ordering!!!
str = strsplit(str,'+');
str = cat(1,str,repmat({'+'},[1 length(str)]));
str = cat(2,str{:});
str = str(1:end-1);

% add a little padding so the 1 step below will work for degree 0
str = [' ' str ' '];

% change * to .*
  old = pwd;  % THIS IS A TOTAL HACK TO AVOID FUNCTION NAME CONFLICTS!
  cd(fullfile(matlabroot,'toolbox','matlab','funfun'));
str = vectorize(str);
  cd(old);

% remove +
str(str=='+') = ' ';

% change 1 to ones(size(x),1)
str0 = strrep(str,' 1 ',' ones(size(x)) '); assert(length(str0) ~= length(str));
str = str0;

% add brackets
str = [ '[' str ']' ];

% prep the linear coordinates
[x,y] = ind2sub(matrixsize,locs(:));
if matrixsize(1)~=1
  x = normalizerange(x,-1,1,1,matrixsize(1));
end
if matrixsize(2)~=1
  y = normalizerange(y,-1,1,1,matrixsize(2));
end

% do it
f = eval(str);


% Showing the effect of the POCS partial Fourier reconstruction.
%
% Mark a cell (code beginning with "%%") and
% press <Ctrl> + <Enter> to evaluate the cell content only.

%% simple example (2D, single coil, no phase errors)
%
N = 256;
iter = 30;
pf = 9/16;      % a typical large partial fourier factor
p = single(phantom(N));
kspRef = fftshift(fftshift(  fft(fft(  fftshift(fftshift( p, 1),2), [],1),[],2), 1),2);
kspRef = kspRef + 3 * complex( randn(N), randn(N) );    % add noise
kspPF  = kspRef;
kspPF(:,1:floor((1-pf)*N)) = 0;

imReco = pocs( kspPF, iter, true );

imRef    = ifftshift(ifftshift(  ifft(ifft(  ifftshift(ifftshift( kspRef, 1),2), [],1),[],2), 1),2);
imDirect = ifftshift(ifftshift(  ifft(ifft(  ifftshift(ifftshift(  kspPF, 1),2), [],1),[],2), 1),2);

figure,
imagesc(abs([imRef  imReco  imDirect]), [0 2*mean(abs(imRef(:)))])
axis image
title('reference (full dataset)   |   POCS reco   |    zero-padded reco')
colormap(gray(256))

%% multicoil example with phase error
%
clear  p  kspRef  kspPF  imReco  imRef  imDirect
N = 256;
iter = 30;
pf = 9/16;
% Set up some pseudo coilmaps:
cmap(1,:,:) = repmat(        ( 3./(1:N)  .^0.10) .* exp(1i*(1:N)  *pi/(N/3)) , N, 1 );
cmap(2,:,:) = repmat(        ( 1./(1:N).'.^0.20) .* exp(1i*(1:N).'*pi/(N/4)) , 1, N );
cmap(3,:,:) = repmat( fliplr(( 2./(1:N)  .^0.04) .* exp(1i*(1:N)  *pi/(N/8))), N, 1 );
cmap(4,:,:) = repmat( flipud(( 2./(1:N).'.^0.15) .* exp(1i*(1:N).'*pi/(N/10))),1, N );
cmap = single(cmap);

Nc = size(cmap,1);
p = reshape( single(phantom(N)), 1, N, N );
subs = { 1, 164:181, 187:193 };
p(subs{:}) = 0;
subs = { 1, 166:179, 189:191 };
p(subs{:}) = 1 * exp(1i * pi/2);
p = bsxfun( @times, cmap, p );
kspRef = fftshift(fftshift(  fft(fft(  fftshift(fftshift( p, 2),3), [],2),[],3), 2),3);
kspRef = kspRef + 10 * complex( randn(Nc,N,N), randn(Nc,N,N) );    % add noise
kspPF  = kspRef;
kspPF(:,:,1:floor((1-pf)*N)) = 0;

imReco = pocs( kspPF, iter, true );

imReco   = squeeze(sqrt(sum( abs(imReco).^2, 1)));
imRef    = ifftshift(ifftshift(  ifft(ifft(  ifftshift(ifftshift( kspRef, 2),3), [],2),[],3), 2),3);
imRef    = squeeze(sqrt(sum( abs(imRef).^2, 1)));
imDirect = ifftshift(ifftshift(  ifft(ifft(  ifftshift(ifftshift(  kspPF, 2),3), [],2),[],3), 2),3);
imDirect = squeeze(sqrt(sum( abs(imDirect).^2, 1)));

figure,
imagesc(abs([imRef  imReco  imDirect]),[0 2*mean(abs(imRef(:)))])
axis image
title('reference (full dataset)   |   POCS reco   |    zero-padded reco')
colormap(gray(256))


function D = generate_2d_3x3_sparse_operator(mat, sz)
% D = generate_first_difference_matrix_for_image(sz, {dif_order, spacing})
% sz -- image size 2x1 or 3x1
% dif_order -- 1 or 2. 1st or 2nd derivative
% spacing -- size of pixel in physical units. 2x1 or 3x1
%
% Creates derivative matrix, if IMG is rect. image or volume, then
% G = D * IMG(:);
% G is [prod(size(IMG))*ndims]x[1] vector, containing drivatives in
% 1, 2, {3} directions.
%
    P = prod(sz);
    s1 = sz(1);
    s2 = sz(2);

    [m1, m2] = ndgrid([1 : s1-1, 1], 1 : s2);
    tp = m1(:) + (m2(:) - 1) * s1;
    tm = (m1(:)+1) + (m2(:) - 1) * s1;
    K = numel(tp);
    A1 = (sparse(1:K, tm, 1, K, P) - sparse(1:K, tp, 1, K, P)) / spacing(1);

    [m1, m2] = ndgrid(1 : s1, [1 : s2-1, 1]);
    tp = m1(:) + (m2(:) - 1) * s1;
    tm = m1(:) + (m2(:)) * s1;
    K = numel(tp);
    A2 = (sparse(1:K, tm, 1, K, P) - sparse(1:K, tp, 1, K, P)) / spacing(2);
    A = [A1; A2];
end


function [Y, s] = standardize(X)
% Unitize the vectors to be unit length
%   By default dim = 1 (columns).
% Written by Mo Chen (sth4nth@gmail.com).
X = bsxfun(@minux,X,mean(X,2));
s = sqrt(mean(sum(X.^2,1)));
Y = X/s;


function [STATS]=scatter_stats(D)

%Fitting Gaussian mixture (1) model
GM=gmdistribution.fit(D,1);
MU=GM.mu; %Means
MU_total=rms(MU); %Total mean
VAR=GM.Sigma; %Variance
STD=sqrt(VAR); %Standard deviations
[VAR_vec,VAR_eig] = eig(VAR); %Get principal components
STD_total=sqrt(trace(VAR_eig)./3); %Standard deviations

%Creating output structure
STATS.GM=GM;
STATS.MU=MU;
STATS.MU_total=MU_total;
STATS.VAR=VAR;
STATS.STD=STD;
STATS.STD_total=STD_total;

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 [ t, dtdr, dtds ] = shape_t10 ( r, s )

%*****************************************************************************80
%
%% SHAPE_T10 evaluates shape functions for a 10 node reference triangle.
%
%  Reference Element T10:
%
%    |
%    1  10
%    |  |\
%    |  | \
%    |  8  9
%    |  |   \
%    S  |    \
%    |  5  6  7
%    |  |      \
%    |  |       \
%    0  1--2--3--4
%    |
%    +--0----R---1-->
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R, S, the reference coordinates of a point.
%
%    Output, real T(10), the basis functions at the point.
%
%    Output, real DTDR(10), the R basis derivatives at the point.
%
%    Output, real DTDS(10), the S basis derivatives at the point.
%
  a = 1.0 / 3.0;
  b = 2.0 / 3.0;
  c = 1.0;

  t(1)  = 4.5 * ( a - r - s ) * ( b - r - s ) * ( c - r - s );
  t(2)  = 13.5 * r * ( b - r - s ) * ( c - r - s );
  t(3)  = - 13.5 * r * ( a - r ) * ( c - r - s );
  t(4)  = 4.5 * r * ( a - r ) * ( b - r );
  t(5)  = 13.5 * s * ( b - r - s ) * ( c - r - s );
  t(6)  = 27.0 * r * s * ( c - r - s );
  t(7)  = 13.5 * r * s * ( r - a );
  t(8)  = 13.5 * s * ( s - a ) * ( c - r - s );
  t(9)  = 13.5 * r * s * ( s - a );
  t(10) = 4.5 * s * ( a - s ) * ( b - s );

  dtdr(1) = 4.5 * ( ( a - s ) * ( 2.0 * r - c - b + 2.0 * s ) ...
    - ( s - b ) * ( s - c ) - 2.0 * ( 2.0 * s - b - c ) * r - 3.0 * r * r );
  dtdr(2) = 13.5 * ( ( s - b ) * ( s - c ) + 2.0 * ( 2.0 * s - b - c ) * r ...
    + 3.0 * r * r );
  dtdr(3) = - 13.5 * ( a * ( c - s ) + 2.0 * ( s - a - c ) * r + 3.0 * r * r );
  dtdr(4) = 4.5 * ( a * b - 2.0 * ( a + b ) * r + 3.0 * r * r );
  dtdr(5) = 13.5 * s * ( 2.0 * s - b - c + 2.0 * r );
  dtdr(6) = 27.0 * s * ( c - s - 2.0 * r );
  dtdr(7) = 13.5 * s * ( 2.0 * r - a );
  dtdr(8) = - 13.5 * s * ( s - a );
  dtdr(9) = 13.5 * s * ( s - a);
  dtdr(10) = 0.0;

  dtds(1) = 4.5 * ( ( a - r ) * ( 2.0 * s - c - b + 2.0 * r ) ...
    - ( r - b ) * ( r - c ) - 2.0 * ( 2.0 * r - b - c ) * s - 3.0 * s * s );
  dtds(2) = 13.5 * r * ( 2.0 * s + 2.0 * r - b - c );
  dtds(3) = 13.5 * r * ( a - r );
  dtds(4) = 0.0;
  dtds(5) = 13.5 * ( ( r - b ) * ( r - c ) + 2.0 * ( 2.0 * r - b - c ) * s ...
    + 3.0 * s * s );
  dtds(6) = 27.0 * r * ( c - r - 2.0 * s );
  dtds(7) = 13.5 * r * ( r - a );
  dtds(8) = - 13.5 * ( a * ( c - r ) + 2.0 * ( r - c - a ) * s + 3.0 * s * s );
  dtds(9) = 13.5 * r * ( 2.0 * s - a );
  dtds(10) = 4.5 * ( a * b - 2.0 * ( a + b ) * s + 3.0 * s * s );

  return
end


function kpn_test ( )

%*****************************************************************************80
%
%% KPN_TEST uses the KPN function for 1D quadrature over (-oo,+oo).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 May 2012
%
%  Author:
%
%    John Burkardt
%
  d = 1;
  exact = hermite_integral ( d );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'KPN_TEST:\n' );
  fprintf ( 1, '  Kronrod-Patterson-Hermite quadrature over (-oo,+oo):\n' );
  fprintf ( 1, '  Exact integral is %g\n', exact );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   Level   Nodes    Estimate  Error\n' );
  fprintf ( 1, '\n' );

  for l = 1 : 5

    n = kpn_order ( l );

    nh = floor ( ( n + 1 ) / 2 );

    [ xh, wh ] = kpn ( l );

    if ( mod ( n, 2 ) == 1 )
      x = [ -xh(nh:-1:2); xh ];
      w = [ wh(nh:-1:2); wh ];
    else
      x = [ -xh(nh:-1:1); xh ];
      w = [ wh(nh:-1:1); wh ];
    end

    fx = hermite_integrand ( d, n, x );
    q = w' * fx;
    e = sqrt ( ( q - exact ).^2 ) / exact;

    fprintf( '  %2d     %6d  %10.5g  %10.5g\n', l, n, q, e )

  end

  return
end


function [ rexp, sexp ] = poly_q4 ( )

%*****************************************************************************80
%
%% POLY_Q4 returns the monomials associated with a 4 node quadrilateral.
%
%  Reference Element Q4:
%
%    |
%    1  4-----3
%    |  |     |
%    |  |     |
%    S  |     |
%    |  |     |
%    |  |     |
%    0  1-----2
%    |
%    +--0--R--1-->
%
%  Formula:
%
%    Given coefficients A(I), the polynomial interpolant at (R,S) is
%
%      P(R,S) = sum ( 1 <= I <= N ) A(I) * R**REXP(I) * S**SEXP(I)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, integer REXP(4), SEXP(4), the powers of R and S associated
%    with each polynomial.
%
  rexp(1:4) = [ 0, 0, 1, 1 ];
  sexp(1:4) = [ 0, 1, 0, 1 ];

  return
end


%
% [idx,netsim,dpsim,expref]=apclusterSparse(s,p)
%
% APCLUSTER uses affinity propagation (Frey and Dueck, Science,
% 2007) to identify data clusters, using a set of real-valued
% pair-wise data point similarities as input. Each cluster is
% represented by a data point called a cluster center, and the
% method searches for clusters so as to maximize a fitness
% function called net similarity. The method is iterative and
% stops after maxits iterations (default of 500 - see below for
% how to change this value) or when the cluster centers stay
% constant for convits iterations (default of 50). The command
% apcluster(s,p,'plot') can be used to plot the net similarity
% during operation of the algorithm.
%
% For N data points, there may be as many as N^2-N pair-wise
% similarities (note that the similarity of data point i to k
% need not be equal to the similarity of data point k to i).
% These may be passed to APCLUSTER in an NxN matrix s, where
% s(i,k) is the similarity of point i to point k. In fact, only
% a smaller number of relevant similarities are needed for
% APCLUSTER to work. If only M similarity values are known,
% where M < N^2-N, they can be passed to APCLUSTER in an Mx3
% matrix s, where each row of s contains a pair of data point
% indices and a corresponding similarity value: s(j,3) is the
% similarity of data point s(j,1) to data point s(j,2).
%
% APCLUSTER automatically determines the number of clusters,
% based on the input p, which is an Nx1 matrix of real numbers
% called preferences. p(i) indicates the preference that data
% point i be chosen as a cluster center. A good choice is to
% set all preference values to the median of the similarity
% values. The number of identified clusters can be increased or
% decreased  by changing this value accordingly. If p is a
% scalar, APCLUSTER assumes all preferences are equal to p.
%
% The fitness function (net similarity) used to search for
% solutions equals the sum of the preferences of the the data
% centers plus the sum of the similarities of the other data
% points to their data centers.
%
% The identified cluster centers and the assignments of other
% data points to these centers are returned in idx. idx(j) is
% the index of the data point that is the cluster center for
% data point j. If idx(j) equals j, then point j is itself a
% cluster center. The sum of the similarities of the data
% points to their cluster centers is returned in dpsim, the
% sum of the preferences of the identified cluster centers is
% returned in expref and the net similarity (sum of the data
% point similarities and preferences) is returned in netsim.
%
% EXAMPLE
%
% N=100; x=rand(N,2); % Create N, 2-D data points
% M=N*N-N; s=zeros(M,3); % Make ALL N^2-N similarities
% j=1;
% for i=1:N
%   for k=[1:i-1,i+1:N]
%     s(j,1)=i; s(j,2)=k; s(j,3)=-sum((x(i,:)-x(k,:)).^2);
%     j=j+1;
%   end;
% end;
% p=median(s(:,3)); % Set preference to median similarity
% [idx,netsim,dpsim,expref]=apclusterSparse(s,p,'plot');
% fprintf('Number of clusters: %d\n',length(unique(idx)));
% fprintf('Fitness (net similarity): %f\n',netsim);
% figure; % Make a figures showing the data and the clusters
% for i=unique(idx)'
%   ii=find(idx==i); h=plot(x(ii,1),x(ii,2),'o'); hold on;
%   col=rand(1,3); set(h,'Color',col,'MarkerFaceColor',col);
%   xi1=x(i,1)*ones(size(ii)); xi2=x(i,2)*ones(size(ii));
%   line([x(ii,1),xi1]',[x(ii,2),xi2]','Color',col);
% end;
% axis equal tight;
%
% PARAMETERS
%
% [idx,netsim,dpsim,expref]=apclusterSparse(s,p,'NAME',VALUE,...)
%
% The following parameters can be set by providing name-value
% pairs, eg, apcluster(s,p,'maxits',1000):
%
%   Parameter    Value
%   'maxits'     Any positive integer. This specifies the
%                maximum number of iterations performed by
%                affinity propagation. Default: 1000.
%   'convits'    Any positive integer. APCLUSTER decides that
%                the algorithm has converged if the estimated
%                cluster centers stay fixed for convits
%                iterations. Increase this value to apply a
%                more stringent convergence test. Default: 100.
%   'dampfact'   A real number that is less than 1 and
%                greater than or equal to 0.5. This sets the
%                damping level of the message-passing method,
%                where values close to 1 correspond to heavy
%                damping which may be needed if oscillations
%                occur. Default: .9
%   'plot'       No value needed. This creates a figure that
%                plots the net similarity after each iteration
%                of the method. If the net similarity fails to
%                converge, consider increasing the values of
%                dampfact and maxits.
%   'details'    No value needed. This causes idx, netsim,
%                dpsim and expref to be stored after each
%                iteration.
%   'nonoise'    No value needed. Degenerate input similarities
%                (eg, where the similarity of i to k equals the
%                similarity of k to i) can prevent convergence.
%                To avoid this, APCLUSTER adds a small amount
%                of noise to the input similarities. This flag
%                turns off the addition of noise.
%
% Copyright (c) Brendan J. Frey and Delbert Dueck (2006). This
% software may be freely used and distributed for
% non-commercial purposes.

function [idx,netsim,dpsim,expref]=apclusterSparse(s,p,varargin);

% Handle arguments to function
if nargin<2 error('Too few input arguments');
else
    maxits=1000; convits=100; lam=0.9; plt=0; details=0; nonoise=0;
    i=1;
    while i<=length(varargin)
        if strcmp(varargin{i},'plot')
            plt=1; i=i+1;
        elseif strcmp(varargin{i},'details')
            details=1; i=i+1;
        elseif strcmp(varargin{i},'nonoise')
            nonoise=1; i=i+1;
        elseif strcmp(varargin{i},'maxits')
            maxits=varargin{i+1};
            i=i+2;
            if maxits<=0 error('maxits must be a positive integer'); end;
        elseif strcmp(varargin{i},'convits')
            convits=varargin{i+1};
            i=i+2;
            if convits<=0 error('convits must be a positive integer'); end;
        elseif strcmp(varargin{i},'dampfact')
            lam=varargin{i+1};
            i=i+2;
            if (lam<0.5)||(lam>=1)
                error('dampfact must be >= 0.5 and < 1');
            end;
        else i=i+1;
        end;
    end;
end;
if lam>0.9
    fprintf('\n*** Warning: Large damping factor in use. Turn on plotting\n');
    fprintf('    to monitor the net similarity. The algorithm will\n');
    fprintf('    change decisions slowly, so consider using a larger value\n');
    fprintf('    of convits.\n\n');
end;

% Check that standard arguments are consistent in size
if length(size(s))~=2 error('s should be a 2D matrix');
elseif length(size(p))>2 error('p should be a vector or a scalar');
elseif size(s,2)==3
    tmp=max(max(s(:,1)),max(s(:,2)));
    if length(p)==1 N=tmp; else N=length(p); end;
    if tmp>N
        error('data point index exceeds number of data points');
    elseif min(min(s(:,1)),min(s(:,2)))<=0
        error('data point indices must be >= 1');
    end;
elseif size(s,1)==size(s,2)
    N=size(s,1);
    if (length(p)~=N)&&(length(p)~=1)
        error('p should be scalar or a vector of size N');
    end;
else error('s must have 3 columns or be square'); end;

% Make vector of preferences
if length(p)==1 p=p*ones(N,1); end;

% Append self-similarities (preferences) to s-matrix
tmps=[repmat([1:N]',[1,2]),p]; s=[s;tmps];
M=size(s,1);

% In case user did not remove degeneracies from the input similarities,
% avoid degenerate solutions by adding a small amount of noise to the
% input similarities
if ~nonoise
    rns=randn('state'); randn('state',0);
    s(:,3)=s(:,3)+(eps*s(:,3)+realmin*100).*rand(M,1);
    randn('state',rns);
end;

% Construct indices of neighbors
ind1e=zeros(N,1);
for j=1:M k=s(j,1); ind1e(k)=ind1e(k)+1; end;
ind1e=cumsum(ind1e); ind1s=[1;ind1e(1:end-1)+1]; ind1=zeros(M,1);
for j=1:M k=s(j,1); ind1(ind1s(k))=j; ind1s(k)=ind1s(k)+1; end;
ind1s=[1;ind1e(1:end-1)+1];
ind2e=zeros(N,1);
for j=1:M k=s(j,2); ind2e(k)=ind2e(k)+1; end;
ind2e=cumsum(ind2e); ind2s=[1;ind2e(1:end-1)+1]; ind2=zeros(M,1);
for j=1:M k=s(j,2); ind2(ind2s(k))=j; ind2s(k)=ind2s(k)+1; end;
ind2s=[1;ind2e(1:end-1)+1];

% Allocate space for messages, etc
A=zeros(M,1); R=zeros(M,1); t=1;
if plt netsim=zeros(1,maxits+1); end;
if details
    idx=zeros(N,maxits+1);
    netsim=zeros(1,maxits+1);
    dpsim=zeros(1,maxits+1);
    expref=zeros(1,maxits+1);
end;

% Execute parallel affinity propagation updates
e=zeros(N,convits); dn=0; i=0;
while ~dn
    i=i+1;

    % Compute responsibilities
    for j=1:N
        ss=s(ind1(ind1s(j):ind1e(j)),3);
        as=A(ind1(ind1s(j):ind1e(j)))+ss;
        [Y,I]=max(as); as(I)=-realmax; [Y2,I2]=max(as);
        r=ss-Y; r(I)=ss(I)-Y2;
        R(ind1(ind1s(j):ind1e(j)))=(1-lam)*r+ ...
            lam*R(ind1(ind1s(j):ind1e(j)));
    end;

    % Compute availabilities
    for j=1:N
        rp=R(ind2(ind2s(j):ind2e(j)));
        rp(1:end-1)=max(rp(1:end-1),0);
        a=sum(rp)-rp;
        a(1:end-1)=min(a(1:end-1),0);
        A(ind2(ind2s(j):ind2e(j)))=(1-lam)*a+ ...
            lam*A(ind2(ind2s(j):ind2e(j)));
    end;

    % Check for convergence
    E=((A(M-N+1:M)+R(M-N+1:M))>0); e(:,mod(i-1,convits)+1)=E; K=sum(E);
    if i>=convits || i>=maxits
        se=sum(e,2);
        unconverged=(sum((se==convits)+(se==0))~=N);
        if (~unconverged&&(K>0))||(i==maxits) dn=1; end;
    end;

    % Handle plotting and storage of details, if requested
    if plt||details
        if K==0
            tmpnetsim=nan; tmpdpsim=nan; tmpexpref=nan; tmpidx=nan;
        else
            tmpidx=zeros(N,1); tmpdpsim=0;
            tmpidx(find(E))=find(E); tmpexpref=sum(p(find(E)));
            discon=0;
            for j=find(E==0)'
                ss=s(ind1(ind1s(j):ind1e(j)),3);
                ii=s(ind1(ind1s(j):ind1e(j)),2);
                ee=find(E(ii));
                if length(ee)==0 discon=1;
                else
                    [smx imx]=max(ss(ee));
                    tmpidx(j)=ii(ee(imx));
                    tmpdpsim=tmpdpsim+smx;
                end;
            end;
            if discon
                tmpnetsim=nan; tmpdpsim=nan; tmpexpref=nan; tmpidx=nan;
            else tmpnetsim=tmpdpsim+tmpexpref;
            end;
        end;
    end;
    if details
        netsim(i)=tmpnetsim; dpsim(i)=tmpdpsim; expref(i)=tmpexpref;
        idx(:,i)=tmpidx;
    end;
    if plt
        netsim(i)=tmpnetsim;
        figure(234);
        tmp=1:i; tmpi=find(~isnan(netsim(1:i)));
        plot(tmp(tmpi),netsim(tmpi),'r-');
        xlabel('# Iterations');
        ylabel('Net similarity of quantized intermediate solution');
        drawnow;
    end;
end;
% Identify  exemplars
E=((A(M-N+1:M)+R(M-N+1:M))>0); K=sum(E);
if K>0
    tmpidx=zeros(N,1); tmpidx(find(E))=find(E); % Identify clusters
    for j=find(E==0)'
        ss=s(ind1(ind1s(j):ind1e(j)),3);
        ii=s(ind1(ind1s(j):ind1e(j)),2);
        ee=find(E(ii));
        [smx imx]=max(ss(ee));
        tmpidx(j)=ii(ee(imx));
    end;
    EE=zeros(N,1);
    for j=find(E)'
        jj=find(tmpidx==j); mx=-Inf;
        ns=zeros(N,1); msk=zeros(N,1);
        for m=jj'
            mm=s(ind1(ind1s(m):ind1e(m)),2);
            msk(mm)=msk(mm)+1;
            ns(mm)=ns(mm)+s(ind1(ind1s(m):ind1e(m)),3);
        end;
        ii=jj(find(msk(jj)==length(jj)));
        [smx imx]=max(ns(ii));
        EE(ii(imx))=1;
    end;
    E=EE;
    tmpidx=zeros(N,1); tmpdpsim=0;
    tmpidx(find(E))=find(E); tmpexpref=sum(p(find(E)));
    for j=find(E==0)'
        ss=s(ind1(ind1s(j):ind1e(j)),3);
        ii=s(ind1(ind1s(j):ind1e(j)),2);
        ee=find(E(ii));
        [smx imx]=max(ss(ee));
        tmpidx(j)=ii(ee(imx));
        tmpdpsim=tmpdpsim+smx;
    end;
    tmpnetsim=tmpdpsim+tmpexpref;
else
    tmpidx=nan*ones(N,1); tmpnetsim=nan; tmpexpref=nan;
end;
if details
    netsim(i+1)=tmpnetsim; netsim=netsim(1:i+1);
    dpsim(i+1)=tmpnetsim-tmpexpref; dpsim=dpsim(1:i+1);
    expref(i+1)=tmpexpref; expref=expref(1:i+1);
    idx(:,i+1)=tmpidx; idx=idx(:,1:i+1);
else
    netsim=tmpnetsim; dpsim=tmpnetsim-tmpexpref;
    expref=tmpexpref; idx=tmpidx;
end;
if plt||details
    fprintf('\nNumber of identified clusters: %d\n',K);
    fprintf('Fitness (net similarity): %f\n',tmpnetsim);
    fprintf('  Similarities of data points to exemplars: %f\n',dpsim(end));
    fprintf('  Preferences of selected exemplars: %f\n',tmpexpref);
    fprintf('Number of iterations: %d\n\n',i);
end;
if unconverged
    fprintf('\n*** Warning: Algorithm did not converge. The similarities\n');
    fprintf('    may contain degeneracies - add noise to the similarities\n');
    fprintf('    to remove degeneracies. To monitor the net similarity,\n');
    fprintf('    activate plotting. Also, consider increasing maxits and\n');
    fprintf('    if necessary dampfact.\n\n');
end;


% TESTFITPLANE - demonstrates RANSAC plane fitting
%
% Usage: testfitplane(outliers, sigma, t, feedback)
%
% Arguments:
%               outliers - Fraction specifying how many points are to be
%                          outliers.
%               sigma    - Standard deviation of inlying points from the
%                          true plane.
%               t        - Distance threshold to be used by the RANSAC
%                          algorithm for deciding whether a point is an
%                          inlier.
%               feedback - Optional flag 0 or 1 to turn on RANSAC feedback
%                          information.
%
%  Try using:  testfitplane(0.3, 0.05, 0.05)
%
% See also: RANSACFITPLANE, FITPLANE

% Copyright (c) 2003-2005 Peter Kovesi
% School of Computer Science & Software Engineering
% The University of Western Australia
% http://www.csse.uwa.edu.au/
%
% 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, 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.

% June 2003

function testfitplane(outliers, sigma, t, feedback)
    if nargin == 3
	feedback = 0;
    end

    % Hard wire some constants - vary these as you wish

    npts = 100;  % Number of 3D data points

    % Define a plane  ax + by + cz + d = 0
    a = 10; b = -3; c = 5; d = 1;

    B = [a b c d]';
    B = B/norm(B);

    outsigma = 30*sigma;  % outlying points have a distribution that is
                          % 30 times as spread as the inlying points

    vpts = round((1-outliers)*npts);  % No of valid points
    opts = npts - vpts;               % No of outlying points

    % Generate npts points in the plane
    X = rand(1,npts);
    Y = rand(1,npts);
    Z = (-a*X -b*Y -d)/c;

    XYZ =  [X
	    Y
	    Z];

    % Add uniform noise of +/-sigma
    XYZ = XYZ + (2*rand(size(XYZ))-1)*sigma;

    % Generate opts random outliers

    n = length(XYZ);
    ind = randperm(n);  % get a random set of point indices
    ind = ind(1:opts);  % ... of length opts

    % Add uniform noise of outsigma to the points chosen to be outliers.
%    XYZ(:,ind) = XYZ(:,ind)  + (2*rand(3,opts)-1)*outsigma;

    XYZ(:,ind) = XYZ(:,ind)  +   sign(rand(3,opts)-.5).*(rand(3,opts)+1)*outsigma;

    % Display the cloud of points
    figure(1), clf, plot3(XYZ(1,:),XYZ(2,:),XYZ(3,:), 'r*');

    % Perform RANSAC fitting of the plane
    [Bfitted, P, inliers] = ransacfitplane(XYZ, t, feedback);

    fprintf('Original plane coefficients: ');
    fprintf('%8.3f ',B);
    fprintf('\nFitted plane coefficients:   ');
    fprintf('%8.3f ',Bfitted);
    fprintf('\n');

    % Display the triangular patch formed by the 3 points that gave the
    % plane of maximum consensus
    patch(P(1,:), P(2,:), P(3,:), 'g')

    box('on'), grid('on'), rotate3d('on')

    fprintf('\nRotate image so that planar patch is seen edge on\n');
    fprintf('If the fit has been successful the inlying points should\n');
    fprintf('form a line\n\n');


function [D] = wishart_kl (arg1,arg2,arg3,arg4)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   [D] = wishart_kl (B_q,B_p,alpha_q,alpha_p)
%
%   computes the divergence
%                /
%      D(q||p) = | q(x)*log(q(x)/p(x)) dx
%               /
%   between two k-dimensional Wishart propability density for C  given
%   scale matrix B and shape parameter alpha
%
%            alpha
%         |B|              alpha-(k+1)/2
%   p(x)= ------------- |C|              exp (-tr(BC))
%         Gamma (alpha)
%              k
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin == 4
    B_q = arg1; B_p = arg2; alpha_q = arg3; alpha_p = arg4;
elseif nargin == 2
    B_q = arg1.Gam_rate; alpha_q = arg1.Gam_shape;
    B_p = arg2.Gam_rate; alpha_p = arg2.Gam_shape;
else
    error('Incorrect number of input arguments');
end

if size(B_q)~=size(B_p)
  error('Distributions must have equal dimensions');
end

K=size(B_p,1);

try
    Lq = -logdet(B_q,'chol');
    Lp = -logdet(B_p,'chol');
catch
    disp('One of the two covariance matrices (or both) is not positive definite')
    disp('Check for NaNs in the time series. Also, are there enough time points?')
    error('Error computing the inverse of the covariance matrix.')
end

lZq = log(2) * (alpha_q*K/2)  - Lq * (-alpha_q/2) + K*(K-1)/4 * log(pi);
lZp = log(2) * (alpha_p*K/2)  - Lp * (-alpha_p/2) + K*(K-1)/4 * log(pi);

Lq = Lq + K * log(2);
Lp = Lp + K * log(2);

for k = 1:K
    lZq = lZq + gammaln(alpha_q/2+0.5-0.5*k);
    lZp = lZp + gammaln(alpha_p/2+0.5-0.5*k);
    Lq = Lq + psi(alpha_q/2+0.5-0.5*k);
    Lp = Lp + psi(alpha_p/2+0.5-0.5*k);
end

D = (alpha_q/2-0.5-0.5*K)*Lq - (alpha_p/2-0.5-0.5*K)*Lp ...
    - alpha_q * K / 2 + alpha_q * trace(B_p / B_q) / 2 + lZp - lZq;

return;


clc;
close all;
clearvars;
rng('default');
n = 10;
% Let's get a symmetric positive definite matrix
A = gallery('gcdmat', n);
% Compute its Cholesky decomposition
L = chol(A, 'lower')
b = randn(n, 1);
% solve the problem
x = L \b
x = spx.la.tris.forward_col(L, b)


% NORMAL_LOGPDF
%
% Y = NORMAL_LOGPDF(X)
% Y = NORMAL_LOGPDF(X, MU, SIGMA)
%
% MU is the location parameter in (-INF, INF), default 0
% SIGMA is the scale parameter in (0, INF)

% Last modified 2010-11-12
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function y = lognormal_logpdf(x, varargin)

if nargin <= 3
  if nargin == 1
    mu = 0;
    sigma = 1;
  else
    mu = varargin{1};
    sigma = varargin{2};
  end
  y = -0.5*log(2*pi) ...
      -log(sigma) ...
      -0.5*((x-mu)./sigma).^2;
end


% J. Shi and J. Malik,
% "Normalized Cuts and Image Segmentation",
% In Proc. IEEE Conf. Computer Vision and Pattern Recognition,
% pages 731-737, 1997.

% Asad Ali
% GIK Institute of Engineering Sciences & Technology, Pakistan
% Email: asad_82@yahoo.com

% CONCEPT: Introduced the use of 2nd generalized eigenvector for clustering
clear all;
close all;

% generate the data
data = GenerateData(2);
figure,plot(data(:,1), data(:,2),'r+'),title('Original Data Points'); grid on;shg

affinity = CalculateAffinity(data);
figure,imshow(affinity,[]), title('Affinity Matrix');

% compute the degree matrix
for i=1:size(affinity,1)
    D(i,i) = sum(affinity(i,:));
end

% compute the unnormalized graph laplacian matrix
L = D - affinity;

[eigVectors,eigValues] = eig(L);

% plot the eigen vector corresponding to the 2nd largest eigen value
figure,plot(eigVectors(:,2),'r*'),title('2nd Largest Eigenvector');

% threshold the eigen vectors
[xx1,yy1,val1] = find(eigVectors(:,2) > 0.15);
[xx2,yy2,val2] = find(eigVectors(:,2) > 0 & eigVectors(:,2) < 0.15);
[xx3,yy3,val3] = find(eigVectors(:,2) < 0);

figure,
hold on;
plot(data(xx1,1),data(xx1,2),'m*');
plot(data(xx2,1),data(xx2,2),'b*');
plot(data(xx3,1),data(xx3,2),'g*');
hold off;
title('Clustering Results using 2nd Generalized Eigen Vector');
grid on;shg


% Mathematics Q2886713
% https://math.stackexchange.com/questions/2886713
% Prox Operator of L1 Norm with Linear Equality Constraint (Sum of Elements)
% References:
%   1.  aa
% Remarks:
%   1.  sa
% TODO:
% 	1.  ds
% Release Notes
% - 1.0.000     18/08/2018
%   *   First release.

%% General Parameters

subStreamNumberDefault = 79;

run('InitScript.m');

figureIdx           = 0; %<! Continue from Question 1
figureCounterSpec   = '%04d';

generateFigures = OFF;

%% Simulation Parameters

numElements = 10;

%% Generate Data

vY = 10 * randn(numElements, 1);

paramGamma  = 0;
paramB      = 1;

%% Solution by CVX

cvx_begin('quiet')
    cvx_precision('best');
    variable vX(numElements);
    minimize( 0.5 * pow_pos(norm(vX - vY, 2), 2) + (paramGamma * norm(vX ,1)) );
    subject to
        sum(vX) == paramB;
cvx_end

disp([' ']);
disp(['CVX Solution Summary']);
disp(['The CVX Solver Status - ', cvx_status]);
disp(['The Optimal Value Is Given By - ', num2str(cvx_optval)]);
disp(['The Optimal Argument Is Given By - [ ', num2str(vX(:).'), ' ]']);
disp([' ']);

%% Solution by Prox Function (Closed Form Solution)
%{
Solving
%}

hObjFun = @(vX) (0.5 * sum((vX - vY) .^ 2)) + (paramGamma * norm(vX ,1));

vXRef = vX;

vX = ProxL1NormSum(vY, paramGamma, paramB);

disp([' ']);
disp(['Projected Gradient Descent Solution Summary']);
disp(['The Optimal Value Is Given By - ', num2str(hObjFun(vX))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(vX(:).'), ' ]']);
disp([' ']);

%% Display Results

figureIdx = figureIdx + 1;

if(generateFigures == ON)
    saveas(hFigure,['Figure', num2str(figureIdx, figureCounterSpec), '.png']);
end

%% Restore Defaults

% set(0, 'DefaultFigureWindowStyle', 'normal');
% set(0, 'DefaultAxesLooseInset', defaultLoosInset);


function test06 ( dim_num, level_max )

%*****************************************************************************80
%
%% TEST06 creates a sparse Clenshaw-Curtis grid and writes it to a file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 August 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer LEVEL_MAX, the level.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST06:\n' );
  fprintf ( 1, '  SPARSE_GRID_CC makes a sparse Clenshaw-Curtis grid.\n' );
  fprintf ( 1, '  Write the data to a set of quadrature files.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  LEVEL_MAX = %d\n', level_max );
  fprintf ( 1, '  Spatial dimension DIM_NUM = %d\n', dim_num );
%
%  Determine the number of points.
%
  point_num = sparse_grid_cfn_size ( dim_num, level_max );

  r(1:dim_num,1) = -1.0;
  r(1:dim_num,2) = +1.0;
%
%  Compute the weights and points.
%
  [ w, x ] = sparse_grid_cc ( dim_num, level_max, point_num );
%
%  Write the data out.
%
  r_filename = sprintf ( 'cc_d%d_level%d_r.txt', dim_num, level_max );
  w_filename = sprintf ( 'cc_d%d_level%d_w.txt', dim_num, level_max );
  x_filename = sprintf ( 'cc_d%d_level%d_x.txt', dim_num, level_max );

  r8mat_write ( r_filename, dim_num, 2,         r );
  r8mat_write ( w_filename, 1,       point_num, w );
  r8mat_write ( x_filename, dim_num, point_num, x );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  R data written to "%s".\n', r_filename );
  fprintf ( 1, '  W data written to "%s".\n', w_filename );
  fprintf ( 1, '  X data written to "%s",\n', x_filename );

  return
end


function [a1, b1, c1, a2, b2, c2] = aaa(A, B, C)
%AAA  gives both solutions to the angle-angle-angle problem, in radians.
%
%   AAA(A, B, C) will result in NaNs if the existence condition
%   |pi - |A|-|B|| <= |C| <= pi - ||A| - |B||
%   is not met.
%
%   See also AAAD.

% Rody P.S. Oldenhuis
% Delft University of Technology
% oldenhuis@gmail.com
%
% Created    : 23/Feb/2009
% Last edited: 30/Nov/2012

    % first solution
    a1 = acos2( (cos(A) + cos(B).*cos(C)) ./ (sin(B).*sin(C)),  A );
    b1 = acos2( (cos(B) + cos(A).*cos(C)) ./ (sin(A).*sin(C)),  B );
    c1 = acos2( (cos(C) + cos(A).*cos(B)) ./ (sin(A).*sin(B)),  C );

    % second solution
    a2 = 2*pi - a1;
    b2 = 2*pi - b1;
    c2 = 2*pi - c1;

    % check constraints
    indices = ( ...
        abs(pi - abs(A)-abs(B)) > abs(C) | ...
        abs(C) > pi - abs(abs(A)-abs(B)) );
    a1(indices) = NaN;    a2(indices) = NaN;
    b1(indices) = NaN;    b2(indices) = NaN;
    c1(indices) = NaN;    c2(indices) = NaN;

end


function p = matrix_T_pdf(A, M, V, K, n)
% MATRIX_T_PDF Evaluate the density of a matrix under a Matrix-T distribution
% p = matrix_T_pdf(A, M, V, K, n)

% See "Bayesian Linear Regression", T. Minka, MIT Tech Report, 2001

[d m] = size(K);
is = 1:d;
c1 = prod(gamma((n+1-is)/2)) / prod(gamma((n-m+1-is)/2));
c2 = det(K)^(d/2) / det(pi*V)^(m/2); %% pi or 2pi?
p = c1 * c2 * det((A-M)'*inv(V)*(A-M)*K + eye(m))^(-n/2);


function arcsinh_values_test ( )

%*****************************************************************************80
%
%% ARCSINH_VALUES_TEST demonstrates the use of ARCSINH_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 June 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ARCSINH_VALUES_TEST:\n' );
  fprintf ( 1, '  ARCSINH_VALUES stores values of \n' );
  fprintf ( 1, '  the hyperbolic arc sine function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X           FX\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx ] = arcsinh_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %24.16f\n', x, fx );

  end

  return
end


function geometry_test027 ( )

%*****************************************************************************80
%
%% TEST027 tests ELLIPSE_POINTS_ARC_2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  n = 13;
  dim_num = 2;

  center(1:dim_num) = [ 5.0, -2.0 ];
  r1 = 3.0;
  r2 = 1.0;
  psi = pi / 6.0;
  theta1 = pi / 2.0;
  theta2 = 2.0 * pi;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST027\n' );
  fprintf ( 1, '  ELLIPSE_POINTS_ARC_2D returns points on an\n' );
  fprintf ( 1,'     elliptical arc.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The ellipse has center %f  %f\n', center(1:dim_num) );
  fprintf ( 1, '  radii R1 = %f, R2 = %f\n', r1, r2 );
  fprintf ( 1, '  and angle PSI = %f\n', psi );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The arc extends from THETA1 = %f\n', theta1 );
  fprintf ( 1, '  to THETA2 = %f\n', theta2 );

  p = ellipse_points_arc_2d ( center, r1, r2, psi, theta1, theta2, n );

  r8mat_transpose_print ( dim_num, n, p, '  Sample points:' );

  return
end


function M=movsp2wv(N,Np);
%MOVSP2WV Movie illustrating the passage from the spectrogram to the WVD.
%	M=MOVSP2WV(N,Np) generates the movie frames illustrating the passage
%	from the spectrogram to the WVD using different smoothing gaussian
%	windows in the smoothed pseudo-WVD.
%
%	N : number of points for the signal;
%	Np : number of snapshots (default : 8)
%	M : matrix of movie frames.
%
%	Example :
%	 M=movsp2wv(128,15); movie(M,5);

%	O. Lemoine - June 1996.
%	Copyright (c) 1996 by CNRS (France).
%
%  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 should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

if nargin<1,
 error('At least one argument required');
elseif nargin==1,
 Np=8;
end
Np=odd(Np)-1;

M  = moviein(Np);

t1=N/4; t2=3*N/4; t3=t1; t4=t2;
f1=0.125; f2=f1; f3=0.375; f4=f3;
t=1:N;
T=sqrt(2*N);

sig1=amgauss(N,t1,T).*fmconst(N,f1,t1);
sig2=amgauss(N,t2,T).*fmconst(N,f2,t2);
sig3=amgauss(N,t3,T).*fmconst(N,f3,t3);
sig4=amgauss(N,t4,T).*fmconst(N,f4,t4);

sig=sig1+sig2+sig3+sig4;

h=tftb_window(49,'gauss');
[tfr,t,f]=tfrsp(sig,t,N,h); tfr=tfr(1:N/2,:); f=f(1:N/2);
Max=max(max(tfr)); V=[0.1 0.3 0.5 0.7 0.9]*Max;
contour(t,f,tfr,V);
xlabel('Time'); ylabel('Frequency'); axis('xy')
M(:,1) = getframe;

Ng0=odd(fliplr(linspace(3,N/4,Np/2-1)));
Nh0=odd(linspace(N/2,9*N/10,Np/2-1));

for k=2:Np/2,
 g=tftb_window(Ng0(k-1),'gauss');
 h=tftb_window(Nh0(k-1),'gauss');
 [tfr,t,f]=tfrspwv(sig,t,N,g,h);
 Max=max(max(tfr)); V=[0.1 0.3 0.5 0.7 0.9]*Max;
 contour(t,f,tfr,V);
 xlabel('Time'); ylabel('Frequency'); axis('xy')
 M(:,k) = getframe;
end

[tfr,t,f]=tfrwv(sig);
Max=max(max(tfr)); V=[0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9]*Max;
contour(t,f,tfr,V);
xlabel('Time'); ylabel('Frequency'); axis('xy')
M(:,Np/2+1) = getframe;

for k=Np/2+2:Np,
 M(:,k) =M(:,Np+2-k);
end


function circle_segment_test11 ( )

%*****************************************************************************80
%
%% CIRCLE_SEGMENT_TEST11 demonstrates CIRCLE_SEGMENT_ROTATION_FROM_CHORD.
%
%  Discussion:
%
%    We make a table of all pairs of angles that are multiples of pi/12.
%
%    For each pair, we compute the rotation, that is, the angle of the
%    central radius of the circle segment.  We print out the result in
%    terms of multiples of pi/12.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 May 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CIRCLE_SEGMENT_TEST11:\n' );
  fprintf ( 1, '  CIRCLE_SEGMENT_ROTATION_FROM_CHORD is given the endpoints\n' );
  fprintf ( 1, '  of a chord, and is asked to determine the angle of the\n' );
  fprintf ( 1, '  central radius vector.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  We make a table of all pairs of angles that are multiples\n' );
  fprintf ( 1, '  of pi/12, determine the corresponding chord endpoints, and\n' );
  fprintf ( 1, '  compute the rotation angle, also printed as a multiple of pi/12.\n' );

  r = 2.0;
  c = [ 3.0; 5.0 ];
  fprintf ( 1, '\n' );
  fprintf ( 1, '            0.0   1.0   2.0   3.0   4.0   5.0   6.0   7.0   8.0   9.0  10.0  11.0  12.0\n' );
  fprintf ( 1, '\n' );
  for i = 0 : 12
    rho1 = i * pi / 6;
    p1 = c + r * [ cos(rho1); sin(rho1) ];
    fprintf ( 1, '  %2d.0:  ', i );
    for j = 0 : 12
      rho2 = j * pi / 6;
      p2 = c + r * [ cos(rho2); sin(rho2) ];
      alpha = circle_segment_rotation_from_chord ( r, c, p1, p2 );
      fprintf ( 1, '  %4.1f', round ( 10 * 6 * alpha / pi ) / 10 );
    end
    fprintf ( 1, '\n' );
  end

  return
end


% P2.7
clear;  clc;
n1 = 0:20;
x1 = 0.9.^n1;
subplot(3,2,1); stem(n1,x1,'ko');  title('First given sequence x1(n)');
axis([-20 20 0 1]);
[x1f,n1f] = sigfold(x1,n1);                 % obtain x1(-n)
n2 = -20:0;
x2 = 0.8.^(-n2);
subplot(3,2,2); stem(n2,x2,'ko');  title('Second given sequence x2(n)');
axis([-20 20 0 1]);
[x2f,n2f] = sigfold(x2,n2);                 % obtain x2(-n)
[r,nr] = conv_m(x1,n1,x1f,n1f);             % auto-correlation
subplot(3,2,3); stem(nr,r,'ko');  title('Auto-Correlation x1(n)*x1(-n)');
[r,nr] = conv_m(x2,n2,x2f,n2f);             % auto-correlation
subplot(3,2,4); stem(nr,r,'ko');  title('Auto-Correlation x2(n)*x2(-n)');
[r,nr] = conv_m(x1f,n1f,x2,n2);             % cross-correlation
subplot(3,1,3); stem(nr,r,'ko');  title('Cross-Correlation');


function [w, dwdx, dwdxx] = Weight(type, para, di, dmI)
% EVALUATE WEIGHT FUNCTION
%
% SYNTAX: [w, dwdr, dwdrr] = GaussWeight(type, para, di, dmi)
%
% INPUT PARAMETERS
%    type - Type of weight function
%    para - Weight function parameter
%    di   - Distance
%    dmi  - Support size
% OUTPUT PARAMETERS
%    w    - Value of weight function at r
%    dwdx - Value of first order derivative of weight function with respect to x at r
%    dwdxx- Value of Second order derivative of weight function with respect to x at r
%
r  = abs(di) / dmI;

if (di >= 0.0)
   drdx = 1.0/dmI;
else
   drdx = -1.0/dmI;
end

% EVALUATE WEIGHT FUNCTION AND ITS FIRST AND SECOND ORDER OF DERIVATIVES WITH RESPECT r AT r
if (type == 'GAUSS')
   [w,dwdr,dwdrr] = Gauss(para,r);
elseif (type == 'CUBIC')
   [w,dwdr,dwdrr] = Cubic(r);
elseif (type == 'SPLIN')
   [w,dwdr,dwdrr] = Spline(r);
elseif (type == 'power')
   [w,dwdr,dwdrr] = power_function(para,r);
elseif (type == 'CSRBF')
   [w,dwdr,dwdrr] = CSRBF2(r);
else
   error('Invalid type of weight function.');
end

dwdx  = dwdr * drdx;
dwdxx = dwdrr * drdx * drdx;

function [w,dwdr,dwdrr] = Gauss(beta,r)
if (r>1.0)
   w     = 0.0;
   dwdr  = 0.0;
   dwdrr = 0.0;
else
   b2 = beta*beta;
   r2 = r*r;
   eb2 = exp(-b2);

   w     = (exp(-b2*r2) - eb2) / (1.0 - eb2);
   dwdr  = -2*b2*r*exp(-b2*r2) / (1.0 - eb2);
   dwdrr = -2*b2*exp(-b2*r2)*(1-2*b2*r2) / (1.0 - eb2);
end

function [w,dwdr,dwdrr] = Cubic(r)
if (r>1.0)
   w     = 0.0;
   dwdr  = 0.0;
   dwdrr = 0.0;
else
   w     = 1-6*r^2+8*r^3-3*r^4;
   dwdr  = -12*r+24*r^2-12*r^3;
   dwdrr = -12+48*r-36*r^2;
end

function [w,dwdr,dwdrr] = power_function(arfa,r)
if (r>1.0)
   w     = 0.0;
   dwdr  = 0.0;
   dwdrr = 0.0;
else
    a2 = arfa*arfa;
   r2 = r*r;
    w     = exp(-r2/a2);
   dwdr  = (-2*r/a2)*exp(-r2/a2);
   dwdrr  = (-2/a2+(-2*r/a2).^2)*exp(-r2/a2);
end

function [w,dwdr,dwdrr] = Spline(r)
if (r>1.0)
   w     = 0.0;
   dwdr  = 0.0;
   dwdrr = 0.0;
elseif (r<=0.5)
   w     = 2/3 - 4*r^2 + 4*r^3;
   dwdr  = -8*r + 12*r^2;
   dwdrr = -8 + 24*r;
else
   w     = 4/3 - 4*r + 4*r^2 - 4*r^3/3;
   dwdr  = -4 + 8*r -4*r^2;
   dwdrr = 8 - 8*r;
end

function [w,dwdr,dwdrr] = CSRBF2(r)
if (r>1.0)
   w     = 0.0;
   dwdr  = 0.0;
   dwdrr = 0.0;
else
	w     = (1-r)^6*(6+36*r+82*r^2+72*r^3+30*r^4+5*r^5);
	dwdr  = 11*r*(r+2)*(5*r^3+15*r^2+18*r+4)*(r-1)^5;
   dwdrr = 22*(25*r^5+100*r^4+142*r^3+68*r^2-16*r-4)*(r-1)^4;
end


function [ dmap ] = dataDensity( x, y, width, height, limits, fudge )
%DATADENSITY Get a data density image of data
%   x, y - two vectors of equal length giving scatterplot x, y co-ords
%   width, height - dimensions of the data density plot, in pixels
%   limits - [xmin xmax ymin ymax] - defaults to data max/min
%   fudge - the amount of smear, defaults to size of pixel diagonal
%
% By Malcolm McLean
%
    if(nargin == 4)
        limits(1) = min(x);
        limits(2) = max(x);
        limits(3) = min(y);
        limits(4) = max(y);
    end
    deltax = (limits(2) - limits(1)) / width;
    deltay = (limits(4) - limits(3)) / height;
    if(nargin < 6)
        fudge = sqrt(deltax^2 + deltay^2);
    end
    dmap = zeros(height, width);
    for ii = 0: height - 1
        yi = limits(3) + ii * deltay + deltay/2;
        for jj = 0 : width - 1
            xi = limits(1) + jj * deltax + deltax/2;
            dd = 0;
            for kk = 1: length(x)
                dist2 = (x(kk) - xi)^2 + (y(kk) - yi)^2;
                dd = dd + 1 / ( dist2 + fudge);
            end
            dmap(ii+1,jj+1) = dd;
        end
    end

end


function [ov ov_n1 ov_n2] = calc_overlap(dres1, f1, dres2, f2)
%%f2 can be an array and f1 should be a scalar.
%%% this will find the overlap between dres1(f1) (only one) and all detection windows in dres2(f2(:))

f2=f2(:)';
n = length(f2);

cx1 = dres1.x(f1);
cx2 = dres1.x(f1)+dres1.w(f1)-1;
cy1 = dres1.y(f1);
cy2 = dres1.y(f1)+dres1.h(f1)-1;

gx1 = dres2.x(f2);
gx2 = dres2.x(f2)+dres2.w(f2)-1;
gy1 = dres2.y(f2);
gy2 = dres2.y(f2)+dres2.h(f2)-1;

ca = dres1.h(f1).*dres1.w(f1);  %% area
ga = dres2.h(f2).*dres2.w(f2);

%%% find the overlapping area
xx1 = max(cx1, gx1);
yy1 = max(cy1, gy1);
xx2 = min(cx2, gx2);
yy2 = min(cy2, gy2);
w = xx2-xx1+1;
h = yy2-yy1+1;

inds  = find((w > 0) .* (h > 0));  %% real overlap
ov    = zeros(1, n);
ov_n1 = zeros(1, n);
ov_n2 = zeros(1, n);
inter = w(inds).*h(inds);         %% area of overlap
u     = ca + ga(inds) - w(inds).*h(inds);     %% area of union
ov(inds)    = inter ./ u;                       %% intersection / union
ov_n1(inds) = inter / ca;                       %% intersection / area in dres1
ov_n2(inds) = inter ./ ga(inds);                  %% intersection / area in dres2


function variance = gamma_variance ( a, b, c )

%*****************************************************************************80
%
%% GAMMA_VARIANCE returns the variance of the Gamma PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, C, the parameters of the PDF.
%    0.0 < B,
%    0.0 < C.
%
%    Output, real VARIANCE, the variance of the PDF.
%
  variance = b * b * c;

  return
end


function [r,normr,nre,s] = reorth(Q,r,normr,index,alpha,method)

%REORTH   Reorthogonalize a vector using iterated Gram-Schmidt
%
%   [R_NEW,NORMR_NEW,NRE] = reorth(Q,R,NORMR,INDEX,ALPHA,METHOD)
%   reorthogonalizes R against the subset of columns of Q given by INDEX.
%   If INDEX==[] then R is reorthogonalized all columns of Q.
%   If the result R_NEW has a small norm, i.e. if norm(R_NEW) < ALPHA*NORMR,
%   then a second reorthogonalization is performed. If the norm of R_NEW
%   is once more decreased by  more than a factor of ALPHA then R is
%   numerically in span(Q(:,INDEX)) and a zero-vector is returned for R_NEW.
%
%   If method==0 then iterated modified Gram-Schmidt is used.
%   If method==1 then iterated classical Gram-Schmidt is used.
%
%   The default value for ALPHA is 0.5.
%   NRE is the number of reorthogonalizations performed (1 or 2).

% References:
%  Aake Bjorck, "Numerical Methods for Least Squares Problems",
%  SIAM, Philadelphia, 1996, pp. 68-69.
%
%  J.~W. Daniel, W.~B. Gragg, L. Kaufman and G.~W. Stewart,
%  ``Reorthogonalization and Stable Algorithms Updating the
%  Gram-Schmidt QR Factorization'', Math. Comp.,  30 (1976), no.
%  136, pp. 772-795.
%
%  B. N. Parlett, ``The Symmetric Eigenvalue Problem'',
%  Prentice-Hall, Englewood Cliffs, NJ, 1980. pp. 105-109

%  Rasmus Munk Larsen, DAIMI, 1998.

% Check input arguments.
% warning('PROPACK:NotUsingMex','Using slow matlab code for reorth.')
if nargin<2
  error('Not enough input arguments.')
end
[n k1] = size(Q);
if nargin<3 | isempty(normr)
%  normr = norm(r);
  normr = sqrt(r'*r);
end
if nargin<4 | isempty(index)
  k=k1;
  index = [1:k]';
  simple = 1;
else
  k = length(index);
  if k==k1 & index(:)==[1:k]'
    simple = 1;
  else
    simple = 0;
  end
end
if nargin<5 | isempty(alpha)
  alpha=0.5; % This choice garanties that
             % || Q^T*r_new - e_{k+1} ||_2 <= 2*eps*||r_new||_2,
             % cf. Kahans ``twice is enough'' statement proved in
             % Parletts book.
end
if nargin<6 | isempty(method)
   method = 0;
end
if k==0 | n==0
  return
end
if nargout>3
  s = zeros(k,1);
end

normr_old = 0;
nre = 0;
while normr < alpha*normr_old | nre==0
  if method==1
    if simple
      t = Q'*r;
      r = r - Q*t;
    else
      t = Q(:,index)'*r;
      r = r - Q(:,index)*t;
    end
  else
    for i=index,
      t = Q(:,i)'*r;
      r = r - Q(:,i)*t;
    end
  end
  if nargout>3
    s = s + t;
  end
  normr_old = normr;
%  normr = norm(r);
  normr = sqrt(r'*r);
  nre = nre + 1;
  if nre > 4
    % r is in span(Q) to full accuracy => accept r = 0 as the new vector.
    r = zeros(n,1);
    normr = 0;
    return
  end
end


function [all_theta] = oneVsAll(X, y, num_labels, lambda)
%ONEVSALL trains multiple logistic regression classifiers and returns all
%the classifiers in a matrix all_theta, where the i-th row of all_theta
%corresponds to the classifier for label i
%   [all_theta] = ONEVSALL(X, y, num_labels, lambda) trains num_labels
%   logisitc regression classifiers and returns each of these classifiers
%   in a matrix all_theta, where the i-th row of all_theta corresponds
%   to the classifier for label i

% Some useful variables
m = size(X, 1);
n = size(X, 2);

% You need to return the following variables correctly
all_theta = zeros(num_labels, n + 1);

% Add ones to the X data matrix
X = [ones(m, 1) X];

% ====================== YOUR CODE HERE ======================
% Instructions: You should complete the following code to train num_labels
%               logistic regression classifiers with regularization
%               parameter lambda.
%
% Hint: theta(:) will return a column vector.
%
% Hint: You can use y == c to obtain a vector of 1's and 0's that tell use
%       whether the ground truth is true/false for this class.
%
% Note: For this assignment, we recommend using fmincg to optimize the cost
%       function. It is okay to use a for-loop (for c = 1:num_labels) to
%       loop over the different classes.
%
%       fmincg works similarly to fminunc, but is more efficient when we
%       are dealing with large number of parameters.
%
% Example Code for fmincg:
%
%     % Set Initial theta
%     initial_theta = zeros(n + 1, 1);
%
%     % Set options for fminunc
%     options = optimset('GradObj', 'on', 'MaxIter', 50);
%
%     % Run fmincg to obtain the optimal theta
%     % This function will return theta and the cost
%     [theta] = ...
%         fmincg (@(t)(lrCostFunction(t, X, (y == c), lambda)), ...
%                 initial_theta, options);
%

for c = 1:num_labels
    options = optimset('GradObj', 'on', 'MaxIter', 50);

    all_theta(c, :) = fmincg (@(t)(lrCostFunction(t, X, (y == c), lambda)), ...
            zeros(n + 1, 1), options);

% =========================================================================

end


function varargout = createTetrakaidecahedron()
%CREATETETRAKAIDECAHEDRON Create a 3D mesh representing a tetrakaidecahedron.
%
%   [V, E, F] = createTetrakaidecahedron;
%   Create a mesh structure representing a tetrakaidecahedron, composed of
%   both square and hexagonal faces. Tetrakaidecahedron can be used to tile
%   the 3D Euclidean space.
%
%   V is a 24-by-3 array with vertex coordinates,
%   E is a 36-by-2 array containing indices of neighbour vertices,
%   F is a 14-by-1 cell array containing vertex indices array of each face.
%
%   [V, F] = createTetrakaidecahedron;
%   Returns only the vertices and the face vertex indices.
%
%   MESH = createTetrakaidecahedron;
%   Returns the data as a mesh structure, with fields 'vertices', 'edges'
%   and 'faces'.
%
%   Example
%   [n, e, f] = createTetrakaidecahedron;
%   drawMesh(n, f);
%
%   See also
%   meshes3d, drawMesh

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2005-02-10
% Copyright 2005-2022 INRA - TPV URPOI - BIA IMASTE

nodes = [...
    1 0 2;0 1 2;-1 0 2;0 -1 2;...
    2 0 1;0 2 1;-2 0 1;0 -2 1;...
    2 1 0;1 2 0;-1 2 0;-2 1 0;-2 -1 0;-1 -2 0;1 -2 0;2 -1 0;...
    2 0 -1;0 2 -1;-2 0 -1;0 -2 -1;...
    1 0 -2;0 1 -2;-1 0 -2;0 -1 -2];

edges = [...
    1 2;1 4;1 5;2 3;2 6;3 4;3 7;4 8;...
    5 9;5 16;6 10;6 11;7 12;7 13;8 14;8 15;...
    9 10;9 17;10 18;11 12;11 18;12 19;13 14;13 19;14 20;15 16;15 20;16 17;....
    17 21;18 22;19 23;20 24;21 22;21 24;22 23;23 24];

faces = {...
    [1 2 3 4], ...
    [1 4 8 15 16 5], [1 5 9 10 6 2], [2 6 11 12 7 3], [3 7 13 14 8 4],...
    [5 16 17 9], [6 10 18 11], [7 12 19 13], [8 14 20 15],...
    [9 17 21 22 18 10], [11 18 22 23 19 12], [13 19 23 24 20 14], [15 20 24 21 17 16], ...
    [21 24 23 22]};
faces = faces';

% format output
varargout = formatMeshOutput(nargout, nodes, edges, faces);


function FBC = FbCrop( FB, delta )
% Crop a 2D filterbank (adjusting filter norms).
%
% Takes a filter bank and crops it by cropping off delta pixels from each
% side. Ensures that the mean response of each filter is 0 and that the L1
% norm is 1, i.e. sum(sum(abs(F)))==1.
%
% USAGE
%  FBC = FbCrop( FB, delta )
%
% INPUTS
%  FB       - original filterbank
%  delta    - amount to crop by
%
% OUTPUTS
%  FBC      - cropped filterbank
%
% EXAMPLE
%  load FbDoG.mat;  FBC=FbCrop(FB,4);
%  figure(1); montage2(FB,struct('extraInfo',1));
%  figure(2); montage2(FBC,struct('extraInfo',1));
%
% See also FBAPPLY2D
%
% Piotr's Computer Vision Matlab Toolbox      Version 2.0
% Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

nd = ndims(FB);
if( nd~=2 && nd~=3 ); error('I must an MxNxK array'); end

cropsiz = size(FB);
cropsiz = [cropsiz(1:2)-2*delta, cropsiz(3)];
FBC = arrayToDims( FB, cropsiz );

for f=1:size(FB,3)
  FC = FBC(:,:,f);
  FC = FC - sum(sum(FC)) / numel(FC); % 0 mean
  FC = FC / sum(sum(abs(FC))); % L1 norm == 1
  FBC(:,:,f) = FC;
end


function xyz = plane_normal_qr_to_xyz ( pp, normal, pq, pr, n, qr )

%*****************************************************************************80
%
%% PLANE_NORMAL_QR_TO_XYZ: QR_TO_XYZ coordinates for a normal form plane.
%
%  Discussion:
%
%    The normal form of a plane in 3D is:
%
%      PP is a point on the plane,
%      NORMAL is a normal vector to the plane.
%
%    Two vectors PQ and PR can be computed with the properties that
%    * NORMAL, PQ and PR are pairwise orthogonal;
%    * PQ and PR have unit length;
%    * every point P in the plane has a "QR" representation
%      as P = PP + q * PQ + r * PR.
%
%    This function is given the QR coordinates of a set of points on the
%    plane, and returns the XYZ coordinates.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 November 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real PP(3), a point on the plane.
%
%    Input, real NORMAL(3), a normal vector N to the plane.  The
%    vector must not have zero length, but it is not necessary for N
%    to have unit length.
%
%    Input, real PQ(3), a vector of unit length,
%    perpendicular to the vector N and the vector PR.
%
%    Input, real PR(3), a vector of unit length,
%    perpendicular to the vector N and the vector PQ.
%
%    Input, integer N, the number of points on the plane.
%
%    Input, real QR(2,N), the QR coordinates of the points.
%
%    Output, real XYZ(3,N), the XYZ coordinates of the points.
%
  xyz = repmat ( pp, 1, n ) + [ pq'; pr' ]' * qr(1:2,1:n);

  return
end


function [ a, seed ] = markov_random ( n, key )

%*****************************************************************************80
%
%% MARKOV_RANDOM returns a random Markov matrix.
%
%  Discussion:
%
%    A Markov matrix, also called a "stochastic" matrix, is distinguished
%    by two properties:
%
%    * All matrix entries are nonnegative;
%    * The sum of the entries in each row is 1.
%
%    A "transition matrix" is the transpose of a Markov matrix, and
%    has column sums equal to 1.
%
%  Example:
%
%    N = 4
%
%    1/10  2/10  3/10  4/10
%    1     0     0     0
%    5/10  2/10  3/10  0
%    2/10  2/10  2/10  4/10
%
%  Properties:
%
%    A is generally not symmetric: A' /= A.
%
%    0 <= A(I,J) <= 1.0 for every I and J.
%
%    The sum of the entries in each row of A is 1.
%
%    Because it has a constant row sum of 1,
%    A has an eigenvalue of 1, and
%    a (right) eigenvector of ( 1, 1, 1, ..., 1 ).
%
%    All the eigenvalues of A have modulus no greater than 1.
%
%    The eigenvalue 1 lies on the boundary of all the Gerschgorin rowsum disks.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Input, integer KEY, a positive value that selects the data.
%
%    Output, real A(N,N), the matrix.
%
  seed = key;

  [ a, seed ] = r8mat_uniform_01 ( n, n, seed );

  for i = 1 : n

    row_sum = sum ( a(i,1:n) );

    a(i,1:n) = a(i,1:n) / row_sum;

  end

  return
end


% function qs = equantile(X, ps, dim)
%
% Calculates empirical (non-smoothed) quantiles of multidimensional
% arrays.  Ignores NaN entries.  Faster than, but similar to, the
% quantile() function in the MATLAB Statistics toolbox.
%
% Input:
%  X: multidimensional vector/matrix over which to calculate
%     empirical quantiles.
%  ps: vector of quantile values (between 0 and 1)
%  dim: (optional) dimension along which to calculate quantiles
%
% Output:
%  qs: vector/matrix of empirical quantiles
%
%---------------------------------------------------------------------------------
%    Author: Eugene Brevdo (http://www.math.princeton.edu/~ebrevdo/)
%---------------------------------------------------------------------------------
function qs = equantile(X,ps,dim)

ndX = ndims(X);
sX = size(X);
psn = length(ps);

assert(all(ps >= 0 & ps <= 1));

needsort = 0;
if nargin<3
  % Sort properly depending on data
  if isvector(X) && ~issorted(X)
    needsort = 1;
  elseif ndX <= 2 && ~issorted(X,'rows')
    needsort = 1;
  elseif ndX > 2
    needsort = 1;
  end

  % Which dimension are we calculating quantiles along?
  if isvector(X)
    [tmpvar,mdim] = max(sX);
    dim = mdim;
  else
    dim = 1;
  end
else
  needsort = 1;
end

% Reshape data to make ensuing steps easier (quantile-dim becomes first dim)
dperm = 1:ndX;
dperm([1 dim]) = dperm([dim 1]);
if (dim ~= 1)
  X = permute(X, dperm);
end
sXperm = sX(dperm);

% Reshape to 2D; we calculate quantiles along first dimension
X = reshape(X, sX(dim), prod(sX([1:dim-1, dim+1:ndX])));

if (needsort),
  X = sort(X,1);
end

% If we see NaNs, need to calculate quantiles row-by-row.
% Otherwise can do it in one fell swoop.
Xnan = isnan(X);
if any(any(Xnan,2))
  hasnan = 1;
else
  hasnan = 0;
end

if (~hasnan)
  ks = round(ps*(sX(dim)-1) + 1);
  assert(all(ks >= 1 & ks <= sX(dim)));
  qs = X(ks,:);
else % if (hasnan)
  ns = zeros(size(X,2), 1);
  qs = nan(psn, size(X,2));

  % Limit ourselves to non-NaN values
  for ci=1:size(X,2)
    ni = find(Xnan(:, ci), 1, 'first')-1;
    % If don't find any NaNs, use full dim
    if isempty(ni), ni = sX(dim); end

    ns(ci) = ni;
  end

  % Calculate quantiles in each case
  Xcol = 1:size(X,2);
  for pi=1:psn
    p = ps(pi);
    ks = round(p*(ns-1)+1);
    % Pull out the proper elements of sorted X
    qs(pi,ks>0) = X(sub2ind(size(X), ks(ks>0)', Xcol(ks>0)));
  end
end

% Convert back to ndim array
qs = reshape(qs, [psn, sX(1:dim-1), sX(dim+1:ndX)]);

% Move coordinates back to their proper places
if (dim ~= 1)
  qs = ipermute(qs, dperm);
end


function geometry_test007 ( )

%*****************************************************************************80
%
%% TEST007 tests BALL_UNIT_SAMPLE_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  dim_num = 3;
  n_sample = 1000;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST007\n' );
  fprintf ( 1, '  For the unit ball in 3 dimensions:\n' );
  fprintf ( 1, '  BALL_UNIT_SAMPLE_3D samples;\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  A few sample values:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : 5
    [ x, seed ] = ball_unit_sample_3d ( seed );
    fprintf ( 1, '  %10f  %10f  %10f\n', x(1:dim_num) );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of sample points = %d\n', n_sample );

  average(1:dim_num) = 0.0;

  for i = 1 : n_sample
    [ x, seed ] = ball_unit_sample_3d ( seed );
    average(1:dim_num) = average(1:dim_num) + x(1:dim_num);
  end

  average(1:dim_num) = average(1:dim_num) / n_sample;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Now average the points, which should get a value\n' );
  fprintf ( 1, '  close to zero, and closer as N_SAMPLE increases.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Average:        %10f  %10f  %10f\n', average(1:dim_num) );

  average_r = 0.0;

  for i = 1 : n_sample
    [ x, seed ] = ball_unit_sample_3d ( seed );
    r = sqrt ( sum ( x(1:dim_num).^2 ) );
    average_r = average_r + r;
  end

  average_r = average_r / n_sample;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Now average the distance of the points from\n' );
  fprintf ( 1, '  the center, which should be the \n' );
  fprintf ( 1, '  1/2**(1/dim_num) = %f\n', 0.5^( 1.0 / dim_num ) );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Average:        %f\n', average_r );

  average_theta = 0.0;

  for i = 1 : n_sample
    [ x, seed ] = ball_unit_sample_3d ( seed );
    theta = r8_atan ( x(2), x(1) );
    average_theta = average_theta + theta;
  end

  average_theta = average_theta / n_sample;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Now average the angle THETA,\n' );
  fprintf ( 1, '  which should be PI.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Average:        %f\n', average_theta );

  average_phi = 0.0;

  for i = 1 : n_sample
    [ x, seed ] = ball_unit_sample_3d ( seed );
    r = sqrt ( sum ( x(1:dim_num).^2 ) );
    phi = acos ( x(3) / r );
    average_phi = average_phi + phi;
  end

  average_phi = average_phi / n_sample;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Now average the angle PHI,\n' );
  fprintf ( 1, '  which should be PI/2.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Average:        %f\n', average_phi );

  return
end


function linplus_test525 ( )

%*****************************************************************************80
%
%% TEST525 tests R8PP_FA, R8PP_SL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  n = 5;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST525\n' );
  fprintf ( 1, '  R8PP_FA factors a R8PP system,\n' );
  fprintf ( 1, '  R8PP_SL solves a R8PP system.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N = %d\n', n );
%
%  Set the matrix.
%
  [ a, seed ] = r8pp_random ( n, seed );

  r8pp_print ( n, a, '  The R8PP matrix:' );
%
%  Set the desired solution.
%
  x = r8vec_indicator ( n );

  r8vec_print ( n, x, '  The desired solution:' );
%
%  Compute the corresponding right hand side.
%
  b = r8pp_mxv ( n, a, x );

  r8vec_print ( n, b, '  The right hand side:' );
%
%  Factor the matrix.
%
  [ a_lu, info ] = r8pp_fa ( n, a );

  if ( info ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Fatal error!\n' );
    fprintf ( 1, '  R8PP_FA declares the matrix is singular!\n' );
    fprintf ( 1, '  The value of INFO is %d\n', info );
    return
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The R8PP matrix has been factored.\n' );
%
%  Solve the linear system.
%
  x = r8pp_sl ( n, a_lu, b );

  r8vec_print ( n, x, '  Solution:' );

  return
end


function [node,elem] = cuboidmesh(cuboid,h)
%% CUBEHEXMESH uniform mesh of cuboid
%
% [node,elem] = cuboidmesh([x0,x1,y0,y1,z0,z1],h) generates a uniform mesh of the
% cuboid [x0,x1]*[y0,y1]*[z0,z1] with mesh size h.
%
% Example
%
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

x0 = cuboid(1); x1 = cuboid(2);
y0 = cuboid(3); y1 = cuboid(4);
z0 = cuboid(5); z1 = cuboid(6);

[x,y,z] = meshgrid(x0:h:x1,y0:h:y1,z0:h:z1);

node = [x(:),y(:),z(:)];

%% Generate elements
nx = size(x,1) - 1; % number of cells in x-direction
ny = size(y,2) - 1; % number of cells in y-direction
nz = size(z,2) - 1; % number of cells in z-direction

elem = zeros(nx*ny*nz,8);
cellidx = 1:nx*ny*nz;

[i, j, k] = ind2sub([nx ny nz],cellidx);  % index of cells in subscript form

s =[ nx+1, ny+1, nz+1];

elem(cellidx,1) = sub2ind3(s,i,j,k);
elem(cellidx,2) = sub2ind3(s,i,j+1,k);
elem(cellidx,3) = sub2ind3(s,i+1,j+1,k);
elem(cellidx,4) = sub2ind3(s,i+1,j,k);
elem(cellidx,5) = sub2ind3(s,i,j,k+1);
elem(cellidx,6) = sub2ind3(s,i,j+1,k+1);
elem(cellidx,7) = sub2ind3(s,i+1,j+1,k+1);
elem(cellidx,8) = sub2ind3(s,i+1,j,k+1);

    function idx = sub2ind3(siz,i,j,k)
        nr = siz(1); nc = siz(2); nv = siz(3);
        if (max(j)>nc) || (max(i)>nr) || (max(k)>nv)
            error(message('MATLAB:mysub2ind:IndexOutOfRange'));
        end
        idx = (k-1)*nr*nc + (j-1)*nr+i;
    end

end


function f=ref_idwilt(c,g,a,M)
%REF_DWILT  Reference Inverse Discrete Wilson Transform
%   Usage:  f=ref_idwilt(c,g,a,M);
%

% Setup transformation matrix.

L=size(g,1);
W=size(c,2);
N=L/a;

F=zeros(L,M*N);

% Weight coefficients.

l=(0:L-1).';

pif=0;

if 1
  % This version uses sines and cosines to express the basis functions.

  for n=0:N/2-1
    % Do the unmodulated coefficient.
    F(:,2*M*n+1)=circshift(g,2*a*n);

    % Setting this to -n*a should produce a time-invariant transform.
    timeinv=0; %-n*a;

    % m odd case
    for m=1:2:M-1
      F(:,m+2*M*n+1)   = sqrt(2)*sin(pi*m/M*(l+timeinv)+pif).*circshift(g,2*n*a);
      F(:,m+2*M*n+M+1) = sqrt(2)*cos(pi*m/M*(l+timeinv)+pif).*circshift(g,(2*n+1)*a);
    end;

    % m even case
    for m=2:2:M-1
      F(:,m+2*M*n+1)     = sqrt(2)*cos(pi*m/M*(l+timeinv)+pif).*circshift(g,2*n*a);
      F(:,m+2*M*n+M+1)   = sqrt(2)*sin(pi*m/M*(l+timeinv)+pif).*circshift(g,(2*n+1)*a);
    end;

    % Most modulated coefficient, Nyquest frequency.
    if mod(M,2)==0
      F(:,M+2*M*n+1)=(-1).^(l+timeinv).*circshift(g,2*n*a);
    else
      F(:,M+2*M*n+1)=(-1).^(l+timeinv).*circshift(g,(2*n+1)*a);
    end;
  end;

else

  % This version uses a cosine,

  for n=0:N/2-1
    % Do the unmodulated coefficient.
    F(:,2*M*n+1)=circshift(g,2*a*n);

    timeinv=-n*a;

    % m odd case
    for m=1:2:M-1
      F(:,m+2*M*n+1)   = sqrt(2)*cos(pi*m/M*(l+timeinv-M/2)+pif).*circshift(g,2*n*a);
      F(:,m+2*M*n+M+1) = sqrt(2)*sin(pi*m/M*(l+timeinv-M/2-a)+pif).*circshift(g,(2*n+1)*a);
    end;

    % m even case
    for m=2:2:M-1
      F(:,m+2*M*n+1)     = sqrt(2)*cos(pi*m/M*(l+timeinv-M/2)+pif).*circshift(g,2*n*a);
      F(:,m+2*M*n+M+1)   = sqrt(2)*sin(pi*m/M*(l+timeinv-M/2-a)+pif).*circshift(g,(2*n+1)*a);
    end;

    % Most modulated coefficient, Nyquest frequency.
    if mod(M,2)==0
      F(:,M+2*M*n+1)=(-1).^(l+timeinv).*circshift(g,2*n*a);
    else
      F(:,M+2*M*n+1)=(-1).^(l+timeinv-a).*circshift(g,(2*n+1)*a);
    end;
  end;

end;

f=F*c;


function [ L, D, C, Cinv ] = get_transform_matrices_2d( R, method )
%UNTITLED4 Summary of this function goes here
%   Detailed explanation goes here
if method == 1
    % LDL Without the permutation
    [L, D] = ldl(R);
    C = inv(L);
    Cinv = L;

elseif method == 2
    % Eigen Decomp
    [V,D] = eig(R);
    C = V.';
    Cinv = V;

elseif method == 3
    % Cholesky Decomp (Special case of LDL, D=I)
    L = chol(R);  % NOTE: this is the UPPER triangular transform
    L = L.';
    C = inv(L);
    Cinv = L;
    D = eye(2,2);

elseif method == 4  %%% ANALYTICAL
    rho = R(1,2);
    L = [1 0; rho 1];
    D = diag([1, 1 - rho^2]);
    C = [1 0; -rho 1];
    % C = inv(L);
    Cinv = L;
end

end


function R = RiemannND(x)
% Riemann's non-differentiable function, R(x) for rational x = p/q

[p,q] = rat(x);
R = 0;
for k = 1:q-1
    R = R + sin(k^2*p*pi/q)/(sin(k*pi/2/q))^2;
end
R = R*pi/4/q/q;
end