function [all] = VBA_classification(X,y,k,verbose,options,sparse)
% performs binary classification using VBA
% function [pv,stat,df,all] = VBA_classification(X,y,k,verbose,options)
% In brief, this function fits the following logistic regresion model:
%   p(y=1) = s(X*beta)
% where y is the binary data, s is the standard sigmoid mapping, X is the
% design matrix and beta are unknown weight parameters.
% Statistical testing is performed using cross-validation scheme, such that
% the p-value computes the probability of the test score under the null H0.
% In cross-validation, the test score is the binary accuracy outcome
% sampled across test sets. The null distribution accounts for potential
% imbalance in the data in that the first-order moment of the corresponding
% binomial distribution is specified in terms of the sample mean of the
% data. VBA_classification uses a k-fold cross-validation scheme, which
% involves partitioning the data into k subsamples of equal size, each of
% which is used as the test set in turn.
% Note: VBA_classification also performs a fully Bayesian model inversion
% on the entire dataset (full-data inversion).
% IN:
%   - X: nXp design matrix
%   - y: nX1 binary data matrix
%   - k: the number of folds for the k-fold cross-validation scheme. If k
%   is set to n (default), then VBA_classification uses a leave-one-out
%   scheme. If k is set to 0, then the cross-validation scheme is entirely
%   skipped (only the full-data inversion is performed).
%   - verbose: flag for displaying results (default is 0)
%   - options: structure containing VBA's options (can be used for passing,
%   e.g., priors on classification weight parameters)
%   - sparse: when sparse=1, VBA_classification uses sparsifying priors
%   (default=0).
% OUT:
%   - all: structure array with fields:
%       .stat: structure of summary statistics:
%           .pv: classical p-value on classifier accuracy
%           .success: nx1 vector of successful classifications
%           .pa: cross-validation prediction accuracy
%           .bpa: balanced prediction accuracy
%           .pBayes: Bayesian exceedance prob. on prediction accuracy
%       .P: pxk matrix of estimated classification weights
%       .r: data imbalance (r=0.5 means balanced data)
%       .in: structure storing the inputs to VBA_classification
%       .date: date stamp
%       .dt: computing time
%       .handles: a structure containing the handles of the graphical
%       objects (filled-in by VBA_classication_display.m)
% Note: cross-validation results can be displayed using the following
% command line: VBA_classification_display(all).

% fill in default I/O
tStart = tic;
all = [];
[n,m] = size(y);
[n0,p] = size(X);

try,k;catch,k=n;end
try,verbose;catch,verbose=0;end
try % check whether VBA's options structure has been sepecified
    options;
catch
    options = [];
end
try % use sparsify tansform?
    sparse = ~~sparse(1);
catch
    sparse = 0;
end

% check basic numerical requirements
try
    if VBA_isWeird (y)
        disp('Error: data contains weird values!')
        return
    end
end
if ~ VBA_isBinary (y)
    disp('Error: data should be binary!')
    return
end
if ~isequal(n,n0)
    disp('Error: design matrix has to have as many rows as the data matrix!')
    return
end
if ~isequal(m,1)
    disp('Error: data should be a nx1 vector!')
    return
end
if k>n || k<0
    disp('Warning: number of folds reduced to data length!')
    k = n;
end

% randomly re-order the data (for k-fold partitioning)
if k ~= n
    io = randperm(n);
    y = y(io);
    X = X(io,:);
end

% specify options and priors for VBA inversion
dim.p = n;
dim.n_t = 1;
dim.n_phi = p;
dim.n_theta = 0;
dim.n = 0;
g_fname = @g_classif0;
options.sources = struct('type',1,'out',1);
options.DisplayWin = 0;
options.verbose = 0;
options.inG.X = X';
options.inG.sparse = sparse;
options.n0 = 0; % number of dummy counts

if ~isequal(k,0) % performing cross-validation scheme
    if verbose
        disp(' ')
        fprintf(1,'Performing k-folds cross-validation scheme...')
        fprintf(1,'%6.2f %%',0)
        et0 = clock; % get time
    end
    sizeFolds = floor(n./k);
    acc = zeros(n,1); % test accuracy
    acc0 = zeros(n,1); % training accuracy
    Eg = zeros(n,1); % out-of-sample prediction E[y] (on test data)
    Vg = zeros(n,1); % out-of-sample prediction V[y] (on test data)
    P = zeros(p,k);
    for i=1:k
        if i<k
            itest = (i-1)*sizeFolds+1:i*sizeFolds;
        else % last test fold contains all remaining data
            itest = (i-1)*sizeFolds+1:n;
        end
        options.isYout = zeros(n,1);
        options.isYout(itest) = 1;
        [posterior,out] = VBA_NLStateSpaceModel(y,[],[],g_fname,dim,options);
        Eg(itest) = out.suffStat.gx(itest);
        Vg(itest) = out.suffStat.vy(itest);
        ytest = Eg(itest)>=0.5;
        acc(itest) = [y(itest)==ytest];
        acc0(itest) = out.fit.acc;
        if sparse
            P(:,i) = VBA_sparsifyPrior (posterior.muPhi);
        else
            P(:,i) = posterior.muPhi;
        end
        if verbose
            fprintf(1,repmat('\b',1,8))
            fprintf(1,'%6.2f %%',floor(100*i/k))
        end
    end
    if verbose
        fprintf(1,repmat('\b',1,8))
        fprintf(1,[' OK (took ',num2str(etime(clock,et0)),' seconds).'])
        fprintf(1,'\n')
    end
end

% performing full-data inversion
if verbose
    fprintf(1,'Performing whole-data inversion...')
    et0 = clock; % get time
end
options.isYout = zeros(n,1);
[posterior,out] = VBA_NLStateSpaceModel(y,[],[],g_fname,dim,options);
if verbose
    fprintf(1,[' OK (took ',num2str(etime(clock,et0)),' seconds).'])
    fprintf(1,'\n')
end

% wrap-up
all.in.X = X;
all.in.y = y;
all.in.k = k;
all.in.sparse = sparse;
all.date = clock;
all.dt = toc(tStart);
all.posterior = posterior;
all.out = out;
if ~isequal(k,0)
    r = mean(y);
    if r<0.5
        r = 1-r;
    end
    nok = sum(acc);
    [pdf0,cdf0] = VBA_binomial(nok,n,r);
    all.stat.pv = 1 - cdf0; % classical p-value on classifier accuracy
    all.stat.success = acc; % nx1 vector of successful classifications
    all.stat.pa = nok./n; % cross-validation prediction accuracy
    all.stat.bpa = 0.5*(sum(acc.*y)./sum(y) + sum(acc.*(1-y))./sum(1-y)); % balanced class. acc.
    all.stat.pBayes = VBA_PPM(nok+options.n0,n-nok+options.n0,r,'beta',0); % Bayesian exceedance prob.
    all.Eg = Eg;
    all.Vg = Vg;
    all.P = P; % set of estimated weights (for each k-fold)
    all.r = r; % data imbalance (r=0.5 means balanced data)
    if verbose
        [all] = VBA_classification_display(all);
    end
end


%% This file is the PDE simulation file of Belousov-Zhabotinsky reaction.
% Coded By: K
% Last Updated: 2019/06/24
%%
function [rhs,x_t,z_t,s_t,u_t,x,z,s,u,x_x,z_x,s_x,u_x,x_y,z_y,s_y,u_y,x_xx,z_xx,s_xx,u_xx,x_yy,z_yy,s_yy,u_yy,x_lap,z_lap,s_lap,u_lap]=...
    BZ_Reaction_PDE(t,xzsut,Kx,Kxx,Ky,Kyy,K22,n,N,Dx,Dz,Ds,Du,q,f,ksi,alpha,beta,gama,ksi2,ksi3,phi,NeedDev)

% Calculate u and v terms
xt=reshape((xzsut(1:N)),n,n);
zt=reshape((xzsut((N+1):(2*N))),n,n);
st=reshape((xzsut(2*N+1:3*N)),n,n);
ut=reshape((xzsut((3*N+1):(4*N))),n,n);

x=real(ifft2(xt));
z=real(ifft2(zt));
s=real(ifft2(st));
u=real(ifft2(ut));

% Reaction Terms
xtrhs=reshape((fft2(  (1/ksi)*( f.*z.*(q-x)./(q+x) + x - x.^2 - beta*x + s )  )),N,1);
ztrhs=reshape((fft2(  x - z - alpha*z + gama*u  )),N,1);
strhs=reshape((fft2(  (1/ksi2)*(beta*x - s + phi*u )  )),N,1);
utrhs=reshape((fft2(  (1/ksi3)*(alpha*z - gama*u )  )),N,1);

rhs=[-(Dx/Dx)*K22.*xzsut(1:N)+xtrhs
     -(Dz/Du)*K22.*xzsut(N+1:2*N)+ztrhs
     -(Ds/Du)*K22.*xzsut(2*N+1:3*N)+strhs
     -(Du/Du)*K22.*xzsut(3*N+1:4*N)+utrhs
     ];

 % If you don't need to extract the value, don't run the following code to
 % speed up
if NeedDev==1
    % Get the derivative you want
    x_x=real(ifft2(reshape((1j*Kx).*xzsut(1:N),n,n)));
    z_x=real(ifft2(reshape((1j*Kx).*xzsut(N+1:2*N),n,n)));
    s_x=real(ifft2(reshape((1j*Kx).*xzsut(2*N+1:3*N),n,n)));
    u_x=real(ifft2(reshape((1j*Kx).*xzsut(3*N+1:4*N),n,n)));
    %
    x_y=real(ifft2(reshape((1j*Ky).*xzsut(1:N),n,n)));
    z_y=real(ifft2(reshape((1j*Ky).*xzsut(N+1:2*N),n,n)));
    s_y=real(ifft2(reshape((1j*Ky).*xzsut(2*N+1:3*N),n,n)));
    u_y=real(ifft2(reshape((1j*Ky).*xzsut(3*N+1:4*N),n,n)));
    %
    x_xx=real(ifft2(reshape((1j*Kxx).^2.*xzsut(1:N),n,n)));
    z_xx=real(ifft2(reshape((1j*Kxx).^2.*xzsut(N+1:2*N),n,n)));
    s_xx=real(ifft2(reshape((1j*Kxx).^2.*xzsut(2*N+1:3*N),n,n)));
    u_xx=real(ifft2(reshape((1j*Kxx).^2.*xzsut(3*N+1:4*N),n,n)));
    %
    x_yy=real(ifft2(reshape((1j*Kyy).^2.*xzsut(1:N),n,n)));
    z_yy=real(ifft2(reshape((1j*Kyy).^2.*xzsut(N+1:2*N),n,n)));
    s_yy=real(ifft2(reshape((1j*Kyy).^2.*xzsut(2*N+1:3*N),n,n)));
    u_yy=real(ifft2(reshape((1j*Kyy).^2.*xzsut(3*N+1:4*N),n,n)));
    %
    x_t=real(ifft2(reshape(rhs(1:N),n,n)));
    z_t=real(ifft2(reshape(rhs(N+1:2*N),n,n)));
    s_t=real(ifft2(reshape(rhs(2*N+1:3*N),n,n)));
    u_t=real(ifft2(reshape(rhs(3*N+1:4*N),n,n)));
    %
    x_lap=real(ifft2(reshape(-K22.*xzsut(1:N),n,n)));
    z_lap=real(ifft2(reshape(-K22.*xzsut(N+1:2*N),n,n)));
    s_lap=real(ifft2(reshape(-K22.*xzsut(2*N+1:3*N),n,n)));
    u_lap=real(ifft2(reshape(-K22.*xzsut(3*N+1:4*N),n,n)));

end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% Tutorial for FAIR: midpoint quadrature rule for 2D spline
%
%==============================================================================

clear, close all, help(mfilename)

omega = [0,6,0,8]; I = 36; T = zeros(6,8); T(3,4) = 1; h = []; Q = [];
psi = @(xc) splineInter(T,omega,xc);
for j=1:10,
  m    = 2^j*[1,1];
  h(j) = prod((omega(2:2:end)-omega(1:2:end))./m);
  xc   = getCellCenteredGrid(omega,m);
  Q(j) = h(j)*sum(psi(xc));
end;
figure(1); clf; p1=semilogx(h/h(1),Q+eps,'kx',h/h(1),Q,'k-');
figure(2); clf; p2=loglog(h/h(1),abs(I-Q)+eps,'kx',h/h(1),abs(I-Q),'k-');
%==============================================================================


function noise = heavisideNoiseParamInit(noise, y)

% HEAVISIDENOISEPARAMINIT Heaviside classification model's parameter initialisation.

% IVM

if nargin > 1
  nClass1 = sum(y==1);
  nClass2 = sum(y==-1);
  noise.bias = invCumGaussian(nClass1./(nClass2+nClass1));
  noise.numProcess = size(y, 2);
else
  noise.bias = zeros(1, noise.numProcess);
end

noise.eta = 0.01;
noise.nParams = length(noise.bias) + length(noise.eta);


function K = cotmatrix3(V,T)
  % COTMATRIX3 computes cotangent matrix for 3D tetmeshes, area/mass terms
  % already cancelled out: laplacian mesh operator
  %
  % This is distinctly NOT following definition that
  % appears in the appendix of: ``Interactive Topology-aware Surface
  % Reconstruction,'' by Sharf, A. et al
  % http://www.cs.bgu.ac.il/~asharf/Projects/InSuRe/Insure_siggraph_final.pdf
  %
  % Instead it is a purely geometric construction. Find more details in Section
  % 1.1 of "Algorithms and Interfaces for Real-Time Deformation of 2D and 3D
  % shapes" [Jacobson 2013]
  %
  % ND derivation given in "A MONOTONE FINITE ELEMENT SCHEME FOR
  % CONVECTION-DIFFUSION EQUATIONS" [Xu & ZIKATANOV 1999]
  %
  % 3D derivation given in "Aspects of unstructured grids and finite-volume
  % solvers for the Euler and Navier-Stokes equations" [Barth 1992]
  %
  % K = cotmatrix(V,T)
  % Inputs:
  %   V  #V x 3 matrix of vertex coordinates
  %   T  #T x 4  matrix of indices of tetrahedral corners
  % Output:
  %   K  #V x #V matrix of cot weights
  %
  % Copyright 2011, Alec Jacobson (jacobson@inf.ethz.ch)
  %
  % See also cotmatrix
  %
  warning('Deprecated. Call cotmatrix directly.');
  K = cotmatrix(V,T);
end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni

% Discrete Fourier Transform  properties

% symmetry

x=[1 2 3 4 5];
Xk=dft(x)

k=0:4
N=5
R=Xk(1+mod(N-k,N))
Xnk=conj(R)


function sF = unimodal(varargin)
% defines a unimodal spherical function
%
% Syntax
%
%   sF = S2Fun.unimodal
%   sF = S2Fun.unimodal('halfwidth',10*degree)
%
%   v = vector3d(1,1,1)
%   psi = S2DeLaValleePoussinKernel('halfwidth',20*degree)
%   sF = S2Fun.unimodal(v,psi)
%
% Input
%  v - symmetry axis @vector3d
%  psi - @S2Kernel
%
% Output
%  sF - @S2FunHarmonic
%

% extract kernel
psi = S2DeLaValleePoussinKernel('halfwidth',get_option(varargin,'halfwidth',25*degree));
psi = getClass(varargin,'S2Kernel',psi);

% define a radially symmetric function
bw = psi.bandwidth;

% 1 3 5  7  9 11 13
% 1 3 7 13 21 31 43

% the indice of diagonal
l = 0:bw; l = 1+l.^2+l;

f_hat = zeros((2*bw+1)^2,1);
f_hat(l) = psi.A ./ sqrt(2*l+1);

sF = S2FunHarmonic(f_hat);

v = getClass(varargin,'vector3d',vector3d.Z);

if angle(v,vector3d.Z) > 0
  rot = rotation.map(v,vector3d.Z);
  sF = rot * sF;
end

end


function [u,dudx,dudp] = u_Fourier(x,P,t,in)
% input function for free-form (Fourier decomposition) deterministic DCM
% function [u,dudx,dudp] = u_Fourier(x,P,t,in)
% IN:
%   - x: [useless]
%   - P: nW*nuX1 vector of Fourier projection parameters, where nW is the
%   number of Fourier basis functions and nu is the dimensionality of the
%   output
%   - t: time at which the Fourier basis set is evaluated
%   - in: user-defined structure containing the set of Fourier frequencies
% OUT:
%   - u: the nuXnt matrix of outputs, where nt is the length of the
%   argument t.
%   - dudx: [useless]
%   - dudp: gradient wrt the parameters

% construct Fourier basis at time t
X = zeros(length(in.W),length(t));
for i = 1:length(t)
    X(:,i) = cos(in.W(:).*pi.*t(i)./in.T);
end

% project basis onto input space
P = reshape(P,length(in.W),[]);
u = (X'*P)';

% get gradients
dudx = [];
dudp = kron(eye(size(P,2)),X);


clear
        % Memorarea timpului pornirii programului
t0=clock;
		% Prealocarea vectorului y
y=zeros(50001,1);
		% Initializarea unui contor de ciclu
n=0;
		% Executarea ciclului for
for t=0:0.001:50
	n=n+1;
	y(n)=sin(t);
end
		% Calcularea timpului parcurs de la
		% pornirea programului
durata=etime(clock,t0)


%% Apparently it must be here!
% Written by Ali Akbar Eftekhari
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or
% without modification, are permitted provided that the following
% conditions are met:
%
%     *   Redistributions of source code must retain the above copyright notice,
%         this list of conditions and the following disclaimer.
%     *   Redistributions in binary form must reproduce the above
%         copyright notice, this list of conditions and the following
%         disclaimer in the documentation and/or other materials provided
%         with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
% THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
% PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
% CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
% EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
% PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
% OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
% NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
% SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

clc; clear; close all;
%% create a mesh and visualize it
% The first part of this demo shows you how to create a mesh and visualize
% it. The aim of the visualization is to introduce you to the mesh
% structure.
% Here, we are going to create and visualize a 1D uniform equidistant mesh
% and show the cell centers with a 'o' marker and the face of each cell
% with a '+' marker.
% We start by defining the length of the domain:
L = 1.0; % length of the domain
%%
% Then we define the number of cells in the domain:
%
Nx = 10; % number of cells in the domain
%%
% Now we call createMesh1D, one of the functions from a group of createMesh*
% functions:
%
m = createMesh1D(Nx, L);
%%
% This function generates a structure which keeps the domain and grid
% information, which will be used by almost every other function in the
% FVMtool.
%
% In general, 1D, 2D, 3D, 1D radial (axial symetry), and 2D cylindrical
% grids can be constructed. For more information, type
%%
%
%   help createMesh2D
%   help createMesh3D
%   help createMeshCylindrical1D
%   help createMeshCylindrical2D
%
%%
% Now, let's have a look at the structure of the created mesh:
%
disp(m);
%%
% You can easily get more information about this structure by typing
%%
%   help createMesh1D
%
%%
% As an example, we use the position of the cell centers to visualize the
% domain, the cells, and the interfaces between them:
%
figure(2);
plot(m.cellcenters.x, ones(size(m.cellcenters.x)), 'or', ...
     m.facecenters.x, ones(size(m.facecenters.x)), '-+b');
legend('cell centers', 'face centers');
title('Visualization of a 1D discretized domain');
%%
% The generated figure shows a 1D domain of length 1.0 [unit], discretized
% into 10 cells of the same size (dx = 1/10).
% I'm going to assume that you are familiar with the cell-centred finite
% volume method, discretization, and especially handling the boundary
% conditions using the ghost cells.
% You can also generate and visualize 2D and 3D meshes. To find some
% examples, type:
%
%   help createMesh2D;
%   help createMesh3D;
%
% You will end up with the following two simple examples:
%
Nx = 5;
Ny = 7;
Lx = 10;
Ly = 20;
m = createMesh2D(Nx, Ny, Lx, Ly);
[X, Y] = ndgrid(m.cellcenters.x, m.cellcenters.y);
[Xf,Yf]=ndgrid(m.facecenters.x, m.facecenters.y);
figure(3);
plot(X, Y, 'or', ...
     Xf, Yf, '-b', Xf', Yf', '-b');
%%
% that shows you a 2D grid, and
%
Nx = 2;
Lx = 1.0;
Ny = 3;
Ly = 2.0;
Nz = 4;
Lz = 3.0;
m = createMesh3D(Nx, Ny, Nz, Lx, Ly, Lz);
[X, Y, Z] = ndgrid(m.cellcenters.x, m.cellcenters.y, m.cellcenters.z);
[Xf, Yf, Zf] = ndgrid(m.facecenters.x, m.facecenters.y, m.facecenters.z);
figure(4);
plot3(X(:), Y(:), Z(:), 'or')
hold on;
plot3(Xf(:), Yf(:), Zf(:), '+b')
legend('cell centers', 'cell corners');
hold off;
%%
% that shows a 3D grid.
%% Boundary condition structure
% I had so many reasons to write this toy toolbox, of which, the most
% important one was to be able to implement different boundary conditions
% in the most convenient way! My final implementation makes the user able
% to define either a _periodic boundary condition_ or a _general boundary
% condition_ of the following form:
%%
%
% $$a (\nabla \phi .\mathbf{n}) + b \phi = c $$
%
%%
% In the above equation, $\phi$ is the unknown, and _a_, _b_, and _c_ are
% constants. In practice, this boundary condition equation will be discretized
% to the following system of algebraic equations:
%%
%
% $$M_{bc} \phi = {RHS}_{bc}$$
%
%%
% By adjusting the values of _a_ and _b_, onne can easily define
% one of the following well-known boundary conditions:
%%
%
% * Neumann (_a_ is nonzero; _b_ is 0)
% * Dirichlet (_a_ is zero; _b_ is nonzero)
% * Robin (_a_ and _b_ are both nonzero)
%
%%
% To clarify the above explanations, let us create a boundary condition
% structure for a 1D mesh.
%
Nx = 10; % number of cells in the domain
Lx = 1.0; % length of the domain
m = createMesh1D(Nx, Lx); % createMesh and createMesh are identical
BC = createBC(m); % creates a boundary condition structure
disp(BC); % display the BC structure
%%
% The BC structure has two substructures, i.e., _left_ that denotes the boundary
% at the left side of the domain (at x=0), and _right_ that denotes the
% boundary at the end of the domain (at x=Lx). Each of these substructures
% have three fields, i.e., _a_, _b_, and _c_. The default values are _a_=1,
% _b_=0, and _c_=0:
%
disp(BC.left); % show the values of the coefficients for the left boundary
%%
% There is one other field, i.e., _periodic_, which has a zero value. If
% you change it to one, then a periodic boundary condition will be created
% and the _a_, _b_, and _c_ values will be ignored.
% clearly, you can change the above boundary condition by assigning new
% values to the |BC.left| fields. For instance, you can define a Dirichlet
%  boundary (i.e., fixed value) with a value of 2.5 by typing
%
BC.left.a = 0;
BC.left.b = 1;
BC.left.c = 2.5;
%%
% You can define a periodic boundary condition simply by writing:
%
BC.left.periodic = 1;
%%
% For boundary condition structures created for 2D and 3D grids, we will
% have _left_, _right_, _bottom_, _top_, _back_, and _front_ boundaries
% and thus substructures. Let me show them to you in action:
%
m = createMesh2D(3,4, 1.0, 2.0);
BC = createBC(m);
disp(BC);
disp(BC.top);
%%
% Yes, that's right. _a_, _b_, and _c_ are vectors. It means that you can
% have different boundary conditions for
% different cell faces at each boundary. For instance, I can have a Neumann
% boundary condition for the first cell and a Dirichlet boundary condition
% for the last cell at the top boundary:
BC.top.a(1) = 1; BC.top.b(1) = 0; BC.top.c(1) = 0; % zero value Neumann
BC.top.a(end) =0; BC.top.b(end)=1; BC.top.c(end) = 0; % zero value Dirichlet
disp('top.  a     b     c'); % some fancy display!
disp('   ---------------');
disp([BC.top.a BC.top.b BC.top.c]);
%%
% The same procedure can be followed for a 3D grid. However, _a_, _b_, and
% _c_ values are 2D matrices for a 3D grid. I will discuss it in more
% details when we reach the practical examples.
% *Important note:* If you need to assign a boundary condition to the
% entire boundary, use (:) in your assignment. For instance, to define a
% Dirichlet boundary for the right boundary, you may write
BC.right.a(:)=0; BC.right.b(:)=1; BC.right.c(:)=0;
%% Solve a diffusion equation
% As the first example, we solve a steady-state diffusion equation of the
% following formform
%
% $$\nabla.\left(-D\nabla c\right)=0,$$
%
%%
% where _D_ is the diffusivity and _c_ is the concentration. Let me assume
% that we have a 1D domain, with Dirichlet boundary conditions at both
% boundaries, i.e.,
% at x=0, c=1; and at x=L, c=0.
% First of all, we need to define our domain, discretize it, and define the
% boundaries at the borders.
clc; clear; % clear the screen and memory
L = 0.01; % a 1 cm domain
Nx = 10; % number of cells
m = createMesh3D(Nx,Nx,Nx,L, L,L); % create the mesh
BC = createBC(m); % construct the BC structure (Neumann by default)
%%
% Now as you may remeber, we have to switch from Neumann to Dirichlet
% boundary conditions
%
BC.left.a(:) = 0; BC.left.b(:) = 1; BC.left.c(:) = 1; % Left boundary to Dirichlet
BC.right.a(:) = 0; BC.right.b(:) = 1; BC.left.c(:) = 1; % right boundary to Dirichlet
%%
% The next sep is to define the diffusivity coefficient. In this FVTool,
% the physical properties of the domain are defined for each cell, with the
% function createCellVariable
D = createCellVariable(m, 1e-5); % assign a constant value of 1e-5 to diffusivity value on each cell
%%
% However, the transfer coefficients must be known on the face of each cell.
% For this reason, we have a few averaging schemes implemented in the
% Utilities folder. For a 1D domain, we can use a harmonic mean scheme:
D_face = harmonicMean(D); % average diffusivity value on the cell faces
%%
% Now, we can convert the PDE to a algebraic system of linear equations,
% i.e.,
%%
%
% $$\nabla.\left(-D\nabla c\right) \approx Mc = 0$$
%
%%
% where M is the matrix of coefficient that is going to be calculated using
% this toolbox. The matrix of coefficient, _M_ has two parts. The diffusion
% equation and the boundary conditions. They are calculated by:
M_diff = diffusionTerm(D_face); % matrix of coefficients for diffusion term
[M_bc, RHS_bc] = boundaryCondition(BC); % matrix of coefficient and RHS vector for the boundary condition
%%
% A vector of right hand side values are always obtained during the
% discretization of the boundary conditions.
% Now that the PDE is discretized, we can solve it by a Matlab linear solver.
c = solvePDE(m, M_diff+M_bc, RHS_bc);
%%
% finally, the resut can be visualized:
visualizeCells(c);
%%
% Just to get excited a little bit, only change the mesh definition command
% from createMesh1D(Nx,L) to createMesh2D(Nx,Nx,L,L), run the code and see
% what happens. For even more excitement, change it to
% createMesh3D(Nx,Nx,Nx,L,L,L).
% This is usually the way we develop new mathematical models for a physical
% phenomenon. Write the equation, solve it in 1D, compare it to the
% analytical solution, then solve it numerically in 2D and 3D for more
% realistic cases with heterogeneous transfer coefficients and other
% nonidealities (and perhaps compare it to some experimental data)
%% Solve a convection-diffuison equation and compare it to analytical solution
% Here, I'm going to add a convection term to what we solved in the
% previous example. This tutorial is adopted from the fipy
% convection-diffusion example you can find at this address:
%%
% <http://www.ctcms.nist.gov/fipy/examples/convection/index.html>
%%
% The differential equation reads
%%
%
% $$\nabla.\left(\mathbf{u} \phi -D\nabla \phi \right)=0$$
%
%%
% Here, $\mathbf{u}$ is a velocity vector (face variable) and $D$ is the
% diffusion coefficient (again a face variable). Please see the PDF
% document for an explanation of cell and face variables. We use Dirichlet
% (constant value) boundary conditions on the left and right boundaries.
% It is zero at the left boundary and one at the right boundary. The
% analytical solution of this differential equation reads
%%
%
% $$c = \frac{1-exp(ux/D)}{1-exp(uL/D)}$$
%
%%
% We start the code as always with some cleaning up:
clc; clear;
%%
% Then we define the domain and mesh size:
L = 1;  % domain length
Nx = 25; % number of cells
meshstruct = createMesh1D(Nx, L);
x = meshstruct.cellcenters.x; % extract the cell center positions
%%
% The next step is to define the boundary condition:
BC = createBC(meshstruct); % all Neumann boundary condition structure
BC.left.a = 0; BC.left.b=1; % switch the left boundary to Dirichlet
BC.left.c=0; % value = 0 at the left boundary
BC.right.a = 0; BC.right.b=1; % switch the right boundary to Dirichlet
BC.right.c=1; % value = 1 at the right boundary
%%
% Now we define the transfer coefficients:
D_val = 1.0; % diffusion coefficient value
D = createCellVariable(meshstruct, D_val); % assign dif. coef. to all the cells
Dave = harmonicMean(D); % convert a cell variable to face variable
u = -10; % velocity value
u_face = createFaceVariable(meshstruct, u); % assign velocity value to cell faces
%%
% Now we discretize the differential equation into a system of linear
% algebraic equations:
%%
%
% $$(M_{conv}-M_{diff}+M_{bc})\phi={RHS}_{bc}$$
%
%%
% or if we use an upwind discretization scheme, we will obtain:
%%
%
% $$(M_{conv,uw}-M_{diff}+M_{bc})\phi={RHS}_{bc}$$
%
%%
%
Mconv =  convectionTerm(u_face); % convection term, central, second order
Mconvupwind = convectionUpwindTerm(u_face); % convection term, upwind, first order
Mdiff = diffusionTerm(Dave); % diffusion term
[Mbc, RHSbc] = boundaryCondition(BC); % boundary condition discretization
M = Mconv-Mdiff+Mbc; % matrix of coefficient for central scheme
Mupwind = Mconvupwind-Mdiff+Mbc; % matrix of coefficient for upwind scheme
RHS = RHSbc; % right hand side vector
c = solvePDE(meshstruct, M, RHS); % solve for the central scheme
c_upwind = solvePDE(meshstruct, Mupwind, RHS); % solve for the upwind scheme
c_analytical = (1-exp(u*x/D_val))/(1-exp(u*L/D_val)); % analytical solution
figure(5);
plot(x, c.value(2:Nx+1), x, c_upwind.value(2:Nx+1), '--',...
    x, c_analytical, '.');
legend('central', 'upwind', 'analytical');
%%
% As you see here, we obtain a more accurate result by using a central
% difference discretization scheme for the convection term compared to the
% first order upwind.
%% solve a transient diffusion equation
% This tutorial is adapted from the fipy 1D diffusion example
%%
% <http://www.ctcms.nist.gov/fipy/examples/diffusion/index.html FiPy diffusion tutorial>
% The transient diffusion equation reads
%%
%
% $$\alpha\frac{\partial c}{\partial t}+\nabla.\left(-D\nabla c\right)=0,$$
%
% where $c$ is the independent variable (concentration, temperature, etc)
% , $D$ is the diffusion coefficient, and $\alpha$ is a constant.
%%
% Once again, clean up:
clc; clear;
%%
% Define the domain and create a mesh structure
L = 50;  % domain length
Nx = 20; % number of cells
m = createMesh1D(Nx, L);
x = m.cellcenters.x; % cell centers position
%%
% Create the boundary condition structure:
BC = createBC(m); % all Neumann boundary condition structure
%%
% Switch the left and right boundaries to Dirichlet:
BC.left.a = 0; BC.left.b=1; BC.left.c=1; % left boundary
BC.right.a = 0; BC.right.b=1; BC.right.c=0; % right boundary
%%
% Define the transfer coefficients:
D_val = 1;
D = createCellVariable(m, D_val);
Dave = harmonicMean(D); % convert it to face variables
% Define alfa, the coefficient of the transient term:
alfa_val = 1;
alfa = createCellVariable(m, alfa_val);
%%
% Define the initial values:
c_init = 0;
c_old = createCellVariable(m, c_init, BC); % initial values
c = c_old; % assign the old value of the cells to the current values
%%
% Now define the time step and the final time:
dt = 0.1; % time step
final_t = 100;
%%
% Here, we first define the matrices of coefficients that will not change
% as we progress in time, viz. diffusion term and boundary condition:
Mdiff = diffusionTerm(Dave);
[Mbc, RHSbc] = boundaryCondition(BC);
%%
% The transitionTerm function gives a matrix of coefficient and a RHS
% vector. The matrix of coefficient does not change in each time step, but
% the RHS does (see the PDF documents). Therefore, we need to call the
% function inside the time loop.
% Start the loop here:
for t=dt:dt:final_t
    [M_trans, RHS_trans] = transientTerm(c_old, dt, alfa);
    M = M_trans-Mdiff+Mbc;
    RHS = RHS_trans+RHSbc;
    c = solvePDE(m,M, RHS);
    c_analytical = 1-erf(x/(2*sqrt(D_val*t)));
    c_old = c;
end
%%
% Now visualize the final results
figure(6)
plot(x, c.value(2:Nx+1), 'o', x, c_analytical);
xlabel('Length [m]'); ylabel('c');
legend('Numerical', 'Analytical');
%%
% you can visualize the results from each time step by moving the plot line
% inside the for loop.
%% convection equations; different discretization schemes
% If I want to highlight one special feature of this FVTool, I will point
% a finger on its various discretization schemes for a linear convection
% term, which includes central difference (second order), upwind (first
% order), and TVD scheme with various flux limiters.
%%
% Here, we are going to compare the performance of each scheme for solving
% two PDE's.
% First, a simple linear transient convection equation with an strange initial
% condition and later, we solve the well-known Burger's equation.
clc; clear;
% define a 1D domain and mesh
W = 1;
Nx = 500;
mesh1 = createMesh1D(Nx, W);
x = mesh1.cellcenters.x;
% define the boundaries
BC = createBC(mesh1); % all Neumann
BC.left.periodic=1;
BC.right.periodic =1;
% Initial values
phi_old = createCellVariable(mesh1, 0.0, BC);
phi_old.value(20:120) = 1;
phi_old.value(180:400)= sin(x(180:400)*10*pi());
% initial guess for phi
phi = phi_old;
phiuw_old=phi_old;
% initial values for upwind scheme
phiuw = phi;
% keep the initial values for visualization
phiinit=phi_old;
% velocity field
u = 0.3;
uf = createFaceVariable(mesh1, u);
% diffusion field
D = 1e-2;
Df = createFaceVariable(mesh1, D);
% transient term coefficient
alfa = createCellVariable(mesh1,1.0);
% upwind convection term
Mconvuw = convectionUpwindTerm1D(uf);
% define the BC term
[Mbc, RHSbc] = boundaryCondition(BC);
% choose a flux limiter
FL = fluxLimiter('Superbee');
% solver
dt = 0.001; % time step
final_t = W/u;
t = 0;
while t<final_t
    t = t+dt;
    % inner loop for TVD scheme
    for j = 1:5
        [Mt, RHSt] = transientTerm(phi_old, dt, alfa);
        [Mconv, RHSconv] = convectionTvdTerm1D(uf, phi, FL);
        M = Mconv+Mt+Mbc;
        RHS = RHSt+RHSbc+RHSconv;
        phi = solvePDE(mesh1, M, RHS);
    end
    [Mtuw, RHStuw] = transientTerm(phiuw_old, dt, alfa);
    Muw = Mconvuw+Mtuw+Mbc;
    RHSuw = RHStuw+RHSbc;
    phiuw = solvePDE(mesh1, Muw, RHSuw);
    phiuw_old = phiuw;
    phi_old = phi;
end
figure(7);plot(x, phiinit.value(2:Nx+1), x, phi.value(2:Nx+1), '-o', x, ...
        phiuw.value(2:Nx+1));

% %% method of lines: using Matlab's ODE solvers for adaptive time stepping
%
%
%
% %% solving a nonlinear PDE
%
% %% solving a system of linear PDE's: sequential and coupled methods
%
% %% solving a system of nonlinear PDE's: sequential and coupled olutions
%
% %% Real life cases: water-flooding in the production of oil
%
% %% and finally your examples?
%
%
%


%System model for geodetic navigation frame mechanization for both phi and psi parametrization.

%This script can be used for both quaternion and DCM attitude parametrizations.

%Position errors are represented as delta_r^n (position error with respect to
%earth defined in navigation frame) in meters rather than
%Delta-Llh (llh perturbation model see:sys_llh_phipsi) or Delta-Theta (Cen perturbation: to be added later)

%Delta_r^n, delta_llh and Delta_theta are assumed to be convertible to each
%other. That is why this system model can be used for any of these position
%mechanizations.

%There is no difference between the psi form of this script and the "sys_wander_psi". This is because
%theta_z does not appear anywhere in the error propagation model for the
%psi model. (Ccn*dv^c is assumed to be equal to dv^c in position error model
%and theta*g is independent of theta_z). Therefore, you can use
%sys_wander_psi instead of this script. (Also note that sys_wander_psi is a
%nonsingular implementation, whereas this script is singular at the pole as geoparam is called with arg 2)

%Returns F and Q of discrete time model. The IMU error model must be
%specified here. (Note that in the previous implementations I had seperated
%nav and imu models in different m-files. I no longer follow that approach)

%mode==1 --> PHI model
%mode==2 --> PSI model

%imutype is directly transferred to imu_err_defs_v000

function [STM Qd]=sys_metric_phipsi(Cen, h, Vn, att, acc, gyro, dt, mode, imutype, modelNo)

%%%Navigation error model
%1-3:Position error in meters
%4-6:vel errors
%7-9:Attitude errors in psi form

%N[da:dw]':Effect of IMU errors on nav states
if(nargin<10)
    modelNo=3; % random walk bias and scale factor
end
Anav=zeros(9); %pos, vel, attitude
Nnav=zeros(9,6);

[Fc, wen_n, wie_n, g]=geoparam_v001(2, Cen(:,3), h, Vn);

if (size(att,1)==1 || size(att,2)==1) %att is qbn
    Cbn=quat2dcm_v000(att);
else %att is dcm
    Cbn=att;
end

%%%Navigation System model
%%Common terms for both phi and psi
%position errors
Anav(1:3,1:3)=-skew(wen_n);
Anav(1:3,4:6)=eye(3);

%Velocity Errors
Anav(6,3)=2*g*Fc(1,1);  %effect of height on gravity (note that, drz is the negative of dh. Therefore, this is positive)
Anav(4:6,4:6)=-skew(wen_n+2*wie_n);
Anav(4:6,7:9)=skew(Cbn*acc);

Nnav(4:6,1:3)=Cbn; %%effect of accelerometer errors

%Attitude errors
Anav(7:9,7:9)=-skew(wen_n+ wie_n);
Nnav(7:9,4:6)=-Cbn; %Effect of gyroscope errors

%PHI model
if (mode==1)
    sL=-Cen(3,3);
    cL=Cen(3,1);    %Note that I assume wander=0 for Cen. (This is not a non-singular implementation)
    %%position errors
    Anav(1:3,1:2)=Anav(1:3,1:2)-skew(Vn)*[0 Fc(1,1);-Fc(2,2) 0; 0 Fc(3,1)];

    %%Velocity errors
    %Effect of 2*d_wie_n
    Anav(4:6,1)=Anav(4:6,1)+2*skew(Vn)*[wie_n(3);0;-wie_n(1)]*Fc(2,2);
    %Effect of d_w_en_n
    Anav(4:6,3:5)=Anav(4:6,3:5)+skew(Vn)*[Vn(2)*Fc(1,1)^2 0 Fc(1,1);-Vn(1)*Fc(2,2)^2 -Fc(2,2) 0;-Vn(2)*Fc(1,1)^2*sL/cL 0 Fc(3,1)];
    Anav(4:6,1)=Anav(4:6,1)+skew(Vn)*[0;0;-(Vn(2)*Fc(1,1)/cL/cL)]*Fc(2,2);

    %%Attitude errors
    %effect of dw_ie_n
    Anav(7:9,1)=Anav(7:9,1)+[wie_n(3);0;-wie_n(1)]*Fc(2,2);

    %effect of dw_en_n
    Anav(7:9,3:5)=Anav(7:9,3:5)+[-Vn(2)*Fc(1,1)^2 0 Fc(1,1);Vn(1)*Fc(2,2)^2 -Fc(2,2) 0;Vn(2)*Fc(1,1)^2*sL/cL 0 Fc(3,1)];
    Anav(9,1)=Anav(9,1)-(Vn(2)*Fc(1,1)/cL/cL)*Fc(2,2);
end

%PSI model
if (mode==2)
    %Velocity errors
    Anav(4:5,1:2)=Anav(4:5,1:2)+g*[-Fc(2,2) 0;0 -Fc(1,1)];    %effect of horizontal position error on gravity
end

%%%%Imu error model parameters
[Aimu_d, Qimu_d, Cimu, Rimu]=imu_err_model_v001(acc, gyro, dt, imutype, modelNo);

%%Convert continuous time Anav into discrete time
%I used van loan's method here to perform conversion so that I can obtain a
%correct Qnav_d. If you use a 1st order taylor series approximation, do not
%forget to correct the cross-correlation effects caused by arw/vrw. (In,
%the final implementation, it would be better to manually compute each
%element rather than using this method)
mx_a=dt*[-Anav,Nnav*Rimu*Nnav';zeros(9),Anav'];
mx_b = expm(mx_a);
Anav_d = mx_b(10:18,10:18)';
Qnav_d = Anav_d*mx_b(1:9,10:18);

%%Combine everything
nst_imu=size(Aimu_d);
STM=zeros(9+nst_imu);
Qd=zeros(9+nst_imu);

STM(1:9,1:9)=Anav_d;
STM(1:9,10:end)=Nnav*Cimu*dt;
STM(10:end,10:end)=Aimu_d;

Qd(1:9,1:9)=Qnav_d;
Qd(10:end,10:end)=Qimu_d;
Qd(1:9,10:end)=Nnav*Cimu*Qimu_d*dt/2;   %not necessary, but let's keep this
Qd(10:end,1:9)=Qd(1:9,10:end)';
end


function mean = normal_01_mean ( )

%*****************************************************************************80
%
%% NORMAL_01_MEAN returns the mean of the Normal 01 PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real MEAN, the mean of the PDF.
%
  mean = 0.0;

  return
end


function [nodes2, edges2] = grMergeNodesMedian(nodes, edges, mnodes)
%GRMERGENODESMEDIAN Replace several nodes by their median coordinate.
%
%   [NODES2, EDGES2] = grMergeNodesMedian(NODES, EDGES, NODES2MERGE)
%   NODES ans EDGES are the graph structure, and NODES2MERGE is the list of
%   indices of nodes to be merged.
%   The median coordinate of merged nodes is computed, and all nodes are
%   merged to this new node.
%
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2003-08-13
% Copyright 2003-2022 INRA - TPV URPOI - BIA IMASTE

% coordinates of reference node
x = median(nodes(mnodes, 1));
y = median(nodes(mnodes, 2));

% index of reference node
refNode = findPoint([x y], nodes);
mnodes = sort(mnodes(mnodes ~= refNode));

for n = 1:length(mnodes)
    node = mnodes(n);

    % process each neighbor of the current node
    neighbors = grAdjacentNodes(edges, node);
    for e = 1:length(neighbors)
        edge = neighbors(e);

        if edges(edge, 1) == refNode || edges(edge, 2) == refNode
            continue;
        end

        % find if the node is referenced as 1 or 2 in the edge,
        % and replace it with the reference node.
        if edges(edge, 1) == node
            edges(edge, 1) = refNode;
        else
            edges(edge, 2) = refNode;
        end

    end
end

% remove nodes from the list, except the reference node.
for n = 1:length(mnodes)
    [nodes, edges] = grRemoveNode(nodes, edges, mnodes(n)-n+1);
end

nodes2 = nodes;
edges2 = edges;


function [vertices, faces] = triangulatePolygonPair(poly1, poly2, varargin)
%TRIANGULATEPOLYGONPAIR Compute triangulation between a pair of 3D closed curves.
%
%   [V, F] = triangulatePolygonPair(POLY1, POLY2)
%
%   [V, F] = triangulatePolygonPair(..., 'recenter', FLAG)
%   Where FLAG is a boolean, specifies whether the second curve should be
%   translated to have the same centroid as the first curve. This can
%   improve mathcing of vertices. Default is true.
%
%
%   Example
%     % triangulate a surface patch between two ellipses
%     % create two sample curves
%     poly1 = ellipseToPolygon([50 50 40 20 0], 36);
%     poly2 = ellipseToPolygon([50 50 40 20 60], 36);
%     poly1 = poly1(1:end-1,:);
%     poly2 = poly2(1:end-1,:);
%     % transform to 3D polygons / curves
%     curve1 = [poly1 10*ones(size(poly1, 1), 1)];
%     curve2 = [poly2 20*ones(size(poly2, 1), 1)];
%     % draw as 3D curves
%     figure(1); clf; hold on;
%     drawPolygon3d(curve1, 'b'); drawPoint3d(curve1, 'bo');
%     drawPolygon3d(curve2, 'g'); drawPoint3d(curve2, 'go');
%     view(3); axis equal;
%     [vertices, faces] = triangulatePolygonPair(curve1, curve2);
%     % display the resulting mesh
%     figure(2); clf; hold on;
%     drawMesh(vertices, faces);
%     drawPolygon3d(curve1, 'color', 'b', 'linewidth', 2);
%     drawPolygon3d(curve2, 'color', 'g', 'linewidth', 2);
%     view(3); axis equal;
%
%   See also
%     meshes3D, triangulatePolygonPair3d, triangulateCurvePair,
%     meshSurfaceArea
%

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2017-05-18, using Matlab 9.1.0.441655 (R2016b)
% Copyright 2017-2022 INRA - Cepia Software Platform

%% Settings

recenterFlag = true;
while length(varargin) > 1
    pname = varargin{1};
    if strcmpi(pname, 'recenter')
        recenterFlag = varargin{2};
    else
        error('Unknown parameter name: %s', pname);
    end
    varargin(1:2) = [];
end

%% Memory allocation

% concatenate vertex coordinates for creating mesh
vertices = [poly1 ; poly2];

% number of vertices on each polygon
n1 = size(poly1, 1);
n2 = size(poly2, 1);

% allocate the array of facets (each edge of each polygon provides a facet)
nFaces = n1 + n2;
faces = zeros(nFaces, 3);

% Translate the second polygon such that the centroids of the bounding
% boxes coincide. This is expected to improve the matching of the two
% curves.
if recenterFlag
    box1 = boundingBox3d(poly1);
    box2 = boundingBox3d(poly2);
    center1 = (box1(2:2:end) + box1(1:2:end-1)) / 2;
    center2 = (box2(2:2:end) + box2(1:2:end-1)) / 2;
    vecTrans = center1 - center2;
    trans = createTranslation3d(vecTrans);
    poly2 = transformPoint3d(poly2, trans);
end

%% Init iteration

% find the pair of points with smallest distance.
% This will be the current diagonal.
[dists, inds] = minDistancePoints(poly1, poly2);
[dummy, ind1] = min(dists); %#ok<ASGLU>
ind2 = inds(ind1);

% consider two consecutive vertices on each polygon
currentIndex1 = ind1;
currentIndex2 = ind2;

%% Main iteration
% For each diagonal, consider the two possible facets (one for each 'next'
% vertex on each polygon), each create current facet according to the
% closest one.
% Then update current diagonal for next iteration.

for iFace = 1:nFaces
    nextIndex1 = mod(currentIndex1, n1) + 1;
    nextIndex2 = mod(currentIndex2, n2) + 1;

    % compute lengths of diagonals
    dist1 = distancePoints(poly1(currentIndex1, :), poly2(nextIndex2,:));
    dist2 = distancePoints(poly1(nextIndex1, :), poly2(currentIndex2,:));

    if dist1 < dist2
        % keep current vertex of curve1, use next vertex on curve2
        face = [currentIndex1 currentIndex2+n1 nextIndex2+n1];
        currentIndex2 = nextIndex2;
    else
        % keep current vertex of curve2, use next vertex on curve1
        face = [currentIndex1 currentIndex2+n1 nextIndex1];
        currentIndex1 = nextIndex1;
    end

    % create the facet
    faces(iFace, :) = face;
end


function [ x, c, m ] = dvandprg ( n, alpha, b, x, c, m )

%*****************************************************************************80
%
%% DVANDPRG solves a Vandermonde system A' * x = f progressively.
%
%  Discussion:
%
%    This function receives the solution to the system of equations A' * x = f
%    where A is a Vandermonde matrix for alpha(0) through alpha(n-1),
%    and new values alpha(n) and f(n).  It updates the solution.
%
%    To solve a system of Nbig equations, this function may be called repeatedly,
%    with N = 1, 2, ..., Nbig.  Each time, a solution to the current subsystem
%    is returned.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 April 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Ake Bjorck, Victor Pereyra,
%    Solution of Vandermonde Systems of Equations,
%    Mathematics of Computation,
%    Volume 24, Number 112, October 1970, pages 893-903.
%
%  Parameters:
%
%    Input, integer N, the new order of the matrix, which is 1 larger
%    than on the previous call.  For the first call, N must be 1.
%
%    Input, real ALPHA(N), the parameters that define the matrix.
%    The values should be distinct.  The value ALPHA(N) has just been
%    added to the system.
%
%    Input, real B(N), the right hand side of the linear system.
%
%    Input, real X(N-1), the previous solution of the linear system.
%
%    Input, real C(N-1), real M(N-1), factorization data from the previous call.
%
%    Output, real X(N), the updated solution to the linear system.
%
%    Output, real C(N), M(N), updated factorization data.
%
  c(n) = b(n);
  for j = n - 1 : -1 : 1
    c(j) = ( c(j+1) - c(j) ) / ( alpha(n) - alpha(j) );
  end

  if ( n == 1 )
    m(n) = 1.0;
  else
    m(n) = 0.0;
  end
  cn = c(1);
  x(n) = c(1);

  for j = n - 1 : -1 : 1
    m(j+1) = m(j+1) - alpha(n-1) * m(j);
    x(n-j) = x(n-j) + m(j+1) * cn;
  end

  return
end


function [ triangle_num, triangle_node, triangle_neighbor ] = r8tris2 ( ...
  node_num, node_xy )

%*****************************************************************************80
%
%% R8TRIS2 constructs a Delaunay triangulation of 2D vertices.
%
%  Discussion:
%
%    The routine constructs the Delaunay triangulation of a set of 2D vertices
%    using an incremental approach and diagonal edge swaps.  Vertices are
%    first sorted in lexicographically increasing (X,Y) order, and
%    then are inserted one at a time from outside the convex hull.
%
%  Modified:
%
%    07 February 2005
%
%  Author:
%
%    Original FORTRAN77 version by Barry Joe,
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Barry Joe,
%    GEOMPACK - a software package for the generation of meshes
%    using geometric algorithms,
%    Advances in Engineering Software,
%    Volume 13, pages 325-331, 1991.
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, real NODE_XY(2,NODE_NUM), the coordinates of the nodes.
%
%    Output, integer TRIANGLE_NUM, the number of triangles in the triangulation;
%    TRIANGLE_NUM is equal to 2*NODE_NUM - BOUNDARY_NUM - 2, where BOUNDARY_NUM
%    is the number of boundary vertices.
%
%    Output, integer TRIANGLE_NODE(3,TRIANGLE_NUM), the nodes that make up each triangle.
%    The elements are indices of P.  The vertices of the triangles are
%    in counter clockwise order.
%
%    Output, integer TRIANGLE_NEIGHBOR(3,TRIANGLE_NUM), the triangle neighbor list.
%    Positive elements are indices of TIL; negative elements are used for links
%    of a counter clockwise linked list of boundary edges; LINK = -(3*I + J-1)
%    where I, J = triangle, edge index; TRIANGLE_NEIGHBOR(J,I) refers to
%    the neighbor along edge from vertex J to J+1 (mod 3).
%
  triangle_num = 0;
  triangle_node = [];
  triangle_neighbor = [];

  tol = 100.0 * eps;
%
%  Sort the vertices by increasing (x,y).
%
  indx = r82vec_sort_heap_index_a ( node_num, node_xy );

  node_xy = r82vec_permute ( node_num, node_xy, indx );
%
%  Make sure that the data points are "reasonably" distinct.
%
  m1 = 1;

  for i = 2 : node_num

    m = m1;
    m1 = i;

    k = 0;

    for j = 1 : 2

      cmax = max ( abs ( node_xy(j,m) ), abs ( node_xy(j,m1) ) );

      if ( tol * ( cmax + 1.0 ) < abs ( node_xy(j,m) - node_xy(j,m1) ) )
        k = j;
        break
      end

    end

    if ( k == 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R8TRIS2 - Fatal error!\n' );
      fprintf ( 1, '  Fails for point number I = %d\n', i );
      fprintf ( 1, '  M = %d\n', m );
      fprintf ( 1, '  M1 = %d\n', m1 );
      fprintf ( 1, '  X,Y(M)  = %f  %f\n', node_xy(1,m), node_xy(2,m) );
      fprintf ( 1, '  X,Y(M1) = %f  %f\n', node_xy(1,m1), node_xy(2,m1) );
      error ( 'R8TRIS2 - Fatal error!' )
      return
    end

  end
%
%  Starting from points M1 and M2, search for a third point M that
%  makes a "healthy" triangle (M1,M2,M)
%
  m1 = 1;
  m2 = 2;
  j = 3;

  while ( 1 )

    if ( node_num < j )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R8TRIS2 - Fatal error!\n' );
      error ( 'R8TRIS2 - Fatal error!' )
      return
    end

    m = j;

    lr = lrline ( node_xy(1,m), node_xy(2,m), node_xy(1,m1), node_xy(2,m1), ...
      node_xy(1,m2), node_xy(2,m2), 0.0 );

    if ( lr ~= 0 )
      break
    end

    j = j + 1;

  end
%
%  Set up the triangle information for (M1,M2,M), and for any other
%  triangles you created because points were collinear with M1, M2.
%
  triangle_num = j - 2;

  if ( lr == -1 )

    triangle_node(1,1) = m1;
    triangle_node(2,1) = m2;
    triangle_node(3,1) = m;
    triangle_neighbor(3,1) = -3;

    for i = 2 : triangle_num

      m1 = m2;
      m2 = i+1;
      triangle_node(1,i) = m1;
      triangle_node(2,i) = m2;
      triangle_node(3,i) = m;
      triangle_neighbor(1,i-1) = -3 * i;
      triangle_neighbor(2,i-1) = i;
      triangle_neighbor(3,i) = i - 1;

    end

    triangle_neighbor(1,triangle_num) = -3 * triangle_num - 1;
    triangle_neighbor(2,triangle_num) = -5;
    ledg = 2;
    ltri = triangle_num;

  else

    triangle_node(1,1) = m2;
    triangle_node(2,1) = m1;
    triangle_node(3,1) = m;
    triangle_neighbor(1,1) = -4;

    for i = 2 : triangle_num
      m1 = m2;
      m2 = i+1;
      triangle_node(1,i) = m2;
      triangle_node(2,i) = m1;
      triangle_node(3,i) = m;
      triangle_neighbor(3,i-1) = i;
      triangle_neighbor(1,i) = -3 * i - 3;
      triangle_neighbor(2,i) = i - 1;
    end

    triangle_neighbor(3,triangle_num) = -3 * triangle_num;
    triangle_neighbor(2,1) = -3 * triangle_num - 2;
    ledg = 2;
    ltri = 1;

  end
%
%  Insert the vertices one at a time from outside the convex hull,
%  determine visible boundary edges, and apply diagonal edge swaps until
%  Delaunay triangulation of vertices (so far) is obtained.
%
  top = 0;

  for i = j+1 : node_num

    m = i;
    m1 = triangle_node(ledg,ltri);

    if ( ledg <= 2 )
      m2 = triangle_node(ledg+1,ltri);
    else
      m2 = triangle_node(1,ltri);
    end

    lr = lrline ( node_xy(1,m), node_xy(2,m), node_xy(1,m1), node_xy(2,m1), ...
      node_xy(1,m2), node_xy(2,m2), 0.0 );

    if ( 0 < lr )
      rtri = ltri;
      redg = ledg;
      ltri = 0;
    else
      l = -triangle_neighbor(ledg,ltri);
      rtri = floor ( l / 3 );
      redg = mod(l,3) + 1;
    end

    [ ltri, ledg, rtri, redg ] = vbedg ( node_xy(1,m), node_xy(2,m), node_num, node_xy, ...
      triangle_num, triangle_node, triangle_neighbor, ltri, ledg, rtri, redg );

    n = triangle_num + 1;
    l = -triangle_neighbor(ledg,ltri);

    while ( 1 )

      t = floor ( l / 3 );
      e = mod ( l, 3 ) + 1;
      l = -triangle_neighbor(e,t);
      m2 = triangle_node(e,t);

      if ( e <= 2 )
        m1 = triangle_node(e+1,t);
      else
        m1 = triangle_node(1,t);
      end

      triangle_num = triangle_num + 1;
      triangle_neighbor(e,t) = triangle_num;
      triangle_node(1,triangle_num) = m1;
      triangle_node(2,triangle_num) = m2;
      triangle_node(3,triangle_num) = m;
      triangle_neighbor(1,triangle_num) = t;
      triangle_neighbor(2,triangle_num) = triangle_num - 1;
      triangle_neighbor(3,triangle_num) = triangle_num + 1;
      top = top + 1;

      if ( node_num < top )
        fprintf ( 1, '\n' );
        fprintf ( 1, 'R8TRIS2 - Fatal error!\n' );
        fprintf ( 1, '  Stack overflow.\n' );
        error ( 'R8TRIS2 - Fatal error!' )
      end

      work(top) = triangle_num;

      if ( t == rtri & e == redg )
        break
      end

    end

    triangle_neighbor(ledg,ltri) = -3 * n - 1;
    triangle_neighbor(2,n) = -3 * triangle_num - 2;
    triangle_neighbor(3,triangle_num) = -l;
    ltri = n;
    ledg = 2;

    [ top, ltri, ledg, triangle_node, triangle_neighbor ] = swapec ( ...
      m, top, ltri, ledg, node_num, node_xy, triangle_num, triangle_node, ...
      triangle_neighbor, work );

  end
%
%  Now account for the sorting that we did.
%
  for i = 1 : 3
    for j = 1 : triangle_num
      triangle_node(i,j) = indx ( triangle_node(i,j) );
    end
  end

  indx = perm_inverse ( node_num, indx );

  node_xy = r82vec_permute ( node_num, node_xy, indx );

  return
end


function y = nanvar(x,w,dim)
%NANVAR Variance, ignoring NaNs.
%   Y = NANVAR(X) returns the sample variance of the values in X, treating
%   NaNs as missing values.  For a vector input, Y is the variance of the
%   non-NaN elements of X.  For a matrix input, Y is a row vector
%   containing the variance of the non-NaN elements in each column of X.
%   For N-D arrays, NANVAR operates along the first non-singleton dimension
%   of X.
%
%   NANVAR normalizes Y by N-1 if N>1, where N is the sample size of the
%   non-NaN elements.  This is an unbiased estimator of the variance of the
%   population from which X is drawn, as long as X consists of independent,
%   identically distributed samples, and data are missing at random.  For
%   N=1, Y is normalized by N.
%
%   Y = NANVAR(X,1) normalizes by N and produces the second moment of the
%   sample about its mean.  NANVAR(X,0) is the same as NANVAR(X).
%
%   Y = NANVAR(X,W) computes the variance using the weight vector W.  The
%   length of W must equal the length of the dimension over which NANVAR
%   operates, and its non-NaN elements must be nonnegative.  Elements of X
%   corresponding to NaN elements of W are ignored.
%
%   Y = NANVAR(X,W,DIM) takes the variance along dimension DIM of X.
%
%   See also VAR, NANSTD, NANMEAN, NANMEDIAN, NANMIN, NANMAX, NANSUM.

%   Copyright 1984-2010 The MathWorks, Inc.
%   $Revision: 1.1.8.2 $  $Date: 2010/10/08 17:25:19 $

if nargin < 2 || isempty(w), w = 0; end

sz = size(x);
if nargin < 3 || isempty(dim)
    % The output size for [] is a special case when DIM is not given.
    if isequal(x,[]), y = NaN(class(x)); return; end

    % Figure out which dimension sum will work along.
    dim = find(sz ~= 1, 1);
    if isempty(dim), dim = 1; end
elseif dim > length(sz)
    sz(end+1:dim) = 1;
end

% Need to tile the mean of X to center it.
tile = ones(size(sz));
tile(dim) = sz(dim);

if isequal(w,0) || isequal(w,1)
    % Count up non-NaNs.
    n = sum(~isnan(x),dim);

    if w == 0
        % The unbiased estimator: divide by (n-1).  Can't do this when
        % n == 0 or 1, so n==1 => we'll return zeros
        denom = max(n-1, 1);
    else
        % The biased estimator: divide by n.
        denom = n; % n==1 => we'll return zeros
    end
    denom(n==0) = NaN; % Make all NaNs return NaN, without a divideByZero warning

    x0 = x - repmat(nanmean(x, dim), tile);
    y = nansum(abs(x0).^2, dim) ./ denom; % abs guarantees a real result

% Weighted variance
elseif numel(w) ~= sz(dim)
    error(message('stats:nanvar:InvalidSizeWgts'));
elseif ~(isvector(w) && all(w(~isnan(w)) >= 0))
    error(message('stats:nanvar:InvalidWgts'));
else
    % Embed W in the right number of dims.  Then replicate it out along the
    % non-working dims to match X's size.
    wresize = ones(size(sz)); wresize(dim) = sz(dim);
    wtile = sz; wtile(dim) = 1;
    w = repmat(reshape(w, wresize), wtile);

    % Count up non-NaNs.
    n = nansum(~isnan(x).*w,dim);

    x0 = x - repmat(nansum(w.*x, dim) ./ n, tile);
    y = nansum(w .* abs(x0).^2, dim) ./ n; % abs guarantees a real result
end


function varargout = drawVector(pos, vect, varargin)
%DRAWVECTOR Draw vector at a given position
%
%   drawVector(POS, VECT)
%   POS should be a N-by-2 or N-by-3 array containing position of vector
%   origins, and VECT should be a N-by-2 or N-by-3 array containing the
%   direction of the vectors.
%
%   Example
%     figure; hold on;
%     drawVector([1 2], [3 2]);
%     drawVector([1 2], [-2 3]);
%     axis equal;
%
%   See also
%     quiver, drawVector3d
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2013-03-18,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2013 INRA - Cepia Software Platform.

% check input dimension
nd = size(pos, 2);
if size(vect, 2) ~= nd
    error('input vector and position must have same dimension');
end

if nd == 2
    % Display 2D vectors
    h = quiver(pos(:, 1), pos(:, 2), vect(:, 1), vect(:, 2), 0, varargin{:});

elseif nd == 3
    % Display 3D vectors
    h = quiver3(pos(:, 1), pos(:, 2), pos(:, 3), ...
        vect(:, 1), vect(:, 2), vect(:, 3), 0, varargin{:});

else
    error('Can not display vectors of dimension > 3');
end

% format output
if nargout > 0
    varargout{1} = h;
end


function [mapping, hs, axes_h, fig_h] = scatter_plot(Y,names,options)
%ENDMEMBER_SCATTER_PLOT Summary of this function goes here
%   Detailed explanation goes here
M = length(names);
Xs = [];
colors = distinguishable_colors(M);
dimension_num = 2;

if nargin > 2 && isstruct(options)
    arg_set = fieldnames(options);
    for i = 1:length(arg_set)
        eval([arg_set{i},'=options.',arg_set{i},';']);
    end
end

[mappedX, mapping] = pca(Y,dimension_num);

fig_h = figure;
hs = cell(0);
if dimension_num == 2
    scatter_h = scatter(mappedX(:,1), mappedX(:,2), 10, '.', ...
        'MarkerEdgeColor', [0.6 0.6 0.6]);
elseif dimension_num == 3
    scatter_h = scatter3(mappedX(:,1), mappedX(:,2), mappedX(:,3), 10, '.', ...
        'MarkerEdgeColor', [0.6 0.6 0.6]);
end

hs(end+1) = {scatter_h};
hold on; axis equal;
axes_h = gca;

for i = 1:length(Xs)
    marker_color = colors(i,:);
    mappedX = (Xs{i} - repmat(mapping.mean, size(Xs{i},1), 1)) * mapping.M;
    if dimension_num == 2
        scatter_h = scatter(axes_h, mappedX(:,1), mappedX(:,2), 5, '+', ...
            'MarkerEdgeColor', marker_color);
    elseif dimension_num == 3
        scatter_h = scatter3(axes_h, mappedX(:,1), mappedX(:,2), ...
            mappedX(:,3), 5, '+', 'MarkerEdgeColor', marker_color);
    end
    hs(end+1) = {scatter_h};
end

% show legend
names = {'Pixels',names{:}};

hs_legend = zeros(1,length(hs));
for j = 1:length(hs)
    hs_legend(j) = hs{j}(1);
end
legend(hs_legend, names,'Location','best');


function [tt,ind_left]=tt_crossl(d,n,fun,ind_right)
%One left-to-right sweep of the TT-cross method.
%   [TT,IND_LEFT]=TT_CROSSL(D,N,FUN,IND_RIGHT) Computes one QR-maxvol sweep
%   of the TT-cross method. The input is the pair (D,N) that determines the
%   size of the tensor, FUN is the function handle to evaluate a particular
%   element of the tensor, i.e., VAL=FUN(IND). To pass parameters, please
%   use anonymous function handles in MATLAB
%
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

if ( numel(n) == 1 )
  n=n*ones(d,1);
end
sz=n;
tt=cell(d,1);
ind_left=cell(d,1); %Computed ind_left
ind_r=ind_right{1};
r1=size(ind_r,2);
ncur=sz(1);
mat=zeros(ncur,r1);
for i=1:ncur
   for s=1:r1
      ind_f=[i,ind_r(:,s)'];
      val=fun(ind_f);
      mat(i,s)=val;
   end
end
[qs,~]=qr(mat,0);
ind=maxvol2(qs);
ind_left{1}=ind;
mat=qs/qs(ind,:);
tt{1}=mat;

for k=2:d-1
     ncur=sz(k);
     ind_l=ind_left{k-1};
     ind_r=ind_right{k};
     r2=size(ind_l,2);
     r3=size(ind_r,2);
     core=zeros(ncur,r2,r3);
     for i=1:ncur
         for s2=1:r2
             for s3=1:r3
                 ind_f=[ind_l(:,s2)',i,ind_r(:,s3)'];
                 val=fun(ind_f);
                 core(i,s2,s3)=val;
             end
         end
     end
     core=permute(core,[2,1,3]); core=reshape(core,[r2*ncur,r3]);
     rnew=min(r2*ncur,r3);
     [qs,rs]=qr(core,0);
     ind=maxvol2(qs);
     ind_old=ind_left{k-1};
     ind_new=zeros(k,rnew);
     ncur=sz(k);
     for s=1:rnew
        f_in=ind(s);
        w1=tt_ind2sub([r2,ncur],f_in);
        rs=w1(1); js=w1(2);
        ind_new(:,s)=[ind_old(:,rs)',js];
     end
     ind_left{k}=ind_new;
     core=qs/qs(ind,:);
     core=reshape(core,[r2,ncur,rnew]); core=permute(core,[2,1,3]);

    tt{k}=core;
end
ncur=sz(d);
ind_l=ind_left{d-1};
r=size(ind_l,2);
mat=zeros(ncur,r);
for j=1:ncur
    for s=1:r
      ind_f=[ind_l(:,s)',j];
      val=fun(ind_f);
      mat(j,s)=val;
    end
end
tt{d}=mat;
tt=tt_tensor(tt);
return
end


function r4vec_uniform_ab_test ( )

%*****************************************************************************80
%
%% R4VEC_UNIFORM_AB_TEST tests  R4VEC_UNIFORM_AB.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 October 2014
%
%  Author:
%
%    John Burkardt
%
  n = 10;
  a = -1.0;
  b = +5.0;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R4VEC_UNIFORM_AB_TEST\n' );
  fprintf ( 1, '  R4VEC_UNIFORM_AB computes a random R4VEC.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  %g <= x <= %g\n', a, b );
  fprintf ( 1, '  Initial seed is %d\n', seed );

  [ v, seed ] = r4vec_uniform_ab ( n, a, b, seed );

  r4vec_print ( n, v, '  Uniform R4VEC:' );

  return
end


function test10
%TEST10 test cs_qr
%
% Example:
%   test10
% See also: testall

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

rand ('state', 0) ;

% f = 185 ;
% f = 449 ;
clf

for trials = 1:100

    m = fix (100 * rand (1)) ;
    n = fix (100 * rand (1)) ;
    d = 0.1 * rand (1) ;
    A = sprandn (m, n, d) ;
    [m n] = size (A) ;
    if (m < n)
        A = A' ;
    end
    [m n] = size (A) ;
    sp = sprank (A) ;
    % if (sp < n)
    %   continue ;
    % end

    Aorig = A ;

    % A = A (:, colamd (A)) ;

    tic ;
    R = qr (A) ;
    t1 = toc ;

    % tic ;
    % [Q,R] = qr (A) ;
    % t1 = toc ;

    [c,h,parent] = symbfact (A, 'col') ;                                    %#ok
    rnz = sum (c) ;                                                         %#ok
    tic ;
    [V2,Beta2,p,R2] = cs_qr (sparse(A)) ;
    t2 = toc ;

    C = A ;
    m2 = size (V2,1) ;
    if (m2 > m)
        C = [A ; sparse(m2-m, n)] ;
    end
    C = C (p,:) ;

    [H1,R1] = myqr (C) ;
    err1 = norm (R1-R2,1) / norm (R1) ;
    disp ('err1 = ') ;
    disp (err1) ;
    % [svd(A) svd(R1) svd(full(R2))]
    s1 = svd (full (A)) ;
    s2 = svd (full (R2)) ;
    if (n > 0)
        err2 = norm (s1 - s2) / s1 (1)  ;
        disp ('err2 = ') ;
        disp (err2) ;
    else
        err2 = 0 ;
    end
    fprintf ('%10.6f %10.6f  cs speedup %8.3f sprank %d vs %d\n', t1, t2, t1/t2, sp, n) ;

    % H2 = full (H2)
    % R2 = full (R2)

    subplot (2,4,1) ; spy (A) ;         title ('A colamd') ;
    subplot (2,4,4) ; spy (Aorig) ;     title ('Aorig') ;
    subplot (2,4,2) ; spy (C) ;         title ('A rperm') ;
    subplot (2,4,5) ; spy (abs(R2)>0) ; title ('spqr R, no zeros') ;
    subplot (2,4,6) ; spy (R) ;         title ('matlab R') ;
    subplot (2,4,7) ; spy (R2) ;        title ('spqr R') ;
    subplot (2,4,8) ; spy (V2) ;        title ('spqr H') ;
    drawnow

    if (err2 > 1e-9)
        error ('!') ;
    end

    if (m2 > m)
        fprintf ('added %d rows, sprank %d n %d\n', m2-m, sp, n) ;
    end
end


classdef ShiftColor < handle

  properties (Access = public)

  end

  methods (Access = public, Static = true)

    function newColor = shiftColorFwd(oldColor)
      oldSize = size(oldColor);
      oldColor = reshape(oldColor,[1 1 3]);
      hh = rgb2hsv(oldColor);
      hh(1) = mod(hh(1)+0.085,1);
      newColor = hsv2rgb(hh);
      newColor = reshape(newColor,oldSize);
    end

    function newColor = shiftColorBkwd(oldColor)
      oldSize = size(oldColor);
      oldColor = reshape(oldColor,[1 1 3]);
      hh = rgb2hsv(oldColor);
      hh(1) = mod(hh(1)-0.065,1);
      newColor = hsv2rgb(hh);
      newColor = reshape(newColor,oldSize);

    end

    function newColor = increaseIntensity(oldColor)
      oldSize = size(oldColor);
      oldColor = reshape(oldColor,[1 1 3]);
      hh = rgb2hsv(oldColor);
      hh(3) = min(hh(3)+0.2,1);
      newColor = hsv2rgb(hh);
      newColor = reshape(newColor,oldSize);

    end

    function newColor = decreaseIntensity(oldColor)
      oldSize = size(oldColor);
      oldColor = reshape(oldColor,[1 1 3]);
      hh = rgb2hsv(oldColor);
      hh(3) = min(hh(3)-0.2,1);
      newColor = hsv2rgb(hh);
      newColor = reshape(newColor,oldSize);
    end

  end
end


function pass = test_innerProduct(pref)

if ( nargin < 1 )
    pref = chebfunpref();
end

singPref = pref;
singPref.blowup = true;

% Seed for random number:
seedRNG(6178);

%% Functions on [-inf inf]:

% Set the domain:
dom = [-Inf Inf];

opf = @(x) 2-exp(-x.^2);
opg = @(x) exp(-x.^2);
f = unbndfun(opf, struct('domain', dom));
g = unbndfun(opg, struct('domain', dom));

I = innerProduct(f, g);
IExact = (sqrt(pi)*(4 - sqrt(2)))/2;
err = abs(I - IExact);
pass(1) = err < 2e7*max(eps*get(f,'vscale'), ...
    eps*get(g,'vscale'));

%% Functions on [a inf]:

% Set the domain:
dom = [1 Inf];

opf = @(x) x;
opg = @(x) exp(-x);
pref = chebfunpref();
f = unbndfun(opf, struct('domain', dom, 'exponents', [0 1]), pref);
g = unbndfun(opg, struct('domain', dom));
warning('off', 'CHEBFUN:UNBNDFUN:sum:slowDecay');
I = innerProduct(f, g);
warning('off', 'CHEBFUN:UNBNDFUN:sum:slowDecay');
IExact = 2*exp(-1);
err = abs(I - IExact);
pass(2) = err < 2e8*max(eps*get(f,'vscale'), ...
    eps*get(g,'vscale'));

%% Functions on [-inf b]:

% Set the domain:
dom = [-Inf -3*pi];

opf = @(x) 1./x;
opg = @(x) 2./x;
f = unbndfun(opf, struct('domain', dom));
g = unbndfun(opg, struct('domain', dom));
I = innerProduct(f, g);
IExact = 2/(3*pi);
err = abs(I - IExact);
pass(3) = err < 1e5*eps*get(f,'vscale');

end


function [atm] = hPa2atm(hPa)
% Convert pressure from hectopascals to atmospheres.
% Chad Greene 2012
atm = hPa*0.000986923;


switch input_sig
    case 1          % 1: synthetic data
        if load_sig
            load matlab
        else
        A_0=randn(n1,data.r)*randn(data.r,n2);  % true low-rank matrix A_0
%         A_max=max(abs(A_0(:)));
        A_max=sqrt(max(n1,n2));
        B_0=((rand(n1,n2)<.5)-.5)*2*A_max;
        B_0(randperm(n1*n2,n1*n2-data.s))=0;    % true sparse matrix B_0
%         X_0=A_0+B_0; Z=X_0;
        Z=A_0+B_0;
%         Z=randn(size(Z));
        end

    case 2          % 2: airport video
        addpath('./video_airport')
        nFrame=200;
        iFrame=1000;
        pathname=['airport',num2str(iFrame),'.bmp'];
        Z1=imread(pathname);
        Z1=double(uint8(round(sum(Z1,3)/3)))/255;
        Z=zeros(size(Z1,1),size(Z1,2),nFrame);
        Z(:,:,1)=Z1;
        for j=1:nFrame-1
            pathname=['airport',num2str(iFrame+j),'.bmp'];
            Z1=imread(pathname);
            Z1=double(uint8(round(sum(Z1,3)/3)))/255;
            Z(:,:,j+1)=Z1;
        end

        [z1,z2,z3]=size(Z);
        n1=z1*z2; n2=z3;
        Z=reshape(Z,n1,n2);

        clear nFrame iFrame pathname Z1

    case 3          % 3: lobby video
        addpath('./video_lobby')
        nFrame=400;
        iFrame=1900;
        pathname=['SwitchLight',num2str(iFrame),'.bmp'];
        Z1=imread(pathname);
        Z1=double(uint8(round(sum(Z1,3)/3)))/255;
        Z=zeros(size(Z1,1),size(Z1,2),nFrame);
        Z(:,:,1)=Z1;
        for j=1:nFrame-1
            pathname=['SwitchLight',num2str(iFrame+j),'.bmp'];
            Z1=imread(pathname);
            Z1=double(uint8(round(sum(Z1,3)/3)))/255;
            Z(:,:,j+1)=Z1;
        end

        [z1,z2,z3]=size(Z);
        n1=z1*z2; n2=z3;
        Z=reshape(Z,n1,n2);

        clear nFrame iFrame pathname Z1
end

% add noise
% noise=.0;
% noise=.05*std(data);
if ~load_sig && noise~=0
    Z=Z+noise*randn(size(Z));                       % Gaussian noise
end


function SDP = chordal_relax_category_registration_v2(problem,varargin)
%% Apply a sparse, chordal, SECOND-order relaxation to category registration
%% Depending on multivariate polynomial package in SPOT
%% residual of v1: b(i) - R * (sum_k c_k ak(i)) - t
%% residual of v2: R*b(i) + t - sum_k c_k ak(i)
%% Heng Yang
%% July 06, 2021

params = inputParser;
params.CaseSensitive = false;

params.addParameter('checkMonomials',true, @(x) islogical(x));
params.addParameter('lambda',0.1, @(x) isscalar(x));

params.parse(varargin{:});

checkMonomials = params.Results.checkMonomials;
lambda         = params.Results.lambda;

fprintf('\n===================================================================')
fprintf('\nApplying Chordal SDP relaxation to category registration problem')
fprintf('\n===================================================================\n')
t0              = tic;

N               = problem.N;
K               = problem.K;
scene           = problem.scene;
shapes          = problem.shapes;
noiseBoundSq    = problem.noiseBoundSq;
tBound          = problem.translationBound;
tBoundSq        = tBound^2; % t'*t <= tBoundSq
cBoundSq        = problem.cBound^2; % should just be 1
barc2           = 1.0;

%% define POP variables
nrPrimalVars    = 9+3+K+N; % rotation: 9, translation: 3, binary: N, shape: K
p               = msspoly('p',nrPrimalVars);
r               = p(1:9);
R               = reshape(r,3,3);
col1 = R(:,1); col2 = R(:,2); col3 = R(:,3);
t               = p(10:12);
c               = p(12+1:12+K);
theta           = p(12+K+1:nrPrimalVars);
x               = [r;t;c];

%% define cost function
shape           = combine_shapes(shapes,c);
residuals       = [];
for i = 1:N
    distance            = R*scene(:,i) + t - shape(:,i);
    residuals           = [residuals; (distance' * distance) / noiseBoundSq];
end
f_cost = lambda * (c'*c); % regularization term
for i = 1:N
    f_cost = [f_cost;(1+theta(i))/2 * residuals(i) + (1-theta(i))/2 * barc2];
end

%% define constraints
h_r  = [1.0-col1'*col1;...
        1.0-col2'*col2;...
        1.0-col3'*col3;... % column unit length
        col1'*col2;...
        col2'*col3;...
        col3'*col1;... % colums orthogonal
        cross(col1,col2) - col3;...
        cross(col2,col3) - col1;...
        cross(col3,col1) - col2]; % columns righthandedness

h_theta = [];
for i = 1:N
    h_theta =[h_theta; 1-theta(i)^2];
end

g_t = tBoundSq - t'*t; % Translation bounded
g_c = [cBoundSq - c'*c;c]; % nonnegative and bounded shape parameters
g_x = [g_t;g_c];

%% Formulate the chordal sparse second-order relaxation
%% the 0-th block [1;x] * [1;x]'
basis0          = [1;x];
n0              = length(basis0);
basis_x0        = get_multiplier_basis(x,basis0,h_r(1));
pop0            = [mykron(basis_x0,h_r);...
                   mykron(basis0,basis0);...
                   f_cost(1)];
[~,degmat,coef_all] = decomp(pop0);
coef_all            = coef_all';
dim_loc0        = length(basis_x0) * length(h_r);
n0delta         = triangle_number(n0);
nterms          = size(degmat,1);
m_mom0          = n0delta - nterms;

assert(m_mom0==0,'The zero-th blk should have 0 moment constraints.')

coef_mom    = coef_all(:,dim_loc0+1:dim_loc0+n0^2);
coef_mom    = coef_mom';
B           = {};
B_normalize = {};

for i = 1:nterms
    [row,~,~]   = find(coef_mom(:,i));
    SDP_coli    = floor((row-1)./n0) + 1;
    SDP_rowi    = mod(row-1,n0) + 1;
    nnz         = length(SDP_rowi);

    Bi          = sparse(SDP_rowi,SDP_coli,ones(nnz,1),n0,n0);
    B{end+1}    = Bi;
    B_normalize{end+1} = Bi/nnz;
end

coef_loc0       = coef_all(:,1:dim_loc0);
A0_local        = {};

for i = 1:dim_loc0
    [rowi,~,vi] = find(coef_loc0(:,i));
    Ai      = sparse(n0,n0);
    for j   = 1:length(rowi)
        Ai  = Ai + vi(j) * B_normalize{rowi(j)};
    end
    A0_local = [A0_local;{Ai}];
end
A0_0    = sparse([1],[1],[1],n0,n0);
% The first block satisfies A0(X0) = b0;
A0      = [{A0_0};A0_local];
b0      = sparse(1,1,1,length(A0),1);

% Now build the cost matrix
coef_cost   = coef_all(:,dim_loc0+n0^2+1);
[row,~,v]   = find(coef_cost);
C           = sparse(n0,n0);
for i = 1:length(row)
    C       = C + v(i) * B_normalize{row(i)};
end

%% the 1-N blocks [1;x;theta(i);theta(i)*x] * [1;x;theta(i);theta(i)*x]'
%% Since there are (2+K) inequality constraints, it will generate (K+3)*N blocks
nrineq          = length(g_x);
Aall            = {};
Asuball         = {};
ball            = [];
Call            = {C};
A0append        = {};
for blkidx = 1:N
    basis       = [1;x;theta(blkidx);theta(blkidx)*x];
    n           = length(basis);
    basis_x     = get_multiplier_basis([x;theta(blkidx)],basis,h_r(1),0);
    basis_theta = get_multiplier_basis([x;theta(blkidx)],basis,h_theta(blkidx),0);
    basis_g     = [1;theta(blkidx)];

    out         = gen_chordal_subblk_catreg_v2(...
        basis,basis_x,h_r,basis_theta,h_theta(blkidx),g_x,basis_g,f_cost(1+blkidx));

    Acell       = out.A;
    Asub        = out.Asub;
    b           = out.b;
    C           = out.C;

    % add constraint that the top-left [1;x]*[1;x]' block is the same as
    % the 0-th block
    A0blk = {};
    for i = 1:n0
        for j = i:n0
            if i == j
                A0i  = sparse(i,j,-1,n0,n0);
                Ai   = sparse(i,j,1,n,n);
            else
                A0i  = sparse([i,j],[j,i],[-0.5,-0.5],n0,n0);
                Ai   = sparse([i,j],[j,i],[0.5,0.5],n,n);
            end
            A0blk    = [A0blk;{A0i}];
            Acell    = [Acell;{Ai}];
        end
    end

    ball             = [ball;b;sparse(n0delta,1)];
    A0append{end+1}  = A0blk;
    Aall{end+1}      = Acell;
    Call             = [Call;C];
    Asuball{end+1}   = Asub;
end

%% Convert to standard SDPT3 format
b           = [b0;ball];
blk         = cell( (nrineq+1)*N+1,2);
blk{1,1}    = 's'; blk{1,2} = n0;
n1          = out.blk{2,2};
n1delta     = triangle_number(n1);
step        = nrineq + 1;
for i = 1:N
    blk{step*i-nrineq+1,1} = 's';
    blk{step*i-nrineq+1,2} = n;
    for j = 1:nrineq
        blk{step*i-nrineq+j+1,1} = 's';
        blk{step*i-nrineq+j+1,2} = n1;
    end
end

A0t     = sparsesvec(blk(1,:),A0);
for i = 1:N
    A0t = [A0t,...
           sparse(n0delta,out.m),...
           sparsesvec(blk(1,:),A0append{i})];
end

ndelta  = triangle_number(n);
At      = {A0t};
for i = 1:N
    Ait = [sparse(ndelta,length(b0)),...
           sparse(ndelta,(i-1)*length(Aall{i})),...
           sparsesvec(blk(step*i-nrineq+1,:),Aall{i}),... % the moment matrix block
           sparse(ndelta,(N-i)*length(Aall{i}))];
    At  = [At;{Ait}];
    for j = 1:nrineq
        Aijt    = [sparse(n1delta,length(b0)),...
                    sparse(n1delta,(i-1)*length(Aall{i})),...
                    sparse(n1delta,out.m_mom+out.m_loc),... % the moment constraint
                    sparse(n1delta,(j-1)*n1delta),...
                    sparsesvec(blk(step*i-nrineq+j+1,:),Asuball{i}{j}),...
                    sparse(n1delta,(nrineq-j)*n1delta),...
                    sparse(n1delta,n0delta),...
                    sparse(n1delta,(N-i)*length(Aall{i}))];

        At  = [At;{Aijt}];
    end
end

SDP.blk = blk;
SDP.At  = At;
SDP.m   = length(b);
SDP.C   = Call;
SDP.b   = b;

tf    = toc(t0);
fprintf('\nDone in %g seconds.\n',tf);
fprintf('===================================================================\n')

%% Convert to Sedumi format
fprintf('Convert to Sedumi format ...\n')
t0    = tic;
sK.s  = [n0];
for i = 1:N
    sK.s        = [sK.s,n,n1*ones(1,nrineq)];
end

A0t     = sparsevec(blk(1,:),A0);
n0sq    = n0^2;
for i = 1:N
    A0t = [A0t,...
           sparse(n0sq,out.m),...
           sparsevec(blk(1,:),A0append{i})];
end

nsq     = n^2;
n1sq    = n1^2;
At      = {A0t};
for i = 1:N
    Ait = [sparse(nsq,length(b0)),...
           sparse(nsq,(i-1)*length(Aall{i})),...
           sparsevec(blk(step*i-nrineq+1,:),Aall{i}),... % the moment matrix block
           sparse(nsq,(N-i)*length(Aall{i}))];
    At  = [At;{Ait}];
    for j = 1:nrineq
        Aijt    = [sparse(n1sq,length(b0)),...
                    sparse(n1sq,(i-1)*length(Aall{i})),...
                    sparse(n1sq,out.m_mom+out.m_loc),... % the moment constraint
                    sparse(n1sq,(j-1)*n1delta),...
                    sparsevec(blk(step*i-nrineq+j+1,:),Asuball{i}{j}),...
                    sparse(n1sq,(nrineq-j)*n1delta),...
                    sparse(n1sq,n0delta),...
                    sparse(n1sq,(N-i)*length(Aall{i}))];

        At  = [At;{Aijt}];
    end
end

sdata.K     = sK;
sdata.At    = cat(1,At{:});
sdata.b     = b;

sc          = [];
for i = 1:length(Call)
    sc      = [sc;sparsevec(blk(i,:),Call(i))];
end
sdata.c     = sc;

SDP.sedumi   = sdata;

tf    = toc(t0);
fprintf('Done in %g seconds.\n',tf);
fprintf('===================================================================\n')

end


function [G1, impact] = sparse_solver(g1,psi,n_v,n_g)
%% NOTE: THIS SOLVER WILL BE UPDATED TO MATCH SCHUR_SOLVER EVENTUALLY
% Solves the rational expectation model with sparse iteration
%    method. You should use this method of the number of stable and
%    unstable solution differ a lot. Otherwise, full matrix
%    decomposition method should be used.
%
% by SeHyoun Ahn, June 2016
%
% PARAMETERS/OUTPUTS:
%     Read attached documentation
%
% SYNTAX:
% [G1, impact] = sparse_solver(g1,psi,n_v,n_g)

%-100 is set to find negative eigenvalues. This value can be changed
[x,v,flag] = eigs(g1,n_g,-100);
impact = real(x*(x(n_v+1:end,:)\psi(n_v+1:end,:)));
G1 = real(x*v*(x(n_v+1:end,:)\[sparse(n_g,n_v),speye(n_g,n_g)]));


classdef nnpdist < nntest
  properties (TestParameter)
    oneToOne = {false, true}
    noRoot = {false, true}
    p = {.5 1 2 3}
    aggregate = {false, true}
  end
  methods (Test)
    function basic(test,oneToOne, noRoot, p, aggregate)
      if aggregate
        % make it smaller to avoid numerical derivative issues with
        % float
        h = 3 ;
        w = 2 ;
      else
        h = 13 ;
        w = 17 ;
      end
      d = 4 ;
      n = 5 ;
      x = test.randn(h,w,d,n) ;
      if oneToOne
        x0 = test.randn(h,w,d,n) ;
      else
        x0 = test.randn(1,1,d,n) ;
      end
      opts = {'noRoot', noRoot, 'aggregate', aggregate} ;

      y = vl_nnpdist(x, x0, p, opts{:}) ;

      % make sure they are not too close in any dimension as this may be a
      % problem for the finite difference dereivatives as one could
      % approach 0 which is not differentiable for some p-norms

      s = abs(bsxfun(@minus, x, x0)) < test.range*1e-1 ;
      x(s) = x(s) + 5*test.range ;

      dzdy = test.rand(size(y)) ;
      dzdx = vl_nnpdist(x,x0,p,dzdy,opts{:}) ;
      test.der(@(x) vl_nnpdist(x,x0,p,opts{:}), x, dzdy, dzdx, test.range * 1e-3) ;
    end
  end
end


function test_ft_timelockstatistics

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_timelockstatistics findcluster clusterstat ft_statistics_montecarlo

%For the case of "chan_time"

% make fake dataset
timelock = cell(1,10);
for idat = 1:10
  timelock{idat}.label = {'chan1','chan2','chan3'};
  timelock{idat}.dimord = 'chan_time';
  timelock{idat}.avg = rand(3,30);
  timelock{idat}.time = 0.1:0.1:3;
  timelock{idat}.cfg = [];
end

% do stats - montecarlo
cfg = [];
neighbours(1).label = 'chan1';
neighbours(1).neighblabel = {'chan2', 'chan3'};
neighbours(2).label = 'chan2';
neighbours(2).neighblabel = {'chan1', 'chan3'};
neighbours(3).label = 'chan3';
neighbours(3).neighblabel = {'chan1', 'chan2'};
cfg.neighbours  = neighbours;
cfg.method      = 'montecarlo';
cfg.statistic   = 'ft_statfun_depsamplesT';
cfg.alpha       = 0.05;
cfg.correctm    = 'cluster';
cfg.clusterstatistic = 'maxsum';
cfg.clusterthreshold = 'parametric';
cfg.numrandomization = 500;
cfg.design = [ones(1,5) ones(1,5).*2; 1:5 1:5;];
cfg.ivar   = 1;
cfg.uvar   = 2;
stat = ft_timelockstatistics(cfg,timelock{:});

% do stats - analytic
cfg = [];
cfg.method      = 'analytic';
cfg.statistic   = 'ft_statfun_depsamplesT';
cfg.alpha       = 0.05;
cfg.design = [ones(1,5) ones(1,5).*2; 1:5 1:5;];
cfg.ivar   = 1;
cfg.uvar   = 2;
stat = ft_timelockstatistics(cfg,timelock{:});

% do stats - analytic
cfg = [];
cfg.method      = 'stats';
cfg.statistic   = 'ttest';
cfg.alpha       = 0.05;
cfg.design = [ones(1,10) ];
stat = ft_timelockstatistics(cfg,timelock{:});


function [gx] = g_rbf(x,P,u,in)

centers = in.centers + exp(P(1));
sig = exp(P(2));

N = length(in.centers);
Xrbf = zeros(length(in.grid),N);
for i=1:N
    Xrbf(:,i) = exp(-0.5*(centers(i)-in.grid).^2./sig);
    Xrbf(:,i) = Xrbf(:,i)./sum(Xrbf(:,i));
end

gx = zeros(N,1);
for i=1:N
    y = Xrbf(:,i);
    X = Xrbf(:,setdiff(1:N,i));
    P0 = eye(length(in.grid)) - X*pinv(X'*X)*X';
    err = y'*P0*y;
    gx(i) = sum(err);
end

gx = gx + P(3);

if ~in.corr
    gx = Xrbf;
end


% Calculate feature vector for a triangle
%

% Feature vector is:
%   centroid_x
%   centroid_y
%   orientation_1
%   orientation_2
%   orientation_3
%   area
function features = triangleFeatures(points)
    features = zeros(6, 1);

    fCentrX = 1;
    fCentrY = 2;
    fOr1 = 3;
    fOr2 = 4;
    fOr3 = 5;
    fArea = 6;

    centr = centroid(points);
    features(fCentrX) = centr(1);
    features(fCentrY) = centr(2);

    line = createLine(centr, points(1, :));
    features(fOr1) = rad2deg(lineAngle(line));

    line = createLine(centr, points(2, :));
    features(fOr2) = rad2deg(lineAngle(line));

    line = createLine(centr, points(3, :));
    features(fOr3) = rad2deg(lineAngle(line));

%     features(fArea) = triangleArea(points(1, :), points(2, :), points(3, :));

    features(6) = points(1, 1);
    features(7) = points(1, 2);
    features(8) = points(2, 1);
    features(9) = points(2, 2);
    features(10) = points(3, 1);
    features(11) = points(3, 2);
end


%Hand-eye calibration
%
%This add on allows to compute the hand to eye calibration based ont he
%calibration toolbox from Jean-Yves Bouguet
%see http://www.vision.caltech.edu/bouguetj/calib_doc/
%more information can be found here:
%http://www.vision.ee.ethz.ch/~cwengert/calibration_toolbox.php
%
%You need to have computed the intrinsic and extrinsic parameters of your
%camera (the extrinsic parameters with respect to the grid's local
%coordinate system). Then you also need to supply the pose / transformation
%of the robot arm / marker with repsect to the robot base / external
%tracking device in the following format:
%Pack into a 4x4xNumber_of_Views Matrix the following data
%Hmarker2world(:,:,i) = [Ri_3x3 ti_3x1;[ 0 0 0 1]]
%with
%i = number of the view,
%Ri_3x3 the rotation matrix
%ti_3x1 the translation vector.
%
%The following parameters can be set (if not set the default values are
%used):
%doShow [default=0]:
%Display the results from the calibration
%doSortHandEyeMovement [default=0]:
%Set this to 1 if you want your views sorted in a way that the interstation
%movement is ideal for hand-eye calibration, see [Tsai]
%HandEyeMethod	[default = 'Tsai']
%Here you specifiy which method you want to use for the
%hand-eye calibration, default is using Tsai's method. See
%ftp://ftp.vision.ee.ethz.ch/publications/proceedings/eth_biwi_00363.pdf
%Possible values are 'Tsai', 'Inria', 'Dual_quaternion', 'Navy'
%Tsai, Inria and Navy give the same results and are usually a bit better
%than the Dual quaternion approach
%
%Christian Wengert
%Computer Vision Laboratory
%ETH Zurich
%Sternwartstrasse 7
%CH-8092 Zurich
%www.vision.ee.ethz.ch/cwengert
%wengert@vision.ee.ethz.ch

%With the doShow flag you can show the results
if(~exist('doShow'))
    doShow = 0;
end

%Tsai states that the inter-station movement should be as large as possible
%for better accuracy. This flag will sort the movements for higher
%accuracy
if(~exist('doSortHandEyeMovement'))
    doSortHandEyeMovement = 0;
end

%If it does, check whether the Hrobot2hand exists
if(~exist('Hmarker2world'))
    disp(['handeye:: No Hmarker2world data available, Hand eye calibration aborted']);
    return
end

%If it does, check whether the Hrobot2hand exists
if(~exist('HandEyeMethod'))
    HandEyeMethod = 'Tsai';
end

%Now go on
disp(['handeye:: Hand-Eye Calibration']);
correctSets = 0;
for i=1:length(active_images)
    if(active_images(i)~=0 && Hmarker2world(1,1,i)~=0) %Make sure its a calibration image and it has tracker info
        %Extract extrinsic parameters
        correctSets = correctSets+1;
        eval(['Rc = Rc_' num2str(active_images(i)) ';']);%%%%%%%%%%%%%%%%%%%%%ACHTUNG
        eval(['Tc = Tc_' num2str(active_images(i)) ';']);
        Hgrid2cam(:,:,correctSets) = inv(([Rc Tc;[ 0 0 0 1]]));
        Hcam2grid(:,:,correctSets) = inv(Hgrid2cam(:,:,correctSets));
        Hm2w(:,:,correctSets) = Hmarker2world(:,:,active_images(i));
    end
end
if(correctSets)
    %Remove bad trackerdata
    badTrackerDataIndex = [];
    for i=1:correctSets
        if(abs(Hm2w(1,1,i))<1e-18) %Its bad
            badTrackerDataIndex = [badTrackerDataIndex;i];
            correctSets = correctSets-1;
        end
    end
    %Init
    Hm2w(:,:,badTrackerDataIndex) = [];
    Hcam2grid(:,:,badTrackerDataIndex) = [];
    Hgrid2cam(:,:,badTrackerDataIndex) = [];
    if(doSortHandEyeMovement)
        index = sortHandEyeMovement(Hm2w);
    else
        index = 1:size(Hm2w,3);
    end
    Hm2w2 = Hm2w(:,:,index);
    Hcam2grid2 = Hcam2grid(:,:,index);
    %Now calibrate
    switch(HandEyeMethod)
        case 'Tsai'
            [Hcam2marker_, err] = TSAIleastSquareCalibration(Hm2w2, Hcam2grid2)
        case 'Inria'
            [Hcam2marker_, err] = inria_calibration(Hm2w, Hcam2grid2);
        case 'Navy'
            [Hcam2marker_, err] = navy_calibration(Hm2w, Hcam2grid2);
        case 'Dual_quaternion'
            [Hcam2marker_, err] = hand_eye_dual_quaternion(Hm2w, Hcam2grid) ;
    end
    %Create the average Hworld2grid, givin an idea where the grid is
    %in the coordinate system of the tracker/robot
    for i=1:correctSets
        Hcam2world_(:,:,i) = Hm2w(:,:,i)*Hcam2marker_;  %Hc2m(:,:,k)
        Hworld2cam_(:,:,i) = inv(Hcam2world_(:,:,i));
        %The above is correct as it gives the same as in my
        %simulation
        Hgrid2world_(:,:,i) = Hcam2world_(:,:,i)*Hcam2grid(:,:,i);
        Hworld2grid_(:,:,i) = inv(Hgrid2world_(:,:,i));
    end
    %Average it, using the algorithm described in ...
    Hgrid2worldAvg = averageTransformation(Hgrid2world_);
    xd = [];

    %BACKPROJECT, this computes and displays the result
    errAvg = [];
    correctSets = 0;
    for i=1:length(active_images)
        if(active_images(i)~=0)
            correctSets  = correctSets + 1;
            if(correctSets<=length(Hm2w))
                err = [];
                eval(['tmp = X_' num2str(active_images(i)) ';'])
                eval(['ALLPTS.X_' num2str(correctSets) ' = X_' num2str(active_images(i)) ';'])
                tmp = [tmp, [[0;0;0],[3;0;0]]];
                eval(['xd_' num2str(correctSets) ' = x_' num2str(active_images(i)) ';']);
                eval(['allpts.xd_' num2str(correctSets) ' = x_' num2str(active_images(i)) ';']);
                Xworld = Hgrid2world_(:,:,correctSets)*[tmp;ones(1,length(tmp))];
                XworldAvg = Hgrid2worldAvg*[tmp;ones(1,length(tmp))];     %
                [P,x] = backprojectDistorted(KK,Hworld2cam_(1:3,1:3,correctSets),Hworld2cam_(1:3,4,correctSets),XworldAvg,kc);

                %Compute error
                eval(['xd = xd_' num2str(correctSets) ';']);
                m = length(xd);
                for j=1:m
                    err(j) = norm(x(:,j)-xd(:,j));
                end
                %per point
                errAvg(correctSets) = sum(err)/m;
                %Store it
                result.error_calib_avg(correctSets) = errAvg(correctSets);
                %GFX
                if(doShow)
                    hold off,figure(n_ima+1),hold on
                    if(isfloat(im))
                        eval(['imshow(I_' num2str(i) '/255);']);,hold on
                    else
                        eval(['imshow(I_' num2str(i) ');']);,hold on
                    end

                    hold on
                    draw2DPoints(x(:,1:m), '', 0, n_ima+1)
                    plot(x(1,end-1:end),x(2,end-1:end),'rx','MarkerSize',25,'LineWidth',3);
                    draw2DPoints(xd, 'blue(real), red guessed', 0, n_ima+1,'bd')
                    title(['Error = ' num2str(sum(err)/m) ' image ' num2str(i)]);
                    axis equal
                    pause
                end
            end
        end
    end
    backprojection_error = sum(errAvg)/correctSets
end


function [mps2] = cmps22mps2(cmps2)
% Convert acceleration from centimeters per square centimeter to meters
% per second-squared.
% Chad A. Greene 2012
mps2 = cmps2*1e-2;
end


function [pvals,is_significant] = test_significance(TestData,W,p,nnull)
%
% USAGE:
%
% [pvals,is_significant] = test_significance(TestData,W,0.01)
%
% ------------------------------------------------------------------------
% DESCRIPTION:
%
% Tests each factor in W for significance using a held out test dataset at
% a p-value of p using Bonferroni correction.
%
% ------------------------------------------------------------------------
%
% INPUTS:
%
% Name              Default                 Description
% TestData                                  Held out data matrix (NxT)
% W                                         NxKxL tensor containing factor exemplars
% p                 0.05                    Desired p-value to test
% nnull             ceil(K/p)*2             Number of null datasets to use
%
% ------------------------------------------------------------------------
% OUTPUTS:
%
% pvals                      A vector (1xK) containing the p-value of each factor
% is_significant             A boolean vector (1xK) which is 1 if a factor
%                            is significant using the specified pvalue with correction
%
% ------------------------------------------------------------------------
% CREDITS:
%   Emily Mackevicius and Andrew Bahle, 2/1/2018
%
%   Please cite our paper:
%       XXXXXXXXXXXXXXXXXXXXX
% Remove factors where there is obviously no sequence
% That is, W is empty, or has one neuron with >99.9% of the power

indempty = sum(sum(W>0,1),3)==0; % W is literally empty
Wflat = sum(W,3);
indempty = indempty | (max(Wflat,[],1).^2> .999*sum(Wflat.^2,1)); % or one neuron has >99.9% of the power
W(:,indempty,:) = []; % Delete factors that meet the above critera

[N,K,L] = size(W);
[~,T] = size(TestData);

if nargin < 3
    p = 0.05;
end

if nargin < 4
    nnull = ceil(K/p)*2;
end

% make nnull shifted datasets
skewnull = zeros(K,nnull);

X = TestData;

for n = 1:nnull
    % Make a null dataset
    Wnull = zeros(N,K,L);
    for k = 1:K
        for ni = 1:N
            %Wnull(ni,k,:) = circshift(W(ni,k,:),randi(L));
            Wnull(ni,k,:) = circshift(W(ni,k,:),[0,0,randi(L)]);
        end
    end
    %figure, imagesc(squeeze(Wnull(:,1,:))), colormap(gca, flip(gray)),axis off
    % Calculate WTX
    WTX = zeros(K, T);
    for l = 1 : L
        %X_shifted = circshift(X,-l+1,2);
        X_shifted = circshift(X,[0,-l+1]);
        WTX = WTX + Wnull(:, :, l)' * X_shifted;
    end
    %figure, histogram(WTX(1,:),0:1:100,'FaceColor','k')
    %set(gca,'yscale','log'), ylim([0,10e2])
    % Get skewness of each
    skewnull(:,n) = skewness(WTX,1,2);
end

WTX = zeros(K, T);
for l = 1 : L
    %X_shifted = circshift(X,-l+1,2);
    X_shifted = circshift(X,[0,-l+1]);
    WTX = WTX + W(:, :, l)' * X_shifted;
end
skew = skewness(WTX,1,2);
for k = 1:K
    % Assign pvals from skewness
    pvals(k) = (1+sum(skewnull(k,:)>skew(k)))/nnull;
end
allpvals(indempty) = Inf;
allpvals(~indempty) = pvals;
pvals = allpvals;
is_significant = (pvals <= p/K);
end


function [SR, report] = mapsr(LRImages, model, varargin)

    if nargin > 2
        solverParams = varargin{end};
    else
        % Use default solver parameters
        solverParams = SRSolverParams;
    end

    % Assemble equation system to be solved consisting of low-resolution
    % observations and the system matrix.
    lrDim = size(LRImages(:,:,1));
    if solverParams.verbose
        disp('Assemble equation system (system matrix)...');
    end
    [W, LRImages] = composeSREquationSystem(LRImages, model);

    if isempty(model.SR)
        % Compute initial guess for the super-resolved image. We use the
        % "average image" estimated from the system matrix and photometric
        % parameters (if available).
        if solverParams.verbose
            disp('Compute average image used as initial guess...');
        end
        SR = getInitialSRImage(W, LRImages, model.photometricParams);
    else
        % Use initial guess provided by the user and reshape 2-D image into
        % parameter vector.
        SR = imageToVector(model.SR);
    end

    scgOptions = setupSCGOptions(solverParams);
    if solverParams.verbose
        disp('Minimize objective function...');
    end
    [SR, ~, flog, ~, ~]  = scg(@mapfunc, SR', scgOptions, @mapfunc_grad, model, LRImages, W, W');
    if nargout > 1
        report.numFunEvals = length(flog);
    end

    % Reshape parameter vector to a 2D image.
    SR = vectorToImage(SR, model.magFactor * lrDim);
    if solverParams.verbose
        disp('DONE!');
    end

function scgOptions = setupSCGOptions(solverParams)

    scgOptions = zeros(1,18);
    scgOptions(1) = 0;
    scgOptions(2) = solverParams.tolX;
    scgOptions(3) = solverParams.tolF;
    scgOptions(9) = solverParams.gradCheck;
    scgOptions(10) = solverParams.maxFunEvals;
    scgOptions(14) = solverParams.maxIter;

function f = mapfunc(SR, model, LR, W, ~)

    if ~iscolumn(SR)
        % Reshape to column vector.
        SR = SR';
    end

    % Evaluate the data fidelity term.
    dataTerm = mapDataTerm(SR, model, LR, W);

    % Evaluate image prior for regularization the super-resolved estimate.
    prior = model.imagePrior.function(SR, model.imagePrior.parameters{1:end});

    % Calculate objective function.
    f = dataTerm + model.imagePrior.weight * prior;

function grad = mapfunc_grad(SR, model, LR, W, Wt)

    if ~iscolumn(SR)
        % Reshape to column vector.
        SR = SR';
    end

    % Calculate gradient of the data fidelity term w.r.t. the
    % super-resolved image.
    dataTerm_grad = mapDataTerm_gradImage(SR, model, LR, W, Wt);

    % Calculate gradient of the regularization term w.r.t. the
    % super-resolved image.
    prior_grad = model.imagePrior.gradient(SR, model.imagePrior.parameters{1:end});

    % Sum up to total gradient
    grad = dataTerm_grad + model.imagePrior.weight * prior_grad;
    grad = grad';


%VGG_KR_FROM_P Extract K, R from camera matrix.
%
%    [K,R,t] = VGG_KR_FROM_P(P [,noscale]) finds K, R, t such that P = K*R*[eye(3) -t].
%    It is det(R)==1.
%    K is scaled so that K(3,3)==1 and K(1,1)>0. Optional parameter noscale prevents this.
%
%    Works also generally for any P of size N-by-(N+1).
%    Works also for P of size N-by-N, then t is not computed.

% Author: Andrew Fitzgibbon <awf@robots.ox.ac.uk>
% Modified by werner.
% Date: 15 May 98

function [K, R, t] = vgg_KR_from_P(P, noscale)

N = size(P,1);
H = P(:,1:N);

[K,R] = vgg_rq(H);

if nargin < 2
  K = K / K(N,N);
  if K(1,1) < 0
    D = diag([-1 -1 ones(1,N-2)]);
    K = K * D;
    R = D * R;

  %  test = K*R;
  %  vgg_assert0(test/test(1,1) - H/H(1,1), 1e-07)
  end
end

if nargout > 2
  t = -P(:,1:N)\P(:,end);
end

return


% demos for ch06

%% Kernel regression with gaussian kernel
clear; close all;
n = 100;
x = linspace(0,2*pi,n);   % test data
t = sin(x)+rand(1,n)/2;
model = knReg(x,t,1e-4,@knGauss);
[y,s] = knRegPred(model,x);
plotCurveBar(x,y,s);
hold on;
plot(x,t,'o');
hold off;


classdef CEC2010_F14 < PROBLEM
% <single> <real> <constrained>
% CEC'2010 constrained optimization benchmark problem

%------------------------------- Reference --------------------------------
% R. Mallipeddi and P. N. Suganthan, Problem definitions and evaluation
% criteria for the CEC 2010 competition on constrained real-parameter
% optimization, Nanyang Technological University, Singapore, 2010.
%------------------------------- 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".
%--------------------------------------------------------------------------

    properties
        O;  % Optimal decision vector
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            CallStack = dbstack('-completenames');
            load(fullfile(fileparts(CallStack(1).file),'CEC2010.mat'),'Data');
            obj.O = Data{14};
            obj.M = 1;
            if isempty(obj.D); obj.D = 10; end
            obj.D = min(obj.D,length(obj.O));
            obj.lower    = zeros(1,obj.D) - 1000;
            obj.upper    = zeros(1,obj.D) + 1000;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            Z = 1 + PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            PopObj = sum(100*(Z(:,1:end-1).^2-Z(:,2:end)).^2+(Z(:,1:end-1)-1).^2,2);
        end
        %% Calculate constraint violations
        function PopCon = CalCon(obj,PopDec)
            Z = PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            PopCon(:,1) = sum((-Z.*cos(sqrt(abs(Z)))),2) - size(Z,2);
            PopCon(:,2) = sum((Z.*cos(sqrt(abs(Z)))),2) - size(Z,2);
            PopCon(:,3) = sum((Z.*sin(sqrt(abs(Z)))),2) - 10*size(Z,2);
        end
    end
end


function gap = evalgap(fnc, fspec, dnorm, ww, uu, A, B, lambda)

[ff,gg]=evalloss(fnc,ww,uu,A,B);

fval = ff+lambda*sum(fspec(ww));
dval = evaldual(fnc,dnorm,-gg,A,B,lambda);

gap = (fval+dval)/fval;

function [fval,gg]=evalloss(fnc, ww, uu, A, B)

if ~isempty(uu)
  zz=A*ww+B*uu;
else
  zz=A*ww;
end

[fval, gg] =fnc.p(zz, fnc.args{:});

function dval = evaldual(fnc, dnorm, aa, A, B, lambda)

mm=length(aa);

if ~isempty(B)
  aa=aa-B*((B'*B)\(B'*aa));
end

[dnm,ishard] =dnorm(A'*aa);

if ishard && dnm>0
  aa  = min(1, lambda/dnm)*aa;
  dnm = 0;
end

dval = fnc.d(aa, fnc.args{:})+dnm;