function [axons,packing] = func_axonpack_main(numelobj, d_mean, d_var, gap, iter_max)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%       AxonPackin : Simulate arrangement of white matter axons
%                     author : Tom Mingasson
%             https://github.com/neuropoly/axonpacking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                       CHANGE INPUTS BELOW
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% MAIN INPUTS
N = numelobj;            % number of axons i.e disks to pack
%d_mean = 3;         % theoretical mean of axon diameters in um
%d_var  = 1;         % theoretical variance of axon diameters in um
Delta  = gap;         % gap between the edge of axons in um
%iter_max = 30000;    % number of iteration i.e migrations to perform. Example: iter_max = 30000 ok if N = 1000

% SECONDARY INPUTS
threshold_high = 10;     % no diameter above 'threshold_high'
threshold_low = 0.2;     % no diameter under 'threshold_low'
iter_fvf = iter_max/10;  % to study the packing convergence the disk density i.e Fiber Volume Fraction (FVF) can be computed and displayed every 'iter_fvf' iterations

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                     AxonPacking Process
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for k=1:length(d_mean)

    % axons features
    axons.N{k}      = N;
    axons.d_mean{k} = d_mean(k);
    axons.d_var{k}  = d_var(k);
    axons.Delta{k}           = Delta(k);
    axons.threshold_high{k} = threshold_high;
    axons.threshold_low{k}  = threshold_low;

    % axon diameters sampling (under a gamma law or lognormal and initialization of positions 'x0' in a square area of length 'side')
    [d, x0, side] = axons_setup(axons,'gamma', k);
    axons.d{k} = d;
    axons.g_ratio{k} = compute_gratio(d);

    % packing process of the axons
    [final_positions, final_overlap, fvf_historic] = process_packing(x0, d, Delta(k), side, iter_max, iter_fvf);

    % store packing results
    % main results
    packing.initial_positions{k}    = reshape(x0,2,length(x0)/2);
    packing.final_positions{k}      = final_positions;
    % secondary results
    packing.final_overlap{k}        = final_overlap;
    packing.FVF_historic{k}         = fvf_historic;
    packing.iter_max{k}             = iter_max;

    % Statistics from the packing
    [FVF, FR, MVF, AVF] = compute_statistics(axons.d{k}, axons.Delta{k}, packing.final_positions{k}, side, axons.g_ratio{k});

    % store stats results
    stats.FVF{k}        = FVF;
    stats.FR{k}         = FR;
    stats.MVF{k}        = MVF;
    stats.AVF{k}        = AVF;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                 Save results in a folder named 'results'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% save_var = num2str(d_var);  save_var(save_var == ' ') = '';
% save_mean = num2str(d_mean); save_mean(save_mean == ' ') = '';
% save_Delta  = num2str(Delta);  save_Delta(save_Delta == ' ') = '';
% save_iter  = num2str(iter_max);
% saveName  = ['Axons', num2str(N), '_Mean', save_mean, '_Var', save_var, '_Delta', save_Delta, '_Iter',save_iter];
%
% mkdir('results')
% cd([pwd,filesep, 'results'])
%
% % save outputs
% save('axons.mat', '-struct', 'axons');
% save('packing.mat', '-struct', 'packing');
% save('stats.mat', '-struct', 'stats');
%
% % save final substrate
% saveas(figure(1000),[saveName,'.png']);
%


function [J,alpha,beta,T] = variationalDynLoreta(Ut,Y,s2,iLV,L,alpha,beta,options)

%[J,alpha,beta,T] = variationalDynLoreta(Ut,Y,s2,iLV,L,alpha,beta,options)
%
% Computes the posterior distribution of the parameters J given some data V.
% The program solves levels of inference: 1) optimization of parameters J, and
% 2) optimization of hyperparameters alpha and beta. See Trujillo-Barreto
% et. al. (2004) for details.
%
% Ut,s2, and iLV are defined as follows:
%     Y: Nsensors x time points data matrix
%     K: N x P predictor matrix
%     L: sparse P x P square root of the precision matrix
%     [U,s,V] = svd( K*inv(L) )
%     iLV = inv(L)*V
%     s2  = s.^2
%
% alpha, beta: hyperparameters
% J: estimated parapeters
%
%                     P(V|J,alpha)*P(J|beta)
% P(J|V,alpha,beta) = ----------------------
%                        P(V|alpha,beta)
%
% Author: Alejandro Ojeda, SCCN/INC/UCSD, Jan-2013
%
% References:
%   Trujillo-Barreto, N., Aubert-Vazquez, E., Valdes-Sosa, P.A., 2004.
%     Bayesian model averaging in EEG/MEG imaging. NeuroImage 21, 1300???1319

if nargin < 5, error('Not enough input arguments.');end
if nargin < 8
    options.maxTol = 1e-3;
    options.maxIter = 100;
    options.verbose = true;
    options.gridSize = 100;
end

s = s2.^(0.5);
n = length(s);
p = size(L,1);
ntp = size(Y,2);

% Initialize hyperparameters
if nargin < 8
    UtY = Ut*Y(:,1);
    tol = max([n p])*eps(max(s));
    lambda2 = logspace(log10(tol),log10(max(s)),options.gridSize);
    gcv = zeros(options.gridSize,1);
    parfor k=1:options.gridSize
        d = lambda2(k)./(s2+lambda2(k));
        f = diag(d)*UtY;
        gcv(k) = dot(f,f,1)/sum(d)^2;
    end
    loc = getMinima(gcv);
    if isempty(loc), loc = 1;end
    loc = loc(end);
    lambda2 = lambda2(loc);

    alpha = 0.001*(Y(:)'*Y(:))/n;
    beta = alpha*lambda2;
end
err = inf;

for it=1:options.maxIter
    if err < options.maxTol, break;end

    % computing statistics
    H = Ut'*diag(alpha.*s2./(alpha.*s2+beta))*Ut;
    SSE = mean( (Y - H*Y).^2 ,2);
    sigma2 = mean(SSE);
    kSjk = diag(H);

    q = diag(alpha.*s./(alpha.*s2+beta))*Ut;%*Y;
    Sj = mean((iLV*q).^2,2);
    aic = -2*log(sigma2) + 2*p;
    alpha_old = alpha;
    beta_old = beta;

    % updating hyperparameters
    alpha = updateAlpha(SSE,kSjk,ntp);
    beta  = updateBeta(Sj,p);

    err = 0.5*abs(sum(alpha_old-alpha)) + 0.5*abs(beta_old-beta);
    if options.verbose
        disp([num2str(it) ' => alpha: ' num2str(alpha_old) '  beta: ' num2str(beta_old) ' sse: ' num2str(mean(SSE)) ' hyrp. error: ' num2str(err) ' aic: ' num2str(aic)]);
    end
end
if it == options.maxIter, warning('Maximum iteration reached. Failed to converge.');end

% parameters's estimation
T = iLV*diag(alpha.*s./(alpha.*s2+beta))*Ut;
J = T*Y;

% standardized Loreta
E = sum(Y-H*Y,2);
sigma = E'*E/(n-trace(H));
dT = 1./sqrt(dot(T,T,2));
S = 1./sigma*dT;
S = S./std(eps+S);
T = bsxfun(@times,T,S);%sqrt(p)*
J = bsxfun(@times,J,S);%sqrt(p)*
end

%---
function indmin = getMinima(x)
fminor = diff(x)>=0;
fminor = ~fminor(1:end-1, :) & fminor(2:end, :);
fminor = [0; fminor; 0];
indmin = find(fminor);
end

function alpha = updateAlpha(SSE,kSjk,ntp,bp,cp)
if nargin < 4, bp = 3;end
if nargin < 5, cp = 3;end
b = 1./( (1/bp) + 0.5*SSE + 0.5*kSjk );
c = cp + ntp/2;
alpha = median(1./(b*c));
end

function beta = updateBeta(r,p,bp,cp)
if nargin < 3, bp = 3;end
if nargin < 4, cp = 3;end
b = 1./( 1/bp + sum(r) );
c = cp+p/2;
beta = 1/(b*c);
end


function [gew,pve,H] = spm_csd2gew(csd,Hz,u)
% Convert cross sspectral density to Geweke Granger causality
% FORMAT [gew,pve,H] = spm_csd2gew(csd,Hz)
%
% ccf  (N,m,m)   - cross covariance functions
% Hz   (n x 1)   - vector of frequencies (Hz)
% u    (1)       - regularizer (default: 1);
%
% gwe  (N,m,m)   - Geweke's frequency domain Granger causality
% pve  (N,m,m)   - proportion of variance explained
% H    (N,m,m)   - transfer function matrix
%
% This routine uses the Wilson-Burg algorithm to perform spectral matrix
% factorisation. The minimum phase factor is then used to form the noise
% covariance (covariance of the innovations) and implicitly derive the
% transfer functions (and spectral Granger causality).
%
% See also:
%  spm_ccf2csd.m, spm_ccf2mar, spm_csd2ccf.m, spm_csd2mar.m, spm_mar2csd.m,
%  spm_csd2coh.m, spm_dcm_mtf.m, spm_Q.m, spm_mar.m and spm_mar_spectral.m
%
%__________________________________________________________________________
% Copyright (C) 2014 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_csd2gew.m 5908 2014-03-05 20:31:57Z karl $

% preliminaries
%--------------------------------------------------------------------------
if nargin < 3
    try
        [gew,pve,H] = spm_csd2gew(csd,Hz,2);
        return
    catch
        [gew,pve,H] = spm_csd2gew(csd,Hz,16);
        return
    end
end

% pad spectrum if necessary
%--------------------------------------------------------------------------
iw    = 1 + round(Hz/(Hz(2) - Hz(1)));
n     = size(csd,2);
nw    = 257;
is    = ceil(nw/2):nw;

% Wilson-Burg algorithm
%==========================================================================

% initialise transfer function
%--------------------------------------------------------------------------
H     = zeros(nw,n,n);
P     = zeros(nw,n,n);
e     = norm(squeeze(max(csd,[],1)))/128;
E     = eye(n,n)*e;

P(iw,:,:) = csd;
for i = 1:n
    P(:,i,i) = P(:,i,i) + e;
    H(:,i,i) = sqrt(P(:,i,i));
end

% iterate until convergence: solve for H*H' = P
%--------------------------------------------------------------------------
for t = 1:128

    % compute left-hand side (deconvolution)
    %----------------------------------------------------------------------
    for w = 1:nw
        S        = squeeze(H(w,:,:)) + E;
        A(w,:,:) = (eye(n,n) + S*S'\squeeze(P(w,:,:)));
    end

    % retain causal signal and half zero lag
    %----------------------------------------------------------------------
    S           = ifft(A);
    S(is,:,:)   = 0;
    S(1,:,:)    = S(1,:,:)/2;
    A           = fft(S);

    % recover next update (convolution)
    %----------------------------------------------------------------------
    nrm   = zeros(nw,1);
    for w = 1:nw
        U        = squeeze(A(w,:,:));
        H(w,:,:) = squeeze(H(w,:,:))*U^(1/u);
        nrm(w)   = nrm(w) + norm(eye(n,n) - U,'inf');
    end

    % break if convergence
    %----------------------------------------------------------------------
    nrm = mean(nrm);
    if nrm < 1e-6, break, end
    if nrm > 8,   return, end

end

% transfer function and noise covariance
%==========================================================================

% get noise covariance
%--------------------------------------------------------------------------
S     = ifft(H);
R     = squeeze(S(1,:,:));
C     = real(R*R');
c     = sqrtm(C);

% recover transfer function
%--------------------------------------------------------------------------
for w = 1:nw
    H(w,:,:) = squeeze(H(w,:,:))/c;
    P(w,:,:) = squeeze(H(w,:,:))*C*squeeze(H(w,:,:))';
end

% Geweke Granger Causality in the Frequency domain
%--------------------------------------------------------------------------
pve   = zeros(nw,n,n);
gew   = zeros(nw,n,n);
for j = 1:n
    for k = 1:n
        rkj        = C(j,j) - (C(j,k)^2)/C(k,k);
        sk         = abs(P(:,k,k));
        hkj        = abs(H(:,k,j)).^2;
        pve(:,k,j) = rkj*hkj./sk;
        gew(:,k,j) = -log(1 - pve(:,k,j));
    end
end

% return  specified frequencies
%--------------------------------------------------------------------------
gew = gew(iw,:,:);
pve = pve(iw,:,:);
H   = H(iw,:,:);


function TR=IcosahedronMesh
% Name speaks for itself.

% Get the vertex coordinates
t=(1+sqrt(5))/2; % golden ratio
x=[0 1 t];
s=[1 1 1; 1 1 -1; 1 -1 -1; 1 -1 1];
x=repmat(x,[4 1]).*s;
x=[x;circshift(x,[0 -1]);circshift(x,[0 -2])];
x_L2=sqrt(sum(x.^2,2));
x=bsxfun(@rdivide,x,x_L2);

% Triangulate the points
Tri = fliplr(convhulln(x));
TR=TriRep(Tri,x);


function [U] = dualquatlbs(V,DQ,W)
  % DUALQUATLBS Compute dual quaternions linear blend skinning deformation of
  % vertices V, using rigid transformations stored as dual quaternions, DQ, at
  % some control points, propogated to the mesh using weights W.
  %
  % [U] = dualquatlbs(V,DQ,W)
  %
  % Inputs:
  %  V  list of vertex positions
  %  DQ  list of rigid transformations for each control point stored as dual
  %    quaternions
  %    2 by 4 by #controls
  %  W  weights, # vertices by # handles matrix of weights
  % Output:
  %  U  list of new vertex positions
  %
  % Copyright 2011, Alec Jacobson (jacobson@inf.ethz.ch)
  %
  % See also: lbs
  %

  % pad V with zeros if not 3D
  if(size(V,2) ~= 3)
    was_2d = true;
    V = [V zeros(size(V,1),1)];
  else
    was_2d = false;
  end

  % number of control points
  m = size(DQ,3);
  % should be same in W
  assert(m == size(W,2));

  % number of domain vertices
  n = size(V,1);

  % See algorithm 1 in "Geometric skinning with approximate dual quaternion
  % blending" by Kavan et al

  % compute weighted combination of DQs for each domain vertex
  % DQs seen by every vertex in domain, 2 by 4 by m by n
  WDQ = permute(repmat(W',[1,1,2,4]),[3 4 1 2]) .* repmat(DQ,[1,1,1,n]);
  % sum of weighted dual quaternions, 2 by 4 by n
  VDQ = permute(sum(WDQ,3),[1 2 4 3]);
  % regular part, n by 4
  VDQ1 = permute(VDQ(1,:,:),[3 2 1]);
  % dual part, n by 4
  VDQ2 = permute(VDQ(2,:,:),[3 2 1]);
  %VDQ2 = permute(sum(WDQ(2,:,:,:),3),[4 2 1 3]);
  len = repmat(sqrt(sum(VDQ1.^2,2)),1,4);
  VDQ1 = VDQ1./len;
  VDQ2 = VDQ2./len;

  U = ...
    V + ...
    2*cross( ...
      VDQ1(:,2:4), ...
      cross(VDQ1(:,2:4),V,2)+repmat(VDQ1(:,1),1,3).*V,2) + ...
    2*( ...
      repmat(VDQ1(:,1),1,3).*VDQ2(:,2:4) - ...
      repmat(VDQ2(:,1),1,3).*VDQ1(:,2:4) + ...
      cross(VDQ1(:,2:4),VDQ2(:,2:4),2));

  %VDQ = zeros([2 size(VDQ1)]);
  %VDQ(1,:,:) = VDQ1;
  %VDQ(2,:,:) = VDQ2;
  %% convert DQs into transformations, multiply against vertices
  %% convert rotation to quaternion and translation
  %[Q,T] = udq2quattrans(VDQ);
  %% convert quaterion
  %R = quat2mat(Q);

  % unpad U if V was 2D
  if(was_2d)
    U = U(:,1:2);
  end
end


% DMRG eigenvalue solver.
%   function [x,theta,testdata]=dmrg_eig(A, tol, varargin)
% Tries to solve the lowest eigenvalue (ground state) problem A*x=theta*x
% using the DMRG iteration.
% With default parameters, reconstructs the genuine 2-site DMRG for one
% targeted state, without enrichments.
% However, it can also work as the "block 1-site DMRG" from ref. [*], where
% at least 2 states must be targeted, but only one block at a time can be
% considered. See parameters 'b' and 'numblocks' below.
%
% A must be a Hermitian matrix in the tt_matrix format,
% tol is the relative truncation/stopping threshold,
% varargin may contain a sequence of tuning parameters of the form
% 'parameter1_name', parameter1_value, 'parameter2_name', parameter2_value
% and so on. Available parameters and default values are the following.
%   o b: Number of eigenpairs targeted (default: 1)
%   o numblocks: Number of TT blocks included into the current supercore.
%       Can be either 2 for 2-site dmrg, or 1 for 1-site dmrg (default: 2)
%   o nswp: maximal number of DMRG sweeps (default: 20)
%   o max_full_size: if the local system size (rx(i)*n(i)*rx(i+1)) is less
%       than max_full_size, the local problem is solved via eig, otherwise via
%       lobpcg (iteratively) (default: 50)
%   o local_iters: maximal number of the lobpcg iterations (default: 100)
%   o usemex: whether shall we use the optimized MEX library eig3d_primme
%       instead of lobpcg. It safely switches off automatically if
%       eig3d_primme is not found in the MATLAB path, as well as on complex
%       data. To obtain eig3d_primme, you need to compile it in the
%       TT-Toolbox/fmex directory, please follow instructions there (default: true)
%   o resid_damp: residual gap for the local solvers, i.e. the real
%       threshold for e.g. lobpcg is tol/(sqrt(d)*resid_damp) (default: 2)
%   o trunc_norm: truncation strategy: either the standard Frobenius norm
%       ('fro'), or the residual ('resid') (default: 'fro')
%   o rmax: maximal TT rank (bond dimension) allowed (default: Inf)
%   o tol_exit: stopping tolerance (default: tol)
%   o verb: verbosity level: silent (0), sweep info (1), block info (2) or
%       the full debug returned in the testdata output (3) (default: 1)
%   o kickrank: size of the random enrichment (default: 0)
%   o x0: Initial guess in the tt_tensor format (default: random rank-2)
%
% Output data are the solution x in the tt_tensor format,
% the eigenvalue theta, and
% (optionally) the history for debug (or convergence tests) purposes testdata.
%
%
% Implementation: Sergey Dolgov (sergey.v.dolgov@gmail.com)
%
% [*] Dolgov, Khoromskij, Oseledets, Savostyanov,
% "Computation of extreme eigenvalues in higher dimensions using block
% tensor train format", Comp. Phys. Comm. 2014, http://dx.doi.org/10.1016/j.cpc.2013.12.017
%

function [x,theta,testdata]=dmrg_eig(A, tol, varargin)
% Number of eigenstates
b = 1;
% Threshold on the local size between direct-iterative local solving
max_full_size=50;
% Number of local iters
local_iters = 100;
% Whether shall we use the local solver from the MEX library eig3d_primme
usemex = true;
% Number of sweeps
nswp=20;
% Residual gap for the local solver
resid_damp = 2;
% Truncation strategy
%trunc_norm = 'resid';
trunc_norm = 'fro';
% Max rank
rmax = Inf;
% Exit tol
tol_exit = tol;
% Verb
verb=1;
% Random enrichment rank
kickrank = 0;
% Number of blocks to consider: 2 or 1
numblocks = 2;
% Initial guess
x=[];

for i=1:2:length(varargin)-1
    switch lower(varargin{i})
        case 'nswp'
            nswp=varargin{i+1};
        case 'rmax'
            rmax=varargin{i+1};
        case 'x0'
            x=varargin{i+1};
        case 'verb'
            verb=varargin{i+1};
        case 'resid_damp'
            resid_damp=varargin{i+1};
        case 'trunc_norm'
            trunc_norm=varargin{i+1};
        case 'kickrank'
            kickrank=varargin{i+1};
        case  'max_full_size'
            max_full_size=varargin{i+1};
        case  'local_iters'
            local_iters=varargin{i+1};
        case 'usemex'
            usemex=varargin{i+1};
        case 'tol_exit'
            tol_exit=varargin{i+1};
        case 'b'
            b = varargin{i+1};
        case 'numblocks'
            numblocks = varargin{i+1};
        otherwise
            error('Unknown tuning parameter "%s"', varargin{i});
    end;
end;

% Extract the dimensions
if (~isa(A, 'tt_matrix'))
    error('A must be given in a tt_matrix class');
end;
d = A.d;
n = A.n;
% Initialize from random initial state if not passed otherwise
if (isempty(x))
    x = tt_rand(n, d, b, -1);
else
    if (~isa(x, 'tt_tensor'))
        error('x0 must be given in a tt_tensor class');
    end;
    if (x.d~=d)
        error('Inconsistent dim(A) and dim(x0)');
    end;
    if (any(x.n~=n))
        error('Inconsistent size(A) and size(x0)');
    end;
end;

% Disable MEX if it does not exist
if (usemex)&&(exist('eig3d_primme', 'file')<2)
    warning('MEX local solver is not found, disabled');
    usemex = false;
end;

% More housekeeping
ra = A.r;
rx = x.r;
if (rx(d+1)>1)
    % Leave only the first component from the initial guess
    x = x*eye(rx(d+1),1);
    rx(d+1) = 1;
end;
crA = core2cell(A);
crx = core2cell(x);

% Threshold for local problems
real_tol = (tol/sqrt(d))/resid_damp;

% Interfaces
phixax = cell(d+1,1); phixax{1}=1; phixax{d+1}=1;

% This is some convergence output for test purposes
testdata = cell(3,1);
testdata{1} = zeros(d-numblocks+1, nswp); % times
testdata{2} = zeros(d-numblocks+1, nswp, b); % evs
testdata{3} = zeros(d-numblocks+1, nswp); % local res or dx
t_dmrg_eig = tic;

% Presetup: compute the initial projections
for i=d:-1:2
    % QR (Gauge conditions) right-to-left
    ux = reshape(crx{i}, rx(i), n(i)*rx(i+1));
    [ux,vx]=qr(ux.', 0);
    cr2 = reshape(crx{i-1}, rx(i-1)*n(i-1), rx(i));
    cr2 = cr2*vx.';
    rx(i) = size(ux,2);
    crx{i} = reshape(ux.', rx(i), n(i), rx(i+1));
    crx{i-1} = reshape(cr2, rx(i-1), n(i-1), rx(i));

    % Pass the reduction to the left
    phixax(i) = rightreduce_matrix(phixax(i+1), crx{i}, crA(i), crx{i}, rx(i),n(i),rx(i+1), 1,ra(i),ra(i+1), rx(i),n(i),rx(i+1));
    if (usemex)&&(~isreal(phixax{i}))
        warning('Complex data detected, turning MEX local solver off');
        usemex = false;
    end;
end;
% Initial guess for EVs
theta = rightreduce_matrix(phixax(2), crx{1}, crA(1), crx{1}, rx(1),n(1),rx(1+1), 1,ra(1),ra(1+1), rx(1),n(1),rx(1+1));
theta = squeeze(theta{1});
if (usemex)&&(~isreal(theta))
    warning('Complex data detected, turning MEX local solver off');
    usemex = false;
end;
theta = diag(theta);
if (b>1)
    crx{1} = randn(rx(1), n(1), rx(2), b);
end;

% DMRG sweeps
swp = 1;
i = 1; % Start from the first block
dir = 1; % Current direction: forward(+1) or backward(-1)
% Meausure the errors and # of operations
max_dx = 0;
max_res = 0;
max_matvecs = 0;

while (swp<=nswp)||(dir>0)
    % Extract the matrix parts, accelerate a plenty of iterations with them
    % Phi1: 1, rx'1, rx1, ra1
    % Phi2: ra2, rx'2, rx2, 1
    Phi1 = phixax(i);
    A1 = crA(i);
    if (numblocks==1)
        % one-block DMRG, everything is ready
        Phi2 = phixax(i+1);
        rx1 = rx(i);
        nloc = n(i);
        rx2 = rx(i+1);
        ra1 = ra(i);
        ra2 = ra(i+1);
        % sol_prev. It is also an initial guess.
        sol_prev = reshape(crx{i}, rx(i)*n(i)*rx(i+1), b);
    else
        % Two-block DMRG, we have to merge some of two blocks into one.
        Phi2 = phixax(i+2);
        A2 = crA(i+1);
        if ((rx(i)*n(i))<=(n(i)*n(i+1)))&&((rx(i)*n(i))<=(n(i+1)*rx(i+2)))
            % Merge Phi1 and A1
            Phi1 = reshape(Phi1{1}, rx(i)*rx(i), ra(i));
            A1 = reshape(A1{1}, ra(i), n(i)*n(i)*ra(i+1));
            Phi1 = Phi1*A1;
            Phi1 = reshape(Phi1, rx(i), rx(i), n(i), n(i)*ra(i+1));
            Phi1 = permute(Phi1, [1,3,2,4]);
            Phi1 = {reshape(Phi1, rx(i)*n(i), rx(i)*n(i), ra(i+1))};
            A1 = A2;
            rx1 = rx(i)*n(i);
            nloc = n(i+1);
            rx2 = rx(i+2);
            ra1 = ra(i+1);
            ra2 = ra(i+2);
        elseif ((n(i)*n(i+1)<=(rx(i)*n(i))))&&((n(i)*n(i+1))<=(n(i+1)*rx(i+2)))
            % Merge A1 and A2
            A1 = reshape(A1{1}, ra(i)*n(i)*n(i), ra(i+1));
            A2 = reshape(A2{1}, ra(i+1), n(i+1)*n(i+1)*ra(i+2));
            A1 = A1*A2;
            A1 = reshape(A1, ra(i)*n(i), n(i), n(i+1), n(i+1)*ra(i+2));
            A1 = permute(A1, [1,3,2,4]);
            A1 = {reshape(A1, ra(i), n(i)*n(i+1), n(i)*n(i+1), ra(i+2))};
            rx1 = rx(i);
            nloc = n(i)*n(i+1);
            rx2 = rx(i+2);
            ra1 = ra(i);
            ra2 = ra(i+2);
        else
            % Merge A2 and Phi2
            Phi2 = reshape(Phi2{1}, ra(i+2), rx(i+2)*rx(i+2));
            A2 = reshape(A2{1}, ra(i+1)*n(i+1)*n(i+1), ra(i+2));
            Phi2 = A2*Phi2;
            Phi2 = reshape(Phi2, ra(i+1)*n(i+1), n(i+1), rx(i+2), rx(i+2));
            Phi2 = permute(Phi2, [1,3,2,4]);
            Phi2 = {reshape(Phi2, ra(i+1), n(i+1)*rx(i+2), n(i+1)*rx(i+2))};
            rx1 = rx(i);
            nloc = n(i);
            rx2 = n(i+1)*rx(i+2);
            ra1 = ra(i);
            ra2 = ra(i+1);
        end;
        % sol_prev. It is also an initial guess.
        if (dir>0)
            sol_prev = reshape(crx{i}, rx(i)*n(i)*rx(i+1), b);
            sol_prev = sol_prev.';
            sol_prev = reshape(sol_prev, b*rx(i)*n(i), rx(i+1));
            sol_prev = sol_prev*reshape(crx{i+1}, rx(i+1), n(i+1)*rx(i+2));
            sol_prev = reshape(sol_prev, b, rx(i)*n(i)*n(i+1)*rx(i+2));
            sol_prev = sol_prev.';
        else
            sol_prev = reshape(crx{i+1}, rx(i+1), n(i+1)*rx(i+2)*b);
            sol_prev = reshape(crx{i}, rx(i)*n(i), rx(i+1))*sol_prev;
            sol_prev = reshape(sol_prev, rx(i)*n(i)*n(i+1)*rx(i+2), b);
        end;
    end;

    % Initial residual
    res_prev = norm(local_matvec(sol_prev, rx1,nloc,rx2,b, rx1,nloc,rx2, Phi1, A1, Phi2, 1,ra1,ra2)-sol_prev*diag(theta));

    if (rx1*nloc*rx2<max_full_size) % Full solution
        %      |     |    |
        % B = Phi1 - A1 - Phi2
        %      |     |    |
        Bxx = assemble_local_matrix(Phi1, A1, Phi2, 1,ra1,ra2, rx1,nloc,rx2, rx1,nloc,rx2);
        Bxx = (Bxx+Bxx')*0.5; % Ensure the symmetry. At least now...
        [sol,L]=eig(Bxx);
        L = diag(L);
        L = real(L); % exclude possible i*1e-15 noise
        [~,ind] = sort(L, 'ascend');
        theta = L(ind(1:b));
        sol = sol(:,ind(1:b));
        num_matvecs = 1;
    else % Iterative solution
        if (usemex)
            Phi1m = reshape(Phi1{1}, rx1, rx1, ra1);
            Phi2m = reshape(Phi2{1}, ra2*rx2, rx2);
            Phi2m = Phi2m.';
            Phi2m = reshape(Phi2m, rx2, ra2, rx2);
            A1m = reshape(A1{1}, ra1, nloc, nloc, ra2);
            [sol,theta, num_matvecs]=eig3d_primme(Phi1m,A1m,Phi2m,real_tol,b,sol_prev,local_iters);
        else
            [sol,theta,~,lambdaHistory]=lobpcg(sol_prev,@(x)local_matvec(x, rx1,nloc,rx2,[], rx1,nloc,rx2, Phi1, A1, Phi2, 1,ra1,ra2),  real_tol, local_iters);
            num_matvecs = numel(lambdaHistory);
        end;
    end;

    % count the number of MatVecs
    max_matvecs = max(max_matvecs, num_matvecs);

    if (~strcmp(trunc_norm, 'fro'))
        % We need the new residual for the corresp. trunc. strategy
        res_new = norm(local_matvec(sol, rx1,nloc,rx2,b, rx1,nloc,rx2, Phi1, A1, Phi2, 1,ra1,ra2)-sol*diag(theta));
    end;

    % L2-norm convergence check.
    dx = norm(sol*(sol'*sol_prev)-sol_prev);
    max_dx = max(max_dx, dx);
    max_res = max(max_res, res_prev);

    if ((dir>0)&&(i<d))||((dir<0)&&(i>1))||(numblocks==2) % SVD and enrichment
        if (dir>0)
            if (numblocks==2)
                % In 2-block DMRG, we have to separate modes
                sol = reshape(sol, rx(i)*n(i), n(i+1)*rx(i+2)*b);
            else
                sol = reshape(sol, rx(i)*n(i), rx(i+1)*b);
            end;
        else
            sol = sol.';
            if (numblocks==2)
                % In 2-block DMRG, we have to separate modes
                sol = reshape(sol, b*rx(i)*n(i), n(i+1)*rx(i+2));
            else
                sol = reshape(sol, b*rx(i), n(i)*rx(i+1));
            end;
        end;

        [ux,s,vx]=svd(sol, 'econ');
        s = diag(s);

        if (strcmp(trunc_norm, 'fro')) % Fro-norm truncation
            r = my_chop2(s, real_tol*resid_damp*norm(s));
        else
            % Residual truncation
            % start from the old rank
            r = min([rx(i),rx(i+1),numel(s)]);
            cursol = ux(:,1:r)*diag(s(1:r))*vx(:,1:r)';
            if (dir>0)
                cursol = reshape(cursol, rx1*nloc*rx2, b);
            else
                cursol = reshape(cursol, b, rx1*nloc*rx2);
                cursol = cursol.';
            end;
            res = norm(local_matvec(cursol, rx1,nloc,rx2,b, rx1,nloc,rx2, Phi1, A1, Phi2, 1,ra1,ra2)-cursol*diag(theta));
            if (res<max(real_tol, res_new)*resid_damp)
                drank = -1; % rank is overestimated; decrease
            else
                drank = 1; % residual is large; increase the rank
            end;
            while (r>0)&&(r<=numel(s))
                cursol = ux(:,1:r)*diag(s(1:r))*vx(:,1:r)';
                if (dir>0)
                    cursol = reshape(cursol, rx1*nloc*rx2, b);
                else
                    cursol = reshape(cursol, b, rx1*nloc*rx2);
                    cursol = cursol.';
                end;
                res = norm(local_matvec(cursol, rx1,nloc,rx2,b, rx1,nloc,rx2, Phi1, A1, Phi2, 1,ra1,ra2)-cursol*diag(theta));
                if (drank>0)
                    if (res<max(real_tol, res_new)*resid_damp)
                        break;
                    end;
                else
                    if (res>=max(real_tol, res_new)*resid_damp)
                        break;
                    end;
                end;
                r = r+drank;
            end;
            if (drank<0)
                r=r+1;
            end;
        end;

        r = min(r, numel(s));
        r = min(r, rmax);

        if (verb==2)
            fprintf('=dmrg_eig= swp=%d, block=%d, dx=%3.3e, r=%d\n', swp, i, dx, r);
        end;
    end;

    if (dir>0)&&(i<d) % Forward sweep
        ux = ux(:,1:r);
        vx = conj(vx(:,1:r))*diag(s(1:r));

        if (kickrank>0)
            % Enrich the solution
            zx = randn(rx(i)*n(i), kickrank);
            % Concatenate the bases and reinforce the gauges.
            % In future: insert symmetries here?
            [ux,rv]=qr([ux,zx], 0);
            rv = rv(:,1:r);
            vx = vx*rv.';
        end;

        r = size(ux, 2);

        if (numblocks==1)
            % Drop the [r x r] factor to the next core -- White's prediction
            cr2 = crx{i+1};
            cr2 = reshape(cr2, rx(i+1), n(i+1)*rx(i+2));
            vx = reshape(vx, rx(i+1), b*r);
            cr2 = vx.'*cr2;
            cr2 = reshape(cr2, b, r*n(i+1)*rx(i+2));
            cr2 = cr2.';
        else
            % Replace the next core by vx
            cr2 = vx.';
            cr2 = reshape(cr2, r, n(i+1), rx(i+2), b);
        end;

        % Stuff them back
        rx(i+1) = r;
        crx{i} = reshape(ux, rx(i), n(i), rx(i+1));
        crx{i+1} = reshape(cr2, rx(i+1), n(i+1), rx(i+2), b);

        % Compute new reductions
        phixax(i+1) = leftreduce_matrix(phixax(i), ux, crA(i), ux, rx(i),n(i),rx(i+1), 1,ra(i),ra(i+1), rx(i),n(i),rx(i+1));
    elseif (dir<0)&&((i>1)||(numblocks==2)) % Backward sweep
        vx = conj(vx(:,1:r));
        ux = ux(:,1:r)*diag(s(1:r));

        if (kickrank>0)
            % Enrich the solution
            if (numblocks==1)
                zx = randn(n(i)*rx(i+1), kickrank);
            else
                zx = randn(n(i+1)*rx(i+2), kickrank);
            end;
            % Concatenate the bases and reinforce the gauges.
            % In future: insert symmetries here?
            [vx,rv]=qr([vx,zx], 0);
            rv = rv(:,1:r);
            ux = ux*rv.';
        end;

        r = size(vx, 2);

        if (numblocks==1)
            % Drop the [r x r] factor to the next core -- White's prediction
            cr2 = crx{i-1};
            cr2 = reshape(cr2, rx(i-1)*n(i-1), rx(i));
            ux = reshape(ux, b, rx(i)*r);
            ux = ux.';
            ux = reshape(ux, rx(i), r*b);
            cr2 = cr2*ux;
            cr2 = reshape(cr2, rx(i-1), n(i-1), r, b);
            % Stuff them back
            rx(i) = r;
            vx = vx.';
            crx{i} = reshape(vx, rx(i), n(i), rx(i+1));
            crx{i-1} = reshape(cr2, rx(i-1), n(i-1), rx(i), b);
            % Compute new reductions
            phixax(i) = rightreduce_matrix(phixax(i+1), vx, A1, vx, rx(i),n(i),rx(i+1), 1,ra(i),ra(i+1), rx(i),n(i),rx(i+1));
        else
            % Replace both cores
            vx = vx.';
            vx = reshape(vx, r, n(i+1), rx(i+2));
            ux = reshape(ux, b, rx(i)*n(i)*r);
            ux = ux.';
            rx(i+1) = r;
            crx{i+1} = vx;
            crx{i} = reshape(ux, rx(i), n(i), r, b);
            % Compute new reductions
            phixax(i+1) = rightreduce_matrix(phixax(i+2), vx, crA(i+1), vx, rx(i+1),n(i+1),rx(i+2), 1,ra(i+1),ra(i+2), rx(i+1),n(i+1),rx(i+2));
        end;
    else
        % Just stuff back the last core
        sol = reshape(sol, rx(i), n(i), rx(i+1), b);
        crx{i} = sol;
    end;

    if (verb>2)
        % Report the debug data if necessary
        if (dir>0)
            testdata{1}(i,swp) = toc(t_dmrg_eig);
            testdata{2}(i,swp,:) = theta;
            if (strcmp(trunc_norm, 'fro'))
                testdata{3}(i,swp) = dx;
            else
                testdata{3}(i,swp) = res_prev;
            end;
        else
            testdata{1}(d-numblocks+2-i,swp) = toc(t_dmrg_eig);
            testdata{2}(d-numblocks+2-i,swp,:) = theta;
            if (strcmp(trunc_norm, 'fro'))
                testdata{3}(d-numblocks+2-i,swp) = dx;
            else
                testdata{3}(d-numblocks+2-i,swp) = res_prev;
            end;
        end;
    end;

    i = i+dir;

    % Check for the end of the sweep
    if ((dir>0)&&(i>d-numblocks+1))||((dir<0)&&(i<1))
        % Report all
        if (verb>0)
            fprintf('=dmrg_eig= sweep %d, max_dx: %3.3e, max_res: %3.3e, erank: %g, theta: %3.15e, mv: %d\n', swp, max_dx, max_res, sqrt(rx(1:d)'*(n.*rx(2:d+1))/sum(n)), sum(theta), max_matvecs);
        end;

        % Check the stops
        if (strcmp(trunc_norm, 'fro'))
            if (max_dx<tol_exit)&&(verb<3)&&(dir>0)
                break;
            end;
        else
            if (max_res<tol_exit)&&(verb<3)&&(dir>0)
                break;
            end;
        end;

        swp = swp+1;
        % Go backward
        dir = -dir;
        i = i+dir;
        % Clear the errors
        max_dx = 0;
        max_res = 0;
        max_matvecs = 0;
    end;
end;

crx{d} = reshape(crx{d}, rx(d), n(d), b);
x = cell2core(tt_tensor, crx);

end

% Accumulates the left reduction W{1:k}'*A{1:k}*X{1:k}
function [WAX2] = leftreduce_matrix(WAX1, w, A, x, rw1,n,rw2, Ra,ra1,ra2, rx1,m,rx2)
% Left WAX has the form of the first matrix TT block, i.e. [rw, rx, ra]
WAX2 = WAX1;
wc = reshape(w, rw1, n*rw2);
xc = reshape(x, rx1*m, rx2);
for k=1:Ra
    WAX2{k} = reshape(WAX2{k}, rw1, rx1*ra1(k));
    WAX2{k} = wc'*WAX2{k}; % size n rw2 x rx1 ra1
    WAX2{k} = reshape(WAX2{k}, n, rw2*rx1*ra1(k));
    WAX2{k} = WAX2{k}.';
    WAX2{k} = reshape(WAX2{k}, rw2*rx1, ra1(k)*n);
    tmp = reshape(A{k}, ra1(k)*n, m*ra2(k));
    WAX2{k} = WAX2{k}*tmp; % size rw2 rx1 m ra2
    WAX2{k} = reshape(WAX2{k}, rw2, rx1*m*ra2(k));
    WAX2{k} = WAX2{k}.';
    WAX2{k} = reshape(WAX2{k}, rx1*m, ra2(k)*rw2);
    WAX2{k} = xc.'*WAX2{k}; % size rx2, ra2 rw2
    WAX2{k} = reshape(WAX2{k}, rx2*ra2(k), rw2);
    WAX2{k} = WAX2{k}.';
end;
end

% Accumulates the right reduction W{k:d}'*A{k:d}*X{k:d}
function [WAX1] = rightreduce_matrix(WAX2, w, A, x, rw1,n,rw2, Ra,ra1,ra2, rx1,m,rx2)
% Right WAX has the form of the last matrix TT block, i.e. [ra, rw, rx]
WAX1 = WAX2;
wc = reshape(w, rw1, n*rw2);
wc = conj(wc);
xc = reshape(x, rx1*m, rx2);
for k=1:Ra
    WAX1{k} = reshape(WAX1{k}, ra2(k)*rw2, rx2);
    WAX1{k} = xc*WAX1{k}.'; % size rx1 m x ra2 rw2
    WAX1{k} = reshape(WAX1{k}, rx1, m*ra2(k)*rw2);
    WAX1{k} = WAX1{k}.';
    WAX1{k} = reshape(WAX1{k}, m*ra2(k), rw2*rx1);
    tmp = reshape(A{k}, ra1(k)*n, m*ra2(k));
    WAX1{k} = tmp*WAX1{k}; % size ra1(k)*n, rw2*rx1
    WAX1{k} = reshape(WAX1{k}, ra1(k), n*rw2*rx1);
    WAX1{k} = WAX1{k}.';
    WAX1{k} = reshape(WAX1{k}, n*rw2, rx1*ra1(k));
    WAX1{k} = wc*WAX1{k}; % size rw1, rx1 ra1
    WAX1{k} = reshape(WAX1{k}, rw1*rx1, ra1(k));
    WAX1{k} = WAX1{k}.';
end;
end

% A matrix-vectors product for the matrix in the 3D TT (WAX1-A-WAX2), and
% full vectors of size (rx1*m*rx2) x b. Returns (rw1*n*rw2) x b
function [w]=local_matvec(x, rx1,m,rx2,b, rw1,n,rw2, WAX1, A, WAX2, Ra,ra1,ra2)
xc = reshape(x, rx1*m*rx2, []);
if (isempty(b))
    b = size(xc, 2);
end;
w = zeros(rw1*n*rw2, b);
xc = xc.';
xc = reshape(xc, b*rx1*m, rx2);
for k=1:Ra
    tmp = reshape(WAX2{k}, ra2(k)*rw2, rx2);
    wk = xc*tmp.';
    wk = reshape(wk, b*rx1, m*ra2(k)*rw2);
    wk = wk.';
    wk = reshape(wk, m*ra2(k), rw2*b*rx1);
    tmp = reshape(A{k}, ra1(k)*n, m*ra2(k));
    wk = tmp*wk;
    wk = reshape(wk, ra1(k)*n*rw2*b, rx1);
    wk = wk.';
    wk = reshape(wk, rx1*ra1(k), n*rw2*b);
    tmp = reshape(WAX1{k}, rw1, rx1*ra1(k));
    wk = tmp*wk;
    wk = reshape(wk, rw1*n*rw2, b);
    w = w+wk;
end;
end

% Builds the full (rw1*n*rw2) x (rx1*m*rx2) matrix from its TT blocks
function [B,sparseflag]=assemble_local_matrix(WAX1, A, WAX2, Ra,ra1,ra2, rw1,n,rw2, rx1,m,rx2)
% Check the sparsity of the matrix blocks
sparseflag = true;
for k=1:Ra
    if (~issparse(A{k}))
        sparseflag=false;
    end;
end;
if (sparseflag)
    B = sparse(rw2*rw1*n, rx2*rx1*m); % reverse order !!!
    % The reverse order is needed since usually the n x m part is large and
    % sparse, so let it be the senior dimension.
    % Note that currently only canonical sparse matrices are allowed
    for k=1:Ra
        tmp = reshape(WAX2{k}, rw2, rx2);
        tmp = sparse(tmp);
        Bk = reshape(WAX1{k}, rw1, rx1);
        Bk = sparse(Bk);
        Bk = kron(Bk, tmp); % mind endiannes
        Bk = kron(A{k}, Bk); % mind endiannes
        B = B+Bk;
    end;
else
    % There are dense blocks, everything is dense, and in the natural index
    % order
    B = zeros(rw1*n*rw2, rx1*m*rx2);
    for k=1:Ra
        Bk = reshape(WAX1{k}, rw1*rx1, ra1(k));
        tmp = reshape(A{k}, ra1(k), n*m*ra2(k));
        if (issparse(tmp))
            % Don't mix sparse if we are already full
            tmp = full(tmp);
        end;
        Bk = Bk*tmp;
        Bk = reshape(Bk, rw1, rx1, n, m*ra2(k));
        Bk = permute(Bk, [1,3,2,4]);
        Bk = reshape(Bk, rw1*n*rx1*m, ra2(k));
        tmp = reshape(WAX2{k}, ra2(k), rw2*rx2);
        Bk = Bk*tmp;
        Bk = reshape(Bk, rw1*n, rx1*m, rw2, rx2);
        Bk = permute(Bk, [1,3,2,4]);
        Bk = reshape(Bk, rw1*n*rw2, rx1*m*rx2);
        B = B+Bk;
    end;
end;
end


function y = m_sxbfbt(x_it,aparams,mparams)
%m_sxbfbt   ideal stabilized least squares commutator preconditioner
%   y = m_sxbfbt(x_it,aparams,mparams);
%   input
%          x_it         operand for preconditioning operator
%          aparams      structure defining coefficient matrix
%          mparams      structure defining preconditioning matrix
%   output
%          y            result of preconditioning operation
%
%   IFISS function: HCE; 15 March 2005.

nv = length(aparams.F);
nu = nv/2;
np = size(aparams.B,1);
Gdiag=spdiags(diag(mparams.G),0,nv,nv);

sigma = mparams.viscosity;

rv=x_it(1:nv); rp=x_it(nv+1:nv+np);

%% pressure solve
xB = (Gdiag\aparams.B')';
BBt = aparams.B*xB';

if mparams.domain==1,
   n_null = mparams.n_null;
   minor = [1:n_null-1,n_null+1:np]';
   rp1 = zeros(np,1);
   rp1(minor) = (BBt(minor,minor)+mparams.Cp1(minor,minor))\rp(minor);
   rp2 =  xB*(aparams.F*(xB'*rp1)) + sigma*(mparams.Cp2*rp1);
   zp = zeros(np,1);
   zp(minor) = - (BBt(minor,minor)+mparams.Cp1(minor,minor))\rp2(minor);
else
   zp = (BBt+mparams.Cp1)\rp;
   zp = -(BBt+mparams.Cp1) \ (xB*(aparams.F*(xB'*zp)) + sigma*(mparams.Cp2*zp) );
end

%% velocity solve
rv = rv-(aparams.B')*zp;
zv = aparams.F \ rv;
y = [zv;zp];


classdef MOMBIII < ALGORITHM
% <multi/many> <real/integer/label/binary/permutation>
% Many objective metaheuristic based on the R2 indicator II
% alpha   ---   0.5 --- Threshold of variances
% epsilon --- 0.001 --- Tolerance threshold
% record  ---     5 --- The record size of nadir vectors

%------------------------------- Reference --------------------------------
% R. Hernandez Gomez and C. A. Coello Coello, Improved metaheuristic based
% on the R2 indicator for many-objective optimization, Proceedings of the
% Annual Conference on Genetic and Evolutionary Computation, 2015, 679-686.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            [alpha,epsilon,recordSize] = Algorithm.ParameterSet(0.5,0.001,5);

            %% Generate random population
            [W,Problem.N] = UniformPoint(Problem.N,Problem.M);
            Population    = Problem.Initialization();
            % Ideal and nadir points
            zmin = min(Population.objs,[],1);
            zmax = max(Population.objs,[],1);
            % For storing the nadir vectors of a few generations
            Record = repmat(zmax,recordSize,1);
            % For storing whether each objective has been marked for a few
            % generations
            Mark = false(recordSize,Problem.M);
            % R2 ranking procedure
            [Rank,Norm] = R2Ranking(Population.objs,W,zmin,zmax);

            %% Optimization
            while Algorithm.NotTerminated(Population)
                MatingPool  = TournamentSelection(2,Problem.N,Rank,Norm);
                Offspring   = OperatorGA(Problem,Population(MatingPool));
                Population  = [Population,Offspring];
                [Rank,Norm] = R2Ranking(Population.objs,W,zmin,zmax);
                [~,rank]    = sortrows([Rank,Norm]);
                Population  = Population(rank(1:Problem.N));
                Rank        = Rank(rank(1:Problem.N));
                Norm        = Norm(rank(1:Problem.N));
                [zmin,zmax,Record,Mark] = UpdateReferencePoints(Population.objs,zmin,zmax,Record,Mark,alpha,epsilon);
            end
        end
    end
end


function d = stoi(x, y, fs_signal)
% The Short-Time Objective Intelligibility measure
%   d = stoi(x, y, fs_signal) returns the output of the short-time
%   objective intelligibility (STOI) measure described in [1, 2], where x
%   and y denote the clean and processed speech, respectively, with sample
%   rate fs_signal in Hz. The output d is expected to have a monotonic
%   relation with the subjective speech-intelligibility, where a higher d
%   denotes better intelligible speech. See [1, 2] for more details.
%
%   References:
%      [1] C.H.Taal, R.C.Hendriks, R.Heusdens, J.Jensen 'A Short-Time
%      Objective Intelligibility Measure for Time-Frequency Weighted Noisy
%      Speech', ICASSP 2010, Texas, Dallas.
%
%      [2] C.H.Taal, R.C.Hendriks, R.Heusdens, J.Jensen 'An Algorithm for
%      Intelligibility Prediction of Time-Frequency Weighted Noisy Speech',
%      IEEE Transactions on Audio, Speech, and Language Processing, 2011.
%
%
% Copyright 2009: Delft University of Technology, Signal & Information
% Processing Lab. The software is free for non-commercial use. This program
% comes WITHOUT ANY WARRANTY.
%
%
%
% Updates:
% 2011-04-26 Using the more efficient 'taa_corr' instead of 'corr'

if length(x)~=length(y)
    error('x and y should have the same length');
end

% initialization
x           = x(:);                             % clean speech column vector
y           = y(:);                             % processed speech column vector

fs          = 10000;                            % sample rate of proposed intelligibility measure 10000
N_frame    	= 256;                              % window support 256
K           = 512;                              % FFT size 512
J           = 15;                               % Number of 1/3 octave bands
mn          = 150;                              % Center frequency of first 1/3 octave band in Hz.
H           = thirdoct(fs, K, J, mn);           % Get 1/3 octave band matrix
N           = 30;                               % Number of frames for intermediate intelligibility measure (Length analysis window)
Beta        = -15;                           	% lower SDR-bound -15
dyn_range   = 40;                               % speech dynamic range 40

% resample signals if other samplerate is used than fs
if fs_signal ~= fs
    x	= resample(x, fs, fs_signal);
    y 	= resample(y, fs, fs_signal);
end

% remove silent frames
[x y] = removeSilentFrames(x, y, dyn_range, N_frame, N_frame/2);

% apply 1/3 octave band TF-decomposition
x_hat     	= stdft(x, N_frame, N_frame/2, K); 	% apply short-time DFT to clean speech
y_hat     	= stdft(y, N_frame, N_frame/2, K); 	% apply short-time DFT to processed speech

x_hat       = x_hat(:, 1:(K/2+1)).';         	% take clean single-sided spectrum
y_hat       = y_hat(:, 1:(K/2+1)).';        	% take processed single-sided spectrum

X           = zeros(J, size(x_hat, 2));         % init memory for clean speech 1/3 octave band TF-representation
Y           = zeros(J, size(y_hat, 2));         % init memory for processed speech 1/3 octave band TF-representation

for i = 1:size(x_hat, 2)
    X(:, i)	= sqrt(H*abs(x_hat(:, i)).^2);      % apply 1/3 octave bands as described in Eq.(1) [1]
    Y(:, i)	= sqrt(H*abs(y_hat(:, i)).^2);
end

% loop al segments of length N and obtain intermediate intelligibility measure for all TF-regions
d_interm  	= zeros(J, length(N:size(X, 2)));                               % init memory for intermediate intelligibility measure
c           = 10^(-Beta/20);                                                % constant for clipping procedure

for m = N:size(X, 2)
    X_seg  	= X(:, (m-N+1):m);                                              % region with length N of clean TF-units for all j
    Y_seg  	= Y(:, (m-N+1):m);                                              % region with length N of processed TF-units for all j
    alpha   = sqrt(sum(X_seg.^2, 2)./sum(Y_seg.^2, 2));                     % obtain scale factor for normalizing processed TF-region for all j
    aY_seg 	= Y_seg.*repmat(alpha, [1 N]);                               	% obtain \alpha*Y_j(n) from Eq.(2) [1]
    for j = 1:J
      	Y_prime             = min(aY_seg(j, :), X_seg(j, :)+X_seg(j, :)*c); % apply clipping from Eq.(3)
        d_interm(j, m-N+1)  = taa_corr(X_seg(j, :).', Y_prime(:));          % obtain correlation coeffecient from Eq.(4) [1]
    end
end

d = mean(d_interm(:));                                                      % combine all intermediate intelligibility measures as in Eq.(4) [1]

%%
function  [A cf] = thirdoct(fs, N_fft, numBands, mn)
%   [A CF] = THIRDOCT(FS, N_FFT, NUMBANDS, MN) returns 1/3 octave band matrix
%   inputs:
%       FS:         samplerate
%       N_FFT:      FFT size
%       NUMBANDS:   number of bands
%       MN:         center frequency of first 1/3 octave band
%   outputs:
%       A:          octave band matrix
%       CF:         center frequencies

f               = linspace(0, fs, N_fft+1);
f               = f(1:(N_fft/2+1));
k               = 0:(numBands-1);
cf              = 2.^(k/3)*mn;
fl              = sqrt((2.^(k/3)*mn).*2.^((k-1)/3)*mn);
fr              = sqrt((2.^(k/3)*mn).*2.^((k+1)/3)*mn);
A               = zeros(numBands, length(f));

for i = 1:(length(cf))
    [a b]                   = min((f-fl(i)).^2);
    fl(i)                   = f(b);
    fl_ii                   = b;

	[a b]                   = min((f-fr(i)).^2);
    fr(i)                   = f(b);
    fr_ii                   = b;
    A(i,fl_ii:(fr_ii-1))	= 1;
end

rnk         = sum(A, 2);
numBands  	= find((rnk(2:end)>=rnk(1:(end-1))) & (rnk(2:end)~=0)~=0, 1, 'last' )+1;
A           = A(1:numBands, :);
cf          = cf(1:numBands);

%%
function x_stdft = stdft(x, N, K, N_fft)
%   X_STDFT = X_STDFT(X, N, K, N_FFT) returns the short-time
%	hanning-windowed dft of X with frame-size N, overlap K and DFT size
%   N_FFT. The columns and rows of X_STDFT denote the frame-index and
%   dft-bin index, respectively.

frames      = 1:K:(length(x)-N);
x_stdft     = zeros(length(frames), N_fft);

w           = hanning(N);
x           = x(:);

for i = 1:length(frames)
    ii              = frames(i):(frames(i)+N-1);
	x_stdft(i, :) 	= fft(x(ii).*w, N_fft);
end

%%
function [x_sil y_sil] = removeSilentFrames(x, y, range, N, K)
%   [X_SIL Y_SIL] = REMOVESILENTFRAMES(X, Y, RANGE, N, K) X and Y
%   are segmented with frame-length N and overlap K, where the maximum energy
%   of all frames of X is determined, say X_MAX. X_SIL and Y_SIL are the
%   reconstructed signals, excluding the frames, where the energy of a frame
%   of X is smaller than X_MAX-RANGE

x       = x(:);
y       = y(:);

frames  = 1:K:(length(x)-N);
w       = hanning(N);
msk     = zeros(size(frames));

for j = 1:length(frames)
    jj      = frames(j):(frames(j)+N-1);
    msk(j) 	= 20*log10(norm(x(jj).*w)./sqrt(N));
end

msk     = (msk-max(msk)+range)>0;
count   = 1;

x_sil   = zeros(size(x));
y_sil   = zeros(size(y));

for j = 1:length(frames)
    if msk(j)
        jj_i            = frames(j):(frames(j)+N-1);
        jj_o            = frames(count):(frames(count)+N-1);
        x_sil(jj_o)     = x_sil(jj_o) + x(jj_i).*w;
        y_sil(jj_o)  	= y_sil(jj_o) + y(jj_i).*w;
        count           = count+1;
    end
end

x_sil = x_sil(1:jj_o(end));
y_sil = y_sil(1:jj_o(end));

%%
function rho = taa_corr(x, y)
%   RHO = TAA_CORR(X, Y) Returns correlation coeffecient between column
%   vectors x and y. Gives same results as 'corr' from statistics toolbox.
xn    	= x-mean(x);
xn  	= xn/sqrt(sum(xn.^2));
yn   	= y-mean(y);
yn    	= yn/sqrt(sum(yn.^2));
rho   	= sum(xn.*yn);


% LFBuild4DFreqHypercone - construct a 4D hypercone passband filter in the frequency domain
%
% Usage:
%
%     [H, FiltOptions] = LFBuild4DFreqHypercone( LFSize, Slope, BW, FiltOptions )
%     H = LFBuild4DFreqHypercone( LFSize, Slope, BW )
%
% This file constructs a real-valued magnitude response in 4D, for which the passband is a hypercone.
%
% Once constructed the filter must be applied to a light field, e.g. using LFFilt4DFFT. The
% LFDemoBasicFilt* files demonstrate how to contruct and apply frequency-domain filters.
%
% A more technical discussion, including the use of filters for denoising and volumetric focus, and
% the inclusion of aliases components, is included in:
%
% [2] D.G. Dansereau, O. Pizarro, and S. B. Williams, "Linear Volumetric Focus for Light Field
% Cameras," to appear in ACM Transactions on Graphics (TOG), vol. 34, no. 2, 2015.
%
% Inputs:
%
%     LFSize : Size of the frequency-domain filter. This should match or exceed the size of the
%     light field to be filtered. If it's larger than the input light field, the input is
%     zero-padded to match the filter's size by LFFilt4DFFT.
%
%     Slope : The slope of the planar passband. If different slopes are desired in s,t and u,v,
%     the optional aspect parameter should be used.
%
%     BW : 3-db Bandwidth of the planar passband.
%
%     [optional] FiltOptions : struct controlling filter construction
%               SlopeMethod : 'Skew' or 'Rotate' default 'skew'
%                 Precision : 'single' or 'double', default 'single'
%                   Rolloff : 'Gaussian' or 'Butter', default 'Gaussian'
%                     Order : controls the order of the filter when Rolloff is 'Butter', default 3
%                  Aspect4D : aspect ratio of the light field, default [1 1 1 1]
%                    Window : Default false. By default the edges of the passband are sharp; this adds
%                             a smooth rolloff at the edges when used in conjunction with Extent4D or
%                             IncludeAliased.
%                  Extent4D : controls where the edge of the passband occurs, the default [1 1 1 1]
%                             is the edge of the Nyquist box. When less than 1, enabling windowing
%                             introduces a rolloff after the edge of the passband. Can be greater
%                             than 1 when using IncludeAliased.
%            IncludeAliased : default false; allows the passband to wrap around off the edge of the
%                             Nyquist box; used in conjunction with Window and/or Extent4D. This can
%                             increase processing time dramatically, e.g. Extent4D = [2,2,2,2]
%                             requires a 2^4 = 16-fold increase in time to construct the filter.
%                             Useful when passband content is aliased, see [2].
%
% Outputs:
%
%                 H : real-valued frequency magnitude response
%       FiltOptions : The filter options including defaults, with an added PassbandInfo field
%                     detailing the function and time of construction of the filter
%
% User guide: LFToolbox.pdf
% See also:  LFDemoBasicFiltGantry, LFDemoBasicFiltIllum, LFDemoBasicFiltLytroF01,
% LFBuild2DFreqFan, LFBuild2DFreqLine, LFBuild4DFreqDualFan, LFBuild4DFreqHypercone,
% LFBuild4DFreqHyperfan, LFBuild4DFreqPlane, LFFilt2DFFT, LFFilt4DFFT, LFFiltShiftSum

% Copyright (c) 2013-2020 Donald G. Dansereau

function [H, FiltOptions] = LFBuild4DFreqHypercone( LFSize, BW, FiltOptions )

FiltOptions = LFDefaultField('FiltOptions', 'HyperconeMethod', 'Rotated'); % 'Direct', 'Rotated'

DistFunc = @(P, FiltOptions) DistFunc_4DCone( P, FiltOptions );
[H, FiltOptions] = LFHelperBuild4DFreq( LFSize, BW, FiltOptions, DistFunc );

TimeStamp = datestr(now,'ddmmmyyyy_HHMMSS');
FiltOptions.PassbandInfo = struct('mfilename', mfilename, 'time', TimeStamp, 'VersionStr', LFToolboxVersion);

end

%-----------------------------------------------------------------------------------------------------------------------
function Dist = DistFunc_4DCone( P, FiltOptions )
switch( lower(FiltOptions.HyperconeMethod ))
	case 'direct'
		Dist = (P(1,:).*P(4,:) - P(2,:).*P(3,:)).^2 * 4;

	case 'rotated'
		R = 1/sqrt(2) .* [1,0,0,1; 0,1,1,0; 0,1,-1,0; 1,0,0,-1];
		P = R*P;
		Dist = (sqrt(P(1,:).^2 + P(3,:).^2) - sqrt(P(2,:).^2 + P(4,:).^2)).^2 /2;

	otherwise
		error('Unrecognized hypercone construction method');
end
end


function [soln,eqn,info] = PoissonWG(node,elem,bdFlag,pde,option)
%% POISSONWG Poisson equation: lowest order weak Galerkin element
%
%   u = POISSONWG(node,elem,bdFlag,pde) produces the linear finite element
%   approximation of the Poisson equation
%
%       -div(d*grad(u))=f  in \Omega, with
%       Dirichlet boundary condition u=g_D on \Gamma_D,
%       Neumann boundary condition   d*grad(u)*n=g_N on \Gamma_N,
%       Robin boundary condition     g_R*u + d*grad(u)*n=g_N on \Gamma _R
%
% The usage is the same as Poisson. Weak Galerkin method on a triangle is
% summarized in PoissonWGfemrate for detail.
%
%   Example
%
%     squarePoissonWG;
%
%   See also Poisson, squarePoissonWG
%
%   Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Preprocess
if ~exist('bdFlag','var'), bdFlag = []; end
if ~exist('option','var'), option = []; end
% important constants
NT = size(elem,1);
N = size(node,1);

%% Diffusion coefficient
time = cputime;  % record assembling time
if ~isfield(pde,'d'), pde.d = []; end
if ~isfield(option,'dquadorder'), option.dquadorder = 1; end
if ~isempty(pde.d) && isnumeric(pde.d)
   K = pde.d;                                 % d is an array
end
if ~isempty(pde.d) && ~isnumeric(pde.d)       % d is a function
    [lambda,weight] = quadpts(option.dquadorder);
    nQuad = size(lambda,1);
    K = zeros(NT,1);
    for p = 1:nQuad
		pxy = lambda(p,1)*node(elem(:,1),:) ...
			+ lambda(p,2)*node(elem(:,2),:) ...
			+ lambda(p,3)*node(elem(:,3),:);
        K = K + weight(p)*pde.d(pxy);
   end
end

%% Construct data structure
[elem2edge,edge] = dofedge(elem);
NE = size(edge,1);
Ndof = NT + NE;
elem2dof = NT + elem2edge;

%% Assemble stiffness matrix
A = sparse(Ndof,Ndof);

% compute ct2 = 1/mean(||x-xc||^2)
center = (node(elem(:,1),:) + node(elem(:,2),:) + node(elem(:,3),:))/3;
mid1 = (node(elem(:,2),:) + node(elem(:,3),:))/2;
mid2 = (node(elem(:,3),:) + node(elem(:,1),:))/2;
mid3 = (node(elem(:,1),:) + node(elem(:,2),:))/2;
ct2 = 3./sum((mid1 - center).^2 + (mid2 - center).^2 + (mid3 - center).^2,2);
[Dphi,area] = gradbasis(node,elem);
clear center mid1 mid2 mid3

% Mbb: edge - edge
for i = 1:3
    for j = i:3
        % local to global index map
        ii = double(elem2dof(:,i));
        jj = double(elem2dof(:,j));
        % local stiffness matrix
        Aij = 4*dot(Dphi(:,:,i),Dphi(:,:,j),2).*area + 4/9*ct2.*area;
        if ~isempty(pde.d)
            Aij = K.*Aij;
        end
        if (j==i)
            A = A + sparse(ii,jj,Aij,Ndof,Ndof);
        else
            A = A + sparse([ii,jj],[jj,ii],[Aij; Aij],Ndof,Ndof);
        end
    end
end

% Mob: interior - edge
Aij = -4/3*ct2.*area;
if ~isempty(pde.d)
    Aij = K.*Aij;
end
Mob = sparse([(1:NT)', (1:NT)', (1:NT)'], ...
             double(elem2dof(:)), [Aij, Aij, Aij], Ndof, Ndof);
A = A + Mob + Mob';

% Moo: diagonal of interor
Aij = 4*ct2.*area;
if ~isempty(pde.d)
    Aij = K.*Aij;
end
A =  A + sparse(1:NT, 1:NT, Aij, Ndof, Ndof);

clear K Aij

%% Assemble the right hand side
b = zeros(Ndof,1);
if ~isfield(option,'fquadorder')
    option.fquadorder = 2;   % default order
end
if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
    pde.f = [];
end
if isreal(pde.f) % f is a real number or vector and not a function
   switch length(pde.f)
       case NT  % f is piecewise constant
         b(1:NT) = pde.f.*area;
       case N   % f is piecewise linear
         b(1:NT) = (pde.f(elem(:,1)) + pde.f(elem(:,2)) + pde.f(elem(:,3)))/3.*area;
       case 1   % f is a scalar e.g. f = 1
         b(1:NT) = pde.f*area;
   end
end
if ~isempty(pde.f) && ~isreal(pde.f)  % f is a function
    [lambda,weight] = quadpts(option.fquadorder);
	nQuad = size(lambda,1);
    bt = zeros(NT,1);
    for p = 1:nQuad
		% quadrature points in the x-y coordinate
		pxy = lambda(p,1)*node(elem(:,1),:) ...
			+ lambda(p,2)*node(elem(:,2),:) ...
			+ lambda(p,3)*node(elem(:,3),:);
		fp = pde.f(pxy);
        bt = bt + weight(p)*fp;
    end
    bt = bt.*area;
    b(1:NT) = bt;
end
clear pxy bt

%% Set up boundary conditions
if nargin<=3, bdFlag = []; end
[AD,b,u,freeDof,isPureNeumann] = getbdWG(A,b);

%% Record assembling time
assembleTime = cputime - time;
if ~isfield(option,'printlevel'), option.printlevel = 1; end
if option.printlevel >= 2
    fprintf('Time to assemble matrix equation %4.2g s\n',assembleTime);
end

%% Solve the system of linear equations
if isempty(freeDof), return; end
% Set up solver type
if isempty(option) || ~isfield(option,'solver')    % no option.solver
    if NE <= 1e3  % Direct solver for small size systems
        option.solver = 'direct';
    else          % MGCG  solver for large size systems
        option.solver = 'mg';
    end
end
solver = option.solver;
% solve
switch solver
    case 'direct'
        tic;
        u(freeDof) = AD(freeDof,freeDof)\b(freeDof);
        residual = norm(b - AD*u);
        info = struct('solverTime',toc,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'none'
        info = struct('solverTime',[],'itStep',0,'err',[],'flag',3,'stopErr',[]);
    case 'mg'
        % eleminate elementwise dof
        option.solver = 'CG';
        if isfield(option,'reducesystem') && (option.reducesystem == 0)
            option.x0 = u;
            [u,info] = mg(AD,b,elem,option,edge);
        else
            option.x0 = u(NT+1:end);
            Aoinv = spdiags(1./diag(AD(1:NT,1:NT)),0,NT,NT);
            Aob = AD(1:NT,NT+1:end);
            Abo = Aob';
            Abb = AD(NT+1:end,NT+1:end);
            Abbm = Abb - Abo*Aoinv*Aob;
            bm = -Abo*Aoinv*b(1:NT) + b(NT+1:end);
            [ub,info] = mg(Abbm,bm,elem,option,edge);
            u(1:NT) = Aoinv*(b(1:NT) - Aob*ub);
            u(NT+1:end) = ub;
        end
    case 'amg'
        option.solver = 'CG';
        [u(freeDof),info] = amg(AD(freeDof,freeDof),b(freeDof),option);
end
% post-process for pure Neumann problem
if isPureNeumann
    uc = sum(u(1:NT).*area);
    u = u - uc;   % normalization for pure Neumann problem
end

%% Compute Du
dudx =  u(elem2dof(:,1)).*Dphi(:,1,1) + u(elem2dof(:,2)).*Dphi(:,1,2) ...
      + u(elem2dof(:,3)).*Dphi(:,1,3);
dudy =  u(elem2dof(:,1)).*Dphi(:,2,1) + u(elem2dof(:,2)).*Dphi(:,2,2) ...
      + u(elem2dof(:,3)).*Dphi(:,2,3);
Du = -2*[dudx, dudy];

%% Output information
if nargout == 1
    soln = u;
else
    soln = struct('u',u,'Du',Du);
    eqn = struct('A',AD,'b',b,'edge',edge,'freeDof',freeDof);
    info.assembleTime = assembleTime;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions getbdWG
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [AD,b,u,freeDof,isPureNeumann]= getbdWG(A,b)
    %% GETBDCR Boundary conditions for Poisson equation: WG element.

    u =zeros(Ndof,1);
    %% Initial check
    if ~isfield(pde,'g_D'), pde.g_D = []; end
    if ~isfield(pde,'g_N'), pde.g_N = []; end
    if ~isfield(pde,'g_R'), pde.g_R = []; end

    %% Part 1: Modify the matrix for Dirichlet and Robin condition
    % Robin boundary condition
    Robin = [];
    idxR = (bdFlag(:) == 3);      % index of Robin edges in bdFlag
    if any(idxR)
        isRobin = false(NE,1);
        isRobin(elem2edge(idxR)) = true;
        Robin = edge(isRobin,:);  % Robin edges
    end
    if ~isempty(Robin) && ~isempty(pde.g_R) && ~(isnumeric(pde.g_R) && (pde.g_R == 0))
        ve = node(Robin(:,1),:) - node(Robin(:,2),:);
        edgeLength = sqrt(sum(ve.^2,2));
        mid = (node(Robin(:,1),:) + node(Robin(:,2),:))/2;
        ii = NT + find(isRobin);  % for WG: edge dof is after elem dof
        ss = pde.g_R(mid).*edgeLength; % exact for linear g_R
        A = A + sparse(ii,ii,ss,Ndof,Ndof);
    end

    % Find Dirichlet boundary nodes: fixedEdge
    fixedEdge = []; freeEdge = [];
    if ~isempty(bdFlag)              % find boundary edges
        idxD = (bdFlag(:) == 1);     % all Dirichlet edges in bdFlag
        isFixedEdge = false(NE,1);
        isFixedEdge(elem2edge(idxD)) = true;  % index of fixed boundary edges
        fixedEdge = find(isFixedEdge);
        freeEdge = find(~isFixedEdge);
    end
    if isempty(bdFlag) && ~isempty(pde.g_D) && isempty(pde.g_N) && isempty(pde.g_R)
        % no bdFlag, only pde.g_D is given in the input
        s = accumarray(elem2edge(:), 1, [NE 1]);
        fixedEdge = find(s == 1);
        freeEdge = find(s == 2);
    end
    isPureNeumann = false;
    if isempty(fixedEdge) && isempty(Robin)  % pure Neumann boundary condition
        % pde.g_N could be empty which is homogenous Neumann boundary condition
        isPureNeumann = true;
        fixedEdge = 1;
        freeEdge = (2:NE)';    % eliminate the kernel by enforcing u(1) = 0;
    end
    % Modify the matrix
    % Build Dirichlet boundary condition into the matrix AD by enforcing
    % AD(fixedEdge,fixedEdge)=I, AD(fixedEdge,freeEdge)=0, AD(freeEdge,fixedEdge)=0.
    if ~isempty(fixedEdge)
        bdidx = zeros(Ndof,1);
        bdidx(NT + fixedEdge) = 1;
        Tbd = spdiags(bdidx,0,Ndof,Ndof);
        T = spdiags(1-bdidx,0,Ndof,Ndof);
        AD = T*A*T + Tbd;
    else
        AD = A;
    end

    %% Part 2: Find boundary edges and modify the right hand side b
    % Find boundary edges: Neumann
    Neumann = [];
    if ~isempty(bdFlag)  % bdFlag specifies different bd conditions
        idxN = (bdFlag(:) == 2);      % all Neumann edges in bdFlag
        isNeumann = elem2edge(idxN | idxR); % index of Neumann and Robin edges
        % since boundary integral is also needed for Robin edges
        Neumann = edge(isNeumann,:);      % Neumann edges
    end
    if isempty(bdFlag) && (~isempty(pde.g_N) || ~isempty(pde.g_R))
        % no bdFlag, only pde.g_N or pde.g_R is given in the input
        s = accumarray(elem2edge(:), 1, [NE 1]);
        Neumann = edge(s == 1,:);
    end

    % Neumann boundary condition
    if ~isempty(Neumann) && ~isempty(pde.g_N) && ~(isnumeric(pde.g_N) && (pde.g_N == 0))
        if ~isfield(option,'gNquadorder')
            option.gNquadorder = 3;   % default order exact for linear gN
        end
        [lambdagN,weightgN] = quadpts1(option.gNquadorder);
        nQuadgN = size(lambdagN,1);
        ge = zeros(size(Neumann,1),1);
        el = sqrt(sum((node(Neumann(:,1),:) - node(Neumann(:,2),:)).^2,2));
        for pp = 1:nQuadgN
            % quadrature points in the x-y coordinate
            ppxy = lambdagN(pp,1)*node(Neumann(:,1),:) ...
                 + lambdagN(pp,2)*node(Neumann(:,2),:);
            gNp = pde.g_N(ppxy);
            ge = ge+ weightgN(pp)*gNp;
        end
        ge = ge.*el;
        b(NT+isNeumann) = b(NT+isNeumann) + ge;
    end
    % The case with non-empty Neumann edges but g_N=0 or g_N=[] corresponds to
    % the zero flux boundary condition on Neumann edges and no modification of
    % A,u,b is needed.

    % Dirichlet boundary condition
    if ~isPureNeumann && ~isempty(fixedEdge) && ...
       ~isempty(pde.g_D) && ~(isnumeric(pde.g_D) && all(pde.g_D == 0))    % nonzero g_D
        if isnumeric(pde.g_D)  % pde.g_D could be a numerical array
            u(fixedEdge) = pde.g_D(fixedEdge);
        else % pde.g_D is a function handle
            mid = (node(edge(fixedEdge,1),:) + node(edge(fixedEdge,2),:))/2;
            u(NT+fixedEdge) = pde.g_D(mid);
        end
        b = b - A*u;
        b(NT+fixedEdge) = u(NT+fixedEdge);
    end
    % The case with non-empty Dirichlet nodes but g_D=0 or g_D=[] corresponds
    % to the zero Dirichlet boundary condition and no modification of u,b is
    % needed.

    % Pure Neumann boundary condition
    if isPureNeumann
        b = b - mean(b);   % compatilbe condition: sum(b) = 0
        b(1) = 0;
    end

    freeDof = [(1:NT)'; NT+freeEdge];
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end % end of PoissonWG


function t = s2t(s, epsilon);

% T = s2t(S, EPSILON)
%
% Scattering to Transmission Transformation
%
% since 2 is the only allowed number of ports for a T-matrix,
% S, T are matrices of size [2,2,F],
% where F is the number of frequencies
%
% EPSILON is used to produce an approximation of the corresponding S-matrix
% in exceptional cases
%
% 3-d version 4 nov 1999

if nargin < 2 epsilon = 1e-14; end;
[n,i] = min(abs(s(2,1,:)));
while n <= epsilon
    s(2,1,i) = s(2,1,i)+rand*epsilon;
    [n,i] = min(abs(s(2,1,:)));
end;

t(1,1,:) = 1./s(2,1,:);
t(1,2,:) = -s(2,2,:)./s(2,1,:);
t(2,1,:) = s(1, 1,:)./s(2,1,:);
t(2,2,:) = s(1, 2,:) - s(1, 1,:).*s(2,2,:)./s(2,1,:);


function [w]=obtencion_w(R)
    j=1;
    x=1;
    i=1;

    %Desmontamos la matriz
    while j<100
        r11(j)=R{j}(1,1);
        r12(j)=R{j}(1,2);
        r13(j)=R{j}(1,3);

        r21(j)=R{j}(2,1);
        r22(j)=R{j}(2,2);
        r23(j)=R{j}(2,3);

        r31(j)=R{j}(3,1);
        r32(j)=R{j}(3,2);
        r33(j)=R{j}(3,3);

        j=j+1;
    end

    %Derivamos los vectores
    rd11=diff(r11);
    rd12=diff(r12);
    rd13=diff(r13);

    rd21=diff(r21);
    rd22=diff(r22);
    rd23=diff(r23);

    rd31=diff(r31);
    rd32=diff(r32);
    rd33=diff(r33);

    %Motamos la matriz derivada
    while x<99
        dR{x}(1,1)=rd11(x);
        dR{x}(1,2)=rd12(x);
        dR{x}(1,3)=rd13(x);

        dR{x}(2,1)=rd21(x);
        dR{x}(2,2)=rd22(x);
        dR{x}(2,3)=rd23(x);

        dR{x}(3,1)=rd31(x);
        dR{x}(3,2)=rd32(x);
        dR{x}(3,3)=rd33(x);

        x=x+1;
    end

    %Multiplicamos dR por R(transpuesta) para sacar w
    while i<99
        w{i}=dR{i}*(R{i}.');

        i=i+1;
    end
end


function b = r8gb_vxm ( m, n, ml, mu, a, x )

%*****************************************************************************80
%
%% R8GB_VXM multiplies a vector by a R8GB matrix.
%
%  Discussion:
%
%    An M by N banded matrix A with lower bandwidth ML and upper bandwidth MU
%    is assumed to be entirely zero, except for the main diagonal, and
%    entries in the ML nearest subdiagonals, and MU nearest superdiagonals.
%
%    LINPACK and LAPACK "R8GB" storage for such a matrix generally includes
%    room for ML extra superdiagonals, which may be required to store
%    nonzero entries generated during Gaussian elimination.
%
%    The original M by N matrix is "collapsed" downward, so that diagonals
%    become rows of the storage array, while columns are preserved.  The
%    collapsed array is logically 2*ML+MU+1 by N.
%
%    LINPACK and LAPACK storage of general band matrices requires
%    an extra ML upper diagonals for possible fill in entries during
%    Gauss elimination.  This routine does not access any entries
%    in the fill in diagonals, because it assumes that the matrix
%    has NOT had Gauss elimination applied to it.  If the matrix
%    has been Gauss eliminated, then the routine R8GB_MU must be
%    used instead.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 November 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of rows of the matrix.
%    M must be positive.
%
%    Input, integer N, the number of columns of the matrix.
%    N must be positive.
%
%    Input, integer ML, MU, the lower and upper bandwidths.
%    ML and MU must be nonnegative, and no greater than min(M,N)-1.
%
%    Input, real A(2*ML+MU+1,N), the R8GB matrix.
%
%    Input, real X(M), the vector to be multiplied by A.
%
%    Output, real B(N), the product X*A.
%
  b(1:n) = 0.0;

  for j = 1 : n
    ilo = max ( 1, j - mu );
    ihi = min ( m, j + ml );
    for i = ilo : ihi
      b(j) = b(j) + x(i) * a(i-j+ml+mu+1,j);
    end
  end

  return
end


function [Xw, Yw, Zw]=imageToWorld(Xi, Yi, camPar)

% if scalar, just up/downscale = orthographic
if camPar.ortho
    Xw=Xi*camPar.scale;
    Yw=Yi*camPar.scale;
    Zw=0;
else

    mGeo=camPar.mGeo;
    mExt=camPar.mExt;
    mInt=camPar.mInt;

    mTx=mExt.mTx;
    mTy=mExt.mTy;
    mTz=mExt.mTz;

    mT=[mExt.mTx;mExt.mTy;mExt.mTz];

    %% internal init
    sa = sin(mExt.mRx);
    ca = cos(mExt.mRx);
    sb = sin(mExt.mRy);
    cb = cos(mExt.mRy);
    sg = sin(mExt.mRz);
    cg = cos(mExt.mRz);

    mR11 = cb * cg;
    mR12 = cg * sa * sb - ca * sg;
    mR13 = sa * sg + ca * cg * sb;
    mR21 = cb * sg;
    mR22 = sa * sb * sg + ca * cg;
    mR23 = ca * sb * sg - cg * sa;
    mR31 = -sb;
    mR32 = cb * sa;
    mR33 = ca * cb;

    % 		/* convert from image to distorted sensor coordinates */
    Xd = mGeo.mDpx * (Xi - mInt.mCx) / mInt.mSx;
    Yd = mGeo.mDpy * (Yi - mInt.mCy);

    % 		/* convert from distorted sensor to undistorted sensor plane coordinates */
    [Xu Yu]=distortedToUndistortedSensorCoord (Xd, Yd, mInt.mKappa1);

    % 		/* calculate the corresponding xw and yw world coordinates	 */
    % 		/* (these equations were derived by simply inverting	 */
    % 		/* the perspective projection equations using Macsyma)	 */
    Zw=0;
    common_denominator = ((mR11 * mR32 - mR12 * mR31) * Yu + ...
        (mR22 * mR31 - mR21 * mR32) * Xu - ...
        mInt.mFocal * mR11 * mR22 + mInt.mFocal * mR12 * mR21);

    Xw = (((mR12 * mR33 - mR13 * mR32) * Yu + ...
        (mR23 * mR32 - mR22 * mR33) * Xu - ...
        mInt.mFocal * mR12 * mR23 + mInt.mFocal * mR13 * mR22) * Zw + ...
        (mR12 * mTz - mR32 * mTx) * Yu + ...
        (mR32 * mTy - mR22 * mTz) * Xu - ...
        mInt.mFocal * mR12 * mTy + mInt.mFocal * mR22 * mTx) / common_denominator;

    Yw = -(((mR11 * mR33 - mR13 * mR31) * Yu + ...
        (mR23 * mR31 - mR21 * mR33) * Xu - ...
        mInt.mFocal * mR11 * mR23 + mInt.mFocal * mR13 * mR21) * Zw + ...
        (mR11 * mTz - mR31 * mTx) * Yu + ...
        (mR31 * mTy - mR21 * mTz) * Xu - ...
        mInt.mFocal * mR11 * mTy + mInt.mFocal * mR21 * mTx) / common_denominator;
% else
%     error('imageToWorld: camera parameters format unknown');
end
end


function y = sgemv ( trans, m, n, alpha, a, lda, x, incx, beta, y, incy )

%*****************************************************************************80
%
%% SGEMV computes y := alpha * A * x + beta * y for general matrix A.
%
%  Discussion:
%
%    SGEMV performs one of the matrix-vector operations
%      y := alpha*A *x + beta*y
%    or
%      y := alpha*A'*x + beta*y,
%    where alpha and beta are scalars, x and y are vectors and A is an
%    m by n matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 February 2014
%
%  Author:
%
%    Jack Dongarra, Jeremy Du Croz, Sven Hammarling,  Richard Hanson.
%
%  Parameters:
%
%    Input, character TRANS, specifies the operation to be performed:
%    'n' or 'N'   y := alpha*A *x + beta*y.
%    't' or 'T'   y := alpha*A'*x + beta*y.
%    'c' or 'C'   y := alpha*A'*x + beta*y.
%
%    Input, integer M, the number of rows of the matrix A.
%    0 <= M.
%
%    Input, integer N, the number of columns of the matrix A.
%    0 <= N.
%
%    Input, real ALPHA, the scalar multiplier for A * x.
%
%    Input, real A(LDA,N).  The M x N subarray contains
%    the matrix A.
%
%    Input, integer LDA, the the first dimension of A as declared
%    in the calling routine.  max ( 1, M ) <= LDA.
%
%    Input, real X(*), an array containing the vector to be
%    multiplied by the matrix A.
%    If TRANS = 'N' or 'n', then X must contain N entries, stored in INCX
%    increments in a space of at least ( 1 + ( N - 1 ) * abs ( INCX ) )
%    locations.
%    Otherwise, X must contain M entries, store in INCX increments
%    in a space of at least ( 1 + ( M - 1 ) * abs ( INCX ) ) locations.
%
%    Input, integer INCX, the increment for the elements of
%    X.  INCX must not be zero.
%
%    Input, real BETA, the scalar multiplier for Y.
%
%    Input/output, real Y(*), an array containing the vector to
%    be scaled and incremented by A*X.
%    If TRANS = 'N' or 'n', then Y must contain M entries, stored in INCY
%    increments in a space of at least ( 1 + ( M - 1 ) * abs ( INCY ) )
%    locations.
%    Otherwise, Y must contain N entries, store in INCY increments
%    in a space of at least ( 1 + ( N - 1 ) * abs ( INCY ) ) locations.
%
%    Input, integer INCY, the increment for the elements of
%    Y.  INCY must not be zero.
%

%
%  Test the input parameters.
%
  info = 0;
  if ( ~ ( trans == 'N' || trans == 'n' || ...
           trans == 'T' || trans == 't' || ...
           trans == 'C' || trans == 'c' ) )
    info = 1;
    fprintf ( 1, 'SGEMV rejects input argument number %d\n', info );
    error ( 'SGEMV - Fatal error!' );
  end

  if ( m < 0 )
    info = 2;
    fprintf ( 1, 'SGEMV rejects input argument number %d\n', info );
    error ( 'SGEMV - Fatal error!' );
  end

  if ( n < 0 )
    info = 3;
    fprintf ( 1, 'SGEMV rejects input argument number %d\n', info );
    error ( 'SGEMV - Fatal error!' );
  end

  if ( lda < max ( 1 : m ) )
    info = 6;
    fprintf ( 1, 'SGEMV rejects input argument number %d\n', info );
    error ( 'SGEMV - Fatal error!' );
  end

  if ( incx == 0 )
    info = 8;
    fprintf ( 1, 'SGEMV rejects input argument number %d\n', info );
    error ( 'SGEMV - Fatal error!' );
  end

  if ( incy == 0 )
    info = 11;
    fprintf ( 1, 'SGEMV rejects input argument number %d\n', info );
    error ( 'SGEMV - Fatal error!' );
  end
%
%  Quick return if possible.
%
  if ( ( m == 0 ) || ...
       ( n == 0 ) || ...
       ( alpha == 0.0 && beta == 1.0 ) )
   return
  end
%
%  Set LENX and LENY, the lengths of the vectors x and y, and set
%  up the start points in X and Y.
%
  if ( trans == 'N' || trans == 'n' )
    lenx = n;
    leny = m;
  else
    lenx = m;
    leny = n;
  end

  if ( 0 < incx )
    kx = 1;
  else
    kx = 1 - ( lenx - 1 ) * incx;
  end

  if ( 0 < incy )
    ky = 1;
  else
    ky = 1 - ( leny - 1 ) * incy;
  end
%
%  Start the operations. In this version the elements of A are
%  accessed sequentially with one pass through A.
%
%  First form  y := beta*y.
%
  if ( beta ~= 1.0 )
    if ( incy == 1 )
      if ( beta == 0.0 )
        y(1:leny) = 0.0;
      else
        y(1:leny) = beta * y(1:leny);
      end
    else
      iy = ky;
      if ( beta == 0.0 )
        for i = 1 : leny
          y(iy) = 0.0;
          iy = iy + incy;
        end
      else
        for i = 1 : leny
          y(iy) = beta * y(iy);
          iy = iy + incy;
        end
      end
    end
  end

  if ( alpha == 0.0 )
    return
  end
%
%  Form y := alpha*A*x + y.
%
  if ( trans == 'N' || trans == 'n' )
    jx = kx;
    if ( incy == 1 )
      for j = 1 : n
        if ( x(jx) ~= 0.0 )
          temp = alpha * x(jx);
          for i = 1 : m
            y(i) = y(i) + temp * a(i,j);
          end
        end
        jx = jx + incx;
      end
    else
      for j = 1 : n
        if ( x(jx) ~= 0.0 )
          temp = alpha * x(jx);
          iy = ky;
          for i = 1 : m
            y(iy) = y(iy) + temp * a(i,j);
            iy = iy + incy;
          end
        end
        jx = jx + incx;
      end
    end
%
%  Form y := alpha*A'*x + y.
%
  else
    jy = ky;
    if ( incx == 1 )
      for j = 1 : n
        temp = 0.0;
        for i = 1 : m
          temp = temp + a(i,j) * x(i);
        end
        y(jy) = y(jy) + alpha * temp;
        jy = jy + incy;
      end
    else
      for j = 1 : n
        temp = 0.0;
        ix = kx;
        for i = 1 : m
          temp = temp + a(i,j) * x(ix);
          ix = ix + incx;
        end
        y(jy) = y(jy) + alpha * temp;
        jy = jy + incy;
      end
    end
  end

  return
end


% THIS SOFTWARE AND ANY ACCOMPANYING DOCUMENTATION IS RELEASED "AS IS."  THE U.S. GOVERNMENT MAKES NO WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, CONCERNING THIS SOFTWARE AND ANY ACCOMPANYING DOCUMENTATION, INCLUDING, WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE.  IN NO EVENT WILL THE U.S. GOVERNMENT BE LIABLE FOR ANY DAMAGES, INCLUDING ANY LOST PROFITS, LOST SAVINGS OR OTHER INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE, OR INABILITY TO USE, THIS SOFTWARE OR ANY ACCOMPANYING DOCUMENTATION, EVEN IF INFORMED IN ADVANCE OF THE POSSIBILITY OF SUCH DAMAGES.
%
% file: get_prop_diff_ci1.m
% one-sided confidence intervals for the difference of proportions

% 030113 tdr created

function ci = get_prop_diff_ci1(x1,n1,x2,n2,alpha,method,verbose)

p1_hat = x1/n1; p2_hat = x2/n2; delta_p_hat = p1_hat - p2_hat;
a = find_lower_limit(x1,n1,x2,n2, alpha);
b = find_upper_limit(x1,n1,x2,n2, alpha);
ci = [delta_p_hat a b];

% ------------------------------------------------------------
% find lower limit

function limit = find_lower_limit(x1,n1,x2,n2,alpha)
% alpha is 1-Pr{delta_p >= a}
p1_hat = x1/n1; p2_hat = x2/n2; delta_p_hat = p1_hat - p2_hat;
too_small = -1; too_big = 1;

tolerance = 1e-6;
max_count = 50;

count = 0;
guess = (too_big + too_small)/2;
alpha_guess = 1- prop_diff(x1,n1,x2,n2,guess);
while (abs(alpha_guess - alpha) > tolerance & (count < max_count))
    if (alpha_guess > alpha)
        too_big = guess;
        guess = (too_small + guess)/2;
    else
        too_small=guess;
        guess = (too_big + guess)/2;
    end;
    count = count+1;
    alpha_guess = 1 - prop_diff(x1,n1,x2,n2,guess);
end;

limit = guess;

% ------------------------------------------------------------
% find upper limit

function limit = find_upper_limit(x1,n1,x2,n2,alpha)
% alpha is Pr{delta_p >= b}
p1_hat = x1/n1; p2_hat = x2/n2; delta_p_hat = p1_hat - p2_hat;
too_small = -1; too_big = 1;

tolerance = 1e-6;
max_count = 50;
count = 0;
guess = (too_big + too_small)/2;
alpha_guess = prop_diff(x1,n1,x2,n2,guess);
while (abs(alpha_guess - alpha) > tolerance & (count < max_count))
    if (alpha_guess < alpha)
        too_big = guess;
        guess = (too_small + guess)/2;
    else
        too_small=guess;
        guess = (too_big + guess)/2;
    end;
    count = count+1;
    alpha_guess = prop_diff(x1,n1,x2,n2,guess);
end;

limit = guess;


function b = onLine(point, line)
%ONLINE test if a point belongs to a line.
%
%   B = onLine(POINT, LINE)
%   with POINT being [xp yp], and LINE being [x0 y0 dx dy].
%   Returns 1 if point lies on the line, 0 otherwise.
%
%   If POINT is an N*2 array of points, B is a N*1 array of booleans.
%
%   If LINE is a N*4 arrat of line, B is a 1*N array of booleans.
%
%   See also:
%   lines2d, points2d, onEdge, onRay, angle3Points

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2003-10-31
% Copyright 2003 INRA - TPV URPOI - BIA IMASTE

% deprecation warning
warning('geom2d:deprecated', ...
    '''onLine'' is deprecated, use ''isPointOnLine'' instead');

Nl = size(line, 1);
Np = size(point, 1);

x0 = repmat(line(:,1)', Np, 1);
y0 = repmat(line(:,2)', Np, 1);
dx = repmat(line(:,3)', Np, 1);
dy = repmat(line(:,4)', Np, 1);
xp = repmat(point(:,1), 1, Nl);
yp = repmat(point(:,2), 1, Nl);

% test if lines are colinear
b = abs((xp-x0).*dy-(yp-y0).*dx)./sqrt(dx.*dx+dy.*dy) < 1e-14;