function [A,B] = kmo(X)
%KMO Kaiser-Meyer-Olkin Measure of Sampling Adequacy.
% Factor analysis can be used as a guide to how coherently a set of variables
% relate to a hypothesized underlying dimension that they are all being used
% to measure. External validity analysis assesses whether the scale that has
% been constructed performs as theoretically expected in correlation with
% other variables to which it is expected to be related.
% There are some assumptions about the characteristics of factors that are
% extracted and defined that are unobserved common dimensions that may be
% listed to account for the correlations among observed variables. Sampling
% adequacy predicts if data are likely to factor well, based on correlation
% and partial correlation. Is used to assess which variables to drop from the
% model because they are too multicollinear.
% It has been suggested that inv(R) should be a near-diagonal matrix in order
% to successfully fit a factor analysis model.  To assess how close inv(R)
% is to a diagonal matrix, Kaiser (1970) proposed a measure of sampling
% adequacy, now called KMO (Kaiser-Meyer-Olkin) index. The common part, called
% the image of a variable, is defined as that part which is predictable by
% regressing each variable on all other variables.
% The anti-image is the specific part of the variable that cannot be predicted.
% Examining the anti-image of the correlation matrix. That is the negative of the
% partial correlations, partialling out all other variables.
% There is a KMO statistic for each individual variable and their sum is
% the overall statistic. If it is not > 0.6 drop the indicator variables with
% the lowest individual statistic value until the overall one rises above 0.6:
% factors which is meritorious. The diagonal elements on the Anti-image
% correlation matrix are the KMO individual statistics for each variable. A KMO
% index <= 0.5 indicates the correlation matrix is not suitable for factor
% analysis.
%
% Syntax: function kmo(X)
%
%     Input:
%          X - Input matrix can be a data matrix (size n-data x p-variables)
%     Output(s):
%            - Kaiser-Meyer-Olkin Index.
%            - Degree of Common Variance Report (shared by a set of variables
%              and thus assesses the degree to which they measure a common
%              underlying factor).
%        optional(s):
%            - Anti-image Covariance Matrix.
%            - Anti-image Correlation Matrix
%
%  Example: From the example given on the web page
%  http://www.ncl.ac.uk/iss/statistics/docs/factoranalysis.html
%  We are interested to calculate the Kaiser-Meyer-Olkin measure of sampling
%  adequacy in order to see if proceeds a satisfactory factor analysis to
%  investigate the reasons why customers buy a product such as a particular
%  brand of soft drink (e.g. coca cola). Several variables were identified
%  which influence customer to buy coca cola. Some of the variables identified
%  as being influential include availability of product (X1), cost of product
%  (X2), experience with product (X3), popularity of product (X4), prestige
%  attached to product (X5), quality of product (X6), quantity of product (X7),
%  and respectability of product (X8). From this, you designed a questionnaire
%  to solicit customers' view on a seven point scale, where 1 = not important
%  and 7 = very important. The results from your questionnaire are show on the
%  table below. Only the first twelve respondents (cases) are used in this
%  example.
%
%  Table 1: Customer survey
%  --------------------------------------------------
%     X1    X2    X3    X4    X5    X6    X7    X8
%  --------------------------------------------------
%      4     1     4     5     2     3     6     7
%      4     2     7     6     6     3     3     4
%      6     4     3     4     2     5     7     7
%      5     3     5     4     3     4     6     7
%      5     2     4     5     2     5     5     6
%      6     3     3     5     4     4     7     7
%      6     2     4     4     4     3     4     5
%      4     1     3     4     3     3     5     6
%      5     3     4     3     4     3     6     6
%      5     4     3     4     4     4     6     7
%      6     2     4     4     4     3     7     5
%      5     2     3     3     3     3     7     6
%  --------------------------------------------------
%
% Data matrix must be:
%  X=[4 1 4 5 2 3 6 7;4 2 7 6 6 3 3 4;6 4 3 4 2 5 7 7;5 3 5 4 3 4 6 7;
%  5 2 4 5 2 5 5 6;6 3 3 5 4 4 7 7;6 2 4 4 4 3 4 5;4 1 3 4 3 3 5 6;
%  5 3 4 3 4 3 6 6;5 4 3 4 4 4 6 7;6 2 4 4 4 3 7 5;5 2 3 3 3 3 7 6];
%
%  Calling on Matlab the function:
%            kmo(X)
%
%  Answer is:
%
%  Kaiser-Meyer-Olkin Measure of Sampling Adequacy: 0.4172
%  The KMO test yields a degree of common variance unacceptable (Don't Factor).
%
%
%  Created by A. Trujillo-Ortiz, R. Hernandez-Walls, A. Castro-Perez,
%             K. Barba-Rojo and A. Otero-Limon
%             Facultad de Ciencias Marinas
%             Universidad Autonoma de Baja California
%             Apdo. Postal 453
%             Ensenada, Baja California
%             Mexico.
%             atrujo@uabc.mx
%
%  Copyright. October 10, 2006.
%
% To cite this file, this would be an appropriate format:
% Trujillo-Ortiz, A., R. Hernandez-Walls, A. Castro-Perez, K. Barba-Rojo
%   and A. Otero-Limon (2006). kmo:Kaiser-Meyer-Olkin Measure of Sampling
%   Adequacy. A MATLAB file. [WWW document]. URL http://www.mathworks.com/
%   matlabcentral/fileexchange/loadFile.do?objectId=12736
%
%  References:
%  Rencher, A. C. (2002), Methods of Multivariate Analysis. 2nd. ed.
%            New-Jersey:John Wiley & Sons. Chapter 13 (pp. 408-450).
%

error(nargchk(1,1,nargin));
msg = nargoutchk(1, 2, nargout);

X = corrcoef(X);

iX = inv(X);
S2 = diag(diag((iX.^-1)));
AIS = S2*iX*S2; %anti-image covariance matrix
IS = X+AIS-2*S2; %image covariance matrix
Dai = diag(diag(sqrt(AIS)));
IR = inv(Dai)*IS*inv(Dai); %image correlation matrix
AIR = inv(Dai)*AIS*inv(Dai); %anti-image correlation matrix
a = sum((AIR - diag(diag(AIR))).^2);
AA = sum(a);
b = sum((X - eye(size(X))).^2);
BB = sum(b);
MSA = b./(b+a); %measures of sampling adequacy
AIR = AIR-eye(size(AIR))+diag(MSA);
%Examine the anti-image of the correlation matrix. That is the negative of the partial correlations,
%partialling out all other variables.
N = BB;
D = AA+BB;
kmo = N/D;

disp(' ')
fprintf('Kaiser-Meyer-Olkin Measure of Sampling Adequacy: %3.4f\n', kmo);
if (kmo >= 0.00 && kmo < 0.50);
    disp('The KMO test yields a degree of common variance unacceptable (Don''t Factor).')
elseif (kmo >= 0.50 && kmo < 0.60);
    disp('The KMO test yields a degree of common variance miserable.')
elseif (kmo >= 0.60 && kmo < 0.70);
    disp('The KMO test yields a degree of common variance mediocre.')
elseif (kmo >= 0.70 && kmo < 0.80);
    disp('The KMO test yields a degree of common variance middling.')
elseif (kmo >= 0.80 && kmo < 0.90);
    disp('The KMO test yields a degree of common variance meritorious.')
else (kmo >= 0.90 && kmo <= 1.00);
    disp('The KMO test yields a degree of common variance marvelous.')
end

if nargout == 1;
    disp(' ')
    disp('A = Anti-image covariance matrix.');
    A = AIS;
elseif nargout > 1;
    disp(' ')
    disp('A = Anti-image covariance matrix.');
    A = AIS;
    disp('B = Anti-image correlation matrix.');
    B = AIR;
end

return,


% INVWISHRND - Inverse Wishart Distribution - Random Matrix Value
% Copyright (c) 1998, Harvard University. Full copyright in the file Copyright
%
%   [ IW ] = invwishrnd(S,d)
%
% S = p x p symmetric, postitive definite "scale" matrix
% d = "degrees of freedom" parameter (integer)
%   = "precision" parameter (d may be non-integer)
%
% IW = random matrix from the inverse Wishart distribution
%
% Note:
%   Different sources use different parameterizations.
%   This routine uses that of Press and Shigemasu (1989):
%   density (IW) is proportional to
%     exp[-.5*trace(S*inv(IW))] / [det(IW) ^ (d/2)].
%
%   With this density definition:
%   mean of IW = S/(d-2p-2) for d>2p+2,
%   mode of IW = S/d.
%
% See also: INVWISHIRND, WISHRND

function [IW] = invwishrnd(S,d)

[p,p2] = size(S) ;

W = wishrnd(inv(S),d-p-1) ;

IW = inv(W) ;


function y = FourierShift2D(x, delta)
%
% y = FourierShift(x, [delta_x delta_y])
%
% Shifts x by delta cyclically. Uses the fourier shift theorem.
%
% Real inputs should give real outputs.
%
% By Tim Hutt, 26/03/2009
% Small fix thanks to Brian Krause, 11/02/2010

% The size of the matrix.
[N, M] = size(x);

% FFT of our possibly padded input signal.
X = fft2(x);

% The mathsy bit. The floors take care of odd-length signals.
x_shift = exp(-1i * 2 * pi * delta(1) * [0:floor(N/2)-1 floor(-N/2):-1]' / N);
y_shift = exp(-1i * 2 * pi * delta(2) * [0:floor(M/2)-1 floor(-M/2):-1] / M);

% Force conjugate symmetry. Otherwise this frequency component has no
% corresponding negative frequency to cancel out its imaginary part.
if mod(N, 2) == 0
	x_shift(N/2+1) = real(x_shift(N/2+1));
end
if mod(M, 2) == 0
	y_shift(M/2+1) = real(y_shift(M/2+1));
end

Y = X .* (x_shift * y_shift);

% Invert the FFT.
y = ifft2(Y);

% There should be no imaginary component (for real input
% signals) but due to numerical effects some remnants remain.
if isreal(x)
    y = real(y);
end

end


function pass = test_sum( pref )
% Test with function cos(cos(lam)*sin(th))

% Grab some preferences
if ( nargin == 0 )
    pref = chebfunpref();
end
tol = 1e4*pref.techPrefs.chebfuneps;

%% Integrate over r

% Example 1
f = ballfun(@(r,lam,th)r.*cos(lam).*sin(th), 'spherical');
g = sum(f, 1);
exact = spherefun(@(lam,th)cos(lam).*sin(th)/4);
pass(1) = norm(g-exact) < tol;

% Example 2
f = ballfun(@(r,lam,th)1, 'spherical');
g = sum(f, 1);
exact = spherefun(@(lam,th)1/3);
pass(2) = norm(g-exact) < tol;

%% Integrate over lambda

% Example 3
f = ballfun(@(r,lam,th)(r.*sin(lam).*sin(th)).^2, 'spherical');
g = sum(f, 2);
exact = diskfun(@(th,r)pi*r.^2.*sin(th).^2,'polar');
pass(3) = norm(g-exact) < tol;

%% Integrate over theta

% Example 4
f = ballfun(@(r,lam,th)r.*cos(lam).*sin(th), 'spherical');
g = sum(f, 3);
exact = diskfun(@(lam,r)r.*cos(lam)*pi/2,'polar');
pass(4) = norm(g-exact) < tol;

if (nargout > 0)
    pass = all(pass(:));
end
end


function [lp,dlp] = priorGaussMulti(mu,s2,x)

% Multivariate Gaussian hyperparameter prior distribution.
% Compute log-likelihood and its derivative or draw a random sample.
% The prior distribution is parameterized as:
%
%   p(x) = exp(-r2/2) / sqrt(det(2*pi*s2)) where r2(x) = (x-mu)'*inv(s2)*(x-mu),
%
% mu(Dx1) is the mean parameter, s2(Dx1) or s2(DxD) is the variance parameter
% and x(DxN) contains query hyperparameters for prior evaluation.
%
% For more help on design of priors, try "help priorDistributions".
%
% Copyright (c) by Roman Garnett and Hannes Nickisch, 2014-09-12.
%
% See also PRIORDISTRIBUTIONS.M, PRIORGAUSS.M.

if nargin<2, error('mu and s2 parameters need to be provided'), end
if ndims(mu)~=2 || size(mu,2)~=1, error('mu needs to be (Dx1)'), end
D = size(mu,1);
s2_ok = ndims(s2)==2 && all(size(s2)==[D,1] | size(s2)==[D,D]);
if ~s2_ok, error('s2 needs to be (DxD) or (Dx1)'), end
if size(s2,2)==D                                        % full multivariate case
  s = chol(s2)'; lds = sum(log(diag(s)));                 % lds = log(det(s2))/2
else                                                       % diagonal covariance
  s = sqrt(s2);  lds = sum(log(s));
end
if nargin<3                                             % return a random sample
  lp = randn(D,1);                                                 % unit sample
  if size(s,2)==D, lp = s*lp+mu; else lp = s.*lp+mu; end % affine transformation
  return
end
if D==1 && size(x,1)>1                            % both mu/s2 scalar => inflate
  D = size(x,1); mu = mu*ones(D,1); s = s*ones(D,1); lds = D*lds;
end
if ~(ndims(x)==2 && size(x,1)==D), error('x needs to be (Dxn)'), end

oN = ones(1,size(x,2));
if size(s,2)==D
  xs = s\(x-mu*oN);       dlp = -s'\xs;
else
  xs = (x-mu*oN)./(s*oN); dlp = -xs./(s*oN);
end
lp = -sum(xs.^2,1)/2 - D*log(2*pi)/2 - lds;


function [Results,u_star2,t]=Arm_1DOF_LinearStateTransition_Fun(StartAngle,Stable,I,l_c)
% this suggests an initial solution for the pendulium problem
%Stable 1 hanging pendulium, -1 inverted
%StartAngle given in degrees

%system parameters
m=1; %mass of plumb, kg
g=9.806; %gravity, m/sec^2
% I and l_c are now set by input

% simumation parameters
X_0=[StartAngle(1),0].'*pi/180; %intial value
X_F=[0,0].'*pi/180; %final value
t0=0;
tf=1;
T=linspace(t0,tf,1000);
T_2=[T, T(end)+(0.0001:0.01:0.1)];

% I*theta_ddot = -m*g*l_c*sin(theta)+u
% regular pendulium
% linearizing about theta=0 for now
A = [0 1; -Stable*m*g*l_c/I, 0];
B=[0; 1/I];

% W_c_fun=@(t) expm(A*t(n))*B*(B.')*expm(A.'*t(n));
% Written as subroutine below
temp11= quad(@(timein) W_c_fun(timein,A,B,1,1),t0,tf);
temp21= quad(@(timein) W_c_fun(timein,A,B,2,1),t0,tf);
temp12= quad(@(timein) W_c_fun(timein,A,B,1,2),t0,tf);
temp22= quad(@(timein) W_c_fun(timein,A,B,2,2),t0,tf);
W_c= [temp11, temp12; temp21, temp22];

u_pre=-B.';
u_post=(W_c\(expm(tf*A)*X_0-X_F));
u_star=zeros(size(T_2));
for alice=1:length(T)
u_star(alice)=u_pre*expm(A.'*(tf-T(alice)))*u_post;
end

Dynamics=@(t,x) [x(2); -Stable*(m*g*l_c/I)*sin(x(1))+...
    (sign(interp1(T_2,u_star,t,'pchip'))*min([abs(interp1(T_2,u_star,t,'pchip')),7]))/I];
[t,Results]=ode45(Dynamics,T_2,X_0);
% Apply saturation
u_star2=sign(interp1(T_2,u_star,t,'pchip')).*...
    min(abs(interp1(T_2,u_star,t,'pchip')),7);
end

function temp = W_c_fun(t,A,B,WhichRow,WhichCol)
temp=zeros([1,length(t)]);
for n=1:length(t)
    temp1=expm(A*t(n))*B*(B.')*expm(A.'*t(n));
    temp(n)=temp1(WhichRow,WhichCol);
end
end


function varargout = sumk(varargin)
% SUMK  Returns sum of k largest (eigen-)values.
%
% s = SUMK(X,k,w)
%
% For a vector X, SUMK returns the sum of the k largest elements.
%
% For a symmetric matrix X, SUMK returns the sum of the k largest eigen-values.
%
% A third argument can be used to generalize to weighted sum.
% The weights have to be non-negative and non-increasing.
%
% See also SUMABSK

if nargin == 1
    error('sumk needs at least two arguments');
end

switch class(varargin{1})

    case 'double' % What is the numerical value of this argument (needed for displays etc)

        X = varargin{1};
        [n,m] = size(X);
        if (min(n,m) > 1 & ~issymmetric(X))
            error('sumk can only be applied on vectors and symmetric matrices');
        else
            k = min(length(X),varargin{2});
            w = ones(k,1);
            if nargin == 3
                w = varargin{3};
                if length(w)==1
                    w = ones(k,1)*w;
                end
            end
            if min(n,m)==1
                sorted = sort(X,'descend');
            else
                sorted = sort(eig(X),'descend');
            end
            sorted = sorted(:);
            w = w(:);
            varargout{1} = sum(sorted(1:k).*w(1:k));
        end

    case 'sdpvar' % Overloaded operator for SDPVAR objects. Pass on args and save them.
        X = varargin{1};
        [n,m] = size(X);
        if nargin < 3
            w = 1;
        else
            w = varargin{3};
            if length(w)>1
                if any(diff(w)>0) || any(w<0)
                    error('The weights have to be non-negative non-increasing')
                end
            end
        end
        if (min(n,m) > 1 & ~issymmetric(X))
            error('sumk can only be applied on vectors and symmetric matrices');
        else
            varargout{1} = yalmip('define',mfilename,varargin{:});
        end

    case 'char' % YALMIP send 'model' when it wants the epigraph or hypograph
        if isequal(varargin{1},'graph')
            t = varargin{2}; % Second arg is the extended operator variable
            X = varargin{3}; % Third arg and above are the args user used when defining t.
            k = min(varargin{4},length(X));
            if nargin == 5
                w = varargin{5};
            else
                w = 1;
            end
            [Model,Properties] = sumk_generator(X,k,t,w);
            varargout{1} = Model;
            varargout{2} = Properties;
            varargout{3} = X;
        else
        end
    otherwise
end


function M = makeSincMovie

%   Copyright 2008-2009 The MathWorks, Inc.

% Make suitable X and Y matricies to plot the sinc function over
[X, Y] = meshgrid(linspace(-3*pi, 3*pi, 50), linspace(-3*pi, 3*pi, 50));
% From X & Y make the matrix R of radius
R = sqrt(X.^2 + Y.^2);
% Calculate the sinc function from R
F = sin(R)./R;
% Make an amplitude vector that represents the various amplitudes we will
% movie the function over
A = linspace(-1, 1, 30);
% Iterate over each amplitude
for j = 1:numel(A)
    % Plot the surface with colouring defined by the function with no
    % amplitude modification
    surf(X, Y, A(j)*F, F);
    % Always use the same axis otherwise the movie will look funny
    axis([-10 10 -10 10 -1 1]);
    % Then turn off the axis - don't want tosee them in the movie. Also
    % remove the lines around each face of the surface
    axis off
    shading flat
    % Finally get this frame and the symmetric one so that when we show the
    % whole movie it ends at the same point it began
    M(j) = getframe; %#ok<AGROW>
    M(2*numel(A) - j) = M(j); %#ok<AGROW>
end


function Ch4Ex1_main()
%% SOS-based Policy Iteration for a car suspension systems
% The function is tested in MATLAB R2014b
% Copyright 2015 Yu Jiang
% Contact Yu Jiang (yu.jiang@nyu.edu)

% System requirements:
% - MATLAB (Manually Tested in MATLAB R2014b)
% - MATLAB Symbolic Toolbox
% - SDPT3-4.0
% - SISOTOOLS (free to download at http://www.cds.caltech.edu/sostools/)

% You can download tools.zip and run setuptools.m in the folder.

syms x1 x2 x3 x4 real
mb = 300;    % kg
mw = 60;     % kg
bs = 1000;   % N/m/s
ks = 16000 ; % N/m
kt = 190000; % N/m
kn = 0.1*ks;

% State matrices
A = [ 0 1 0 0;
    [-ks -bs ks bs]/mb ; ...
    0 0 0 1;
    [ks bs -ks-kt -bs]/mw];
B = [ 0 0; 0 10000/mb ; 0 0 ; [kt -10000]/mw];
B = B(:,2);

% LQR feedback gains for the linearized system
% Klqr = lqr(A,B,eye(4),1);
% f(x)
f = A*[x1;x2;x3;x4] + [0;-kn*(x1-x3)^3/mb;0;kn*(x1-x3)^3/mw];

% Polynomial Weighting functions
q0 = 100*x1^2+x2^2+x3^2+x4^2;
rx = 1;%+(x1^2+x2^2+x3^2+x4^2);
vars = [x1;x2;x3;x4];

% Initialize the SOSp
prog = sosprogram(vars);

% The Lyapunov function V(x)
[prog,V] = sospolyvar(prog,monomials([x1;x2;x3;x4],2:4),'wscoeff');

% Objective of the SOSp
myObj = int(int(int(int(V,-.5,.5),-10,10),-.5,.5),-10,10);

% Add Inequality constraint to assure V is positive definite
prog = sosineq(prog,V-0.0001*(x1^2+x2^2+x3^2+x4^2));

% Add Inequality constraint to assure stability and performance
expr = -[diff(V,x1) diff(V,x2) diff(V,x3) diff(V,x4)]*rx*f-rx*q0;
prog = sosineq(prog,expr);

% Solve the SOSp
prog = sossolve(prog);

% Obtain the Initial Lyapunov function
V0 = sosgetsol(prog,V);
V_old = V0;

% Initializing the old contol policy
u_prev = zeros(size(x1));

% Iteration
for i=1:10
    clear prog V
    %------------------------------ SOSp Start ----------------------------
    prog = sosprogram(vars);
    [prog,V] = sospolyvar(prog,monomials([x1;x2;x3;x4],2:4),'wscoeff');
    prog = sosineq(prog,V_old - V);
    prog = sosineq(prog, V);
    u = -1/2*B'*[diff(V_old,x1) diff(V_old,x2) diff(V_old,x3) diff(V_old,x4)].';
    qfcn =rx*q0 + u'*u;
    expr = -[diff(V,x1) diff(V,x2) diff(V,x3) diff(V,x4)]*(rx*f+B*u)-qfcn;
    prog = sosineq(prog,expr);
    prog = sossetobj(prog, myObj);
    prog = sossolve(prog);
    %------------------------------ SOSp End ------------------------------
    V_ = sosgetsol(prog,V);
    V_old = V_;
end

% Save the improved value function
Vnew = V_;

%% Post-processing results
x0 = [0,0,0,0];  % Iniial Condition
tIntv = [0 3];
[t1,y1] = ode23s(@(t,x) LocalSuspSys(t,x,u), [0 3], x0);
[t,y] = ode23s(@(t,x) LocalSuspSys(t,x,0), tIntv, x0);

%% Plot Results
figure(1)
subplot(411);
plot(t1,y1(:,1),t,y(:,1), 'r:','linewidth',2);
xlabel('time (sec)', 'FontSize',12)
ylabel('x_1','FontSize',12)
hl = legend('Improved performance', 'Uncontrolled performance');
set(hl, 'FontSize', 12');
subplot(412);
plot(t1,y1(:,2),t,y(:,2),'r:','linewidth',2);
xlabel('time (sec)', 'FontSize',12)
ylabel('x_2','FontSize',12)
hl = legend('Improved performance', 'Uncontrolled performance');
set(hl, 'FontSize', 12');
subplot(413);
plot(t1,y1(:,3),t,y(:,3), 'r:','linewidth',2);
xlabel('time (sec)', 'FontSize',12)
ylabel('x_3','FontSize',12)
hl = legend('Improved performance', 'Uncontrolled performance');
set(hl, 'FontSize', 12');
subplot(414);
plot(t1,y1(:,4),t,y(:,4),'r:','linewidth',2);
xlabel('time (sec)', 'FontSize',12)
ylabel('x_4','FontSize',12)
hl = legend('Improved performance', 'Uncontrolled performance');
set(hl, 'FontSize', 12');

figure(2)
xx1 = -.4:.04:.4;
xx2 = -5:0.5:5;
vn = zeros(length(xx1),length(xx2));
v1 = zeros(length(xx1),length(xx2));
un=vn;
ulqr = un;
kn=vn;
k1=v1;
x3=0;
x4=0;
for i=1:length(xx1)
    x1 = xx1(i);
    for j=1:length(xx2)
        x2 = xx2(j);
        vn(i,j)=eval(Vnew);
        v1(i,j)=eval(V0);
    end
end
surf(xx1,xx2,vn')
hold on
surf(xx1,xx2,v1')
hold off
xlabel('x_1', 'FontSize', 12)
ylabel('x_2', 'FontSize', 12)
view(gca,[-30.5 28]);
% Create textarrow
annotation(gcf,'textarrow',[0.210714285714286 0.174535137214669],...
    [0.895238095238095 0.631440045897884],'TextEdgeColor','none','FontSize',12,...
    'String',{'V_0(x1,x2,0,0)'});

% Create textarrow
annotation(gcf,'textarrow',[0.139285714285714 0.186735060271868],...
    [0.183333333333333 0.386454388984516],'TextEdgeColor','none','FontSize',12,...
    'String',{'V_{10}(x1,x2,0,0)'});
% export_fig Ex3_cost -pdf -transparent

end

%% LocalSuspSys
% Dynamics of the nonlinear suspension system
function dx = LocalSuspSys(t,x,u)
mb = 300;    % kg
mw = 60;     % kg
bs = 1000;   % N/m/s
ks = 16000 ; % N/m
kt = 190000; % N/m

[x1,x2,x3,x4] = deal(x(1),x(2),x(3),x(4));

% State matrices
A = [ 0 1 0 0;
    [-ks -bs ks bs]/mb ; ...
    0 0 0 1;
    [ks bs -ks-kt -bs]/mw];
B = [ 0; 10000/mb ; 0 ; -10000/mw];
B1 = [ 0; 0 ; 0 ; kt/mw];

if ~isdouble(u)
    u = eval(u);
end

if t <= 0.001
    r = 10;
else
    r = 0;
end

dx = A*x + B*u + B1*r;
end


function x=t2x(T,str)

% x=t2x(T,str);
%
% Converts transformation matrix T between B and A coordinate
% frames into a generalized position vector x, which contains
% position and orientation vectors of B with respect to A.
% Orientation can be expressed with quaternions, euler angles
% (xyz or zxz convention), unit vector and rotation angle.
% Also both orientation and position can be expressed with
% Denavitt-Hartemberg parameters.
%
% ---------------------------------------------------------------------------
%
% The transformation matrix T between B and A coordinate
% frames is a 4 by 4 matrix such that:
% T(1:3,1:3) = Orientation matrix between B and A = unit vectors
%              of x,y,z axes of B expressed in the A coordinates.
% T(1:3,4)   = Origin of B expressed in A coordinates.
% T(4,1:3)   = zeros(1,3)
% T(4,4)     = 1
%
% ---------------------------------------------------------------------------
%
% The generalized position vector x contains the origin of B
% expressed in the A coordinates in the first four entries,
% and orientation of B with respect to A in the last four entries.
% In more detail, its shape depends on the value of str as
% specified below :
%
% ---------------------------------------------------------------------------
%
% str='van' : UNIT VECTOR AND ROTATION ANGLE
%
%           [ Ox ]     origin of the B coordinate frame
%  x(1:4) = [ Oy ]     with respect to A.
%           [ Oz ]
%           [  1 ]
%
%           [ Vx ]     Vx,Vy,Vz = unit vector respect to A,
%  x(5:8) = [ Vy ]     which B is rotated about.
%           [ Vz ]
%           [ Th ]     Th = angle which B is rotated (-pi,pi].
% ---------------------------------------------------------------------------
%
% str='qua' : UNIT QUATERNION
%
%           [ Ox ]     origin of the B coordinate frame
%  x(1:4) = [ Oy ]     with respect to A.
%           [ Oz ]
%           [  1 ]
%
%           [ q1 ]     q1,q2,q3 = V*sin(Th/2)
%  x(5:8) = [ q2 ]     q0 = cos(Th/2) where :
%           [ q3 ]     V = unit vector respect to A, which B is
%           [ q0 ]     rotated about, Th = angle which B is rotated (-pi,pi].
% ---------------------------------------------------------------------------
%
% str='erp' : EULER-RODRIGUEZ PARAMETERS
%
%           [ Ox ]     origin of the B coordinate frame
%  x(1:4) = [ Oy ]     with respect to A.
%           [ Oz ]
%           [  1 ]
%
%           [ r1 ]     r1,r2,r3 = V*tan(Th/2), where :
%  x(5:8) = [ r2 ]     V = unit vector with respect to A, which B is
%           [ r3 ]     rotated about.
%           [  0 ]     Th = angle which B is rotated (-pi,pi) (<> pi).
% ---------------------------------------------------------------------------
%
% str='rpy' : ROLL, PITCH, YAW ANGLES (euler x-y-z convention)
%
%           [ Ox ]     origin of the B coordinate frame
%  x(1:4) = [ Oy ]     with respect to A.
%           [ Oz ]
%           [  1 ]
%
%            [ r ]     r = roll angle  ( fi    (-pi,pi], about x,          )
%  x(5:8) =  [ p ]     p = pitch angle ( theta (-pi,pi], about y, <> +-pi/2)
%            [ y ]     y = yaw angle   ( psi   (-pi,pi], about z,          )
%            [ 0 ]
% ---------------------------------------------------------------------------
%
% str='rpm' : ROTATION, PRECESSION, MUTATION ANGLES (euler z-x-z convention)
%
%           [ Ox ]     origin of the B coordinate frame
%  x(1:4) = [ Oy ]     with respect to A.
%           [ Oz ]
%           [  1 ]
%
%            [ r ]     r = rotation angle   ( (-pi,pi] ,about z           )
%  x(5:8) =  [ p ]     p = precession angle ( (-pi,pi] ,about x , <> 0,pi )
%            [ y ]     y = mutation angle   ( (-pi,pi] ,about z           )
%            [ 0 ]
% ---------------------------------------------------------------------------
%
% str='dht' : DENAVITT-HARTEMBERG PARAMETERS
%
%            [ b ]                [ a ]     this four-parameter
%  x(1:4) =  [ d ] ,   x(5:8) =   [ t ] ,   description does not involve
%            [ 0 ]                [ 0 ]     a loss of information if and
%            [ 0 ]                [ 0 ]     only if T has this shape:
%
%           [    ct   -st     0     b ]     where :
%  T =      [ ca*st ca*ct   -sa -d*sa ]
%           [ sa*st sa*ct    ca  d*ca ]     sa = sin(a), ca = cos(a)
%           [     0     0     0     1 ]     st = sin(t), ct = cos(t)
% ---------------------------------------------------------------------------
%
% Example (see also x2t):
% x=[rand(3,1);1;rand(3,1);0];x-t2x(x2t(x,'rpm'),'rpm')
%
%
% Giampiero Campa 1/11/96
%

% ---------------------------------------------------------------------------
% UNIT VECTOR AND ROTATION ANGLE

if [ str=='van' size(T)==[4 4] ],

O=T(1:3,4);
R=T(1:3,1:3);
d=round(.5*(trace(R)-1)*1e12)/1e12;

if d==1,
   v=[0 0 1]';
   th=0;

elseif d==-1,
    v0=sum(R'+eye(3,3))';

    if v0 == 0
	 v0=R(:,2)+R(:,3)+[0 1 1]';
    end;

    if v0 == 0
	 v0=R(:,3)+[0 0 1]';
    end;

    v=v0/norm(v0);
    th=pi;

else

   sg=(vp([1 0 0]',R(:,1))+vp([0 1 0]',R(:,2))+vp([0 0 1]',R(:,3)));

    if norm(sg) < 1e-12
        disp(' ');
        disp('T2x warning: det(R)<>1, unit vector assumed to be [0 0 1]''.');
        disp(' ');
        sg=[0 0 1]';
    end

   v=sg/norm(sg);
   th=atan2(norm(sg)/2,d);

end

x=[O;1;v;th];

% ---------------------------------------------------------------------------
% UNIT QUATERNION

elseif [ str=='qua' size(T)==[4 4] ],

O=T(1:3,4);
R=T(1:3,1:3);
d=round(.5*(trace(R)-1)*1e12)/1e12;

if d==1,
   v=[0 0 1]';
   th=0;

elseif d==-1,
    v0=sum(R'+eye(3,3))';

    if v0 == 0
	v0=R(:,2)+R(:,3)+[0 1 1]';
    end;

    if v0 == 0
	v0=R(:,3)+[0 0 1]';
    end;

    v=v0/norm(v0);
    th=pi;

else

   sg=(vp([1 0 0]',R(:,1))+vp([0 1 0]',R(:,2))+vp([0 0 1]',R(:,3)));

    if norm(sg) < 1e-12
        disp(' ');
        disp('T2x warning: det(R)<>1, unit vector assumed to be [0 0 1]''.');
        disp(' ');
        sg=[0 0 1]';
    end

   v=sg/norm(sg);
   th=atan2(norm(sg)/2,d);

end

q=v*sin(th/2);
q0=cos(th/2);

x=[O;1;q;q0];

% ---------------------------------------------------------------------------
% EULER-RODRIGUEZ PARAMETERS

elseif [ str=='erp' size(T)==[4 4] ],

O=T(1:3,4);
R=T(1:3,1:3);
d=round(.5*(trace(R)-1)*1e12)/1e12;

if d==1,
   v=[0 0 1]';
   th=0;

elseif d==-1,
    v0=sum(R'+eye(3,3))';

    if v0 == 0
	v0=R(:,2)+R(:,3)+[0 1 1]';
    end;

    if v0 == 0
	v0=R(:,3)+[0 0 1]';
    end;

    v=v0/norm(v0);
    th=pi;

else

   sg=(vp([1 0 0]',R(:,1))+vp([0 1 0]',R(:,2))+vp([0 0 1]',R(:,3)));

    if norm(sg) < 1e-12
        disp(' ');
        disp('T2x warning: det(R)<>1, unit vector assumed to be [0 0 1]''.');
        disp(' ');
        sg=[0 0 1]';
    end

   v=sg/norm(sg);
   th=atan2(norm(sg)/2,d);

end

p=v*tan(th/2);
x=[O;1;p;0];

% ---------------------------------------------------------------------------
% ROLL, PITCH, YAW ANGLES (euler x-y-z convention)

elseif [ str=='rpy' size(T)==[4 4] ],

O=T(1:3,4);
R=T(1:3,1:3);
d=round([0 0 1]*R(:,1)*1e12)/1e12;

if d==1,
   y=atan2([0 1 0]*R(:,2),[1 0 0]*R(:,2));
   p=-pi/2;
   r=-pi/2;

elseif d==-1
   y=atan2([0 1 0]*R(:,2),[1 0 0]*R(:,2));
   p=pi/2;
   r=pi/2;

else
   sg=vp([0 0 1]',R(:,1));
   j2=sg/sqrt(sg'*sg);
   k2=vp(R(:,1),j2);

   r=atan2(k2'*R(:,2),j2'*R(:,2));
   p=atan2(-[0 0 1]*R(:,1),[0 0 1]*k2);
   y=atan2(-[1 0 0]*j2,[0 1 0]*j2);
end

y1=y+(1-sign(y)-sign(y)^2)*pi;
p1=p+(1-sign(p)-sign(p)^2)*pi;
r1=r+(1-sign(r)-sign(r)^2)*pi;

% takes smaller values of angles

if norm([y1 p1 r1]) < norm([y p r])
    x=[O;1;r1;-p1;y1;0];
else
    x=[O;1;r;p;y;0];
end

% ---------------------------------------------------------------------------
% ROTATION, PRECESSION, MUTATION ANGLES (euler z-x-z convention)

elseif [ str=='rpm' size(T)==[4 4] ],

O=T(1:3,4);
R=T(1:3,1:3);
d=round([0 0 1]*R(:,3)*1e12)/1e12;

if d==1,
   m=0;
   p=0;
   r=atan2([0 1 0]*R(:,1),[1 0 0]*R(:,1));

elseif d==-1
   m=0;
   p=pi;
   r=atan2([0 1 0]*R(:,1),[1 0 0]*R(:,1));

else
   sg=vp([0 0 1]',R(:,3));
   i2=sg/norm(sg);
   j2=vp(R(:,3),i2);

   m=atan2(j2'*R(:,1),i2'*R(:,1));
   p=atan2([0 0 1]*j2,[0 0 1]*R(:,3));
   r=atan2([0 1 0]*i2,[1 0 0]*i2);

end

r1=r+(1-sign(r)-sign(r)^2)*pi;
p1=p;
m1=m+(1-sign(m)-sign(m)^2)*pi;

% takes the smaller values of angles

if norm([r1 p1 m1]) < norm([r p m])
    x=[O;1;r1;-p1;m1;0];
else
    x=[O;1;r;p;m;0];
end

% ---------------------------------------------------------------------------
% DENAVITT-HARTEMBERG PARAMETERS

elseif [ str=='dht' size(T)==[4 4] ],

% b calculation
b=T(1,4);

% d calculation
if norm(T(3,3)) > norm(T(2,3))
    d=T(3,4)/T(3,3);
else
    d=T(2,4)/T(2,3);
end

% alfa and theta computation by mean of euler-zxz angles
R=T(1:3,1:3);
dl=round([0 0 1]*R(:,3)*1e12)/1e12;

if dl==1,
   th=0;
   alfa=0;
   n=atan2([0 1 0]*R(:,1),[1 0 0]*R(:,1));
elseif dl==-1
   th=0;
   alfa=pi;
   n=atan2([0 1 0]*R(:,1),[1 0 0]*R(:,1));
else
   sg=vp([0 0 1]',R(:,3));
   i2=sg/norm(sg);
   j2=vp(R(:,3),i2);

   th=atan2(j2'*R(:,1),i2'*R(:,1));
   alfa=atan2([0 0 1]*j2,[0 0 1]*R(:,3));
   n=atan2([0 1 0]*i2,[1 0 0]*i2);
end

th1=th+(1-sign(th)-sign(th)^2)*pi;
alfa1=alfa;
n1=n+(1-sign(n)-sign(n)^2)*pi;

% takes the smaller values of angles

if norm([th1 alfa1 n1]) < norm([th alfa n])
   x=[b;d;0;0;-alfa1;th1;0;0];
else
   x=[b;d;0;0;alfa;th;0;0];
end

% ---------------------------------------------------------------------------
% OTHER STRING

else

disp('   ');
disp('   x=T2x(T,str)');
disp('   where T is a 4 by 4 matrix (see help for details)');
disp('   and str can be : ''van'',''qua'',''erp'',''rpy'',''rpm'',''dht''. ');
disp('   ');

end

function z=vp(x,y)

% z=vp(x,y); z = 3d cross product of x and y
% vp(x) is the 3d cross product matrix : vp(x)*y=vp(x,y).
%
% by Giampiero Campa.

z=[  0    -x(3)   x(2);
    x(3)    0    -x(1);
   -x(2)   x(1)    0   ];

if nargin>1, z=z*y; end


% GETMSWTFEAT Gets the Multiscale Wavelet Transform features, these
% include: Energy, Variance, Standard Deviation, and Waveform Length
% feat = getmswtfeat(x,winsize,wininc,SF)
% ------------------------------------------------------------------
% The signals in x are divided into multiple windows of size
% "winsize" and the windows are spaced "wininc" apart.
% Inputs
% ------
%    x: 		columns of signals
%    winsize:	window size (length of x)
%    wininc:	spacing of the windows (winsize)
%    SF:        sampling frequency (Not used in the current implementation, but I left you some options down there)
% Outputs
% -------
%    feat:     WT features organized as [Energy, Variance, Waveform Length, Entropy]

% Example
% -------
% feat = getmswtfeat(rand(1024,1),128,32,32)
% Assuming here rand(1024,1) (this can be any one dimensional signal,
% for example EEG or EMG) is a one dimensional signal sampled at 32
% for 32 seconds only. Utilizing a window size of 128 at 32 increments,
% features are extracted from the wavelet tree.
% I assumed 10 decomposition levels (J=10) below in the code.
% For a full tree at 10 levels you should get 11 features
% as we have decided to extract 4 types of features then we get 11 x 4 =44
% features.
% =========================================================================
% Multiscale Wavelet Transform feature extraction code by Dr. Rami Khushaba
% Research Fellow - Faculty of Engineering and IT
% University of Technology, Sydney.
% Email: Rami.Khushaba@uts.edu.au
% URL: www.rami-khushaba.com (Matlab Code Section)
% last modified 29/08/2012
% last modified 09/02/2013

function feat = getmswtfeat(x,winsize,wininc,SF)

if nargin < 4
    if nargin < 3
        if nargin < 2
            winsize = size(x,1);
        end
        wininc = winsize;
    end
    error('Please provide the sampling frequency of this signal')
end

datawin = ones(winsize,1);
datasize = size(x,1);
Nsignals = size(x,2);

%% Chop the signal according to a sliding window approach
numwin = floor((datasize - winsize)/wininc)+1;
% allocate memory
feat = zeros(winsize,numwin);
st = 1;
en = winsize;
for i = 1:numwin
    curwin = x(st:en,:).*repmat(datawin,1,Nsignals);
    feat(1:winsize,i) = detrend(curwin);

    st = st + wininc;
    en = en + wininc;
end
%% ---------------- Various options for J -----------------------
% Note I put SF above in the inputs because you can use SF to determine the
% best decompisition level J, however for simplicity here I put it J=10;
J=10;% Number of decomposition levels which can also be set using
% or J=wmaxlev(winsize,'Sym5');
% or J=(log(SF/2)/log(2))-1;
%% Multisignal one-dimensional wavelet transform decomposition
dec = mdwtdec('col',feat,J,'db4');
% Proceed with Multisignal 1-D decomposition energy distribution

if isequal(dec.dirDec,'c')
    dim = 1;
end
[cfs,longs] = wdec2cl(dec,'all');
level = length(longs)-2;

if dim==1
    cfs = cfs';
    longs = longs';
end
numOfSIGs  = size(cfs,1);
num_CFS_TOT = size(cfs,2);
absCFS   = abs(cfs);
absCFS0   = (cfs);
cfs_POW2 = absCFS.^2;
Energy  = sum(cfs_POW2,2);
percentENER = 0*ones(size(cfs_POW2));
notZER = (Energy>0);
percentENER(notZER,:) = 100*cfs_POW2(notZER,:)./Energy(notZER,ones(1,num_CFS_TOT));

%% or try this version below and tell us which  one is the best on your data
% percentENER(notZER,:) = cfs_POW2(notZER,:);

%% Pre-define and allocate memory
tab_ENER = zeros(numOfSIGs,level+1);
tab_VAR = zeros(numOfSIGs,level+1);
% tab_STD = zeros(numOfSIGs,level+1);
tab_WL = zeros(numOfSIGs,level+1);
tab_entropy = zeros(numOfSIGs,level+1);

%% Feature extraction section
st = 1;
for k=1:level+1
    nbCFS = longs(k);
    en  = st+nbCFS-1;
    tab_ENER(:,k) = mean(percentENER(:,st:en),2);%.*sum(abs(diff(absCFS0(:,st:en)')'),2); % energy per waveform length
    tab_VAR(:,k) = var(percentENER(:,st:en),0,2); % variance of coefficients
    %     tab_STD(:,k) = std(percentENER(:,st:en),[],2); % standard deviation of coefficients
    tab_WL(:,k) = sum(abs(diff(percentENER(:,st:en)').^2))'; % waveform length
    percentENER(:,st:en) = percentENER(:,st:en)./repmat(sum(percentENER(:,st:en),2),1,size(percentENER(:,st:en),2));
    tab_entropy(:,k) = -sum(percentENER(:,st:en).*log(percentENER(:,st:en)),2)./size(percentENER(:,st:en),2);
    st = en + 1;
end
feat =([log1p([tab_ENER tab_VAR  tab_WL]) tab_entropy]);


function prob_test1341 ( )

%*****************************************************************************80
%
%% TEST01341 checks RIBESL against BESSEL_IX_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST1341:\n' );
  fprintf ( 1, '  RIBESL computes values of Bessel functions\n' );
  fprintf ( 1, '  of NONINTEGER order.\n' );
  fprintf ( 1, '  BESSEL_IX_VALUES returns selected values of the\n' );
  fprintf ( 1, '  Bessel function In for NONINTEGER order.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '      ALPHA         X             FX                        FX2\n' );
  fprintf ( 1, ...
    '                                  (table)                   (RIBESL)\n' );
  fprintf ( 1, '\n' );
  n_data = 0;

  while ( 1 )

    [ n_data, alpha, x, fx ] = bessel_ix_values ( n_data );

    if ( n_data == 0 );
      break
    end

    ize = 1;
    nb = floor ( alpha ) + 1;
    alpha_frac = alpha - floor ( alpha );

    [ b, ncalc ] = ribesl ( x, alpha_frac, nb, ize );
    fx2 = b(nb);

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

  end

  return
end


%  Script file: doy.m
%
%  Purpose:
%    This program calculates the day of year corresponding
%    to a specified date.  It illustrates the use switch
%    and for constructs.
%
%  Record of revisions:
%      Date       Programmer          Description of change
%      ====       ==========          =====================
%    01/27/07    S. J. Chapman        Original code
%
% Define variables:
%   day          -- Day (dd)
%   day_of_year  -- Day of year
%   ii           -- Loop index
%   leap_day     -- Extra day for leap year
%   month        -- Month (mm)
%   year         -- Year (yyyy)

% Get day, month, and year to convert
disp('This program calculates the day of year given the ');
disp('specified date.');
month = input('Enter specified month (1-12): ');
day   = input('Enter specified day(1-31):    ');
year  = input('Enter specified year(yyyy):   ');

% Check for leap year, and add extra day if necessary
if mod(year,400) == 0
   leap_day = 1;          % Years divisible by 400 are leap years
elseif mod(year,100) == 0
   leap_day = 0;          % Other centuries are not leap years
elseif mod(year,4) == 0
   leap_day = 1;          % Otherwise every 4th year is a leap year
else
   leap_day = 0;          % Other years are not leap years
end

% Calculate day of year by adding current day to the
% days in previous months.
day_of_year = day;
for ii = 1:month-1

   % Add days in months from January to last month
   switch (ii)
   case {1,3,5,7,8,10,12},
      day_of_year = day_of_year + 31;
   case {4,6,9,11},
      day_of_year = day_of_year + 30;
   case 2,
      day_of_year = day_of_year + 28 + leap_day;
   end

end

% Tell user
fprintf('The date %2d/%2d/%4d is day of year %d.\n', ...
         month, day, year, day_of_year);


function centroid = polyhedronCentroid(vertices, faces) %#ok<INUSD>
%POLYHEDRONCENTROID Compute the centroid of a 3D convex polyhedron.
%
%   CENTRO = polyhedronCentroid(V, F)
%   Computes the centroid (center of mass) of the polyhedron defined by
%   vertices V and faces F.
%   The polyhedron is assumed to be convex.
%
%   Example
%     % Creates a polyhedron centered on origin, and add an arbitrary
%     % translation
%     [v, f] = createDodecahedron;
%     v2 = bsxfun(@plus, v, [3 4 5]);
%     % computes the centroid, that should equal the translation vector
%     centroid = polyhedronCentroid(v2, f)
%     centroid =
%         3.0000    4.0000    5.0000
%
%
%   See also
%   meshes3d, meshVolume, meshSurfaceArea, polyhedronMeanBreadth
%

% ------
% Author: David Legland
% E-mail: david.legland@nantes.inra.fr
% Created: 2012-04-05, using Matlab 7.9.0.529 (R2009b)
% Copyright 2012-2022 INRA - Cepia Software Platform

% compute set of elementary tetrahedra
DT = delaunayTriangulation(vertices);
T = DT.ConnectivityList;

% number of tetrahedra
nT  = size(T, 1);

% initialize result
centroid = zeros(1, 3);
vt = 0;

% Compute the centroid and the volume of each tetrahedron
for i = 1:nT
    % coordinates of tetrahedron vertices
    tetra = vertices(T(i, :), :);

    % centroid is the average of vertices.
    centi = mean(tetra);

    % compute volume of tetrahedron
    vol = det(tetra(1:3,:) - tetra([4 4 4],:)) / 6;

    % add weighted centroid of current tetraedron
    centroid = centroid + centi * vol;

    % compute the sum of tetraedra volumes
    vt = vt + vol;
end

% compute by sum of tetrahedron volumes
centroid = centroid / vt;


function linplus_test035 ( )

%*****************************************************************************80
%
%% TEST035 tests R83_GS_SL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  n = 100;
  maxit = 1000;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST035\n' );
  fprintf ( 1, '  For a real tridiagonal system,\n' );
  fprintf ( 1, '  R83_GS_SL solves a linear system using\n' );
  fprintf ( 1, '    Gauss-Seidel iteration\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N = %d\n', n );
  fprintf ( 1, '  Iterations per call = %d\n', maxit );
  fprintf ( 1, '\n' );
%
%  Set the matrix values.
%
  a(1,1)     =  0.0E+00;
  a(1,2:n)   = -1.0E+00;
  a(2,1:n)   =  2.0E+00;
  a(3,1:n-1) = -1.0E+00;
  a(3,n)     =  0.0E+00;

  for job = 0 : 1

    if ( job == 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, '  Solving A * x = b.\n' );
      fprintf ( 1, '\n' );
    else
      fprintf ( 1, '\n' );
      fprintf ( 1, '  Solving A'' * x = b.\n' );
      fprintf ( 1, '\n' );
    end
%
%  Set the desired solution.
%
    x = r8vec_indicator ( n );
%
%  Compute the corresponding right hand side.
%
    if ( job == 0 )
      b = r83_mxv ( n, a, x );
    else
      b = r83_vxm ( n, a, x );
    end
%
%  Set the starting solution.
%
    x(1:n) = 0.0E+00;
%
%  Solve the linear system.
%
    for i = 1 : 3

      x_new = r83_gs_sl ( n, a, b, x, maxit, job );

      r8vec_print_some ( n, x_new, 1, 10, '  Current solution estimate:' );

      x(1:n) = x_new(1:n);

    end

  end

  return
end


function [der,errest,finaldelta] = derivest(fun,x0,varargin)
% DERIVEST: estimate the n'th derivative of fun at x0, provide an error estimate
% usage: [der,errest] = DERIVEST(fun,x0)  % first derivative
% usage: [der,errest] = DERIVEST(fun,x0,prop1,val1,prop2,val2,...)
%
% Derivest will perform numerical differentiation of an
% analytical function provided in fun. It will not
% differentiate a function provided as data. Use gradient
% for that purpose, or differentiate a spline model.
%
% The methods used by DERIVEST are finite difference
% approximations of various orders, coupled with a generalized
% (multiple term) Romberg extrapolation. This also yields
% the error estimate provided. DERIVEST uses a semi-adaptive
% scheme to provide the best estimate that it can by its
% automatic choice of a differencing interval.
%
% Finally, While I have not written this function for the
% absolute maximum speed, speed was a major consideration
% in the algorithmic design. Maximum accuracy was my main goal.
%
%
% Arguments (input)
%  fun - function to differentiate. May be an inline function,
%        anonymous, or an m-file. fun will be sampled at a set
%        of distinct points for each element of x0. If there are
%        additional parameters to be passed into fun, then use of
%        an anonymous function is recommended.
%
%        fun should be vectorized to allow evaluation at multiple
%        locations at once. This will provide the best possible
%        speed. IF fun is not so vectorized, then you MUST set
%        'vectorized' property to 'no', so that derivest will
%        then call your function sequentially instead.
%
%        Fun is assumed to return a result of the same
%        shape as its input x0.
%
%  x0  - scalar, vector, or array of points at which to
%        differentiate fun.
%
% Additional inputs must be in the form of property/value pairs.
%  Properties are character strings. They may be shortened
%  to the extent that they are unambiguous. Properties are
%  not case sensitive. Valid property names are:
%
%  'DerivativeOrder', 'MethodOrder', 'Style', 'RombergTerms'
%  'FixedStep', 'MaxStep'
%
%  All properties have default values, chosen as intelligently
%  as I could manage. Values that are character strings may
%  also be unambiguously shortened. The legal values for each
%  property are:
%
%  'DerivativeOrder' - specifies the derivative order estimated.
%        Must be a positive integer from the set [1,2,3,4].
%
%        DEFAULT: 1 (first derivative of fun)
%
%  'MethodOrder' - specifies the order of the basic method
%        used for the estimation.
%
%        For 'central' methods, must be a positive integer
%        from the set [2,4].
%
%        For 'forward' or 'backward' difference methods,
%        must be a positive integer from the set [1,2,3,4].
%
%        DEFAULT: 4 (a second order method)
%
%        Note: higher order methods will generally be more
%        accurate, but may also suffere more from numerical
%        problems.
%
%        Note: First order methods would usually not be
%        recommended.
%
%  'Style' - specifies the style of the basic method
%        used for the estimation. 'central', 'forward',
%        or 'backwards' difference methods are used.
%
%        Must be one of 'Central', 'forward', 'backward'.
%
%        DEFAULT: 'Central'
%
%        Note: Central difference methods are usually the
%        most accurate, but sometiems one must not allow
%        evaluation in one direction or the other.
%
%  'RombergTerms' - Allows the user to specify the generalized
%        Romberg extrapolation method used, or turn it off
%        completely.
%
%        Must be a positive integer from the set [0,1,2,3].
%
%        DEFAULT: 2 (Two Romberg terms)
%
%        Note: 0 disables the Romberg step completely.
%
%  'FixedStep' - Allows the specification of a fixed step
%        size, preventing the adaptive logic from working.
%        This will be considerably faster, but not necessarily
%        as accurate as allowing the adaptive logic to run.
%
%        DEFAULT: []
%
%        Note: If specified, 'FixedStep' will define the
%        maximum excursion from x0 that will be used.
%
%  'Vectorized' - Derivest will normally assume that your
%        function can be safely evaluated at multiple locations
%        in a single call. This would minimize the overhead of
%        a loop and additional function call overhead. Some
%        functions are not easily vectorizable, but you may
%        (if your matlab release is new enough) be able to use
%        arrayfun to accomplish the vectorization.
%
%        When all else fails, set the 'vectorized' property
%        to 'no'. This will cause derivest to loop over the
%        successive function calls.
%
%        DEFAULT: 'yes'
%
%
%  'MaxStep' - Specifies the maximum excursion from x0 that
%        will be allowed, as a multiple of x0.
%
%        DEFAULT: 100
%
%  'StepRatio' - Derivest uses a proportionally cascaded
%        series of function evaluations, moving away from your
%        point of evaluation. The StepRatio is the ratio used
%        between sequential steps.
%
%        DEFAULT: 2.0000001
%
%        Note: use of a non-integer stepratio is intentional,
%        to avoid integer multiples of the period of a periodic
%        function under some circumstances.
%
%
% See the document DERIVEST.pdf for more explanation of the
% algorithms behind the parameters of DERIVEST. In most cases,
% I have chosen good values for these parameters, so the user
% should never need to specify anything other than possibly
% the DerivativeOrder. I've also tried to make my code robust
% enough that it will not need much. But complete flexibility
% is in there for your use.
%
%
% Arguments: (output)
%  der - derivative estimate for each element of x0
%        der will have the same shape as x0.
%
%  errest - 95% uncertainty estimate of the derivative, such that
%
%        abs(der(j) - f'(x0(j))) < erest(j)
%
%  finaldelta - The final overall stepsize chosen by DERIVEST
%
%
% Example usage:
%  First derivative of exp(x), at x == 1
%   [d,e]=derivest(@(x) exp(x),1)
%   d =
%       2.71828182845904
%
%   e =
%       1.02015503167879e-14
%
%  True derivative
%   exp(1)
%   ans =
%       2.71828182845905
%
% Example usage:
%  Third derivative of x.^3+x.^4, at x = [0,1]
%   derivest(@(x) x.^3 + x.^4,[0 1],'deriv',3)
%   ans =
%       6       30
%
%  True derivatives: [6,30]
%
%
% See also: gradient
%
%
% Author: John D'Errico
% e-mail: woodchips@rochester.rr.com
% Release: 1.0
% Release date: 12/27/2006

par.DerivativeOrder = 1;
par.MethodOrder = 4;
par.Style = 'central';
par.RombergTerms = 2;
par.FixedStep = [];
par.MaxStep = 100;
% setting a default stepratio as a non-integer prevents
% integer multiples of the initial point from being used.
% In turn that avoids some problems for periodic functions.
par.StepRatio = 2.0000001;
par.NominalStep = [];
par.Vectorized = 'yes';

na = length(varargin);
if (rem(na,2)==1)
  error 'Property/value pairs must come as PAIRS of arguments.'
elseif na>0
  par = parse_pv_pairs(par,varargin);
end
par = check_params(par);

% Was fun a string, or an inline/anonymous function?
if (nargin<1)
  help derivest
  return
elseif isempty(fun)
  error 'fun was not supplied.'
elseif ischar(fun)
  % a character function name
  fun = str2func(fun);
end

% no default for x0
if (nargin<2) || isempty(x0)
  error 'x0 was not supplied'
end
par.NominalStep = max(x0,0.02);

% was a single point supplied?
nx0 = size(x0);
n = prod(nx0);

% Set the steps to use.
if isempty(par.FixedStep)
  % Basic sequence of steps, relative to a stepsize of 1.
  delta = par.MaxStep*par.StepRatio .^(0:-1:-25)';
  ndel = length(delta);
else
  % Fixed, user supplied absolute sequence of steps.
  ndel = 3 + ceil(par.DerivativeOrder/2) + ...
     par.MethodOrder + par.RombergTerms;
  if par.Style(1) == 'c'
    ndel = ndel - 2;
  end
  delta = par.FixedStep*par.StepRatio .^(-(0:(ndel-1)))';
end

% generate finite differencing rule in advance.
% The rule is for a nominal unit step size, and will
% be scaled later to reflect the local step size.
fdarule = 1;
switch par.Style
  case 'central'
    % for central rules, we will reduce the load by an
    % even or odd transformation as appropriate.
    if par.MethodOrder==2
      switch par.DerivativeOrder
        case 1
          % the odd transformation did all the work
          fdarule = 1;
        case 2
          % the even transformation did all the work
          fdarule = 2;
        case 3
          % the odd transformation did most of the work, but
          % we need to kill off the linear term
          fdarule = [0 1]/fdamat(par.StepRatio,1,2);
        case 4
          % the even transformation did most of the work, but
          % we need to kill off the quadratic term
          fdarule = [0 1]/fdamat(par.StepRatio,2,2);
      end
    else
      % a 4th order method. We've already ruled out the 1st
      % order methods since these are central rules.
      switch par.DerivativeOrder
        case 1
          % the odd transformation did most of the work, but
          % we need to kill off the cubic term
          fdarule = [1 0]/fdamat(par.StepRatio,1,2);
        case 2
          % the even transformation did most of the work, but
          % we need to kill off the quartic term
          fdarule = [1 0]/fdamat(par.StepRatio,2,2);
        case 3
          % the odd transformation did much of the work, but
          % we need to kill off the linear & quintic terms
          fdarule = [0 1 0]/fdamat(par.StepRatio,1,3);
        case 4
          % the even transformation did much of the work, but
          % we need to kill off the quadratic and 6th order terms
          fdarule = [0 1 0]/fdamat(par.StepRatio,2,3);
      end
    end
  case {'forward' 'backward'}
    % These two cases are identical, except at the very end,
    % where a sign will be introduced.

    % No odd/even trans, but we already dropped
    % off the constant term
    if par.MethodOrder==1
      if par.DerivativeOrder==1
        % an easy one
        fdarule = 1;
      else
        % 2:4
        v = zeros(1,par.DerivativeOrder);
        v(par.DerivativeOrder) = 1;
        fdarule = v/fdamat(par.StepRatio,0,par.DerivativeOrder);
      end
    else
      % par.MethodOrder methods drop off the lower order terms,
      % plus terms directly above DerivativeOrder
      v = zeros(1,par.DerivativeOrder + par.MethodOrder - 1);
      v(par.DerivativeOrder) = 1;
      fdarule = v/fdamat(par.StepRatio,0,par.DerivativeOrder+par.MethodOrder-1);
    end

    % correct sign for the 'backward' rule
    if par.Style(1) == 'b'
      fdarule = -fdarule;
    end

end % switch on par.style (generating fdarule)
nfda = length(fdarule);

% will we need fun(x0)?
if (rem(par.DerivativeOrder,2) == 0) || ~strncmpi(par.Style,'central',7)
  if strcmpi(par.Vectorized,'yes')
    f_x0 = fun(x0);
  else
    % not vectorized, so loop
    f_x0 = zeros(size(x0));
    for j = 1:numel(x0)
      f_x0(j) = fun(x0(j));
    end
  end
else
  f_x0 = [];
end

% Loop over the elements of x0, reducing it to
% a scalar problem. Sorry, vectorization is not
% complete here, but this IS only a single loop.
der = zeros(nx0);
errest = der;
finaldelta = der;
for i = 1:n
  x0i = x0(i);
  h = par.NominalStep(i);

  % a central, forward or backwards differencing rule?
  % f_del is the set of all the function evaluations we
  % will generate. For a central rule, it will have the
  % even or odd transformation built in.
  if par.Style(1) == 'c'
    % A central rule, so we will need to evaluate
    % symmetrically around x0i.
    if strcmpi(par.Vectorized,'yes')
      f_plusdel = fun(x0i+h*delta);
      f_minusdel = fun(x0i-h*delta);
    else
      % not vectorized, so loop
      f_minusdel = zeros(size(delta));
      f_plusdel = zeros(size(delta));
      for j = 1:numel(delta)
        f_plusdel(j) = fun(x0i+h*delta(j));
        f_minusdel(j) = fun(x0i-h*delta(j));
      end
    end

    if ismember(par.DerivativeOrder,[1 3])
      % odd transformation
      f_del = (f_plusdel - f_minusdel)/2;
    else
      f_del = (f_plusdel + f_minusdel)/2 - f_x0(i);
    end
  elseif par.Style(1) == 'f'
    % forward rule
    % drop off the constant only
    if strcmpi(par.Vectorized,'yes')
      f_del = fun(x0i+h*delta) - f_x0(i);
    else
      % not vectorized, so loop
      f_del = zeros(size(delta));
      for j = 1:numel(delta)
        f_del(j) = fun(x0i+h*delta(j)) - f_x0(i);
      end
    end
  else
    % backward rule
    % drop off the constant only
    if strcmpi(par.Vectorized,'yes')
      f_del = fun(x0i-h*delta) - f_x0(i);
    else
      % not vectorized, so loop
      f_del = zeros(size(delta));
      for j = 1:numel(delta)
        f_del(j) = fun(x0i-h*delta(j)) - f_x0(i);
      end
    end
  end

  % check the size of f_del to ensure it was properly vectorized.
  f_del = f_del(:);
  if length(f_del)~=ndel
    error 'fun did not return the correct size result (fun must be vectorized)'
  end

  % Apply the finite difference rule at each delta, scaling
  % as appropriate for delta and the requested DerivativeOrder.
  % First, decide how many of these estimates we will end up with.
  ne = ndel + 1 - nfda - par.RombergTerms;

  % Form the initial derivative estimates from the chosen
  % finite difference method.
  der_init = vec2mat(f_del,ne,nfda)*fdarule.';

  % scale to reflect the local delta
  der_init = der_init(:)./(h*delta(1:ne)).^par.DerivativeOrder;

  % Each approximation that results is an approximation
  % of order par.DerivativeOrder to the desired derivative.
  % Additional (higher order, even or odd) terms in the
  % Taylor series also remain. Use a generalized (multi-term)
  % Romberg extrapolation to improve these estimates.
  switch par.Style
    case 'central'
      rombexpon = 2*(1:par.RombergTerms) + par.MethodOrder - 2;
    otherwise
      rombexpon = (1:par.RombergTerms) + par.MethodOrder - 1;
  end
  [der_romb,errors] = rombextrap(par.StepRatio,der_init,rombexpon);

  % Choose which result to return

  % first, trim off the
  if isempty(par.FixedStep)
    % trim off the estimates at each end of the scale
    nest = length(der_romb);
    switch par.DerivativeOrder
      case {1 2}
        trim = [1 2 nest-1 nest];
      case 3
        trim = [1:4 nest+(-3:0)];
      case 4
        trim = [1:6 nest+(-5:0)];
    end

    [der_romb,tags] = sort(der_romb);

    der_romb(trim) = [];
    tags(trim) = [];
    errors = errors(tags);
    trimdelta = delta(tags);

    [errest(i),ind] = min(errors);

    finaldelta(i) = h*trimdelta(ind);
    der(i) = der_romb(ind);
  else
    [errest(i),ind] = min(errors);
    finaldelta(i) = h*delta(ind);
    der(i) = der_romb(ind);
  end
end

end % mainline end

% ============================================
% subfunction - romberg extrapolation
% ============================================
function [der_romb,errest] = rombextrap(StepRatio,der_init,rombexpon)
% do romberg extrapolation for each estimate
%
%  StepRatio - Ratio decrease in step
%  der_init - initial derivative estimates
%  rombexpon - higher order terms to cancel using the romberg step
%
%  der_romb - derivative estimates returned
%  errest - error estimates
%  amp - noise amplification factor due to the romberg step

srinv = 1/StepRatio;

% do nothing if no romberg terms
nexpon = length(rombexpon);
rmat = ones(nexpon+2,nexpon+1);
switch nexpon
  case 0
    % rmat is simple: ones(2,1)
  case 1
    % only one romberg term
    rmat(2,2) = srinv^rombexpon;
    rmat(3,2) = srinv^(2*rombexpon);
  case 2
    % two romberg terms
    rmat(2,2:3) = srinv.^rombexpon;
    rmat(3,2:3) = srinv.^(2*rombexpon);
    rmat(4,2:3) = srinv.^(3*rombexpon);
  case 3
    % three romberg terms
    rmat(2,2:4) = srinv.^rombexpon;
    rmat(3,2:4) = srinv.^(2*rombexpon);
    rmat(4,2:4) = srinv.^(3*rombexpon);
    rmat(5,2:4) = srinv.^(4*rombexpon);
end

% qr factorization used for the extrapolation as well
% as the uncertainty estimates
[qromb,rromb] = qr(rmat,0);

% the noise amplification is further amplified by the Romberg step.
% amp = cond(rromb);

% this does the extrapolation to a zero step size.
ne = length(der_init);
rhs = vec2mat(der_init,nexpon+2,max(1,ne - (nexpon+2)));
rombcoefs = rromb\(qromb.'*rhs);
der_romb = rombcoefs(1,:).';

% uncertainty estimate of derivative prediction
s = sqrt(sum((rhs - rmat*rombcoefs).^2,1));
rinv = rromb\eye(nexpon+1);
cov1 = sum(rinv.^2,2); % 1 spare dof
errest = s.'*12.7062047361747*sqrt(cov1(1));

end % rombextrap

% ============================================
% subfunction - vec2mat
% ============================================
function mat = vec2mat(vec,n,m)
% forms the matrix M, such that M(i,j) = vec(i+j-1)
[i,j] = ndgrid(1:n,0:m-1);
ind = i+j;
mat = vec(ind);
if n==1
  mat = mat.';
end

end % vec2mat

% ============================================
% subfunction - fdamat
% ============================================
function mat = fdamat(sr,parity,nterms)
% Compute matrix for fda derivation.
% parity can be
%   0 (one sided, all terms included but zeroth order)
%   1 (only odd terms included)
%   2 (only even terms included)
% nterms - number of terms

% sr is the ratio between successive steps
srinv = 1./sr;

switch parity
  case 0
    % single sided rule
    [i,j] = ndgrid(1:nterms);
    c = 1./factorial(1:nterms);
    mat = c(j).*srinv.^((i-1).*j);
  case 1
    % odd order derivative
    [i,j] = ndgrid(1:nterms);
    c = 1./factorial(1:2:(2*nterms));
    mat = c(j).*srinv.^((i-1).*(2*j-1));
  case 2
    % even order derivative
    [i,j] = ndgrid(1:nterms);
    c = 1./factorial(2:2:(2*nterms));
    mat = c(j).*srinv.^((i-1).*(2*j));
end

end % fdamat

% ============================================
% subfunction - check_params
% ============================================
function par = check_params(par)
% check the parameters for acceptability
%
% Defaults
% par.DerivativeOrder = 1;
% par.MethodOrder = 2;
% par.Style = 'central';
% par.RombergTerms = 2;
% par.FixedStep = [];

% DerivativeOrder == 1 by default
if isempty(par.DerivativeOrder)
  par.DerivativeOrder = 1;
else
  if (length(par.DerivativeOrder)>1) || ~ismember(par.DerivativeOrder,1:4)
    error 'DerivativeOrder must be scalar, one of [1 2 3 4].'
  end
end

% MethodOrder == 2 by default
if isempty(par.MethodOrder)
  par.MethodOrder = 2;
else
  if (length(par.MethodOrder)>1) || ~ismember(par.MethodOrder,[1 2 3 4])
    error 'MethodOrder must be scalar, one of [1 2 3 4].'
  elseif ismember(par.MethodOrder,[1 3]) && (par.Style(1)=='c')
    error 'MethodOrder==1 or 3 is not possible with central difference methods'
  end
end

% style is char
valid = {'central', 'forward', 'backward'};
if isempty(par.Style)
  par.Style = 'central';
elseif ~ischar(par.Style)
  error 'Invalid Style: Must be character'
end
ind = find(strncmpi(par.Style,valid,length(par.Style)));
if (length(ind)==1)
  par.Style = valid{ind};
else
  error(['Invalid Style: ',par.Style])
end

% vectorized is char
valid = {'yes', 'no'};
if isempty(par.Vectorized)
  par.Vectorized = 'yes';
elseif ~ischar(par.Vectorized)
  error 'Invalid Vectorized: Must be character'
end
ind = find(strncmpi(par.Vectorized,valid,length(par.Vectorized)));
if (length(ind)==1)
  par.Vectorized = valid{ind};
else
  error(['Invalid Vectorized: ',par.Vectorized])
end

% RombergTerms == 2 by default
if isempty(par.RombergTerms)
  par.RombergTerms = 2;
else
  if (length(par.RombergTerms)>1) || ~ismember(par.RombergTerms,0:3)
    error 'RombergTerms must be scalar, one of [0 1 2 3].'
  end
end

% FixedStep == [] by default
if (length(par.FixedStep)>1) || (~isempty(par.FixedStep) && (par.FixedStep<=0))
  error 'FixedStep must be empty or a scalar, >0.'
end

% MaxStep == 10 by default
if isempty(par.MaxStep)
  par.MaxStep = 10;
elseif (length(par.MaxStep)>1) || (par.MaxStep<=0)
  error 'MaxStep must be empty or a scalar, >0.'
end

end % check_params

% ============================================
% Included subfunction - parse_pv_pairs
% ============================================
function params=parse_pv_pairs(params,pv_pairs)
% parse_pv_pairs: parses sets of property value pairs, allows defaults
% usage: params=parse_pv_pairs(default_params,pv_pairs)
%
% arguments: (input)
%  default_params - structure, with one field for every potential
%             property/value pair. Each field will contain the default
%             value for that property. If no default is supplied for a
%             given property, then that field must be empty.
%
%  pv_array - cell array of property/value pairs.
%             Case is ignored when comparing properties to the list
%             of field names. Also, any unambiguous shortening of a
%             field/property name is allowed.
%
% arguments: (output)
%  params   - parameter struct that reflects any updated property/value
%             pairs in the pv_array.
%
% Example usage:
% First, set default values for the parameters. Assume we
% have four parameters that we wish to use optionally in
% the function examplefun.
%
%  - 'viscosity', which will have a default value of 1
%  - 'volume', which will default to 1
%  - 'pie' - which will have default value 3.141592653589793
%  - 'description' - a text field, left empty by default
%
% The first argument to examplefun is one which will always be
% supplied.
%
%   function examplefun(dummyarg1,varargin)
%   params.Viscosity = 1;
%   params.Volume = 1;
%   params.Pie = 3.141592653589793
%
%   params.Description = '';
%   params=parse_pv_pairs(params,varargin);
%   params
%
% Use examplefun, overriding the defaults for 'pie', 'viscosity'
% and 'description'. The 'volume' parameter is left at its default.
%
%   examplefun(rand(10),'vis',10,'pie',3,'Description','Hello world')
%
% params =
%     Viscosity: 10
%        Volume: 1
%           Pie: 3
%   Description: 'Hello world'
%
% Note that capitalization was ignored, and the property 'viscosity'
% was truncated as supplied. Also note that the order the pairs were
% supplied was arbitrary.

npv = length(pv_pairs);
n = npv/2;

if n~=floor(n)
  error 'Property/value pairs must come in PAIRS.'
end
if n<=0
  % just return the defaults
  return
end

if ~isstruct(params)
  error 'No structure for defaults was supplied'
end

% there was at least one pv pair. process any supplied
propnames = fieldnames(params);
lpropnames = lower(propnames);
for i=1:n
  p_i = lower(pv_pairs{2*i-1});
  v_i = pv_pairs{2*i};

  ind = strmatch(p_i,lpropnames,'exact');
  if isempty(ind)
    ind = find(strncmp(p_i,lpropnames,length(p_i)));
    if isempty(ind)
      error(['No matching property found for: ',pv_pairs{2*i-1}])
    elseif length(ind)>1
      error(['Ambiguous property name: ',pv_pairs{2*i-1}])
    end
  end
  p_i = propnames{ind};

  % override the corresponding default in params
  params = setfield(params,p_i,v_i); %#ok

end

end % parse_pv_pairs


% Usage: e = Faddeeva_erfi(z [, relerr])
%
% Compute erfi(z) = -i*erf(i*z), the imaginary error function,
% for an array or matrix of complex values z.
%
% relerr, if supplied, indicates a desired relative error tolerance in
% w; the default is 0, indicating that machine precision is requested (and
% a relative error < 1e-13 is usually achieved).  Specifying a larger
% relerr may improve performance for some z (at the expense of accuracy).
%
% S. G. Johnson, http://ab-initio.mit.edu/Faddeeva


i = imread('lab.pgm');

%Make image greyscale
if length(size(i)) == 3
	im =  double(i(:,:,2));
else
	im = double(i);
end

cs = fast_corner_detect_9(im, 30);
c = fast_nonmax(im, 30, cs);

image(im/4)
axis image
colormap(gray)
hold on
plot(cs(:,1), cs(:,2), 'r.')
plot(c(:,1), c(:,2), 'g.')
legend('9 point FAST corners', 'nonmax-suppressed corners')
title('9 point FAST corner detection on an image')


function [x_sigma, x_gam] = gaussian_para_esti(x)
% x = x(:);
gam = 0.2:0.001:10;
r_gam = (gamma(1./gam).*gamma(3./gam))./((gamma(2./gam)).^2);
x_mu = mean(x);
x_sigma_sq = mean((x - x_mu).^2);
x_sigma = sqrt(x_sigma_sq);
E_x = mean(abs(x - x_mu));
rho_x = x_sigma_sq/E_x^2;
[x_diff, x_ind] = min(abs(rho_x - r_gam));
x_gam = gam(x_ind);


function varargout = interp2(varargin)
% bsarray/interp2: 2-D interpolation (table lookup)
% usage: ZI = interp2(B,XI,YI);
%    or: ZI = interp2(B,XI,YI,EXTRAPVAL);
%
% arguments:
%   B - bsarray object having tensorOrder 2. The positions of the
%       x-coordinates of the underlying data array are assumed to be
%       X = s(2).*(1:Nx), where Nx is the number of data points in the X
%       dimension (i.e., second element of get(B,'dataSize')), and s
%       is the element spacing (i.e., get(B,'elementSpacing')). The
%       positions of the y-coordinates of the underlying data array are
%       assumed to be Y = s(1).*(1:Ny).
%   XI - x-coordinates of points at which to interpolate B.
%   YI - y-coordinates of points at which to interpolate B. YI must be the
%       same size as XI.
%
%   EXTRAPVAL - value to return for points in XI and YI that are outside
%       the range of X and Y, respectively. Default EXTRAPVAL = NaN.
%
%   ZI - the values of the underlying bsarray B evaluated at the points in
%       the array XI.
%

% author: Nathan D. Cahill
% email: ndcahill@gmail.com
% date: 18 April 2008

% parse input arguments
[b,xi,yi,extrapval] = parseInputs(varargin{:});

% get flag to determine if basis functions in each dimension are centred or
% shifted
m = double(get(b,'centred'));
mx = m(2); my = m(1);

% get number of data elements and coefficients, determine amount of padding
% that has been done to create coefficients
nData = get(b,'dataSize'); nDatax = nData(2); nDatay = nData(1);
n = get(b,'coeffsSize'); nx = n(2); ny = n(1);
padNum = (n-nData-1)/2; padNumx = padNum(2); padNumy = padNum(1);

% get the spacing between elements, and then construct vectors of the
% locations of the data and of the BSpline coefficients
h = get(b,'elementSpacing'); hx = h(2); hy = h(1);
xCol = hx.*((1-padNumx):(nDatax+padNumx+1))';
xDataCol = xCol((1+padNumx):(end-padNumx));
yCol = hy.*((1-padNumy):(nDatay+padNumy+1))';
yDataCol = yCol((1+padNumy):(end-padNumy));

% turn evaluation points into a column vector, but retain original size so
% output can be returned in same size as input
siz_xi = size(xi);
siz_zi = siz_xi;

% grab the BSpline coefficients
cMat = get(b,'coeffs');

% initialize some variables for use in interpolation
numelXi = numel(xi);
ziMat = zeros(numelXi,1);
p = 1:numelXi;

% Find indices of subintervals, x(k) <= u < x(k+1),
% or u < x(1) or u >= x(m-1).
kx = min(max(1+floor((xi(:)-xCol(1))/hx),1+padNumx),nx-padNumx) + 1-mx;
ky = min(max(1+floor((yi(:)-yCol(1))/hy),1+padNumy),ny-padNumy) + 1-my;
sx = (xi(:) - xCol(kx))/hx;
sy = (yi(:) - yCol(ky))/hy;

% perform interpolation
d = get(b,'degree'); dx = d(2); dy = d(1);
xflag = (~mx && mod(dx,2));
yflag = (~my && mod(dy,2));
for j=1:ceil((dx+1)/2) % loop over BSpline degree in x dimension
    Bx1 = evalBSpline(sx+j-(1+mx)/2,dx);
    Bx2 = evalBSpline(sx-j+(1-mx)/2,dx);
    for i=1:ceil((dy+1)/2) % loop over BSpline degree in y dimension
        By1 = evalBSpline(sy+i-(1+my)/2,dy);
        By2 = evalBSpline(sy-i+(1-my)/2,dy);
        for ind = 1:numelXi % loop over evaluation points, computing interpolated value
            ziMat(ind) = ziMat(ind) + cMat(ky(ind)-i+my,kx(ind)-j+mx).*By1(ind).*Bx1(ind) + ...
                cMat(ky(ind)+i-1+my,kx(ind)-j+mx).*By2(ind).*Bx1(ind) + ...
                cMat(ky(ind)-i+my,kx(ind)+j-1+mx).*By1(ind).*Bx2(ind) + ...
                cMat(ky(ind)+i-1+my,kx(ind)+j-1+mx).*By2(ind).*Bx2(ind);
        end
    end
    if yflag % add a correction factor if BSpline in y direction is shifted and of odd degree
        By1 = evalBSpline(sy+i+1/2,dy);
        for ind = 1:numelXi
            ziMat(ind) = ziMat(ind) + By1(ind).*...
                (cMat(ky(ind)-i-1,kx(ind)-j+mx).*Bx1(ind) + cMat(ky(ind)-i-1,kx(ind)+j-1+mx).*Bx2(ind));
        end
    end
end
if xflag % add a correction factor if BSpline in x direction is shifted and of odd degree
    Bx1 = evalBSpline(sx+j+1/2,dx);
    for i=1:ceil((dy+1)/2)
        By1 = evalBSpline(sy+i-(1+my)/2,dy);
        By2 = evalBSpline(sy-i+(1-my)/2,dy);
        for ind = 1:numelXi
            ziMat(ind) = ziMat(ind) + Bx1(ind).*...
                (cMat(ky(ind)-i+my,kx(ind)-j-1).*By1(ind) + cMat(ky(ind)+i-1+my,kx(ind)-j-1).*By2(ind));
        end
    end
end

% perform extrapolation
outOfBounds = xi(:)<xDataCol(1) | xi(:)>xDataCol(nDatax) | yi(:)<yDataCol(1) | yi(:)>yDataCol(nDatay);
ziMat(p(outOfBounds)) = extrapval;

% reshape result to have same size as input xi
zi = reshape(ziMat,siz_zi);
varargout{1} = zi;

%% subfunction parseInputs
function [b,xi,yi,extrapval] = parseInputs(varargin)

nargs = length(varargin);
error(nargchk(3,4,nargs));

% Process B
b = varargin{1};
if ~isequal(b.tensorOrder,2)
    error([mfilename,'parseInputs:WrongOrder'], ...
        'bsarray/interp2 can only be used with bsarray objects having tensor order 2.');
end

% Process XI
xi = varargin{2};
if ~isreal(xi)
    error([mfilename,'parseInputs:ComplexInterpPts'], ...
        'The interpolation points XI should be real.')
end

% Process YI
yi = varargin{3};
if ~isreal(yi)
    error([mfilename,'parseInputs:ComplexInterpPts'], ...
        'The interpolation points YI should be real.')
end
if ~isequal(size(xi),size(yi))
    error([mfilename,'parseInputs:YIXINotSameSize'], ...
        'YI must be the same size as XI');
end

% Process EXTRAPVAL
if nargs > 3
    extrapval = varargin{4};
else
    extrapval = [];
end
if isempty(extrapval)
    extrapval = NaN;
end
if ~isscalar(extrapval)
    error([mfilename,':NonScalarExtrapValue'],...
        'EXTRAP option must be a scalar.')
end


%%%% calculation of field spatial distributions, starting from the eigenvectors (Fourier coefficients)
%%%% omega = normalized frequency
%%%% eta = inverse of matrix containing the Fourier coeff. of dielectric
%%%% function
%%%% Phi = matrix of column eigenvectors [hx,hy]'
%%%% kz = vector of real eigenvalues
%%%% u = index of selected eigenvalue/eigenvector
function [Ex,Ey,Ez,Hx,Hy,Hz] = rfields(omega,eta,kGx,kGy,kz,Phi,N1,N2,u)
N=N1*N2;
hx=Phi(1:N,u); hy=Phi((N+1):2*N,u);
hz=-(1/kz(u))*(kGx*hx+kGy*hy);
ex=-(1/omega)*eta*(kGy*hz-kz(u)*hy);
ey=-(1/omega)*eta*(kz(u)*hx-kGx*hz);
ez=-(1/omega)*eta*(kGx*hy-kGy*hx);

ex=reshape(ex,N1,N2); ey=reshape(ey,N1,N2); ez=reshape(ez,N1,N2);
hx=reshape(hx,N1,N2); hy=reshape(hy,N1,N2); hz=reshape(hz,N1,N2);
Ex=(N1*N2)*ifft2(ifftshift(ex'));
Ey=(N1*N2)*ifft2(ifftshift(ey'));
Ez=(N1*N2)*ifft2(ifftshift(ez'));

Hx=(N1*N2)*ifft2(ifftshift(hx'));
Hy=(N1*N2)*ifft2(ifftshift(hy'));
Hz=(N1*N2)*ifft2(ifftshift(hz'));


function ret = wrap_boundary_liu(img, img_size)
% wrap_boundary_liu.m
%
%   pad image boundaries such that image boundaries are circularly smooth
%
%     written by Sunghyun Cho (sodomau@postech.ac.kr)
%
% This is a variant of the method below:
%   Reducing boundary artifacts in image deconvolution
%     Renting Liu, Jiaya Jia
%     ICIP 2008
%
    [H, W, Ch] = size(img);
    H_w = img_size(1) - H;
    W_w = img_size(2) - W;

    ret = zeros(img_size(1), img_size(2), Ch);
    for ch = 1:Ch
        alpha = 1;
        HG = img(:,:,ch);

        r_A = zeros(alpha*2+H_w, W);
        r_A(1:alpha, :) = HG(end-alpha+1:end, :);
        r_A(end-alpha+1:end, :) = HG(1:alpha, :);
        a = ((1:H_w)-1)/(H_w-1);
        r_A(alpha+1:end-alpha, 1) = (1-a)*r_A(alpha,1) + a*r_A(end-alpha+1,1);
        r_A(alpha+1:end-alpha, end) = (1-a)*r_A(alpha,end) + a*r_A(end-alpha+1,end);

        A2 = solve_min_laplacian(r_A(alpha:end-alpha+1,:));
        r_A(alpha:end-alpha+1,:) = A2;
        A = r_A;

        r_B = zeros(H, alpha*2+W_w);
        r_B(:, 1:alpha) = HG(:, end-alpha+1:end);
        r_B(:, end-alpha+1:end) = HG(:, 1:alpha);
        a = ((1:W_w)-1)/(W_w-1);
        r_B(1, alpha+1:end-alpha) = (1-a)*r_B(1,alpha) + a*r_B(1,end-alpha+1);
        r_B(end, alpha+1:end-alpha) = (1-a)*r_B(end,alpha) + a*r_B(end,end-alpha+1);

        B2 = solve_min_laplacian(r_B(:, alpha:end-alpha+1));
        r_B(:,alpha:end-alpha+1,:) = B2;
        B = r_B;

        r_C = zeros(alpha*2+H_w, alpha*2+W_w);
        r_C(1:alpha, :) = B(end-alpha+1:end, :);
        r_C(end-alpha+1:end, :) = B(1:alpha, :);
        r_C(:, 1:alpha) = A(:, end-alpha+1:end);
        r_C(:, end-alpha+1:end) = A(:, 1:alpha);

        C2 = solve_min_laplacian(r_C(alpha:end-alpha+1, alpha:end-alpha+1));
        r_C(alpha:end-alpha+1, alpha:end-alpha+1) = C2;
        C = r_C;

        A = A(alpha:end-alpha-1, :);
        B = B(:, alpha+1:end-alpha);
        C = C(alpha+1:end-alpha, alpha+1:end-alpha);

        ret(:,:,ch) = [img(:,:,ch) B; A C];
    end

end

function [img_direct] = solve_min_laplacian(boundary_image)
% function [img_direct] = poisson_solver_function(gx,gy,boundary_image)
% Inputs; Gx and Gy -> Gradients
% Boundary Image -> Boundary image intensities
% Gx Gy and boundary image should be of same size
    [H,W] = size(boundary_image);

    % Laplacian
    f = zeros(H,W);                          clear j k

    % boundary image contains image intensities at boundaries
    boundary_image(2:end-1, 2:end-1) = 0;
    j = 2:H-1;      k = 2:W-1;      f_bp = zeros(H,W);
    f_bp(j,k) = -4*boundary_image(j,k) + boundary_image(j,k+1) + ...
        boundary_image(j,k-1) + boundary_image(j-1,k) + boundary_image(j+1,k);
    clear j k

    %f1 = f - reshape(f_bp,H,W); % subtract boundary points contribution
    f1 = f - f_bp; % subtract boundary points contribution
    clear f_bp f

    % DST Sine Transform algo starts here
    f2 = f1(2:end-1,2:end-1);                   clear f1
    % compute sine tranform
    tt = dst(f2);       f2sin = dst(tt')';      clear f2

    % compute Eigen Values
    [x,y] = meshgrid(1:W-2, 1:H-2);
    denom = (2*cos(pi*x/(W-1))-2) + (2*cos(pi*y/(H-1)) - 2);

    % divide
    f3 = f2sin./denom;                          clear f2sin x y

    % compute Inverse Sine Transform
    tt = idst(f3);      clear f3;       img_tt = idst(tt')';        clear tt

    % put solution in inner points; outer points obtained from boundary image
    img_direct = boundary_image;
    img_direct(2:end-1,2:end-1) = 0;
    img_direct(2:end-1,2:end-1) = img_tt;
end


classdef VNT4 < PROBLEM
% <multi> <real> <constrained>
% Benchmark MOP proposed by Viennet

%------------------------------- Reference --------------------------------
% R. Viennet, C. Fonteix, and I. Marc, Multicriteria optimization using a
% genetic algorithm for determining a Pareto set, International Journal of
% Systems Science, 1996, 27(2): 255-260.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M        = 3;
            obj.D        = 2;
            obj.lower    = [-4,-4];
            obj.upper    = [4,4];
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            PopObj(:,1) = (PopDec(:,1)-2).^2/2 + (PopDec(:,2)+1).^2/13 + 3;
            PopObj(:,2) = (PopDec(:,1)+PopDec(:,2)-3).^2/175 + (2*PopDec(:,2)-PopDec(:,1)).^2/17 - 13;
            PopObj(:,3) = (3*PopDec(:,1)-2*PopDec(:,2)+4).^2/8 + (PopDec(:,1)-PopDec(:,2)+1).^2/27 + 15;
        end
        %% Calculate constraint violations
        function PopCon = CalCon(obj,PopDec)
            PopCon(:,1) = PopDec(:,2) + 4*PopDec(:,1) - 4;
            PopCon(:,2) = -1 - PopDec(:,1);
            PopCon(:,3) = PopDec(:,1) - 2 - PopDec(:,2);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            N = ceil(sqrt(N));
            x = linspace(-1,1.2,N);
            X = [];
            for i = 1 : N
                X = [X;repmat(x(i),N,1),linspace(x(i)-2,-4*x(i)+4,N)'];
            end
            R = obj.CalObj(X);
            R = R(NDSort(R,1)==1,:);
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            x = linspace(-1,1.2,40);
            X = [];
            for i = 1 : 40
                X = [X;repmat(x(i),40,1),linspace(x(i)-2,-4*x(i)+4,40)'];
            end
            R = obj.CalObj(X);
            R(NDSort(R,1)>1,:) = nan;
            R = {reshape(R(:,1),40,40),reshape(R(:,2),40,40),reshape(R(:,3),40,40)};
        end
    end
end


function beta_nc_test01 ( )

%*****************************************************************************80
%
%% BETA_NC_TEST01 tests BETA_NONCENTRAL_CDF against tabulated values.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2008
%
%  Author:
%
%    John Burkardt
%
  error_max = 1.0E-10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'BETA_NC_TEST01:\n' );
  fprintf ( 1, '  Compare tabulated values of the noncentral\n' );
  fprintf ( 1, '  incomplete Beta Function against values\n' );
  fprintf ( 1, '  computed by BETA_NONCENTRAL_CDF.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1,'        A        B   LAMBDA      X      ' );
  fprintf ( 1,'  CDF                       CDF                     DIFF\n' );
  fprintf ( 1,'                                        ' );
  fprintf ( 1,' (tabulated)               (BETA_NC)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, a, b, lambda, x, fx ] = beta_noncentral_cdf_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fx2 = beta_noncentral_cdf ( a, b, lambda, x, error_max );

    fprintf ( 1, '  %7.1f  %7.1f  %7.1f  %7.3f  %24.16e  %24.16e  %10.4e\n', ...
    a, b, lambda, x, fx, fx2, abs ( fx - fx2 ) );

  end

  return
end


function [ n_data, n, z, fx ] = polylogarithm_values ( n_data )

%*****************************************************************************80
%
%% POLYLOGARITHM_VALUES returns some values of the polylogarithm.
%
%  Discussion:
%
%    The polylogarithm of n and z is defined as
%
%      f[n,z] = Sum ( 1 <= k < infinity ) z^k / k^n
%
%    In Mathematica, the function can be evaluated by:
%
%      PolyLog[n,z]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, integer N, the exponent of the denominator.
%
%    Output, real Z, the base of the numerator.
%
%    Output, real FX, the value of the function.
%
  n_max = 12;

  fx_vec = [ ...
     0.1644934066848226E+01, ...
     0.1202056903159594E+01, ...
     0.1000994575127818E+01, ...
     0.5822405264650125E+00, ...
     0.5372131936080402E+00, ...
     0.5002463206060068E+00, ...
     0.3662132299770635E+00, ...
     0.3488278611548401E+00, ...
     0.3334424797228716E+00, ...
     0.1026177910993911E+00, ...
     0.1012886844792230E+00, ...
     0.1000097826564961E+00 ];

  n_vec = [ ...
     2, 3, 10, 2, 3, 10, 2, 3, 10, 2, 3, 10 ];

  z_vec = [ ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.5000000000000000E+00, ...
     0.5000000000000000E+00, ...
     0.5000000000000000E+00, ...
     0.3333333333333333E+00, ...
     0.3333333333333333E+00, ...
     0.3333333333333333E+00, ...
     0.1000000000000000E+00, ...
     0.1000000000000000E+00, ...
     0.1000000000000000E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    n = 0;
    z = 0.0;
    fx = 0.0;
  else
    n = n_vec(n_data);
    z = z_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function a = fourier_sine_inverse ( n )

%*****************************************************************************80
%
%% FOURIER_SINE_INVERSE returns the inverse of the FOURIER_SINE matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  a = fourier_sine ( n );

  return
end


%
%  Compute the factorization of a multivariate polynomial
%  by Ruppert matrix, projection, Newton polygon, generalized eigenvalue
%  and numerical GCD
%
%  Syntax:   >> fac = NIF(F,tol)
%
%    Input:    F -- (matrix) multivariate polynomial in coeff. matrices
%                            !!!must be squarefree!!!
%            tol -- (numeric) coefficient error tolerence
%
%    Output: fac -- (cell) irreducible factors of G in coeff. matrices
%
%  Example:
%
%        >> F
%
%        F =
%
%             0     1     1     2
%             0     1     0     1
%             0     0     2     2
%            15    10     3     2
%
%        >> fac = NIF(F,1e-10);
%        >> fac{:}
%
%        ans =
%
%                0             1.0000
%                0             1.0000
%                0                  0
%           0.8320 - 0.0035i   0.5547 - 0.0024i
%
%
%        ans =
%
%                0             1.0000
%                0                  0
%                0             2.0000
%           0.9729 + 0.1223i   0.1946 + 0.0245i
%


%TEST 07: Simple v.s. Area (two methods of building weight matrix)
clear
clc
fprintf('TEST 07: Simple v.s. Area (two methods of building weight matrix)\r');
im = imtest('s4',156);
figure('name','Original Image: Phantom')
imshow(im)
siz = size(im);
angles = 0:3:179;
[W1,p1,~,~] = buildWeightMatrixSimple(im,angles);
[W2,p2,~,~] = buildWeightMatrixArea(im,angles);
%Simple
fprintf('Reconstruct from simple-W:\r')
im_rec1 = tomo_recon_lsqr(W1, p1, siz, 1e-6, 1000);
im_rec1 = uint8(imscale(im_rec1));
figure('name','Simple')
imshow(im_rec1);
%Area
fprintf('Reconstruct from area-W:\r')
im_rec2 = tomo_recon_lsqr(W2, p2, siz, 1e-6, 1000);
im_rec2 = uint8(imscale(im_rec2));
figure('name','Area')
imshow(im_rec2);


function determ = hankel_n_determinant ( n )

%*****************************************************************************80
%
%% HANKEL_N_DETERMINANT returns the determinant of the HANKEL_N matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 January 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Output, real DETERM, the determinant.
%
  determ = r8_mop ( floor ( n / 2 ) ) * n ^ n;

  return
end


function [xH, H, HNMax, HNMin] = hopkins_main (dataXY_all, xlim1, xlim2, ylim1, ylim2, m, N, Nbins, envelopes, Nsimul, filename)
% HOPKINS_MAIN computes hopkins statistics
% [xH, H, HNMax, HNMin] = hopkins_main (dataXY_all, xlim1, xlim2, ylim1, ylim2, m, N, Nbins, envelopes, Nsimul, filename)
% dataXY_all - whole data
% [xlim1, xlim2, ylim1, ylim2] - selected ROI
% m - number of random events and points for Hopkins computation
% N - number of iterations for histogram computation
% Nbins - numbero of bins in histogram
% envelopes - if set to 1 maximum and minimum envelopes for Poisson random
% process are computed
% Nsimul - number of simulations for envelopes computation
% filename - name of the txt file where results are written

Hopp.xlim1=xlim1; Hopp.xlim2=xlim2; Hopp.ylim1=ylim1; Hopp.ylim2=ylim2;
Hopp.Nbins = Nbins; Hopp.m = m; Hopp.N = N;
if envelopes, Hopp.Nsimul = Nsimul; end

figure
dataXY = ROIdata(dataXY_all, Hopp.xlim1, Hopp.xlim2, Hopp.ylim1, Hopp.ylim2, 1);
Hopp.Npoints = length(dataXY);

fprintf('Computing Hopkins statistics for selected ROI... \n');
h = waitbar(0,'Computing Hopkins...');
for ii=1:Hopp.N
    HopD(ii) = hopkins(dataXY, Hopp.xlim1, Hopp.xlim2, Hopp.ylim1, Hopp.ylim2, Hopp.m);
    waitbar(ii/Hopp.N,h)
end
close (h)

[H, xH] = hist(HopD, Nbins);
H = H/trapz(xH, H); % normalization

if envelopes
    [HNMin, HNMax, xH] = ...
        hopkinsEnv(Hopp.Npoints, Hopp.xlim1, Hopp.xlim2, Hopp.ylim1, Hopp.ylim2, Hopp.m,Hopp.N, xH, Hopp.Nsimul);
    if nargin>10
        writedata(xH, HNMax, Hopp,  [filename '_envelope_Max'], ['Hopkins statistics - Maximum of ' num2str(Hopp.Nsimul) ' simulations of Poisson process']);
        writedata(xH, HNMin, Hopp,  [filename '_envelope_Min'], ['Hopkins statistics - Mainimum of ' num2str(Hopp.Nsimul) ' simulations of Poisson process'])
    end
else
    HNMin = [];
    HNMax = [];
end

if nargin>10
    writedata(xH, H, Hopp,  filename, 'Hopkins statistics')
    save (filename)
end

function [HhistMin, HhistMax, xHhist] = hopkinsEnv(Npoints, xlim1, xlim2, ylim1, ylim2, m,N, bins, Nsimul)
% function [HhistMin, HhistMax, xHhist] = hopkinsEnv(Npoints, xlim1, xlim2,
% ylim1, ylim2, m,N, bins, Nsimul)

h = waitbar(0,'Computing Hopkins Envelope...');
for ii=1 : Nsimul
    waitbar(ii/Nsimul,h)
    simNoise = generateNoise(Npoints, [xlim1,xlim2,ylim1,ylim2]);
    for jj=1 : N
        H(jj) = hopkins (simNoise,xlim1, xlim2, ylim1, ylim2, m);
    end
    [Hhist, xHhist] = hist(H, bins);
    Hhist = Hhist/trapz(xHhist, Hhist); % normalization
    if ii==1
        HhistMin = Hhist;
        HhistMax = Hhist;
    else
        HhistMin = min(HhistMin, Hhist);
        HhistMax = max(HhistMax, Hhist);
    end
end
close (h)


%% meshCleave
% Below is a demonstration of the features of the |meshCleave| function

%%
clear; close all; clc;

%% Syntax
% |[logicAt]=meshCleave(E,V);|
% |[logicAt]=meshCleave(E,V,P,n);|
% |[logicAt,logicAbove,logicBelow]=meshCleave(E,V,P,n,inclusiveSwitch);|
% |[logicAt,logicAbove,logicBelow]=meshCleave(E,V,P,n,inclusiveSwitch);|

%% Description
% This function creates logic arrays for the mesh components (e.g. elements
% or faces) which are at, above, or below a plane defined by the point P,
% and the normal direction n.
% The optional inclusiveSwitch is a 2-component vector (default [0 0]) and
% sets how "inclusive", the below/above logic is, i.e. they set wether <
% and > is used ([0 0]), or <= and >= are used ([1 1]). A combination may
% also be used e.g. [1 0] results in below checks which features <= and
% above checks using >.

%% Examples
clear; close all; clc;

%% Example: Cleaving a surface mesh

%%
% Create example patch data
[F,V]=stanford_bunny;

%%

n=[0 0 1]; %Normal direction to plane
P=mean(V,1); %Point on plane
[logicAt,logicAbove,logicBelow]=meshCleave(F,V,P,n);

%%
% Visualize
cFigure;
subplot(1,3,1); hold on;
title('At cleave plane');
gpatch(F,V,'w','none',0.2);
gpatch(F(logicAt,:),V,'bw','k',1);
camlight headlight;
axisGeom;

subplot(1,3,2); hold on;
title('Above cleave plane');
gpatch(F,V,'w','none',0.2);
gpatch(F(logicAbove,:),V,'bw','k',1);
camlight headlight;
axisGeom;

subplot(1,3,3); hold on;
title('Below cleave plane');
gpatch(F,V,'w','none',0.2);
gpatch(F(logicBelow,:),V,'bw','k',1);
camlight headlight;
axisGeom;

gdrawnow;

%% Example 2: Cleaving a tetrahedral mesh

%%
% Creating an example tetrahedral mesh
boxDim=[5 5 5]; % Box dimenstions
pointSpacing=0.25;
[meshStruct]=tetMeshBox(boxDim,pointSpacing);
E=meshStruct.elements;
V=meshStruct.nodes;
F=meshStruct.facesBoundary;
VE=patchCentre(E,V);
C=minDist(VE,mean(VE,1));

%%

n=[0 0 1]; %Normal direction to plane
P=mean(V,1); %Point on plane
[logicAt,logicAbove,logicBelow]=meshCleave(E,V,P,n);

% Get faces and matching color data for visualization
[F_cleave,CF_cleave]=element2patch(E(logicAt,:),C(logicAt));
[F_above,CF_above]=element2patch(E(logicAbove,:),C(logicAbove));
[F_below,CF_below]=element2patch(E(logicBelow,:),C(logicBelow));

%%
% Visualize
cFigure;
subplot(1,3,1); hold on;
title('At cleave plane');
gpatch(F,V,'w','none',0.2);
gpatch(F_cleave,V,CF_cleave,'k',1);
camlight headlight;
axisGeom;

subplot(1,3,2); hold on;
title('Above cleave plane');
gpatch(F,V,'w','none',0.2);
gpatch(F_above,V,CF_above,'k',1);
camlight headlight;
axisGeom;

subplot(1,3,3); hold on;
title('Below cleave plane');
gpatch(F,V,'w','none',0.2);
gpatch(F_below,V,CF_below,'k',1);
camlight headlight;
axisGeom;

gdrawnow;

%%
% Visualizing cleaving operation for varying angles

hf=cFigure; hold on;
gpatch(F,V,'w','none',0.2);
hp1=gpatch(F_cleave,V,CF_cleave,'k',1);
axisGeom; axis manual;
camlight headligth;
gdrawnow;

nSteps=50; %Number of animation steps

%Create the time vector
animStruct.Time=linspace(0,1,nSteps);

%The vector lengths
a=linspace(0,2*pi,nSteps);
b=linspace(0,2*pi,nSteps);
for q=1:1:nSteps
    R=euler2DCM([a(q) b(q) 0]);
    nn=n*R;

    logicAt=meshCleave(E,V,P,nn,[1 0]);

    % Get faces and matching color data for cleaves elements
    [F_cleave,CF_cleave]=element2patch(E(logicAt,:),C(logicAt));

    %Set entries in animation structure
    animStruct.Handles{q}=[hp1 hp1]; %Handles of objects to animate
    animStruct.Props{q}={'Faces','CData'}; %Properties of objects to animate
    animStruct.Set{q}={F_cleave,CF_cleave}; %Property values for to set in order to animate
end
anim8(hf,animStruct);

%% Example 3: Cleaving a hexahedral mesh

boxDim=[5 5 5]; % Box dimenstions
boxEl=[20 20 20];
[meshStruct]=hexMeshBox(boxDim,boxEl,2);

E=meshStruct.elements;
V=meshStruct.nodes;
F=meshStruct.facesBoundary;
VE=patchCentre(E,V);
C=minDist(VE,mean(VE,1));

%%

n=[0 0 1]; %Normal direction to plane
P=mean(V,1); %Point on plane
inclusiveSwitch=[1 0];
[logicAt,logicAbove,logicBelow]=meshCleave(E,V,P,n,inclusiveSwitch);

logicPlot=logicAt;

% Get faces and matching color data for visualization
[F_cleave,CF_cleave]=element2patch(E(logicAt,:),C(logicAt));
[F_above,CF_above]=element2patch(E(logicAbove,:),C(logicAbove));
[F_below,CF_below]=element2patch(E(logicBelow,:),C(logicBelow));

%%
% Visualize
cFigure;
subplot(1,3,1); hold on;
title('At cleave plane');
gpatch(F,V,'w','none',0.2);
gpatch(F_cleave,V,CF_cleave,'k',1);
camlight headlight;
axisGeom;

subplot(1,3,2); hold on;
title('Above cleave plane');
gpatch(F,V,'w','none',0.2);
gpatch(F_above,V,CF_above,'k',1);
camlight headlight;
axisGeom;

subplot(1,3,3); hold on;
title('Below cleave plane');
gpatch(F,V,'w','none',0.2);
gpatch(F_below,V,CF_below,'k',1);
camlight headlight;
axisGeom;

gdrawnow;

%%
% Visualizing slicing operation for varying angles

hf=cFigure; hold on;
gpatch(F,V,'w','none',0.2);
hp1=gpatch(F_cleave,V,CF_cleave,'k',1);
axisGeom; axis manual;
camlight headligth;
gdrawnow;

nSteps=50; %Number of animation steps

%Create the time vector
animStruct.Time=linspace(0,1,nSteps);

%The vector lengths
a=linspace(0,2*pi,nSteps);
b=linspace(0,2*pi,nSteps);
for q=1:1:nSteps
    R=euler2DCM([a(q) b(q) 0]);
    nn=n*R;

    logicAt=meshCleave(E,V,P,nn,inclusiveSwitch);

    % Get faces and matching color data for cleaves elements
    [F_cleave,CF_cleave]=element2patch(E(logicAt,:),C(logicAt));

    %Set entries in animation structure
    animStruct.Handles{q}=[hp1 hp1]; %Handles of objects to animate
    animStruct.Props{q}={'Faces','CData'}; %Properties of objects to animate
    animStruct.Set{q}={F_cleave,CF_cleave}; %Property values for to set in order to animate
end
anim8(hf,animStruct);

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