function uh = CurlCurl2dNd0(node,elem,bdFlag,pde)
%% CURLCURL2DND0 curlcurl equation: lowest order edge element on triangular mesh
% rot(\mu^{-1} curl u) + \kappa u = f in \Omega
% u\times n = 0 on \Gamma_D
% \mu^{-1} curl u = g_N on \Gamma_N

% Solve u_h in Nd0 \cap {u \times n = 0 on \Gamma_D} = V_h
% (\mu^{-1} curl u_h, curl v_h) + (\kappa u_h, v_h)
% = (f, v_h) + <v_h \cdot \tau, g_N>_{\Gamma_N} for any v_h \in V_h

% basis function associated with an edge |e|
% \phi_e = \frac{|e|}{2|K|} (\lamda_{i+1} n_{i-1} - \lambda_{i+1} n_{i-1})
% implented based on an older iFEM circa 2010.
%
% For 3D curl-curl problems
% See also Maxwell, cubeMaxwell
%
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.
%%
NT = size(elem,1);
T = auxstructure(elem);
elem2edge = T.elem2edge;
edge = T.edge;
elem2edgeSign = ones(NT,3);
totalEdge = uint32([elem(:,[2,3]); elem(:,[3,1]); elem(:,[1,2])]);
idx = (totalEdge(:,1)>totalEdge(:,2));
elem2edgeSign(idx) = -1;

if ~isfield(pde,'mu'), pde.mu = 1; end
if ~isfield(pde,'kappa'), pde.kappa = 1; end
if ~isfield(pde,'g_D'), pde.g_D = []; end
if ~isfield(pde,'g_N'), pde.g_N = []; end

NE = size(edge,1);
uh = zeros(NE,1);
g_D = pde.g_D;
g_N = pde.g_N;
f_x = pde.f_x;
f_y = pde.f_y;
mu = pde.mu;
kappa = pde.kappa;

%% Boundary
idxD = (bdFlag(:) == 1);     % all Dirichlet edges in bdFlag
isFixedEdge = false(NE,1);
isFixedEdge(elem2edge(idxD)) = true;  % index of fixed boundary edges
freeEdge = ~isFixedEdge;

idxN = (bdFlag(:) == 2);     % all Neumann edges in bdFlag
Neumann = edge(elem2edge(idxN),:);

%% geometric quantities
%edge vector which follows elem2edge counterclockwisely
ve(:,:,1) = node(elem(:,3),:)-node(elem(:,2),:);
ve(:,:,2) = node(elem(:,1),:)-node(elem(:,3),:);
ve(:,:,3) = node(elem(:,2),:)-node(elem(:,1),:);
area = 0.5*abs(-ve(:,1,3).*ve(:,2,2)+ve(:,2,3).*ve(:,1,2));

%length_ve(:,:,i) is the length of i^th edge
length_ve = sqrt(sum(ve.^2,2));
length_ve = reshape(length_ve,NT,3);

%outer normal vector of each edge which follows the index of elem2edge
ne = [ve(:,2,:), -ve(:,1,:)];

%%
%M is the mass matrix M_{ij} = (\kappa \phi_i, \phi_j)
%B is the stiffness matrix B_{ij} = (\mu^{-1} curl \phi_i, curl \phi_j)
c = [3 1 2 3 1];
%cyclic group of {1,2,3}: c(i)=i-1, c(i+2)=i+1

M = sparse(NE,NE);
B = sparse(NE,NE);
muinv = 1/mu;

for i = 1:3
    for j = 1:3
        sij = elem2edgeSign(:,i).*elem2edgeSign(:,j);

        if i==j
            nini = length_ve(:,i).^2;
            njnk = dot(ne(:,:,c(i)),ne(:,:,c(i+2)),2);
            Mij = kappa.*length_ve(:,i).*(nini - 3*njnk)./(24*area);
        else
            ninj = dot(ne(:,:,i),ne(:,:,j),2);
            ni2 = length_ve(:,i).^2;
            nj2 = length_ve(:,j).^2;
            Mij = -sij.*kappa.*length_ve(:,i).*length_ve(:,j).*(ni2 + nj2 + 3*ninj)./(24*area);
        end

        Bij = muinv.*sij.*length_ve(:,i).*length_ve(:,j)./area;

        M = M + sparse(elem2edge(:,i),elem2edge(:,j),Mij,NE,NE);
        B = B + sparse(elem2edge(:,i),elem2edge(:,j),Bij,NE,NE);
    end
end

A = M+B;

%% rhs (old hard-coded implementation)
Fx = quadelem2node(node,elem,f_x);
Fy = quadelem2node(node,elem,f_y);

bt1x = -0.5*elem2edgeSign(:,1).*length_ve(:,1)...
    .*(Fx(:,2).*ne(:,1,3) - Fx(:,3).*ne(:,1,2))./area;
bt2x = -0.5*elem2edgeSign(:,2).*length_ve(:,2)...
    .*(Fx(:,3).*ne(:,1,1) - Fx(:,1).*ne(:,1,3))./area;
bt3x = -0.5*elem2edgeSign(:,3).*length_ve(:,3)...
    .*(Fx(:,1).*ne(:,1,2) - Fx(:,2).*ne(:,1,1))./area;

bt1y = -0.5*elem2edgeSign(:,1).*length_ve(:,1)...
    .*(Fy(:,2).*ne(:,2,3) - Fy(:,3).*ne(:,2,2))./area;
bt2y = -0.5*elem2edgeSign(:,2).*length_ve(:,2)...
    .*(Fy(:,3).*ne(:,2,1) - Fy(:,1).*ne(:,2,3))./area;
bt3y = -0.5*elem2edgeSign(:,3).*length_ve(:,3)...
    .*(Fy(:,1).*ne(:,2,2) - Fy(:,2).*ne(:,2,1))./area;

bt1 = bt1x + bt1y;
bt2 = bt2x + bt2y;
bt3 = bt3x + bt3y;

F = accumarray(elem2edge(:),[bt1;bt2;bt3],[NE 1]);

%% Dirichlet Boundary
if ~isempty(pde.g_D)
    fixedEdge = edge(isFixedEdge,:);
    signFixedEdge = elem2edgeSign(idxD);
    Nve = node(fixedEdge(:,1),:) - node(fixedEdge(:,2),:);
    NveLength = sqrt(sum(Nve.^2,2));
    GD = quadedge(node,fixedEdge,g_D).*signFixedEdge./NveLength;
    uh(isFixedEdge) = GD;
end

%% Neumann Boundary
if (~isempty(Neumann) && ~isempty(pde.g_N))
    signNeumannEdge = elem2edgeSign(idxN);
    GN = quadedge(node,Neumann,g_N).*signNeumannEdge;
    F = F + accumarray(idxN(:),GN(:),[NE,1]);
end

%direct solver
uh(freeEdge) = A(freeEdge,freeEdge)\F(freeEdge);


function solution = CS_GWO(UAV, SearchAgents, Max_iter)
%CS_GWO 灰狼-布谷鸟优化算法
%Gray Wolf Cuckoo Optimization

% 超参数
pa = 0.25;   % 布谷鸟搜索参数

% 算法初始化
[WolfPops, Tracks] = PopsInit(UAV, SearchAgents, false);   % 随机生成 初始狼群 和 轨迹们
dim = WolfPops.PosDim;                                                         % 状态变量维度

% 初始化解
Alpha_pos = zeros(1, dim);   % α解
Alpha_score = inf;                  % α解适应度
Alpha_no = 1;                         % α解编号

Beta_pos = zeros(1, dim);      % β解
Beta_score = inf;                     % β解适应度
Beta_no = 1;                             % β解编号

Delta_pos = zeros(1, dim);     % δ解
Delta_score = inf;                    % δ解适应度
Delta_no = 1;                           % δ解编号

Fitness_list = zeros(1, Max_iter);

% 迭代求解
tic
fprintf('>>CS_GWO 优化中    00.00%%')
for iter = 1 : Max_iter

    % ①  计算每只狼的适应度，更改其种群等级
    ProbPoints = cell(SearchAgents, 1);
    for i = 1 : SearchAgents
        % 计算目标函数
        [fitness, ~, Data] = ObjFun(Tracks{i}, UAV);    % 一个智能体的目标函数
        ProbPoints{i} = Data.ProbPoint;                       % 所有智能体不符合条件的状态

        % 更新 Alpha、Beta 和 Delta 解
        if fitness <= Alpha_score  % 适应能力最强（因为性能指标越小越好，因此为小于号）
            Alpha_score = fitness;
            Alpha_pos = WolfPops.Pos(i, :);
            Alpha_no = i;
        end
        if fitness > Alpha_score && fitness <= Beta_score
            Beta_score = fitness;
            Beta_pos = WolfPops.Pos(i, :);
            Beta_no = i;
        end
        if fitness > Alpha_score && fitness > Beta_score && fitness <= Delta_score
            Delta_score = fitness;
            Delta_pos = WolfPops.Pos(i, :);
            Delta_no = i;
        end
    end

    % ②  更新参数a
    a = 2 - iter * 2 / Max_iter;                  % 线性递减
    %a = 2 * cos((iter / Max_iter) * pi/2);   % 非线性递减

    % ③  更新位置（朝着前三只狼位置前进）
    for i = 1 : SearchAgents
        for j = 1 : dim

            r1 = rand();
            r2 = rand();
            A1 = 2*a*r1 - a;
            C1 = 2*r2;
            D_alpha = abs(C1*Alpha_pos(j) - WolfPops.Pos(i, j));
            X1(i, j) = Alpha_pos(j) - A1*D_alpha;

            r1 = rand();
            r2 = rand();
            A2 = 2*a*r1 - a;
            C2 = 2*r2;
            D_beta = abs(C2*Beta_pos(j) - WolfPops.Pos(i, j));
            X2(i, j) = Beta_pos(j) - A2*D_beta;

            r1 = rand();
            r2 = rand();
            A3 = 2*a*r1 - a;
            C3 = 2*r2;
            D_delta = abs(C3*Delta_pos(j) - WolfPops.Pos(i, j));
            X3(i, j) = Delta_pos(j) - A3*D_delta;

            %更新
            %WolfPops.Pos(i, j) = (X1(i, j) + X2(i, j) + X3(i, j)) / 3;

        end

    end

    % ④  Cuckoo 搜索
    fitness = nan(SearchAgents, 1);
    for i = 1 : SearchAgents
        [fitness(i), ~, ~] = ObjFun(Tracks{i}, UAV);
    end
    [~, index] = min(fitness);
    best = WolfPops.Pos(index, :);
    X1 = get_cuckoos(X1, best, WolfPops.lb, WolfPops.ub);
    X2 = get_cuckoos(X2, best, WolfPops.lb, WolfPops.ub);
    X3 = get_cuckoos(X3, best, WolfPops.lb, WolfPops.ub);
    X1 = empty_nests(X1, WolfPops.lb, WolfPops.ub, pa);
    X2 = empty_nests(X2, WolfPops.lb, WolfPops.ub, pa);
    X3 = empty_nests(X3, WolfPops.lb, WolfPops.ub, pa);
    WolfPops.Pos = (X1 + X2 + X3) / 3;

    % ⑤  调整不符合要求的状态变量
    [WolfPops, Tracks] = BoundAdjust(WolfPops, ProbPoints, UAV);

    % ⑥  存储适应度
    Fitness_list(iter) = Alpha_score;

    if iter/Max_iter*100 < 10
        fprintf('\b\b\b\b\b%.2f%%', iter/Max_iter*100)
    else
        fprintf('\b\b\b\b\b\b%.2f%%', iter/Max_iter*100)
    end
end
fprintf('\n\n>>计算完成！\n\n')
toc

% 输出值
solution.method = 'GWO';                % 算法
solution.WolfPops = WolfPops;       % 所有解种群信息
solution.Tracks = Tracks;                  % 所有解航迹信息
solution.Fitness_list = Fitness_list;   % α解适应度曲线
solution.Alpha_Data = Data;            % α解的威胁信息
solution.Alpha_no = Alpha_no;        % α解的位置
solution.Beta_no = Beta_no;             % β解的位置
solution.Delta_no = Delta_no;          % δ解的位置

end

%%%%% 布谷鸟搜索 %%%%%
function nest = get_cuckoos(nest, best, Lb, Ub)
    n = size(nest, 1);
    beta = 3/2;
    sigma = (gamma(1 + beta) * sin(pi*beta/2)/(gamma((1 + beta)/2)*beta*2^((beta - 1)/2)))^(1/beta);
    for j = 1:n
        s = nest(j, :);
        u = randn(size(s))*sigma;
        v = randn(size(s));
        step = u./abs(v).^(1/beta);
        stepsize = 0.01*step.*(s - best);
        s = s + stepsize.*randn(size(s));
        nest(j, :) = BoundClamp(s, Lb, Ub);
    end
end
function new_nest = empty_nests(nest, Lb, Ub, pa)
    n = size(nest, 1);
    K = rand(size(nest)) > pa;
    stepsize = rand*(nest(randperm(n), :) - nest(randperm(n), :));
    new_nest = nest + stepsize.*K;
    for j = 1:size(new_nest, 1)
        s = new_nest(j, :);
        new_nest(j, :) = BoundClamp(s, Lb, Ub);
    end
end
function x = BoundClamp(x, lb, ub)
    Flag4ub = x > ub;
    Flag4lb = x < lb;
    x = x .* ( ~(Flag4ub + Flag4lb) ) + ub .* Flag4ub + lb .* Flag4lb;
end


clear, clc;

% This is an example for running the function tree_LogisticR
%
%  Problem:
%
%  min  f(x,c) = - sum_i weight_i * log (p_i) + z * sum_j w_j ||x_{G_j}||
%
%  a_i denotes a training sample,
%      and a_i' corresponds to the i-th row of the data matrix A
%
%  y_i (either 1 or -1) is the response
%
%  p_i= 1/ (1+ exp(-y_i (x' * a_i + c) ) ) denotes the probability
%
%  G_j's are nodes with tree structure
%
%  The tree structured group information is contained in
%  opts.ind, which is a 3 x nodes matrix, where nodes denotes the number of
%  nodes of the tree.
%
%  opts.ind(1,:) contains the starting index
%  opts.ind(2,:) contains the ending index
%  opts.ind(3,:) contains the corresponding weight (w_j)
%
%  Note:
%  1) If each element of x is a leaf node of the tree and the weight for
%  this leaf node are the same, we provide an alternative "efficient" input
%  for this kind of node, by creating a "super node" with
%  opts.ind(1,1)=-1; opts.ind(2,1)=-1; and opts.ind(3,1)=the common weight.
%
%  2) If the features are well ordered in that, the features of the left
%  tree is always less than those of the right tree, opts.ind(1,:) and
%  opts.ind(2,:) contain the "real" starting and ending indices. That is to
%  say, x( opts.ind(1,j):opts.ind(2,j) ) denotes x_{G_j}. In this case,
%  the entries in opts.ind(1:2,:) are within 1 and n.
%
%
%  If the features are not well ordered, please use the input opts.G for
%  specifying the index so that
%   x( opts.G ( opts.ind(1,j):opts.ind(2,j) ) ) denotes x_{G_j}.
%  In this case, the entries of opts.G are within 1 and n, and the entries of
%  opts.ind(1:2,:) are within 1 and length(opts.G).
%
%% Related papers
%
% [1] Jun Liu and Jieping Ye, Moreau-Yosida Regularization for
%     Grouped Tree Structure Learning, NIPS 2010
%
%%

cd ..
cd ..

root=cd;
addpath(genpath([root '/SLEP']));
                     % add the functions in the folder SLEP to the path

% change to the original folder
cd Examples/tree;

m=50;  n=100;       % The data matrix is of size m x n

% ---------------------- generate random data ----------------------
%randn('state',(randNum-1)*3+1);
A=randn(m,n);        % the data matrix

y=[-ones(25,1); ones(25,1)];      % the response

%% In this example, the tree is set as:
%
% root, 1:100, with weight 0
% its children nodes, 1:50, and 51:100
%
% For 1:50, its children are 1:20, 21:40, and 41:50
%
% For 51:100, its children are 51:70, and 71:100
%
% These nodes in addition have each individual features (they contain) as
% children nodes.
%
%%

%% One efficient way
% We make use of the fact that the indices of the left nodes of the tree
% are smaller than the right nodes.

%----------------------- Set optional items -----------------------
opts=[];

% Starting point
opts.init=2;        % starting from a zero point

% Termination
opts.tFlag=5;       % run .maxIter iterations
opts.maxIter=100;   % maximum number of iterations

% regularization
opts.rFlag=1;       % use ratio

% Normalization
opts.nFlag=0;       % without normalization

% Group Property
opts.ind=[[-1, -1, 1]',... % leave nodes (each node contains one feature)
    [1, 20, sqrt(20)]', [21, 40, sqrt(20)]',... % the layer above the leaf
    [41, 50, sqrt(10)]', [51, 70, sqrt(20)]', [71,100, sqrt(30)]',...
    [1, 50, sqrt(50)]', [51, 100, sqrt(50)]']; % the higher layer

%----------------------- Run the code mc_cgLassoLeast -----------------------
z=0.1;
tic;
[x, c, funVal, ValueL]= tree_LogisticR(A, y, z, opts);
toc;

%% An alternative way
% We make use of the fact that the indices of the left nodes of the tree
% are smaller than the right nodes.
%%

%----------------------- Set optional items -----------------------
opts=[];

% Starting point
opts.init=2;        % starting from a zero point

% Termination
opts.tFlag=5;       % run .maxIter iterations
opts.maxIter=100;   % maximum number of iterations

% regularization
opts.rFlag=1;       % use ratio

% Normalization
opts.nFlag=0;       % without normalization

% Group Property
opts.ind=[[-1, -1, 1]',... % leave nodes (each node contains one feature)
    [1, 20, sqrt(20)]', [21, 40, sqrt(20)]',... % the layer above the leaf
    [41, 50, sqrt(10)]', [51, 70, sqrt(20)]', [71,100, sqrt(30)]',...
    [101, 150, sqrt(50)]', [151, 200, sqrt(50)]']; % the higher layer
opts.G=[1:100, 1:100];

%----------------------- Run the code mc_cgLassoLeast -----------------------
z=0.1;
tic;
[x2, c2, funVal2, Value2L]= tree_LogisticR(A, y, z, opts);
toc;


%% simple example of use of renorm_sibling_3d_same_scale
clear; close all;
x = uiuc_sample;
%% compute roto-translation scattering of an image
options.Q = 1;
options.J = 5;
Wop = wavelet_factory_3d_pyramid(options, options, options);
Sx = scat(x, Wop);

%% L1 renormalization
op = @(x)(sum(x, 3));
Sx_renorm = renorm_sibling_3d_same_scale(Sx, op);

%% L1 + smoothing renormalization
op = renorm_factory_L1_smoothing(2);
[Sx_renorm, siblings] = renorm_sibling_3d_same_scale(Sx, op);

%%
image_scat(Sx, 0, 0);
%%
image_scat(Sx_renorm, 0, 0);
%%
close all;
imagesc(min(image_scat_layer(Sx_renorm{3},0,0),0.2));

%%
image_scat(Sx_renorm, 1, 1);
%%
image_scat(Sx, 1, 1);


function v=cart2curv(vcart,typeout,bcontr,bcovar)
% v=cart2curv(vcart,typeout,bcontr,bcovar) converts
% cartesian coordinate components to curvilinear
% components.
% vcart   - cartesian vector components
% typeout - 1 for contravariant output or
%           2 for covariant output
% bcontr  - contravariant base vector components
% bcovar  - covariant base vector components
% v       - vector components in either contravariant
%           or covariant form

if typeout==1, v=bcontr.'*vcart(:);
else v=bcovar.'*vcart(:); end
v=simple(v);


function [x, infos] = acc_online_mu_nmf(V, rank, in_options)
% Accelerated online non-negative matrix factorization (ONMF) algorithm.
%
% Inputs:
%       matrix      V
%       rank        rank
%       options     options
% Output:
%       w           solution of w
%       infos       information
%
% References:
%       S. S. Bucak, B. Gunsel,
%       "Incremental Subspace Learning via Non-negative Matrix Factorization,"
%       Pattern Recognition, 2009.
%
%
% This file is part of NMFLibrary.
%
% Created by H.Kasai and H.Sakai on Feb. 12, 2017
%
% Change log:
%
%       Feb. 12, 2017 (Hiroyuki Kasai): Fixed algorithm.
%
%       May. 20, 2019 (Hiroyuki Kasai): Added initialization module.
%
%       Jul. 12, 2022 (Hiroyuki Kasai): Modified code structures.
%

    % set dimensions and samples
    [m, n] = size(V);

    % set local options
    local_options = [];
    local_options.rep_mode = 'fix';
    local_options.w_repeat = 1;
    local_options.h_repeat = 1;
    local_options.alpha    = 2;
    local_options.delta    = 0.1;

    % check input options
    if ~exist('in_options', 'var') || isempty(in_options)
        in_options = struct();
    end
    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    % initialize factors
    init_options = options;
    [init_factors, ~] = generate_init_factors(V, rank, init_options);
    Wt = init_factors.W;
    H = init_factors.H;

    % initialize
    method_name = 'ACC-Online-NMF';
    epoch = 0;
    grad_calc_count = 0;

    if options.verbose > 0
        fprintf('# %s: started ...\n', method_name);
    end

    %At = zeros(m, rank);
    %Bt = zeros(rank, rank);

    % initialize for this algorithm
    if strcmp(options.rep_mode, 'adaptive')
        K = m*n;
        rhoh = 1+(K+m*rank)/(n*(rank+1));
    end

    % store initial info
    clear infos;
    [infos, f_val, optgap] = store_nmf_info(V, Wt, H, [], options, [], epoch, grad_calc_count, 0);

    if options.verbose > 1
        fprintf('%s: Epoch = 0000, cost = %.16e, optgap = %.4e\n', method_name, f_val, optgap);
    end

    % set start time
    start_time = tic();

    % main outer loop
    while true

        % check stop condition
        [stop_flag, reason, max_reached_flag] = check_stop_condition(epoch, infos, options);
        if stop_flag
            display_stop_reason(epoch, infos, options, method_name, reason, max_reached_flag);
            break;
        end

        % Reset sufficient statistic
        At = zeros(m, rank);
        Bt = zeros(rank, rank);

        % main inner loop
        for t = 1 : options.batch_size : n - 1

            % retrieve vt and ht
            vt = V(:, t:t+options.batch_size-1);
            ht = H(:, t:t+options.batch_size-1);

%             % uddate ht
%             Wtv = Wt.' * vt;
%             WtW = Wt.' * Wt;
%             for iii=1:h_repeat
%                 ht = ht .* (Wtv) ./ (WtW * ht);
%                 ht = ht + (ht<eps) .* eps;
%             end

            Wtv = Wt.' * vt;
            WtW = Wt.' * Wt;
            if strcmp(options.rep_mode, 'adaptive')
                gamma = 1;
                eps0 = 1;
                j = 1;
                rhoh_alpha = rhoh * options.alpha;

                %while j <= floor(1+rhoh*alpha) &&  gamma >= delta*eps0
                while j <= rhoh_alpha && gamma >= options.delta * eps0
                    ht0 = ht;
                    ht = ht .* (Wtv) ./ (WtW * ht);
                    ht = ht + (ht<eps) .* eps;
                    if j == 1
                        eps0 = norm(ht0 - ht);
                    end
                    gamma = norm(ht0 - ht);
                    j = j+1;
                end
            else
                for iii = 1 : options.h_repeat
                    ht = ht .* (Wtv) ./ (WtW * ht);
                    ht = ht + (ht<eps) .* eps;
                end
            end

            % update sufficient statistics
            At = At + vt *  ht';
            Bt = Bt + ht *  ht';

            % update W
            for iii = 1 : options.w_repeat
                Wt = Wt .* At ./ (Wt * Bt);
                Wt = Wt + (Wt<eps) .* eps;
            end

            % store new h
            H(:, t:t+options.batch_size-1) = ht;

            grad_calc_count = grad_calc_count + m * options.batch_size;
        end

        % measure elapsed time
        elapsed_time = toc(start_time);

        % update epoch
        epoch = epoch + 1;

        % store info
        infos = store_nmf_info(V, Wt, H, [], options, infos, epoch, grad_calc_count, elapsed_time);

        % display info
        display_info(method_name, epoch, infos, options);

    end

    x.W = Wt;
    x.H = H;
    x.R = [];

end


clear all
close all
path(path,'..\..\FUZZCLUST')
colors={'r.' 'gx' 'b+' 'ys' 'md' 'cv' 'k.' 'r*' 'g*' 'b*' 'y*' 'm*' 'c*' 'k*' };

%the data
data.X=nDexample(5,250,2,1);
%normalization
data=clust_normalize(data,'range');

%parameters
param.c=3;
param.m=2;
param.e=1e-3;
param.ro=ones(1,param.c);
param.val=1;

%Gustafson Kessel-clustering
result = GKclust(data,param);

%validation
result = validity(result,data,param);
plot(data.X(:,1),data.X(:,2),'b.',result.cluster.v(:,1),result.cluster.v(:,2),'ro');
hold on
plot(result.cluster.v(:,1),result.cluster.v(:,2),'ro');

%evaluation
new.X=data.X;
eval = clusteval(new,result,param);
result.validity


function nn = nnff(nn, x, y)
%NNFF performs a feedforward pass
% nn = nnff(nn, x, y) returns an neural network structure with updated
% layer activations, error and loss (nn.a, nn.e and nn.L)

    n = nn.n;
    m = size(x, 1);

    nn.a{1} = x;

    %feedforward pass
    for i = 2 : n-1
        nn.a{i} = sigm(repmat(nn.b{i - 1}', m, 1) + nn.a{i - 1} * nn.W{i - 1}');
        if(nn.dropoutFraction > 0)
            if(nn.testing)
                nn.a{i} = nn.a{i}.*(1 - nn.dropoutFraction);
            else
                nn.a{i} = nn.a{i}.*(rand(size(nn.a{i}))>nn.dropoutFraction);
            end
        end
        %calculate running exponential activations for use with sparsity
        if(nn.nonSparsityPenalty>0)
            nn.p{i} = 0.99 * nn.p{i} + 0.01 * mean(nn.a{i}, 1);
        end
    end
    switch nn.output
        case 'sigm'
            nn.a{n} = sigm(repmat(nn.b{n - 1}', m, 1) + nn.a{n - 1} * nn.W{n - 1}');
        case 'linear'
            nn.a{n} = repmat(nn.b{n - 1}', m, 1) + nn.a{n - 1} * nn.W{n - 1}';
        case 'softmax'
            nn.a{n} = repmat(nn.b{n - 1}', m, 1) + nn.a{n - 1} * nn.W{n - 1}';
            nn.a{n} = exp(bsxfun(@minus, nn.a{n}, max(nn.a{n},[],2)));
            nn.a{n} = bsxfun(@rdivide, nn.a{n}, sum(nn.a{n}, 2));
    end

    %error and loss
    nn.e = y - nn.a{n};
    switch nn.output
        case {'sigm','linear'}
            nn.L = 1/2 * sum(sum(nn.e .^ 2)) / m;
        case 'softmax'
            nn.L = -sum(sum(y .* log(nn.a{n}))) / m;
    end
end


%This is a more real-life example

load DataTest; %load some simulation data for acoustic pressure coming from a focused device

h1=figure;
subplot(1,2,1);

 [X,Y,Z]=meshgrid(Rx,Ry,Rz);
 pf = abs(uf);

[Maxima,MaxPos,Minima,MinPos]=MinimaMaxima3D(pf,1,0,10,10); %We find the first 10 minima and maxima

ufa=20*log10(abs(uf)/max(abs(uf(:))));

is=isosurface(X,Y,Z,ufa,-6); %we display the focused pressure at -6dB
patch(is,'facecolor','red','edgecolor','none');
view(24,28);
lighting gouraud;
camlight;
daspect([1 1 1]);
title(sprintf('(%3.1f,%3.1f,%3.1f) mm',CoordinatesToEval(nc,1)*1000, CoordinatesToEval(nc,2)*1000,CoordinatesToEval(nc,3)*1000));
xlabel('x (mm)');
ylabel('y (mm)');
zlabel('z (mm)');
for nm=1:5
   %we display the location of the maxima, note that for my needs I
   %switched X<->Y
   posmax=[Rx(MaxPos(nm,2)),Ry(MaxPos(nm,1)),Rz(MaxPos(nm,3))];
   line([posmax(1)-1 posmax(1)+1],[posmax(2) posmax(2)],[posmax(3) posmax(3)],'linewidth',1,'color','b');
   line([posmax(1) posmax(1)],[posmax(2)-1 posmax(2)+1],[posmax(3) posmax(3)],'linewidth',1,'color','b');
   line([posmax(1) posmax(1)],[posmax(2) posmax(2)],[posmax(3)-1 posmax(3)+1],'linewidth',1,'color','b');
   TT=['\bf \leftarrow ' char(nm+96)];
   Align='Cap';
   FSize=12;
   text(posmax(1),posmax(2),posmax(3),TT,'FontSize',FSize,'color','b','VerticalAlignment',Align,'Tag','IgnoreReFormatting','HorizontalAlignment','center');
end
grid on;
subplot(1,2,2);

xlim([0 10]);
ylim([0 10]);
daspect([1 1 1]);

TT={};
TT{1}='First 5 Maxima';
for nm=1:5
    posmax=[Rx(MaxPos(nm,2)),Ry(MaxPos(nm,1)),Rz(MaxPos(nm,3))];
    TT{nm+1}= [char(nm+96) sprintf(': %4.3f%',Maxima(nm)) ', X=' sprintf('%2.1f, ',posmax(1)) ', Y=' sprintf('%2.1f',posmax(2)) ', Z=' sprintf('%2.1f',posmax(3))];
end
Align='Cap';
FSize=12;

t1=text(-1.5,7,TT,'FontSize',FSize,'color','k','VerticalAlignment',Align,'BackgroundColor','w','Edgecolor','k','Margin',4);
set(gca,'visible','off');
set(t1,'visible','on');

%at end, you should see three iosurfaces, where in each isosurface we can
%see that global maxima is inside the -6dB region (as expected for my
%needs) and we can see that the other 4 maxima are located oustide this
%region (what for me is also expected).


function e = vgg_rms_rrror(M)
% e = vgg_rms_rrror(M)
%
% Get RMS diff from zero of matrix or vector M

e = sqrt(sum(sum(M.*M)) / prod(size(M)));


function [cl] = m32cl(m3)
% Convert volume from cubic meters to centiliters.
% Chad Greene 2012
cl = m3*100000;


%% housekeeping
clc
%% RISE the model
linear=~true;
if linear
    m=rise('targets_lin');
else
    m=rise('targets','steady_state_file','sstate_model');
end
%% get the parameters
version='a';
% 'a' % model with an endogenous inflation target.
% 'b' % model with an exogenous inflation target.
% 'd' % model with backward-looking price setting.
start_at_mode=false;

[p,priors]=create_parameters(version,linear,start_at_mode);

%% push the parameters
m=set(m,'parameters',p);

%% create data
[data]=create_data();

%% estimate the model
ms=estimate(m,'data',data,'estim_priors',priors);

%% Impulse responses
myirfs=irf(ms);

%% plot responses
myvars={'Y','PAI','R','X'};
locs=locate_variables(myvars,ms.endogenous.name);
myvtex=ms.endogenous.tex_name(locs);
sstate=get(ms,'sstate');
ssdev=false;

shocks={'EPS_A','EPS_E','EPS_Z','EPS_V','EPS_PAI'};%m.exogenous.name;
locs=locate_variables(shocks,ms.exogenous.name);
myshtex=ms.exogenous.tex_name(locs);
close all
figure('name','Impulse reponses');
iter=0;
ss=1;
for ishock=1:numel(shocks)
    shock=shocks{ishock};
    for ivar=1:numel(myvars)
        vname=myvars{ivar};
        if ssdev
            ss=sstate.(vname);
        end
        iter=iter+1;
        subplot(5,4,iter)
        plot('0:16',myirfs.(shock).(vname)/ss,'linewidth',2)
        title([myvtex{ivar},' to ',myshtex{ishock}])
    end
end


function [H,S,D]=sobi(X,n,p)
% SOBI - Second Order Blind Identification (SOBI) by joint diagonalization of
%          correlation  matrices. THIS CODE ASSUMES TEMPORALLY CORRELATED SIGNALS,
%          and uses correlations across times in performing the signal separation.
%          Thus, estimated time delayed covariance matrices must be nonsingular
%          for at least some time delays.
% Usage:
%         >> winv = sobi(data);
%         >> [winv,act] = sobi(data,n,p);
% Inputs:
%   data - data matrix of size [m,N] ELSE of size [m,N,t] where
%                m is the number of sensors,
%                N is the  number of samples,
%                t is the  number of trials (avoid epoch boundaries)
%         n - number of sources {Default: n=m}
%         p - number of correlation matrices to be diagonalized
%             {Default: min(100, N/3)} Note that for non-ideal data,
%             the authors strongly recommend using at least 100 time delays.
%
% Outputs:
%   winv - Matrix of size [m,n], an estimate of the *mixing* matrix. Its
%          columns are the component scalp maps. NOTE: This is the inverse
%          of the usual ICA unmixing weight matrix. Sphering (pre-whitening),
%          used in the algorithm, is incorporated into winv. i.e.,
%
%             >> icaweights = pinv(winv); icasphere = eye(m);
%
%   act  - matrix of dimension [n,N] an estimate of the source activities
%
%             >> data            = winv            * act;
%                [size m,N]        [size m,n]        [size n,N]
%             >> act = pinv(winv) * data;
%
% Authors:  A. Belouchrani and A. Cichocki (references: See function body)
% Note:     Adapted by Arnaud Delorme and Scott Makeig to process data epochs by
%           computing covariances while respecting epoch boundaries.

% REFERENCES:
% A. Belouchrani, K. Abed-Meraim, J.-F. Cardoso, and E. Moulines, ``Second-order
%  blind separation of temporally correlated sources,'' in Proc. Int. Conf. on
%  Digital Sig. Proc., (Cyprus), pp. 346--351, 1993.
%
%  A. Belouchrani and K. Abed-Meraim, ``Separation aveugle au second ordre de
%  sources correlees,'' in  Proc. Gretsi, (Juan-les-pins),
%  pp. 309--312, 1993.
%
%  A. Belouchrani, and A. Cichocki,
%  Robust whitening procedure in blind source separation context,
%  Electronics Letters, Vol. 36, No. 24, 2000, pp. 2050-2053.
%
%  A. Cichocki and S. Amari,
%  Adaptive Blind Signal and Image Processing, Wiley,  2003.

% Authors note: For non-ideal data, use at least p=100 the time-delayed covariance matrices.
DEFAULT_LAGS = 100;

[m,N,ntrials]=size(X);

if nargin<1 || nargin > 3

    help sobi

elseif nargin==1

    n=m; % Source detection (hum...)
    p=min(DEFAULT_LAGS,ceil(N/3)); % Number of time delayed correlation matrices to be diagonalized

elseif nargin==2

    p=min(DEFAULT_LAGS,ceil(N/3)); % Default number of correlation matrices to be diagonalized
    % Use < DEFAULT_LAGS delays if necessary for short data epochs
end

%
% Make the data zero mean
%
X(:,:)=X(:,:)-kron(mean(X(:,:)')',ones(1,N*ntrials));

%
% Pre-whiten the data based directly on SVD
%
[UU,S,VV]=svd(X(:,:)',0);
Q= pinv(S)*VV';
X(:,:)=Q*X(:,:);

% Alternate whitening code
% Rx=(X*X')/T;
% if m<n, % assumes white noise
%   [U,D]=eig(Rx);
%   [puiss,k]=sort(diag(D));
%   ibl= sqrt(puiss(n-m+1:n)-mean(puiss(1:n-m)));
%    bl = ones(m,1) ./ ibl ;
%   BL=diag(bl)*U(1:n,k(n-m+1:n))';
%   IBL=U(1:n,k(n-m+1:n))*diag(ibl);
% else    % assumes no noise
%    IBL=sqrtm(Rx);
%    Q=inv(IBL);
% end
% X=Q*X;

%
% Estimate the correlation matrices
%
k=1;
pm=p*m; % for convenience
for u=1:m:pm
    k=k+1;
    for t = 1:ntrials
        if t == 1
            Rxp=X(:,k:N,t)*X(:,1:N-k+1,t)'/(N-k+1)/ntrials;
        else
            Rxp=Rxp+X(:,k:N,t)*X(:,1:N-k+1,t)'/(N-k+1)/ntrials;
        end
    end
    M(:,u:u+m-1)=norm(Rxp,'fro')*Rxp;  % Frobenius norm =
end                                    % sqrt(sum(diag(Rxp'*Rxp)))

%
% Perform joint diagonalization
%
epsil=1/sqrt(N)/100;
encore=1;
V=eye(m);
step_n=0;
while encore
    encore=0;
    for p=1:m-1
        for q=p+1:m
            % Perform Givens rotation
            g=[   M(p,p:m:pm)-M(q,q:m:pm)  ;
                M(p,q:m:pm)+M(q,p:m:pm)  ;
                i*(M(q,p:m:pm)-M(p,q:m:pm)) ];
            [vcp,D] = eig(real(g*g'));
            [la,K]=sort(diag(D));
            angles=vcp(:,K(3));
            angles=sign(angles(1))*angles;
            c=sqrt(0.5+angles(1)/2);
            sr=0.5*(angles(2)-j*angles(3))/c;
            sc=conj(sr);
            oui = abs(sr)>epsil ;
            encore=encore | oui ;
            if oui  % Update the M and V matrices
                colp=M(:,p:m:pm);
                colq=M(:,q:m:pm);
                M(:,p:m:pm)=c*colp+sr*colq;
                M(:,q:m:pm)=c*colq-sc*colp;
                rowp=M(p,:);
                rowq=M(q,:);
                M(p,:)=c*rowp+sc*rowq;
                M(q,:)=c*rowq-sr*rowp;
                temp=V(:,p);
                V(:,p)=c*V(:,p)+sr*V(:,q);
                V(:,q)=c*V(:,q)-sc*temp;
            end %% if
        end %% q loop
    end %% p loop
    step_n=step_n+1;
    fprintf('%d step\n',step_n);
end %% while

%
% Estimate the mixing matrix
%
H = pinv(Q)*V;

%
% Estimate the source activities
%
if nargout>1
    S=V'*X(:,:); % estimated source activities
end


function automobile_plot ( )

%*****************************************************************************80
%
%% AUTOMOBILE_PLOT reads the automobile dataset and makes a scatterplot.
%
%  Discussion:
%
%    The hardest part of this exercise was reading the comma-separated data file,
%    which contains integer, real and string data.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 April 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    A Frank, A Asuncion,
%    UCI Machine Learning Repository,
%    http://archive.ics.uci.edu/ml,
%    School of Information and Computer Science,
%    University of California, Irvine, California.
%

%
%  Read the data from the file.
%
  auto_cell = automobile_read ( 'automobile.txt' );
%
%  Price and weight are specific columns of the cell array.
%  Extract them, and convert them to numeric vectors.
%
  price = auto_cell(:,26);
  price = cell2mat ( price );
  weight = auto_cell(:,14);
  weight = cell2mat ( weight );
%
%  Ignore missing data.
%
  index = ( ( price ~= -1 ) & ( weight ~= -1 ) );
%
%  Create a scatter plot.
%
  figure ( 1 )
  clf

  plot ( price(index), weight(index), 'bo', 'MarkerSize', 5 )
  grid on
  xlabel ( 'Price in 1985 Dollars' );
  ylabel ( 'Curb weight in pounds' );
  title ( 'Scatter plot of price versus weight.' );

  return
end


function [dist_matrix] = pos2dist(positions)
% pos2dist - Compute the relative distances matrix from the positions
%            matrix
%
% Inputs:
%   positions - matrix of size (3,nb_agents)
%
% Ouputs:
%   dist_matrix - symmetric matrix with pairwise distances
%
%
    distances = pdist(positions');
    dist_matrix = squareform(distances);

end


%%
%nested cross validation for testing the logistic rMTFL model
%The stratified cv are integrated into method to handle
%"not_enough_data_sample" problems and privide more accurate estimator
%Han Cao
%24.02.2017
%%

clear;
clc;
close;

addpath('../MALSAR/functions/rMTFL/'); % load function
addpath('../MALSAR/utils/'); % load utilities
addpath('./train_and_test/');

% simulate the data
n = 50;
d = 300;
T = 10;

X = cell(T, 1);
Y = cell(T, 1);
W = randn(d, T);
W_mask = abs(randn(d, T))<1;
W(W_mask) = 0;
for i = 1: T
    X{i} = randn(n, d);
    Y{i} = sign(X{i} * W(:, i) + rand(n, 1) * 0.01);
end

%optimization options
opts.init = 2;
opts.tFlag = 1;
opts.tol = 10^-5;
opts.maxIter = 60000;

% lambda range
lambda1_range = [1:-0.01:0.01];
lambda2_range = [2:-0.05:0.05];

%container for holding the results
r_acc=cell(1,3);
r_inCvAcc=cell(1,3); %
r_S=cell(1,3);

%nested cross validation
out_cv_fold=3;
in_cv_fold=5;
for i = 1: out_cv_fold
    Xtr = cell(T, 1);
    Ytr = cell(T, 1);
    Xte = cell(T, 1);
    Yte = cell(T, 1);

    %stratified cross validation
    for t = 1: T
        task_sample_size = length(Y{t});
        ct = find(Y{t}<0);
        cs = find(Y{t}>0);
        ct_idx = i : out_cv_fold : length(ct);
        cs_idx = i : out_cv_fold : length(cs);
        te_idx = [ct(ct_idx); cs(cs_idx)];
        tr_idx = setdiff(1:task_sample_size, te_idx);

        Xtr{t} = X{t}(tr_idx, :);
        Ytr{t} = Y{t}(tr_idx, :);
        Xte{t} = X{t}(te_idx, :);
        Yte{t} = Y{t}(te_idx, :);
    end

    %inner cv
    fprintf('inner CV started\n')
    [best_lambda1 best_lambda2 accuracy_mat] = CrossValidationDirty( Xtr, Ytr, ...
        'Logistic_rMTFL', opts, lambda1_range,lambda2_range, in_cv_fold, ...
        'eval_MTL_accuracy');

    %train
    %warm start for one turn
    [W C P Q L F] = Logistic_rMTFL(Xtr, Ytr, best_lambda1, best_lambda2, opts);
    opts2=opts;
    opts2.init=1;
    opts2.C0=C;
    opts2.P0=P;
    opts2.Q0=Q;
    opts2.tol = 10^-10;
    [W2 C2 P2 Q2 L2 F2] = Logistic_rMTFL(Xtr, Ytr, best_lambda1, best_lambda2, opts2);

     %test
    final_performance = eval_MTL_accuracy(Yte, Xte, W2, C2);

    %collect results
    r_acc{i}=final_performance;
    r_inCvAcc{i}=accuracy_mat;
    r_S{i}=nnz((sum(P2,2)==0))/size(P2,1);

end

 fprintf('the average accuracy is \n')
 disp(mean(cell2mat(r_acc)))

 %cv accuracy cross lambda
for i=1:out_cv_fold
    surf(lambda1_range', lambda2_range,r_inCvAcc{i}' );
    xlabel('Parameter for P');
    ylabel('parameter for Q');
    hold on;
end
hold off;
title('cross validation accuracy over different lambda');
set(gca,'FontSize',12);
print('-dpdf', '-r100', 'LogisticDirty');


function plotData(X, y)
%PLOTDATA Plots the data points X and y into a new figure
%   PLOTDATA(x,y) plots the data points with + for the positive examples
%   and o for the negative examples. X is assumed to be a Mx2 matrix.

% Create New Figure
figure; hold on;

% ====================== YOUR CODE HERE ======================
% Instructions: Plot the positive and negative examples on a
%               2D plot, using the option 'k+' for the positive
%               examples and 'ko' for the negative examples.
%

ids = find(y==0);
Xo = X(ids,:);
plot(Xo(:,1), Xo(:,2), 'ko');

ids = find(y==1);
Xo = X(ids,:);
plot(Xo(:,1), Xo(:,2), 'k+');

% =========================================================================

hold off;

end


function Z=fftfilter(X, H)
F=fft2(X, size(H,1), size(H, 2));
Z=H.*F;
Z=ifftshift(Z);
Z=abs(ifft2(Z));
Z=Z(1:size(X, 1), 1:size(X, 2));
end


clear
clc
close all

addpath('data')
addpath('src')
dataset = {'1_mECS', '2_Kolod', '3_Pollen', '4_Usoskin'}

for i = 1:4

    % perform the analysis for the current dataset
    load(['Test_' dataset{i}]);
    C = max(true_labs); %%% number of clusters
    rng(i,'twister'); %%% for reproducibility
    [y, S, F, ydata,alpha] = SIMLR(in_X,C,10);

    % report NMI values
    NMI_i = Cal_NMI(y,true_labs);
    fprintf(['The NMI value for dataset ' dataset{i} ' is %f\n'], NMI_i);

    % visualization
    figure;
    gscatter(ydata(:,1),ydata(:,2),true_labs);

end


% StackExchange Signal Processing Q62024
% https://dsp.stackexchange.com/questions/62024
% Proximal Gradient Method (PGM) for a Function Model with More than 2 Functions (Sum of Functions)
% References:
%   1.  aa
% Remarks:
%   1.  sa
% TODO:
% 	1.  ds
% Release Notes
% - 1.0.000     25/11/2019
%   *   First release.

%% General Parameters

subStreamNumberDefault = 79;

run('InitScript.m');

figureIdx           = 0; %<! Continue from Question 1
figureCounterSpec   = '%04d';

generateFigures = OFF;

%% Simulation Parameters

numElements = 40;
paramLambda1 = 0.5; %<! L1 Norm
paramLambda2 = 0.75; %<! TV Norm

numIterations = 1000; %<! For the ADMM

%% Generate Data

vY = 10 * randn(numElements, 1);

% Generate the Diff Operator (1D Gradient) by Finite Differences
mD = spdiags([-ones(numElements, 1), ones(numElements, 1)], [0, 1], numElements - 1, numElements);

hSolveProxTv = @(vY, paramLambda) SolveProxTvAdmm(vY, mD, paramLambda, numIterations);

% Objective Function
hObjFun = @(vX) (0.5 * sum( (vX - vY) .^ 2)) + (paramLambda1 * sum(abs(vX))) + (paramLambda2 * sum(abs(mD * vX)));

%% Solution by CVX

cvx_begin('quiet')
    cvx_precision('best');
    variable vX(numElements);
    minimize( (0.5 * pow_pos(norm(vX - vY, 2), 2)) + (paramLambda1 * norm(vX, 1)) + (paramLambda2 * norm(mD * vX, 1)));
cvx_end

disp([' ']);
disp(['CVX Solution Summary']);
disp(['The CVX Solver Status - ', cvx_status]);
disp(['The Optimal Value Is Given By - ', num2str(cvx_optval)]);
disp(['The Optimal Argument Is Given By - [ ', num2str(vX(:).'), ' ]']);
disp([' ']);

%% Solution by Analytical Solution
%{
Solving $ \arg \min_x \frac{1}{2} {\left\| x - y \right\|}_{2}^{2} +
{\lambda}_{1} {\left\| x \right\|}_{1} + {\lambda}_{2} {\left\| D x \right\|}_{1} $
%}

vX = SolveProxL1(hSolveProxTv(vY, paramLambda2), paramLambda1);

disp([' ']);
disp(['Analytical Direct Solution Summary']);
disp(['The Optimal Value Is Given By - ', num2str(hObjFun(vX))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(vX(:).'), ' ]']);
disp([' ']);

%% Solution by Analytical Solution
%{
Solving $ \arg \min_x \frac{1}{2} {\left\| x - y \right\|}_{2}^{2} +
{\lambda}_{1} {\left\| x \right\|}_{1} + {\lambda}_{2} {\left\| D x \right\|}_{1} $
%}

vX = hSolveProxTv(SolveProxL1(vY, paramLambda1), paramLambda2);

disp([' ']);
disp(['Analytical Direct Solution Summary']);
disp(['The Optimal Value Is Given By - ', num2str(hObjFun(vX))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(vX(:).'), ' ]']);
disp([' ']);

%% Restore Defaults

% set(0, 'DefaultFigureWindowStyle', 'normal');
% set(0, 'DefaultAxesLooseInset', defaultLoosInset);


function y=gauss_res2(p)
global grab;
global xpix;
global ypix;
global psf_w02;

yfit=p(3)*exp(-2*((xpix-p(1)).*(xpix-p(1))+(ypix-p(2)).*(ypix-p(2)))/psf_w02);
ydev=yfit-double(grab);
ydev2=ydev.*ydev;
y=sum(sum(ydev2));
%y=100*(p(2)-p(1)^2)^2+(1-p(1))^2;
end


function [sol, infos] = gsp_ml_rls(xl, y, k, tau, A, At, param)
%GSP_ML_RLS Manifold Learning regularized least square
%   Usage: sol = gsp_ml_rls(xl, y, k, tau);
%          sol = gsp_ml_rls(xl, y, k, tau, A, At);
%          sol = gsp_ml_rls(xl, y, k, tau,  A, At, param);
%          [sol, infos] = gsp_ml_rls(...)
%
%   Input parameters:
%       xl      : labeled points
%       y       : labels
%       k       : kernel
%       tau     : regularization parameters
%       A       : Operator
%       At      : Adoint operator
%       param   : Optional parameters
%   Output parameters:
%       sol     : solution of the problem (kernel coefficients)
%       infos   : convergence info
%
%   *param* is a structure of optional argument given to the solver
%   gradient_descent. Please see the function gradient descent for more
%   information.
%
%   In *param*, you also have to set an upperbound for the operator A as
%   param.nu!
%
%   This function solves the following problem:
%
%   ..  argmin_alpha  || A (K alpha) - y ||_2^2 + tau *alpha^T K alpha
%
%   If tau is set to zero, then the following problem is solved
%
%   ..  argmin_alpha alpha^T K alpha s. t.  A (K alpha) = y
%

% Author: Nathanael Perraudin
% Date  : 8 decembre 2014

if nargin<5
    A = @(x) x;
end

if nargin<6
    At = A;
end

if nargin<7
    param = struct;
end

if ~isfield(param, 'tol'), param.tol = 1e-6; end
if ~isfield(param, 'nu'), param.nu = 1; end
if ~isfield(param, 'verbose'), param.verbose = 1; end

%N = size(xl,2);

% Evaluate the kernel on the data points
K = gsp_rkhs_evaluate(k,xl);
nu = norm(K);

[N,M] = size(y);
Nk = length(k);
alpha_in = zeros(N*Nk,M);

if tau >0

    fp.eval = @(x) tau * sum(norm_rkhs( K,x ));
    fp.grad = @(x) tau * grad_rkhs( K,x);
    fp.beta =   2*nu*tau;

    ffid.eval = @(x) norm(A(K*x)-y,'fro')^2;
    ffid.grad = @(x) 2*K'*At(A(K*x)-y);
    ffid.beta = 2*nu^2*param.nu^2;

%     paramfid.A = @(x) A(K*x);
%     paramfid.At = @(x) K'*At(x);
%     paramfid.nu = nu^2;
%     paramfid.tight = 0;
%     paramfid.y = y;
%     paramfid.verbose = param.verbose -1;
%     ffid.eval = @(x) norm(A(K*x)-y,'fro')^2;
%     ffid.prox = @(x,T) prox_l2(x,T,paramfid);

    [sol,infos] = solvep(alpha_in, {fp,ffid}, param);

%     param.gamma = 0.5/(tau*nu);
%     [sol,infos] = forward_backward(alpha_in, ffid, fp, param);
else

    fp.eval = @(x) sum(norm_rkhs( K,x ));
    fp.grad = @(x) grad_rkhs( K,x);
    fp.beta =   2*nu;

    paramproj.A = @(x) A(K*x);
    paramproj.At = @(x) K'*At(x);
    paramproj.nu = nu^2*param.nu^2;
    paramproj.tight = 0;
    paramproj.verbose = param.verbose-1;
    paramproj.y = y;
    paramproj.maxit = 50;
    ffid.eval = @(x) eps;
    ffid.prox = @(x,T) proj_b2(x,T,paramproj);

    [sol,infos] = forward_backward(alpha_in, ffid, fp, param);

end

end


function o = boxoverlap(a, b)
% Compute the symmetric intersection over union overlap between a set of
% bounding boxes in a and a single bounding box in b.
%
% a  a matrix where each row specifies a bounding box
% b  a single bounding box

% AUTORIGHTS
% -------------------------------------------------------
% Copyright (C) 2011-2012 Ross Girshick
% Copyright (C) 2008, 2009, 2010 Pedro Felzenszwalb, Ross Girshick
%
% This file is part of the voc-releaseX code
% (http://people.cs.uchicago.edu/~rbg/latent/)
% and is available under the terms of an MIT-like license
% provided in COPYING. Please retain this notice and
% COPYING if you use this file (or a portion of it) in
% your project.
% -------------------------------------------------------

x1 = max(a(:,1), b(1));
y1 = max(a(:,2), b(2));
x2 = min(a(:,3), b(3));
y2 = min(a(:,4), b(4));

w = x2-x1+1;
h = y2-y1+1;
inter = w.*h;
aarea = (a(:,3)-a(:,1)+1) .* (a(:,4)-a(:,2)+1);
barea = (b(3)-b(1)+1) * (b(4)-b(2)+1);
% intersection over union overlap
o = inter ./ (aarea+barea-inter);
% set invalid entries to 0 overlap
o(w <= 0) = 0;
o(h <= 0) = 0;


%IM_SELECT_BLOB Fixed mapping selecting largest blob in binary images (DIP_Image)
%
%       B = IM_SELECT_BLOB(A)
%       B = A*IM_SELECT_BLOB
%
% Just the largest object in the image is returned.
%
% SEE ALSO (PRTools Guide)
% DATASETS, DATAFILES, DIP_IMAGE

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

function b = im_select_blob(a)

  if nargin < 1 | isempty(a)
    b = prmapping(mfilename,'fixed');
    b = setname(b,'Select largest blob');
	elseif isa(a,'prdataset') % allows datafiles too
		isobjim(a);
    b = filtim(a,mfilename);
		b = setfeatsize(b,getfeatsize(a));
  elseif isa(a,'double') | isa(a,'dip_image') % here we have a single image
    if ~isa(a,'dip_image')
			a = dip_image(a,'bin');
		end;
		labim = label(a);
		sz = measure(labim,labim,{'size'});
		[cc,ind] = max(double(sz));
		I = double(measure(labim,labim,{'mean'}));
		b = a.*(labim==round(I(ind)));
%		c = measure(labim,labim,{'size','mean'});
%		c = double(c);
%		[cc,ind] = max(c(:,1));
%		b = a.*(labim==round(c(ind,2)));
	end

return


classdef KnEA < ALGORITHM
% <many> <real/integer/label/binary/permutation> <constrained/none>
% Knee point driven evolutionary algorithm
% rate --- 0.5 --- Rate of knee points in the population

%------------------------------- Reference --------------------------------
% X. Zhang, Y. Tian, and Y. Jin, A knee point-driven evolutionary algorithm
% for many-objective optimization, IEEE Transactions on Evolutionary
% Computation, 2015, 19(6): 761-776.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            rate = Algorithm.ParameterSet(0.5);

            %% Generate random population
            Population = Problem.Initialization();
            FrontNo    = NDSort(Population.objs,Population.cons,inf);
            KneePoints = zeros(1,Problem.N);     % Set of knee points
            r          = -ones(1,2*Problem.N);	% Ratio of size of neighorhood
            t          = -ones(1,2*Problem.N);	% Ratio of knee points

            %% Optimization
            while Algorithm.NotTerminated(Population)
                MatingPool = MatingSelection(Population.objs,FrontNo,KneePoints);
                Offspring  = OperatorGA(Problem,Population(MatingPool));
                Population = [Population,Offspring];
                [FrontNo,MaxFNo]                = NDSort(Population.objs,Population.cons,Problem.N);
                [KneePoints,Distance,r,t]       = FindKneePoints(Population.objs,FrontNo,MaxFNo,r,t,rate);
                [Population,FrontNo,KneePoints] = EnvironmentalSelection(Population,FrontNo,MaxFNo,KneePoints,Distance,Problem.N);
            end
        end
    end
end


function [yd3] = gal2yd3(gal)
% Convert volume from US liquid gallons to cubic yards.
% Chad Greene 2012
yd3 = gal*0.0049511316873;


function rs = amutual2(s, len)

%tstoolbox/@signal/amutual2
%   Syntax:
%     * amutual2(s, len)
%
%   Input arguments:
%     * len - maximal lag
%
%   Auto mutual information (average) function for real scalar signals
%   using 128 equidistant partitions.
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt
narginchk(2,2);

    if (ndim(s) > 1) | (~isreal(data(s)))
	help(mfilename)
	return
end

c = amutual2(s.core, len);
rs = signal(c, s);	% special constructor calling syntax for working routines
a = getaxis(s, 1);
dl = delta(a);
a = setfirst(a, 0);
rs = setaxis(rs, 1, a);
rs = setyunit(rs, unit('Bit'));		% acf values are scalars without unit
rs = addhistory(rs, ['Auto mutual information of length ' num2str(len)]);
rs = addcommandlines(rs, 's = amutual(s', len);


function set_theory_test ( )

%*****************************************************************************80
%
%% SET_THEORY_TEST tests the SET_THEORY library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2011
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SET_THEORY_TEST:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the SET_THEORY library.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Demonstrate some set theory operations that\n' );
  fprintf ( 1, '  can be implemented in MATLAB.\n' );

  set_theory_test01 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SET_THEORY_TEST:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function linpack_z_test14 ( )

%*****************************************************************************80
%
%% TEST14 tests ZHPCO.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 3;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST14\n' );
  fprintf ( 1, '  For a double precision complex (C)\n' );
  fprintf ( 1, '  Hermitian matrix using packed storage (HP),\n' );
  fprintf ( 1, '  ZHPCO factors the matrix and estimates\n' );
  fprintf ( 1, '  the reciprocal condition number.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The matrix order is N = %d\n', n );
%
%  Set the values of the matrix A.
%
  k = 0;
  seed = 123456789;

  for j = 1 : n

    for i = 1 : j-1
      k = k + 1;
      [ a(k), seed ] = c8_uniform_01 ( seed );
      a_save(i,j) = a(k);
      a_save(j,i) = conj ( a(k) );
    end

    k = k + 1;
    [ a(k), seed ] = r8_uniform_01 ( seed );
    a_save(j,j) = a(k);

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The matrix A:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '  (%8f  %8f)', real ( a_save(i,j) ), imag ( a_save(i,j) ) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Factor the matrix A.
%
  [ a, ipvt, rcond ] = zhpco ( a, n );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Estimated reciprocal condition RCOND = %f\n', rcond );

  return
end


function [distance,branch] = breadth(CIJ,source)
%BREADTH        Auxiliary function for breadthdist.m
%
%   [distance,branch] = breadth(CIJ,source);
%
%   Implementation of breadth-first search.
%
%   Input:      CIJ,        binary (directed/undirected) connection matrix
%               source,     source vertex
%
%   Outputs:    distance,   distance between 'source' and i'th vertex
%                           (0 for source vertex)
%               branch,     vertex that precedes i in the breadth-first search tree
%                           (-1 for source vertex)
%
%   Notes: Breadth-first search tree does not contain all paths (or all
%   shortest paths), but allows the determination of at least one path with
%   minimum distance. The entire graph is explored, starting from source
%   vertex 'source'.
%
%
%   Olaf Sporns, Indiana University, 2002/2007/2008

N = size(CIJ,1);

% colors: white, gray, black
white = 0;
gray = 1;
black = 2;

% initialize colors
color = zeros(1,N);
% initialize distances
distance = inf*ones(1,N);
% initialize branches
branch = zeros(1,N);

% start on vertex 'source'
color(source) = gray;
distance(source) = 0;
branch(source) = -1;
Q = source;

% keep going until the entire graph is explored
while ~isempty(Q)
   u = Q(1);
   ns = find(CIJ(u,:));
   for v=ns
% this allows the 'source' distance to itself to be recorded
      if (distance(v)==0)
         distance(v) = distance(u)+1;
      end;
      if (color(v)==white)
         color(v) = gray;
         distance(v) = distance(u)+1;
         branch(v) = u;
         Q = [Q v];                                             %#ok<AGROW>
      end;
   end;
   Q = Q(2:length(Q));
   color(u) = black;
end


function varargout = drawGrid3d(varargin)
%DRAWGRID3D Draw a 3D grid on the current axis.
%
%   drawGrid3d
%   draws a 3D square grid, with origin (0,0,0) and spacing 1 in each
%   direction, with bounds corresponding to the bounds of current axis.
%
%   drawGrid3d(SPACING)
%   where spacing is either a scalar or a [1x3] matrix, specifies the size
%   of the unit cell.
%
%   drawGrid3d(ORIGIN, SPACING)
%   Also specify origin of grid. ORIGIN is a [1x3] array.
%
%   drawGrid3d(..., EDGE)
%   specifies whether function should draw edges touching edges of axis.
%   EDGE is a characheter string, which can be :
%   - 'OPEN' : each line start from one face of window to the opposite
%   face. This results in a 'spiky' grid.
%   - 'CLOSED' (default value) : each line stops at the last visible point
%   of the grid for this line. The result looks like a box (no free spikes
%   around the grid).
%
%   H = drawGrid3d(...);
%   return a vector of handles for each LINE object which was crated.
%

%   ------
%   Author: David Legland
%   e-mail: david.legland@grignon.inra.fr
%   Created: 2005-11-17
%   Copyright 2005 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas).

%% initialize variables -----

% default values
closed = true;
origin = [0 0 0];
spacing = [1 1 1];

% check if grid is open or not
str = '';
if ~isempty(varargin)
    str = varargin{end};
end
if ischar(str)
    if strncmpi(str, 'open', 4)
        closed = false;
    end
    varargin = varargin(1:end-1);
end

% check origin and grid spacing
if length(varargin)==1
    spacing = varargin{1};
elseif length(varargin)==2
    origin = varargin{1};
    spacing = varargin{2};
end

%% Compute internam data -----

% get axis limits
ax = axis;
x0 = ax(1); x1 = ax(2);
y0 = ax(3); y1 = ax(4);
z0 = ax(5); z1 = ax(6);

% get first and last coordinates of the grid in each direction
dx = spacing(1); dy = spacing(2); dz = spacing(3);
xe = x0 + mod(origin(1) - x0, dx);
xf = x1 - mod(x1 - origin(1), dx);
ye = y0 + mod(origin(2) - y0, dy);
yf = y1 - mod(y1 - origin(2), dy);
ze = z0 + mod(origin(1) - z0, dz);
zf = z1 - mod(z1 - origin(1), dz);

% update first and last coordinate if grid is 'closed'
if closed
    x0 = xe; x1 = xf;
    y0 = ye; y1 = yf;
    z0 = ze; z1 = zf;
end

%% Draw the grid -----

h = [];
%TODO: rewrite code, avoiding loops

% draw lines parallel to x axis
for y = ye:dy:yf
    for z = ze:dz:zf
        h = [h; drawEdge3d([x0 y z x1 y z])]; %#ok<AGROW>
    end
end

% draw lines parallel to y axis
for x = xe:dx:xf
    for z = ze:dz:zf
        h = [h; drawEdge3d([x y0 z x y1 z])]; %#ok<AGROW>
    end
end

% draw lines parallel to z axis
for x = xe:dx:xf
    for y = ye:dy:yf
        h = [h; drawEdge3d([x y z0 x y z1])]; %#ok<AGROW>
    end
end

%% Check output arguments -----

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


function varargout = chebpolyval3(f, varargin)
%CHEBPOLYVAL3   Values of a CHEBFUN3 object F on a tensor product grid.
%   X = CHEBPOLYVAL3(F, M, N, P) returns an M x N x P tensor of values of a
%   CHEBFUN3 object F on a tensor product grid.
%
%   X = CHEBPOLYVAL3(F) does the same but sets M, N, and P equal to the
%   length of F.
%
%   [CORE, C, R, T] = CHEBPOLYVAL3(F) returns the low rank representation
%   of the values of F on a tensor product grid, i.e.,
%   X = CORE x_1 C x_2 R x_3 T.
%
%   Example: For an order-3 discrete tensor R, we should have
%                                       R \approx chebpolyval3(chebfun3(R)).

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Empty check.
if ( isempty(f) )
    varargout = { [] };
    return
end

if ( nargin == 1 )
    % Get degrees:
    [m, n, p] = length(f);
elseif ( nargin ~= 4 )
    error('CHEBFUN:CHEBFUN3:chebpolyval3:inputs', 'Dimension not specified.');
else
    m = varargin{1};
    n = varargin{2};
    p = varargin{3};
end

% Get low rank representation of f:
[core, cols, rows, tubes] = tucker(f);

tech = chebfunpref().tech();
colVals = tech.coeffs2vals(chebcoeffs(cols, m));
rowVals = tech.coeffs2vals(chebcoeffs(rows, n));
tubeVals = tech.coeffs2vals(chebcoeffs(tubes, p));

% Evaluate:
if ( nargout <= 1 )
    varargout = {chebfun3.txm(chebfun3.txm(chebfun3.txm(core, colVals, 1), ...
        rowVals, 2), tubeVals, 3)};
else
    varargout = {core, colVals, rowVals, tubeVals};
end

end


function sparse_unique_index = sgmga_unique_index ( dim_num, level_weight, ...
  level_max, rule, growth, np, p, tol, point_num, point_total_num )

%*****************************************************************************80
%
%% SGMGA_UNIQUE_INDEX maps nonunique to unique points of an SGMGA grid.
%
%  Discussion:
%
%    The sparse grid usually contains many points that occur in more
%    than one product grid.
%
%    When generating the point locations, it is easy to realize that a point
%    has already been generated.
%
%    But when it's time to compute the weights of the sparse grids, it is
%    necessary to handle situations in which weights corresponding to
%    the same point generated in multiple grids must be collected together.
%
%    This routine generates ALL the points, including their multiplicities,
%    and figures out a mapping from them to the collapsed set of unique points.
%
%    This mapping can then be used during the weight calculation so that
%    a contribution to the weight gets to the right place.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 April 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    An Anisotropic Sparse Grid Stochastic Collocation Method for Partial
%    Differential Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2411-2442.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, real LEVEL_WEIGHT(DIM_NUM), the anisotropic weights.
%
%    Input, integer LEVEL_MAX, the maximum value of LEVEL.
%
%    Input, integer RULE(DIM_NUM), the rule in each dimension.
%     1, "CC",  Clenshaw Curtis, Closed Fully Nested.
%     2, "F2",  Fejer Type 2, Open Fully Nested.
%     3, "GP",  Gauss Patterson, Open Fully Nested.
%     4, "GL",  Gauss Legendre, Open Weakly Nested.
%     5, "GH",  Gauss Hermite, Open Weakly Nested.
%     6, "GGH", Generalized Gauss Hermite, Open Weakly Nested.
%     7, "LG",  Gauss Laguerre, Open Non Nested.
%     8, "GLG", Generalized Gauss Laguerre, Open Non Nested.
%     9, "GJ",  Gauss Jacobi, Open Non Nested.
%    10, "HGK", Hermite Genz-Keister, Open Fully Nested.
%    11, "UO",  User supplied Open, presumably Non Nested.
%    12, "UC",  User supplied Closed, presumably Non Nested.
%
%    Input, integer GROWTH(DIM_NUM), the growth in each dimension.
%    0, "DF", default growth associated with this quadrature rule;
%    1, "SL", slow linear, L+1;
%    2  "SO", slow linear odd, O=1+2((L+1)/2)
%    3, "ML", moderate linear, 2L+1;
%    4, "SE", slow exponential;
%    5, "ME", moderate exponential;
%    6, "FE", full exponential.
%
%    Input, integer NP(DIM_NUM), the number of parameters used by each rule.
%
%    Input, real P(*), the parameters needed by each rule.
%
%    Input, real TOL, the tolerance for point equality.
%
%    Input, integer POINT_NUM, the number of unique points in
%    the grid.
%
%    Input, integer POINT_TOTAL_NUM, the total number of points
%    in the grid.
%
%    Output, integer SPARSE_UNIQUE_INDEX(POINT_TOTAL_NUM), lists,
%    for each (nonunique) point, the corresponding index of the same point in
%    the unique listing.
%

%
%  Special cases.
%
  if ( level_max < 0 )
    sparse_unique_index = [];
    return
  end

  if ( level_max == 0 )
    sparse_unique_index(1) = 1;
    return
  end
%
%  Generate SPARSE_TOTAL_ORDER and SPARSE_TOTAL_INDEX arrays
%  for the TOTAL set of points.
%
  sparse_total_order = zeros(dim_num,point_total_num);
  sparse_total_index = zeros(dim_num,point_total_num);

  point_total_num2 = 0;
%
%  Initialization for SGMGA_VCN_ORDERED.
%
  level_weight_min_pos = r8vec_min_pos ( dim_num, level_weight );
  q_min = level_max * level_weight_min_pos - sum ( level_weight(1:dim_num) );
  q_max = level_max * level_weight_min_pos;
  level_1d_max = zeros(dim_num,1);
  for dim = 1 : dim_num
    if ( 0.0 < level_weight(dim) )
      level_1d_max(dim) = floor ( q_max / level_weight(dim) ) + 1;
      if ( q_max <= ( level_1d_max(dim) - 1 ) * level_weight(dim) )
        level_1d_max(dim) = level_1d_max(dim) - 1;
      end
    else
      level_1d_max(dim) = 0;
    end
  end
  more_grids = 0;
  level_1d = [];
%
%  Seek all vectors LEVEL_1D which satisfy the constraint:
%
%    LEVEL_MAX * LEVEL_WEIGHT_MIN_POS - sum ( LEVEL_WEIGHT )
%      < sum ( 1 <= I <= DIM_NUM ) LEVEL_WEIGHT(I) * LEVEL_1D(I)
%      <= LEVEL_MAX * LEVEL_WEIGHT_MIN_POS.
%
  while ( 1 )

    [ level_1d, more_grids ] = sgmga_vcn_ordered ( dim_num, level_weight, ...
      level_1d_max, level_1d, q_min, q_max, more_grids );

    if ( ~more_grids )
      break
    end
%
%  Compute the combinatorial coefficient.
%
    coef = sgmga_vcn_coef ( dim_num, level_weight, level_1d, q_max );

    if ( coef == 0.0 )
      continue
    end
%
%  Transform each 1D level to a corresponding 1D order.
%
    order_1d = level_growth_to_order ( dim_num, level_1d, rule, growth );
%
%  The inner loop generates a POINT of the GRID of the LEVEL.
%
    point_index = [];
    more_points = 0;

    while ( 1 )

      [ point_index, more_points ] = vec_colex_next3 ( dim_num, order_1d, ...
        point_index, more_points );

      if ( ~more_points )
        break;
      end

      point_total_num2 = point_total_num2 + 1;
      sparse_total_order(1:dim_num,point_total_num2) = order_1d(1:dim_num);
      sparse_total_index(1:dim_num,point_total_num2) = point_index(1:dim_num);

    end

  end
%
%  Now compute the coordinates of the TOTAL set of points.
%
  sparse_total_point = zeros(dim_num,point_total_num);
  sparse_total_point(1:dim_num,1:point_total_num) = r8_huge ( );

  level_weight_min_pos = r8vec_min_pos ( dim_num, level_weight );
  q_max = level_max * level_weight_min_pos;

  p_index = 1;

  for dim = 1 : dim_num

    if ( 0 < level_weight(dim) )
      level_1d_max(dim) = floor ( q_max / level_weight(dim) ) + 1;
      if ( q_max <= ( level_1d_max(dim) - 1 ) * level_weight(dim) )
        level_1d_max(dim) = level_1d_max(dim) - 1;
      end
    else
      level_1d_max(dim) = 0;
    end

    for level = 0 : level_1d_max(dim)

      order = level_growth_to_order  ( 1, level, rule(dim), growth(dim) );

      if ( rule(dim) == 1 )
        points = clenshaw_curtis_compute_points ( order );
      elseif ( rule(dim) == 2 )
        points = fejer2_compute_points ( order );
      elseif ( rule(dim) == 3 )
        points = patterson_lookup_points ( order );
      elseif ( rule(dim) == 4 )
        points = legendre_compute_points ( order );
      elseif ( rule(dim) == 5 )
        points = hermite_compute_points ( order );
      elseif ( rule(dim) == 6 )
        alpha = p(p_index);
        points = gen_hermite_compute_points ( order, alpha );
      elseif ( rule(dim) == 7 )
        points = laguerre_compute_points ( order );
      elseif ( rule(dim) == 8 )
        alpha = p(p_index);
        points = gen_laguerre_compute_points ( order, alpha );
      elseif ( rule(dim) == 9 )
        alpha = p(p_index);
        beta = p(p_index+1);
        points = jacobi_compute_points ( order, alpha, beta );
      elseif ( rule(dim) == 10 )
        points = hermite_genz_keister_lookup_points ( order );
      elseif ( rule(dim) == 11 )
        fprintf ( 1, '\n' );
        fprintf ( 1, 'SGMGA_UNIQUE_INDEX - Fatal error!\n' );
        fprintf ( 1, '  Do not know how to assign points for rule 11.\n' );
        error ( 'SGMGA_UNIQUE_INDEX - Fatal error!' );
      elseif ( rule(dim) == 12 )
        fprintf ( 1, '\n' );
        fprintf ( 1, 'SGMGA_UNIQUE_INDEX - Fatal error!\n' );
        fprintf ( 1, '  Do not know how to assign points for rule 12.\n' );
        error ( 'SGMGA_UNIQUE_INDEX - Fatal error!' );
      else
        fprintf ( 1, '\n' );
        fprintf ( 1, 'SGMGA_UNIQUE_INDEX - Fatal error!\n' );
        fprintf ( 1,'  Unexpected value of RULE = %d\n', rule(dim) );
        error ( 'SGMGA_UNIQUE_INDEX - Fatal error!' );
      end

      index = find ( sparse_total_order(dim,1:point_total_num) == order );

      sparse_total_point(dim,index) = points ( sparse_total_index(dim,index) );

    end

    p_index = p_index + np(dim);

  end
%
%  Merge points that are too close.
%
  seed = 123456789;

  [ point_num, undx, sparse_unique_index, seed ] = ...
    point_radial_tol_unique_index ( dim_num, point_total_num, ...
    sparse_total_point, tol, seed );

  for point = 1 : point_total_num
    rep = undx(sparse_unique_index(point));
    if ( point ~= rep )
      sparse_total_point(1:dim_num,point) = sparse_total_point(1:dim_num,rep);
    end
  end
%
%  Construct an index that indicates the "rank" of the unique points.
%
  [ undx, sparse_unique_index ] = point_unique_index ( dim_num, ...
    point_total_num, sparse_total_point, point_num );

  return
end


function [ res ] = bellman_residual(Qt,Qtmp)
%BELLMAN_RESIDUAL Computes the Bellman residual
%
%   input -------------------------------------------------------
%
%       o Q: Q-value function at time t
%
%       o Qtmp: Q-value function at time t-1
%

vt   = compute_value_function(Qt);
vtmp =  compute_value_function(Qtmp);

res = (vt - vtmp).^2;
res = sum(res(:)) / size(vt(:),1);

end

function v = compute_value_function(nfq_Q)

u           = BuildActionList();
num_actions = 2;
nbSamples   = 100;
xs          = linspace(-1,1,nbSamples);
[Xs,Ys]     = meshgrid(xs,xs);
test        = [Xs(:),Ys(:)];
vs          = zeros(size(test,1),num_actions);

M           = size(test,1);

if iscell(nfq_Q)
    for i=1:num_actions
        vs(:,i) = nfq_Q{i}.f(test);
    end
else
    for i=1:num_actions
        test_ui = [test,repmat(u(i),M,1)];
        vs(:,i) = nfq_Q.f(test_ui);
    end
end

v          = min(vs,[],2);

end


function h=semiaudplot(x,y,varargin)
%SEMIAUDPLOT  2D plot on auditory scale
%   Usage: h=semiaudplot(x,y);
%
%   `semiaudplot(x,y)` plots the data $(x,y)$ on an auditory scale. By
%   default the values of the x-axis will be shown on the Erb-scale.
%
%   `semiaudplot` takes the following parameters at the end of the line of input
%   arguments:
%
%     'x'       Make the x-axis use the auditory scale. This is the default.
%
%     'y'       Make the y-axis use the auditory scale.
%
%     'opts',c  Pass options stored in a cell array onto the plot
%               function.
%
%   In addition to these parameters, the auditory scale can be
%   specified. All scales supported by |freqtoaud| are supported. The default
%   is to use the erb-scale.
%
%   See also: freqtoaud

if nargin<2
  error('%s: Too few input parameters.',upper(mfilename));
end;

definput.import       = {'ltfattranslate','freqtoaud'};
definput.flags.plotdir= {'x','y'};
definput.keyvals.tick = [0,100,250,500,1000,2000,4000,8000,16000];
definput.keyvals.res  = 500;
definput.keyvals.opts = {};

[flags,kv]=ltfatarghelper({},definput,varargin);

n=500;
tickpos=freqtoaud(kv.tick,flags.audscale);

if flags.do_x
  xmin=min(x);
  xmax=max(x);
  audminmax=freqtoaud([xmin,xmax],flags.audscale);

  plotval=spline(x,y,audspace(xmin,xmax,n,flags.audscale));
  plot(linspace(audminmax(1),audminmax(2),n),plotval,kv.opts{:});
  set(gca,'XTick',tickpos);
  set(gca,'XTickLabel',num2str(kv.tick(:)));
  xlabel(sprintf('%s (Hz)',kv.frequency));

end;

if flags.do_y
  ymin=min(y);
  ymax=max(y);
  audminmax=freqtoaud([ymin,ymax],flags.audscale);

  plot(x,freqtoerb(y),kv.opts{:});
  set(gca,'YTick',tickpos);
  set(gca,'YTickLabel',num2str(tick(:)));

  ylabel(sprintf('%s (Hz)',kv.frequency));
end;


function [A,B, S, err] = wls(x,y)
    %WEIGHTED LINEAR LEAST SQUARES REGRESSION
    %	[A, B, S, err] = WLS(x,y) finds the A and B coefficients of A log cumalative frequency
    %curve and the error.
    %      A, B: a- and b- values of a weighted linear regression fit.
    %    err: estimate of the std deviation of the error in predicting a future observation at X by A and B
    %      S: contains fields for triangular factor(R) from QR decomp... see polyfit (used in polyval)
    %
    %report_this_filefun();
    %partially vectorized version

%    global S % output of POLYFIT used for error estimates
    %mima = min(x);

    S=[];
    err=inf;

    if any(size(x) ~= size(y))
        error('X and Y vectors must be the same size.')
    end
    x = x(:);
    y = y(:);
    l = isinf(y);
    y(l) = [];
    x(l) = [];
    % weight the values
    teny= 10.^y;
    wx = ones(1,ceil(sum(teny)));
    wy = wx;
    fteny=floor(teny);
    ks = cumsum([1; fteny]);
    for i = 1:length(x)
        wx(ks(i):ks(i+1)-1) = wx(ks(i):ks(i+1)-1) * x(i);
        wy(ks(i):ks(i+1)-1) = wy(ks(i):ks(i+1)-1) * teny(i);
    end
    %x = wx;
    %y = log10(wy);

    l = wx  > min(x);%mima;

    %[B, A,err ] = ma(x',y');
    %b2 = -abs(B);

    if sum(l) <= 5
        p = [NaN NaN] ;
    elseif nargout > 2
        [p,S] = polyfit(wx(l),log10(wy(l)),1);
        if nargout==4 && S.df > 0
            [~,err] = polyval(p,wx,S);
            err = mean(err);
        end
    else
        p = polyfit(wx(l),log10(wy(l)),1);
    end
    A = p(2);
    B = p(1) ;
end


function [W V] = genhull(U, wtype)
%GENHULL Generate "hull" matrices from orthonormal matrices
%	W = GENHULL(U)
%	W = GENHULL(U, wtype)
%	[W V] = GENHULL(U)
%	[W V] = GENHULL(U, wtype)
%
%	U     - matrices in a cell with orthonormal column vectors (U{i}' * U{i} = I)
%	wtype - weight function type for each matrix (default: 'snnn')
%
%	W     - "hull" matrices (weight functions)
%	V     - vertex matrices (W{i}*V{i} = U{i})
%
%	wtype parameter controls the type of the column vectors in the resulting
%	W matrices.
%	"hull" matrix means that the row sums are 1 and each element is positive
%		ie.: W{i}*ones == ones && all(all(W{i} >= 0)) &&
%		     exists V{i} such that W{i}*V{i} = U{i}
%
%	possible values of wtype:
%		'eye': return eye(size(U{i},1))
%		'ortho': keep as is: W{i} = U{i} (not hull matrix!)
%		'snnn': general non-negative sum-normalized (convex combination)
%		'cno': 'snnn' and each weight function comes close to 1 (tight hull)
%		'irno': 'snnn' and each weight function comes close to 0
%		'box': perform box_decomp
%
%	See also HOSVD, DECOMP.

if nargin <= 1
	% TODO
	wtype = 'close';
end

if ~iscell(U)
	% only one matrix
	if nargout < 2
		W = decomp(U, wtype);
	else
		[W V] = decomp(U, wtype);
	end
	return
end

if ~iscell(wtype)
	tmp = wtype;
	wtype = cell(1,length(U));
	for i = 1:length(U)
		wtype{i} = tmp;
	end
end

% TODO: allow skipping last few U?
%assert(length(U) == length(wtype));

W = cell(1,length(wtype));
if nargout < 2
	for i = 1:length(wtype)
		if isempty(U{i})
			W{i} = [];
		else
			W{i} = decomp(U{i}, wtype{i});
		end
	end
else
	V = cell(1,length(wtype));
	for i = 1:length(wtype)
		if isempty(U{i})
			W{i} = [];
			V{i} = [];
		else
			[W{i} V{i}] = decomp(U{i}, wtype{i});
		end
	end
end


% MatrixUser, a multi-dimensional matrix analysis software package
% https://sourceforge.net/projects/matrixuser/
%
% The MatrixUser is a matrix analysis software package developed under Matlab
% Graphical User Interface Developing Environment (GUIDE). It features
% functions that are designed and optimized for working with multi-dimensional
% matrix under Matlab. These functions typically includes functions for
% multi-dimensional matrix display, matrix (image stack) analysis and matrix
% processing.
%
% Author:
%   Fang Liu <leoliuf@gmail.com>
%   University of Wisconsin-Madison
%   Aug-30-2014

function MU_funcSurfl(Temp,Event,handles)
handles = guidata(handles.MU_matrix_display);

figure;
surfl(double(handles.BMatrix));
shading interp;
colormap(handles.V.Color_map);

end


clear; close all; clc;

% vector field

[x,y]=meshgrid(-2:0.3:2,-2:.3:2);

u=x.^2;
v=y;

quiver(x,y,u,v)

grid on;
xlabel('x');
ylabel('y');
set(gca,'fontsize',15);


function combo_test07 ( )

%*****************************************************************************80
%
%% COMBO_TEST07 tests I4_FACTORIAL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 January 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'COMBO_TEST07:\n' );
  fprintf ( 1, '  I4_FACTORIAL evaluates the factorial function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     X       Exact F       FACTORIAL(X)\n' );
  fprintf ( 1, '\n' );

  n = 0;

  while ( 1 )

    [ n, x, fx ] = i4_factorial_values ( n );

    if ( n == 0 )
      break
    end

    if ( x <= 0.0 )
      continue
    end

    fx2 = i4_factorial ( x );

    fprintf ( 1, '  %4d  %12d  %12d\n', x, fx, fx2 );

  end

  return
end


function anim = nrsfmXiaoKanade( W, nBasis )
% Compute orthographic non-rigid structure from motion using Xiao-Kanade
%
% Just check the Xiao's CVPR04 paper
%
% USAGE
%  anim = nrsfmXiaoKanade( W, nBasis )
%
% INPUTS
%  W             - [ 2 x nPoint x nFrame ] set of 2D points
%  nBasis        - {Computed } number of bases to use
%
% OUTPUTS
%  anim          - Animation object (help Animation for details)
%
% EXAMPLE
%
% See also COMPUTESMFROMW
%
% Vincent's Structure From Motion Toolbox      Version 3.0
% Copyright (C) 2008-2010 Vincent Rabaud.  [vrabaud-at-cs.ucsd.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the GPL [see external/gpl.txt]

nFrame = size( W, 3 ); nPoint = size( W, 2 );
animBest=Animation; animBest.W = W;

W = reshape( permute( W, [ 1 3 2 ] ), [], nPoint );
animBest.t = reshape( mean(W,2), 2, nFrame ); animBest.t(3,:) = 0;
W = bsxfun(@minus,W,mean(W,2));

if nargin<2 || isempty(nBasis)
  [ U S V ] = svd(W,'econ'); S = diag(S); sumVal = S(1); Kd = 1;
  while ( Kd<=length(S) ) && ( sumVal < 0.995*sum(abs(S)) )
    if S(Kd)<0; break; end
    Kd = Kd + 1; sumVal = sumVal + S(Kd);
  end
  W = U(:,1:Kd)*diag(S(1:Kd))*V(:,1:Kd)';
else
  Kd = 3*nBasis;
  % Check if Kd is not smaller (degenerate deformations)
  [ U S V ] = svd(W,'econ');
  S=cumsum(diag(S));
  ind = find(S>=0.995*S(end));
  Kd=min(Kd,ind(1));
end

%%%%%%%%%%%%%%%%%%%%%% Determine K3 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
K3 = floor(Kd/3);
% Basis constraints
WTildeOri = [ W(1:2:end,:) W(2:2:end,:) ];

while 1
  % Perform random search for 5 seconds
  t0 = clock; mini = Inf;
  while etime( clock, t0 )<5
    indWTmp = randSample(nFrame,K3);
    tmp = cond( WTildeOri( indWTmp, : ) );
    if tmp < mini; mini = tmp; indW = indWTmp; end
  end

  if mini>1e2 && K3>0; K3=K3-1; else break; end
end

%%%%%%%%%%%%%%%%%%%%%% Deal with the K3 basis %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute initial variables
[ MTilde S disc ] = svd( W );
MTilde = MTilde( :, 1:Kd )*S(1:Kd,1:Kd); gj3=zeros(Kd,3,K3);

% Rotation constraints
Qi = sdpvar( Kd, Kd );
F = set( Qi >= 0 );
% vect(AXB)=kron(B',A)*vect(X)
kron1=zeros(nFrame,Kd^2); kron2=zeros(nFrame,Kd^2);
for i=1:nFrame
  kron1(i,:)=kron(MTilde(2*i-1,:),MTilde(2*i-1,:)) - ...
    kron(MTilde(2*i,:),MTilde(2*i,:));
  kron2(i,:)=kron(MTilde(2*i,:),MTilde(2*i-1,:));
end
objRotation=abs(kron1*Qi(:))+abs(kron2*Qi(:));

% Solve for Qk using all the constraints
kk = 1;
for i = indW
  nc = 2*nFrame + 1;
  objBasis = 0;
  disp('Computing the basis constraints');
  for m = indW
    if m==i; interN = m; else interN = 1 : nFrame; end
    for n = interN % could be sampled
      if m==n && m==i
        objBasis = objBasis +abs(MTilde(2*m-1,:)*Qi*MTilde(2*n-1,:)'-1) ...
          + abs( MTilde(2*m,:)*Qi*MTilde(2*n,:)' - 1);
      else
        objBasis = objBasis +abs(MTilde(2*m-1,:)*Qi*MTilde(2*n-1,:)') ...
          + abs( MTilde(2*m,:)*Qi*MTilde(2*n,:)' );
      end

      objBasis = objBasis + abs( MTilde(2*m-1,:)*Qi*MTilde(2*n,:)' ) + ...
        abs( MTilde(2*m,:)*Qi*MTilde(2*n-1,:)' );
    end
  end

  % Sample the constraints
  disp('Solving the SDP');
  diagno = solvesdp( F,objRotation+objBasis,sdpsettings('solver',...
    'sdpa,csdp,sedumi,*','dualize',1,'verbose',0));

  QiTmp = double(Qi);

  [ U S disc ] = svd( QiTmp ); gj3(:,:,kk) = U(:,1:3)*sqrt(S(1:3,1:3));
  kk = kk + 1;
end

%  [U S V]=svd(constr,'econ');
%  rank(constr)
%  size(null(constr(1:2*nFrame,:)))
%  size(constr)
%  tmp=diag(S)';
%  tmp(end-3:end)
%  size(null(constr),2)
%  return
%%%%%%%%%%%%%%%%%%%%%% Get K2 and K1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% The explanations given by Xiao04 seems wrong to determine K1 and K2
%% So, let's do it the lazy way: let's try all possible combinations of K1
%% and K2
errBest = Inf;
fprintf('\nSo far, Kd=%d, K3=%d\n', Kd, K3 );
for K2 = floor((Kd-3*K3)/2) : -1 : 0
  K1 = Kd - 3*K3 - 2*K2;
  K = K3+K2+K1;

  fprintf('\nTrying, Kd=%d, K1=%d, K2=%d, K3=%d\n', Kd, K1, K2, K3 );

  %%%%%%%%%%%%%%%%%%%%%% Get the sets of rotations %%%%%%%%%%%%%%%%%%%%%%%%
  G = reshape( gj3, Kd, [] );
  M = MTilde*G; M = M/max(abs(M(:)))*K3; % Just for numerical stability
  R = zeros( 2*nFrame, 3*K3 ); l = zeros( K, nFrame ); thres = 0.001;
  rotIsBad = zeros( K3, nFrame );
  for i = 1 : nFrame
    for k = 1 : K3
      tmp = M(2*i-1:2*i,3*k-2:3*k);
      l(k,i) = norm(tmp);

      if abs( l(k,i) )>thres; R(2*i-1:2*i,3*k-2:3*k) = tmp/l(k,i);
      else rotIsBad(k,i)=1;
      end
    end
  end

  % Rectify R (not really mentioned by Xiao)
  for k=2:K3
    for i = 2:nFrame
      goodFrame = find( sum( rotIsBad([1 k], 1:i), 1 )==0);
      goodFrame = sort( [ 2*goodFrame 2*goodFrame-1 ] );
      if length(goodFrame)<4; continue; end
      R1 = R( goodFrame, 1:3 );
      R2 = R( goodFrame, 3*k-2:3*k );
      R3 = R2; R3(end-1:end,:) = -R3(end-1:end,:);

      if norm( R2*(R2\R1)-R1, 'fro' )>norm( R3*(R3\R1)-R1, 'fro' )
        R( goodFrame(end-1:end), 3*k-2:3*k ) = ...
          -R( goodFrame(end-1:end), 3*k-2:3*k );
      end
    end

    goodFrame = find( sum( rotIsBad([1 k], 1:nFrame), 1 )==0);
    goodFrame = sort( [ 2*goodFrame 2*goodFrame-1 ] );
    R1 = R( goodFrame, 1:3 );
    R2 = R( goodFrame, 3*k-2:3*k );
    tmp=R2\R1;

    R( :, 3*k-2:3*k ) = R( :, 3*k-2:3*k )*tmp;
    M( :, 3*k-2:3*k ) = M( :, 3*k-2:3*k )*tmp;
  end

  % Get all the rotations and coefficients in K3 (if non-degenerate)
  RTot = zeros(3,3,nFrame);
  for i = 1 : nFrame
    % Get the average rotation matrix
    tmp=find(~rotIsBad(:,i)');
    if isempty(tmp); continue; end
    RTmp=zeros(3,3,length(tmp));

    qTot=cell(1,2);
    for k=1:2
      kk=1;
      for j=tmp
        RTmp(:,:,kk)=rotationMatrix( R(2*i-1:2*i,3*j-2:3*j)*(2*k-3) );
        kk=kk+1;
      end

      q=quaternion(RTmp);
      for j=2:size(q,2)
        if norm(-q(:,j)-q(:,1))<norm(q(:,j)-q(:,1)); q(:,j)=-q(:,j); end
      end
      qTot{k}=mean(q,2);
    end

    q=qTot{1};
    if i>=2
      q0 = quaternion( RTot(:,:,i-1) );
      if min( norm(qTot{2}-q0), norm(-qTot{2}-q0) )<...
          min( norm(qTot{1}-q0), norm(-qTot{1}-q0) )
        q=qTot{2};
      end
    end

    RTot(:,:,i) = quaternion(q);

    % Recover the coefficients
    l(1:K3,i)=(reshape(RTot(1:2,:,i),[],1)\reshape(M(2*i-1:2*i,1:3*K3),6,K3))';

    % Compute MTilde
    MTilde(2*i-1:2*i,1:3*K3)=kron(l(1:K3,i)',RTot(1:2,:,i));
  end
  for k = 1 : K3
    gj3(:,:,k) = 0; gj3(3*k-2:3*k,:,k) = eye(3);
  end
  R = RTot;
  rotIsUnknown = sum(abs(l(1:K3,:))<thres,1)==K3;
  if sum(~rotIsUnknown)==0
    [disc indMax]=max(sum(l(1:K3,:).^2,1)); rotIsUnknown(indMax)=1;
  end
  goodFrame = find( ~rotIsUnknown );
  tmp=sort([2*goodFrame,2*goodFrame-1]);

  % Get the MTilde where l(1:K3,i) is close to 0
  M=MTilde(tmp,:); B=M\W(tmp,:); M2=W/B;
  for i=find(rotIsUnknown)
    MTilde(2*i-1:2*i,:)=M2(2*i-1:2*i,:);
  end

  %   find(rotIsUnknown)
  %   plot(l')
  %     B=MTilde\W;
  %   norm(MTilde*B-W,'fro')
  %   pause

  % The first 3*K3 columns of MTilde and MHat are now identical
  % We now need to deal with the last columns

  %%%%%%%%%%%%%%%%%%%%%% Find gj and rj %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % We here perform a full optimization and not the hacky alternate
  % optimization from the paper
  MNull = zeros( 2*K3, Kd );
  for i = 1 : K3
    MNull(2*i-1:2*i,:) = MTilde( 2*indW(i)-1:2*indW(i), : );
  end
  [ U S V ] = svd( MNull, 'econ' );
  [ V disc ] =rq(V);
  gj2 = reshape( V(:,end-(2*K2+K1)+1:end-K1), [], 2, K2);rj2=zeros(3,2,K2);
  gj1 = V(:,end-K1+1:end); rj1=zeros(3,K1);

  % Perform iterations for K2 bases
  opt = optimset( 'Display', 'on', 'GradObj', 'off', 'Hessian', 'off',...
    'LargeScale', 'off' );
  for j=1:K2
    % Get the best rj2
    ATmp=zeros(4*length(goodFrame),3);
    for kk=1:2
      n = 1;
      for m=goodFrame
        for k=1:2
          ATmp(n,:)=gj2(:,k,j)'*MTilde(2*m-1,:)'*R(2,:,m) - ...
            gj2(:,k,j)'*MTilde(2*m,:)'*R(1,:,m);
          n = n + 1;
        end
      end

      % Force the rj2 to be independent
      zeroStart=2+kk;
      [ U S V ] = svd( ATmp(:,1:zeroStart-1), 'econ' );
      rj2(1:zeroStart-1,kk,j) = V(:,end)/V(end,end);
      rj2(zeroStart:end,kk,j) = 0;
    end

    % Perform the full optimization
    onePos=3*K3+1+2*(j-1);
    x=[gj2(1:onePos-1,1,j); gj2(1:onePos,2,j); rj2(1:2,1,j); rj2(1:2,2,j)];
    MTildeTmp= MTilde(:,1:onePos+1); MNullTmp=MNull(:,1:onePos+1);
    x = fminunc( @(X)( optimizeGj2( X,goodFrame, MTildeTmp, ...
      R, MNullTmp ) ), x,opt);

    % Save the best results
    rj2(:,:,j)=[x(end-3:end-2) x(end-1:end); 0 1 ];
    gj2(:,:,j)=0;
    gj2(1:onePos,1,j) = [ x(1:onePos-1); 1 ];
    gj2(1:onePos+1,2,j) = [ x(1:onePos); 1 ];
  end

  % Perform iterations for K1 bases
  for j=1:K1
    % Get the best rj1
    ATmp=zeros(length(goodFrame),3);
    for m=goodFrame
      ATmp(m,:) = gj1(:,j)'*( MTilde( 2*m-1 , : )'*R(2,:,m) - ...
        MTilde( 2*m , : )'*R(1,:,m) );
    end

    [ U S V ] = svd( ATmp, 'econ' );
    rj1(:,j) = V(:,end);

    % Perform the full optimization
    onePos=3*K3+2*K2+j;
    x=[gj1(1:onePos-1,j); rj1(:,j) ];
    MTildeTmp= MTilde(:,1:onePos); MNullTmp=MNull(:,1:onePos);
    x = fminunc( @(X)( optimizeGj1( X,goodFrame, MTildeTmp, ...
      R, MNullTmp ) ), x,opt);

    % Save the best results
    rj1(:,j)=x(end-2:end);
    gj1(:,j)=0; gj1(1:onePos,j) = [ x(1:onePos-1); 1 ];
  end

  % % quality
  % err=zeros(1,length(goodFrame));
  % for j = 1:K2
  %   for m=goodFrame
  %     err(m)=err(m)+abs(gj2(:,1,j)'*( MTilde( 2*m-1 , : )'*R(2,:,m) - ...
  %       MTilde( 2*m , : )'*R(1,:,m) )*rj2(:,1,j)) ...
  %        + abs(gj2(:,2,j)'*( MTilde( 2*m-1 , : )'*R(2,:,m) - ...
  %       MTilde( 2*m , : )'*R(1,:,m) )*rj2(:,2,j));
  %   end
  % end
  %       norm(err)
  % %       norm(MNull*gj1(:,j))
  %       plot(err)
  %
  % pause

  %%%%%%%%%%%%%%%%%%%%%% Recover the coefficients for K2 K1 %%%%%%%%%%%%%%%
  % Rectify the right columns of MTilde
  for m=1:nFrame
    for i = 1 : K2
      MTilde(2*m-1:2*m,3*K3+2*i-1:3*K3+2*i)=MTilde(2*m-1:2*m,:)*gj2(:,:,i);
    end
    for i = 1 : K1
      MTilde(2*m-1:2*m,3*K3+2*K2+i)=MTilde(2*m-1:2*m,:)*gj1(:,i);
    end
  end
  %   l(K3+i,m)*R(1:2,:,m)*rj2(:,:,i)=
  %                   MTilde(2*m-1:2*m,3*K3+2*i-1:3*K3+2*i)*RAmbiguityi
  %   l(K3+i,m)*R(1:2,:,m)*rj1(:,i)=
  %                   MTilde(2*m-1:2*m,3*K3+2*K2+i)
  for m=goodFrame
    for i = 1 : K2
      tmp1=R(1:2,:,m)*rj2(:,:,i); tmp1=tmp1*tmp1';
      tmp2=MTilde(2*m-1:2*m,3*K3+2*i-1:3*K3+2*i); tmp2=tmp2*tmp2';
      l(K3+i,m) = sqrt( tmp1(:)\tmp2(:) );
    end
    for i = 1 : K1
      tmp1=R(1:2,:,m)*rj1(:,i);
      tmp2=MTilde(2*m-1:2*m,3*K3+2*K2+i);
      l(K3+K2+i,m) = tmp1\tmp2;
    end
  end

  %   B=MTilde\W;
  %   norm(MTilde*B-W,'fro')
  %   plot(l')
  %   pause

  % Change the signs of the coefficients
  for i = 1 : K2
    A=zeros(2*nFrame,2);
    A(1:2,:)=l(K3+i,goodFrame(1))*R(1:2,:,goodFrame(1))*rj2(:,:,i);

    for j=2:length(goodFrame)
      m=goodFrame(j); A(2*j-1:2*j,:)=l(K3+i,m)*R(1:2,:,m)*rj2(:,:,i);
      tmp=goodFrame(1:j); tmp=sort( [ 2*tmp-1, 2*tmp ] );
      MTildeLoc=MTilde(tmp,3*K3+2*i-1:3*K3+2*i);

      RAmb1=MTildeLoc\A(1:2*j,:);
      err1=norm(MTildeLoc*RAmb1-A(1:2*j,:),'fro');

      A(2*j-1:2*j,:)=-A(2*j-1:2*j,:);
      RAmb2=MTildeLoc\A(1:2*j,:);
      err2=norm(MTildeLoc*RAmb2-A(1:2*j,:),'fro');

      if err2<err1; l(K3+i,m)=-l(K3+i,m);
      else A(2*j-1:2*j,:)=-A(2*j-1:2*j,:);
      end
    end
  end

  B=MTilde\W;
  %   norm(MTilde*B-W,'fro')
  %   plot(l')
  %   pause

  %%%%%%%%%%%%%%%%%%%%%% Recover the shape basis %%%%%%%%%%%%%%%%%%%%%%%%%%
  M=zeros(2*length(goodFrame),3*K);
  for i=1:length(goodFrame)
    M(2*i-1:2*i,:)=kron(l(:,goodFrame(i))',R(1:2,:,goodFrame(i)));
  end

  tmp=sort([2*goodFrame,2*goodFrame-1]);
  B=M\W(tmp,:);
  %   norm(M*B-W(tmp,:),'fro')
  %   tmp=sum((M*B-W(tmp,:)).^2,2);
  %   plot( tmp(1:2:end)+tmp(2:2:end) )
  %   'coiin'
  %   pause

  % Recover the full basis
  SBasis = zeros( 3, nPoint, K );
  for i = 1 : K; SBasis(:,:,i) = B(3*i-2:3*i,:); end

  %%%%%%%%%%%%%%%%%%%%%% Recover the missing coefficients %%%%%%%%%%%%%%%%%
  opt = optimset( 'Display', 'off', 'GradObj', 'off', 'Hessian', 'off',...
    'LargeScale', 'off' );

  while 1
    for m=1:nFrame
      if rotIsUnknown(m)
        if m==1 || rotIsUnknown(m-1)
          if m==nFrame; continue
          else if ~rotIsUnknown(m+1); iIni=m+1; else continue; end
          end
        else iIni=m-1;
        end

        % Optimize over the shape coefficients/rotation
        coeff=[ l(:,iIni); quaternion( R(:,:,iIni) ) ];
        Wm=W(2*m-1:2*m,:);

        [ c res ] = fminunc( @(X)( optimRl( X, K,B,Wm ) ), coeff, opt );

        l(:,m)=c(1:K); R(:,:,m)=quaternion(c(K+1:end));
        rotIsUnknown(m)=0;
      else
        if m>=2
          %           plot(quaternion(R)')
          %           pause
          q0 = quaternion( R(:,:,m-1) );
          q1 = quaternion( R(:,:,m) );
          q2 = quaternion( rotationMatrix( -R(1:2,:,m) ) );
          if min( norm(q2-q0), norm(-q2-q0) )<min(norm(q1-q0),norm(-q1-q0))
            l(:,m)=-l(:,m); R(:,:,m)=rotationMatrix(-R(1:2,:,m));
          end
        end
      end
    end
    if ~rotIsUnknown(1); break; end
  end

  %%%%%%%%%%%%%%%%%%%%%% Finalize the anim object %%%%%%%%%%%%%%%%%%%%%%%%%
  anim=Animation('SBasis', SBasis, 'l', l, 'R', R, 'isProj', false, ...
    't', animBest.t, 'W', animBest.W );
  err = anim.computeError(); err=err(1);
  fprintf( 'Reprojection error: %f \n', err );

  if err<errBest; errBest=err; animBest=anim; end
end
anim=animBest;

fprintf( 'Best reprojection error: %f \n', errBest );
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function res = optimRl(X,K,B,Wm)
lm=X(1:K);
Rm=quaternion(X(K+1:end)/norm(X(K+1:end)));
res = norm(kron(lm',Rm(1:2,:))*B-Wm,'fro')^2;
end

function val = optimizeGj2( x, goodFrame, MTilde, R, MNull )
% gjk' MTilde_{2m-1}' R_m2 rjkk - gjk' MTilde_2m' R_m1 rjkk = 0,
% k and kk in {1 2}
% rj can be multiplied by any 2x2
rj2=[x(end-3:end-2) x(end-1:end); 0 1 ]; rj2(:,1)=rj2(:,1)/norm(rj2(:,1));
n=(length(x)-4-1)/2;
gj2=[ [ x(1:n); 1; 0 ] [ x(n+1:2*n+1); 1 ] ];
n=n+2;

n=1;
val=zeros(1,length(goodFrame));
A=gj2'*MTilde';
for m=goodFrame
  val(n)=norm( ( A(:,2*m-1,:)*R(2,:,m) - A(:,2*m)*R(1,:,m) )*rj2, 'fro' );
  n=n+1;
end
val=norm([ val reshape(MNull*gj2,[],1)' ]);
end

function val = optimizeGj1( x, goodFrame, MTilde, R, MNull )
% gj' ( MTilde_{2m-1} R_m2 - MTilde_2m R_m1 ) rj = 0
gj1=[ x(1:end-3); 1 ];
rj1=x(end-2:end); rj1=rj1/norm(rj1);

n=1;
A=gj1'*MTilde';
B=zeros(length(goodFrame),3);
for m=goodFrame
  B(n,:)=A(:,2*m-1)*R(2,:,m) - A(:,2*m)*R(1,:,m);
  n=n+1;
end
val=norm([ (B*rj1); reshape(MNull*gj1,[],1) ]);

end


%% Experiment with the cnn_mnist_fc_bnorm

[net_bn, info_bn] = cnn_mnist(...
  'expDir', 'data/mnist-bnorm', 'useBnorm', true);

[net_fc, info_fc] = cnn_mnist(...
  'expDir', 'data/mnist-baseline', 'useBnorm', false);

figure(1) ; clf ;
subplot(1,2,1) ;
semilogy(info_fc.val.objective', 'o-') ; hold all ;
semilogy(info_bn.val.objective', '+--') ;
xlabel('Training samples [x 10^3]'); ylabel('energy') ;
grid on ;
h=legend('BSLN', 'BNORM') ;
set(h,'color','none');
title('objective') ;
subplot(1,2,2) ;
plot(info_fc.val.error', 'o-') ; hold all ;
plot(info_bn.val.error', '+--') ;
h=legend('BSLN-val','BSLN-val-5','BNORM-val','BNORM-val-5') ;
grid on ;
xlabel('Training samples [x 10^3]'); ylabel('error') ;
set(h,'color','none') ;
title('error') ;
drawnow ;


function W=RegCsp(EEGdata,gnd,genSs,genMs,beta,gamma)
% RCSP: Regularized Common Spatial Pattern
%
% %[Prototype]%
% function W=RegCsp(EEGdata,gnd,genSs,genMs,beta,gamma)
%
% %[Author Notes]%
% Author: Haiping LU
% Email : hplu@ieee.org   or   eehplu@gmail.com
% Release date: March 20, 2012 (Version 1.0)
% Please email me if you have any problem, question or suggestion
%
% %[Algorithm]%:
% This function implements the Regularized Common Spatial Pattern
% (R-CSP) algorithm presented in the follwing paper:
%    Haiping Lu, How-Lung Eng, Cuntai Guan, K.N. Plataniotis, and A.N. Venetsanopoulos,
%    "Regularized Common Spatial Pattern With Aggregation for EEG Classification in Small-Sample Setting",
%    IEEE Trans. on Biomedical Engineering, Vol. 57, No. 12, Pages
%    2936-2946, Dec. 2010.
% Please reference this paper when reporting work done using this code.
%
% The following is an earlier conference version
%    Haiping Lu, K.N. Plataniotis, and A.N. Venetsanopoulos,
%    "Regularized Common Spatial Patterns with Generic Learning for EEG Signal Classification",
%    in Proceedings of the 31st Annual International Conference of the
%    IEEE Engineering in Medicine and Biology Society (EMBC), Sep., 2009.
%
% %[Syntax]%: W=RegCsp(EEGdata,gnd,genSs,genMs,beta,gamma)
%
% %[Inputs]%:
%    EEGdata: the input EEG data for training, a 3D array with size [numT,numCh,nTrl]
%       numT is the number of samples in each channle (T in the paper)
%       numCh is the number of channels (N in the paper)
%       nTrl is the number of trails for training (2*M in the paper)
%
%    gnd: the ground truth class labels for the nTrl trials, size nTrl x 1
%
%    genSs: size numCh x numCh x 2, the sum of generic covariance matrices
%       from generic training trials for two classes, equation (6) in the paper
%
%    genMs: size 2 x 1, the number of generic training trials for two classes
%
%    beta,gamma: the reguarlizatio parameters in the paper, setting both to
%       zero gives the conventional CSP
%
% %[Outputs]%:
%    W: \hat{W} in the paper, the projection matrix, we often use only the
%         first 6 columns from W
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %[Notes]%:
% A. Developed using Matlab R2006a
% B. Revision history:
%       Version 1.0 released on March 20, 2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%
[numT,numCh,nTrl] = size(EEGdata);%Input data, see documentation
Ns=zeros(2,1);
Sigma2=cell(2,1);%Two average spatial covariance matrices, one for each class
%%%%%%%%%%%%%%%%%%%
for i=1:2%for each class
    Idxs=find(gnd==i);
    EEG=EEGdata(:,:,Idxs);
    Ns(i)=length(Idxs);
    C=zeros(numCh,numCh,Ns(i));%Sample covariance matrix, equation (1) in the paper
    for trial=1:Ns(i)
        E=EEG(:,:,trial)';
        tmpC = (E*E');
        C(:,:,trial) = tmpC./trace(tmpC);%normalization
    end
    Csum=sum(C,3);
    %Reguarlization, see equations (3) and (4) in the paper
    Sigma1=((1-beta)*Csum+beta*genSs(:,:,i))/((1-beta)*Ns(i)+beta*genMs(i));
    Sigma2{i}=(1-gamma)*Sigma1+gamma*trace(Sigma1)*eye(numCh)/numCh;
end

%Equation (7) in the paper
SigmaComps=Sigma2{1}+Sigma2{2};
[Ucomps,lmds] = eig(SigmaComps);
[lmds,Idxs] = sort(diag(lmds),'descend');
Ucomps = Ucomps(:,Idxs);

%Note equations (8) to (12) in the conference version is now condensed in
%one equation (8) in the journal version
%Equation (8) in the CONFERENCE paper
P=sqrt(inv(diag(lmds)))*Ucomps';

%Equation (9) in CONFERENCE the paper
Sgm1=P*Sigma2{1}*P';

%Equation (11) in the CONFERENCE paper
[B,D] = eig(Sgm1);
[D,Idxs] = sort(diag(D),'descend');
B = B(:,Idxs);

%Equation (12) in the CONFERENCE paper
%Equation (8) in the JOURNAL paper
W=(B'*P);
%Normalize the projrection matrix
for i=1:length(Idxs), W(i,:)=W(i,:)./norm(W(i,:)); end

%Sort columns, take first and last columns first, etc
W0=W;
W=zeros(size(W));
i=0;
for d=1:numCh
    if (mod(d,2)==0)
        W(d,:)=W0(numCh-i,:);
        i=i+1;
    else
        W(d,:)=W0(1+i,:);
    end
end


function [A,B,C,D] = vibsHmxBlockOperator(omega,U,sigma,u,cL,cT,rhoS,c0,rho0,f,tol)
%+========================================================================+
%|                                                                        |
%|                 OPENVIBS - LIBRARY FOR VIBRO-ACOUSTIC                  |
%|           openVibs is part of the GYPSILAB toolbox for Matlab          |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal, Marc Bakry (c) 2017-2019.                 |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%|             marc.bakry@polytechnique.edu                               |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : vibsHmxBlockOperator.m                        |
%|    #    |   VERSION    : 0.55                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal & Marc Bakry                  |
%|  ( # )  |   CREATION   : 14.03.2019                                    |
%|  / 0 \  |   LAST MODIF :                                               |
%| ( === ) |   SYNOPSIS   :                                               |
%|  `---'  |                                                              |
%+========================================================================+

% Constants
mu     = rhoS*cT^2;
lambda = rhoS*(cL^2 - 2*cT^2);
w      = 2*pi*f;
k      = w/c0;

% Dimension
n = size(omega.msh.elt,2)-1;

% Green kernel function
if (n == 2)
    Gxy         = @(X,Y) femGreenKernel(X,Y,'[H0(kr)]',k);
    gradyGxy{1} = @(X,Y) femGreenKernel(X,Y,'grady[H0(kr)]1',k);
    gradyGxy{2} = @(X,Y) femGreenKernel(X,Y,'grady[H0(kr)]2',k);
    gradyGxy{3} = @(X,Y) femGreenKernel(X,Y,'grady[H0(kr)]3',k);
    G0          = '[log(r)]';
    gradyG0     = 'grady[log(r)]';
    cteGxy      = 1i/4;
    cteG0       = -1/(2*pi);

elseif (n == 3)
    Gxy         = @(X,Y) femGreenKernel(X,Y,'[exp(ikr)/r]',k);
    gradyGxy{1} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]1',k);
    gradyGxy{2} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]2',k);
    gradyGxy{3} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]3',k);
    G0          = '[1/r]';
    gradyG0     = 'grady[1/r]';
    cteGxy      =  1/(4*pi);
    cteG0       =  1/(4*pi);

else
    error('vibsNeumannBW.m : unavailable case.')
end

% Coupling coeff for Brackage-Werner simulation
beta = 1i*k;

%%%%%%%%%%%%%%%%%%%%%%%%%%% ELASTO-ELASTO (A11) %%%%%%%%%%%%%%%%%%%%%%%%%%%

% Initialization
A = cell(n,n);

% Static part
GG = integral(omega,grad(U),grad(U));
for i = 1:n
    for j = 1:n
        % Operator div(U):div(U)
        DD = integral(omega,grad(U,i),grad(U,j));

        % Operator e(U):e(U)
        EE = integral(omega,grad(U,j),grad(U,i));
        if (i==j)
            EE = EE + GG;
        end

        % Summation
        A{i,j} = lambda.*DD + mu.*EE;
    end
end

% Dynamic part
if (f ~= 0)
    Id = integral(omega,U,U);
    for i = 1:n
       A{i,i} = A{i,i} - (rhoS*w^2) .* Id;
    end
end

% Final form (sparse)
A = cell2mat(A);

%%%%%%%%%%%%%%%%%%%%%%%%%% ELASTO-ACOUSTIC (A12) %%%%%%%%%%%%%%%%%%%%%%%%%%

% Gaussian quadrature
[Xqud,Wx] = sigma.qud;
Wx        = spdiags(Wx,0,length(Wx),length(Wx));

% Collocation mass operator
Id = u.uqm(sigma);

% Collocation boundary operator
S = cteGxy .* integral(Xqud,sigma,Gxy,u,tol) + ...
    cteG0  .* regularize(Xqud,sigma,G0,u);

% Collocation boundary operator
D = cteGxy .* integral(Xqud,sigma,gradyGxy,ntimes(u),tol) + ...
    cteG0  .* regularize(Xqud,sigma,gradyG0,ntimes(u));

% Final operator Brackage-Werner : [1i*k*beta*S - (Id/2 + D)]
B.Mr = beta.*S - (0.5*Id + D);

% Normal trace of the volumn element matrix
nU   = ntimes(U);
nPHI = nU.uqm(sigma);

% Coupling to FEM
B.Ml = cell(n,1);
for i = 1:n
    B.Ml{i} = (nPHI{i}' * Wx);
end
B.Ml = cell2mat(B.Ml);

%%%%%%%%%%%%%%%%%%%%%%%%%% ELASTO-ACOUSTIC (A21) %%%%%%%%%%%%%%%%%%%%%%%%%%

% Initialization
C = cell(1,n);

% Coupling FEM
for i = 1:n
    C{i} = (rho0*w^2) .* integral(sigma,u,ntimes(U,i));
end

% Final format (sparse)
C = cell2mat(C);

%%%%%%%%%%%%%%%%%%%%%%%%%% ACOUSTIC-ACOUSTIC (A22) %%%%%%%%%%%%%%%%%%%%%%%%

% Finite element mass matrix
Id = integral(sigma,u,u);

% Finite element boundary operator
H  = cteGxy .* (k^2 * integral(sigma,sigma,ntimes(u),Gxy,ntimes(u),tol) ...
    - integral(sigma,sigma,nxgrad(u),Gxy,nxgrad(u),tol));
Hr = cteG0  .* (k^2 * regularize(sigma,sigma,ntimes(u),G0,ntimes(u)) ...
    - regularize(sigma,sigma,nxgrad(u),G0,nxgrad(u)));

% Finite element boundary operator
D  = cteGxy .* integral(sigma,sigma,u,gradyGxy,ntimes(u),tol);
Dr = cteG0  .* regularize(sigma,sigma,u,gradyG0,ntimes(u));

% Final operator Brackage-Werner : - [1i*k*beta*(-Id/2 + Dt) - H]
D = - (beta.*(-0.5*Id + (D+Dr).') - (H+Hr));
end