function [G11, G12, G21, G22] = d2Sbus_dV2(Ybus, V, lam, vcart)
%D2SBUS_DV2   Computes 2nd derivatives of power injection w.r.t. voltage.
%
%   The derivatives can be take with respect to polar or cartesian coordinates
%   of voltage, depending on the 4th argument.
%
%   [GAA, GAV, GVA, GVV] = D2SBUS_DV2(YBUS, V, LAM)
%   [GAA, GAV, GVA, GVV] = D2SBUS_DV2(YBUS, V, LAM, 0)
%
%   Returns 4 matrices containing the partial derivatives w.r.t. voltage angle
%   and magnitude of the product of a vector LAM with the 1st partial
%   derivatives of the complex bus power injections.
%
%   [GRR, GIR, GIR, GII] = D2SBUS_DV2(YBUS, V, LAM, 1)
%
%   Returns 4 matrices containing the partial derivatives w.r.t. real and
%   imaginary parts of voltage of the product of a vector LAM with the 1st
%   partial derivatives of the complex bus power injections.
%
%   Takes sparse bus admittance matrix YBUS, voltage vector V and nb x 1 vector
%   of multipliers LAM. Output matrices are sparse.
%
%   Examples:
%       [Ybus, Yf, Yt] = makeYbus(baseMVA, bus, branch);
%       [Gaa, Gav, Gva, Gvv] = d2Sbus_dV2(Ybus, V, lam);
%
%       Here the output matrices correspond to:
%           Gaa = d/dVa (dSbus_dVa.' * lam)
%           Gav = d/dVm (dSbus_dVa.' * lam)
%           Gva = d/dVa (dSbus_dVm.' * lam)
%           Gvv = d/dVm (dSbus_dVm.' * lam)
%
%       [Grr, Gri, Gir, Gii] = d2Sbus_dV2(Ybus, V, lam, 1);
%
%       Here the output matrices correspond to:
%           Grr = d/dVr (dSbus_dVr.' * lam)
%           Gri = d/dVi (dSbus_dVr.' * lam)
%           Gir = d/dVr (dSbus_dVi.' * lam)
%           Gii = d/dVi (dSbus_dVi.' * lam)
%
%   For more details on the derivations behind the derivative code used
%   in MATPOWER, see:
%
%   [TN2]  R. D. Zimmerman, "AC Power Flows, Generalized OPF Costs and
%          their Derivatives using Complex Matrix Notation", MATPOWER
%          Technical Note 2, February 2010. [Online]. Available:
%          https://matpower.org/docs/TN2-OPF-Derivatives.pdf
%          doi: 10.5281/zenodo.3237866
%   [TN4]  B. Sereeter and R. D. Zimmerman, "AC Power Flows and their
%          Derivatives using Complex Matrix Notation and Cartesian
%          Coordinate Voltages," MATPOWER Technical Note 4, April 2018.
%          [Online]. Available: https://matpower.org/docs/TN4-OPF-Derivatives-Cartesian.pdf
%          doi: 10.5281/zenodo.3237909

%   MATPOWER
%   Copyright (c) 2008-2019, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%   and Baljinnyam Sereeter, Delft University of Technology
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%% default input args
if nargin < 4
    vcart = 0;      %% default to polar coordinates
end

n = length(V);

diaglam = sparse(1:n, 1:n, lam, n, n);
if vcart
    E = diaglam * conj(Ybus);
    F = E + E.';
    G = 1j * (E - E.');

    G11 = F;        %% Grr
    G21 = G;        %% Gir
    G12 = G21.';    %% Gri
    G22 = G11;      %% Gii
else
    Ibus    = Ybus * V;
    diagV   = sparse(1:n, 1:n, V, n, n);

    A = sparse(1:n, 1:n, lam .* V, n, n);
    B = Ybus * diagV;
    C = A * conj(B);
    D = Ybus' * diagV;
    E = conj(diagV) * (D * diaglam - sparse(1:n, 1:n, D*lam, n, n));
    F = C - A * sparse(1:n, 1:n, conj(Ibus), n, n);
    G = sparse(1:n, 1:n, ones(n, 1)./abs(V), n, n);

    G11 = E + F;
    G21 = 1j * G * (E - F);
    G12 = G21.';
    G22 = G * (C + C.') * G;
end


 function proj = cuboid_proj(cg, params, varargin)
%function proj = cuboid_proj(cg, params, varargin)
%|
%| Compute set of 2d line-integral projection views of cuboids.
%| Works for both parallel-beam and cone-beam geometry.
%|
%| in
%|	cg			ct_geom()
%|	params [ne 9]		cuboid parameters:
%|			[x_center y_center z_center  x_radius y_radius z_radius
%|				xy_angle_degrees z_angle_degrees  amplitude]
%| options
%|	oversample		over-sampling factor for emulating "strips"
%|				(to account for finite detector size)
%|
%| out
%|	proj	[ns nt na]	projection views
%|
%| Yong Long, 2008-08-28, University of Michigan, adapted from ellipsoid_proj()
%| 2013-05-24 Jeff Fessler

if nargin == 1 && streq(cg, 'test'), cuboid_proj_test, return, end
if nargin < 2, ir_usage, end

arg.oversample = 1;
arg = vararg_pair(arg, varargin);

proj = cuboid_proj_do(params, cg.s, cg.t, cg.dt, cg.ar, cg.source_zs, ...
		cg.dso, cg.dod, cg.dfs, arg.oversample);

end % cuboid_proj()

% cuboid_proj_line1()
function [lxmin lxmax] = cuboid_proj_line1(rx, p1, e1)
tmp = (e1 == 0);
e1(tmp) = inf;
% bounds of l corresponding to rect(x/rx)
lxmin = (-rx/2 - p1) ./ e1;
lxmax = ( rx/2 - p1) ./ e1;
% re-arrange the bounds so that lxmin contains the minimum l values
% and lxmax contains the maximum l values
temp = lxmin;
lxmin = min(lxmin, lxmax);
lxmax = max(temp, lxmax);
% exclude points where e1=0 by setting lxmin = -Inf and lxmax = Inf
lxmin(tmp) = -inf;
lxmax(tmp) = inf;
end % cuboid_proj_line1()

% cuboid_proj_do()
function proj = cuboid_proj_do(params, ss, tt, dt, ...
		beta, ... % [radians]
		source_zs, dso, dod, dfs, oversample)

if size(params, 2) ~= 9, error '9 parameters per cuboid', end

if oversample > 1
	ds = ss(2) - ss(1);
%	dt = tt(2) - tt(1); % fails if nt=1 so pass dt instead
	if any(abs(diff(ss) / ds - 1) > 1e-6) ...
	|| any(abs(diff(tt) / dt - 1) > 1e-6)
		fail 'uniform spacing required for oversampling'
	end
	No = oversample;
	% determine new finer sampling positions
	ss = outer_sum([-(No-1):2:(No-1)]'/(2*No)*ds, ss(:)'); % [No ns]
	tt = outer_sum([-(No-1):2:(No-1)]'/(2*No)*dt, tt(:)'); % [No nt]
	proj = cuboid_proj_do(params, ss(:), tt(:), dt/No, ...
		beta, source_zs, dso, dod, dfs, 1);
	proj = downsample3(proj, [No No 1]);
return
end

% determine equivalent parallel-beam projection coordinates, at beta=0
ns = length(ss);
nt = length(tt);
[sss ttt] = ndgrid(ss, tt);

if isinf(dso) % parallel beam
	uu = sss;
	vv = ttt;
	azim0 = zeros(size(sss));
	polar = zeros(size(sss));

elseif isinf(dfs) % cone-beam with flat detector
	[uu vv azim0 polar] = ir_coord_cb_flat_to_par(sss, ttt, dso, dod);

elseif dfs == 0 % cone-beam with arc detector
	[uu vv azim0 polar] = ir_coord_cb_arc_to_par(sss, ttt, dso, dod);

else
	fail 'not done'
end

clear sss ttt

cpolar = cos(polar);
spolar = sin(polar);
proj = zeros(ns, nt, numel(beta));

% loop over cuboids
for ip = 1:size(params,1)
	par = params(ip,:);

	cx = par(1);	rx = par(4);
	cy = par(2);	ry = par(5);
	cz = par(3);	rz = par(6);
	xang = deg2rad(par(7)); % xy-plane rotation
	zang = deg2rad(par(8)); % z-plane rotation
	if zang, error 'z rotation not done', end
	val = par(9);

	for ib = 1:length(beta)
		az = beta(ib) + azim0;

		% shift property of 3D transform:
		cz_eff = cz - source_zs(ib); % center relative to source
		ushift = cx * cos(az) + cy * sin(az);
		vshift = (cx * sin(az) - cy * cos(az)) .* spolar + cz_eff * cpolar;

		az = az - xang;
		us = uu - ushift;
		vs = vv - vshift;
		p1 = us .* cos(az) + vs .* sin(az) .* spolar;
		p2 = us .* sin(az) - vs .* cos(az) .* spolar;
		p3 = vs .* cpolar;

		e1 = -sin(az) .* cpolar; % x = p1 + l*e1
		e2 = cos(az) .* cpolar; % y = p2 + l*e2
		e3 = spolar; % z = p3 + l*e3

		[lxmin lxmax] = cuboid_proj_line1(rx, p1, e1);
%		clear p1
		[lymin lymax] = cuboid_proj_line1(ry, p2, e2);
%		clear p2
		[lzmin lzmax] = cuboid_proj_line1(rz, p3, e3);
%		clear p3

		lmin = max(lxmin, lymin);
		lmin = max(lmin, lzmin); % lower bound for l

		lmax = min(lxmax, lymax);
		lmax = min(lmax, lzmax); % upper bound for l

		ll = max(lmax - lmin, 0); % intersection only when lmax > lmin

		% cases where e(k) = 0 (rays along axes)

		tmp = e1 == 0; % sin(az) = 0
		zero_e = (-rx/2 <= us(tmp)) & (us(tmp) <= rx/2);
		ll(tmp) = ll(tmp) .* zero_e;

		tmp = e2 == 0; % cos(az) = 0
		zero_e = (-ry/2 <= us(tmp)) & (us(tmp) <= ry/2);
		ll(tmp) = ll(tmp) .* zero_e;

		tmp = e3 == 0; % sin(polar) = 0
		zero_e = (-rz/2 <= vs(tmp)) & (vs(tmp) <= rz/2);
		ll(tmp) = ll(tmp) .* zero_e;

if 0 % old way.  todo cut.  and verify e1 and e2 cases above
		if (e3 == 0) % sin(polar) = 0; so line along z
			zero_e = (-rz/2 <= vs) & (vs <= rz/2);
			ll = ll .* zero_e;
		end

		if (e1 == 0) % sin(az) = 0;
			zero_e = (-rx/2 <= us) & (us <= rx/2);
			ll = ll .* zero_e;
		end

		if (e2 == 0) % cos(az) = 0;
			zero_e = (-ry/2 <= us) & (us <= ry/2);
			ll = ll .* zero_e;
		end
end

		proj(:,:,ib) = proj(:,:,ib) + val * ll;
		clear proj_i
	end
end

end % cuboid_proj_do()

% cuboid_proj_test()
% internal test routine
function cuboid_proj_test

if 1
	params = [30 20 5  200 100 20  45 0 10];
	fun_proj = @(cg) cuboid_proj(cg, params, 'oversample', 2); % analytical
	fun_im = @(ig) cuboid_im(ig, params, 'oversample', 2); %, 'checkfov', true);

	ir_proj3_compare1(fun_proj, fun_im, 'chat', 1);
return % todo
end

down = 30;
cg = ct_geom('fan', 'ns', round(888/down), 'nt', 64, ...
	'na', 18, ...
	'ds', 1.0*down, 'dt', 1.1, ...
	'down', 1, ... % only downsample s and beta
	'dsd', 949, 'dod', 408, 'dfs', 0, ... % 3rd gen CT
	'dsd', inf, ... % parallel-beam
	'pitch', 0.5, ...
	'offset_t', 0.0, ...
	'offset_s', 0.25); % quarter detector
%	'dsd', 949, 'dod', 408, 'dfs', inf, ... % flat detector

ig = image_geom('nx', 512/2^2, 'nz', 64, 'fov', 512, 'dz', 0.625);
%cg.plot3(ig);

params = [30 20 5  200 100 20  45 0 10];
x = cuboid_im(ig, params, 'oversample', 2);

im plc 2 2
im(1, x), cbar

ya = cuboid_proj(cg, params, 'oversample', 2);
im(2, cg.s, cg.t, ya), cbar
titlef('analytical cone-beam projections, dfs=%g', cg.dfs)
xlabel s, ylabel t

%im clf, im(cg.s, cg.ad, permute(ya, [1 3 2])), cbar
%xlabel s, ylabel '\beta'

A = Gcone(cg, ig);
yd = A * x;

im(3, cg.s, cg.t, yd, 'discrete projections'), cbar

im(4, cg.s, cg.t, ya-yd), cbar
titlef('analytical - discrete')

max_percent_diff(ya, yd)

end % cuboid_proj_test()


function [is_planar ksubgraph EI]=boyer_myrvold_planarity_test(A,varargin)
% BOYER_MYRVOLD_PLANARITY_TEST Test a graph for planarity
%
% is_planar = boyer_myrvold_planaity_test(A) yields 1 if A is a planar
% graph or 0 otherwise.  A planar graph can be drawn on the xy plane with
% no edge crossings.
%
% [is_planar K] = ... identifies a Kuratowski subgraph of A when A is not
% planar.
% [is_planar K EI] = ... computes the planar embedding edge order in
% EI.edge_order.  EI.vp gives the starting and ending entries in EI.edge_order
% for the order of edges from each vertex.  (EI is the compressed
% sparse row representation of the matrix A with the order of the column
% indices permuted to the planar graph embedding order.)
%
% ... = boyer_myrvold_planaity_test(A,...) takes a set of
% key-value pairs or an options structure.  See set_matlab_bgl_options
% for the standard options.
%   No additional options for this function
%
% Example:
%   G = grid_graph(6,5);
%   boyer_myrvold_planarity_test(G) % G is planar
%   K5 = clique_graph(5);
%   boyer_myrvold_planarity_test(K5) % K5 is not planar

% David Gleich
% Copyright, Stanford University, 2008

%% History
%  2007-10-06: Initial coding
%%

[trans check full2sparse] = get_matlab_bgl_options(varargin{:});
if full2sparse && ~issparse(A), A = sparse(A); end

options = struct();
options = merge_options(options,varargin{:});

if nargout <= 1
    is_planar = planar_test_mex(A,0);
else
    if nargout <= 2
        [is_planar ki kj] = planar_test_mex(A,0);
    else
        [is_planar ki kj eip eie] = planar_test_mex(A,0);
        EI = struct('vp',eip,'edge_order',eie);
    end
    if ~isempty(ki)
        ksubgraph = sparse(ki,kj,1,size(A,1),size(A,2));
    else
        ksubgraph = sparse([]);
    end
    ksubgraph = ksubgraph|ksubgraph'; % placed here to get the type right
end


function [perc,n,which] = misclass(Y,Yest)
% The rate of misclassifications.
%
% '[perc,n,which] = misclass(Y,Yest)'
%
%  'Y' contains the real class labels;
%  'Yest' contains the estimated class labels;
%  'perc' is the rate of misclassifications (between 0 and 1);
%  'n' is the number of misclassifications;
%  'which' contains the indices of the misclassificated instances
%     (the first column gives the row, the second the column index)
%
%
% see also:
%    validate, mse, linf, medae, mae

% Copyright (c) 2011,  KULeuven-ESAT-SCD, License & help @ http://www.esat.kuleuven.be/sista/lssvmlab

n = sum(sum(Y~=Yest));
perc = n/numel(Y);
[I,J] = find(Y~=Yest);
which = [J I];


% Author:  Javier Lopez-Calderon
% 2009
%
function m = modeone(A)
if iscell(A)
        while any(cellfun('isclass',A,'cell'))
                A = cat(2,A{:});
        end

        n = length(A);
        h = cellfun(@isnumeric,A);
        g = nnz(h);

        if g>0 && g~=n % mixed
                error('Elements have to be of same class')
        elseif g>0 && g==n  % only numbers
                A = cell2mat(A);
        end
end
u = unique_bc2(A);
[tf, ind] = ismember_bc2(A, u);
if ind>0
        k = mode(ind);
        m = u(k);
else
        m = NaN;
end


function P = optimizer(self,x,u)
%OPTIMIZER  Container for optimization problem
%
%   OPT = OPTIMIZER(Problem,x,u) exports an object that contains
%   precompiled numerical data to be solved for varying arguments
%   x, returning the optimal value of the expression u.
%
%   SEE OPTIMIZER for more info
%
%   Example
%
%    The following problem creates an LP with varying upper and lower
%    bounds on the decision variable.
%
%    The optimizing argument is obtained by indexing (with {}) the optimizer
%    object with the point of interest. The argument should be a column
%    vector (if the argument has a width larger than 1, YALMIP assumes that
%    the optimal solution should be computed in several points)
%
%     A = randn(10,3);
%     b = rand(10,1)*19;
%     c = randn(3,1);
%
%     z = sdpvar(3,1);
%     sdpvar UB LB
%
%     Constraints = [A*z < b, LB < z < UB];
%     Objective = c'*z;
%     P = optproblem(Constraints,Objective);
%     % We want the optimal z as a function of [LB;UB]
%     optZ = optimizer(P,[LB; UB],z);
%
%     % Compute the optimal z when LB=1, UB = 3;
%     zopt = optZ{[1; 3]}
%
%     % Compute two solutions, one for (LB,UB) [1;3] and one for (LB,UB) [2;6]
%     zopt = optZ{[[1; 3], [2;6]]}
%
%     A second output argument can be used to catch infeasibility
%     [zopt,infeasible] = optZ{[1; 3]}

P = optimizer(self.Constraints,self.Objective,self.Options,x,u);


function sub = ind2subv(siz,index)
%IND2SUBV   Subscript vector from linear index.
% IND2SUBV(SIZ,IND) returns a vector of the equivalent subscript values
% corresponding to a single index into an array of size SIZ.
% If IND is a vector, then the result is a matrix, with subscript vectors
% as rows.

% Written by Tom Minka
% Part of Tom Minka's lightspeed package.
% (c) Microsoft Corporation. All rights reserved.

n = length(siz);
cum_size = cumprod(siz(:)');
prev_cum_size = [1 cum_size(1:end-1)];
index = index(:) - 1;
sub = rem(repmat(index,1,n),repmat(cum_size,length(index),1));
sub = floor(sub ./ repmat(prev_cum_size,length(index),1))+1;

% slow way
%for dim = n:-1:1
%  sub(:,dim) = floor(index/cum_size(dim))+1;
%  index = rem(index,cum_size(dim));
%end


function [goal]=Target_Allocation(goal,quad_init_x,quad_init_y,num,target_num,goal_series,temp_goal,mirror_dis)
mirror_count=0;
row=0;
line=0;
temp_sum_dis=0;
sum_dis=[0;0;0;0];
limited_init_distance=0;%?????????
quad_init=[quad_init_x quad_init_y];

distance=[sqrt((quad_init(1,1)-goal(1,1))^2+(quad_init(1,2)-goal(1,2))^2) sqrt((quad_init(1,1)-goal(2,1))^2+(quad_init(1,2)-goal(2,2))^2) sqrt((quad_init(1,1)-goal(3,1))^2+(quad_init(1,2)-goal(3,2))^2) sqrt((quad_init(1,1)-goal(4,1))^2+(quad_init(1,2)-goal(4,2))^2);
          sqrt((quad_init(2,1)-goal(1,1))^2+(quad_init(2,2)-goal(1,2))^2) sqrt((quad_init(2,1)-goal(2,1))^2+(quad_init(2,2)-goal(2,2))^2) sqrt((quad_init(2,1)-goal(3,1))^2+(quad_init(2,2)-goal(3,2))^2) sqrt((quad_init(2,1)-goal(4,1))^2+(quad_init(2,2)-goal(4,2))^2);
          sqrt((quad_init(3,1)-goal(1,1))^2+(quad_init(3,2)-goal(1,2))^2) sqrt((quad_init(3,1)-goal(2,1))^2+(quad_init(3,2)-goal(2,2))^2) sqrt((quad_init(3,1)-goal(3,1))^2+(quad_init(3,2)-goal(3,2))^2) sqrt((quad_init(3,1)-goal(4,1))^2+(quad_init(3,2)-goal(4,2))^2);
          sqrt((quad_init(4,1)-goal(1,1))^2+(quad_init(4,2)-goal(1,2))^2) sqrt((quad_init(4,1)-goal(2,1))^2+(quad_init(4,2)-goal(2,2))^2) sqrt((quad_init(4,1)-goal(3,1))^2+(quad_init(4,2)-goal(3,2))^2) sqrt((quad_init(4,1)-goal(4,1))^2+(quad_init(4,2)-goal(4,2))^2)];

distance_list=[sqrt((quad_init(1,1)-goal(1,1))^2+(quad_init(1,2)-goal(1,2))^2) sqrt((quad_init(1,1)-goal(2,1))^2+(quad_init(1,2)-goal(2,2))^2) sqrt((quad_init(1,1)-goal(3,1))^2+(quad_init(1,2)-goal(3,2))^2) sqrt((quad_init(1,1)-goal(4,1))^2+(quad_init(1,2)-goal(4,2))^2) ...,
              sqrt((quad_init(2,1)-goal(1,1))^2+(quad_init(2,2)-goal(1,2))^2) sqrt((quad_init(2,1)-goal(2,1))^2+(quad_init(2,2)-goal(2,2))^2) sqrt((quad_init(2,1)-goal(3,1))^2+(quad_init(2,2)-goal(3,2))^2) sqrt((quad_init(2,1)-goal(4,1))^2+(quad_init(2,2)-goal(4,2))^2) ...,
              sqrt((quad_init(3,1)-goal(1,1))^2+(quad_init(3,2)-goal(1,2))^2) sqrt((quad_init(3,1)-goal(2,1))^2+(quad_init(3,2)-goal(2,2))^2) sqrt((quad_init(3,1)-goal(3,1))^2+(quad_init(3,2)-goal(3,2))^2) sqrt((quad_init(3,1)-goal(4,1))^2+(quad_init(3,2)-goal(4,2))^2) ...,
              sqrt((quad_init(4,1)-goal(1,1))^2+(quad_init(4,2)-goal(1,2))^2) sqrt((quad_init(4,1)-goal(2,1))^2+(quad_init(4,2)-goal(2,2))^2) sqrt((quad_init(4,1)-goal(3,1))^2+(quad_init(4,2)-goal(3,2))^2) sqrt((quad_init(4,1)-goal(4,1))^2+(quad_init(4,2)-goal(4,2))^2);
              1 1 1 1 2 2 2 2 3 3 3 3 4 4 4 4;
              1 2 3 4 1 2 3 4 1 2 3 4 1 2 3 4];
distance_sort=distance_list;
for k=1:(num*target_num)
    for j=2:(num*target_num-k+1)
        if distance_sort(1,j-1)>distance_sort(1,j)
            Q=distance_sort(:,j-1);
            distance_sort(:,j-1)=distance_sort(:,j);
            distance_sort(:,j)=Q;
        end
    end
end
for k=1:(num*target_num)
    if (mirror_dis(distance_sort(2,k),distance_sort(3,k))~=2) && (mirror_count<3)
        mirror_dis(distance_sort(2,k),:)=2;
        mirror_dis(:,distance_sort(3,k))=2;
        mirror_dis(distance_sort(2,k),distance_sort(3,k))=1;
        mirror_count=mirror_count+1;
    end
end
for k=1:num
    for j=1:target_num
        if (mirror_dis(k,j)==1) || (mirror_dis(k,j)==0)
            goal_series(k)=j;
        end
        if mirror_dis(k,j)==0
            row=k;
            line=j;
        end
    end
end
if distance(row,line)>limited_init_distance
    for k=1:num%????????
        sum_dis(k)=(distance(k,line)+distance(row,goal_series(k)))-(distance(k,goal_series(k))+distance(row,line));
        if temp_sum_dis >= sum_dis(k)
            temp_sum_dis=sum_dis(k);
            second_row=k;
        end
    end
    goal_series(row)=goal_series(second_row);
    goal_series(second_row)=line;
end
for k=1:target_num
    temp_goal(k,:)=goal(goal_series(k),:);
end
goal=temp_goal;
end


function [MHz] = rpm2MHz(rpm)
% Convert frequency from revolutions per minute to Megahertz.
% Chad A. Greene 2012
MHz = rpm/60/1000000;


function demo_blockproc_slidingerblets(source,varargin) %RUNASSCRIPT
%DEMO_BLOCKPROC_SLIDINGERBLETS Basic real-time rolling erblet-spectrogram visualization
%   Usage: demo_blockproc_slidingerblets('gspi.wav')
%
%   For additional help call |demo_blockproc_slidingerblets| without arguments.
%
%   This demo shows a simple rolling erblet-spectrogram of whatever is specified in
%   source.

if demo_blockproc_header(mfilename,nargin)
   return;
end

% Control pannel (Java object)
% Each entry determines one parameter to be changed during the main loop
% execution.
p = blockpanel({
               {'GdB','Gain',-20,20,0,21},...
               {'cMult','C mult',0,80,20,81}
               });

fobj = blockfigure();

% Setup blocktream
try
    fs=block(source,varargin{:},'loadind',p);
catch
    % Close the windows if initialization fails
    blockdone(p,fobj);
    err = lasterror;
    error(err.message);
end

% Buffer length (30 ms)
bufLen = floor(30e-3*fs);
zpad = floor(bufLen/2);

% Number of filters
M = 200;
F = frame('erbletfb',fs,2*bufLen+2*zpad,'fractionaluniform','M',M);
Fa = blockframeaccel(F,bufLen,'sliced','zpad',zpad);

flag = 1;
cola = [];
%Loop until end of the stream (flag) and until panel is opened
while flag && p.flag
  % Get parameters
  [gain,mult] = blockpanelget(p,'GdB','cMult');
  gain = 10^(gain/20);
  mult = 10^(mult/20);

  % Read block of length bufLen
  [f,flag] = blockread(bufLen);
  f = f*gain;
  % Apply analysis frame
  c = blockana(Fa, f);
  % Plot
  cola = blockplot(fobj,Fa,mult*c(:,1),cola);

  blockplay(f);
end
blockdone(p,fobj,Fa);


function sparse_grid_mixed_weight_test ( dim_num, level_max_min, ...
  level_max_max, rule, alpha, beta, tol )

%*****************************************************************************80
%
%% SPARSE_GRID_MIXED_WEIGHT_TEST checks the sum of the quadrature weights.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 March 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer LEVEL_MAX_MIN, LEVEL_MAX_MAX, the minimum and
%    maximum values of LEVEL_MAX.
%
%    Input, integer RULE(DIM_NUM), the rule in each dimension.
%     1, "CC",  Clenshaw Curtis, Closed Fully Nested rule.
%     2, "F2",  Fejer Type 2, Open Fully Nested rule.
%     3, "GP",  Gauss Patterson, Open Fully Nested rule.
%     4, "GL",  Gauss Legendre, Open Weakly Nested rule.
%     5, "GH",  Gauss Hermite, Open Weakly Nested rule.
%     6, "GGH", Generalized Gauss Hermite, Open Weakly Nested rule.
%     7, "LG",  Gauss Laguerre, Open Non Nested rule.
%     8, "GLG", Generalized Gauss Laguerre, Open Non Nested rule.
%     9, "GJ",  Gauss Jacobi, Open Non Nested rule.
%    10, "GW",  Golub Welsch, (presumed) Open Non Nested rule.
%    11, "CC_SE", Clenshaw Curtis Slow Exponential, Closed Fully Nested rule.
%    12, "F2_SE", Fejer Type 2 Slow Exponential, Closed Fully Nested rule.
%    13, "GP_SE", Gauss Patterson Slow Exponential, Closed Fully Nested rule.
%    14, "CC_ME", Clenshaw Curtis Moderate Exponential, Closed Fully Nested rule.
%    15, "F2_ME", Fejer Type 2 Moderate Exponential, Closed Fully Nested rule.
%    16, "GP_ME", Gauss Patterson Moderate Exponential, Closed Fully Nested rule.
%    17, "CCN", Clenshaw Curtis Nested, Linear, Closed Fully Nested rule.
%
%    Input, real ALPHA(DIM_NUM), BETA(DIM_NUM), parameters used for
%    Generalized Gauss Hermite, Generalized Gauss Laguerre, and Gauss Jacobi rules.
%
%    Input, real TOL, the tolerance for point equality.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPARSE_GRID_MIXED_WEIGHT_TEST:\n' );
  fprintf ( 1, '  Compute the weights of a sparse grid.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Each sparse grid is of spatial dimension DIM_NUM,\n' );
  fprintf ( 1, '  and is made up of product grids of levels up to LEVEL_MAX.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, ' Dimension      Rule     Alpha          Beta\n' );
  fprintf ( 1, '\n' );

  for dim = 1 : dim_num
    fprintf ( 1, '  %8d  %8d', dim, rule(dim) );
    if ( rule(dim) == 6 | rule(dim) == 8 | rule(dim) == 9 )
      fprintf ( 1, '  %12e', alpha(dim) );
    end
    if ( rule(dim) == 9 )
      fprintf ( 1, '  %12e', beta(dim) );
    end
    fprintf ( 1, '\n' );
  end

  weight_sum_exact = 1.0;

  for dim = 1 : dim_num

    if ( rule(dim) == 1 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 2 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 3 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 4 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 5 )
      weight_sum_exact = weight_sum_exact * sqrt ( pi );
    elseif ( rule(dim) == 6 )
      weight_sum_exact = weight_sum_exact * gamma ( 0.5 * ( alpha(dim) + 1.0 ) );
    elseif ( rule(dim) == 7 )
      weight_sum_exact = weight_sum_exact * 1.0;
    elseif ( rule(dim) == 8 )
      weight_sum_exact = weight_sum_exact * gamma ( alpha(dim) + 1.0 );
    elseif ( rule(dim) == 9 )
      arg1 = - alpha(dim);
      arg2 = 1.0;
      arg3 = beta(dim) + 2.0;
      arg4 = - 1.0;
      value1 = r8_hyper_2f1 ( arg1, arg2, arg3, arg4 );
      arg1 = - beta(dim);
      arg2 = 1.0;
      arg3 = alpha(dim) + 2.0;
      arg4 = - 1.0;
      value2 = r8_hyper_2f1 ( arg1, arg2, arg3, arg4 );
      weight_sum_exact = weight_sum_exact * ( ...
        value1 / ( beta(dim) + 1.0 ) + value2 / ( alpha(dim) + 1.0 ) );
    elseif ( rule(dim) == 10 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SPARSE_GRID_MIXED_WEIGHT_TEST - Fatal error!\n' );
      fprintf ( 1, '  Do not know weight sum for rule 10.\n' );
      error ( 'SPARSE_GRID_MIXED_WEIGHT_TEST - Fatal error!' );
    elseif ( rule(dim) == 11 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 12 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 13 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 14 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 15 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 16 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 17 )
      weight_sum_exact = weight_sum_exact * 2.0;
    else
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SPARSE_GRID_MIXED_WEIGHT_TEST - Fatal error!\n' );
      fprintf ( 1, '  Unexpected value of RULE = %d\n', rule(dim) );
      error ( 'SPARSE_GRID_MIXED_WEIGHT_TEST - Fatal error!' );
    end

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  As a simple test, sum these weights.\n' );
  fprintf ( 1, '  They should sum to exactly %f\n', weight_sum_exact );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     Level      Weight sum  Expected sum    Difference\n' );
  fprintf ( 1, '\n' );

  for level_max = level_max_min : level_max_max

    point_total_num = sparse_grid_mixed_size_total ( dim_num, level_max, rule );

    point_num  = sparse_grid_mixed_size ( dim_num, level_max, rule, alpha, beta, tol );

    sparse_unique_index = sparse_grid_mixed_unique_index ( ...
      dim_num, level_max, rule, alpha, beta, tol, point_num, point_total_num );

    sparse_weight = sparse_grid_mixed_weight ( dim_num, level_max, rule, ...
      alpha, beta, point_num, point_total_num, sparse_unique_index );

    weight_sum = sum ( sparse_weight(1:point_num) );

    weight_sum_error = abs ( weight_sum - weight_sum_exact );

    fprintf ( 1, '  %8d  %14e  %14e  %14e\n', ...
      level_max, weight_sum, weight_sum_exact, weight_sum_error );

  end

  return
end


function REP = REPSelection(PopObj,N,div)
% Select one of the particles in REP as the global best position for each
% particle

%------------------------------- 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".
%--------------------------------------------------------------------------

    NoP = size(PopObj,1);

    %% Calculate the grid location of each solution
    fmax = max(PopObj,[],1);
    fmin = min(PopObj,[],1);
    d    = (fmax-fmin)/div;
    fmin = repmat(fmin,NoP,1);
    d    = repmat(d,NoP,1);
    GLoc = floor((PopObj-fmin)./d);
    GLoc(GLoc>=div) = div - 1;
    GLoc(isnan(GLoc)) = 0;

    %% Detect the grid of each solution belongs to
    [~,~,Site] = unique(GLoc,'rows');

    %% Calculate the crowd degree of each grid
    CrowdG = hist(Site,1:max(Site));

    %% Roulette-wheel selection
    TheGrid = RouletteWheelSelection(N,CrowdG);
    REP     = zeros(1,N);
    for i = 1 : length(REP)
        InGrid = find(Site==TheGrid(i));
        Temp   = randi(length(InGrid));
        REP(i) = InGrid(Temp);
    end
end


function ptch = showsurface( voxels )
%SHOWSURFACE: draw a surface based on some voxels
%
%   SHOWSURFACE(VOXELS) tries to render the supplied voxel structure as a
%   surface using MATLAB's ISOSURFACE command.
%
%   PTCH = SHOWSURFACE(VOXELS) also returns handles to the patches
%   created.

%   Copyright 2005-2009 The MathWorks, Inc.
%  $Revision: 1.0 $    $Date: 2006/06/30 00:00:00 $

% First grid the data
ux = unique(voxels.XData);
uy = unique(voxels.YData);
uz = unique(voxels.ZData);

% Expand the model by one step in each direction
ux = [ux(1)-voxels.Resolution; ux; ux(end)+voxels.Resolution];
uy = [uy(1)-voxels.Resolution; uy; uy(end)+voxels.Resolution];
uz = [uz(1)-voxels.Resolution; uz; uz(end)+voxels.Resolution];

% Convert to a grid
[X,Y,Z] = meshgrid( ux, uy, uz );

% Create an empty voxel grid, then fill only those elements in voxels
V = zeros( size( X ) );
N = numel( voxels.XData );
for ii=1:N
    ix = (ux == voxels.XData(ii));
    iy = (uy == voxels.YData(ii));
    iz = (uz == voxels.ZData(ii));
    V(iy,ix,iz) = voxels.Value(ii);
end

% Now draw it
ptch = patch( isosurface( X, Y, Z, V, 0.5 ) );
isonormals( X, Y, Z, V, ptch )
set( ptch, 'FaceColor', 'g', 'EdgeColor', 'none' );

set(gca,'DataAspectRatio',[1 1 1]);
xlabel('X');
ylabel('Y');
zlabel('Z');
view(-140,22)
lighting( 'gouraud' )
camlight( 'right' )
axis( 'tight' )


%
%   colour transfer algorithm based on linear Monge-Kantorovitch solution
%
%   IR = colour_transfer_MKL(I_original, I_target, nbiterations);
%
%  (c) F. Pitie 2007
%
%  see reference:
%
%
function IR = colour_transfer_MKL(I0, I1)

if (ndims(I0)~=3)
    error('pictures must have 3 dimensions');
end

X0 = reshape(I0, [], size(I0,3));
X1 = reshape(I1, [], size(I1,3));

A = cov(X0);
B = cov(X1);

T = MKL(A, B);

mX0 = repmat(mean(X0), [size(X0,1) 1]);
mX1 = repmat(mean(X1), [size(X0,1) 1]);

XR = (X0-mX0)*T + mX1;

IR = reshape(XR, size(I0));

function [T] = MKL(A, B)
N = size(A,1);
[Ua,Da2] = eig(A);
Da2 = diag(Da2);
Da2(Da2<0) = 0;
Da = diag(sqrt(Da2 + eps));
C = Da*Ua'*B*Ua*Da;
[Uc,Dc2] = eig(C);
Dc2 = diag(Dc2);
Dc2(Dc2<0) = 0;
Dc = diag(sqrt(Dc2 + eps));
Da_inv = diag(1./(diag(Da)));
T = Ua*Da_inv*Uc*Dc*Uc'*Da_inv*Ua';


function [cIX,gIX] = GrowClustersFromSeedsItr(thres_merge,thres_cap,thres_minsize,thres_reg,cIX,gIX,M_0)
disp('find ROIs')

% Set params

% may be determined by hist of distances between all cells (in a sample)
% in correlation distance, i.e. 1 - corr.coeff
% thres_merge = 0.4;
% thres_cap = 0.5;

% thres_minsize = 10; % cell number in final clusters

%%
gIX = SqueezeGroupIX(gIX);
M = M_0(cIX,:);
C = FindCentroid_Direct(gIX,M); % kmeans 20x20, 'supervoxels'
nFoxels = size(C,1);

%% Calculate correlation distance between all cluster-centroids
temp = pdist(C,'correlation');
Dist = squareform(temp);
for i = 1:nFoxels,
    Dist(i,i) = NaN;
end

%% ITERATION:
% initialize
ROI = [];
ROI(nFoxels).fxlist = []; % not capping
ROI(nFoxels).numcell = [];
ROIcount = 0;

% analogy ~ mask to store new ROI
BW = zeros(nFoxels,1);

%%
tic
while true, % loop through all qualified seeds
    % look for next seed
    [~,ix] = min(Dist(:));
    [I,J] = ind2sub(size(Dist),ix);
    % and add seed position to mask
    BW(I) = 1;
    BW(J) = 1;

    if Dist(I,J)>(1-thres_merge),
        break;
    end

    while true % grow ROI: expand mask and examine next neighbors
        IX_in = find(BW);
        IX_out = find(BW==0);

        % find next closest cluster
        D = 1-corr(C(IX_in,:)',C(IX_out,:)'); % distance matrix between new core and the original cluster means
        D2 = min(D,[],1);
        [a,ix] = min(D2);
        ix_pretend = IX_out(ix);

        BW_pretend = BW;
        BW_pretend(ix_pretend) = 1;

        % find correlation between potential cluster and new core
        list = find(BW_pretend);
        IX = [];
        for i_gIX = 1:length(list),
            IX = [IX;find(gIX==list(i_gIX))];
        end
        M_core = M(IX,:);
        [~,newCore] = kmeans(M_core,1,'distance','correlation');
        coredist = 1-corr(C(ix_pretend,:)',newCore');

        if a<(1-thres_merge) && coredist<(1-thres_cap),
            BW(ix_pretend) = 1;
        else
            break; % finished expanding this seed
        end
    end

    % save this ROI if bigger than thres
    list = find(BW);
    IX = [];
    for i_gIX = 1:length(list),
        IX = [IX;find(gIX==list(i_gIX))]; %#ok<AGROW>
    end
    numcell = length(IX);
    if numcell >= thres_minsize,
        ROIcount = ROIcount+1;
        ROI(ROIcount).fxlist = find(BW);
        ROI(ROIcount).numcell = numcell;
    end

    % update/reset
    Dist(list,:) = NaN;
    Dist(:,list) = NaN;
    C(list,:) = NaN;
    BW = zeros(nFoxels,1);
end

ROI(ROIcount+1:end) = [];
toc
%% Merge accordingly
for i = 1:ROIcount,
    fxlist = ROI(i).fxlist;
    for j = 2:length(fxlist),
        gIX(gIX==fxlist(j)) = fxlist(1);
    end
end
[gIX,numU] = SqueezeGroupIX(gIX);
disp(numU);

%% Regression with the centroid of each cluster, round 2
% disp('auto-reg');
% Reg = FindCentroid_Direct(gIX,M);
% [cIX,gIX] = AllCentroidRegression_direct(M_0,thres_reg,Reg);

%% size threshold
U = unique(gIX);
numU = length(U);
for i=1:numU,
    if length(find(gIX==U(i)))<thres_minsize,
        cIX(gIX==U(i)) = [];
        gIX(gIX==U(i)) = [];
    end
end
[gIX,numU] = SqueezeGroupIX(gIX);
disp(numU);
end


%
% LibQPEP: A Library for Globally Optimal Solving Quadratic Pose Estimation Problems (QPEPs),
%          It also gives highly accurate uncertainty description of the solutions.
%
%
% Article:
%      Wu, J., Zheng, Y., Gao, Z., Jiang, Y., Hu, X., Zhu, Y., Jiao, J., Liu, M. (2020)
%           Quadratic Pose Estimation Problems: Unified Solutions,
%           Solvability/Observability Analysis and Uncertainty Description
%           in A Globally Optimal Framework.
%
%
% Authors:      Jin Wu and Ming Liu
% Affiliation:  Hong Kong University of Science and Technology (HKUST)
% Emails:       jin_wu_uestc@hotmail.com; eelium@ust.hk
% Websites:     https://zarathustr.github.io
%               https://ram-lab.com

function R = q2R(q)
q0 = q(1); q1 = q(2); q2 = q(3); q3 = q(4);
R = [
        q0^2 + q1^2 - q2^2 - q3^2,         2*q0*q3 + 2*q1*q2,         2*q1*q3 - 2*q0*q2;
                2*q1*q2 - 2*q0*q3, q0^2 - q1^2 + q2^2 - q3^2,         2*q0*q1 + 2*q2*q3;
                2*q0*q2 + 2*q1*q3,         2*q2*q3 - 2*q0*q1, q0^2 - q1^2 - q2^2 + q3^2];
end


function [u,eqn,info] = fracLapP1P2(node,elem,pde,option)
%% FRACLAPP1P2 solves fractional Laplacian equation using P1-P2 element
%
% [u,eqn,info] = fracLapP1P1(node,elem,pde,option) solves the fractional
% Laplacian equation
%
%  (-\Delta^^s u = f in \Omega with u = 0 on \partial \Omega.

%% Options
if ~exist('option','var'), option = []; end
if ~isfield(option,'plotflag'), option.plotflag = 0; end

%% Parameters
global s;
alpha = 1-2*s;
if s == 0.5
    Gamma = 1;
else
    Gamma = 5/(2*s)+0.01;  % a different grading factor
end

%% Mesh
N = size(node,1); NT = size(elem,1);
% My = round(sqrt(NT));
L = 1+log10(NT)/3; % to control the truncated error, L should increase as logN
My = round(L*2*NT^(1/4)); % number of elements in y direction
% My = round(pde.L*sqrt(sqrt(NT/2)));
y = gradmap(0,L,Gamma,My);
Ny = length(y); % number of vertices in y-direction
NTy = Ny - 1;   % number of elements in y-direction
NTtotal = NT*NTy;
Nxdof = N;      % dof in x direction is number of vertices
Nydof = (Ny + NTy); % dof in y direction is number of vertices plus elements
Ndof = Nxdof*Nydof;

tic; % start the timing for the assembling
%% Stiffness matrix and mass matrix in the extended direction
% quantities in y direction
hy = diff(y);
a = zeros(NTy,5);
for i = 1:5
    a(:,i) = diff(y.^(alpha+i)/(alpha+i));
end
y1 = y(1:end-1);
y2 = y(2:end);
y12 = y1 + y2;
y1y2 = y1.*y2;
ym = y12/2;
% stiffness matrix in y direction
Ay = zeros(NTy,3,3);
Ay(:,1,1) = a(:,1)./hy.^2;
Ay(:,1,2) = -Ay(:,1,1);
Ay(:,2,1) = -Ay(:,1,1);
Ay(:,2,2) = Ay(:,1,1);
Ay(:,1,3) = 8*(a(:,2) - ym.*a(:,1))./hy.^3;
Ay(:,3,1) = Ay(:,1,3);
Ay(:,2,3) = -Ay(:,1,3);
Ay(:,3,2) = Ay(:,2,3);
Ay(:,3,3) = 64*(a(:,3) - 2*ym.*a(:,2) + ym.^2.*a(:,1))./hy.^4;
% mass matrix in y direction
My = zeros(NTy,3,3);
My(:,1,1) = (a(:,3) - 2*y1.*a(:,2) + y1.^2.*a(:,1))./hy.^2;
My(:,1,2) = (-a(:,3) + (y1+y2).*a(:,2) - y1.*y2.*a(:,1))./hy.^2;
My(:,2,1) = My(:,1,2);
My(:,2,2) = (a(:,3) - 2*y2.*a(:,2) + y2.^2.*a(:,1))./hy.^2;
My(:,1,3) = 4*(a(:,4) - (y12+y2).*a(:,3) + (y2.*y12 + y1y2).*a(:,2) ...
             - y2.*y1y2.*a(:,1))./hy.^3;
My(:,3,1) = My(:,1,3);
My(:,2,3) = 4*(-a(:,4) + (y12+y1).*a(:,3) - (y1.*y12 + y1y2).*a(:,2) ...
             + y1.*y1y2.*a(:,1))./hy.^3;
My(:,3,2) = My(:,2,3);
My(:,3,3) = 16*(a(:,5) - 2*y12.*a(:,4) + (y12.^2 + 2*y1y2).*a(:,3)...
               -2*y12.*y1.*y2.*a(:,2) + y1y2.^2.*a(:,1))./hy.^4;

%% Stiffness matrix and mass matrix in the original direction
[Dphi,area] = gradbasis(node,elem);
% Compute a piecewise constant diffusion coefficient
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
if ~isempty(pde.d) % build the coefficients into the scaled area
    areaK = K.*area;
else
    areaK = area;
end
At = zeros(NT,3,3);
Mt = zeros(NT,3,3);
for i = 1:3
    for j = 1:3
        At(:,i,j) = (Dphi(:,1,i).*Dphi(:,1,j) + Dphi(:,2,i).*Dphi(:,2,j)).*areaK;
        Mt(:,i,j) = areaK*((i==j)+1)/12;
    end
end

%% Assemble stiffness matrix
dofMap = repmat(1:Nxdof,Nydof,1) + repmat((0:Nydof-1)'*Nxdof,1,Nxdof);
ii = zeros(9*9*NTtotal,1);
jj = zeros(9*9*NTtotal,1);
sA = zeros(9*9*NTtotal,1);
index = 0;
for m = 1:3
    for n = 1:3
        for i = 1:3
            for j = 1:3
                if m < 3
                    ii(index+1:index+NTtotal) = dofMap(m:Ny+m-2,elem(:,i));
                else % m = 3: middle points of elements in y direction
                    ii(index+1:index+NTtotal) = dofMap(Ny+1:Nydof,elem(:,i));
                end
                if n < 3
                    jj(index+1:index+NTtotal) = dofMap(n:Ny+n-2,elem(:,j));
                else % n = 3: middle points of elements in y direction
                    jj(index+1:index+NTtotal) = dofMap(Ny+1:Nydof,elem(:,j));
                end
                sA(index+1:index+NTtotal) = kron(At(:,i,j),My(:,m,n)) + ...
                                            kron(Mt(:,i,j),Ay(:,m,n));
                index = index + NTtotal;
            end
        end
    end
end
A = sparse(ii,jj,sA,Ndof,Ndof);
clear ii jj sA At Mt Ay My

%% Assemble the right hand side
b = zeros(Ndof,1);
u = zeros(Ndof,1);

%% Set up boundary conditions
% find Dirichlet boundary nodes
bdNode = findboundary(elem);  % boundary vertices of mesh in x-direction
lateralbd = dofMap(:,bdNode);
topbd = dofMap(Ny,:);

% Modify the matrix to include the Dirichlet boundary condition
bdidx = zeros(Ndof,1);
bdidx(lateralbd) = 1;
bdidx(topbd) = 1;
Tbd = spdiags(bdidx,0,Ndof,Ndof);
T = spdiags(1-bdidx,0,Ndof,Ndof);
A = T*A*T + Tbd;

% Compute boundary integral over the original domain
if ~isfield(option,'fquadorder')
    option.fquadorder = 4;
end
ft = zeros(NT,3);
ds = 2^(1-2*s)*gamma(1-s)/gamma(s);
if isfield(pde,'f') && ~ischar(pde.f)
    [lambdaf,weightf] = quadpts(option.fquadorder);
    phif = lambdaf;                 % linear bases
    nQuadf = size(lambdaf,1);
    for p = 1:nQuadf
        % quadrature points in the x-y coordinate
        pxy = lambdaf(p,1)*node(elem(:,1),:) ...
            + lambdaf(p,2)*node(elem(:,2),:) ...
            + lambdaf(p,3)*node(elem(:,3),:);
        fp = pde.f(pxy);
        for i = 1:3
            ft(:,i) = ft(:,i) + weightf(p)*phif(p,i)*fp;
        end
    end
elseif isfield(pde,'f') && ischar(pde.f)
    if strcmp(pde.f,'intx') % f is a distribution
        % compute (intx, d_x phi)
        [lambdaf,weightf] = quadpts(option.fquadorder);
        nQuad = size(lambdaf,1);
        bt = zeros(NT,1);
        for p = 1:nQuad
            pxy = lambdaf(p,1)*node(elem(:,1),:) ...
                + lambdaf(p,2)*node(elem(:,2),:) ...
                + lambdaf(p,3)*node(elem(:,3),:);
            bt = bt + weightf(p)*pde.intx(pxy);
        end
%         [Dphi,area] = gradbasis(node,elem);
        for i = 1:3
            ft(:,i) = Dphi(:,1,i).*bt; % (intx, d_xphi)
        end
    end
end
ft = ds*ft.*repmat(area,1,3);
b = b + accumarray(elem(:),ft(:),[Ndof,1]);
% Neumann edfts are considered as open set. So the corner points should be
% set as Dirichlet boundary condition!
b(bdNode) = 0;
clear ft

%% Record assembling time
assembleTime = toc;
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
freeDof = find(bdidx==0);
if isempty(freeDof), return; end
% Set up solver type
if isempty(option) || ~isfield(option,'solver')    % no option.solver
    if Ndof <= 2e3  % 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) = A(freeDof,freeDof)\b(freeDof);
        residual = norm(b - A*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'
        option.x0 = u;
        option.solver = 'VCYCLE';
        option.freeDof = freeDof;
        [u,info] = mgfracLapP1P2(A,b,elem,option);
    case 'amg'
        option.solver = 'CG';
        [u(freeDof),info] = amg(A(freeDof,freeDof),b(freeDof),option);
end

%% Compute error using boundary integral
if isfield(pde,'exactu')
    err = zeros(NT,1);
    [lambda,weight] = quadpts(8);
    phi = lambda;                 % linear bases
    nQuadf = size(lambda,1);
    for p = 1:nQuadf
        % 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);
        uhp = u(elem(:,1))*phi(p,1) + u(elem(:,2))*phi(p,2) + u(elem(:,3))*phi(p,3);
        up = pde.exactu(pxy);
        err = err + weight(p)*fp.*(up - uhp);
%         err = err + weight(p)*fp.*(up - 2*uhp);
    end
    err = ds*sum(err.*area);
%     err = ds*sum(err.*area) + u'*(A*u);
    err = sqrt(abs(err));
else
    err = 0;
end
info.errH1 = err;

%% Output information
eqn = struct('A',A,'b',b,'freeDof',freeDof);
info.assembleTime = assembleTime;


function  SELVEparam = initSELVE(traindata, SELVEparam)

SELVEparam.s = 2;   %number of nearest anchors, please tune this parameter on different datasets
SELVEparam.lambda = 0.1;
SELVEparam.beta = 0.5;
SELVEparam.RedDim = 300;  % the left dimensions of original data
SELVEparam.m = 300;       % the number of landmark
SELVEparam.sigma = 0;

kmMaxIter = 10;
kmNumRep = 1;
[label,anchor] = litekmeans(traindata,SELVEparam.m,'MaxIter',kmMaxIter,'Replicates',kmNumRep);
clear kmMaxIter kmNumRep;

temptraindata = zeros(size(traindata,1),size(traindata,2));
tempanchor = zeros(SELVEparam.m,size(traindata,2));
totalnum = 0;

for i=1:length(unique(label))
    tempnum = find(label == i);
    temptraindata(totalnum +1: totalnum + length(tempnum),:) = traindata(tempnum,:);
    tempanchor(totalnum +1:totalnum + length(tempnum),:) = repmat(anchor(i,:),length(tempnum),1);
    totalnum = totalnum + length(tempnum);
end
Gamma = temptraindata - tempanchor;  % ins * fea
SELVEparam.GtG = Gamma'*Gamma;
clear temptraindata tempanchor Gamma tempnum totalnum;

%% searching for transformation matrix
SELVEparam.XtX = traindata'*traindata;
SELVEparam.anchor = anchor;
SELVEparam.label = label;


% [INPUT]
% p = A vector of floats [0,Inf) of length t representing the prices.
% r = A vector of floats (-Inf,Inf) of length t representing the logarithmic returns.
% v = A vector of floats [0,Inf) of length t representing the trading volumes.
% cp = A vector of floats [0,Inf) of length t representing the market capitalization.
% bwl = An integer [90,252] representing the dimension of the long bandwidth.
% bwm = An integer [21,90) representing the dimension of the medium bandwidth.
% bws = An integer [5,21) representing the dimension of the short bandwidth.
%
% [OUTPUT]
% hhlr = A column vector of floats [0,Inf) of length t representing the Hui-Heubel Liquidity Ratio.
% tr = A column vector of floats [0,Inf) of length t representing the Turnover Ratio.
% vr = A column vector of floats [0,Inf) of length t representing the Variance Ratio.

function [hhlr,tr,vr] = liquidity_metrics(varargin)

    persistent ip;

    if (isempty(ip))
        ip = inputParser();
        ip.addRequired('p',@(x)validateattributes(x,{'double'},{'real' 'finite' 'nonnegative' 'vector' 'nonempty'}));
        ip.addRequired('r',@(x)validateattributes(x,{'double'},{'real' 'finite' 'vector' 'nonempty'}));
        ip.addRequired('v',@(x)validateattributes(x,{'double'},{'real' 'finite' 'nonnegative' 'vector' 'nonempty'}));
        ip.addRequired('cp',@(x)validateattributes(x,{'double'},{'real' 'finite' 'nonnegative' 'vector' 'nonempty'}));
        ip.addRequired('bwl',@(x)validateattributes(x,{'double'},{'real' 'finite' 'integer' '>=' 90 '<=' 252 'scalar'}));
        ip.addRequired('bwm',@(x)validateattributes(x,{'double'},{'real' 'finite' 'integer' '>=' 21 '<' 90 'scalar'}));
        ip.addRequired('bws',@(x)validateattributes(x,{'double'},{'real' 'finite' 'integer' '>=' 5 '<' 21 'scalar'}));
    end

    ip.parse(varargin{:});

    ipr = ip.Results;
    [p,r,v,cp,bwl,bwm,bws] = validate_input(ipr.p,ipr.r,ipr.v,ipr.cp,ipr.bwl,ipr.bwm,ipr.bws);

    nargoutchk(3,3);

    [hhlr,tr,vr] = liquidity_metrics_internal(p,r,v,cp,bwl,bwm,bws);

end

function [hhlr,tr,vr] = liquidity_metrics_internal(p,r,v,cp,bwl,bwm,bws)

    hhlr = calculate_hhlr(p,v,cp,bwl,bws);
    tr = calculate_tr(v,cp,bwl);
    vr = calculate_vr(r,bwl,bwm);

end

function hhlr = calculate_hhlr(p,v,cp,bwl,bws)

    tr = v ./ cp;
    tr(~isfinite(tr)) = 0;

    windows_p = extract_rolling_windows(p,bws);
    dp = cellfun(@(x)(max(x) - min(x)) / min(x),windows_p);

    alpha = 2 / (bwl + 1);

    hhlr = dp ./ tr;
    hhlr(~isfinite(hhlr)) = 0;
    hhlr(1:bws) = mean(hhlr(bws+1:bws*2+1));
    hhlr = [hhlr(1); filter(alpha,[1 (alpha - 1)],hhlr(2:end),(1 - alpha) * hhlr(1))];
    hhlr = (hhlr - min(hhlr)) ./ (max(hhlr) - min(hhlr));

end

function tr = calculate_tr(v,cp,bwl)

    alpha = 2 / (bwl + 1);

    tr = v ./ cp;
    tr(~isfinite(tr)) = 0;
    tr = [tr(1); filter(alpha,[1 (alpha - 1)],tr(2:end),(1 - alpha) * tr(1))];
    tr = (tr - min(tr)) ./ (max(tr) - min(tr));

end

function vr = calculate_vr(r,bwl,bwm)

    alpha = 2 / (bwl + 1);
    t = bwl / bwm;

    windows_long = extract_rolling_windows(r,bwl);
    var_long = cellfun(@var,windows_long);

    windows_short = extract_rolling_windows(r,bwm);
    var_short = cellfun(@var,windows_short);

    vr = var_long ./ (t .* var_short);
    vr(~isfinite(vr)) = 0;
    vr(1:bwm) = mean(vr(bwm+1:bwm*2+1));
    vr = [vr(1); filter(alpha,[1 (alpha - 1)],vr(2:end),(1 - alpha) * vr(1))];

end

function [p,r,v,cp,bwl,bwm,bws] = validate_input(p,r,v,cp,bwl,bwm,bws)

    data = {p(:) r(:) v(:) cp(:)};

    l = unique(cellfun(@numel,data));

    if (numel(l) ~= 1)
        error('The number of elements of ''p'', ''r'' and ''v'' must be equal.');
    end

    if (l < 5)
        error('The value of ''p'', ''r'' and ''v'' is invalid. Expected inputs to be vectors containing at least 5 elements.');
    end

    [p,r,v,cp] = deal(data{:});

    if (bwl < (bwm * 2))
        error(['The long bandwidth (' num2str(bwl) ') must be at least twice the medium bandwidth (' num2str(bwm) ').']);
    end

    if (bwm < (bws * 2))
        error(['The medium bandwidth (' num2str(bwm) ') must be at least twice the short bandwidth (' num2str(bws) ').']);
    end

end


%
%   Copyright (C) 2016  Starsky Wong <sununs11@gmail.com>
%
%   Note: The SIFT algorithm is patented in the United States and cannot be
%   used in commercial products without a license from the University of
%   British Columbia.  For more information, refer to the file LICENSE
%   that accompanied this distribution.

function [ descrs, locs ] = getFeatures( input_img )
% Function: Get sift features and descriptors
global gauss_pyr;
global dog_pyr;
global init_sigma;
global octvs;
global intvls;
global ddata_array;
global features;
if(size(input_img,3)==3)
    input_img = rgb2gray(input_img);
end
input_img = im2double(input_img);

%% Build DoG Pyramid
% initial sigma
init_sigma = 1.6;
% number of intervals per octave
intvls = 3;
s = intvls;
k = 2^(1/s);
sigma = ones(1,s+3);
sigma(1) = init_sigma;
sigma(2) = init_sigma*sqrt(k*k-1);
for i = 3:s+3
    sigma(i) = sigma(i-1)*k;
end
% default cubic method
input_img = imresize(input_img,2);
% assume the original image has a blur of sigma = 0.5
input_img = gaussian(input_img,sqrt(init_sigma^2-0.5^2*4));
% smallest dimension of top level is about 8 pixels
octvs = floor(log( min(size(input_img)) )/log(2) - 2);

% gaussian pyramid
[img_height,img_width] =  size(input_img);
gauss_pyr = cell(octvs,1);
% set image size
gimg_size = zeros(octvs,2);
gimg_size(1,:) = [img_height,img_width];
for i = 1:octvs
    if (i~=1)
        gimg_size(i,:) = [round(size(gauss_pyr{i-1},1)/2),round(size(gauss_pyr{i-1},2)/2)];
    end
    gauss_pyr{i} = zeros( gimg_size(i,1),gimg_size(i,2),s+3 );
end
for i = 1:octvs
    for j = 1:s+3
        if (i==1 && j==1)
            gauss_pyr{i}(:,:,j) = input_img;
        % downsample for the first image in an octave, from the s+1 image
        % in previous octave.
        elseif (j==1)
            gauss_pyr{i}(:,:,j) = imresize(gauss_pyr{i-1}(:,:,s+1),0.5);
        else
            gauss_pyr{i}(:,:,j) = gaussian(gauss_pyr{i}(:,:,j-1),sigma(j));
        end
    end
end
% dog pyramid
dog_pyr = cell(octvs,1);
for i = 1:octvs
    dog_pyr{i} = zeros(gimg_size(i,1),gimg_size(i,2),s+2);
    for j = 1:s+2
    dog_pyr{i}(:,:,j) = gauss_pyr{i}(:,:,j+1) - gauss_pyr{i}(:,:,j);
    end
end
% for i = 1:size(dog_pyr,1)
%     for j = 1:size(dog_pyr{i},3)
%         imwrite(im2bw(im2uint8(dog_pyr{i}(:,:,j)),0),['dog_pyr\dog_pyr_',num2str(i),num2str(j),'.png']);
%     end
% end

%% Accurate Keypoint Localization
% width of border in which to ignore keypoints
img_border = 5;
% maximum steps of keypoint interpolation
max_interp_steps = 5;
% low threshold on feature contrast
contr_thr = 0.04;
% high threshold on feature ratio of principal curvatures
curv_thr = 10;
prelim_contr_thr = 0.5*contr_thr/intvls;
ddata_array = struct('x',0,'y',0,'octv',0,'intvl',0,'x_hat',[0,0,0],'scl_octv',0);
ddata_index = 1;
for i = 1:octvs
    [height, width] = size(dog_pyr{i}(:,:,1));
    % find extrema in middle intvls
    for j = 2:s+1
        dog_imgs = dog_pyr{i};
        dog_img = dog_imgs(:,:,j);
        for x = img_border+1:height-img_border
            for y = img_border+1:width-img_border
                % preliminary check on contrast
                if(abs(dog_img(x,y)) > prelim_contr_thr)
                    % check 26 neighboring pixels
                    if(isExtremum(j,x,y))
                        ddata = interpLocation(dog_imgs,height,width,i,j,x,y,img_border,contr_thr,max_interp_steps);
                        if(~isempty(ddata))
                            if(~isEdgeLike(dog_img,ddata.x,ddata.y,curv_thr))
                                 ddata_array(ddata_index) = ddata;
                                 ddata_index = ddata_index + 1;
                            end
                        end
                    end
                end
            end
        end
    end
end

function [ flag ] = isExtremum( intvl, x, y)
% Function: Find Extrema in 26 neighboring pixels
    value = dog_imgs(x,y,intvl);
    block = dog_imgs(x-1:x+1,y-1:y+1,intvl-1:intvl+1);
    if ( value > 0 && value == max(block(:)) )
        flag = 1;
    elseif ( value == min(block(:)) )
        flag = 1;
    else
        flag = 0;
    end
end

%% Orientation Assignment
% number of detected points
n = size(ddata_array,2);
% determines gaussian sigma for orientation assignment
ori_sig_factr = 1.5;
% number of bins in histogram
ori_hist_bins = 36;
% orientation magnitude relative to max that results in new feature
ori_peak_ratio = 0.8;
% array of feature
features = struct('ddata_index',0,'x',0,'y',0,'scl',0,'ori',0,'descr',[]);
feat_index = 1;
for i = 1:n
    ddata = ddata_array(i);
    ori_sigma = ori_sig_factr * ddata.scl_octv;
    % generate a histogram for the gradient distribution around a keypoint
    hist = oriHist(gauss_pyr{ddata.octv}(:,:,ddata.intvl),ddata.x,ddata.y,ori_hist_bins,round(3*ori_sigma),ori_sigma);
    for j = 1:2
        smoothOriHist(hist,ori_hist_bins);
    end
    % generate feature from ddata and orientation hist peak
    % add orientations greater than or equal to 80% of the largest orientation magnitude
    feat_index = addOriFeatures(i,feat_index,ddata,hist,ori_hist_bins,ori_peak_ratio);
end

%% Descriptor Generation
% number of features
n = size(features,2);
% width of 2d array of orientation histograms
descr_hist_d = 4;
% bins per orientation histogram
descr_hist_obins = 8;
% threshold on magnitude of elements of descriptor vector
descr_mag_thr = 0.2;
descr_length = descr_hist_d*descr_hist_d*descr_hist_obins;
local_features = features;
local_ddata_array = ddata_array;
local_gauss_pyr = gauss_pyr;
clear features;
clear ddata_array;
clear gauss_pyr;
clear dog_pyr;
parfor feat_index = 1:n
    feat = local_features(feat_index);
    ddata = local_ddata_array(feat.ddata_index);
    gauss_img = local_gauss_pyr{ddata.octv}(:,:,ddata.intvl);
% computes the 2D array of orientation histograms that form the feature descriptor
    hist_width = 3*ddata.scl_octv;
    radius = round( hist_width * (descr_hist_d + 1) * sqrt(2) / 2 );
    feat_ori = feat.ori;
    ddata_x = ddata.x;
    ddata_y = ddata.y;
    hist = zeros(1,descr_length);
    for i = -radius:radius
        for j = -radius:radius
            j_rot = j*cos(feat_ori) - i*sin(feat_ori);
            i_rot = j*sin(feat_ori) + i*cos(feat_ori);
            r_bin = i_rot/hist_width + descr_hist_d/2 - 0.5;
            c_bin = j_rot/hist_width + descr_hist_d/2 - 0.5;
            if (r_bin > -1 && r_bin < descr_hist_d && c_bin > -1 && c_bin < descr_hist_d)
                mag_ori = calcGrad(gauss_img,ddata_x+i,ddata_y+j);
                if (mag_ori(1) ~= -1)
                    ori = mag_ori(2);
                    ori = ori - feat_ori;
                    while (ori < 0)
                        ori = ori + 2*pi;
                    end
                    % i think it's theoretically impossible
                    while (ori >= 2*pi)
                        ori = ori - 2*pi;
                        disp('###################what the fuck?###################');
                    end
                    o_bin = ori * descr_hist_obins / (2*pi);
                    w = exp( -(j_rot*j_rot+i_rot*i_rot) / (2*(0.5*descr_hist_d*hist_width)^2) );
                    hist = interpHistEntry(hist,r_bin,c_bin,o_bin,mag_ori(1)*w,descr_hist_d,descr_hist_obins);
                end
            end
        end
    end
    local_features(feat_index) = hist2Descr(feat,hist,descr_mag_thr);
end
% sort the descriptors by descending scale order
features_scl = [local_features.scl];
[~,features_order] = sort(features_scl,'descend');
% return descriptors and locations
descrs = zeros(n,descr_length);
locs = zeros(n,2);
for i = 1:n
    descrs(i,:) = local_features(features_order(i)).descr;
    locs(i,1) = local_features(features_order(i)).x;
    locs(i,2) = local_features(features_order(i)).y;
end

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/
%==============================================================================
% This function manages the solution of H * dy = - dJ' for various settings
%
%   'backslash',         simply solve dy = H\rhs;
%   'mbCG',               matrixBased, plain CG
%   'mbPCG-SGS',         matrixBased, PCG with Symmetric Gauss Seidel preconditioner
%   'mbPCG-ICHOL',     matrixBased, PCG with incomplete Cholesky preconditioner
%   'mbPCG-Jacobi', matrixBased, PCG with Jacobi preconditioner
%
%   'MG-elastic',           matrixFree, PCG Multigrid for elastic regularizer
%   'CG-elastic',       plain CG with H = A as in (1) below
%   'CG-curvature',          plain CG with H = A as in (1) below
%   'PCG-elastic',      Jacobi preconditioned with H = A as in (1,) below
%   'PCG-curvature',      Jacobi preconditioned with H = A as in (1,) below
%   'PCG-hyperElastic', ???
%   'CG'                      plain CG with H full or as operator
%
% the operator H can be a
%    - matrix (matrixBased)
%    - function (coding the action ofH)
%    - struct where H describes the pieces of a complex H
%
%  (1) action of H: distance + regularizer,
%       H = P'*dr'*d2psi*dr*P + d2S
%
%      Hoperator = @(x) ...
%       H.d2D.P((H.d2D.dr'*H.d2D.d2psi*H.d2D.dr)*H.d2D.P(x)) ...
%       + H.d2S.d2S(x,H.omega,H.m);
%
%  (2) Jacobi (diagonal) preconditing
%      Ddiag = diag(H.d2D.dr'*H.d2D.d2psi*H.d2D.dr);
%      D = H.d2D.P(full(Ddiag))  +  H.d2S.diag(H.omega,H.m);
%      PC = @(x) D.\x; % Jacobi preconditioner
%==============================================================================

function [dy,solver] = solveLinearSystem(rhs,H,solver,varargin)

if nargin == 0,
  testSolver
  help(mfilename);
  runMinimalExample;
  dy = 'endOfMinimalExample';
  return;
end;

maxIterCG = 500;
tolCG     = 1e-1;

for k=1:2:length(varargin), % overwrites default parameter
  eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

if isempty(solver) && isnumeric(H),
  warning('no solver specified, try backslash');
  solver = 'backslash';
end;

if isa(solver,'function_handle')
  dy     = solver(rhs,H,maxIterCG,tolCG);
  return;
end

if isstruct(H), % matrixFree mode, configure operator
  Hoperator = @(x) ...
    H.d2D.P((H.d2D.dr'*H.d2D.d2psi*H.d2D.dr)*H.d2D.P(x)) ...
    + H.d2S.d2S(x,H.omega,H.m);
end;

%------------------------------------------------------------------------------
switch solver
  %------------------------------------------------------------------------------

  % ---------------------------------------------------------------------------
  % matrix based
  % ---------------------------------------------------------------------------

  case 'backslash',
    dy = H\rhs;

  case 'mbCG',
    [dy,flag,relres,iter] = pcg(H,rhs,tolCG,maxIterCG);

  case 'mbPCG-SGS',
    L   = tril(H); % Symmetric Gauss Seidel Preconditioning,
    D   = diag(H); % L is lower, D is diagonal, U = L'
    SGS = @(x) L\(D.*(L'\x));
    [dy,flag,relres,iter] = pcg(H,rhs,tolCG,maxIterCG,SGS);

  case 'mbPCG-ICHOL',
    %L1   = cholinc(sparse(H),'0');
    L = ichol(H);
    [dy,flag,relres,iter] = pcg(H,rhs,tolCG,maxIterCG,L,L');

  case 'mbPCG-Jacobi',
    D   = diag(H); % D is diagonal
    PC = @(x) D.\x;
    [dy,flag,relres,iter] = pcg(H,rhs,tolCG,maxIterCG,PC);

    % ---------------------------------------------------------------------------
    % matrix free
    % ---------------------------------------------------------------------------

    % In all matrix free solvers the approximates Hessian is supplied by function handle.
    % Note, that the functionals have the common form
    %
    %   Jc = D + S (+ P) ==> d_2 Jc = d2D + d2S (+d2P)
    %
    % regularization and penalization (S,P) supply a struct stored in H.d2S or H.d2P
    % which already has a function handle describing the operator.
    % The  distance term (d2D) needs more care and the approximation to the Hessian
    % depends on the solver as well. Therefore the objective function does NOT supply
    % the function handle, but all variables needed to built such.
    % For multi-grid (available for elastic and curvature), only the diagonal of the distance
    % term is used
    % whereas in conjugate gradient (CG, available for elastic, curvature and hyperelastic)
    % methods we can model also the off-diagonals d2D(x) = (P'*dr'*d2psi*dr*P) * x
    %
    % The default solver for a chosen regularizer is parameterized by d2S.solver.
    % However, one can overload this setting using
    %    >> MLIR(..., 'solverNPIR','myFavoriteSolver', ... );

  case {'MG-elastic','MG'}, % multigrid for elastic regularizer
    dy = MGsolver(rhs,H);

  case {'CG-elastic','CG-curvature'},
    if isstruct(H)
      %       A         = @(x) ...
      %         H.d2D.P((H.d2D.dr'*H.d2D.d2psi*H.d2D.dr)*H.d2D.P(x)) ...
      %         + H.d2S.d2S(x,H.omega,H.m);
      [dy,flag,relres,iter] = pcg(Hoperator,rhs,tolCG,maxIterCG);
    else
      [dy,flag,relres,iter] = pcg(H,rhs,tolCG,maxIterCG);
    end

  case {'PCG-elastic','PCG-curvature'}

    % operator
    %     A         = @(x) ...
    %       H.d2D.P((H.d2D.dr'*H.d2D.d2psi*H.d2D.dr)*H.d2D.P(x)) ...
    %       + H.d2S.d2S(x,H.omega,H.m);
    % preconditioner
    Ddiag = diag(H.d2D.dr'*H.d2D.d2psi*H.d2D.dr);
    D = H.d2D.P(full(Ddiag))  +  H.d2S.diag(H.omega,H.m);
    PC = @(x) D.\x; % Jacobi preconditioner
    [dy,flag,relres,iter] = pcg(Hoperator,rhs,tolCG,maxIterCG,PC);

  case 'CG'
    if isstruct(H)
      [dy,flag,relres,iter] = pcg(H.operator,rhs,tolCG,maxIterCG);
    else
      [dy,flag,relres,iter] = pcg(H,rhs,tolCG,maxIterCG);
    end

  case {'PCG-hyperElastic'}
    M         = @(x) H.d2D.P((H.d2D.dr'*H.d2D.d2psi*H.d2D.dr)*H.d2D.P(x));
    Hoperator = @(x) M(x) + H.d2S.d2S(x,H.omega,H.m,H.d2S.yc);
    Ddiag     = diag(H.d2D.dr'*H.d2D.d2psi*H.d2D.dr);
    D         = H.d2D.P(full(Ddiag))  +  H.d2S.diag(H.d2S.yc);
    Preconditioner = @(x) D.\x; % Jacobi preconditioner
    [dy,flag,relres,iter] = pcg(Hoperator,rhs,tolCG,maxIterCG,Preconditioner);

  otherwise,
    keyboard
    error(1)

    %------------------------------------------------------------------------------
end
%------------------------------------------------------------------------------

if exist('flag','var')
  flags = {
    sprintf('iter=%d>maxIter=%d but relres=%e>relResTol=%s',...
    iter,maxIterCG,relres,tolCG)
    'preconditioner is ill-conditioned'
    'stagnation: x_k=x_{k+1}'
    'scalars too small/large to continue computation'
    };
  switch flag
    case {1,2,3,4}
      fprintf('%s // PCG: %s\n',flags{flag});
    otherwise
      fprintf('%s // PCG :iter = %d of %d, relRes= %1.2e, tolCG= %1.2e\n',...
        mfilename,iter,maxIterCG,relres,tolCG);
  end
end

%------------------------------------------------------------------------------

function runMinimalExample

%% prepare 3D data
setup3DbrainData
level = 4;
omega = ML{level}.omega;
m     = ML{level}.m;

viewImage('reset','viewImage','viewImage2D','colormap',bone(256),'axis','off');
imgModel('reset','imgModel','splineInterMex','regularizer','moments','theta',1e-2);
distance('reset','distance','SSD');
trafo('reset','trafo','affine3Dsparse');

[T,R] = imgModel('coefficients',ML{level}.T,ML{level}.R,omega,'out',0);
xc = getCellCenteredGrid(omega,m);
Rc = imgModel(R,omega,xc);
w0   = trafo('w0');
beta = 0; M = []; wRef = [];
[Jc,para,dJPIRsparse,HPIRsparse] = PIRobjFctn(T,Rc,omega,m,beta,M,wRef,xc,w0);

%% prepare 2D data and approximations to Hessian
setup2DhandData

viewImage('reset','viewImage','viewImage2D','colormap',bone(256),'axis','off');
imgModel('reset','imgModel','splineInter','regularizer','moments','theta',1e-2);
distance('reset','distance','SSD');

level = 4;
omega = ML{level}.omega;
m     = ML{level}.m;
[T,R] = imgModel('coefficients',ML{level}.T,ML{level}.R,omega,'out',0);

% initialize the image model, distance measure and regularizer
xc    = getCellCenteredGrid(omega,m);
Rc    = imgModel(R,omega,xc);

% setup PIR objective
w0   = trafo('w0');
beta = 1e3;
M    = diag([4,4,4,4,1,1]);

trafo('reset','trafo','affine2D');
w0   = trafo('w0');
wRef = w0;
[Jc,para,dJPIR,HPIR] = PIRobjFctn(T,Rc,omega,m,beta,M,wRef,xc,w0);

% setup NPIR objective
yc   = getStaggeredGrid(omega,m);
yRef = 0*yc;

regularizer('reset','regularizer','mbElastic','alpha',1e4,'mu',1,'lambda',0);
[Jc,para,dJmbElas,HmbElas] = NPIRobjFctn(T,Rc,omega,m,yRef,yc);

regularizer('reset','regularizer','mfElastic','alpha',1e4,'mu',1,'lambda',0);
[Jc,para,dJmfElas,HmfElas] = NPIRobjFctn(T,Rc,omega,m,yRef,yc);

HxElas = @(x) HmfElas.d2D.P((HmfElas.d2D.dr'*HmfElas.d2D.d2psi*HmfElas.d2D.dr)*HmfElas.d2D.P(x)) ...
  + HmfElas.d2S.d2S(x,HmfElas.omega,HmfElas.m);

yc   = getCellCenteredGrid(omega,m);
yRef = 0*yc;

regularizer('reset','regularizer','mbCurvature','alpha',1e4);
[Jc,para,dJmbCurv,HmbCurv] = NPIRobjFctn(T,Rc,omega,m,yRef,yc);

regularizer('reset','regularizer','mfCurvature','alpha',1e4);
[Jc,para,dJmfCurv,HmfCurv] = NPIRobjFctn(T,Rc,omega,m,yRef,yc);
HxCurv = @(x) HmfCurv.d2D.P((HmfCurv.d2D.dr'*HmfCurv.d2D.d2psi*HmfCurv.d2D.dr)*HmfCurv.d2D.P(x)) ...
  + HmfCurv.d2S.d2S(x,HmfCurv.omega,HmfCurv.m);

problems = {
  'PIR-backslash'
  'PIR-sparse'
  'mbElastic-backslash'
  'mbElastic-PCG-SGS'
  'mbElastic-PCG-ICHOL'
  'mbElastic-PCG-Jacobi'
  'mbElastic-CG'
  'mfElastic-MG'
  'mfElastic-CG'
  'mfElastic-PCG'
  'mbCurvature-backslash'
  'mbCurvature-PCG-SGS'
  'mbCurvature-PCG-ICHOL'
  'mbCurvature-PCG-Jacobi'
  'mbCurvature-CG'
  'mfCurvature-PCG'
  'mfCurvature-CG'
  }

for j = 1:length(problems)

  para = {};

  switch problems{j},

    case 'PIR-backslash',
      solver = 'backslash';
      H = HPIR; b = -dJPIR'; Hx = @(y) H*y;

    case 'PIR-sparse',
      solver = 'backslash';
      H = HPIRsparse; b = -dJPIRsparse'; Hx = @(y) H*y;

      %     case 2, % PRIR with conensed matrix
      % % fprintf('test PIR with condensed matrix\n');
      % % trafo('reset','trafo','affine2Dsparse');
      % % [Jc,para,dJ,H] = PIRobjFctn(T,Rc,omega,m,beta,M,wRef,xc,w0)
      % % dy = solveLinearSystem(-dJ',H,'backslash');
      % % test2 = norm(H*dy + dJ')

    case 'mbElastic-backslash'
      solver = 'backslash';
      H = HmbElas; b = -dJmbElas'; Hx = @(y) H*y;

    case 'mbElastic-PCG-SGS',
      solver = 'mbPCG-SGS';
      H = HmbElas; b = -dJmbElas'; Hx = @(y) H*y;
      para = {'tolCG',1e-5};

    case 'mbElastic-PCG-ICHOL',
      solver = 'mbPCG-ICHOL';
      H = HmbElas; b = -dJmbElas'; Hx = @(y) H*y;
      para = {'tolCG',1e-5};

    case 'mbElastic-PCG-Jacobi'
      solver = 'mbPCG-Jacobi';
      H = HmbElas; b = -dJmbElas'; Hx = @(y) H*y;
      para = {'tolCG',1e-5};

    case 'mbElastic-CG'
      solver = 'mbCG';
      H = HmbElas; b = -dJmbElas'; Hx = @(y) H*y;
      para = {'tolCG',1e-5};

    case 'mfElastic-MG'
      solver = 'MG-elastic';
      H = HmfElas; b = -dJmfElas'; Hx = HxElas;

    case 'mfElastic-CG'
      solver = 'CG-elastic';
      H = HmfElas; b = -dJmfElas'; Hx = HxElas;

    case 'mfElastic-PCG'
      solver = 'PCG-elastic';
      H = HmfElas; b = -dJmfElas'; Hx = HxElas;

    case 'mbCurvature-backslash'
      solver = 'backslash';
      H = HmbElas; b = -dJmbElas'; Hx = @(y) H*y;

    case 'mbCurvature-PCG-SGS',
      solver = 'mbPCG-SGS';
      H = HmbElas; b = -dJmbElas'; Hx = @(y) H*y;
      para = {'tolCG',1e-5};

    case 'mbCurvature-PCG-ICHOL',
      solver = 'mbPCG-ICHOL';
      H = HmbElas; b = -dJmbElas'; Hx = @(y) H*y;
      para = {'tolCG',1e-5};

    case 'mbCurvature-PCG-Jacobi'
      solver = 'mbPCG-Jacobi';
      H = HmbElas; b = -dJmbElas'; Hx = @(y) H*y;
      para = {'tolCG',1e-5};

    case 'mbCurvature-CG'
      solver = 'mbCG';
      H = HmbElas; b = -dJmbElas'; Hx = @(y) H*y;
      para = {'tolCG',1e-5};

    case 'mfCurvature-PCG'
      solver = 'PCG-curvature';
      H = HmfElas; b = -dJmfElas'; Hx = HxElas;

    case 'mfCurvature-CG'
      solver = 'CG-curvature';
      H = HmfElas; b = -dJmfElas'; Hx = HxElas;

    case 'MG'

    otherwise,
      error('1');
  end;

  dy = solveLinearSystem(b,H,solver,para{:});
  test = norm(Hx(dy)-b)/norm(b);
  fprintf('%-2d of %2d: %-40s test = %s\n',j,length(problems),...
    sprintf('test <%s>:',problems{j}),num2str(test));
end;

return;
%==============================================================================

%   case 'mfCurvature',
%     Afun     = @(dy) mfAy(dy,H);
%     [dy,FLAG] = pcg(Afun,rhs,tolCG,maxIterCG);
%     %

%
%   if isnumeric(H),
%
%   elseif isstruct(H),
%     if isfield(H,'solver'),
%       solver = H.solver;
%     elseif isfield(H,'d2S') && isfield(H.d2S,'solver'),
%       solver = H.d2S.solver;
%     else
%       error('solver has not been defined')
%     end;
%   else
%     keyboard
%   end;
%   fprintf('[set solver to <%s> in %s]\n',solver,mfilename);
% end;
% if isempty(solver)
%   if isstruct(H),
%   else % no regularizer initialized, assuming PIR
%     dy = H\rhs;
%     return;
%   end;
% end;
%
% if isa(solver, 'function_handle')
%     dy = feval(solver, rhs, H, maxIterCG, tolCG);
%     return
% end

%
%     %     case {'Joint-CG-hyperElastic'}
%     %         Afctn = @(x) H.M(x) + H.d2S.d2S(x,H.omega,H.m,H.d2S.yc);
%     %         [dy,flag,relres,iter] = pcg(Afctn,rhs,tolCG,maxIterCG);
%     %     case {'CG-hyperElastic'}
%     %         M =@(x) H.d2D.P((H.d2D.dr'*H.d2D.d2psi*H.d2D.dr)*H.d2D.P(x));
%     %         Afctn = @(x) M(x) + H.d2S.d2S(x,H.omega,H.m,H.d2S.yc);
%     %         [dy,flag,relres,iter] = pcg(Afctn,rhs,tolCG,maxIterCG);
%     %     otherwise
%     %         if isnumeric(H)
%     %             % if H is a matrix, solve the linear system using MATLAB's backslash
%     %             dy = H\rhs;
%     %         else
%     %             error(solver)
%     %         end
%   otherwise,
%     keyboard
%     error(1)
%     %
%   case {'PCG-hyperElastic'}
%     M =@(x) H.d2D.P((H.d2D.dr'*H.d2D.d2psi*H.d2D.dr)*H.d2D.P(x));
%     Afctn = @(x) M(x) + H.d2S.d2S(x,H.omega,H.m,H.d2S.yc);
%     Ddiag = diag(H.d2D.dr'*H.d2D.d2psi*H.d2D.dr);
%     D = H.d2D.P(full(Ddiag))  +  H.d2S.diag(H.d2S.yc);
%     PC = @(x) D.\x; % Jacobi preconditioner
%     [dy,flag,relres,iter] = pcg(Afctn,rhs,tolCG,maxIterCG,PC);
%
%     %   case {'PCG-elastic','PCG-curvature'}
%     %     % operator
%     %     A         = @(x) ...
%     %       H.d2D.P((H.d2D.dr'*H.d2D.d2psi*H.d2D.dr)*H.d2D.P(x)) ...
%     %       + H.d2S.d2S(x,H.omega,H.m);
%     %     % preconditioner
%     %     Ddiag = diag(H.d2D.dr'*H.d2D.d2psi*H.d2D.dr);
%     %     D = H.d2D.P(full(Ddiag))  +  H.d2S.diag(H.omega,H.m);
%     %     PC = @(x) D.\x; % Jacobi preconditioner
%     %     [dy,flag,relres,iter] = pcg(A,rhs,tolCG,maxIterCG,PC);
%
%
%     %     case {'Joint-CG-hyperElastic'}
%     %         Afctn = @(x) H.M(x) + H.d2S.d2S(x,H.omega,H.m,H.d2S.yc);
%     %         [dy,flag,relres,iter] = pcg(Afctn,rhs,tolCG,maxIterCG);
%     %     case {'CG-hyperElastic'}
%     %         M =@(x) H.d2D.P((H.d2D.dr'*H.d2D.d2psi*H.d2D.dr)*H.d2D.P(x));
%     %         Afctn = @(x) M(x) + H.d2S.d2S(x,H.omega,H.m,H.d2S.yc);
%     %         [dy,flag,relres,iter] = pcg(Afctn,rhs,tolCG,maxIterCG);
%     %     otherwise
%     %         if isnumeric(H)
%     %             % if H is a matrix, solve the linear system using MATLAB's backslash
%     %             dy = H\rhs;
%     %         else
%     %             error(solver)
%     %         end


% =========================================================================
% This code is part of the Matlab-based toolbox
% LagLDDDM - A Lagrangian Gauss--Newton--Krylov Solver for Mass- and
%                        Intensity-Preserving Diffeomorphic Image Registration
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM
%
% Example illustrating the performance of different preconditioners for
% the Gauss-Newton system in LDDMM. The example is described in detail in
% Section 4.2 of the paper:
%
% @article{MangRuthotto2017,
%   Title = {A {L}agrangian {G}auss--{N}ewton--{K}rylov solver for mass- and intensity-preserving diffeomorphic image registration},
%   Year = {2017},
%   Journal = {SIAM Journal on Scientific Computing},
%   Author = {A. Mang, L. Ruthotto},
% }
%
% =========================================================================
clear all; clc;
setup2Ddisc2CData

%% run affine pre-registration
imgModel('reset','imgModel','splineInterMex','regularizer','moments','theta',.1);
        trafo('set','trafo','affine2D');
        distance('set','distance','SSD');

alpha = [6e2 0];
parametric = 0;
pad  = 0.5;
nt = 0;
N    = 3;
mV     = @(m) ceil(1*m);
minLevel = 5;
maxLevel = 5;

%% run multilevel LDDMM registration

% 1) setup grid for velocities (padded)
omegaV = omega; omegaV(1:2:end) = omegaV(1:2:end)-pad;  omegaV(2:2:end) = omega(2:2:end)+pad;

% 2) setup regularizer
regularizer('reset','regularizer','mfDiffusionCC','alpha',alpha,'nt',nt,'HessianShift',1e-2); % stationary velocity

NPIRpara    = optPara('NPIR-GN');
NPIRpara.maxIter = 40;
NPIRpara.scheme = @GaussNewtonLDDMM;
[vc,~,wc,his] = MLLDDMM(ML,'minLevel',minLevel,'maxLevel',maxLevel,...
    'omegaV',omegaV,'mV',mV,'N',N,'parametric',parametric,'NPIRpara',NPIRpara,'plots',1);

%% generate plots to explore conditioning / sparsity
xc = getNodalGrid(omega,ML{minLevel}.m);
[T,R] = imgModel('coefficients',ML{minLevel}.T,ML{minLevel}.R,omega);
Rc    = imgModel(R,omega,center(xc,ML{minLevel}.m));

mV = ML{minLevel}.m;
fctn = @(vc) LDDMMobjFctn(T,Rc,omega,ML{minLevel}.m,0*vc,center(xc,ML{minLevel}.m),omegaV,mV,N,vc);

close all
regularizer('set','regularizer','mbDiffusionCC')
[Sc,dS,d2Smb] = regularizer(vc,omegaV,mV);
% get objective function
[J0,p0,dJ0,Hmb0] = fctn(0*vc);
[Jc,pc,dJc,Hmbc] = fctn(vc);

regularizer('set','regularizer','mfDiffusionCC')
[J0,p0,dJ0,Hmf0] = fctn(0*vc);
[Jc,pc,dJc,Hmfc] = fctn(vc);

%%
fig = figure(); clf;
fig.Name =sprintf('%s: Sparsity of Hessian',mfilename);

subplot(1,2,1)
spy(Hmb0);
title('H(v0)');

subplot(1,2,2);
spy(Hmbc);
title('H(vOpt');
%% explore PCG convergence at first iteration
hd = prod((omega(2:2:end)-omega(1:2:end))./ML{minLevel}.m);
Hmb0 = Hmb0 + hd* regularizer('get','HessianShift')* speye(size(Hmb0));
D   = diag(Hmb0); % D is diagonal
L   = tril(Hmb0); % Symmetric Gauss Seidel Preconditioning,

PCjac = @(x) D.\x;
PCsgs = @(x) full(L\(D.*(L'\x)));
PCnone = @(x) x;

[~,iter0cg,relres0cg,resvec0cg] = spectralPrecondPCG(-dJ0(:), Hmf0, 250, 1e-10,'prec',PCnone);
[~,iter0jac,relres0jac,resvec0jac] = spectralPrecondPCG(-dJ0(:), Hmf0, 250, 1e-10,'prec',PCjac);
[~,iter0sgs,relres0sgs,resvec0sgs] = spectralPrecondPCG(-dJ0(:), Hmf0, 250, 1e-10,'prec',PCsgs);
[~,iter0sp,relres0sp,resvec0sp] = spectralPrecondPCG(-dJ0(:), Hmf0, 250, 1e-10);

%%
figConv = figure(); clf
figConv.Name = 'PCG convergence';

subplot(1,2,1);
semilogy(resvec0cg/resvec0cg(1));
hold on;
semilogy(resvec0jac/resvec0jac(1));
semilogy(resvec0sgs/resvec0sgs(1));
semilogy(resvec0sp/resvec0sp(1));
title('first GN iteration');
ax = [0 250 1e-10 10];
axis(ax)
legend('CG','PCG-Jac','PCG-SGS','PCG-Spec','Location','SouthWest')

%% explore PCG convergence at final iteration
hd = prod((omega(2:2:end)-omega(1:2:end))./ML{minLevel}.m);
Hmb0 = Hmb0 + hd* regularizer('get','HessianShift')* speye(size(Hmb0));
D   = diag(Hmbc); % D is diagonal
L   = tril(Hmbc); % Symmetric Gauss Seidel Preconditioning,

linSol = @spectralPrecondPCG;
PCjac = @(x) D.\x;
PCsgs = @(x) full(L\(D.*(L'\x)));
PCnone = @(x) x;

[~,itercg,relrescg,resveccg]    = linSol(-dJc(:), Hmfc, 250, 1e-10,'prec',PCnone);
[~,iterjac,relresjac,resvecjac] = linSol(-dJc(:), Hmfc, 250, 1e-10,'prec',PCjac);
[~,itersgs,relressgs,resvecsgs] = linSol(-dJc(:), Hmfc, 250, 1e-10,'prec',PCsgs);
[~,itersp,relressp,resvecsp]    = linSol(-dJc(:), Hmfc, 250, 1e-10);

%%
fig = figure(figConv);
subplot(1,2,2);
semilogy(resveccg/resveccg(1));
hold on;
semilogy(resvecjac/resvecjac(1));
semilogy(resvecsgs/resvecsgs(1));
semilogy(resvecsp/resvecsp(1));
title('final iteration');

legend('CG','PCG-Jac','PCG-SGS','PCG-Spec','Location','SouthWest')
axis(ax)


function k = rbfwhiteKernDiagCompute(kern, t)

% RBFWHITEKERNDIAGCOMPUTE Compute diagonal of RBF-WHITE kernel.
% FORMAT
% DESC computes the diagonal of the kernel matrix for the RBF-WHITE kernel
% given a column vector of inputs.
% ARG kern : the kernel structure for which the kernel matrix is computed.
% ARG t : input data in the form of a column vector.
% RETURN k : a vector of the same size as t containing the diagonal of the
% kernel matrix computed at the given points.
%
% SEEALSO : rbfwhiteKernParamInit, kernDiagCompute, kernCreate,
% rbfwhiteKernCompute
%
% COPYRIGHT : David Luengo, 2009

% KERN

if size(t, 2) > 1
  error('Input can only have one column');
end

if (kern.isStationary == false)
    k = kern.variance/sqrt(8*pi) * erf(kern.inverseWidth*t/sqrt(2));
else
    k = zeros(size(t));
end


function res = vect_isPointInMesh(point, v, f, varargin)
%ISPOINTINMESH Check if a point is inside a 3D mesh.
%
%   B = isPointInMesh(PT, V, F)
%   Check if the point PT (given as a 1-by-3 array) is inside the mesh
%   defined by the vertices V and the face array F. The result is a
%   boolean.
%
%   If PT is a N-by-3 point array, the result is a N-by-1 array of logical.
%
%   Example
%     [v, f] = torusMesh([50 50 50 30 10 30 45]);
%     [x, y, z] = meshgrid(5:5:100, 5:5:100, 5:5:100);
%     res = false(size(x));
%     res(:) = isPointInMesh([x(:) y(:) z(:)], v, f);
%     figure; plot3(x(res), y(res), z(res), 'b.'); axis equal;
%
%   Algorithm:
%   The method computes the intersection with a ray starting from the
%   point(s) and with a random orientation. Some errors are possible if
%   rays crosses the mesh between two or three faces.
%
%   See also
%     meshes3d, intersectLineMesh3d

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2018-01-26,    using Matlab 9.3.0.713579 (R2017b)
% Copyright 2018 INRA - Cepia Software Platform.

% choose a random vector
vect = rand(1, 3);

% initialize array for result
np = size(point, 1);
res = false(np, 1);

% iterate over the various points
lines = createLine3d(point, vect);
[~, pos] = vect_intersectLineMesh3d(lines, v, f);
for i = 1:np
    res(i) = mod(sum(pos(:,:,i) > 0), 2) > 0;
end


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

%%
clear; close all; clc;

%%
% Plot settings
fontSize=15;
faceColor='b';
faceAlpha=1;
edgeColor='k';
edgeWidth=2;
markerSize=5;

%%

hf=cFigure; % Open figure for plotting
pColor=gjet(5);
for q=1:1:5
    %Defining the faces (F) and vertices (V) of a platonic solid
    [V,F]=platonic_solid(q,1); %q indicates solid type, r is the radius

    subplot(2,3,q); hold on;

    hp=gpatch(F,V,pColor(q,:),'k',faceAlpha,3);
    patchNormPlot(F,V);

    axisGeom(gca,fontSize);
    camlight('headlight'); lighting flat;
    axis off;
end
drawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

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


function temp_obj_eval = my_compute_precision_recall_fmeasure(u, u0_GT, u0_SKL_GT)
% 121227: Reza FARRAHI MOGHADDAM and Hossein ZIAEI NAFCHI
% 090519: Reza FARRAHI MOGHADDAM
% used in objective_evaluation_core.m

%
[xm ym] = size(u);
% figure, imshow(u)

%
if (numel(u0_GT) == 0)
    u0_GT = NaN * ones([xm ym]);
end

%
if (nargin == 2)
    % u0_SKL_GT = NaN * ones([xm ym]);
	u0_SKL_GT = ~bwmorph(~u0_GT, 'thin', 'inf');
end

% TP pixels
temp_tp = [u == 0] & [u0_GT == 0];

% FP pixels
temp_fp = [u == 0] & [u0_GT ~= 0];

% FN pixels
temp_fn = [u ~= 0] & [u0_GT == 0];

% TN pixels
temp_tn = [u ~= 0] & [u0_GT ~= 0];

% SKL TP / FN pixels
temp_skl_tp = [u == 0] & [u0_SKL_GT == 0];
temp_skl_fp = [u == 0] & [u0_SKL_GT ~= 0];
temp_skl_fn = [u ~= 0] & [u0_SKL_GT == 0];
temp_skl_tn = [u ~= 0] & [u0_SKL_GT ~= 0];

% counts
count_tp = sum(sum(temp_tp));
count_fp = sum(sum(temp_fp));
count_fn = sum(sum(temp_fn));
count_tn = sum(sum(temp_tn));
count_skl_tp = sum(sum(temp_skl_tp));
count_skl_fp = sum(sum(temp_skl_fp));
count_skl_fn = sum(sum(temp_skl_fn));
count_skl_tn = sum(sum(temp_skl_tn));

% precision
temp_p = count_tp / (count_fp + count_tp);

% recall
temp_r = count_tp / (count_fn + count_tp);
if (temp_r == 0)
    temp_r = NaN;
end

% p-recall
temp_pseudo_p = count_skl_tp / (count_skl_fp + count_skl_tp);
temp_pseudo_r = count_skl_tp / (count_skl_fn + count_skl_tp);
if (temp_pseudo_r == 0)
    temp_pseudo_r = NaN;
end

% f-measure
temp_f = 100 * 2 * (temp_p * temp_r) / (temp_p + temp_r);

% p-f-measure
temp_pseudo_f = 100 * 2 * (temp_p * temp_pseudo_r) / (temp_p + temp_pseudo_r);

% sensetivity
temp_sens = count_tp / (count_tp + count_fn);
if (temp_sens == 0)
    temp_sens = NaN;
end

% specificity
temp_spec = count_tn / (count_tn + count_fp);
if (temp_spec == 0)
    temp_spec = NaN;
end

% BCR: Balanced Classification Rate
temp_BCR = 0.5 * (temp_sens + temp_spec);

% AUC: Area Under the Curve
temp_AUC = 0.5 * (temp_sens + temp_spec);

% BER: Balanced Error Rate
temp_BER = 100 * (1 - temp_BCR);

% S-F-measure: harmonic mean of sensetivity and specificity
temp_s_f_measure = 100 * 2 * (temp_sens * temp_spec) / (temp_sens + temp_spec);

% Accuracy: mean of sensetivity and specificity
temp_accu = (count_tp + count_tn) / (count_tp + count_tn + count_fp + count_fn);

% gAccuracy: Geometric mean of sensetivity and specificity
temp_g_accu = sqrt(temp_sens * temp_spec);

% NRM (Negative Rate Metric) (*10^-2)
NR_FN = count_fn / (count_fn + count_tp);
NR_FP = count_fp / (count_fp + count_tn);
temp_NRM = (NR_FN + NR_FP) / 2;

% PSNR
err=sum(sum(temp_fp | temp_fn)) / (xm * ym);
temp_PSNR = 10 * log10( 1 / err);

% DRD: Distance Reciprocal Distortion Metric
blkSize=8; % even number
MaskSize=5; % odd number
u0_GT1 = false(xm + 2, ym + 2);
u0_GT1(2 : xm + 1, 2 : ym + 1) = u0_GT;
intim = cumsum(cumsum(u0_GT1, 1), 2);
NUBN = 0; blkSizeSQR = blkSize ^ 2;
for i= 2 : blkSize : (xm - blkSize + 1)
    for j = 2 : blkSize : (ym - blkSize + 1)
        blkSum=intim(i + blkSize - 1, j + blkSize - 1) - intim(i - 1, j + blkSize - 1) - intim(i + blkSize - 1, j - 1) + intim(i - 1, j - 1);
        if blkSum == 0 || blkSum == blkSizeSQR
        else
            NUBN = NUBN + 1;
        end
    end
end

wm = zeros(MaskSize, MaskSize);
ic = (MaskSize + 1) / 2; jc = ic; % center coordinate
for i = 1 : MaskSize
    for j = 1 : MaskSize
        wm(i, j) = 1 / (sqrt((i - ic) .^ 2 + (j - jc) .^ 2));
    end
end
wm(ic, jc) = 0;
wnm = wm ./ (sum(wm(:))); % Normalized weight matrix

u0_GT_Resized = zeros(xm + ic + 1, ym + jc + 1);
u0_GT_Resized(ic : xm + ic - 1, jc : ym + jc - 1) = u0_GT;
u_Resized = zeros(xm + ic + 1, ym + jc + 1);
u_Resized(ic : xm + ic - 1, jc : ym + jc - 1) = u;
temp_fp_Resized = [u_Resized == 0] & [u0_GT_Resized ~= 0];
temp_fn_Resized = [u_Resized ~= 0] & [u0_GT_Resized == 0];
Diff = temp_fp_Resized | temp_fn_Resized;
[xm2 ym2] = size(Diff);
SumDRDk = 0;
for i = ic : xm2 - ic + 1
    for j = jc : ym2 - jc + 1
        if Diff(i, j) == 1
            Local_Diff = my_xor_infile(u0_GT_Resized(i - ic + 1 : i + ic -1 , j - ic + 1 : j + ic - 1), u_Resized(i, j));
            DRDk = sum(sum(Local_Diff.* wnm));
            SumDRDk = SumDRDk + DRDk;
        end
    end
end
temp_DRD = SumDRDk / NUBN;

% MPM: Misclassification penalty metric
Contour = bwmorph(~u0_GT, 'remove');
Dist = bwdist(Contour, 'Chessboard');
D = sum(Dist(:));
MP_FN = sum(Dist(temp_fn)) / D;
MP_FP = sum(Dist(temp_fp)) / D;
temp_MPM = (MP_FN + MP_FP) / 2;

% output
%
temp_obj_eval.Precision = temp_p;
temp_obj_eval.Recall = temp_r;
temp_obj_eval.Fmeasure = temp_f;
temp_obj_eval.P_Precision = temp_pseudo_p;
temp_obj_eval.P_Recall = temp_pseudo_r;
temp_obj_eval.P_Fmeasure = temp_pseudo_f;
temp_obj_eval.Sensitivity = temp_sens;
temp_obj_eval.Specificity = temp_spec;
temp_obj_eval.BCR = temp_BCR;
temp_obj_eval.AUC = temp_AUC;
temp_obj_eval.BER = temp_BER;
temp_obj_eval.SFmeasure = temp_s_f_measure;
temp_obj_eval.Accuracy = temp_accu;
temp_obj_eval.GAccuracy = temp_g_accu;
temp_obj_eval.NRM = temp_NRM;
temp_obj_eval.PSNR = temp_PSNR;
temp_obj_eval.DRD = temp_DRD;
temp_obj_eval.MPM = temp_MPM;

end

function temp_xor_infile = my_xor_infile(u_infile, u0_GT_infile)
% Reza

temp_fp_infile = [u_infile == 0] & [u0_GT_infile ~= 0];
temp_fn_infile = [u_infile ~= 0] & [u0_GT_infile == 0];
temp_xor_infile = temp_fp_infile | temp_fn_infile;

end


function [V,E,I] = spline_to_poly(P,C,tol)
  % SPLINE_TO_POLY Evaluate a cubic Bezier spline as a polyline where each
  % segment corresponds to a locally flat segment of the curve up to given
  % tolerance
  %
  % [V,E] = spline_to_poly(P,C,tol)
  %
  % Inputs:
  %   P  #P by dim list of control point locations
  %   C  #C by 4 list of indices into P of cubic Bezier curves
  %   tol  tolerance
  % Outputs:
  %   V  #V by dim list of vertex locations
  %   E  #E by dim list of edge indices into V
  %   I  #E list of indices into 1:#C
  %

  % use transpose for amortized concatenation...
  V = P';
  E = [];
  I = [];
  % consider each cubic
  for c = 1:size(C,1)
    Pc = cubic_flat_eval(P(C(c,:),:),tol);
    J = [C(c,1) size(V,2)+(1:size(Pc,1)-2) C(c,4)];
    Ec = [J(1:end-1);J(2:end)]';
    % use transpose for amortized concatenation...
    E = [E,                    Ec'];
    V = [V,         Pc(2:end-1,:)'];
    I = [I;repmat(c,size(Ec,1),1)];
  end
  V = V';
  E = E';
  [V,~,~,E] = remove_unreferenced(V,E);

end


function [hog2x2_arr, info] = dense_hog2x2(I, HOG2x2param)
% Adapted from LabelMeToolbox
% URL: http://labelme.csail.mit.edu/Release3.0/browserTools/php/matlab_toolbox.php

grid_spacing = HOG2x2param.grid_spacing;
patch_size = HOG2x2param.patch_size;
half_win = floor(patch_size/2);

I = double(I);
d = pixelwise_hog31(I,half_win);
xmax = size(d,1); ymax = size(d,2);

grid_x = (1:grid_spacing:xmax-half_win);
grid_y = (1:grid_spacing:ymax-half_win);

hog2x2_arr = zeros([length(grid_x),length(grid_y),124]);

hog2x2_arr(:,:, 1: 31) = d(grid_x         ,grid_y         ,:);
hog2x2_arr(:,:,32: 62) = d(grid_x+half_win,grid_y         ,:);
hog2x2_arr(:,:,63: 93) = d(grid_x         ,grid_y+half_win,:);
hog2x2_arr(:,:,94:124) = d(grid_x+half_win,grid_y+half_win,:);

[x y] = meshgrid(grid_x, grid_y);
info.y = x(:);
info.x = y(:);


% Fig. 5.2   Feedback Control of Dynamic Systems, 5e
%             Franklin, Powell, Emami
clf
hold off
n2=1;
d2=[1 1 0];
rlocus(n2,d2)
axis([-2 2 -1.5 1.5])
title('Fig.5.2 Root locus of 1/s(s+1)')
grid on
  z=0:.1:.9;
 wn=.5:.5:2;
 sgrid(z, wn)


function smallT = marg_table(bigT, bigdom, bigsz, onto, maximize)
% MARG_TABLE Marginalize a table
% smallT = marg_table(bigT, bigdom, bigsz, onto, maximize)

if nargin < 5, maximize = 0; end

smallT = myreshape(bigT, bigsz); % make sure it is a multi-dim array
sum_over = mysetdiff(bigdom, onto);
ndx = find_equiv_posns(sum_over, bigdom);
if maximize
  for i=1:length(ndx)
    smallT = max(smallT, [], ndx(i));
  end
else
  for i=1:length(ndx)
    smallT = sum(smallT, ndx(i));
  end
end

ns = zeros(1, max(bigdom));
%ns(bigdom) = mysize(bigT); % ignores trailing dimensions of size 1
ns(bigdom) = bigsz;

smallT = squeeze(smallT); % remove all dimensions of size 1
smallT = myreshape(smallT, ns(onto)); % put back relevant dims of size 1


function [x,g,xn,gg] = kmeanhar(d,k,l,e,x0)
%KMEANS Vector quantisation using K-harmonic means algorithm [X,G,XN,GG]=(D,K,L,E,X0)
%
%  Inputs:
%
%    D(N,P)  contains N data vectors of dimension P
%    K       is number of centres required
%    L       integer portion is max loop count, fractional portion
%            gives stopping threshold as fractional reduction in performance criterion
%    E       is exponent in the cost function. Significantly faster if this is an even integer. [default 4]
%    X0(K,P) are the initial centres (optional)
%            Alternatively, X0 can be a character determining the initialization method:
%                'f'    Initialize with K randomly selected data points [default]
%                'p'    Initialize with centroids and variances of random partitions
%
%  Outputs:
%
%    X(K,P)  is output row vectors
%    G       is the final performance criterion value (normalized by N)
%    XN      nearest centre for each input point
%    GG(L+1) value of performance criterion before each iteration and at end
%
% The k-harmonic means algorithm selects K cluster centres to minimize
%                           sum_n(K/sum_k((d_n-x_k)^-e))
% where sum_n is over the N inputs points d_n and sum_k is over the K cluster centres x_k.
%
% It is often a good idea to scale the input data so that it has equal variance in each
% dimension before calling KMEANHAR so that approximately equal weight is given
% to each dimension in the distance calculation.

%  [1] Bin Zhang, "Generalized K-Harmonic Means - Boosting in Unsupervised Learning",
%      Hewlett-Packartd Labs, Technical Report HPL-2000-137, 2000 [Zhang2000]
%      http://www.hpl.hp.com/techreports/2000/HPL-2000-137.pdf

%  Bugs:
%      (1) Could use nested blocking to allow very large data arrays
%      (2) Could then allow incremental calling with partial data arrays (but messy)

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: kmeanhar.m 713 2011-10-16 14:45:43Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% sort out the input arguments

if nargin<5
    x0='f';
    if nargin<4
        e=[];
        if nargin<3
            l=[];
        end
    end
end
if isempty(e)
    e=4;  % default value
end
if isempty(l)
    l=50+1e-3; % default value
end
sd=5;       % number of times we must be below threshold

% split into chunks if there are lots of data points

memsize=voicebox('memsize');
[n,p] = size(d);
nb=min(n,max(1,floor(memsize/(8*p*k))));    % block size for testing data points
nl=ceil(n/nb);                  % number of blocks

% initialize if X0 argument is not supplied

if ischar(x0)
    if k<n
        if any(x0=='p')                  % Initialize using a random partition
            ix=ceil(rand(1,n)*k);       % allocate to random clusters
            ix(rnsubset(k,n))=1:k;      % but force at least one point per cluster
            x=zeros(k,p);
            for i=1:k
                x(i,:)=mean(d(ix==i,:),1);
            end
        else                                % Forgy initialization: choose k random points [default]
            x=d(rnsubset(k,n),:);         % sample k centres without replacement
        end
    else
        x=d(mod((1:k)-1,n)+1,:);    % just include all points several times
    end
else
    x=x0;
end
eh=e/2;
th=l-floor(l);
l=floor(l)+(nargout>1);   % extra loop needed to calculate final performance value
if l<=0
    l=100;      % max number of iterations ever
end
if th==0
    th=-1;      % prevent any stopping if l has no fractional part
end
gg=zeros(l+1,1);
im=repmat(1:k,1,nb); im=im(:);

% index arrays for replication

wk=ones(k,1);
wp=ones(1,p);
% wn=ones(1,n);
%
% % Main calculation loop
%
% We have the following relationships to [1] where i and k index
% the data values and cluster centres respectively:
%
%   This program     [Zhang2000]                            Equation
%
%     d(i,:)            x_i                                 input data
%     x(k,:)            m_k                                 cluster centres
%     py(k,i)           (d_ik)^2
%     dm(i)'            d_i,min^2
%     pr(k,i)           (d_i,min/d_ik)^2
%     pe(k,i)           (d_i,min/d_ik)^p                    (7.6)
%     qik(k,i)          q_ik                                (7.2)
%     qk(k)             q_k                                 (7.3)
%     qik(k,i)./qk(k)   p_ik                                (7.4)
%     se(i)'            d_i,min^p * sumk(d_ik^-p)
%     xf(i)'            d_i,min^-2 / sumk(d_ik^-p)
%     xg(i)'            d_i,min^-(p+2) / sumk(d_ik^-p)^2

ss=sd+1;        % one extra loop at the start
g=0;                % dummy initial value of g
xn=zeros(n,1);
for j=1:l

    g1=g;                           % save old performance
    x1=x;                           % save old centres
    % first do partial chunk

    jx=n-(nl-1)*nb;
    ii=1:jx;
    kx=repmat(ii,k,1);
    km=repmat(1:k,1,jx);
    py=reshape(sum((d(kx(:),:)-x(km(:),:)).^2,2),k,jx);
    [dm,xn(ii)]=min(py,[],1);                 % min value in each column gives nearest centre
    dmk=dm(wk,:);                   % expand into a matrix
    dq=py>dmk;                      % update only these values
    pr=ones(k,jx);                   % leaving others at 1
    pr(dq)=dmk(dq)./py(dq);            % ratio of min(py)./py
    pe=pr.^eh;
    se=sum(pe,1);
    xf=dm.^(eh-1)./se;
    g=xf*dm.';                     % performance criterion (divided by k)
    xg=xf./se;
    qik=xg(wk,:).*pe.*pr;           % qik(k,i) is equal to q_ik in [Zhang2000]
    qk=sum(qik,2);
    xs=qik*d(ii,:);
    ix=jx+1;
    for il=2:nl
        jx=jx+nb;        % increment upper limit
        ii=ix:jx;
        kx=ii(wk,:);
        py=reshape(sum((d(kx(:),:)-x(im,:)).^2,2),k,nb);
        [dm,xn(ii)]=min(py,[],1);                 % min value in each column gives nearest centre
        dmk=dm(wk,:);                   % expand into a matrix
        dq=py>dmk;                      % update only these values
        pr=ones(k,nb);                   % leaving others at 1
        pr(dq)=dmk(dq)./py(dq);            % ratio of min(py)./py
        pe=pr.^eh;
        se=sum(pe,1);
        xf=dm.^(eh-1)./se;
        g=g+xf*dm.';                     % performance criterion (divided by k)
        xg=xf./se;
        qik=xg(wk,:).*pe.*pr;           % qik(k,i) is equal to q_ik in [Zhang2000]
        qk=qk+sum(qik,2);
        xs=xs+qik*d(ii,:);
        ix=jx+1;
    end
    gg(j)=g;
    x=xs./qk(:,wp);
    if g1-g<=th*g1
        ss=ss-1;
        if ~ss break; end  %  stop if improvement < threshold for sd consecutive iterations
    else
        ss=sd;
    end
end
gg=gg(1:j)*k/n;                       % scale and trim the performance criterion vector
g=g(end);
% gg' % *** DEBUIG ***
if nargout>1
    x=x1;                               % go back to the previous x values if G and/or XN value is output
end


classdef SMD8 < PROBLEM
% <multi> <real> <bilevel>
% Bilevel optimization problems proposed by Sinha, Malo, and Deb
% maxFElower --- 500 --- Maximum number of lower level function evaluations for each solution

%------------------------------- Reference --------------------------------
% A. Sinha, P. Malo, K. Deb, Test problem construction for single-objective
% bilevel optimization, Evolutionary Computation, 2014, 22(3): 439-477.
%------------------------------- 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(SetAccess = private)
        maxFElower; % Maximum number of lower level function evaluations for each solution
        DU;         % Number of decision variables of the upper level
        DL;         % Number of decision variables of the lower level
        C;       	% Number of upper constraints
        p;          % The length of xu1
        q;          % The length of xl1
        r;          % The length of xu2 and xl2
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.maxFElower = obj.ParameterSet(500);
            obj.M = 2;
            obj.C = 0;
            if isempty(obj.D); obj.D = 5; end
            obj.DU = floor(obj.D/2);
            obj.DL = obj.D - obj.DU;
            obj.r  = floor(obj.DU/2);
            obj.p  = obj.DU - obj.r;
            obj.q  = obj.DL - obj.r;
            obj.lower    = -5*ones(1,obj.D);
            obj.upper    = 10*ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate upper level and lower level objective values
        function PopObj = CalObj(obj,PopDec)
            xu1   = PopDec(:,1:obj.p);
            xu2   = PopDec(:,obj.p+1:obj.p+obj.r);
            xl1   = PopDec(:,obj.p+obj.r+1:obj.p+obj.r+obj.q);
            xl2   = PopDec(:,obj.p+obj.r+obj.q+1:end);
            term2 = sum((xl1(:,2:end)-xl1(:,1:end-1).^2).^2+(xl1(:,1:end-1)-1).^2,2);
            % Upper level function value
            PopObj(:,1) = 20 + exp(1) - 20*exp(-0.2*sqrt(1/obj.p*sum(xu1.^2,2))) - exp(1/obj.p*sum(cos(2*pi*xu1),2)) - term2 + sum(xu2.^2,2) - sum((xu2-xl2.^3).^2,2);
            % Lower level function value
            PopObj(:,2) = sum(abs(xu1),2) + term2 + sum((xu2-xl2.^3).^2,2);
        end
        %% Calculate lower level objective values
        function llPopulation = EvaluationLower(obj,varargin)
            PopDec            = obj.CalDec(varargin{1});
            PopObj            = obj.CalObj(PopDec);
            PopObj(:,1)       = nan;
            PopCon            = obj.CalCon(PopDec);
            PopCon(:,1:obj.C) = nan;
            llPopulation      = SOLUTION(PopDec,PopObj,PopCon,varargin{2:end});
        end
    end
end