% Compute the closeness centrality for every vertex: 1/sum(dist to all other nodes)
%
% INPUTs: adjacency matrix, nxn
% OUTPUTs: vector of closeness centralities, nx1
%
% Source: social networks literature (example: Wasserman, Faust, "Social Networks Analysis")
% Other routines used: simpleDijkstra.m
% GB: last updated, Sep 28, 2012

function C=closeness(adj)

C=zeros(length(adj),1);  % initialize closeness vector

for i=1:length(adj); C(i)=1/sum( simpleDijkstra(adj,i) ); end


clear all

nInst = 500;
nVars = 200;
X = randn(nInst,nVars);
w = randn(nVars,1);
y = sign(X*w + randn(nInst,1));

w_init = zeros(nVars,1);
funObj = @(w)LogisticLoss(w,X,y);

fprintf('\nRunning Steepest Descent\n');
options.Method = 'sd';
minFunc(@LogisticLoss,w_init,options,X,y);
pause;

fprintf('\nRunning Cyclic Steepest Descent\n');
options.Method = 'csd';
minFunc(@LogisticLoss,w_init,options,X,y);
pause;

fprintf('\nRunning Conjugate Gradient\n');
options.Method = 'cg';
minFunc(@LogisticLoss,w_init,options,X,y);
pause;

fprintf('\nRunning Scaled Conjugate Gradient\n');
options.Method = 'scg';
minFunc(@LogisticLoss,w_init,options,X,y);
pause;

fprintf('\nRunning Preconditioned Conjugate Gradient (Diagonal preconditioner)\n');
options.Method = 'pcg';
options.precFunc = @LogisticDiagPrecond;
minFunc(@LogisticLoss,w_init,options,X,y);
pause;

fprintf('\nRunning Preconditioned Conjugate Gradient (L-BFGS preconditioner)\n');
options.Method = 'pcg';
options.precFunc = [];
minFunc(@LogisticLoss,w_init,options,X,y);
pause;

fprintf('\nRunning Hessian-Free Newton w/ numerical Hessian-Vector products\n');
options.Method = 'newton0';
minFunc(@LogisticLoss,w_init,options,X,y);
pause;

fprintf('\nRunning Preconditioned Hessian-Free Newton w/ numerical Hessian-Vector products (Diagonal preconditioner)\n');
options.Method = 'pnewton0';
options.precFunc = @LogisticDiagPrecond;
minFunc(@LogisticLoss,w_init,options,X,y);
pause;

fprintf('\nRunning Preconditioned Hessian-Free Newton w/ numerical Hessian-Vector products (L-BFGS preconditioner)\n');
options.Method = 'pnewton0';
options.precFunc = [];
minFunc(@LogisticLoss,w_init,options,X,y);
pause;

fprintf('\nRunning Hessian-Free Newton w/ analytic Hessian-Vector products\n');
options.Method = 'newton0';
options.HvFunc = @LogisticHv;
minFunc(@LogisticLoss,w_init,options,X,y);
pause;

fprintf('\nRunning Preconditioned Hessian-Free Newton w/ analytic Hessian-Vector products (Diagonal preconditioner)\n');
options.Method = 'pnewton0';
options.HvFunc = @LogisticHv;
options.precFunc = @LogisticDiagPrecond;
minFunc(@LogisticLoss,w_init,options,X,y);
pause;

fprintf('\nRunning Preconditioned Hessian-Free Newton w/ analytic Hessian-Vector products (L-BFGS preconditioner)\n');
options.Method = 'pnewton0';
options.precFunc = [];
options.HvFunc = @LogisticHv;
minFunc(@LogisticLoss,w_init,options,X,y);
pause;


function K=kronSym(A,B)
%%KRONSYM Take the symmetric Kronecker product of the matrices A and B. The
%         standard Kronecker product of two real, square matrices A and B,
%         one can write the following relation with respect to any real
%         square matrix S:
%         kron(A,B)*vec(S)==vec(B*S*A')
%         However, the symmetric Kronecker product is such that for any two
%         real square matrices A and B and a square SYMMETRIC matrix S, one
%         can write
%         kronSym(A,B)*vech(S,sqrt(2))==vech((1/2)*(B*S*A'+A*S*B'),sqrt(2))
%         Whereas is A and B are nXn, the standard kronecker product is
%         n^2Xn^2, the symmetric Kronecker product is
%         (n*(n+1)/2)X(n*(n+1)/2).
%
%INPUTS: A, B Two real nXn matrices. They need not be square.
%
%OUTPUTS: K The (n*(n+1)/2)X(n*(n+1)/2) symmetric Kronecker product of A
%           and B.
%
%Symmetric Kronecker products are discussed in the appendix of [1], where
%they play a role in the implementation of a semidefinite programming
%algorithm.
%
%The symmetric Kronecker product is implemented by noting that in the
%identity
%kronSym(A,B)*vech(S,sqrt(2))==vech((1/2)*(B*S*A'+A*S*B'),sqrt(2)), if S is
%a symmetric matrix where on or below the main diagonal there is only a
%single nonzero element, then one effectively selects a column of
%kronSym(A,B). To get an unscaled column of kronSym(A,B), the nonzero
%element should be 1 if it is on the main diagonal and it should be
%1/sqrt(2) for elements below (and above) the main diagonal due to the
%sqrt(2) scaling in the vech command. Basically, a simple implementation
%would be
% n=size(A,1);
% prodDim=n*(n+1)/2;
%
% K=zeros(prodDim,prodDim);
% SelMat=zeros(n,n);
% for curEl=1:prodDim
%     [row,col]=vechInd2Sub(n,curEl);
%     if(row==col)
%         SelMat(row,col)=1;
%     else%row>col
%         SelMat(row,col)=1/sqrt(2);
%         SelMat(col,row)=1/sqrt(2);
%     end
%     K(:,curEl)=vech((1/2)*(B*SelMat*A'+A*SelMat*B'),sqrt(2));
%
%     SelMat(row,col)=0;
%     SelMat(col,row)=0;
% end
%However, that is rather slow. Thus, this implementation tries to directly
%write the elements of the output matrix K.
%
%REFERENCES:
%[1] F. Alizadeh, J.-P. A. Haeberly, and M. L. Overton, "Primal-dual
%    interior-point methods for semidefinite programming: Convergence
%    rates, stability and numerical results," SIAM Journal on Optimization,
%    vol. 8, no. 3, pp. 746-768, 1998.
%
%February 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%The columns of K are indexed using (i1,i2) and the rows are indexed using
%(j1,j2).
n=size(A,1);
prodDim=n*(n+1)/2;

K=zeros(prodDim,prodDim);
sqrt2=sqrt(2);
dblSqrt2=2*sqrt2;
for i1=1:n
    col=i1+(i1-1)*(2*n-i1)/2;
    for j1=1:n
        row=j1+(j1-1)*(2*n-j1)/2;

        K(row,col)=(A(j1,i1)*B(j1,i1)+B(j1,i1)*A(j1,i1))/2;
        for j2=(j1+1):n
            row=row+1;

            K(row,col)=(A(j1,i1)*B(j2,i1)+B(j1,i1)*A(j2,i1))/sqrt2;
        end
    end

    for i2=(i1+1):n
      col=col+1;
      for j1=1:n
         row=j1+(j1-1)*(2*n-j1)/2;

         K(row,col)=(A(j1,i1)*B(j1,i2)+A(j1,i2)*B(j1,i1)+B(j1,i1)*A(j1,i2)+B(j1,i2)*A(j1,i1))/dblSqrt2;
         for j2=(j1+1):n
            row=row+1;

            K(row,col)=(A(j1,i1)*B(j2,i2)+A(j1,i2)*B(j2,i1)+B(j1,i1)*A(j2,i2)+B(j1,i2)*A(j2,i1))/2;
         end
      end
    end
end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function linplus_test195 ( )

%*****************************************************************************80
%
%% TEST195 tests R8CI_EVAL.
%
%  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, 'TEST195\n' );
  fprintf ( 1, '  R8CI_EVAL finds the eigenvalues of\n' );
  fprintf ( 1, '  a real circulant system.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N = %d\n', n );
%
%  Set the matrix.
%
  [ a, seed ] = r8ci_random ( n, seed );

  r8ci_print ( n, a, '  The circulant matrix:' );

  lambda = r8ci_eval ( n, a );

  c8vec_print ( n, lambda, '  The eigenvalues:' );

  return
end


function m = nanmean(x,dim)
%NANMEAN Mean value, ignoring NaNs.
%   M = NANMEAN(X) returns the sample mean of X, treating NaNs as missing
%   values.  For vector input, M is the mean value of the non-NaN elements
%   in X.  For matrix input, M is a row vector containing the mean value of
%   non-NaN elements in each column.  For N-D arrays, NANMEAN operates
%   along the first non-singleton dimension.
%
%   NANMEAN(X,DIM) takes the mean along dimension DIM of X.
%
%   See also MEAN, NANMEDIAN, NANSTD, NANVAR, NANMIN, NANMAX, NANSUM.

%   Copyright 1993-2004 The MathWorks, Inc.
%   $Revision: 1.1.8.1 $  $Date: 2010/03/16 00:15:50 $

% Find NaNs and set them to zero
nans = isnan(x);
x(nans) = 0;

if nargin == 1 % let sum deal with figuring out which dimension to use
    % Count up non-NaNs.
    n = sum(~nans);
    n(n==0) = NaN; % prevent divideByZero warnings
    % Sum up non-NaNs, and divide by the number of non-NaNs.
    m = sum(x) ./ n;
else
    % Count up non-NaNs.
    n = sum(~nans,dim);
    n(n==0) = NaN; % prevent divideByZero warnings
    % Sum up non-NaNs, and divide by the number of non-NaNs.
    m = sum(x,dim) ./ n;
end


function s_factor_cell = generate_s_factor(omega, grid3d, m, R)
% For example, s_factor_cell{Axis.x, GT.dual} is the s-factor multiplied to Ex
% (or eps_ww).

chkarg(istypesizeof(omega, 'complex'), '"omega" should be complex.');
chkarg(istypesizeof(grid3d, 'Grid3d'), '"grid3d" should be instance of Grid3d.');

if nargin < 3  % no m
	m = 4;
end
chkarg(istypeof(m, 'real') && all(m(:) >= 1), 'element of "m" should be real and at least 1.0.');
chkarg(isexpandable2mat(m, Axis.count, Sign.count), ...
	'"m" should be scalar, length-%d vector, or %d-by-%d matrix.', Axis.count, Axis.count, Sign.count);
m = expand2mat(m, Axis.count, Sign.count);

if nargin < 4  % no R
	R = exp(-16);  % R = exp(-16) ~= 1e-7
end
chkarg(istypeof(R, 'real') && all(R(:) > 0) && all(R(:) <= 1), ...
	'element of "R" should be real and between 0.0 and 1.0.');
chkarg(isexpandable2mat(R, Axis.count, Sign.count), ...
	'"R" should be scalar, length-%d vector, or %d-by-%d matrix.', Axis.count, Axis.count, Sign.count);
R = expand2mat(R, Axis.count, Sign.count);
lnR = log(R);  % log() is the natural logarithm

s_factor_cell = cell(Axis.count, GT.count);
for w = Axis.elems
	Nw = grid3d.N(w);

	lpml = grid3d.lpml(w,:);  % locations of PML interfaces
	Lpml = grid3d.Lpml(w,:);  % thicknesses of PML

	for g = GT.elems
		l = grid3d.l{w, g};  % length(l) == Nw, rather than Nw+1.
		ind_pml = {l < lpml(Sign.n), l > lpml(Sign.p)};  % indices of locatiotns indise PML

		s_factor = ones(1, Nw);
		for s = Sign.elems
			s_factor(ind_pml{s}) = calc_s_factor(omega, abs(lpml(s) - l(ind_pml{s})), Lpml(s), m(w,s), lnR(w,s));
		end

		s_factor_cell{w,g} = s_factor;
	end
end

function s_factor = calc_s_factor(omega, depth, Lpml, m, lnR)
% assert(Lpml > 0);

sigma_max = -(m+1) * lnR/(2*Lpml);  % -(m+1) ln(R)/(2 eta Lpml), where eta = 1 in the unit of eta_0
sigma = sigma_max * (depth/Lpml).^m;

kappa_max = 1;
kappa = 1 + (kappa_max-1) * (depth/Lpml).^m;

ma = m;
amax = 0;
a = amax * (1 - depth/Lpml).^ma;

s_factor = kappa + sigma ./ (a + 1i*omega);  % s = kappa + sigma/(a + i omega eps), where eps = 1 in the unit of eps_0


function [C, sigma] = dataset3Params(X, y, Xval, yval)
    %% EX6PARAMS returns your choice of C and sigma for Part 3 of the exercise
    %where you select the optimal (C, sigma) learning parameters to use for SVM
    %with RBF kernel
    %   [C, sigma] = EX6PARAMS(X, y, Xval, yval) returns your choice of C and
    %   sigma. You should complete this function to return the optimal C and
    %   sigma based on a cross-validation set.
    %

    % ====================== YOUR CODE HERE ======================
    % Instructions: Fill in this function to return the optimal C and sigma
    %               learning parameters found using the cross validation set.
    %               You can use svmPredict to predict the labels on the cross
    %               validation set. For example,
    %                   predictions = svmPredict(model, Xval);
    %               will return the predictions on the cross validation set.
    %
    %  Note: You can compute the prediction error using
    %        mean(double(predictions ~= yval))
    %
    c_values = [0.01, 0.03, 0.1, 0.3, 1, 3, 10, 30];
    sigma_values = c_values(:);
    best_accuracy = 0.0;

    for i = 1 : length(c_values)

        for j = 1 : length(sigma_values)

            % test this grid entry on our validation set
            test_C = c_values(i);
            test_sigma = sigma_values(j);
            model = svmTrain(X, y, test_C, @(x1, x2) gaussianKernel(x1, x2, test_sigma));
            predictions = svmPredict(model, Xval);
            accuracy = sum(predictions == yval) / length(yval);

            % is this the best performing so far?
            if accuracy > best_accuracy
                best_accuracy = accuracy;
                C = test_C;
                sigma = test_sigma;
            end
        end
    end

    fprintf('Best hyperparameters were C = %f, sigma = %f => best cv acc = %f\n', ...
        C, sigma, best_accuracy);

end


function [xpeak, ypeak, max_f] = findpeak(f,subpixel)
%FINDPEAK Find extremum of matrix.
%   [XPEAK,YPEAK,MAX_F] = FINDPEAK(F,SUBPIXEL) finds the extremum of F,
%   MAX_F, and its location (XPEAK, YPEAK). F is a matrix. MAX_F is the maximum
%   absolute value of F, or an estimate of the extremum if a subpixel
%   extremum is requested.
%
%   SUBPIXEL is a boolean that controls if FINDPEAK attempts to estimate the
%   extremum location to subpixel precision. If SUBPIXEL is false, FINDPEAK
%   returns the coordinates of the maximum absolute value of F and MAX_F is
%   max(abs(F(:))). If SUBPIXEL is true, FINDPEAK fits a 2nd order
%   polynomial to the 9 points surrounding the maximum absolute value of
%   F. In this case, MAX_F is the absolute value of the polynomial evaluated
%   at its extremum.
%
%   Note: Even if SUBPIXEL is true, there are some cases that result
%   in FINDPEAK returning the coordinates of the maximum absolute value
%   of F:
%   * When the maximum absolute value of F is on the edge of matrix F.
%   * When the coordinates of the estimated polynomial extremum would fall
%     outside the coordinates of the points used to constrain the estimate.

%   Copyright 1993-2004 The MathWorks, Inc.
%

% get absolute peak pixel
[max_f, imax] = max(abs(f(:)));
[ypeak, xpeak] = ind2sub(size(f),imax(1));

if ~subpixel || ...
    xpeak==1 || xpeak==size(f,2) || ypeak==1 || ypeak==size(f,1) % on edge
    return % return absolute peak

else
    % fit a 2nd order polynomial to 9 points
    % using 9 pixels centered on irow,jcol
    u = f(ypeak-1:ypeak+1, xpeak-1:xpeak+1);
    u = u(:);
    x = [-1 -1 -1  0  0  0  1  1  1]';
    y = [-1  0  1 -1  0  1 -1  0  1]';

    % u(x,y) = A(1) + A(2)*x + A(3)*y + A(4)*x*y + A(5)*x^2 + A(6)*y^2
    X = [ones(9,1),  x,  y,  x.*y,  x.^2,  y.^2];

    % u = X*A
    A = X\u;

    % get absolute maximum, where du/dx = du/dy = 0
    x_offset = (-A(3)*A(4)+2*A(6)*A(2)) / (A(4)^2-4*A(5)*A(6));
    y_offset = -1 / ( A(4)^2-4*A(5)*A(6))*(A(4)*A(2)-2*A(5)*A(3));

    if abs(x_offset)>1 || abs(y_offset)>1
        % adjusted peak falls outside set of 9 points fit,
        return % return absolute peak
    end

    % return only one-tenth of a pixel precision
    x_offset = round(10*x_offset)/10;
    y_offset = round(10*y_offset)/10;

    xpeak = xpeak + x_offset;
    ypeak = ypeak + y_offset;

    % Calculate extremum of fitted function
    max_f = [1 x_offset y_offset x_offset*y_offset x_offset^2 y_offset^2] * A;
    max_f = abs(max_f);

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@grignon.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 = vectorNorm(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;


function [ Velocity_top_lid ] = CreateLidVelocity( alpha,N )
%CREATEALPHAINPUT creates a parabolic velcoity input on the top lid with
%amplitude alpha

% form the grid if not already done it
persistent Grid

if isempty(Grid)
    [Grid]=CollocationGrid_q(N);    % the computational grid
end

Velocity_top_lid = alpha*(1-Grid.x.^2).^2;

end


%% OPT01_RUN
%
%  Modified:
%
%    09 January 2008
%
   %---------------------------------------------------------------------
   %  Running the first testcase, from D+S, pp. 100, 202; x_* = [2;-1].
   %  This should finish in 10 iterations for Newton's method and in
   %  12 iterations for BFGS.
   %---------------------------------------------------------------------

   fprintf('---------------------------------------------------------\n')
   fprintf('Running testcase_1:  exact solution (2,-1)\n')
   fprintf('---------------------------------------------------------\n')

   fname = 'opt01_fgh';

   options = [];
   options.verbose            = 0;
   options.step_tolerance     = 1.e-11;
   options.gradient_tolerance = 1.e-11;
   options.max_iterations     = 15;

   x0 = [ 1; 1];
%
%  Run with Newton method.
%
   fprintf('Newton method:\n')
   options.method             = 'newton';
   options.globalization      = 'none';

   x = entrust(fname, x0, options);

   fprintf('Newton method produced (%10.7e,%10.7e)\n\n',x(1),x(2))
   f = opt01_fgh ( x, 'f' );
   fprintf('Value of F(X) = %f\n', f );
%
%  Run with secant method.
%
   fprintf('Secant method:\n')
   options.method             = 'secant';
   options.initial_hessian    = [ 14., -4. ; -4.  4. ];
   x = entrust(fname, x0, options);
   fprintf('BFGS   method produced (%10.7e,%10.7e)\n\n',x(1),x(2))
   f = opt01_fgh ( x, 'f' );
   fprintf('Value of F(X) = %f\n', f );
   %---------------------------------------------------------------------
   %  Test Gauss-Newton strategies.
   %---------------------------------------------------------------------
   fprintf('---------------------------------------------------------\n')
   fprintf('Running testcase_1 as least squares problem: \n')
   fprintf('Exact solution (2,-1)\n')
   fprintf('---------------------------------------------------------\n')

   fname = 'opt01_rj';
   options = [];
   options.verbose            = 0;
   options.method             = 'gauss_newton';
   options.step_tolerance     = 1.e-15;
   options.globalization      = 'none';
   options.gradient_tolerance = 1.e-10;
   options.max_iterations     = 40;

   x0 = [1,1];

   x = entrust(fname, x0, options );

   fprintf('Gauss-Newton produced  (%10.7e, %10.7e)\n\n',x(1),x(2))
   [ res, jac ] = opt01_rj ( x, 'f' );
   fprintf('Norm of RES(X) = %f\n', norm ( res ) );


function cpu_timing ( n )

%*****************************************************************************80
%
%% CPU_TIMING measures execution time with CPUTIME.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CPU_TIMING:\n' );
  fprintf ( 1, '  MATLAB has a built in command called CPUTIME,\n' );
  fprintf ( 1, '  which measures the CPU time elapsed so far.\n' );
  fprintf ( 1, '  The difference of two readings measures the work done lately.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TASK #1:\n' )
  fprintf ( 1, '  Multiply two random dense matices of order N\n' );
  fprintf ( 1, '  using MATLAB''s A*B operation.\n' );

  A = rand ( n, n );
  B = rand ( n, n );

  t1 = cputime ( );
  C = A * B;
  t2 = cputime ( );

  f = n * n * n;
  w = f / 1000000;
  s = t2 - t1;
  r = w / s;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  MegaFLOPs = %d\n', w );
  fprintf ( 1, '  Elapsed CPU time  = %f\n', s );
  fprintf ( 1, '  Rate  = %f\n', r );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TASK #2:\n' )
  fprintf ( 1, '  Multiply two random dense matices of order N\n' );
  fprintf ( 1, '  using explicit loops.\n' );

  A = rand ( n, n );
  B = rand ( n, n );

  t1 = cputime ( );
  for i = 1 : n
    for k = 1 : n
      C(i,k) = 0;
      for j = 1 : n
        C(i,k) = C(i,k) + A(i,j) * B(j,k);
      end
    end
  end
  t2 = cputime ( );

  f = n * n * n;
  w = f / 1000000;
  s = t2 - t1;
  r = w / s;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  MegaFLOPs = %d\n', w );
  fprintf ( 1, '  Elapsed CPU time  = %f\n', s );
  fprintf ( 1, '  Rate  = %f\n', r );

  return
end


%   ****** See nozzle.m for instructions ******
%  Program to extrapolate the data points from nozzle design and make a
%  uniform grid spacing in the x-direction
%  Change nothing... simply run this script

%  Find the minimum spacing given by the method of characteristics
%  and set as the dx value
dx = 0;
for i=1: size(noz,1)-1
    len = noz(i+1,1) - noz(i,1);
    if (len < dx || i == 1)
        dx = len;
    end
end

%  Explicitly give the dx value here
dx = max(noz(:,1))/ceil(max(noz(:,1))/dx);
n = max(noz(:,1))/dx;

% len = max(noz(:,1));
% n = 50;   %  Note # of points is actually n+1
% dx = len/n

%  Pick m points in y as some factor of x points
yfactor = .8;
m = ceil(yfactor*n);

%  Make uniform x-distribution of points
xmax = 0;
i = 1;
while xmax < max(noz(:,1))
    xmax = dx*(i-1);
    x(i,1:m) = xmax;
    i = i+1;
end

%  Make the y-points and extrapolate linearly from closest points to fit
%  the nozzle geometry

%  Initialize and assign last value
y(1:size(x,1),1:size(x,2)) = 0;
y(1,size(y,2)) = noz(1,size(noz,2));
y(size(y,1),size(y,2)) = noz(size(noz,1),size(noz,2));
for i = 1 : size(x,1)-1

    j = 1;
    while x(i,1) >= noz(j,1)
        x1 = noz(j,1);
        x2 = noz(j+1,1);
        y1 = noz(j,2);
        y2 = noz(j+1,2);
        j = j + 1;
    end

    slope = (y2 - y1)/(x2 - x1);
    y(i,size(y,2)) = y1 + slope*(x(i,1)-x1);

    %Fill in mesh
    dy = y(i,size(y,2))/(size(y,2)-1);
    for k = 1: size(y,2)
        y(i,k) = dy*(k-1);
    end

end

%Fill in mesh
dy = y(size(y,1),size(y,2))/(size(y,2)-1);
for k = 1: size(y,2)
    y(size(y,1),k) = dy*(k-1);
end

'Mesh done'


function cvx_optval = det_rootn( X )

%DET_ROOTN nth-root of the determinant of an SPD matrix.
%   For a square matrix X, DET_ROOTN(X) returns
%       POW(DET(X),1/(size(X,1))
%   if X is symmetric (real) or Hermitian (complex) and positive semidefinite,
%   and -Inf otherwise.
%
%   This function can be used in many convex optimization problems that call for
%   LOG(DET(X)) instead. For example, if the objective function contains nothing
%   but LOG(DET(X)), it can be replaced with DET_ROOTN(X), and the same optimal
%   point will be produced.
%
%   Disciplined convex programming information:
%       DET_ROOTN is concave and nonmonotonic; therefore, when used in
%       CVX specifications, its argument must be affine.

error( nargchk( 1, 1, nargin ) );
if ndims( X ) > 2 || size( X, 1 ) ~= size( X, 2 ),
    error( 'Second argument must be a square matrix.' );
end
err = X - X';
X   = 0.5 * ( X + X' );
if norm( err, 'fro' ) > 8 * eps * norm( X, 'fro' ),
    cvx_optval = -Inf;
else
    [R,p] = chol(X);
    if p > 0,
        cvx_optval = geo_mean(eig(full(X)));
    else
        cvx_optval = geo_mean(diag(R)).^2;
    end
end

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function yc = pwl_approx_1d ( nd, xd, yd, nc, xc )

%*****************************************************************************80
%
%% PWL_APPROX_1D determines the control values for a PWL approximant.
%
%  Discussion:
%
%    The piecewise linear approximant is defined by NC control pairs
%    (XC(I),YC(I)) and approximates ND data pairs (XD(I),YD(I)).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 October 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer ND, the number of data points.
%    ND must be at least 1.
%
%    Input, real XD(ND,1), the data points.
%
%    Input, real YD(ND,1), the data values.
%
%    Input, integer NC, the number of control points.
%    NC must be at least 1.
%
%    Input, real XC(NC,1), the control points.
%
%    Output, real YC(NC,1), the control values.
%

%
%  Define the NDxNC linear system that determines the control values.
%
  a = pwl_approx_1d_matrix ( nd, xd, yd, nc, xc );
%
%  Solve the system.
%
  yc = a \ yd;

  return
end


% peq.m - Parametric EQ with matching gain at Nyquist frequency
% Author: Sophocles J. Orfanidis,
% Usage:  [b, a] = peq(G0, G, w0, Dw, NdB)
%
% G0 = reference gain at DC in dB
% G  = boost/cut gain in dB
% GB = bandwidth gain in dB
%
% w0 = center frequency in Hz
% Dw = bandwidth in Hz
% NdB = amount of db less than the peak/notch gain as the bandwidth frequencies
% b  = [b0, b1, b2] = numerator coefficients
% a  = [1,  a1, a2] = denominator coefficients

% Modified by Arvind using the equations from IIR Filter Design Chapter in
% the book
function [b, a, beta] = peq(G0, G, w0, Dw, NdB)

%Convert from Decibels to real values
GB = G * NdB;
G  = (10^(G/20));
GB = (10^(GB/20));
G0 = (10^(G0/20));

%Convert absolute frequencies to rads/sec
%w0 = w0*2*pi/fs;
%Dw = Dw*2*pi/fs;

beta = sqrt((GB^2-G0^2)/(G^2-GB^2)) * tan(Dw/2);
b = [(G0+G*beta)/(1+beta) -(2*G0*cos(w0))/(1+beta)  (G0-G*beta)/(1+beta)];
a = [1 -(2*cos(w0))/(1+beta) (1-beta)/(1+beta)];

%***********************
% double beta;
%
% beta = sqrt((GB[0]^2-G0[0]^2)/(G^2[0]-GB[0]^2)) * tan(Dw[0]/2)
% Num[0] = (G0[0]+G[0]*beta)/(1+beta);
% Num[1] =  -(2*G0[0]*cos(w0[0]))/(1+beta) ;
% Num[2] = (G0[0]-G[0]*beta)/(1+beta);
%
% Den[0] = 1;
% Den[1] = -(2*cos(w0[0]))/(1+beta);
% Den[2] = (1-beta)/(1+beta);
%*************************


%% Transform Compression JPEG

% JPEG Baseline Encoder

% Jorge Calderon <calderonjorge9@gmail.com>
% University Of Antioquia
% Created: November 2012
% Modified: January 2013
% Copyright 2012
% All rights reserved

%% Clean Workplace

clear all;
close all;
clc;

%% Initialize Variables

check = 0;
stream=[];

DCTQ=[...
    16 11 10 16 24 40 51 61;...
    12 12 14 19 26 58 60 55;...
    14 13 16 24 40 57 69 56;...
    14 17 22 29 51 87 80 62;...
    18 22 37 56 68 109 103 77;...
    24 36 55 64 81 194 113 92;...
    49 64 78 87 103 121 120 101;...
    72 92 95 98 112 100 103 99];

z=[...
    9 2 3 10 17 25 18 11 4 5 12 19 26 ...
    33 41 34 27 20 13 6 7 14 21 28 35 ...
    42 49 57 50 43 36 29 22 15 8 16 23 ...
    30 37 44 51 58 59 52 45 38 31 24 32 ...
    39 46 53 60 61 54 47 40 48 55 62 63 56 64];

%% Read Image

while check<1
    [img_in pathname]=uigetfile({'*.bmp;*.tif','Image(*.bmp,*.tif)'},'Select Uncompressed Image','Multiselect','off');
    if img_in>0
        cd(pathname)
        image = double(imread(img_in));
        [row, col, dim] = size(image);
        if col<row
            disp('Error: The image must be horizontal');
            disp('Press any key to continue...');
            pause;
        else
            if (row<720 && col<1280)
                check=1;
            else
                disp('Error: The image size is too large');
                disp('Press any key to continue...');
                pause;
            end
        end
    else
        if img_in==0
            return;
        end
    end
end

%% Check Size

if mod(row,2)~=0
    row = row-1;
end
if mod(col,2)~=0
    col = col-1;
end
image = imresize(image, [row col]);

%% Convert Image To YCBCR

img = rgb2ycbcr(image);

%% Adjust Aspect Ratio

if mod(row,72)~=0
    row_aspect = row+(72-mod(row,72));
else
    row_aspect = row;
end
col_aspect = (row_aspect/9)*16;
image_aspect = zeros(row_aspect, col_aspect, dim);
zig_zag_ac = zeros(row_aspect*col_aspect/64, 63);
zig_zag_dc = zeros(row_aspect*col_aspect/64, 1);

for i=1:col
    for j=1:row
        for k=1:dim
            image_aspect(j+abs(row_aspect-row)/2,i+abs(col_aspect-col)/2,k)=img(j,i,k);
        end
    end
end

% figure(1)
% imshow(uint8(image_aspect));

%% Digitization Scheme 420

comp_blue = image_aspect(:,:,2);
comp_red = image_aspect(:,:,3);
luminance = double(image_aspect(:,:,1))-128;
imgcb_420 = double(comp_blue(1:2:size(comp_blue,1),1:2:size(comp_blue,2)))-128;
imgcr_420 = double(comp_red(1:2:size(comp_red,1),1:2:size(comp_red,2)))-128;

%% Segmenting Image Blocks Of 8x8

k=0;
for i=1:8:row_aspect
    for j=1:8:col_aspect
        f=luminance(i:i+7,j:j+7);
        T=dct2(f);
        TQ=round(T./DCTQ);
        k=k+1;
        zig_zag_dc(k,1) = TQ(1,1);
        zig_zag_ac(k,1:63) = TQ(z);
    end
end

%% Huffman Compression

dpcm(1,1)=zig_zag_dc(1,1);
stream=cat(2,stream,huffman_dc(dpcm(1,1)),huffman_ac(zig_zag_ac(1,1:63)));
for m=2:k
    dpcm(m,1)=zig_zag_dc(m,1)-zig_zag_dc(m-1,1);
    stream=cat(2,stream,huffman_dc(dpcm(m,1)),huffman_ac(zig_zag_ac(m,1:63)));
end

%% Byte Stuffing

p=0;
G=size(stream,2);
for i=1:8:size(stream,2)
    bit_val(1,1:8)=stream(1,i:i+7);
    if strcmp(bit_val(1,1:8),'11111111')==1
        tempbitstream = stream(1,i+8:G+p);
        stream(1,i+8:i+15)='00000000';
        p=p+8;
        temp2_bitstream=stream(1,1:i+15);
        stream(1,1:G+p)=cat(2,temp2_bitstream,tempbitstream);
    end
end

%% Convert String To Decimal

numbytes=floor(length(stream)/8);
diff_stream=length(stream)-numbytes*8;
if diff_stream==0
    matrix_code_decimal= zeros(numbytes+2,8);
else
    matrix_code_decimal= zeros(numbytes+3,8);
end
s=0;
for count2=1:8:numbytes*8
    s=s+1;
    matrix_code_decimal(s,1)=bin2dec(stream(1,count2:count2+7));
end

if diff_stream~=0
    s=s+1;
    matrix_code_decimal(s,1)=bin2dec(stream(1,numbytes*8+1:length(stream)));
end

%% Final Marker

matrix_code_decimal(s+1,1)=255;
matrix_code_decimal(s+2,1)=217;

%% Header JFIF

signal=[255	216	255	224	000	016	074	070	073	070	000	001	002	000	000	096	000	096	000	000	...
        255	219	000	067	000	016	011	012	014	012	010	016	014	013	014	018	017	016	019	024	...
        040	026	024	022	022	024	049	036	037	029	040	058	051	061	060	057	051	056	055	064	...
        072	092	078	064	068	087	069	055	056	080	109	081	087	095	098	103	194	103	062	077	...
        113	121	112	100	120	092	101	103	099	255	219	000	067	001	017	018	018	024	021	024	...
        047	026	026	047	099	066	056	066	099	099	099	099	099	099	099	099	099	099	099	099	...
        099	099	099	099	099	099	099	099	099	099	099	099	099	099	099	099	099	099	099	099	...
        099	099	099	099	099	099	099	099	099	099	099	099	099	099	099	099	099	099	255	192	...
        000	011	008	000	000	000	000	001	001	034	000	255	196	000	031	000	000	001	005	001	...
        001	001	001	001	001	000	000	000	000	000	000	000	000	001	002	003	004	005	006	007	...
        008	009	010	011	255	196	000	181	016	000	002	001	003	003	002	004	003	005 	005	004	...
        004	000	000	001	125	001	002	003	000	004	017	005	018	033	049	065	006	019	081	097	...
        007	034	113	020	050	129	145	161	008	035	066	177	193	021	082	209	240	036	051	098	...
        114	130	009	010	022	023	024	025	026	037	038	039	040	041	042	052	053	054	055	056	...
        057	058	067	068	069	070	071	072	073	074	083	084	085	086	087	088	089	090	099	100	...
        101	102	103	104	105	106	115	116	117	118	119	120	121	122	131	132	133	134	135	136	...
        137	138	146	147	148	149	150	151	152	153	154	162	163	164	165	166	167	168	169	170	...
        178	179	180	181	182	183	184	185	186	194	195	196	197	198	199	200	201	202	210	211	...
        212	213	214	215	216	217	218	225	226	227	228	229	230	231	232	233	234	241	242	243	...
        244	245	246	247	248	249	250	255	218	000	008	001	001	000	000	063	000];

% Start of Image (SOI) marker:FFD8=255,216
% JFIF marker:FFE0=255,224
%        Length=000,016
%        Identifier:4A46494600=074,070,073,070,000
%        Version=001,002
%        Units=000
%        Xdensity=000,096
%        Ydensity=000,096
%        Xthumbnail=000
%        Ythumbnail=000
%        (RGB)n, n=Xthumbnail*Ythumbnail required 3*n bytes=null

% Define Quantization table marker (luma):FFDB=255,219
%         Length:two bytes that indicate the number of bytes, including the two length bytes, that this header contains=000,067
%         Precision=000 (baseline)
%         Quantization values=016,011,012,014,012,010,016,...,101,103,099 (zigzag)

% Define Quantization table marker (Chroma):FFDB=255,219
%         Length:two bytes that indicate the number of bytes, including the two length bytes, that this header contains=000,067
%         Precision=000 (baseline)
%         Quantization values=017,018,018,024,021,024,047,...,099,099,099 (zigzag)

% Start of frame marker:FFC0=255,192
%         Length:two bytes that indicate the number of bytes, including the two length bytes, that this header contains=000,011
%         Sample precision=008
%         X=000,000 (This will be defined later)
%         Y=000,000 (This will be defined later)
%         Number of components in the image=001
%             * 3 for color baseline
%             * 1 for grayscale baseline
%         Component ID=001
%         H and V sampling factors=034
%         Quantization table number=000

% Define Huffman table marker (DC):FFC4=255,196
%        Length:two bytes that indicate the number of bytes, including the two length bytes, that this header contains=000,031
%        Index=000 (Huffman DC)
%        Bits=The next 16 bytes from an array of unsigned 1-byte integers whose elements give the number of Huffman codes for each possible code length (1-16).
%        Huffman values=000,001,002,...,010,011

% Define Huffman table marker (AC):FFC4=255,196
%        Length:two bytes that indicate the number of bytes, including the two length bytes, that this header contains=000,181
%        Index=016 (Huffman AC)
%        Bits=The next 16 bytes from an array of unsigned 1-byte integers whose elements give the number of Huffman codes for each possible code length (1-16).
%        Huffman values=001,002,003,...,249,250

% Start of Scan marker:FFDA=255,218
%        Length:two bytes that indicate the number of bytes, including the two length bytes, that this header contains=000,008
%        Number of components=001
%        Component ID=001
%        DC and AC table numbers=000
%        Ss=000
%        Se=063
%        Ah and Al=000

%% Define Size Image

Y = dec2hex(row_aspect,4);
X = dec2hex(col_aspect,4);
signal(1,164) = hex2dec(Y(1,1:2));
signal(1,165) = hex2dec(Y(1,3:4));
signal(1,166) = hex2dec(X(1,1:2));
signal(1,167) = hex2dec(X(1,3:4));

%% Concatenate Coding + Header

JP_STREAM(1,1:size(signal,2))=signal(1,1:size(signal,2));
for j=1:1:size(matrix_code_decimal,1)
    JP_STREAM(1,size(signal,2)+j)=matrix_code_decimal(j,1);
end

%% JPG Data Store

JP_STREAM=JP_STREAM';
img_name = regexp(img_in,'\.','split');
img_name = img_name{1};
fid = fopen([img_name '.jpg'], 'wb');
if fid < 0
    error('Failed to open data file for write');
end
fwrite(fid,JP_STREAM,'uint8');
fclose(fid);


function v = planeIntersect(N1,N2,alpha1,alpha2)
% compute the intersection of two planes

d = dot(N1,N2);
c1 = (alpha1 - alpha2 .* d)./(1-d.^2);
c2 = (alpha2 - alpha1 .* d)./(1-d.^2);

% the line intersecting both planes is
% a + lambda b
a = c1 .* N1 + c2 .* N2;
b = cross(N1,N2);

% determine lambda such that norm(a + lambda b) = 1
p = dot(a,b) ./ norm(b).^2;
q = (norm(a).^2 - 1)./norm(b).^2;

lambda1 = - p + sqrt(p.^2 - q);
lambda2 = - p - sqrt(p.^2 - q);

% compute the points on the sphere
v = [a(:) + lambda1(:) .* b(:), a(:) + lambda2(:) .* b(:)];

end


function fx2 = p12_fx2 ( x )

%*****************************************************************************80
%
%% P12_FX2 evaluates the second derivative of the function for problem 12.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 May 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the abscissa.
%
%    Output, real FX2, the second derivative of the function at X.
%
  factor = 1000.0;

  if ( x == 1.0 )
    fx2 = 0.0;
  else
    y = x - 1.0;
    fx2 = - 2.0 * factor * exp ( - 1.0 / y.^2 ) * ( y.^2 - 2.0 ) / y.^5;
  end


function [ t, wts ] = cgqfs ( nt, kind, alpha, beta, lo )

%*****************************************************************************80
%
%% CGQFS computes knots and weights of a Gauss quadrature formula.
%
%  Discussion:
%
%    This routine computes the knots and weights of a Gauss quadrature
%    formula with:
%
%    * a classical weight function with default values for a,b
%    * only simple knots
%    * optionally print knots and weights and a check of the moments
%
%    Use routine EIQFS to evaluate a quadrature formula computed by
%    this routine.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 February 2010
%
%  Author:
%
%    Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Sylvan Elhay, Jaroslav Kautsky,
%    Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
%    Interpolatory Quadrature,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 4, December 1987, pages 399-415.
%
%  Parameters:
%
%    Input, integer NT, the number of knots.
%
%    Input, integer KIND, the rule.
%    1, Legendre,             (a,b)       1.0
%    2, Chebyshev Type 1,     (a,b)       ((b-x)*(x-a))^(-0.5)
%    3, Gegenbauer,           (a,b)       ((b-x)*(x-a))^alpha
%    4, Jacobi,               (a,b)       (b-x)^alpha*(x-a)^beta
%    5, Generalized Laguerre, (a,+oo)     (x-a)^alpha*exp(-b*(x-a))
%    6, Generalized Hermite,  (-oo,+oo)   |x-a|^alpha*exp(-b*(x-a)^2)
%    7, Exponential,          (a,b)       |x-(a+b)/2.0|^alpha
%    8, Rational,             (a,+oo)     (x-a)^alpha*(x+b)^beta
%    9, Chebyshev Type 2,     (a,b)       ((b-x)*(x-a))^(+0.5)
%
%    Input, real ALPHA, the value of Alpha, if needed.
%
%    Input, real BETA, the value of Beta, if needed.
%
%    Input, integer LO, selects the action.
%    > 0, compute and print knots and weights.  Print moments check.
%    = 0, compute knots and weights.
%    < 0, compute and print knots and weights.
%
%    Output, real T(NT), the knots.
%
%    Output, real WTS(NT), the weights.
%

%
%  Compute the Gauss quadrature formula.
%
  [ t, wts ] = cdgqf ( nt, kind, alpha, beta );
%
%  Exit if no print required.
%
  if ( lo == 0 )
    return
  end

  key = 1;
  mop = 2 * nt;
  m = mop + 1;
  mmex = max ( 2 * nt + 3, 1 );
%
%  All knots have multiplicity = 1.
%
  mlt = zeros(nt,1);
  mlt(1:nt) = 1;
%
%  NDX(I) = I.
%
  ndx = ( 1 : nt );

  w = zeros(m,1);

  chkqfs ( t, wts, mlt, nt, nt, ndx, key, mop, mmex, ...
    kind, alpha, beta, lo, w );

  return
end


function learningConvergence

k = 10;
iter = 1:200;

lblRate  = LowerBoundLipshitzFirstOrder(iter);
lbscRate = lowerBoundStronglyConvexFirstOrder(iter,k);
sRate = subgradientConvergence(iter);
nRate = stronglyConvexNesterovConvergence(iter,k);
qRate = quadraticConvergence(iter,1/k);
lgRate = LipschitzGradientConvergence(iter);
scRate = stronglyConvexGradientConvergence(iter,k);

colors =    {
            [0.9290, 0.6940, 0.1250];
            [0, 0.4470, 0.7410];
          	[0.8500, 0.3250, 0.0980];
            [0, 0.4470, 0.7410];
            [0.8500, 0.3250, 0.0980];
          	[0.4940, 0.1840, 0.5560];
          	[0.4660, 0.6740, 0.1880];
          	[0.3010, 0.7450, 0.9330]};
style = {'-';'--';'--';'-';'-';'-';'-';};

rates = [sRate;lblRate;lbscRate;lgRate;scRate;nRate;qRate];

figure(1)
subplot(1,2,1);
hold on
for i = 1:size(rates,1)
    plot(iter,rates(i,:),'Color',colors{i},'LineStyle',style{i});
end
legend('Subgradient','LowerBound Lipshitz','LowerBound Strongly Convex', 'Lipschitz Diff Gradient','StronglyConvex Diff Gradient','Strongly Convex Diff Nesterov','Quadratic Diff')

figure(1)
subplot(1,2,2);
hold on
for i = 1:size(rates,1)
h = plot(iter,rates(i,:),'Color',colors{i},'LineStyle',style{i});
set(gca,'yscale','log');
end

legend('Subgradient','LowerBound Lipshitz','LowerBound Strongly Convex', 'Lipschitz Diff Gradient','StronglyConvex Diff Gradient','Strongly Convex Diff Nesterov','Quadratic Diff')

end

function rate = subgradientConvergence(x)

rate = 1./sqrt(x);

end

function rate = LowerBoundLipshitzFirstOrder(x)

rate = 1./(x + 1).^2;

end

function rate = LipschitzGradientConvergence(x)

rate = 1./(x + 1);

end

function rate = stronglyConvexGradientConvergence(x,k)

rate = (((k) - 1)/((k)+1)).^(x);
end

function rate = quadraticConvergence(x,k)

rate = k.^(2.^x);

end

function rate = lowerBoundStronglyConvexFirstOrder(x,k)

rate = ((sqrt(k) - 1)/(sqrt(k) + 1)).^(2*x);

end

function rate = stronglyConvexNesterovConvergence(x,k)

rate = ((sqrt(k) - 1)/(sqrt(k)+1)).^(x);

end


function E = bryant2angular(rpy)
% Jacobian of Omega in terms of rolldot, pitchdot and yawdot
% "Bryant Angle Representation"
% http://www.u.arizona.edu/~pen/ame553/Hallway/CAAMS/CAAMS_15.pdf

roll = rpy(1); %x-axis
pitch = rpy(2); %y-axis
yaw = rpy(3); %z-axis

% paul's version
% E = [ cos(yaw)*cos(pitch),           -sin(yaw),                   0;
%       sin(yaw)*cos(pitch),            cos(yaw),                   0;
%               -sin(pitch),                   0,                   1];

% bryant angle version
E = [   cos(roll)*cos(yaw),  sin(yaw),  0;
      -cos(pitch)*sin(yaw),  cos(yaw),  0;
                sin(pitch),         0,  1];

end


function value = r8_lgmc ( x )

%*****************************************************************************80
%
%% R8_LGMC evaluates the log gamma correction factor for an R8 argument.
%
%  Discussion:
%
%    For 10 <= X, compute the log gamma correction factor so that
%
%      log ( gamma ( x ) ) = log ( sqrt ( 2 * pi ) )
%                          + ( x - 0.5 ) * log ( x ) - x
%                          + r8_lgmc ( x )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the correction factor.
%
  persistent algmcs
  persistent nalgm
  persistent xbig
  persistent xmax

  if ( isempty ( nalgm ) )

    algmcs = [ ...
      +0.1666389480451863247205729650822, ...
      -0.1384948176067563840732986059135E-04, ...
      +0.9810825646924729426157171547487E-08, ...
      -0.1809129475572494194263306266719E-10, ...
      +0.6221098041892605227126015543416E-13, ...
      -0.3399615005417721944303330599666E-15, ...
      +0.2683181998482698748957538846666E-17, ...
      -0.2868042435334643284144622399999E-19, ...
      +0.3962837061046434803679306666666E-21, ...
      -0.6831888753985766870111999999999E-23, ...
      +0.1429227355942498147573333333333E-24, ...
      -0.3547598158101070547199999999999E-26, ...
      +0.1025680058010470912000000000000E-27, ...
      -0.3401102254316748799999999999999E-29, ...
      +0.1276642195630062933333333333333E-30 ]';

    nalgm = r8_inits ( algmcs, 15, r8_mach ( 3 ) );
    xbig = 1.0 / sqrt ( r8_mach ( 3 ) );
    xmax = exp ( min ( log ( r8_mach ( 2 ) / 12.0 ), ...
      - log ( 12.0 * r8_mach ( 1 ) ) ) );
  end

  if ( x < 10.0 )

    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_LGMC - Fatal error!\n' );
    fprintf ( 1, '  X must be at least 10.\n' );
    error ( 'R8_LGMC - Fatal error!' )

  elseif ( x < xbig )

    value = r8_csevl ( 2.0 * ( 10.0 / x ) ...
      * ( 10.0 / x ) - 1.0, algmcs, nalgm ) / x;

  elseif ( x < xmax )

    value = 1.0 / ( 12.0 * x );

  else

    value = 0.0;

  end

  return
end


function [ a, q, lambda, seed ] = r8symm_test ( n, lambda_mean, lambda_dev, ...
  seed )

%*****************************************************************************80
%
%% R8SYMM_TEST determines a symmetric matrix with a certain eigenstructure.
%
%  Discussion:
%
%    An R8SYMM is a real symmetric matrix stored using full storage, and
%    using R8 arithmetic.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real LAMBDA_MEAN, the mean value of the normal
%    distribution from which the eigenvalues will be chosen.
%
%    Input, real LAMBDA_DEV, the standard deviation of the normal
%    distribution from which the eigenvalues will be chosen.
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Output, real A(N,N), the test matrix.
%
%    Output, real Q(N,N), the eigenvector matrix.
%
%    Output, real LAMBDA(N), the eigenvalue vector.
%

%
%  Choose the eigenvalues LAMBDA.
%
  [ lambda, seed ] = r8vec_normal ( n, lambda_mean, lambda_dev, seed );
%
%  Get a random orthogonal matrix Q.
%
  [ q, seed ] = r8mat_orth_uniform ( n, seed );
%
%  Set A = Q * Lambda*I * Q'.
%
  a = q * diag ( lambda ) * q';

  return
end


function a=v_polygonarea(p)
%V_POLYGONAREA Calculate the area of a polygon
%
% Inputs:
%    P(n,2) is the polygon vertices
%
% Outputs:
%    A is teh area of teh polygon
%
% The area is positive if the vertices go anti-clockwise around the polygon.
%

%      Copyright (C) Mike Brookes 2009
%      Version: $Id: v_polygonarea.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
p(end+1,:)=p(1,:);      % append an extra point
a=0.5*sum((p(1:end-1,1)-p(2:end,1)).*(p(1:end-1,2)+p(2:end,2)),1);
if ~nargout
    plot(p(:,1),p(:,2),'b-');
    mnx=[1.05 -0.05;-0.05 1.05]*[min(p);max(p)];
    set(gca,'xlim',mnx(:,1)','ylim',mnx(:,2)');
    title(sprintf('Area = %.2g',a));
end


%  Figure 3.30e      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%

clf;
num=1;
a=.5;

zeta =1;
den2=[1/(zeta*a) 1];
den1=[1 2*zeta 1];
den=conv(den1,den2);
t=0:.1:10;
y1=step(num,den,t);

zeta =.7;
den2=[1/(zeta*a) 1];
den1=[1 2*zeta 1];
den=conv(den1,den2);
y2=step(num,den,t);

zeta =.5;
den2=[1/(zeta*a) 1];
den1=[1 2*zeta 1];
den=conv(den1,den2);
y3=step(num,den,t);

axis([1 10 .1 .9])
plot(t,y1,'-',t,y2,'--',t,y3,'-.'),grid
title('Fig. 3.30e Step response with extra pole, \alpha= .5')
xlabel('\omega_n t')
ylabel('y(t)')


function [G]=gsp_low_stretch_tree(k)
%GSP_LOW_STRETCH_TREE  Initialize a low stretch tree
%   Usage:  G = gsp_low_stretch_tree(k);
%           G = gsp_low_stretch_tree();
%
%   Input parameters:
%         k     : 2^k points on each side of the grid of vertices. (default 6)
%   Output parameters:
%         G     : Graph structure.
%
%   'gsp_create_low_stretch_tree(k)' initializes a graph structure containing
%   the weighted adjacency matrix (G.W), the number of vertices (G.N), the
%   plotting coordinates (G.coords), the plotting coordinate limits
%   (G.limits), and the root of a low stretch tree on a grid of points.
%   There are $2^k$ points on each side of the grid, and therefore $2^{2k}$
%   total vertices. The edge weights are all equal to 1.
%
%   Example:::
%
%          G = gsp_low_stretch_tree(3);
%          paramplot.show_edges = 1;
%          gsp_plot_graph(G,paramplot);
%
%   See also: gsp_graph

% Author : David I Shuman, Nathanael Perraudin

if nargin < 1
   k = 6;
end

ii=[2 3 1 1 4 3];
jj=[1 1 2 3 3 4];

XCoords=[1, 2, 1, 2];
YCoords=[1, 1, 2, 2];

for p=2:k
    % create the indicies of the weight matrice
    ii_new=[ii,ii+4^(p-1),ii+2*4^(p-1),ii+3*4^(p-1)];
    ii_new_middle=[4^(p-1),4^(p-1),4^(p-1)+(4^p+2)/3,5/3*4^(p-1)+1/3,4^(p-1)+(4^p+2)/3,3*4^(p-1)+1];
    ii=[ii_new,ii_new_middle];

    jj_new=[jj,jj+4^(p-1),jj+2*4^(p-1),jj+3*4^(p-1)];
    jj_new_middle=[5/3*4^(p-1)+1/3,4^(p-1)+(4^p+2)/3,3*4^(p-1)+1,4^(p-1),4^(p-1),4^(p-1)+(4^p+2)/3];
    jj=[jj_new,jj_new_middle];

    % Create Coords
    YCoords=repmat(YCoords,1,2);
    YCoords_new=[YCoords,YCoords+2^(p-1)];
    YCoords=YCoords_new;

    XCoords_new=[XCoords,XCoords+2^(p-1)];
    XCoords=repmat(XCoords_new,1,2);
end

G.W=sparse(ii,jj,ones(size(ii)));
G.coords=[XCoords',YCoords'];

G.limits=[0,2^k+1,0,2^k+1];
G.N=(2^k)^2;
G.root=4^(k-1);

G.plotting.edge_width=1.25;
G.plotting.vertex_size=75;

G.type = 'low strech tree';

G = gsp_graph_default_parameters(G);

end


function determ = aegerter_determinant ( n )

%*****************************************************************************80
%
%% AEGERTER_DETERMINANT returns the determinant of the AEGERTER matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of rows and columns of A.
%
%    Output, real DETERM, the determinant.
%
  determ = ( n - ( ( n - 1 ) * n * ( 2 * n - 1 ) ) / 6 );

  return
end


function [t0,t1,t2]=calculateInterpolationTimes(L,v,a)
%% About:
% calculates the times of the acceleration, stable motion, deceleration

%% Arreguments:
% L: length of the curve (mm).
% v: linear velocity of eef on the the curve (mm/sec).
% a: linear acceleration of eef on the the curve (mm/sec2).

%% Return values:
% t0; time for acceleration.
% t1: time for acceleration, constant velocity.
% t2: time for acceleration, constant velocity and for deceleration

% Copy right: Mohammad SAFEEA
% 16th-Oct-2017

taw=v/a;

if(L>a*taw*taw)
    t0=taw;
    deltat=(L-a*taw*taw)/v;
    t1=t0+deltat;
    t2=t1+taw;
else
    taw1=(L/a)^0.5;
    t0=taw1;
    t1=taw1;
    t2=2*taw1;
end

end


function y=rdct(x,n,a,b)
%RDCT     Discrete cosine transform of real data Y=(X,N,A,B)
% Data is truncated/padded to length N.
%
% This routine is equivalent to multiplying by the matrix
%
%   rdct(eye(n)) = diag([sqrt(2)*B/A repmat(2/A,1,n-1)]) * cos((0:n-1)'*(0.5:n)*pi/n)
%
% Default values of the scaling factors are A=sqrt(2N) and B=1 which
% results in an orthogonal matrix. Other common values are A=1 or N and/or B=1 or sqrt(2).
% If b~=1 then the columns are no longer orthogonal.
%
% see IRDCT for the inverse transform

% BUG: in line 51 we should do chopping after transform and not before

fl=size(x,1)==1;
if fl x=x(:); end
[m,k]=size(x);
if nargin<2 n=m;
end
if nargin<4 b=1;
    if nargin<3 a=sqrt(2*n);
    end
    end
if n>m x=[x; zeros(n-m,k)];
elseif n<m x(n+1:m,:)=[];
end

x=[x(1:2:n,:); x(2*fix(n/2):-2:2,:)];
z=[sqrt(2) 2*exp((-0.5i*pi/n)*(1:n-1))].';
y=real(fft(x).*z(:,ones(1,k)))/a;
y(1,:)=y(1,:)*b;
if fl y=y.'; end


function HTotal=uHessian3D(xG,lRx,M)
%%UHESSIAN3D Determine the Hessian matrix (second derivative matrix) of a
%           a single direction cosine u component in 3D with respect to 3D
%           position. Relativity and atmospheric effects are not taken into
%           account. This function is essentially the same as uvHessian
%           without the v component.
%
%INPUTS: xG The 3XN target position vectors in the global coordinate system
%           with [x;y;z] components for which gradients are desired.
%       lRx The 3X1  position vector of the receiver. If omitted, the
%           receiver is placed at the origin.
%         M A 3X3 rotation matrix from the global Coordinate system to the
%           orientation of the coordinate system at the receiver. If
%           omitted, it is assumed to be the identity matrix.
%
%OUTPUTS: HTotal A 3X3XN matrix such that HTotal(:,:,i) is the Hessian
%          matrix with respect to the u component of the ith measurement.
%          The elements in the matrices for each component/ point are
%          ordered [d^2/(dxdx), d^2/(dxdy), d^2/(dxdz);
%                   d^2/(dydx), d^2/(dydy), d^2/(dydz);
%                   d^2/(dzdx), d^2/(dzdy), d^2/(dzdz)];
%          note that each matrix is symmetric (i.e.
%           d^2/(dydx)=d^2/(dxdy) ).
%
%A derivation of the components of the Jacobian is given in [1]. The
%Hessian is just one derivative higher.
%
%REFERENCES:
%[1] D. F. Crouse, "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems
%    Magazine, vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%
%July 2021 David F.Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(M))
	M=eye(3,3);
end

if(nargin<2||isempty(lRx))
	lRx=zeros(3,1);
end

N=size(xG,2);
HTotal=zeros(3,3,1,N);
for curPoint=1:N
    %Convert the state into the local coordinate system.
    xLocal=M*(xG(1:3)-lRx(1:3));

    x=xLocal(1);
    y=xLocal(2);
    z=xLocal(3);

    r5=norm(xLocal)^5;

    H=zeros(3,3,1);

    %u Hessian values
    %du/(dxdx)
    H(1,1,1)=-((3*x*(y^2+z^2))/r5);
    %du/(dydy)
    H(2,2,1)=-((x*(x^2-2*y^2+z^2))/r5);
    %du/(dzdz)
    H(3,3,1)=-((x*(x^2+y^2-2*z^2))/r5);
    %du/(dxdy)
    H(1,2,1)=-((y*(-2*x^2+y^2+z^2))/r5);
    %du/(dydx)
    H(2,1,1)=H(1,2,1);
    %du/(dxdz)
    H(1,3,1)=-((z*(-2*x^2+y^2+z^2))/r5);
    %du/(dzdx)
    H(3,1,1)=H(1,3,1);
    %du/(dydz)
    H(2,3,1)=(3*x*y*z)/r5;
    %du/(dzdy)
    H(3,2,1)=H(2,3,1);

    %Rotate the values back into global coordinates
    HTotal(:,:,1,curPoint)=M'*H(:,:,1)*M;
end

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function out = CO_trev(y,tau)
% CO_trev   Normalized nonlinear autocorrelation, trev function of a time series
%
% Calculates the trev function, a normalized nonlinear autocorrelation,
% mentioned in the documentation of the TSTOOL nonlinear time-series analysis
% package (available here: http://www.physik3.gwdg.de/tstool/).
%
% The quantity is often used as a nonlinearity statistic in surrogate data
% analysis, cf. "Surrogate time series", T. Schreiber and A. Schmitz, Physica D,
% 142(3-4) 346 (2000).
%
%---INPUTS:
%
% y, time series
%
% tau, time lag (can be 'ac' or 'mi' to set as the first zero-crossing of the
%       autocorrelation function, or the first minimum of the automutual
%       information function, respectively)
%
%---OUTPUTS:
% The raw trev expression, its magnitude, the numerator and its magnitude, and
% the denominator.

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

%-------------------------------------------------------------------------------
%% Set defaults:
if nargin < 2 || isempty(tau)
    tau = 'ac';
end

%-------------------------------------------------------------------------------
% Can set the time lag, tau, to be 'ac' or 'mi':
if strcmp(tau,'ac')
    tau = CO_FirstCrossing(y,'ac',0,'discrete');
    % tau is first zero crossing of the autocorrelation function
elseif strcmp(tau,'mi')
    tau = CO_FirstMin(y,'mi');
    % tau is the first minimum of the automutual information function
end
if isnan(tau)
    error('No valid setting for time delay. (Is the time series too short?)');
end

%-------------------------------------------------------------------------------
% Compute trev quantities:

yn = y(1:end-tau);
yn1 = y(1+tau:end); % yn, tau steps ahead

% The trev expression used in TSTOOL:
out.raw = mean((yn1-yn).^3)/(mean((yn1-yn).^2))^(3/2);

% The magnitude
out.abs = abs(out.raw);

% The numerator
out.num = mean((yn1-yn).^3);
out.absnum = abs(out.num);

% The denominator
out.denom = (mean((yn1-yn).^2))^(3/2);

end


function best_weights = GetBestMixingWeight(stand_dev_of_variables)
% Best mixing of random variables with common mean
% and built-in constraint
%   best_weights = GetBestMixingWeight(stand_dev_of_variables)
%
% Input argument
%   stand_dev_of_variables: standard deviation of each variable
%
% Output argument
%   best_weights: best set of weights for mixing random variables

% Copyright 2016 Google Inc. All Rights Reserved.
% Author: hidekik@google.com (Hideki Kawahara)

narginchk(1, 1);
n_variables = length(stand_dev_of_variables);
H = ones(n_variables - 1, n_variables - 1) * ...
  stand_dev_of_variables(n_variables) ^ 2;
H = H + diag(stand_dev_of_variables(1 : n_variables - 1) .^ 2);
v = ones(n_variables - 1, 1) * stand_dev_of_variables(n_variables) ^ 2;
a = H \ v;
best_weights = [a; 1 - sum(a)];
end


function [LS,LD,I] = isolines(V,F,S,iso)
  % ISOLINES compute a list of isolines for a scalar field S defined on the
  % mesh (V,F)
  %
  % [LS,LD,I] = isolines(V,F,S,iso)
  %
  % Inputs:
  %   V  #V by dim list of vertex positions
  %   F  #F by 3 list of triangle indices
  %   S  #V list of scalar values defined on V
  %   iso #iso list of iso values
  % Outputs:
  %   LS  #L by dim list of isoline start positions
  %   LD  #L by dim list of isoline destination positions
  %   I  #L list of indices into iso revealing corresponding iso value
  %
  % alternative: tracing isolines should result in smaller plot data (every
  % vertex only appears once
  %
  % Example:
  %   iso = linspace(min(S),max(S),10);
  %   [LS,LD] = isolines(V,F,S,iso);
  %   colormap(jet(numel(iso)-1));
  %   tsurf(F,V,'CData',S,fphong);
  %   hold on;
  %   plot([LS(:,1) LD(:,1)]',[LS(:,2) LD(:,2)]', ...
  %     'Color','k','LineWidth',10);
  %   hold off;
  %

  % make sure iso is a ROW vector
  iso = iso(:)';
  % number of isolines
  niso = numel(iso);

  % number of domain positions
  n = size(V,1);
  % number of dimensions
  dim = size(V,2);

  % number of faces
  m = size(F,1);

  % Rename for convenience
  S1 = S(F(:,1),:);
  S2 = S(F(:,2),:);
  S3 = S(F(:,3),:);

  % t12(i,j) reveals parameter t where isovalue j falls on the line from
  % corner 1 to corner 2 of face i
  t12 = bsxfun(@rdivide,bsxfun(@minus,iso,S1),S2-S1);
  t23 = bsxfun(@rdivide,bsxfun(@minus,iso,S2),S3-S2);
  t31 = bsxfun(@rdivide,bsxfun(@minus,iso,S3),S1-S3);

  % replace values outside [0,1] with NaNs
  t12( (t12<-eps)|(t12>(1+eps)) ) = NaN;
  t23( (t23<-eps)|(t23>(1+eps)) ) = NaN;
  t31( (t31<-eps)|(t31>(1+eps)) ) = NaN;

  % masks for line "parallel" to 12
  l12 = ~isnan(t23) & ~isnan(t31);
  l23 = ~isnan(t31) & ~isnan(t12);
  l31 = ~isnan(t12) & ~isnan(t23);

  % find non-zeros (lines) from t23 to t31
  [F12,I12,~] = find(l12);
  [F23,I23,~] = find(l23);
  [F31,I31,~] = find(l31);
  % indices directly into t23 and t31 corresponding to F12 and I12
  %ti12 = sub2ind(size(l12),F12,I12);
  %ti23 = sub2ind(size(l23),F23,I23);
  %ti31 = sub2ind(size(l31),F31,I31);
  % faster sub2ind
  ti12 = F12+(I12-1)*size(l12,1);
  ti23 = F23+(I23-1)*size(l23,1);
  ti31 = F31+(I31-1)*size(l31,1);

  % compute actual position values
  LS = [ ...
    ... % average of vertex positions between 2 and 3
    bsxfun(@times,1-t23(ti12), V(F(F12,2),:)) + ...
    bsxfun(@times,  t23(ti12), V(F(F12,3),:)) ...
    ; ... % average of vertex positions between 2 and 3
    bsxfun(@times,1-t31(ti23), V(F(F23,3),:)) + ...
    bsxfun(@times,  t31(ti23), V(F(F23,1),:)) ...
    ;... % average of vertex positions between 2 and 3
    bsxfun(@times,1-t12(ti31), V(F(F31,1),:)) + ...
    bsxfun(@times,  t12(ti31), V(F(F31,2),:)) ...
    ];
  LD = [...
    ... % hcat with average of vertex positions between 3 and 1
    bsxfun(@times,1-t31(ti12), V(F(F12,3),:)) + ...
    bsxfun(@times,  t31(ti12), V(F(F12,1),:)) ...
    ;... % hcat with average of vertex positions between 3 and 1
    bsxfun(@times,1-t12(ti23), V(F(F23,1),:)) + ...
    bsxfun(@times,  t12(ti23), V(F(F23,2),:)) ...
    ;... % hcat with average of vertex positions between 3 and 1
    bsxfun(@times,1-t23(ti31), V(F(F31,2),:)) + ...
    bsxfun(@times,  t23(ti31), V(F(F31,3),:)) ...
    ];

  % I is just concatenation of each I12
  I = [I12;I23;I31];

end


%% SQUARESTOKE Stokes equations on the unit square
%
%   SQUARESTOKE computes P2-P1 approximations of the Stokes equations in
%   the unit square on a sequence of meshes obtained by uniform refinement.
%   It plots the approximation error (pressue in L2 norm and velocity in H1
%   norm) vs the number of dof.
%
% See also StokesP2P1, collidingflow, squarePoisson
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

close all; clear all;
%% Set up
maxIt = 4;
N = zeros(maxIt,1); erru = zeros(maxIt,1); errp = zeros(maxIt,1);

%% Generate initial mesh
[node,elem] = squaremesh([0 1 0 1], 0.25);
bdFlag = setboundary(node,elem,'Dirichlet');

%% PDE and options
pde = Stokesdata2;

%% Finite Element Method
for k = 1:maxIt
    % solve the equation
    [u,p,edge,A] = StokesP2P1(node,elem,pde,bdFlag);
    N(k) = length(u)+length(p);
    if N(k) < 2e3 % show solution for small size
        figure(1);  showresult(node,elem,p);
    end
    % compute error
    uI = pde.exactu([node; (node(edge(:,1),:)+node(edge(:,2),:))/2]);
    erru(k) = sqrt((u-uI(:))'*A*(u-uI(:)));
    errp(k) = getL2error(node,elem,pde.exactp,p);
    % refine mesh
    [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
end

%% Plot convergence rates
figure(2); clf;
showrate2(N,erru,2,'-*','||Du_I-Du_h||',N,errp,2,'k-+','|| p - p_h||');


% IMSHAPES Generation of images representing geometric shapes
% Version 1.1 01-Jul-2011 .
%
%   This module contains several function for creating synthetic images of
%   common shapes, in 2D (ellipses, rectangles...), or in 3D (balls and
%   ellipsoids, cuboids, torus...).
%   Such discrete representation can be used as phantom to test and / or
%   validate algorithms of binary images measurements.
%
%   Requires the 'geom2d', the 'geom3d', and the 'imFilters' toolboxes.
%
% Shapes 2D:
%   discretePoints         - Discretize a set of points
%   discreteDisc           - Discretize a disc
%   discreteEllipse        - Discretize a planar ellipse
%   discreteSquare         - Discretize a planar square
%   discreteRectangle      - Discretize a planar rectangle
%   discreteCapsule        - Discretize a planar capsule
%   discretePolygon        - Discretize a planar polygon
%   discretePolyline       - Discretize a planar polyline
%   discreteHalfPlane      - Discretize a half plane
%   discreteParabola       - Discretize a planar parabola
%   discreteEgg            - Discretize a planar egg
%   discreteStarfish       - Discretize a starfish curve
%   discreteTrefoil        - Discretize a trefoil curve
%   discreteCurve          - Discretize a planar curve
%
% Shapes 3D:
%   discreteBall           - Discretize a 3D Ball
%   discreteHalfBall       - discretize a 3D half-ball
%   discreteEllipsoid      - discretize a 3D ellipsoid
%   discreteCuboid         - discretize a 3D cuboid
%   discreteCube           - discretize a 3D cube
%   discreteTorus          - Discretize a 3D Torus
%   discreteCylinder       - Discretize a 3D cylinder
%   discreteCapsule3d      - Create binary image of a 3D capsule
%   discreteReuleauxRevol  - Discretize the revolution of a Reuleaux triangle
%   discreteSphereEighth   - Discretize a 3D sphere eighth
%
% Tessellations:
%   imPointsInfluenceZones - Maps influence zones of a set of 2D/3D points
%   imvoronoi2d            - Generate a 2D voronoi image from a set of points
%   imvoronoi3d            - generate a 3D voronoi image from a set of points
%   dilatedVoronoi         - Simulate a 'thick' voronoi tesselation
%   imAWVoronoi            - generate Additively Weighted Voronoi Diagram image
%   imPowerDiagram         - Power diagramm of a set of points
%
%
% Author: David Legland
% e-mail: david.legland@inra.fr
% http://github.com/mattools/matImage
% Copyright 2009 INRA - Cepia Software Platform.

help('imShapes');


section_initializeVariablesAndSettings;

%% read images
targetFile = {'tomo.img', 'lena.bmp', 'bridge.img'};   % images
imageChoice = 1;
targetFile1 = strcat(targetFolder, '\', char(targetFile(imageChoice)));
image1 = readImage(targetFile1, 1);

imageChoice = 2;
targetFile2 = strcat(targetFolder, '\', char(targetFile(imageChoice)));
image2 = readImage(targetFile2, 1);

%% process
image1InGray = setImageToGray( image1 );
image2InGray = setImageToGray( image2 );
% adjust the size
newSize = [350 350];
image1InGray = imresize(image1InGray, newSize);
image2InGray = imresize(image2InGray, newSize);

% perform fft2, obtain magnitude and phase information
[image1InGray_fft_amplitudeOnly, image1InGray_fft_phaseOnly] = FFT_image( image1InGray );
[image2InGray_fft_amplitudeOnly, image2InGray_fft_phaseOnly] = FFT_image( image2InGray );

% interleaved the 2 FFT2 results
image1_amplitudeOnlyWithImage2_phaseOnly = image1InGray_fft_amplitudeOnly.*image2InGray_fft_phaseOnly;
image2_amplitudeOnlyWithImage1_phaseOnly = image2InGray_fft_amplitudeOnly.*image1InGray_fft_phaseOnly;
% take only the real parts
image1_amplitudeWithImage2_phase_realOnly = real(ifft2(image1_amplitudeOnlyWithImage2_phaseOnly));
image2_amplitudeWithImage1_phase_realOnly = real(ifft2(image2_amplitudeOnlyWithImage1_phaseOnly));

% recover image 1
imageInGray_InSpatialDomain = IFFT_image(image1InGray_fft_amplitudeOnly, image1InGray_fft_phaseOnly);

% display the FFT2 interleaved
section_plotResults;
% results commentary
section_resultsReporting;


function [w] = projectSimplex(v)
% Computest the minimum L2-distance projection of vector v onto the probability simplex
nVars = length(v);
mu = sort(v,'descend');
sm = 0;
for j = 1:nVars
    sm = sm+mu(j);
   if mu(j) - (1/j)*(sm-1) > 0
       row = j;
       sm_row = sm;
   end
end
theta = (1/row)*(sm_row-1);
w = max(v-theta,0);


function [] = test_hexagram_shape

r1 = 2;
r2 = 1;

n = 6;
t = linspace(0, 2*pi, n+1);
x1 = r1 *[cos(t); sin(t) ];

t2 = t +pi /n;
x2 = r2 *[cos(t2); sin(t2) ];

m = 2 *n +1;
x(:, 1:2:m) = x1;
x(:, 2:2:m) = x2(:, 1:n);

ax = gca;
plotmd(ax, x)
axis(ax, 'equal')


function [C,T]=hungarian(A)
% HUNGARIAN - Solve the assignment problem using the Hungarian method.
%
% Usage: >> [C,T]=hungarian(A)
%
% Inputs:
%   A - a square (correlation/distance/cost) matrix.
%   C - the optimal assignment (closest row/col pairs)
%   T - the total (minimized) cost of the optimal assignment.
%
% Author: Niclas Borlin, 1996

% Adapted from the FORTRAN IV code in Carpaneto and Toth, "Algorithm 548:
% Solution of the assignment problem [H]", ACM Transactions on
% Mathematical Software, 6(1):104-111, 1980.
%
% v1.0  96-06-14. Niclas Borlin, niclas@cs.umu.se.
%                 Department of Computing Science, Ume University,
%                 Sweden.
%
% NOTE: A substantial effort was put into this code. If you use it for a
% publication or otherwise, please include an acknowledgement and notify
% me by email. /Niclas

[m,n]=size(A);

if (m~=n)
    error('HUNGARIAN: Cost matrix must be square!');
end

% Save original cost matrix.
orig=A;

% Reduce matrix.
A=hminired(A);

% Do an initial assignment.
[A,C,U]=hminiass(A);

% Repeat while we have unassigned rows.
while (U(n+1))
    % Start with no path, no unchecked zeros, and no unexplored rows.
    LR=zeros(1,n);
    LC=zeros(1,n);
    CH=zeros(1,n);
    RH=[zeros(1,n) -1];

    % No labelled columns.
    SLC=[];

    % Start path in first unassigned row.
    r=U(n+1);
    % Mark row with end-of-path label.
    LR(r)=-1;
    % Insert row first in labelled row set.
    SLR=r;

    % Repeat until we manage to find an assignable zero.
    while (1)
        % If there are free zeros in row r
        if (A(r,n+1)~=0)
            % ...get column of first free zero.
            l=-A(r,n+1);

            % If there are more free zeros in row r and row r in not
            % yet marked as unexplored..
            if (A(r,l)~=0 && RH(r)==0)
                % Insert row r first in unexplored list.
                RH(r)=RH(n+1);
                RH(n+1)=r;

                % Mark in which column the next unexplored zero in this row
                % is.
                CH(r)=-A(r,l);
            end
        else
            % If all rows are explored..
            if (RH(n+1)<=0)
                % Reduce matrix.
                [A,CH,RH]=hmreduce(A,CH,RH,LC,LR,SLC,SLR);
            end

            % Re-start with first unexplored row.
            r=RH(n+1);
            % Get column of next free zero in row r.
            l=CH(r);
            % Advance "column of next free zero".
            CH(r)=-A(r,l);
            % If this zero is last in the list..
            if (A(r,l)==0)
                % ...remove row r from unexplored list.
                RH(n+1)=RH(r);
                RH(r)=0;
            end
        end

        % While the column l is labelled, i.e. in path.
        while (LC(l)~=0)
            % If row r is explored..
            if (RH(r)==0)
                % If all rows are explored..
                if (RH(n+1)<=0)
                    % Reduce cost matrix.
                    [A,CH,RH]=hmreduce(A,CH,RH,LC,LR,SLC,SLR);
                end

                % Re-start with first unexplored row.
                r=RH(n+1);
            end

            % Get column of next free zero in row r.
            l=CH(r);

            % Advance "column of next free zero".
            CH(r)=-A(r,l);

            % If this zero is last in list..
            if(A(r,l)==0)
                % ...remove row r from unexplored list.
                RH(n+1)=RH(r);
                RH(r)=0;
            end
        end

        % If the column found is unassigned..
        if (C(l)==0)
            % Flip all zeros along the path in LR,LC.
            [A,C,U]=hmflip(A,C,LC,LR,U,l,r);
            % ...and exit to continue with next unassigned row.
            break;
        else
            % ...else add zero to path.

            % Label column l with row r.
            LC(l)=r;

            % Add l to the set of labelled columns.
            SLC=[SLC l];

            % Continue with the row assigned to column l.
            r=C(l);

            % Label row r with column l.
            LR(r)=l;

            % Add r to the set of labelled rows.
            SLR=[SLR r];
        end
    end
end

% Calculate the total cost.
T=sum(orig(logical(sparse(C,1:size(orig,2),1))));

function A=hminired(A)
%HMINIRED Initial reduction of cost matrix for the Hungarian method.
%
%B=assredin(A)
%A - the unreduced cost matris.
%B - the reduced cost matrix with linked zeros in each row.

% v1.0  96-06-13. Niclas Borlin, niclas@cs.umu.se.

[m,n]=size(A);

% Subtract column-minimum values from each column.
colMin=min(A);
A=A-colMin(ones(n,1),:);

% Subtract row-minimum values from each row.
rowMin=min(A')';
A=A-rowMin(:,ones(1,n));

% Get positions of all zeros.
[i,j]=find(A==0);

% Extend A to give room for row zero list header column.
A(1,n+1)=0;
for k=1:n
    % Get all column in this row.
    cols=j(k==i)';
    % Insert pointers in matrix.
    A(k,[n+1 cols])=[-cols 0];
end

function [A,C,U]=hminiass(A)
%HMINIASS Initial assignment of the Hungarian method.
%
%[B,C,U]=hminiass(A)
%A - the reduced cost matrix.
%B - the reduced cost matrix, with assigned zeros removed from lists.
%C - a vector. C(J)=I means row I is assigned to column J,
%              i.e. there is an assigned zero in position I,J.
%U - a vector with a linked list of unassigned rows.

% v1.0  96-06-14. Niclas Borlin, niclas@cs.umu.se.

[n,np1]=size(A);

% Initialize return vectors.
C=zeros(1,n);
U=zeros(1,n+1);

% Initialize last/next zero "pointers".
LZ=zeros(1,n);
NZ=zeros(1,n);

for i=1:n
    % Set j to first unassigned zero in row i.
	lj=n+1;
	j=-A(i,lj);

    % Repeat until we have no more zeros (j==0) or we find a zero
	% in an unassigned column (c(j)==0).

	while (C(j)~=0)
		% Advance lj and j in zero list.
		lj=j;
		j=-A(i,lj);

		% Stop if we hit end of list.
		if (j==0)
			break;
		end
	end

	if (j~=0)
		% We found a zero in an unassigned column.

		% Assign row i to column j.
		C(j)=i;

		% Remove A(i,j) from unassigned zero list.
		A(i,lj)=A(i,j);

		% Update next/last unassigned zero pointers.
		NZ(i)=-A(i,j);
		LZ(i)=lj;

		% Indicate A(i,j) is an assigned zero.
		A(i,j)=0;
	else
		% We found no zero in an unassigned column.

		% Check all zeros in this row.

		lj=n+1;
		j=-A(i,lj);

		% Check all zeros in this row for a suitable zero in another row.
		while (j~=0)
			% Check the in the row assigned to this column.
			r=C(j);

			% Pick up last/next pointers.
			lm=LZ(r);
			m=NZ(r);

			% Check all unchecked zeros in free list of this row.
			while (m~=0)
				% Stop if we find an unassigned column.
				if (C(m)==0)
					break;
				end

				% Advance one step in list.
				lm=m;
				m=-A(r,lm);
			end

			if (m==0)
				% We failed on row r. Continue with next zero on row i.
				lj=j;
				j=-A(i,lj);
			else
				% We found a zero in an unassigned column.

				% Replace zero at (r,m) in unassigned list with zero at (r,j)
				A(r,lm)=-j;
				A(r,j)=A(r,m);

				% Update last/next pointers in row r.
				NZ(r)=-A(r,m);
				LZ(r)=j;

				% Mark A(r,m) as an assigned zero in the matrix . . .
				A(r,m)=0;

				% ...and in the assignment vector.
				C(m)=r;

				% Remove A(i,j) from unassigned list.
				A(i,lj)=A(i,j);

				% Update last/next pointers in row r.
				NZ(i)=-A(i,j);
				LZ(i)=lj;

				% Mark A(r,m) as an assigned zero in the matrix . . .
				A(i,j)=0;

				% ...and in the assignment vector.
				C(j)=i;

				% Stop search.
				break;
			end
		end
	end
end

% Create vector with list of unassigned rows.

% Mark all rows have assignment.
r=zeros(1,n);
rows=C(C~=0);
r(rows)=rows;
empty=find(r==0);

% Create vector with linked list of unassigned rows.
U=zeros(1,n+1);
U([n+1 empty])=[empty 0];

function [A,C,U]=hmflip(A,C,LC,LR,U,l,r)
%HMFLIP Flip assignment state of all zeros along a path.
%
%[A,C,U]=hmflip(A,C,LC,LR,U,l,r)
%Input:
%A   - the cost matrix.
%C   - the assignment vector.
%LC  - the column label vector.
%LR  - the row label vector.
%U   - the
%r,l - position of last zero in path.
%Output:
%A   - updated cost matrix.
%C   - updated assignment vector.
%U   - updated unassigned row list vector.

% v1.0  96-06-14. Niclas Borlin, niclas@cs.umu.se.

n=size(A,1);

while (1)
    % Move assignment in column l to row r.
    C(l)=r;

    % Find zero to be removed from zero list..

    % Find zero before this.
    m=find(A(r,:)==-l);

    % Link past this zero.
    A(r,m)=A(r,l);

    A(r,l)=0;

    % If this was the first zero of the path..
    if (LR(r)<0)
        % remove row from unassigned row list and return.
        U(n+1)=U(r);
        U(r)=0;
        return;
    else

        % Move back in this row along the path and get column of next zero.
        l=LR(r);

        % Insert zero at (r,l) first in zero list.
        A(r,l)=A(r,n+1);
        A(r,n+1)=-l;

        % Continue back along the column to get row of next zero in path.
        r=LC(l);
    end
end

function [A,CH,RH]=hmreduce(A,CH,RH,LC,LR,SLC,SLR)
%HMREDUCE Reduce parts of cost matrix in the Hungerian method.
%
%[A,CH,RH]=hmreduce(A,CH,RH,LC,LR,SLC,SLR)
%Input:
%A   - Cost matrix.
%CH  - vector of column of 'next zeros' in each row.
%RH  - vector with list of unexplored rows.
%LC  - column labels.
%RC  - row labels.
%SLC - set of column labels.
%SLR - set of row labels.
%
%Output:
%A   - Reduced cost matrix.
%CH  - Updated vector of 'next zeros' in each row.
%RH  - Updated vector of unexplored rows.

% v1.0  96-06-14. Niclas Borlin, niclas@cs.umu.se.

n=size(A,1);

% Find which rows are covered, i.e. unlabelled.
coveredRows=LR==0;

% Find which columns are covered, i.e. labelled.
coveredCols=LC~=0;

r=find(~coveredRows);
c=find(~coveredCols);

% Get minimum of uncovered elements.
m=min(min(A(r,c)));

% Subtract minimum from all uncovered elements.
A(r,c)=A(r,c)-m;

% Check all uncovered columns..
for j=c
    % ...and uncovered rows in path order..
    for i=SLR
        % If this is a (new) zero..
        if (A(i,j)==0)
            % If the row is not in unexplored list..
            if (RH(i)==0)
                % ...insert it first in unexplored list.
                RH(i)=RH(n+1);
                RH(n+1)=i;
                % Mark this zero as "next free" in this row.
                CH(i)=j;
            end
            % Find last unassigned zero on row I.
            row=A(i,:);
            colsInList=-row(row<0);
            if (length(colsInList)==0)
                % No zeros in the list.
                l=n+1;
            else
                l=colsInList(row(colsInList)==0);
            end
            % Append this zero to end of list.
            A(i,l)=-j;
        end
    end
end

% Add minimum to all doubly covered elements.
r=find(coveredRows);
c=find(coveredCols);

% Take care of the zeros we will remove.
[i,j]=find(A(r,c)<=0);

i=r(i);
j=c(j);

for k=1:length(i)
    % Find zero before this in this row.
    lj=find(A(i(k),:)==-j(k));
    % Link past it.
    A(i(k),lj)=A(i(k),j(k));
    % Mark it as assigned.
    A(i(k),j(k))=0;
end

A(r,c)=A(r,c)+m;


% this function calculate the PSNR

function [PeakSNR, Mean2err]=psnr(OriginalImage, DegradedImage)
OriginalImage=double(OriginalImage);
DegradedImage=double(DegradedImage);
[N,M] = size(OriginalImage);
Imax = max(max(DegradedImage));
SumOfDiff2 = sum(sum((OriginalImage-DegradedImage).*(OriginalImage-DegradedImage)));
Mean2err=SumOfDiff2./(M*N);
sdf=Imax^2./(Mean2err);
if sdf ==0
    sdf=1;
end

PeakSNR = 10*log10(sdf);


function [ vX, mX ] = SolveLsL1Admm( vX, mA, vY, paramLambda, sSolverParams )
% ----------------------------------------------------------------------------------------------- %
%[ vX ] = SolveProxTvAdmm( vY, mD, paramLambda, numIterations )
% Solves the L1 Norm Regualrized Least Squares using Alternating
% Direction Method of Multipliers (ADMM) Method.
% Basically solves the problem given by:
% $$ \arg \min_{ x \in \mathbb{R}^{n} } \frac{1}{2} {\left\| A x - y \right|}_{2}^{2} + \lambda {\left\| x \right\|}_{1} $$
% Input:
%   - vX                -   Optimization Vector.
%                           The vector to be optimized. Initialization of
%                           the iterative process.
%                           Structure: Vector (n X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - mA                -   Input Matirx.
%                           The model matrix.
%                           Structure: Matrix (m X n).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - vY                -   Measurements Vector.
%                           The model known data.
%                           Structure: Vector (m X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - paramLambda       -   Parameter Lambda.
%                           The L1 Regularization parameter.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: (0, inf).
%   - paramRho          -   The Rho Parameter.
%                           Sets the weight of the equality constraint.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range (0, inf).
%   - numIterations     -   Number of Iterations.
%                           Number of iterations of the algorithm.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range {1, 2, ...}.
% Output:
%   - vX                -   Output Vector.
%                           Structure: Vector (n X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
% References
%   1.  Wikipedia ADMM - https://en.wikipedia.org/wiki/Augmented_Lagrangian_method#Alternating_direction_method_of_multipliers.
% Remarks:
%   1.  Using vanilla ADMM with no optimization of the parameter or
%       smoothing.
%   2.  For high values of `paramLambda` it will require many iterations.
% Known Issues:
%   1.  C
% TODO:
%   1.  D
% Release Notes:
%   -   1.0.000     05/08/2021  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

arguments
    vX (:, 1) {mustBeFloat, mustBeReal}
    mA (:, :) {mustBeFloat, mustBeReal}
    vY (:, 1) {mustBeFloat, mustBeReal}
    paramLambda (1, 1) {mustBeFloat, mustBeReal, mustBePositive}
    sSolverParams.paramRho (1, 1) {mustBeNumeric, mustBeReal, mustBePositive} = 5
    sSolverParams.numIterations (1, 1) {mustBeNumeric, mustBeReal, mustBePositive, mustBeInteger} = 100
end

MAT_TYPE_SKINNY = 1;
MAT_TYPE_FAT    = 2;

if(size(mA, 1) >= size(mA, 2))
    matType = MAT_TYPE_SKINNY;
else
    matType = MAT_TYPE_FAT;
end

paramRho        = sSolverParams.paramRho;
numIterations   = sSolverParams.numIterations;

mX = zeros(size(mA, 2), numIterations);

% Caching facotirzation
mC = MatrixFactorize(mA, paramRho, matType);
vAy = mA.' * vY;

vZ = ProxL1(vX, paramLambda / paramRho);
vU = vX - vZ;

mX(:, 1) = vX;

for ii = 2:numIterations

    vQ = vAy + (paramRho * (vZ - vU));

    % Matrix Inversion Lemma
    switch(matType)
        case(MAT_TYPE_SKINNY)
            vX = mC \ vQ;
        case(MAT_TYPE_FAT)
            vX = (vQ / paramRho) - ((mA.' * (mC \ (mA * vQ))) / (paramRho * paramRho));
    end

    vZ = ProxL1(vX + vU, paramLambda / paramRho);
    vU = vU + vX - vZ;

    mX(:, ii) = vX;

end

end

function [ mC ] = MatrixFactorize( mA, paramRho, matType )

MAT_TYPE_SKINNY = 1;
MAT_TYPE_FAT    = 2;

switch(matType)
    case(MAT_TYPE_SKINNY)
        mI = speye(size(mA, 2));
        mC = decomposition((mA.' * mA) + (paramRho * mI), 'chol');
    case(MAT_TYPE_FAT)
        mI = speye(size(mA, 1));
        mC = decomposition(mI + ((1 / paramRho) * (mA * mA.')), 'chol');
end

end

function [ vX ] = ProxL1( vX, lambdaFactor )

% Soft Thresholding
vX = max(vX - lambdaFactor, 0) + min(vX + lambdaFactor, 0);

end


function c = roots_to_i4poly ( n, x )

%*****************************************************************************80
%
%% ROOTS_TO_I4POLY converts polynomial roots to polynomial coefficients.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of roots specified.
%
%    Input, integer X(N), the roots.
%
%    Output, integer C(1:N+1), the coefficients of the polynomial.
%

%
%  Initialize C to (0, 0, ..., 0, 1).
%  Essentially, we are setting up a divided difference table.
%
  c(1:n) = 0;
  c(n+1) = 1;
%
%  Convert to standard polynomial form by shifting the abscissas
%  of the divided difference table to 0.
%
  for j = 1 : n
    for i = 1 : n+1-j
      c(n-i) = c(n-i) - x(n+1-i-j+1) * c(n-i+1);
    end
  end

  return
end


%DELTA2TR Convert differential motion  to SE(3) homogeneous transform
%
% T = DELTA2TR(D) is a homogeneous transform (4x4) representing differential
% motion D (6x1).
%
% The vector D=(dx, dy, dz, dRx, dRy, dRz) represents infinitessimal translation
% and rotation, and is an approximation to the instantaneous spatial velocity
% multiplied by time step.
%
% Reference::
% - Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p67.
%
% See also tr2delta, SE3.delta.

% 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 delta = delta2tr(d)
    d = d(:);
    delta = eye(4,4) + [skew(d(4:6)) d(1:3); 0 0 0 0];


function cg_test023 ( )

%*****************************************************************************80
%
%% CG_TEST023 tests R83S_CG.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 July 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CG_TEST023\n' );
  fprintf ( 1, '  Test R83S_CG, applying CG to an R83S matrix.\n' );

  n = 10;
%
%  Let A be the -1 2 -1 matrix.
%
  a = r83s_dif2 ( n, n );
%
%  Choose a random solution.
%
  seed = 123456789;
  [ x1, seed ] = r8vec_uniform_01 ( n, seed );
%
%  Compute the corresponding right hand side.
%
  b = r83s_mv ( n, n, a, x1 );
%
%  Call the CG routine.
%
  x2 = ones ( n, 1 );
  x2 = r83s_cg ( n, a, b, x2 );
%
%  Compute the residual.
%
  r = r83s_resid ( n, n, a, x2, b );
  r_norm = r8vec_norm ( n, r );
%
%  Compute the error.
%
  e_norm = r8vec_diff_norm ( n, x1, x2 );
%
%  Report.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of variables N = %d\n', n );
  fprintf ( 1, '  Norm of residual ||Ax-b|| = %g\n', r_norm );
  fprintf ( 1, '  Norm of error ||x1-x2|| = %g\n', e_norm );

  return
end


function X = PQRFFT (x, N, ifn)
% Calculate the DFT of a real N-point sequence or the inverse
% DFT corresponding to a real N-point sequence.
% ifn > 0, forward transform
%          input x(n)  - N real values
%          output X(k) - The first N/2+1 points are the real
%            parts of the transform, the next N/2-1 points
%            are the imaginary parts of the transform. However
%            the imaginary part for the first point and the
%            middle point which are known to be zero are not
%            stored.
% ifn < 0, inverse transform
%          input X(k) - The first N/2+1 points are the real
%            parts of the transform, the next N/2-1 points
%            are the imaginary parts of the transform. However
%            the imaginary part for the first point and the
%            middle point which are known to be zero are not
%            stored.
%          output x(n) - N real values

% P. Kabal $Revision: 1.1 $  $Date: 2003/12/07 13:34:11 $

if (ifn > 0)
    X = fft (x, N);
    XR = real(X(0+1:N/2+1));
    XI = imag(X(1+1:N/2-1+1));
    X = [XR XI];
else
    xR = [x(0+1:N/2+1)];
    xI = [0 x(N/2+1+1:N-1+1) 0];
    x = complex ([xR xR(N/2-1+1:-1:1+1)], [xI -xI(N/2-1+1:-1:1+1)]);
    X = real (ifft (x, N));
end


%% this is an implementation of kalman filter using belief propagation.
% author: Shuang Wang
% email: shw070@ucsd.edu
% Division of Biomedical Informatics, University of California, San Diego.

clear;
addpath(genpath('.'));
%%
% x_k = A x_t-1 + q; N(0, V_Q);
% y_k = B x_k + w; N(0, V_W);
% x_1 ~ N(mu_0, V_0);

%% generating fake data.
s = RandStream('mt19937ar','Seed',4);
RandStream.setGlobalStream(s);
T = 40;
x = zeros(2, T);
y = zeros(2, T);
d_y = size(y, 1);
d_x = size(x, 1);
mu_0 = [4; 4];
V_0 = [1, 0
       0, 1];
V_Q = [0.1,   0
         0, 0.3];
V_W = [0.05,   0
          0, 0.1];
A = [   1, 0.04
     0.02,    1];
B = [1, 0
     0, 1];

 x(:, 1) = mvnrnd(mu_0, V_0, 1)';
 y(:, 1) = B * x(:, 1) + mvnrnd([0, 0], V_W, 1)';
 for t = 2:T
     x(:, t) = A * x(:, t-1) + mvnrnd([0, 0], V_Q, 1)';
     y(:, t) = B * x(:, t) + mvnrnd([0, 0], V_W, 1)';
 end

[msg, varNode_x, factorNode_x, factorNode_y, belief] = initializeMessage(T, mu_0, V_0, d_x, y, B, V_W);

for bp_iter = 1:2*T
    %% variable node update
    for i = 1:T
        if (i < T)
            msg{varNode_x{i}.backwardNeighborMsgID}.toFactorNode ...
                = GaussianMultiply(msg{varNode_x{i}.forwardNeighborMsgID}.toVarNode,...
                msg{varNode_x{i}.lowerNeighborMsgID}.toVarNode);

            msg{varNode_x{i}.forwardNeighborMsgID}.toFactorNode ...
                = GaussianMultiply(msg{varNode_x{i}.backwardNeighborMsgID}.toVarNode,...
                msg{varNode_x{i}.lowerNeighborMsgID}.toVarNode);
        else
            msg{varNode_x{i}.backwardNeighborMsgID}.toFactorNode = msg{varNode_x{i}.lowerNeighborMsgID}.toVarNode;
        end
    end

    %% factor node update
    for i = 2:T
        msg{factorNode_x{i}.forwardNeighborMsgID}.toVarNode.mu ...
            = A * msg{factorNode_x{i}.backwardNeighborMsgID}.toFactorNode.mu;
        if(~isempty(msg{factorNode_x{i}.backwardNeighborMsgID}.toFactorNode.V))
            msg{factorNode_x{i}.forwardNeighborMsgID}.toVarNode.V ...
                = A * msg{factorNode_x{i}.backwardNeighborMsgID}.toFactorNode.V * (A') + V_Q;
            msg{factorNode_x{i}.forwardNeighborMsgID}.toVarNode.iV = [];
        else
            msg{factorNode_x{i}.forwardNeighborMsgID}.toVarNode.V = [];
            tmp_iAtiviA = (A')\msg{factorNode_x{i}.backwardNeighborMsgID}.toFactorNode.iV/A;
            msg{factorNode_x{i}.forwardNeighborMsgID}.toVarNode.iV ...
                = tmp_iAtiviA -(eye(d_x) + tmp_iAtiviA*V_Q)\tmp_iAtiviA*V_Q*tmp_iAtiviA;
        end
        msg{factorNode_x{i}.backwardNeighborMsgID}.toVarNode.mu ...
            = A\msg{factorNode_x{i}.forwardNeighborMsgID}.toFactorNode.mu;
        if(~isempty(msg{factorNode_x{i}.forwardNeighborMsgID}.toFactorNode.V))
            msg{factorNode_x{i}.backwardNeighborMsgID}.toVarNode.iV = [];
            msg{factorNode_x{i}.backwardNeighborMsgID}.toVarNode.V ...
                = A\(msg{factorNode_x{i}.forwardNeighborMsgID}.toFactorNode.V + V_Q)/(A');
        else
            tmp_iv = msg{factorNode_x{i}.forwardNeighborMsgID}.toFactorNode.iV;
            msg{factorNode_x{i}.backwardNeighborMsgID}.toVarNode.V = [];
            msg{factorNode_x{i}.backwardNeighborMsgID}.toVarNode.iV ...
                = (A')*(tmp_iv -(eye(d_x) + tmp_iv*V_Q)\tmp_iv*V_Q*tmp_iv)*A;
        end
    end
end % bp_iter

%% update belief
for i = 1:T
    belief{i} = GaussianMultiply(msg{varNode_x{i}.backwardNeighborMsgID}.toFactorNode,...
                msg{varNode_x{i}.backwardNeighborMsgID}.toVarNode);
    mu_tmp_sm(:,i) = belief{i}.mu;
end
%%
figure(1); clf;
plot(x(1, :), x(2, :), '.'); hold on;
plot(y(1, :), y(2, :), 'ro'); hold on;
plot(mu_tmp_sm(1, :), mu_tmp_sm(2, :), 'g*'); hold on;
h = legend('Hidden state', 'observation', 'kalman smoother');
set(h, 'box', 'off', 'location', 'best');
fprintf('SAD:x_z = %f\n', sum(abs((y(:) - x(:)))));
fprintf('SAD:kf_sm_z = %f\n', sum(abs((mu_tmp_sm(:) - x(:)))));


% AUTHOR :   CHANDAN KUMAR
% SUBJECT:   COMPUTER GRAPHICS AND SOLID MODELLING.
% TITLE  :   LINE CLIPPING ALGORITHM (CYRUS BECK )
% DATE   :   31/08/09
function lineclip()
clc, clear all;
x = input('Give coord[ x0 y0 x1 y1]: ');
v = input('Give Viewport coord[vx0 vy0 vx1 vy1]: ');
d = [(x(3)-x(1)) (x(4)-x(2)) -(x(3)-x(1))  -(x(4)-x(2))];
for i = 1:4
    if (i==1)||(i==3)
        j =1;
    else
        j =2;
    end
    t(i) = (v(i)-x(j))/d(j);
end
tl = min(t(1),t(3));
ti = max(t(2),t(4));

z(1) = x(1) +d(1)*ti ;   % x coord of clipped line
z(3) = x(1) +d(1)*tl ;   % end X coord of clipped line
z(2) = x(2) +d(2)*ti ;
z(4) = x(2) +d(2)*tl ;

plot([z(1) z(3)] ,[z(2) z(4)],'r-','LineWidth',2);   % Plots Clipped line
hold on,grid on
plot([v(1) v(1)] ,[v(2) v(4)],'b-','LineWidth',2);   % Plots Viewport
plot([x(1) z(3)],[x(2) z(4)],'k-');                  % Plots Unclipped line
legend('Clipped line','Viewport','Unclipped line',3)
plot([z(1) x(3)],[z(2) x(4)],'k-');                  % Plots Unclipped line
plot([v(1) v(3)] ,[v(2) v(2)],'b-','LineWidth',2);   % Plots Viewport
plot([v(3) v(3)] ,[v(2) v(4)],'b-','LineWidth',2);   % Plots Viewport
plot([v(1) v(3)] ,[v(4) v(4)],'b-','LineWidth',2);   % Plots Viewport
axis([v(1)-1 v(3)+1 v(2)-1 v(4)+1]);
title('Line clipping by Cyrus Beck algorithm')


function ival = c8mat_is_column_orthogonal ( m, n, a )

%*****************************************************************************80
%
%% C8MAT_IS_COLUMN_ORTHOGONAL checks if a complex matrix is column orthogonal.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the row and column dimensions
%    of the matrix.  M and N must be positive.
%
%    Input, complex A(M,N), the matrix.
%
%    Output, integer IVAL:
%    -1, the matrix is not column orthogonal.
%    1, the matrix is column orthogonal.
%
  tol = 0.0001;
  ival = 1;

  for j1 = 1 : n

    for j2 = j1+1 : n

      deviation = abs ( a(1:m,j1)' * a(1:m,j2) );
      deviation_max = max ( deviation_max, deviation );

    end

  end

  if ( deviation_max < tol )
    ival = +1;
  else
    ival = -1;
  end

  return
end


function a = myiswt2(swc, lo_r, hi_r)
% ISWT2 Inverse discrete stationary wavelet transform 2-D.
% ISWT2 performs a multilevel 2-D stationary wavelet
% reconstruction using either a specific orthogonal wavelet
% ('wname', see WFILTERS for more information) or specific
% reconstruction filters (Lo_r and Hi_r).
%
% Filters lo_r and hi_r must be normalized by dividing by sqrt(2)
% (necessary for implmentation of Stationary Discrete WT)
%
% For X = MYISWT2(swc,Lo_r,Hi_r)
% swc contains H(1:N) V(1:N) D(1:N) A(N)
% lo_r is the reconstruction low-pass filter.
% hi_r is the reconstruction high-pass filter.
%
% See also IDWT2, SWT2, WAVEREC2.

% based on iswt2.m
% Sathish Ramani, October 11, 2009

%% Set DWT_Mode to 'per'.
old_modeDWT = dwtmode('status','nodisp');
modeDWT = 'per';
dwtmode(modeDWT,'nodisp');

%% Load coefficients
n = (size(swc, 3)-1)/3; %% Number of levels
h = swc(:,:,1:n);
v = swc(:,:,n+1:2*n);
d = swc(:,:,2*n+1:3*n);
a = swc(:,:,3*n+1);

[rx, cx, dump] = size(h);
for k = n:-1:1
	step = 2^(k-1);
	last = step;
	for first1 = 1:last
		iRow = first1:step:rx;
		lR   = length(iRow);
		for first2 = 1:last
			iCol = first2:step:cx;
			lC   = length(iCol);
			sR   = iRow;
			sC   = iCol;
			a(iRow,iCol) = idwt2LOC(a(sR,sC), h(sR,sC,k), v(sR,sC,k), d(sR,sC,k), ...
				lo_r, hi_r, [lR lC]);
		end
	end
end

% Restore DWT_Mode.
dwtmode(old_modeDWT, 'nodisp');

%===============================================================%
% INTERNAL FUNCTIONS
%===============================================================%
function y = idwt2LOC(a, h, v, d, lo_r, hi_r, sy)

y = upconvLOC(a,lo_r,lo_r,sy) ... % Approximation
	+ upconvLOC(h,hi_r,lo_r,sy) ... % Horizontal Detail
	+ upconvLOC(v,lo_r,hi_r,sy) ... % Vertical Detail
	+ upconvLOC(d,hi_r,hi_r,sy); % Diagonal Detail

%---------------------------------------------------------------%
function y = upconvLOC(x, f1, f2, s)

lf = length(f1);
% y  = dyadup(x,'mat',0,1);
y = x;
y  = wextend('2D', 'per', y, [lf/2,lf/2]);
y  = wconv2('col', y, f1);
y  = wconv2('row', y, f2);
y  = wkeep2(y, s, [lf lf]);
%===============================================================%