function [ output_args ] = gmm_hu(I,M,options)
%GMM_HU Summary of this function goes here
%   Detailed explanation goes here
beta1 = 1e4;
beta2 = 0; % 2e4
eta = 0.05;

rho1 = 1e4;

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

names = cell(1,M);
for i = 1:M
    names{i} = ['endmember ',num2str(i)];
end

%% initialize parameters
[Y,~,rows,cols] = reshape_hsi(I,[]);
[N,B] = size(Y);

beta1 = beta1*B/M;
beta2 = beta2*B/M;
rho1 = rho1*N/M^2;

if beta1 < 1e-9
    beta1 = 1e-9;
end

[W,Neighbors] = image2graph(I,eta,1e-9);

D = diag(sum(W,2));
L = D - W;
L = sparse(L);
KL = L - beta2/beta1*speye(N);

W = ones(M,M);
D = diag(sum(W,2));
H = D - W;

[mu_jk,sigma_jk,w_jk,K,A] = gmm_init(I,M);

%% Create all the k indices
% K = [1 2 3 1];
K_inds  = cell(1,M);
for j = 1:M
    K_inds{j} = (1:K(j));
end
K_all = cartprod(K_inds{:});
K1 = size(K_all,1);

%% iterate by MM (EM)
w_k = w_jk2w_k(w_jk,K_all);

sigma = Y_noise;

I_B = eye(B);

delta_t0 = 1e-12;

delta_t_mu = delta_t0;
delta_t_sigma = delta_t0;
delta_t_A = delta_t0;

der_mu0 = mu_jk;
der_sigma0 = sigma_jk;
der_A0 = zeros(N,M);
for j = 1:length(der_mu0)
    der_mu0{j}(:,:) = 0;
    der_sigma0{j}(:,:,:) = 0;
end

s = [];
s.N = N;
s.B = B;
s.K1 = K1;
s.sigma = sigma;
s.K_all = K_all;
s.Y = Y;
s.beta1 = beta1;
s.KL = KL;

s.A = A;
s.mu_jk = mu_jk;
s.sigma_jk = sigma_jk;
s.w_k = w_k;

Is = (1:N*B);
Is = repmat(reshape(Is, [B,1,N]), 1, B);
Is = Is(:);

Js = (1:N*B);
Js = repmat(reshape(Js, [1,B,N]), B, 1);
Js = Js(:);

s.Is = Is;
s.Js = Js;

max_iter = 200;
eval_totals = zeros(max_iter, 1);
eval_Ns = zeros(max_iter, 1);
eval_As = zeros(max_iter, 1);
eval_Rs = zeros(max_iter, 1);

for iter = 1:200
    %% E step
    % update gamma_nk

    N_nk = calc_gaussians(A, sigma, mu_jk, sigma_jk, K_all, Y, s);

    gamma_nk = (ones(N,1)*w_k) .* N_nk;
    gamma_nk = gamma_nk ./ repmat(sum(gamma_nk,2), 1, K1);
    s.gamma_nk = gamma_nk;

    %% M step
    % update w_k
    w_k = sum(gamma_nk, 1) / N;
    s.w_k = w_k;

    delta_t = delta_t0;
    disp('Process M step optimization.');

    % update mu
    der_mu = calc_der_mu(A, sigma, mu_jk, sigma_jk, K_all, gamma_nk, Y);
    s.der_mu = der_mu;
    delta_t_mu = calc_time_step_adaptive(@eval_obj_fun_mu, @update_mu, ...
        mu_jk, s, delta_t_mu, delta_t0);
    mu_jk = update_mu(mu_jk, s, delta_t_mu);
    s.mu_jk = mu_jk;

    % update sigma
    der_sigma = calc_der_sigma(A, sigma, mu_jk, sigma_jk, K_all, gamma_nk, Y);
    s.der_sigma = der_sigma;
    delta_t_sigma = calc_time_step_adaptive(@eval_obj_fun_sigma, @update_sigma, ...
        sigma_jk, s, delta_t_sigma, delta_t0);
    sigma_jk = update_sigma(sigma_jk, s, delta_t_sigma);
    s.sigma_jk = sigma_jk;

    % update A
    der_A = calc_der_A(A, sigma, mu_jk, sigma_jk, K_all, gamma_nk, Y, KL, beta1);
    s.der_A = der_A;
    delta_t_A = calc_time_step_adaptive(@eval_obj_fun_A, @update_A, ...
        A, s, delta_t_A, delta_t0);
    A = update_A(A, s, delta_t_A);
    s.A = A;

    if mod(iter,40) == 0
        endmember_scatter_plot_end_var(Y,w_jk,mu_jk,sigma_jk,names);
        set(gcf,'name',['Scatter plot of iteration ',num2str(iter)]);
        show_abundances(A,size(I,1),size(I,2));
        set(gcf,'name',['Abundances of iteration ',num2str(iter)]);
        pause(0.01);
    end

    [eval_total, eval_N, eval_A, eval_R] = calc_obj_fun(A, ...
        sigma, mu_jk, sigma_jk, K_all, Y, KL, beta1, w_k, s);
    eval_totals(iter) = eval_total;
    eval_Ns(iter) = eval_N;
    eval_As(iter) = eval_A;
    eval_Rs(iter) = eval_R;

    disp(['EM iteration ', num2str(iter)]);

end

w_jk = w_k2w_jk(w_k, K_all);

show_abundances(A,size(I,1),size(I,2));
figure('name', 'Total objective function value vs iteration number');
plot(eval_totals);

figure('name', 'Data fidelity term value vs iteration number');
plot(eval_Ns);

figure('name', 'Abundance smoothness term value vs iteration number');
plot(eval_As);

figure('name', 'Endmembers clossness term value vs iteration number');
plot(eval_Rs);

function [eval_total, eval_N, eval_A, eval_R] = calc_obj_fun(A, ...
    sigma, mu_jk, sigma_jk, K_all, Y, KL, beta1, w_k, options)
N = size(A,1);

N_nk = calc_gaussians(A, sigma, mu_jk, sigma_jk, K_all, Y, options);
eval_N = -sum(log(sum((ones(N,1)*w_k) .* N_nk, 2)));
eval_A = (beta1/2)*trace(A'*KL*A);
eval_R = 0;
eval_total = eval_N + eval_A + eval_R;

function N_nk = calc_gaussians(A, sigma, mu_jk, sigma_jk, K_all, Y, options)
[N,B] = size(Y);
K1 = size(K_all,1);

[mu_nk,sigma_nk] = calc_mu_sigma_nk(A, sigma, mu_jk, sigma_jk, K_all);
% old implementation

% N_nk1 = zeros(N,K1);
% for n = 1:N
%     for k = 1:K1
%         y_n_mu_nk = Y(n,:)'-mu_nk(:,:,n,k);
%         N_nk1(n,k) = det(sigma_nk(:,:,n,k))^(-1/2) * ...
%             exp(-0.5 * y_n_mu_nk' * (sigma_nk(:,:,n,k) \ y_n_mu_nk));
%     end
% end
% N_nk1 = N_nk1 / (2*pi)^(B/2);

% new implementation

N_nk = zeros(N,K1);
for k = 1:K1
    y = logmvn(Y, mu_nk(:,:,k), sigma_nk(:,:,:,k), options);
    N_nk(:,k) = exp(y);
end

% norm(N_nk - N_nk1) / norm(N_nk)

function val = eval_obj_fun_mu(params, options)
mu_jk = params;

A = options.A;
sigma = options.sigma;
% mu_jk = options.mu_jk;
sigma_jk = options.sigma_jk;
K_all = options.K_all;
gamma_nk = options.gamma_nk;
Y = options.Y;
rho1 = options.rho1;
w_k = options.w_k;
% beta1 = options.beta1;
% KL = options.KL;
H = options.H;

[N,B] = size(Y);
K1 = size(K_all,1);
[mu_nk,sigma_nk] = calc_mu_sigma_nk(A, sigma, mu_jk, sigma_jk, K_all);

val2 = zeros(1,K1);
for k = 1:K1
    Y1 = (Y - mu_nk(:,:,k))';
    Y2 = Y1 .* repmat(gamma_nk(:,k)', B, 1);
    sigma_k = block_diag(sigma_nk(:,:,:,k), options);
    val2(k) = Y1(:)' * (sigma_k \ Y2(:));
end
val = sum(sum(0.5 * val2));

% tic
% N_nk = zeros(N,K1);
% for k = 1:K1
%     N_nk(:,k) = logmvn(Y, reshape(mu_nk(:,:,:,k),B,N)', sigma_nk(:,:,:,k), options);
% end
%
% val3 = -sum(sum(0.5 * gamma_nk .* N_nk));
% toc

val_mu = 0;
[mu_all,sigma_all] = calc_mu_sigma_all(mu_jk, sigma_jk, K_all);
for k = 1:K1
    val_mu = val_mu + w_k(k) * trace(mu_all(:,:,k) * H * mu_all(:,:,k)');
end
val_mu = rho1 / 2 * val_mu;

val = val + val_mu;

function val = eval_obj_fun_sigma(params, s)
sigma_jk = params;

A = s.A;
sigma = s.sigma;
mu_jk = s.mu_jk;
% sigma_jk = options.sigma_jk;
K_all = s.K_all;
gamma_nk = s.gamma_nk;
Y = s.Y;
% beta1 = options.beta1;
% KL = options.KL;

N = size(A,1);
K1 = size(K_all,1);

[mu_nk,sigma_nk] = calc_mu_sigma_nk(A, sigma, mu_jk, sigma_jk, K_all);
% val1 = zeros(N,K1);
% for n = 1:N
%     for k = 1:K1
%         y_n_mu_nk = Y(n,:)' - mu_nk(:,:,n,k);
%         val1(n,k) = log(det(sigma_nk(:,:,n,k))) + y_n_mu_nk' * (sigma_nk(:,:,n,k) \ y_n_mu_nk);
%     end
% end
%
% val = sum(sum(0.5 * gamma_nk .* val1));

N_nk = zeros(N,K1);
for k = 1:K1
    N_nk(:,k) = logmvn(Y, mu_nk(:,:,k), sigma_nk(:,:,:,k), s);
end

val = -sum(sum(0.5 * gamma_nk .* N_nk));

function val = eval_obj_fun_A(params, options)
A = params;

% A = options.A;
sigma = options.sigma;
mu_jk = options.mu_jk;
sigma_jk = options.sigma_jk;
K_all = options.K_all;
gamma_nk = options.gamma_nk;
Y = options.Y;
beta1 = options.beta1;
KL = options.KL;

N = size(A,1);
K1 = size(K_all,1);
[mu_nk,sigma_nk] = calc_mu_sigma_nk(A, sigma, mu_jk, sigma_jk, K_all);
% val1 = zeros(N,K1);
% for n = 1:N
%     for k = 1:K1
%         y_n_mu_nk = Y(n,:)' - mu_nk(:,:,n,k);
%         val1(n,k) = log(det(sigma_nk(:,:,n,k))) + y_n_mu_nk' * (sigma_nk(:,:,n,k) \ y_n_mu_nk);
%     end
% end

% val = sum(sum(0.5 * gamma_nk .* val1)) + beta1/2 * trace(A'*KL*A);

N_nk = zeros(N,K1);
for k = 1:K1
    N_nk(:,k) = logmvn(Y, mu_nk(:,:,k), sigma_nk(:,:,:,k), options);
end

val = -sum(sum(0.5 * gamma_nk .* N_nk)) + beta1/2 * trace(A'*KL*A);

function der_mu = calc_der_mu(A, sigma, mu_jk, sigma_jk, K_all, gamma_nk, Y)
[N,M] = size(A);
B = size(mu_jk{1},2);
K1 = size(K_all,1);
K = max(K_all,[],1);

[mu_nk,sigma_nk] = calc_mu_sigma_nk(A, sigma, mu_jk, sigma_jk, K_all);
lambda_nk = calc_lambda_nk(gamma_nk,mu_nk,sigma_nk,Y);

% calculate der_mu
% tic
% der_mu = mu_jk;
% for j = 1:M
%     for k = 1:K(j)
%         lambda_sum = sum(lambda_nk(:,:,K_all(:,j)==k), 3);
%         lambda_alpha = repmat(A(:,j), [1,B]);
%         der_mu{j}(k,:) = -sum(lambda_sum .* lambda_alpha, 1);
%     end
% end
% toc

% tic
der_mu = mu_jk;
temp = zeros(M,B,K1);
for k = 1:K1
    temp(:,:,k) = A' * lambda_nk(:,:,k);
end

for j = 1:M
    for k = 1:K(j)
        der_mu{j}(k,:) = -sum(temp(j,:,K_all(:,j)==k), 3)';
    end
end
% toc

% mdif(der_mu{1},der_mu1{1});

function der_sigma = calc_der_sigma(A, sigma, mu_jk, sigma_jk, K_all, gamma_nk, Y)
[N,M] = size(A);
[~,B] = size(Y);
K = max(K_all,[],1);
K1 = size(K_all,1);

[mu_nk,sigma_nk,prec] = calc_mu_sigma_prec(A, sigma, mu_jk, sigma_jk, K_all);
[lambda_nk,psi_nk] = calc_lambda_psi_nk(gamma_nk,mu_nk,sigma_nk,prec,Y);

% calculate der_mu, der_sigma, der_A
% tic
% der_sigma1 = sigma_jk;
% for j = 1:M
%     for k = 1:K(j)
%         psi_sum = sum(psi_nk(:,:,:,K_all(:,j)==k), 4);
%         psi_alpha = reshape(repmat(A(:,j)'.^2, [B*B,1]), [B,B,N]);
%         der_sigma1{j}(:,:,k) = -sum(psi_sum .* psi_alpha, 3);
%     end
% end
% toc

% tic
psi_k = reshape(psi_nk, [B*B,N,K1]);
der_sigma = sigma_jk;
temp = zeros(M,B*B,K1);
for k = 1:K1
    temp(:,:,k) = (A.^2)' * psi_k(:,:,k)';
end

for j = 1:M
    for k = 1:K(j)
        der_sigma{j}(:,:,k) = reshape(-sum(temp(j,:,K_all(:,j)==k), 3)', B, B);
    end
end
% toc

% disp('');

function lambda_nk = calc_lambda_nk(gamma_nk,mu_nk,sigma_nk,Y)
[N,B,K1] = size(mu_nk);

% tic
% lambda_nk1 = zeros(N,B,K1);
% for n = 1:N
%     for k = 1:K1
%         lambda_nk1(n,:,k) = gamma_nk(n,k) * ...
%             (sigma_nk(:,:,n,k) \ (Y(n,:)' - mu_nk(n,:,k)'));
%     end
% end
% toc

% tic
lambda_nk = zeros(N,B,K1);
for k = 1:K1
    Y1 = Y - mu_nk(:,:,k);
    Y1 = Y1';
    Y2 = Y1 .* repmat(gamma_nk(:,k)', B, 1);
    sigma_k = block_diag(sigma_nk(:,:,:,k));
    lambda_nk(:,:,k) = reshape(sigma_k \ Y2(:), [B,N])';
end
% toc

function [lambda_nk,psi_nk] = calc_lambda_psi_nk(A, sigma, mu_jk, ...
    sigma_jk, K_all, gamma_nk, Y)
[mu_nk,sigma_nk,prec] = calc_mu_sigma_prec(A, sigma, mu_jk, sigma_jk, K_all);

[N,B,K1] = size(mu_nk);

% tic
% lambda_nk1 = zeros(N,B,K1);
% psi_nk1 = zeros(B,B,N,K1);
%
% for k = 1:K1
%     Y1 = Y - mu_nk(:,:,k);
%     for n = 1:N
%         sigma_y_n_mu_nk = prec(:,:,n,k) * Y1(n,:)';
%         lambda_nk1(n,:,k) = gamma_nk(n,k) * sigma_y_n_mu_nk;
%         psi_nk1(:,:,n,k) = 0.5 * gamma_nk(n,k) * ( ...
%             sigma_y_n_mu_nk * sigma_y_n_mu_nk' - prec(:,:,n,k));
%     end
% end
% toc

% tic
lambda_nk = zeros(N,B,K1);
psi_nk = zeros(B,B,N,K1);

for k = 1:K1
    Y1 = Y - mu_nk(:,:,k);
    sigma_y_n_mu_nk = multiprod(prec(:,:,:,k), Y1', [1 2], [1]);
    lambda_nk(:,:,k) = (repmat(gamma_nk(:,k)', B, 1) .* sigma_y_n_mu_nk)';

    tmp1 = reshape(sigma_y_n_mu_nk, [B 1 N]);
    tmp2 = reshape(sigma_y_n_mu_nk, [1 B N]);
    tmp3 = multiprod(tmp1, tmp2, [1 2], [1 2]) - prec(:,:,:,k);
    psi_nk(:,:,:,k) = 0.5 * multiprod(reshape(gamma_nk(:,k), [1 1 N]), tmp3);
end
% toc
% mdiff(lambda_nk,lambda_nk1);
% mdiff(psi_nk,psi_nk1);

function der_A = calc_der_A(A, sigma, mu_jk, sigma_jk, K_all, gamma_nk, ...
    Y, KL, beta1)
[N,K1] = size(gamma_nk);
M = length(mu_jk);
B = size(mu_jk{1},2);

[mu_nk,sigma_nk,prec] = calc_mu_sigma_prec(A, sigma, mu_jk, sigma_jk, K_all);
[lambda_nk,psi_nk] = calc_lambda_psi_nk(gamma_nk,mu_nk,sigma_nk,prec,Y);

% calculate der_mu, der_sigma, der_A
% tic
%
% der_A1 = zeros(N,M);
% for n = 1:N
%     for j = 1:M
%         for k = 1:K1
%             der_A1(n,j) = der_A1(n,j) - sum(lambda_nk(n,:,k) .* mu_jk{j}(K_all(k,j),:)) ...
%                 - 2 * A(n,j) * sum(sum(psi_nk(:,:,n,k) .* sigma_jk{j}(:,:,K_all(k,j))));
%         end
%     end
% end
% toc

% tic
[mu_all,sigma_all] = calc_mu_sigma_all(mu_jk, sigma_jk, K_all);
psi_k = reshape(psi_nk, [B*B,N,K1]);

der_A = zeros(N,M);
temp1 = zeros(N,M);
for k = 1:K1
    der_A = der_A - lambda_nk(:,:,k) * mu_all(:,:,k);
    temp1 = temp1 - psi_k(:,:,k)' * sigma_all(:,:,k);
end
der_A = der_A + 2 * A .* temp1;
% toc

der_A = der_A + beta1 * KL * A;

function mu_jk_new = update_mu(mu_jk, options, delta_t)
der_mu = options.der_mu;

mu_jk_new = mu_jk;

for j = 1:length(mu_jk)
    mu_jk_new{j} = mu_jk{j} - delta_t * der_mu{j};
end

function sigma_jk_new = update_sigma(sigma_jk, options, delta_t)
der_sigma = options.der_sigma;

sigma_jk_new = sigma_jk;

for j = 1:length(sigma_jk)
    sigma_jk_new{j} = sigma_jk{j} - delta_t * der_sigma{j};
%     sigma_jk_new{j} = sigma_jk{j};
    for k = 1:size(sigma_jk{j},3)
        cov_mat = sigma_jk_new{j}(:,:,k);
        [V,D] = eig((cov_mat + cov_mat')/2);
        d = diag(D);
        if (min(d) < 0)
            disp('There is a negative eigenvalue in the updated sigma_jk');
            d(d<0) = 1e-6 * max(abs(d));
            D = diag(d);
        end
        sigma_jk_new{j}(:,:,k) = V*D*V';
    end
end

function A_new = update_A(A, options, delta_t)
der_A = options.der_A;

A_new = A - delta_t * der_A;
A_new = project_to_simplex(A_new);

function [mu_nk,sigma_nk] = calc_mu_sigma_nk(A, sigma, mu_jk, sigma_jk, K_all)
[N,M] = size(A);
K1 = size(K_all,1);
B = size(mu_jk{1},2);

[mu_all,sigma_all] = calc_mu_sigma_all(mu_jk, sigma_jk, K_all);

sigma2_I = sigma^2 * eye(B);

% old implementation
% mu_nk1 = zeros(B,1,N,K1);
% sigma_nk1 = zeros(B,B,N,K1);
% for n = 1:N
%     for k = 1:K1
%         mu_nk1(:,:,n,k) = mu_all(:,:,k) * A(n,:)';
%         sigma_nk1(:,:,n,k) = reshape(sigma_all(:,:,k) * (A(n,:).^2)', B, B) + sigma2_I;
%     end
% end

% new implementation is 10 times faster
mu_nk = zeros(N,B,K1);
sigma_nk = zeros(B*B,N,K1);
for k = 1:K1
    mu_nk(:,:,k) = A * mu_all(:,:,k)';
    sigma_nk(:,:,k) = sigma_all(:,:,k) * (A.^2)' + repmat(sigma2_I(:),1,N);
end
% mu_nk = reshape(mu_nk,[B,1,N,K1]);
sigma_nk = reshape(sigma_nk,[B,B,N,K1]);

function [mu_all,sigma_all] = calc_mu_sigma_all(mu_jk, sigma_jk, K_all)
[~,B] = size(mu_jk{1});
[K1,M] = size(K_all);

mu_all = zeros(B,M,K1);
sigma_all = zeros(B*B,M,K1);
for i = 1:K1
    for j = 1:M
        mu_all(:,j,i) = mu_jk{j}(K_all(i,j),:)';
        sigma_all(:,j,i) = reshape(sigma_jk{j}(:,:,K_all(i,j)), B*B, 1);
    end
end

function [mu_nk,sigma_nk,prec] = calc_mu_sigma_prec(A, sigma, mu_jk, sigma_jk, K_all)
[mu_nk,sigma_nk] = calc_mu_sigma_nk(A, sigma, mu_jk, sigma_jk, K_all);
% [N,B,K1] = size(mu_nk);
% tic
% prec1 = zeros(size(sigma_nk));
% for n = 1:N
%     for k = 1:K1
%         prec1(:,:,n,k) = inv(sigma_nk(:,:,n,k));
%     end
% end
% toc
%
% tic
prec = multinv(sigma_nk);
% toc
%
% mdiff(prec,prec1);

% tic
% prec1 = cell(1,K1);
% for k = 1:K1
%     sigma_k = block_diag(sigma_nk(:,:,:,k), options);
%     R = chol(sigma_k);
%     S = inv(R);
%     prec1{k} = S*S';
% end
%
% prec2 = zeros(B,B,N,K1);
% for k = 1:K1
%     [~,~,S] = find(prec1{k});
%     prec2(:,:,:,k) = reshape(S, [B,B,N]);
% end
%
% toc
%
% mdif(prec,prec2)

function w_k = w_jk2w_k(w_jk, K_all)
w_k = ones(1, size(K_all,1));
for i = 1:size(K_all,1)
    k = K_all(i,:);
    for j = 1:length(w_jk)
        w_k(i) = w_k(i)*w_jk{j}(k(j));
    end
end

function w_jk = w_k2w_jk(w_k, K_all)
M = size(K_all,2);
w_jk = cell(1,M);
K = zeros(1,M);
for j = 1:M
    K(j) = max(K_all(:,j));
    w_jk{j} = zeros(1,K(j));
end

for j = 1:M
    for l = 1:K(j)
        w = w_k(K_all(:,j)==l);
        w_jk{j}(l) = sum(w);
    end
end

%% obsolete
function delta_t = calc_time_step(A, sigma, mu_jk, sigma_jk, K_all, ...
    gamma_nk, Y, delta_t, delta_t0, der_mu, der_sigma, der_A, beta1, KL)
val_ori = eval_obj_fun_M(A, sigma, mu_jk, sigma_jk, K_all, gamma_nk, Y, beta1, KL);
[mu_jk_new, sigma_jk_new, A_new] = update_params(mu_jk, sigma_jk, A, der_mu, der_sigma, der_A, delta_t);
val_new = eval_obj_fun_M(A_new, sigma, mu_jk_new, sigma_jk_new, K_all, gamma_nk, Y, beta1, KL);

if val_new < val_ori
    val_old = val_new;
    while 1
        delta_t = delta_t * 10;
        [mu_jk_new, sigma_jk_new, A_new] = update_params(mu_jk, sigma_jk, A, ...
            der_mu, der_sigma, der_A, delta_t);
        val = eval_obj_fun_M(A_new, sigma, mu_jk_new, sigma_jk_new, K_all, gamma_nk, Y, beta1, KL);
        if val < val_old
            val_old = val;
        else
            delta_t = delta_t / 10;
            break;
        end
    end
else
    while delta_t > delta_t0
        delta_t = delta_t / 10;
        [mu_jk_new, sigma_jk_new, A_new] = update_params(mu_jk, sigma_jk, A, ...
            der_mu, der_sigma, der_A, delta_t);
        val = eval_obj_fun_M(A_new, sigma, mu_jk_new, sigma_jk_new, K_all, gamma_nk, Y, beta1, KL);
        if val < val_ori
            break;
        end
    end
end

function [mu_jk_new, sigma_jk_new, A_new] = update_params(mu_jk, sigma_jk, A, ...
    der_mu, der_sigma, der_A, delta_t)
mu_jk_new = mu_jk;
sigma_jk_new = sigma_jk;

for j = 1:length(mu_jk)
    mu_jk_new{j} = mu_jk{j} - delta_t * der_mu{j};
    sigma_jk_new{j} = sigma_jk{j} - delta_t * der_sigma{j};
%     sigma_jk_new{j} = sigma_jk{j};
    for k = 1:size(sigma_jk{j},3)
        cov_mat = sigma_jk_new{j}(:,:,k);
        [V,D] = eig((cov_mat + cov_mat')/2);
        d = diag(D);
        if (min(d) < 0)
            disp('There is a negative eigenvalue in the updated sigma_jk');
            d(d<0) = 1e-6 * max(abs(d));
            D = diag(d);
        end
        sigma_jk_new{j}(:,:,k) = V*D*V';
    end
end

A_new = A - delta_t * der_A;
A_new = project_to_simplex(A_new);

function val = eval_obj_fun_M(A, sigma, mu_jk, sigma_jk, K_all, gamma_nk, Y, beta1, KL)
N = size(A,1);
K1 = size(K_all,1);
[mu_nk,sigma_nk] = calc_mu_sigma_nk(A, sigma, mu_jk, sigma_jk, K_all);
val1 = zeros(N,K1);
for n = 1:N
    for k = 1:K1
        y_n_mu_nk = Y(n,:)' - mu_nk(:,:,n,k);
        val1(n,k) = log(det(sigma_nk(:,:,n,k))) + y_n_mu_nk' * (sigma_nk(:,:,n,k) \ y_n_mu_nk);
    end
end

val = sum(sum(0.5 * gamma_nk .* val1)) + beta1/2 * trace(A'*KL*A);

function [der_mu, der_sigma, der_A] = calc_derivatives(A, sigma, mu_jk, ...
    sigma_jk, K_all, gamma_nk, Y, beta1, KL)
[N,M] = size(A);
B = size(mu_jk{1},2);
K1 = size(K_all,1);
K = max(K_all,[],1);

der_mu = mu_jk;
der_sigma = sigma_jk;

[mu_nk,sigma_nk,prec] = calc_mu_sigma_prec(A, sigma, mu_jk, sigma_jk, K_all);

lambda_nk = zeros(B,1,N,K1);
psi_nk = zeros(B,B,N,K1);
der_A = zeros(N,M);
for n = 1:N
    for k = 1:K1
        sigma_y_n_mu_nk = prec(:,:,n,k) * (Y(n,:)' - mu_nk(:,:,n,k));
        lambda_nk(:,:,n,k) = gamma_nk(n,k) * sigma_y_n_mu_nk;
        psi_nk(:,:,n,k) = 0.5 * gamma_nk(n,k) * ( ...
            sigma_y_n_mu_nk * sigma_y_n_mu_nk' - prec(:,:,n,k));
    end
end
% calculate der_mu, der_sigma, der_A
for j = 1:M
    for k = 1:K(j)
        lambda_sum = sum(lambda_nk(:,:,:,K_all(:,j)==k), 4);
        lambda_alpha = reshape(repmat(A(:,j)', [B,1]), [B,1,N]);
        der_mu{j}(k,:) = -sum(lambda_sum .* lambda_alpha, 3);

        psi_sum = sum(psi_nk(:,:,:,K_all(:,j)==k), 4);
        psi_alpha = reshape(repmat(A(:,j)'.^2, [B*B,1]), [B,B,N]);
        der_sigma{j}(:,:,k) = -sum(psi_sum .* psi_alpha, 3);
    end
end

for n = 1:N
    for j = 1:M
        for k = 1:K1
            der_A(n,j) = der_A(n,j) - sum(lambda_nk(:,:,n,k) .* mu_jk{j}(K_all(k,j),:)') ...
                - 2 * A(n,j) * sum(sum(psi_nk(:,:,n,k) .* sigma_jk{j}(:,:,K_all(k,j))));
        end
    end
end
der_A = der_A + beta1 * KL * A;

function [der_mu, der_sigma] = calc_derivative_sigma(A, sigma, mu_jk, sigma_jk, K_all, gamma_nk, Y)
[N,M] = size(A);
B = size(mu_jk{1},2);
K1 = size(K_all,1);
K = max(K_all,[],1);

der_mu = mu_jk;
der_sigma = sigma_jk;

for j = 1:M
    der_mu{j} = zeros(size(der_mu{j}));
end

[mu_nk,sigma_nk,prec] = calc_mu_sigma_prec(A, sigma, mu_jk, sigma_jk, K_all);

psi_nk = zeros(B,B,N,K1);
for n = 1:N
    for k = 1:K1
        sigma_y_n_mu_nk = prec(:,:,n,k) * (Y(n,:)' - mu_nk(:,:,n,k));
        psi_nk(:,:,n,k) = 0.5 * gamma_nk(n,k) * ( ...
            sigma_y_n_mu_nk * sigma_y_n_mu_nk' - prec(:,:,n,k));
    end
end
% calculate der_sigma
for j = 1:M
    for k = 1:K(j)
        psi_sum = sum(psi_nk(:,:,:,K_all(:,j)==k), 4);
        psi_alpha = reshape(repmat(A(:,j)'.^2, [B*B,1]), [B,B,N]);
        der_sigma{j}(:,:,k) = -sum(psi_sum .* psi_alpha, 3);

%         temp = zeros(B,B);
%         for n = 1:N
%             for k1 = 1:K1
%                 if k == K_all(k1,j)
%                     temp = temp - A(n,j)^2 * psi_nk(:,:,n,k1);
%                 end
%             end
%         end
    end
end


function [odf,alpha] = calcFEMODF(pf,varargin)
% PDF to ODF inversion
%
% *calcFEMODF* is one of the main function of the MTEX toolbox.
% It estimates an ODF from given Polefigure intensities by
% <PoleFigure2ODF.html fitting an ODF that consists of a large number of unimodal ODFs to the data>.
% It does so by minimizing a least squares functional. The command
% *calcODF* supports <PoleFigure2ODFGhostCorrection.html automatic ghost correction> and
% <PoleFigureDubna.html the zero range method>.
% The function *calcFEMODF* has several options to control convergence,
% resolution, smoothing, etc. See below for a complete description.
%
%
% Input
%  pf - @PoleFigure
%
% Options
%  resolution     - localization grid for the ansatz fucntions (default = 3/2 resolution(pf))
%  iterMax        - maximum number of iterations (default = 11)
%  regularisation - weighting coefficient lambda (default = 0)
%
% Flags
%  zeroRange         - apply zero range method (default = )
%  noGhostCorrection - omit ghost correction
%
% Output
%  odf    - reconstructed @FEMODF
%  alpha  - scaling factors, calculated during reconstruction
%
% See also
% PoleFigure2odf ODF_demo PoleFigureSimulation_demo
% PoleFigure.load ImportPoleFigureData examples_index

tic
vdisp('------ MTEX -- PDF to ODF inversion ------------------',varargin{:})

% ------------------- get input--------------------------------------------

CS = pf.CS; SS = pf.SS;

% generate FEM discretization of orientation space
res = get_option(varargin,'resolution',pf.resolution);
%ori = equispacedSO3Grid(CS,SS,'resolution',min(res,10*degree));
%DSO3 = DelaunaySO3(ori);
DSO3 = varargin{1};

% zero range method - TODO
%if check_option(varargin,'zero_range'), S3G = zero_range(pf,S3G,varargin{:});end

vdisp('Setting up matrices',varargin{:});
% compute matrices
for ipf = 1:length(pf)
  h = pf(ipf).h;
  vdisp(char(h),varargin{:});
  M{ipf} = sparse(length(pf(ipf).r),length(DSO3));
  for ih = 1:length(h)
    M{ipf} = M{ipf} + pf(ipf).c(ih) .* DSO3.pdfMatrix(h(ih),pf(ipf).r,varargin{:});
  end

  b{ipf} = pf(ipf).intensities(:);
end

M = vertcat(M{:});
b = vertcat(b{:});

vdisp(['starting solver'],varargin{:});
vdisp(['matrix is ' sizestr(M) ],varargin{:});
% solve the linear system of equation
c = lsqnonneg(M,b);
%c = M \ b;

% set up FEMODF
odf = femODF(DSO3,'weights',c);

end


function [KE, PE] = energy(q,dq,p)
%[KE, PE] = energy(q,dq,p)
%
% This function computes the mechanical energy for the five-link biped
%
% INPUTS:
%   q = [5,n] = configuration
%   dq = [5,n] = rates
%   p = parameter struct
%
% OUTPUTS:
%   Fx = [1,n] = horizontal contact force acting on robot
%   Fy = [1,n] = vertical contact force acting on robot
%

[KE, PE] = autoGen_energy(...
 q(1,:),q(2,:),q(3,:),q(4,:),q(5,:),...
    dq(1,:),dq(2,:),dq(3,:),dq(4,:),dq(5,:),...
    p.m1, p.m2, p.m3, p.m4, p.m5, p.I1, p.I2, p.I3, p.I4, p.I5, p.l1, p.l2, p.l3, p.l4, p.c1, p.c2, p.c3, p.c4, p.c5, p.g);

end


% @author: Maziar Raissi

function params_list = KDV()
% quantile(params_list,[0.025 0.25 0.50 0.75 0.975])
clc; close all;

plt = 1;
save_plt = 0;

addpath ..
addpath ../Utilities
addpath ../Kernels/KDV
addpath ../Utilities/export_fig

function CleanupFun()
    rmpath ..
    rmpath ../Utilities
    rmpath ../Kernels/KDV
    rmpath ../Utilities/export_fig
end

finishup = onCleanup(@() CleanupFun());

rng('default')

set(0,'defaulttextinterpreter','latex')

%% Load Data
load('../Data/kdv.mat', 'usol', 't', 'x')
u_star = real(usol); % 512x201
t_star = t; % 201x1
x_star = x';   % 512x1
N_star = size(x_star,1);
nsteps = size(t_star,1)-1;

if plt ==1
    figure(1);
    plot_surface(t_star, x_star, u_star, '$t$', '$x$', '$u(t,x)$');
    view(3)

    drawnow()
end

%% Setup
noise = 0.00;
u_data = u_star + noise*std(u_star(:))*randn(size(u_star));

N0 = 111;
N1 = 109;
%% Optimize model
params_list = zeros(nsteps,2);
hyp = [log([1.0 1.0]) 0.0 0.0 -4.0];
idx1 = randsample(N_star, N0);
step = 1;
for i = 1:step:nsteps
    dt = t_star(i+step) - t_star(i);

    idx0 = idx1;
    x0 = x_star(idx0,:);
    u0 = u_data(idx0,i);

    idx1 = randsample(N_star,N1);
    x1 = x_star(idx1,:);
    u1 = u_data(idx1,i+step);

    model = HPM(x1, u1, x0, u0, dt, hyp);
    model = model.train(50);

    hyp = model.hyp;
    params_list(i,:) = hyp(3:4);

    [pred_n_star, var_n_star] = model.predict(x_star);
    var_n_star = abs(diag(var_n_star));

    error = norm(pred_n_star - u_star(:,i+step))/norm(u_star(:,i+step));

    fprintf(1,'=========================\n');
    fprintf(1,'Step: %d, Time = %.2f\n\nNLML = %.2f, Error = %.2e\n\n', i, ...
        t_star(i+step), model.NLML, error);

    str = sprintf('%.2f  ', params_list(i,:));
    fprintf('Parameters: %s\n\n', str)

    str = sprintf('%.2f  ', median(params_list(1:step:i,:),1));
    fprintf('Median: %s\n', str)
    fprintf(1,'=========================\n\n');

    if plt == 1
        if ~exist('fig','var')
            fig = figure(2);
        end
        set(fig,'units','normalized','outerposition',[0 0 1 1])
        clf

        subplot(3,1,1);
        tit = sprintf('Time: %.2f\n%d training points', t_star(i), N0);
        plot_data_1D(x_star, u_star(:,i), x0, u0, '$x$', '$u(t,x)$', tit);

        subplot(3,1,2);
        tit = sprintf('Time: %.2f\n%d training points', t_star(i+step), N1);
        plot_data_1D(x_star, u_star(:,i+step), x1, u1, '$x$', '$u(t,x)$', tit);

        subplot(3,1,3);
        plot_prediction_1D(x_star, u_star(:,i+step), pred_n_star, var_n_star, ...
            '$x$', '$u(t,x)$', tit);

        drawnow;
    end

end

if save_plt == 1
    export_fig ./Figures/KDV.png -r300
end

end


function F = normalizePivots(F)
%NORMALIZEPIVOTS   Scale rows and cols of a SEPARABLEAPPROX so that all pivots are 1.
%
% Additionally, the norm of the kth row and column will be the same.

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

% TODO: Document
% TODO: is this useful?

F = normalizeRowsAndCols(F);

d = F.pivotValues(:).';
s = sign(d);
sqrtp = sqrt(abs(d));
F.cols = F.cols./(s.*sqrtp);
F.rows = F.rows./sqrtp;
F.pivotValues = ones(1, length(d));

end


switch method
    case{1} % TVD 0(h^2)
        % Using discrete ordinate method (discrete and constant velocity
        % values in phase-space domain)
        a = v(:,1);
        % Load initial condition
        f = f0;
%            for tsteps = time
               f_eq = f_equilibrium_1d(r,ux,v,t,theta);
                % initialize variables
                 u_next = zeros(1,nx);
                 u_eq = zeros(1,nx);
                 u = zeros(1,nx);
                 for i = 1:nv
                      % load subcase
                      u_eq(:) = f_eq(i,:);
                      u(:) = f(i,:);
                      % Compute the smoothness factors, r(j), from data, u(j).
                       [r] = theta1d(u,a(i));
                        % Compute the Flux Limiter
                       [phi] = fluxlimiter1d(r,1); % using limiter = 1
                       % Compute TVD Fluxes
                       [F_left,F_right] = TVDflux1d(u,a(i),dtdx,phi);
                       % Compute next time step
                        u_next = u - dtdx*(F_right - F_left) ...
                        + (dt/r_time)*(u_eq-u);
                        % BC
                        u_next(1) = u_next(2);
                        u_next(nx) = u_next(nx-1);
                        % UPDATE info
                         u = u_next;
                        % Going back to f
                         f(i,:) = u(:);
                 end
                   % Compute macroscopic moments
                   [n,j_x,E] = macromoments1d(k,w,f,v);

                   % UPDATE macroscopic properties
                    % (here lies a paralellizing computing chalenge)
                   [r,ux,t,p,yun] = macroproperties1d(n,j_x,E,nx,nv,theta);
%                         [p,yun] = macroproperties1d(n,j_x,E,nx,nv,theta);
%            end
           case{2} % WENO k = 3 i.e. O(h^5)
    otherwise
          error('Order must be between 1 and 2');
end


%%*********************************************************************
%% gdcomp: Compute gd = 1/td in Equation (15) of FOT's paper.
%%
%% [gd,info,blk2,At2,C2,b2] = gdcomp(blk,At,C,b,OPTIONS);
%%
%%*********************************************************************

  function [gd,info,blk2,At2,C2,b2] = gdcomp(blk,At,C,b,OPTIONS);

  if (nargin == 4)
     OPTIONS = sqlparameters;
     OPTIONS.vers = 1;
     OPTIONS.printlevel = 3;
  end
  if ~iscell(C); tmp = C; clear C; C{1} = tmp; end
%%
  m = length(b);
  blk2 = blk;
  At2 = cell(size(blk,1),1);
  C2 = cell(size(blk,1),1);
  b2 = [zeros(m,1); 1; 0];
%%
%%
%%
  for p = 1:size(blk,1)
     pblk = blk(p,:);
     n = sum(pblk{2});
     if strcmp(pblk{1},'s')
        C2{p,1} = sparse(n,n);
     else
        C2{p,1} = zeros(n,1);
     end
  end
%%
%% New multipliers in dual problem: tt, theta.
%% [v; tt; theta].
%%
   ss = 0; cc = 0; aa = zeros(1,m);
   exist_ublk = 0;
   for p = 1:size(blk,1)
      pblk = blk(p,:);
      n = sum(pblk{2});
      if strcmp(pblk{1},'s')
         At2{p} = [At{p}, svec(pblk,speye(n,n),1), -svec(pblk,C{p},1)];
         ss = ss + n;
         cc = cc + trace(C{p});
         aa = aa + svec(pblk,speye(n),1)'*At{p};
      elseif strcmp(pblk{1},'q')
         eq = zeros(n,1);
         idx1 = 1+[0,cumsum(pblk{2})];
         idx1 = idx1(1:length(idx1)-1);
         eq(idx1) = ones(length(idx1),1);
         At2{p} = [At{p}, 2*sparse(eq), -sparse(C{p})];
         ss = ss + 2*length(pblk{2});
         cc = cc + sum(C{p}(idx1));
         aa = aa + eq'*At{p};
      elseif strcmp(pblk{1},'l')
         el = ones(n,1);
         At2{p} = [At{p}, sparse(el), -sparse(C{p})];
         ss = ss + n;
         cc = cc + el'*C{p};
         aa = aa + el'*At{p};
      elseif strcmp(pblk{1},'u')
         At2{p} = [At{p}, sparse(n,1), -sparse(C{p})];
         exist_ublk = 1;
      end
   end
%%
%% 3 additional inequality constraints in dual problem.
%%
   alp = max(1,sqrt(sum(abs(aa))));
   numblk = size(blk,1);
   blk2{numblk+1,1} = 'l'; blk2{numblk+1,2} = 3;
   C2{numblk+1,1}  = [1; alp; 0];
   At2{numblk+1,1} = [-aa,        0,   cc;
		     zeros(1,m),  0,   alp;
		     zeros(1,m), alp, -alp];
%%
%% Solve SDP
%%
   OPTIONS.gaptol = 1e-10;
   [obj,X,y,Z,info] = HSDsqlp(blk2,At2,C2,b2,OPTIONS);
   gd = 1/abs(obj(2));
   err = max([info.gap/(1+mean(abs(obj))), info.pinfeas, info.dinfeas]);
   if (OPTIONS.printlevel)
      fprintf('\n ******** gd = %3.1e, err = %3.1e\n',gd,err);
      if (err > 1e-6);
         fprintf('\n----------------------------------------------------')
         fprintf('\n gd problem is not solved to sufficient accuracy');
         fprintf('\n----------------------------------------------------\n')
      end
   end
%%*********************************************************************


function [gK_uf, gK_ff] = gpsimCandidateCovGrads(model, M)

% GPSIMCANDIDATECOVGRADS Sparse objective function gradients wrt Covariance function.
% FORMAT
% DESC gives the gradients of the log likelihood with respect to the
% components of the posterior covariance.
% ARG model : the model for which the gradients are to be computed.
% ARG M : The training data for which the computation is to be made
% RETURN gK_uf : the gradient of the likelihood with respect to the
% elements of K_uf.
% RETURN gK_ff : the gradient of the likelihood with respect to K_ff
%
% COPYRIGHT : Neil D. Lawrence, 2007
%
% SEEALSO : gpsimCreate, gpsimAddCandidate, gpsimCandidateLogLikeGradient

% SHEFFIELDML
%E = model.candidate.K_uf*M;
E = model.candidate.K_uf*model.candidate.invK*M;
EET = E*E';
AinvE = model.candidate.Ainv*E;
%AinvEET = model.candidate.Ainv*EET;
diagK_fuAinvEMT = sum(model.candidate.K_uf.*(model.candidate.Ainv*E*M'), 1)';
K_fuAinvEMT = model.candidate.K_uf'*model.candidate.Ainv*E*M';
AinvEETAinv = AinvE*AinvE';
K_ufdAinvplusAinvEETAinvK_fu = model.candidate.K_uf'*(model.candidate.Ainv ...
                                                  + AinvEETAinv)*model.candidate.K_uf;
invK_uuK_uf = model.invK*model.candidate.K_uf;
invK_uuK_ufDinv = invK_uuK_uf*model.candidate.invK;
MMT = M*M';
Q = -model.candidate.K + MMT ...
    + K_ufdAinvplusAinvEETAinvK_fu...
    -K_fuAinvEMT - K_fuAinvEMT';
%gK_uu = 0.5*((model.invK ...
%              -model.candidate.Ainv) - AinvEETAinv ...
%             + invK_uuK_ufDinv*Q*invK_uuK_ufDinv');
gK_uf = -invK_uuK_ufDinv*Q*model.candidate.invK ...
        -model.candidate.Ainv*model.candidate.K_uf*model.candidate.invK ...
        -AinvEETAinv*model.candidate.K_uf*model.candidate.invK ...
        +model.candidate.Ainv*E*M'*model.candidate.invK;
gK_ff = 0.5*model.candidate.invK*Q*model.candidate.invK;


function p = JDDLDR_UP3(X,pi,ZA,ZB,D,C,gamma1,gamma2,MaxIteration_num)
% X: original training data

beta = 0.001;
A  = [];
for ci = 1:size(C,2)
    A = [A D(ci).M*C(ci).M];
end

S = gamma1*ZA*ZA'+gamma2*ZB*ZB';
% S = gamma1*ZA*ZA';
p = pi;
numcomps = size(p,2);

iter_num_sub= 1;
phi1 = X*X';
phi2 = 2*X*A';

while iter_num_sub<MaxIteration_num % subject iteration for updating p, iteration ends when the contiguous function get close enough or reach teh max_iteration
    phi = (X-p*A)*(X-p*A)';
%     phi = eye(size(phi));
    [U,V] = eig(phi-S); p1 = U(:,1:numcomps);
      sum(sum(abs(p+beta*(p1-p) - p)));
      p = p+beta*(p1-p);
%     p = p1;
%     clear U V W phi
    iter_num_sub=iter_num_sub+1;
end


% correlate frames with each other
% sort frames by correlations
% take mean image as mean over most correlated frames

function mimg = pick_reg_init(data)
%%
dd = bsxfun(@minus, data, mean(mean(data,1),2));

% WHITENING???
% for i = 1:size(data,3)
%    d0 = dd(:,:,i);
%    fd0 = fft2(d0);
%    dd(:,:,i) = real(ifft2(fd0./abs(fd0)));
% end

dd = reshape(dd, [], size(dd,ndims(dd)));

CC = dd'*dd;
CC = CC./(diag(CC) * diag(CC)').^.5;

% CC = corrcoef(dd);

[CCsort, isort] = sort(CC, 2, 'descend');
nc = size(CCsort,2);
bestCC = mean(CCsort(:, 1:min(nc,20)), 2);
[~, imax] = max(bestCC);

mimg = mean(data(:,:,isort(imax, 1:min(nc,20))), 3);
end


function yFiltM = applyRecursGaussFilter(xM,coeffS,dim)
% function yFiltM = applyRecursGaussFilter(xM,coeffS,dim)
%
% This function applies recursive filter of order 4 to xM along the passed dim.
% coeffS is struture with filter coefficients.
% For example for LoG filter:
%     scan3M: 3D scanArray (for example, planC{indexS.scan}.scanArray - ctOffset).
%     pixelSizeV = [1,1,3]; % voxel size
%     sigmaVal = 3; % sigma
%     dim = 1; % filter along rows
%     derivativeOrder = 'zero';
%     coeffS.sigmad = sigmaVal / pixelSizeV(dim);
%     coeffS = setGaussOrder(coeffS,derivativeOrder);
%     filt3M = applyRecursGaussFilter(scan3M,coeffS,dim);
%
% APA, 6/12/2018

ykPlusM = zeros(size(xM));
ykMinusM = zeros(size(xM));

switch dim
    case 1

        % Causal
        ykPlusM(1,:,:) = coeffS.N0 * xM(1,:,:) + coeffS.N1 * xM(1,:,:) + coeffS.N2 * xM(1,:,:)...
            + coeffS.N3 * xM(1,:,:);
        ykPlusM(2,:,:) = coeffS.N0 * xM(2,:,:) + coeffS.N1 * xM(1,:,:) + coeffS.N2 * xM(1,:,:)...
            + coeffS.N3 * xM(1,:,:);
        ykPlusM(3,:,:) = coeffS.N0 * xM(3,:,:) + coeffS.N1 * xM(2,:,:) + coeffS.N2 * xM(1,:,:)...
            + coeffS.N3 * xM(1,:,:);
        ykPlusM(4,:,:) = coeffS.N0 * xM(4,:,:) + coeffS.N1 * xM(3,:,:) + coeffS.N2 * xM(2,:,:)...
            + coeffS.N3 * xM(1,:,:);

        ykPlusM(1,:,:) = ykPlusM(1,:,:) - coeffS.BN1 * xM(1,:,:) - coeffS.BN2 * xM(1,:,:) - coeffS.BN3 * xM(1,:,:)...
            - coeffS.BN4 * xM(1,:,:);
        ykPlusM(2,:,:) = ykPlusM(2,:,:) + -coeffS.D1 * ykPlusM(1,:,:) - coeffS.BN2 * xM(1,:,:) - coeffS.BN3 * xM(1,:,:)...
            - coeffS.BN4 * xM(1,:,:);
        ykPlusM(3,:,:) = ykPlusM(3,:,:)  + -coeffS.D1 * ykPlusM(1,:,:) -+ coeffS.D2 * ykPlusM(2,:,:) - coeffS.BN3 * xM(1,:,:)...
            - coeffS.BN4 * xM(1,:,:);
        ykPlusM(4,:,:) = ykPlusM(4,:,:) + -coeffS.D1 * ykPlusM(1,:,:) -+ coeffS.D2 * ykPlusM(2,:,:) -+ coeffS.D3 * ykPlusM(3,:,:)...
            - coeffS.BN4 * xM(1,:,:);

        for ind = 5:size(xM,dim)
            ykPlusM(ind,:,:) = coeffS.N0 * xM(ind,:,:) + coeffS.N1 * xM(ind-1,:,:) + coeffS.N2 * xM(ind-2,:,:)...
                + coeffS.N3 * xM(ind-3,:,:) -+ coeffS.D1 * ykPlusM(ind-1,:,:) -+ coeffS.D2 * ykPlusM(ind-2,:,:) ...
                -+ coeffS.D3 * ykPlusM(ind-3,:,:) -+ coeffS.D4 * ykPlusM(ind-4,:,:);
        end

        % Anticausal
        ykMinusM(end,:,:) = coeffS.M1 * xM(end,:,:) + coeffS.M2 * xM(end,:,:) + coeffS.M3 * xM(end,:,:)...
            + coeffS.M4 * xM(end,:,:);
        ykMinusM(end-1,:,:) = coeffS.M1 * xM(end,:,:) + coeffS.M2 * xM(end,:,:) + coeffS.M3 * xM(end,:,:)...
            + coeffS.M4 * xM(end,:,:);
        ykMinusM(end-2,:,:) = coeffS.M1 * xM(end-1,:,:) + coeffS.M2 * xM(end-1,:,:) + coeffS.M3 * xM(end,:,:)...
            + coeffS.M4 * xM(end,:,:);
        ykMinusM(end-3,:,:) = coeffS.M1 * xM(end-2,:,:) + coeffS.M2 * xM(end-2,:,:) + coeffS.M3 * xM(end-1,:,:)...
            + coeffS.M4 * xM(end,:,:);

        ykMinusM(end,:,:) = ykMinusM(end,:,:) - coeffS.BM1 * xM(end,:,:) - coeffS.BM2 * xM(end,:,:) - coeffS.BM3 * xM(end,:,:)...
            - coeffS.BM4 * xM(end,:,:);
        ykMinusM(end-1,:,:) = ykMinusM(end-1,:,:) + -coeffS.D1 * ykMinusM(end,:,:) - coeffS.BM2 * xM(end,:,:) - coeffS.BM3 * xM(end,:,:)...
            - coeffS.BM4 * xM(end,:,:);
        ykMinusM(end-2,:,:) = ykMinusM(end-2,:,:) + -coeffS.D1 * ykMinusM(end-1,:,:) +- coeffS.D2 * ykMinusM(end,:,:) - coeffS.BM3 * xM(end,:,:)...
            - coeffS.BM4 * xM(end,:,:);
        ykMinusM(end-3,:,:) = ykMinusM(end-3,:,:) + -coeffS.D1 * ykMinusM(end-2,:,:) +- coeffS.D2 * ykMinusM(end-1,:,:) +- coeffS.D3 * ykMinusM(end,:,:)...
            - coeffS.BM4 * xM(end,:,:);

        for ind = size(xM,1)-4:-1:1
            ykMinusM(ind,:,:) = coeffS.M1 * xM(ind+1,:,:) + coeffS.M2 * xM(ind+2,:,:) + coeffS.M3 * xM(ind+3,:,:)...
                + coeffS.M4 * xM(ind+4,:,:) - coeffS.D1 * ykMinusM(ind+1,:,:) - coeffS.D2 * ykMinusM(ind+2,:,:) ...
                - coeffS.D3 * ykMinusM(ind+3,:,:) - coeffS.D4 * ykMinusM(ind+4,:,:);
        end

    case 2

        % Causal
        ykPlusM(:,1,:) = coeffS.N0 * xM(:,1,:) + coeffS.N1 * xM(:,1,:) + coeffS.N2 * xM(:,1,:)...
            + coeffS.N3 * xM(:,1,:);
        ykPlusM(:,2,:) = coeffS.N0 * xM(:,2,:) + coeffS.N1 * xM(:,1,:) + coeffS.N2 * xM(:,1,:)...
            + coeffS.N3 * xM(:,1,:);
        ykPlusM(:,3,:) = coeffS.N0 * xM(:,3,:) + coeffS.N1 * xM(:,2,:) + coeffS.N2 * xM(:,1,:)...
            + coeffS.N3 * xM(:,1,:);
        ykPlusM(:,4,:) = coeffS.N0 * xM(:,4,:) + coeffS.N1 * xM(:,3,:) + coeffS.N2 * xM(:,2,:)...
            + coeffS.N3 * xM(:,1,:);

        ykPlusM(:,1,:) = ykPlusM(:,1,:) - coeffS.BN1 * xM(:,1,:) - coeffS.BN2 * xM(:,1,:) - coeffS.BN3 * xM(:,1,:)...
            - coeffS.BN4 * xM(:,1,:);
        ykPlusM(:,2,:) = ykPlusM(:,2,:) + -coeffS.D1 * ykPlusM(:,1,:) - coeffS.BN2 * xM(:,1,:) - coeffS.BN3 * xM(:,1,:)...
            - coeffS.BN4 * xM(:,1,:);
        ykPlusM(:,3,:) = ykPlusM(:,3,:) + -coeffS.D1 * ykPlusM(:,2,:) -+ coeffS.D2 * ykPlusM(:,1,:) - coeffS.BN3 * xM(:,1,:)...
            - coeffS.BN4 * xM(:,1,:);
        ykPlusM(:,4,:) = ykPlusM(:,4,:) + -coeffS.D1 * ykPlusM(:,3,:) -+ coeffS.D2 * ykPlusM(:,2,:) -+ coeffS.D3 * ykPlusM(:,1,:)...
            - coeffS.BN4 * xM(:,1,:);

        for ind = 5:size(xM,dim)
            ykPlusM(:,ind,:) = coeffS.N0 * xM(:,ind,:) + coeffS.N1 * xM(:,ind-1,:) + coeffS.N2 * xM(:,ind-2,:)...
                + coeffS.N3 * xM(:,ind-3,:) -+ coeffS.D1 * ykPlusM(:,ind-1,:) -+ coeffS.D2 * ykPlusM(:,ind-2,:) ...
                -+ coeffS.D3 * ykPlusM(:,ind-3,:) -+ coeffS.D4 * ykPlusM(:,ind-4,:);
        end

        % Anticausal
        ykMinusM(:,end,:) = coeffS.M1 * xM(:,end,:) + coeffS.M2 * xM(:,end,:) + coeffS.M3 * xM(:,end,:)...
            + coeffS.M4 * xM(:,end,:);
        ykMinusM(:,end-1,:) = coeffS.M1 * xM(:,end,:) + coeffS.M2 * xM(:,end,:) + coeffS.M3 * xM(:,end,:)...
            + coeffS.M4 * xM(:,end,:);
        ykMinusM(:,end-2,:) = coeffS.M1 * xM(:,end-1,:) + coeffS.M2 * xM(:,end-1,:) + coeffS.M3 * xM(:,end,:)...
            + coeffS.M4 * xM(:,end,:);
        ykMinusM(:,end-3,:) = coeffS.M1 * xM(:,end-2,:) + coeffS.M2 * xM(:,end-2,:) + coeffS.M3 * xM(:,end-1,:)...
            + coeffS.M4 * xM(:,end,:);

        ykMinusM(:,end,:) = ykMinusM(:,end,:) - coeffS.BM1 * xM(:,end,:) - coeffS.BM2 * xM(:,end,:) - coeffS.BM3 * xM(:,end,:)...
            - coeffS.BM4 * xM(:,end,:);
        ykMinusM(:,end-1,:) = ykMinusM(:,end-1,:) + -coeffS.D1 * ykMinusM(:,end,:) - coeffS.BM2 * xM(:,end,:) - coeffS.BM3 * xM(:,end,:)...
            - coeffS.BM4 * xM(:,end,:);
        ykMinusM(:,end-2,:) = ykMinusM(:,end-2,:) + -coeffS.D1 * ykMinusM(:,end-1,:) +- coeffS.D2 * ykMinusM(:,end,:) - coeffS.BM3 * xM(:,end,:)...
            - coeffS.BM4 * xM(:,end,:);
        ykMinusM(:,end-3,:) = ykMinusM(:,end-3,:) + -coeffS.D1 * ykMinusM(:,end-2,:) +- coeffS.D2 * ykMinusM(:,end-1,:) +- coeffS.D3 * ykMinusM(:,end,:)...
            - coeffS.BM4 * xM(:,end,:);

        for ind = size(xM,dim)-4:-1:1
            ykMinusM(:,ind,:) = coeffS.M1 * xM(:,ind+1,:) + coeffS.M2 * xM(:,ind+2,:) + coeffS.M3 * xM(:,ind+3,:)...
                + coeffS.M4 * xM(:,ind+4,:) - coeffS.D1 * ykMinusM(:,ind+1,:) - coeffS.D2 * ykMinusM(:,ind+2,:) ...
                - coeffS.D3 * ykMinusM(:,ind+3,:) - coeffS.D4 * ykMinusM(:,ind+4,:);
        end

    case 3

        % Causal
        ykPlusM(:,:,1) = coeffS.N0 * xM(:,:,1) + coeffS.N1 * xM(:,:,1) + coeffS.N2 * xM(:,:,1)...
            + coeffS.N3 * xM(:,:,1);
        ykPlusM(:,:,2) = coeffS.N0 * xM(:,:,2) + coeffS.N1 * xM(:,:,1) + coeffS.N2 * xM(:,:,1)...
            + coeffS.N3 * xM(:,:,1);
        ykPlusM(:,:,3) = coeffS.N0 * xM(:,:,3) + coeffS.N1 * xM(:,:,2) + coeffS.N2 * xM(:,:,1)...
            + coeffS.N3 * xM(:,:,1);
        ykPlusM(:,:,4) = coeffS.N0 * xM(:,:,4) + coeffS.N1 * xM(:,:,3) + coeffS.N2 * xM(:,:,2)...
            + coeffS.N3 * xM(:,:,1);

        ykPlusM(:,:,1) = ykPlusM(:,:,1) - coeffS.BN1 * xM(:,:,1) - coeffS.BN2 * xM(:,:,1) - coeffS.BN3 * xM(:,:,1)...
            - coeffS.BN4 * xM(:,:,1);
        ykPlusM(:,:,2) = ykPlusM(:,:,2) + -coeffS.D1 * ykPlusM(:,:,1) - coeffS.BN2 * xM(:,:,1) - coeffS.BN3 * xM(:,:,1)...
            - coeffS.BN4 * xM(:,:,1);
        ykPlusM(:,:,3) = ykPlusM(:,:,3) + -coeffS.D1 * ykPlusM(:,:,1) -+ coeffS.D2 * ykPlusM(:,:,2) - coeffS.BN3 * xM(:,:,1)...
            - coeffS.BN4 * xM(:,:,1);
        ykPlusM(:,:,4) = ykPlusM(:,:,4) + -coeffS.D1 * ykPlusM(:,:,1) -+ coeffS.D2 * ykPlusM(:,:,2) -+ coeffS.D3 * ykPlusM(:,:,3)...
            - coeffS.BN4 * xM(:,:,1);

        for ind = 5:size(xM,dim)
            ykPlusM(:,:,ind) = coeffS.N0 * xM(:,:,ind) + coeffS.N1 * xM(:,:,ind-1) + coeffS.N2 * xM(:,:,ind-2)...
                + coeffS.N3 * xM(:,:,ind-3) -+ coeffS.D1 * ykPlusM(:,:,ind-1) -+ coeffS.D2 * ykPlusM(:,:,ind-2) ...
                -+ coeffS.D3 * ykPlusM(:,:,ind-3) -+ coeffS.D4 * ykPlusM(:,:,ind-4);
        end

        % Anticausal
        ykMinusM(:,:,end) = coeffS.M1 * xM(:,:,end) + coeffS.M2 * xM(:,:,end) + coeffS.M3 * xM(:,:,end)...
            + coeffS.M4 * xM(:,:,end);
        ykMinusM(:,:,end-1) = coeffS.M1 * xM(:,:,end) + coeffS.M2 * xM(:,:,end) + coeffS.M3 * xM(:,:,end)...
            + coeffS.M4 * xM(:,:,end);
        ykMinusM(:,:,end-2) = coeffS.M1 * xM(:,:,end-1) + coeffS.M2 * xM(:,:,end-1) + coeffS.M3 * xM(:,:,end)...
            + coeffS.M4 * xM(:,:,end);
        ykMinusM(:,:,end-3) = coeffS.M1 * xM(:,:,end-2) + coeffS.M2 * xM(:,:,end-2) + coeffS.M3 * xM(:,:,end-1)...
            + coeffS.M4 * xM(:,:,end);

        ykMinusM(:,:,end) = ykMinusM(:,:,end) - coeffS.BM1 * xM(:,:,end) - coeffS.BM2 * xM(:,:,end) - coeffS.BM3 * xM(:,:,end)...
            - coeffS.BM4 * xM(:,:,end);
        ykMinusM(:,:,end-1) = ykMinusM(:,:,end-1) + -coeffS.D1 * ykMinusM(:,:,end) - coeffS.BM2 * xM(:,:,end) - coeffS.BM3 * xM(:,:,end)...
            - coeffS.BM4 * xM(:,:,end);
        ykMinusM(:,:,end-2) = ykMinusM(:,:,end-2) + -coeffS.D1 * ykMinusM(:,:,end-1) +- coeffS.D2 * ykMinusM(:,:,end) - coeffS.BM3 * xM(:,:,end)...
            - coeffS.BM4 * xM(:,:,end);
        ykMinusM(:,:,end-3) = ykMinusM(:,:,end-3) + -coeffS.D1 * ykMinusM(:,:,end-2) +- coeffS.D2 * ykMinusM(:,:,end-1) +- coeffS.D3 * ykMinusM(:,:,end)...
            - coeffS.BM4 * xM(:,:,end);

        for ind = size(xM,dim)-4:-1:1
            ykMinusM(:,:,ind) = coeffS.M1 * xM(:,:,ind+1) + coeffS.M2 * xM(:,:,ind+2) + coeffS.M3 * xM(:,:,ind+3)...
                + coeffS.M4 * xM(:,:,ind+4) - coeffS.D1 * ykMinusM(:,:,ind+1) - coeffS.D2 * ykMinusM(:,:,ind+2) ...
                - coeffS.D3 * ykMinusM(:,:,ind+3) - coeffS.D4 * ykMinusM(:,:,ind+4);
        end

end

yFiltM = ykPlusM + ykMinusM;


function X = sigm(P)
    X = 1./(1+exp(-P));
end


function Dist = bst_surfdist(Points, Vertices, Faces)
% BST_SURFDIST: Compute the distances between points and a surface.
%
% USAGE:  Dist = bst_surfdist(Points, Vertices, Faces)
%
% DESCRIPTION:
%     Exact distance computation, which checks all 3 sets of distances: points
%     to vertices, points to edges, and points to faces, keeping the smallest
%     for each point.
%
% INPUTS:
%    - Points   : [Qx3] double matrix, points to compare to the mesh defined by Vertices/Faces
%    - Vertices : [Mx3] double matrix
%    - Faces    : [Nx3] double matrix
%
% OUTPUTS:
%    - Dist     : [Qx1] final distance between points and mesh

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Marc Lalancette, 2022

% TODO: A bit slow, look for alternatives
% This seems similar: https://www.mathworks.com/matlabcentral/fileexchange/52882-point2trimesh-distance-between-point-and-triangulated-surface
Epsilon = 1e-9; % nanometer
nP = size(Points, 1);
nF = size(Faces, 1);

% Prepare surface quantities, independent of points
% (In bst_meshfit, this can be done only once before iterative fitting.)
% Edges as indices
Edges = unique(sort([Faces(:,[1,2]); Faces(:,[2,3]); Faces(:,[3,1])], 2), 'rows');
% Edge direction "doubly normalized" so that later projection should be between 0 and 1.
EdgeDir = Vertices(Edges(:,2),:) - Vertices(Edges(:,1),:);
EdgeL = sqrt(sum(EdgeDir.^2, 2));
EdgeDir = bsxfun(@rdivide, EdgeDir, EdgeL);
% Edges as vectors
EdgesV = zeros(nF, 3, 3);
EdgesV(:,:,1) = Vertices(Faces(:,2),:) - Vertices(Faces(:,1),:);
EdgesV(:,:,2) = Vertices(Faces(:,3),:) - Vertices(Faces(:,2),:);
EdgesV(:,:,3) = Vertices(Faces(:,1),:) - Vertices(Faces(:,3),:);
% First edge to second edge: counter clockwise = up
FaceNormals = cross(EdgesV(:,:,1), EdgesV(:,:,2));
%FaceArea = sqrt(sum(FaceNormals.^2, 2));
FaceNormals = bsxfun(@rdivide, FaceNormals, sqrt(sum(FaceNormals.^2, 2)));
% Perpendicular vectors to edges, pointing inside triangular face.
for e = 3:-1:1
    EdgeTriNormals(:,:,e) = cross(FaceNormals, EdgesV(:,:,e));
end
FaceVertices = zeros(nF, 3, 3);
FaceVertices(:,:,1) = Vertices(Faces(:,1),:);
FaceVertices(:,:,2) = Vertices(Faces(:,2),:);
FaceVertices(:,:,3) = Vertices(Faces(:,3),:);

% Check distance to vertices
if license('test','statistics_toolbox')
    DistVert = pdist2(Vertices, Points, 'euclidean', 'Smallest', 1)';
else
    DistVert = zeros(nP, 1);
    for iP = 1:nP
        % Find closest surface vertex.
        DistVert(iP) = sqrt(min(sum(bsxfun(@minus, Points(iP, :), Vertices).^2, 2)));
    end
end
% Check distance to faces
DistFace = inf(nP, 1);
for iP = 1:nP
    % Considered Möller and Trumbore 1997, Ray-Triangle Intersection (https://stackoverflow.com/questions/42740765/intersection-between-line-and-triangle-in-3d), but this is simpler still.
    % Vectors from triangle vertices to point.
    Pyramid = bsxfun(@minus, Points(iP, :), FaceVertices);
    % Does the point project inside each face?
    InFace = all(sum(Pyramid .* EdgeTriNormals, 2) > -Epsilon, 3);
    if any(InFace)
        DistFace(iP) = min(abs(sum(Pyramid(InFace,:,1) .* FaceNormals(InFace,:), 2)));
    end
end
% Check distance to edges
DistEdge = inf(nP, 1);
for iP = 1:nP
    % Vector from first edge vertex to point.
    Pyramid = bsxfun(@minus, Points(iP, :), Vertices(Edges(:, 1), :));
    Projection = sum(Pyramid .* EdgeDir, 2);
    InEdge = Projection > -Epsilon & Projection < (EdgeL + Epsilon);
    if any(InEdge)
        DistEdge(iP) = sqrt(min(sum((Pyramid(InEdge,:) - bsxfun(@times, Projection(InEdge), EdgeDir(InEdge,:))).^2, 2)));
    end
end

Dist = min([DistVert, DistEdge, DistFace], [], 2);
end


function pass = test_abs( )
% Test abs in SPHEREFUN

tol = 1000*chebfunpref().cheb2Prefs.chebfun2eps;

f = spherefun(@(x,y,z) -(x.^2 + y.^2 + z.^2));
pass(1) = norm(abs(f) + f, inf) < tol;

end


function varargout = implies(varargin)
%IMPLIES Logical implication
%
% IMPLIES(X,Y) creates a mixed integer representation of
% the constraint X --> Y, i.e. Y is true if X is true.
%
% Syntax
%   F = implies(X,Y,tol)
%
% Input
%   X : binary SDPVAR variable or a constraint
%   Y : binary SDPVAR variable or a constraint
%  tol: Optional threshhold for defining zero (see NOTE)
%
% Output
%   F : SET object
%
% Examples
%
%  binvar X Y; F = implies(X,Y);
%  binvar X;sdpvar Y; F = [implies(X,Y>=5), -10 <= Y <= 100];
%  binvar X;Y=sdpvar(3,1); F = [implies(X,[sum(Y);Y(2)]>=[5;0]), -1<= Y <= 10];
%
% Note
%
%  All variables in the expressions have to be explicitly bounded somewhere
%  in the model (implicit constraints are not sufficients such as [Y <= Z,...,Z<= 10]
%
%  Using implies with X non-binary is highly sensitive numerically.
%  The problem comes from the definition of 0 in a floating-point
%  environment, and precision in the solver. To account for this,
%  the user can supply a third argument to define a dead-zone around
%  zero, i.e Implies(X<=0,Y) will be replaced with IMPLIES(X<=-tol,Y)
%  Note, you typically need to tweak this number for your
%  application/solver. By default, YALMIP uses tol = 0, which means you
%  easily can get garbage... A positive number means YALMIP is cautious in
%  terms of activating the condition, while a negative number means YALMIP
%  will be aggressive  in activating the condition.
%
%   See also @SDPVAR/AND, @SDPVAR/OR, IFF

% There are some cases to take care of...
%
% X --> Y     binary/binary :                     Implemented
% X --> Y     binary/(LP,equality,sdp)            Implemented
% X --> Y     (LP,equality,sdp)/binary            Not implemented
% X --> Y     (LP,equality,sdp)/(LP,equality,sdp) Not implemented

X = varargin{1};
Y = varargin{2};

if isempty(X)
    varargout{1} = [];
end

switch class(X)

    case {'sdpvar','constraint','lmi'}

        if nargin > 2
            if ~isa(varargin{3},'double')
                error('Third argument in IMPLIES should be a numerical value (tolerance)');
            end
        end
        varargout{1} = setupMeta(lmi([]), mfilename,varargin{:});

    case 'char'
        varargout{1} = implies_internal(varargin{3:end});

    case 'logical'
        if length(X)==1
            if X
                varargout{1} = Y;
            else
                varargout{1} = [];
            end
        else
            if length(X) == length(Y)
                i = find(X);
                if isempty(i)
                    varargout{1} = [];
                else
                    varargout{1} = Y(i);
                end
            else
                error('Size mismatch in input arguments');
            end
        end
end


function test_failed=test_idft
Lr=[1, 19, 20];

test_failed=0;

disp(' ===============  TEST_IDFT ==============');

for jj=1:length(Lr)
  L=Lr(jj);
    for n = 1:2

    if (n==1)
       type = 'complex';
       c=tester_crand(L,1);
    elseif (n==2)
       type = 'real';
       c=tester_rand(L,1);
    end

    f1=idft(c);
    f2=ref_idft(c);

    res=norm(f1-f2);
    [test_failed,fail]=ltfatdiditfail(res,test_failed);
    s=sprintf('IDFT %6s  L:%3i %0.5g %s',type,L,res,fail);
    disp(s);
    end
  end;
end;


function [Population,FrontNo,CrowdDis] = EnvironmentalSelection(Population,N,Psi)
% The environmental selection of NSGA-II-conflict

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

    Selected = zeros(1,N);
    FrontNo  = zeros(1,N);
    CrowdDis = zeros(1,N);
    PopObj   = Population.objs;
    for i = 1 : length(Psi)
        index = (i-1)*ceil(N/length(Psi))+1 : min(N,i*ceil(N/length(Psi)));
        [Selected(index),FrontNo(index),CrowdDis(index)] = SubSelection(PopObj(:,Psi{i}),length(index));
    end
    Population = Population(Selected);
end

function [Next,FrontNo,CrowdDis] = SubSelection(PopObj,N)
% Environmental selection based on only several objectives

    %% Non-dominated sorting
    [FrontNo,MaxFNo] = NDSort(PopObj,N);
    Next = FrontNo < MaxFNo;

    %% Calculate the crowding distance of each solution
    CrowdDis = CrowdingDistance(PopObj,FrontNo);

    %% Select the solutions in the last front based on their crowding distances
    Last     = find(FrontNo==MaxFNo);
    [~,Rank] = sort(CrowdDis(Last),'descend');
    Next(Last(Rank(1:N-sum(Next)))) = true;

    %% Population for next generation
    Next     = find(Next);
    FrontNo  = FrontNo(Next);
    CrowdDis = CrowdDis(Next);
end


function high_card_simulation_test02 ( )

%*****************************************************************************80
%
%% HIGH_CARD_SIMULATION_TEST02 plots the results for a deck of 100 cards.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 March 2012
%
%  Author:
%
%    John Burkardt
%
  deck_size = 100;
  trial_num = 1000;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'HIGH_CARD_SIMULATION_TEST02\n' );
  fprintf ( 1, '  Using %d cards and %d trials, compute the chances of\n', deck_size, trial_num );
  fprintf ( 1, '  picking the high card with a skip of 0 through 99.\n' );

  p = zeros ( deck_size, 1 );

  for skip_num = 0 : deck_size - 1

    p(skip_num+1) = high_card_simulation ( deck_size, skip_num, trial_num );

  end

  plot ( 0:deck_size-1, p, 'b', 'Linewidth', 2 )
  grid on
  title ( 'Estiamted chance of winning per given skip number' )
  xlabel ( 'Number of cards to skip before choice' )
  ylabel ( 'Chance of correct choice.' )

  print ( '-dpng', 'high_card_simulation_test02.png' )
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Data plotted in "high_card_simulation_test02.png".\n' );

  return
end


function [ abd, rcond, info ] = cpbco ( abd, lda, n, m )

%*****************************************************************************80
%
%% CPBCO factors a complex hermitian positive definite band matrix.
%
%  Discussion:
%
%    The routine also estimates the condition number of the matrix.
%
%    If RCOND is not needed, CPBFA is slightly faster.
%
%    To solve A*X = B, follow CPBCO by CPBSL.
%
%    To compute inverse(A)*C, follow CPBCO by CPBSL.
%
%    To compute determinant(A), follow CPBCO by CPBDI.
%
%  Band storage:
%
%    If A is a hermitian positive definite band matrix,
%    the following program segment will set up the input.
%
%      m = (band width above diagonal)
%      do j = 1, n
%        i1 = max ( 1, j-m )
%        do i = i1, j
%          k = i-j+m+1
%          abd(k,j) = a(i,j)
%        end
%      end
%
%    This uses M+1 rows of A, except for the M by M
%    upper left triangle, which is ignored.
%
%  Example:
%
%    If the original matrix is
%
%      11 12 13  0  0  0
%      12 22 23 24  0  0
%      13 23 33 34 35  0
%       0 24 34 44 45 46
%       0  0 35 45 55 56
%       0  0  0 46 56 66
%
%    then N = 6, M = 2 and ABD should contain
%
%       *  * 13 24 35 46
%       * 12 23 34 45 56
%      11 22 33 44 55 66
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input/output, complex ABD(LDA,N); on input, the matrix to be factored.
%    The columns of the upper triangle are stored in the columns of ABD,
%    and the diagonals of the upper triangle are stored in the rows of ABD.
%
%    Input, integer LDA, the leading dimension of ABD.
%    LDA must be at least M+1.
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer M, the number of diagonals above the main diagonal.
%    0 <= M < N.
%
%    Output, complex ABD(LDA,N);an upper triangular matrix R, stored in band
%    form, so that A = hermitian(R) * R.  If INFO ~= 0, the factorization
%    is not complete.
%
%    Output, real RCOND, an estimate of the reciprocal condition of
%    the matrix.  For the system A*X = B, relative perturbations in A and B
%    of size EPSILON may cause relative perturbations in X of size
%    (EPSILON/RCOND).  If RCOND is so small that the logical expression
%      1.0 + RCOND == 1.0
%    is true, then A may be singular to working precision.  In particular,
%    RCOND is zero if exact singularity is detected or the estimate underflows.
%
%    Output, integer INFO.
%    0, for normal return.
%    K, signals an error condition.  The leading minor of order K is not
%    positive definite.
%
%  Local parameters:
%
%    Local, complex Z(N), a work vector whose contents are usually
%    unimportant.  If A is singular to working precision, then Z is
%    an approximate null vector in the sense that
%    norm ( A * Z ) = RCOND * norm ( A ) * norm ( Z ).
%    If INFO ~= 0, Z is unchanged.
%

%
%  Find the norm of A.
%
  for j = 1 : n

    l = min ( j, m + 1 );
    mu = max ( m + 2 - j, 1 );
    z(j) = scasum ( l, abd(mu:mu+l-1,j), 1 );
    k = j - l;

    for i = mu : m
      k = k + 1;
      z(k) = real ( z(k) ) + cabs1 ( abd(i,j) );
    end

  end

  anorm = 0.0;
  for j = 1 : n
    anorm = max ( anorm, real ( z(j) ) );
  end
%
%  Factor.
%
  [ abd, info ] = cpbfa ( abd, lda, n, m );

  if ( info ~= 0 )
    return
  end
%
%  RCOND = 1/(norm(A)*(estimate of norm(inverse(A)))).
%
%  Estimate = norm(Z)/norm(Y) where A*Z = Y and A*Y = E.
%
%  The components of E are chosen to cause maximum local
%  growth in the elements of W where hermitian(R)*W = E.
%
%  The vectors are frequently rescaled to avoid overflow.
%
%  Solve hermitian(R)*W = E.
%
  ek = 1.0;
  z(1:n) = 0.0;

  for k = 1 : n

    if ( cabs1 ( z(k) ) ~= 0.0 )
      ek = csign1 ( ek, -z(k) );
    end

    if ( real ( abd(m+1,k) ) < cabs1 ( ek - z(k) ) )
      s = real ( abd(m+1,k) ) / cabs1 ( ek - z(k) );
      z(1:n) = z(1:n) * s;
      ek = s * ek;
    end

    wk = ek - z(k);
    wkm = - ek - z(k);
    s = cabs1 ( wk );
    sm = cabs1 ( wkm );
    wk = wk / abd(m+1,k);
    wkm = wkm / abd(m+1,k);
    j2 = min ( k + m, n );
    i = m + 1;

    if ( k+1 <= j2 )

      for j = k+1 : j2
        i = i - 1;
        sm = sm + cabs1 ( z(j) + wkm * conj ( abd(i,j) ) );
        z(j) = z(j) + wk * conj ( abd(i,j) );
        s = s + cabs1 ( z(j) );
      end

      if ( s < sm )
        t = wkm - wk;
        wk = wkm;
        i = m + 1;
        for j = k+1 : j2
          i = i - 1;
          z(j) = z(j) + t * conj ( abd(i,j) );
        end
      end

    end

    z(k) = wk;

  end

  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
%
%  Solve R * Y = W.
%
  for k = n : -1 : 1

    if ( real ( abd(m+1,k) ) < cabs1 ( z(k) ) )
      s = real ( abd(m+1,k) ) / cabs1 ( z(k) );
      z(1:n) = z(1:n) * s;
    end

    z(k) = z(k) / abd(m+1,k);
    lm = min ( k - 1, m );
    la = m + 1 - lm;
    lb = k - lm;
    t = -z(k);
    z(lb:lb+lm-1) = z(lb:lb+lm-1) + t * transpose ( abd(la:la+lm-1,k) );

  end

  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
  ynorm = 1.0;
%
%  Solve hermitian(R)*V = Y.
%
  for k = 1 : n

    lm = min ( k - 1, m );
    la = m + 1 - lm;
    lb = k - lm;
    z(k) = z(k) - abd(la:la+lm-1,k)' * transpose ( z(lb:lb+lm-1) );

    if ( real ( abd(m+1,k) ) < cabs1 ( z(k) ) )
      s = real ( abd(m+1,k) ) / cabs1 ( z(k) );
      z(1:n) = z(1:n) * s;
      ynorm = s * ynorm;
    end

    z(k) = z(k) / abd(m+1,k);

  end

  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
  ynorm = s * ynorm;
%
%  Solve R * Z = W.
%
  for k = n : -1 : 1

    if ( real ( abd(m+1,k) ) < cabs1 ( z(k) ) )
      s = real ( abd(m+1,k) ) / cabs1 ( z(k) );
      z(1:n) = z(1:n) * s;
      ynorm = s * ynorm;
    end

    z(k) = z(k) / abd(m+1,k);
    lm = min ( k - 1, m );
    la = m + 1 - lm;
    lb = k - lm;
    t = -z(k);
    z(lb:lb+lm-1) = z(lb:lb+lm-1) + t * transpose ( abd(la:la+lm-1,k) );

  end
%
%  Make ZNORM = 1.
%
  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
  ynorm = s * ynorm;

  if ( anorm ~= 0.0 )
    rcond = ynorm / anorm;
  else
    rcond = 0.0;
  end

  return
end


function tet_mesh_test006 ( )

%*****************************************************************************80
%
%% TET_MESH_TEST006 tests TET_MESH_TET_NEIGHBORS.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 August 2009
%
%  Author:
%
%    John Burkardt
%
  tet_order = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TET_MESH_TEST006\n' );
  fprintf ( 1, '  TET_MESH_TET_NEIGHBORS computes the 4 neighboring\n' );
  fprintf ( 1, '  tetrahedrons of each tetrahedron in a tet mesh.\n' );
  fprintf ( 1, '  containing a point.\n' );
%
%  Set up the example tetrahedron mesh.
%
  [ node_num, tet_num ] = tet_mesh_order4_example_size ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  This mesh has tetrahedron order %d\n', tet_order );
  fprintf ( 1, '  The number of tetrahedrons is   %d\n', tet_num );

  [ node_xyz, tet_node ] = tet_mesh_order4_example_set ( node_num, tet_num );
%
%  Print the tets.
%
  i4mat_transpose_print_some ( tet_order, tet_num, tet_node, ...
    1, 1, tet_order, 10, '  First 10 Tets:' );
%
%  The TET_NEIGHBOR array is needed by TET_MESH_DELAUNAY_SEARCH.
%
  tet_neighbor = tet_mesh_neighbor_tets ( tet_order, tet_num, tet_node );

  i4mat_transpose_print_some ( 4, tet_num, tet_neighbor, ...
    1, 1, 4, 10, '  First 10 Tet Neighbors:' );

  return
end


 function x = dtft_adj_arrayfun(X, omega, Nd, n_shift, usearrayfun)
%function x = dtft_adj(X, omega, Nd, n_shift, usecellfun)
%|
%| Compute adjoint of d-dim DTFT for spectrum X at frequency locations omega
%|
%| in
%|	X	[M L]		dD DTFT values
%|	omega	[M d]		frequency locations (radians)
%|	n_shift [d 1]		use [0:N-1]-n_shift (default [0 ... 0])
%|	usearrayfun			1 to reduce memory use (slower)
%| out
%|	x	[(Nd) L]	signal values
%|
%| Requires enough memory to store M * (*Nd) size matrices. (For testing only.)
%|
%| Copyright 2003-4-13, Jeff Fessler, University of Michigan
%| Revised 2013-3-22, Daniel Weller, University of Michigan

% if no arguments, then run a simple test
if nargin < 2
	help(mfilename)
	Nd = [4 6 5]; Nd = [50 20 10];
	n_shift = [2 1 3];
	n_shift = 0*[2 1 3]; n_shift = [20 10 4];
	% test with uniform frequency locations:
	o1 = 2*pi*[0:(Nd(1)-1)]'/Nd(1);
	o2 = 2*pi*[0:(Nd(2)-1)]'/Nd(2);
	o3 = 2*pi*[0:(Nd(3)-1)]'/Nd(3);
	[o1 o2 o3] = ndgrid(o1, o2, o3);
	X = o1 + o2 - o3; % test spectrum
	om = [o1(:) o2(:) o3(:)];
	td = tic(); xd = dtft_adj(X(:), om, Nd, n_shift); td = toc(td);
	tl = tic(); xl = dtft_adj(X(:), om, Nd, n_shift, 1); tl = toc(tl);
	printm('loop max %% difference = %g (te = %g/%g)', max_percent_diff(xl,xd), tl, td)
	tc = tic(); xc = dtft_adj_arrayfun(X(:), om, Nd, n_shift, 1); tc = toc(tc);
	printm('arrayfun max %% difference = %g (te = %g/%g)', max_percent_diff(xc,xd), tc, td)
	Xp = X .* reshape(exp(-1i * om * n_shift(:)), size(X));
	xf = ifftn(Xp) * prod(Nd);
	printm('ifft max %% difference = %g', max_percent_diff(xf,xd))
return
end

if ~isvar('n_shift') || isempty(n_shift), n_shift = zeros(size(Nd)); end
if ~isvar('usearrayfun') || isempty(usearrayfun), usearrayfun = 0; end

% if length(Nd) == 1
%	nn{1} = [0:(Nd(1)-1)] - n_shift(1);
% elseif length(Nd) == 2
%	nn{1} = [0:(Nd(1)-1)] - n_shift(1);
%	nn{2} = [0:(Nd(2)-1)] - n_shift(2);
%	[nn{1} nn{2}] = ndgrid(nn{1}, nn{2});
% elseif length(Nd) == 3
%	nn{1} = [0:(Nd(1)-1)] - n_shift(1);
%	nn{2} = [0:(Nd(2)-1)] - n_shift(2);
%	nn{3} = [0:(Nd(3)-1)] - n_shift(3);
%	[nn{1} nn{2} nn{3}] = ndgrid(nn{1}, nn{2}, nn{3});
% else
%	'only 1D-3D done'
% end
dd = size(omega,2);
Nd(end+1:dd) = 1;
n_shift(end+1:dd) = 0;

nn = cell(1,dd);
for id=1:dd % fixed: dd
	nn{id} = (0:(Nd(id)-1))-n_shift(id);
end
[nn{:}] = ndgrid(nn{:}); % fixed: dd
nn = cellfun(@(x) col(x),nn,'UniformOutput',false);
nn = cat(2,nn{:}); % [*Nd dd]
omega = omega.'; % [dd M]

if usearrayfun
    x = arrayfun(@(n) exp(1i.*(nn(n,:)*omega)) * X,1:prod(Nd),'UniformOutput',false);
    x = cat(1,x{:});
else
    x = exp(1i.*(nn*omega)) * X;
end
x = reshape(x, [Nd ncol(X)]); % [Nd L]


classdef SymmetricFourthOrder3DVoigtTensor <  AbstractTensor ...
                                              & FourthOrderDescriptor ...
                                              & VoigtRepresentation ...
                                              & Elasticity3dDescriptor

    methods (Access = public)

        function obj = SymmetricFourthOrder3DVoigtTensor()
        end

        function createRandomTensor(obj)
            obj.createRandomTensor@AbstractTensor();
            obj.makeSymmetrization();
        end
    end

    methods (Access = protected)

        function makeSymmetrization(obj)
            t = obj.getValue;
            ts = 0.5*(t + t');
            obj.setValue(ts);
        end

        function loadTensorSize(obj)
            obj.tensorSize = [6,6];
        end
    end

end


function c = times(a,b)
% implements e .* c and c .* e
%
% Description
% If |e| is a matrix of embeddings and |c| is a matrix of coefficients
% then |e .* c| is again a matrix of embeddings defined by
%
% $$ [\mathrm{e .* c}]_{j\ell} = mathrm{e}_{j\ell} \mathrm{c}_{j \ell}$$
%
% Syntax
%   out = e .* c
%   out = c .* e
%   out = e1 .* e2
%
% Input
%  e, e1, e2 - @embedding
%  c - double
%
% Output
%  out- @embedding
%

if isa(a,'embedding')

  if isa(b,'embedding')

    for i = 1:length(a.u), a.u{i} = a.u{i} .* b.u{i}; end

  else
    for i = 1:length(a.u), a.u{i} = a.u{i} .* b; end
  end
  c = a;

else

  for i = 1:length(b.u), b.u{i} = a .* b.u{i}; end
  c = b;

end

end


%  INTERNAL FUNCTION: generate candidates for optimization
%
%  ::
%
%    [x,f,viol,funevals]=generate_candidates(objective,lb,ub,n,...
%        restrictions,penalty,varargin)
%
%  Args:
%     - **objective** [function_handle]: objective to minimize
%     - **lb** [vector]: lower bound of the search space
%     - **ub** [vector]: upper bound of the search space
%     - **n** [integer]: number of candidates to generate
%     - **max_trials** [integer]: number of trials after which the procedure
%       crashes
%     - **restrictions** [empty|function_handle]: function evaluating the
%       violations
%     - **opt** [struct]: structure with fields
%
%       - **restrictions_in_objective** [true|false]:
%       - **returns_retcode** [true|false]:
%       - **restrictions_same_weights** [true|{false}]:
%       - **allow_restrictions_violations** [true|{false}]:
%
%     - **penalty** [numeric]: value functions exceeding this value in
%       absolute value are assigned this value
%     - **varargin** []: additional input arguments for the objective function
%
%  Returns:
%     :
%
%     - **x** [d x n matrix]: parameter vectors
%     - **f** [row vector]: value function for each parameter vector
%     - **viol** [row vector]: violation penalty for each parameter vector
%     - **funevals** [integer]: number of function evaluations
%
%


function cleanClass = cleanSingletonWhitePoints(class);
%
%Author:  RFD, BW
%Date:
%Purpose:
%   Remove singleton white matter points from each plane
%

% Mask will find points that are singletons
mask = -1*ones(3,3);
mask(2,2) = 1;

%
im = class.data;
imSize = size(im);
cleanIm = im;

%

changes = [];
for ii=1:imSize(1);
    curSlice = squeeze(cleanIm(ii,:,:));
    tst = conv2(curSlice,mask,'same');
    changes = [changes, sum(sum(tst > 0))];
    curSlice(tst > 0) = class.type.unknown;
    cleanIm(ii,:,:) = curSlice;
end
changes, sum(changes)

changes = [];
for ii=1:imSize(2);
    curSlice = squeeze(cleanIm(:,ii,:));
    tst = conv2(curSlice,mask,'same');
    changes = [changes, sum(sum(tst > 0))];
    curSlice(tst > 0) = class.type.unknown;
    cleanIm(:,ii,:) = curSlice;
end
changes, sum(changes)

changes = [];
for ii=1:imSize(3);
    curSlice = squeeze(cleanIm(:,:,ii));
    tst = conv2(curSlice,mask,'same');
    changes = [changes, sum(sum(tst > 0))];
    curSlice(tst > 0) = class.type.unknown;
    cleanIm(:,:,ii) = curSlice;
end
changes, sum(changes)

% Assign results to returned structure
cleanClass = class;
cleanClass.data = cleanIm;


classdef LogarithmNode < GraphNode
    properties
        const = 1e-2;
    end

    methods
        function obj = LogarithmNode(dimOut, const)
            obj = obj@GraphNode('Logarithm',dimOut);
            if nargin>=2
                obj.const = const;
            end
        end

        function obj = forward(obj,prev_layers)
            obj = obj.preprocessingForward(prev_layers);
            input = prev_layers{1}.a;
            [D,T,N] = size(input);

            if strcmpi(class(gather(input(1))), 'single')
                obj.const = single(obj.const);
            end

            if N==1
                obj.a = log(input+obj.const);
            else
                if obj.variableLength
                    input = obj.PadShortTrajectory(input, 0);
                    if sum(input(:)+obj.const<=0)
                        fprintf('error: input to log is negative');
                    end
                    obj.a = log(input+obj.const);
                else
                    obj.a = log(input+obj.const);
                end
            end
            obj = forward@GraphNode(obj, prev_layers);
        end

        function obj = backward(obj,prev_layers, future_layers)
            future_grad = obj.GetFutureGrad(future_layers);
            input = prev_layers{1}.a;
            obj.grad{1} = 1./(input+obj.const).*future_grad;
        end
    end
end


function mori = parents(mori)
% variants of an orientation relationship
%
% Syntax
%
%   ori_parents = ori_child * inv(mori.parents)
%
% Input
%  mori - child to parent @orientation relationship
%  ori_child - child orientation
%
% Output
%  ori_parents - all possible parent @orientation
%
% Example
%   % parent symmetry
%   cs_fcc = crystalSymmetry('m-3m', [3.6599 3.6599 3.6599], 'mineral', 'Iron fcc');
%
%   % child symmetry
%   cs_bcc = crystalSymmetry('m-3m', [2.866 2.866 2.866], 'mineral', 'Iron bcc')
%
%   % define a bcc child orientation
%   ori_bcc = orientation.goss(cs_bcc)
%
%   % define Nishiyama Wassermann fcc to bcc orientation relation ship
%   NW = orientation.NishiyamaWassermann (cs_fcc,cs_bcc)
%
%   % compute a fcc parent orientation related to the bcc child orientation
%   ori_fcc = ori_bcc * NW
%
%   % compute all symmetrically possible parent orientations
%   ori_fcc = unique(ori_bcc.symmetrise * NW)
%
%   % same using the function parents
%   ori_fcc2 = ori_bcc * NW.parents
%
% See also
% orientation/variants
%

% store child symmetry
CS_child = mori.SS;

% symmetrise only with respect to child symmetry
mori = CS_child * mori;

% ignore all variants symmetrically equivalent
% with respect to the parent symmetry
mori.SS = crystalSymmetry('1');
mori = unique(mori);
mori.SS = CS_child;


function pass = test_divide(pref)
% Test contour

if ( nargin == 0)
    pref = chebfunpref;
end
tol = 1000*pref.cheb3Prefs.chebfun3eps;

f = chebfun3(@(x,y,z) cos(x.*y.*z));
g = chebfun3(@(x,y,z) cos(x.*y.*z)./2);

pass(1) = norm( f./2 - g ) < tol;

pass(2) = norm( f/2 - g ) < tol;

pass(3) = norm( 2.\f - g ) < tol;

pass(4) = norm( 2\f - g ) < tol;

end


clear
clc

A0=imread('cam.jpg');
% chk=[1 1 1 3 1 1 2 1 1 2 3 2 2 2 1 1 1 2 3 1 1 4 1 3 2 1 1,...
%     1 1 1 1 1 2 1 2 2 3 1 1 2 1 2 3 2 1 4 1 1 1 1 3 2 2 2 2,...
%     1 1 1 1]; % for cam1
% chk=[1 1 1 3 1 1 2 1 1 2 3 2 1 2 2 1 1 2 3 3 1 2 1 3 2, ...
%     1 1 1 1 1 1 1 3 2 1 1 2 2 2 1 2 2 2 1 1 1 3 2 1 1 3 2, ...
%     1 2 3 1 1 1 1]; % for cam
% chk=[1 1 1 3 1 1 2 1 1 2 3 1 1 1 4 1 1 2 3 1 1 4 1 1, ...
%     1 1 4 1 1 1 1 1 1 1 1 4 1 3 1 2 3 2 1 1 1 1 3 2 2,...
%     2 2 1 3 2 1 1 1 1 1]; % for cam3
% chk=[1 1 1 1 3 1 2 3 1 2 1 1 1 2 3 3 2 1 1 2 1 3 1 3 2 1,...
%     1 1 1 1 1 1 2 2 2 1 1 1 3 2 1 1 3 2 1 3 1 2 3 1 1 2 1,...
%     2 1 3 1 1 1]; % for cambook3
A0=rgb2gray(A0);
% A=A0([90:330],[1:604]); % for cam1
A=A0([99+30:99+260],[71:608]); % for cam
% A=A0([182:389],[1:600]); % for cam3
% A=A0([193:349],[1:504]); % for cambook3
return
figure(1), imshow(A)%, return

nSeg=10;
wSeg=floor(size(A,1)/nSeg);

for i=1:nSeg

    At=A((i-1)*wSeg+1:i*wSeg,:);
    %figure(2), imshow(At)

    gsv(i,:)=255-mean(double(At),1);
    x=[1:length(gsv(i,:))]; g=gsv(i,:);
    [gw,xw]=movAvg(g,1);
    %figure(3), plot(x,gsv(i,:),xw,gw)

    dgw=diffScheme(gw);
    [pk1,loc1]=peakDetect(dgw,20,5);
    % sub-pixel edge detection
    for i1=1:length(loc1)
        [X1(i1),P1(i1)]=subPx(dgw(loc1(i1)-5:loc1(i1)+5),...
            xw(loc1(i1)-5:loc1(i1)+5),...
            dgw(loc1(i1)),100);
        %g=dgw(loc1(i)-5:loc1(i)+20); gMax=dgw(loc1(i)); save subPxData.mat g gMax
    end
    [pk2,loc2]=peakDetect(-dgw,20,5);
    for i1=1:length(loc2)
        [X2(i1),P2(i1)]=subPx(-dgw(loc2(i1)-5:loc2(i1)+5),...
            xw(loc2(i1)-5:loc2(i1)+5),...
            -dgw(loc2(i1)),100);
    end
    P2=-P2;

    if length(loc1)~=length(loc2)
        disp('oops! ... left edge & right edge don''t match!!');
    end
    %figure(4), plot(xw,dgw,'.k-',xw(loc1),dgw(loc1),'o',...
    %    xw(loc2),dgw(loc2),'o',X1,P1,'o',X2,P2,'o')

    % black gap
    bGap=X2-X1;
    % white gap
    wGap=-X2(1:end-1)+X1(2:end);

    % discretizing barcodes
    %figure(5), subplot(2,1,1), plot(sort(bGap),'ok')
    %subplot(2,1,2), plot(sort(wGap),'or'), pause(0.1)

    % working on white gaps
    [wGap2,wid]=sort(wGap);
    dwGap2=wGap2(2:end)-wGap2(1:end-1);
    [wGap21,dwid]=sort(dwGap2);
    bound(1:3)=sort(dwid(end:-1:end-2)+1);
    clear wGap21 dwid
    s1=mean(wGap2(1:bound(1)-1));
    s2=mean(wGap2(bound(1):bound(2)-1));
    s3=mean(wGap2(bound(2):bound(3)-1));
    s4=mean(wGap2(bound(3):end));
    if s4/s1 <3.5 % 4 is not present
        if s2/s1>1.5
            S1=wid(1:bound(1)-1); % s1 and s2 are exclusive
            if s3/s2<1.2 % s2 and s3 are same
                S2=wid(bound(1):bound(3)-1);
                S3=wid(bound(3):end);
            else % s2 and s3 are separate
                S2=wid(bound(1):bound(2)-1);
                S3=wid(bound(2):end);
            end
        else
            S1=wid(1:bound(2)-1); % s1 and s2 are inclusive
            S2=wid(bound(2)-1:bound(3));
            S3=wid(bound(3)-1:end);
        end
        S4=[];
    else % 4 is present
        S1=wid(1:bound(1)-1);
        S2=wid(bound(1):bound(2)-1);
        S3=wid(bound(2):bound(3)-1);
        S4=wid(bound(3):end);
    end
    %figure(6),plot(wGap(S1),'o'),hold on,plot(wGap(S2),'.'),plot(wGap(S3),'x'),hold off
    wGap3(S1)=1;
    wGap3(S2)=2;
    wGap3(S3)=3;
    if ~isempty('S4'), wGap3(S4)=4; end

    % working on black gaps
    [bGap2,bid]=sort(bGap);
    dbGap2=bGap2(2:end)-bGap2(1:end-1);
    [bGap21,dbid]=sort(dbGap2);
    bound(1:3)=sort(dbid(end:-1:end-2)+1);
    clear bGap21 dbid
    t1=mean(bGap2(1:bound(1)-1));
    t2=mean(bGap2(bound(1):bound(2)-1));
    t3=mean(bGap2(bound(2):bound(3)-1));
    t4=mean(bGap2(bound(3):end));
    if t4/t1 <3.5 % 4 is not present
        if t2/t1>1.5
            T1=bid(1:bound(1)-1); % t1 and t2 are exclusive
            if t3/t2<1.2 % t2 and t3 are same
                T2=bid(bound(1):bound(3)-1);
                T3=bid(bound(3):end);
            else % t2 and t3 are separate
                T2=bid(bound(1):bound(2)-1);
                T3=bid(bound(2):end);
            end
        else
            T1=bid(1:bound(2)-1); % t1 and t2 are inclusive
            T2=bid(bound(2)-1:bound(3));
            T3=bid(bound(3)-1:end);
        end
        T4=[];
    else % 4 is present
        T1=bid(1:bound(1)-1);
        T2=bid(bound(1):bound(2)-1);
        T3=bid(bound(2):bound(3)-1);
        T4=bid(bound(3):end);
    end
    %figure(7),plot(bGap(T1),'o'),hold on,plot(bGap(T2),'.'),plot(bGap(T3),'x'),hold off
    bGap3(T1)=1;
    bGap3(T2)=2;
    bGap3(T3)=3;
    if ~isempty('T4'), bGap3(T4)=4; end

    %wGap3=round(wGap/mean(wGap2(1:10)));
    %bGap3=round(bGap/mean(bGap2(1:10)));

    I(i).gap=zeros(length(bGap)+length(wGap),1);
    I(i).gap(1:2:end)=bGap3;
    I(i).gap(2:2:end)=wGap3;

    %disp(['segment: ',num2str(i),' | errors: ',num2str(sum((I(i).gap'-chk)~=0))])
    if i==1
        G=I(i).gap;
    else
        for j=1:length(I(i).gap)
            G(j)=G(j)+I(i).gap(j);
        end
    end

end

G=round(G/nSeg);
barcode=barcodeEAN13(G);
%reading_error=sum(sum(G'-chk))


function [newVal,newUnitStr,oldUnitStr] = unitConvert(oldVal,valueType,oldUnitStr,newUnitStr)
% [newVal,newUnitStr,oldUnitStr] = unitConvert(oldVal,valType,oldUnitStr)
%
% Value types:
%  'length' - recognized units: 'nm', 'um', 'mm', 'cm'
%  'time'   - recognized units: 'usec', 'msec', 'sec'
%
% Adapted from isetbio unitConvert
%
% Examples
%
% newVal = unitConvert(1500,'time','ms' ,'s')
% newVal = unitConvert(1,'length','cm' ,'mm')

% Switch on value type
switch (valueType)
    case 'length'
        % Length

        % Factor from old to default
        switch oldUnitStr
            case 'm'
                oldConversionFactor = 1;
            case 'nm'
                oldConversionFactor = 1e-9;
            case 'um'
                oldConversionFactor = 1e-6;
            case 'mm'
                oldConversionFactor = 1e-3;
            case 'cm'
                oldConversionFactor = 1e-2;
            otherwise
                error('Bad units %s passed for type %s',oldUnitStr,valueType);
        end

         % Factor from old to default
        switch newUnitStr
            case 'm'
                newConversionFactor = 1;
            case 'nm'
                newConversionFactor = 1e9;
            case 'um'
                newConversionFactor = 1e6;
            case 'mm'
                newConversionFactor = 1e3;
            case 'cm'
                newConversionFactor = 1e2;
            otherwise
                error('Bad units %s passed for type %s',oldUnitStr,valueType);
        end

    case 'time'
        % Time

        % Factor from old to default
        switch oldUnitStr
            case {'s' 'sec' 'seconds'}
                oldConversionFactor = 1;
            case {'ms' 'msec' 'milliseconds'}
                oldConversionFactor = 1e-3;
            otherwise
                error('Bad units %s passed for type %s',oldUnitStr,valueType);
        end

         % Factor from old to default
        switch newUnitStr
            case {'s' 'sec' 'seconds'}
                newConversionFactor = 1;
            case {'ms' 'msec' 'milliseconds'}
                newConversionFactor = 1e3;
            otherwise
                error('Bad units %s passed for type %s',oldUnitStr,valueType);
        end

    otherwise
        error('Unknown value type passed');
end

% Convert
newVal = oldConversionFactor*newConversionFactor*oldVal;

end


function [ r, z, c, s ] = schex ( r, ldr, p, k, l, z, ldz, nz, job )

%*****************************************************************************80
%
%% SCHEX updates the Cholesky factorization of a positive definite matrix.
%
%  Discussion:
%
%    The factorization has the form
%
%      A = R' * R
%
%    where A is a positive definite matrix of order P.
%
%    The updating involves diagonal permutations of the form
%
%      E' * A * E
%
%    where E is a permutation matrix.  Specifically, given
%    an upper triangular matrix R and a permutation matrix
%    E (which is specified by K, L, and JOB), SCHEX determines
%    an orthogonal matrix U such that
%
%      U * R * E = RR,
%
%    where RR is upper triangular.  At the user's option, the
%    transformation U will be multiplied into the array Z.
%    If A = X'*X, so that R is the triangular part of the
%    QR factorization of X, then RR is the triangular part of the
%    QR factorization of X*E, that is, X with its columns permuted.
%
%    For a less terse description of what SCHEX does and how
%    it may be applied, see the LINPACK guide.
%
%    The matrix Q is determined as the product U(L-K)*...*U(1)
%    of plane rotations of the form
%
%      (    C(I)       S(I) )
%      (                    ),
%      (   -S(I)       C(I) )
%
%    where C(I) is real, the rows these rotations operate on
%    are described below.
%
%    There are two types of permutations, which are determined
%    by the value of JOB.
%
%    1, right circular shift.  The columns are rearranged in the order:
%
%         1,...,K-1,L,K,K+1,...,L-1,L+1,...,P.
%
%       U is the product of L-K rotations U(I), where U(I)
%       acts in the (L-I,L-I+1)-plane.
%
%    2, left circular shift: the columns are rearranged in the order
%
%         1,...,K-1,K+1,K+2,...,L,K,L+1,...,P.
%
%       U is the product of L-K rotations U(I), where U(I)
%       acts in the (K+I-1,K+I)-plane.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real R(LDR,P), the upper triangular factor that is to be updated.
%    Elements of R below the diagonal are not referenced.
%
%    Input, integer LDR, the leading dimension of the array R.
%    LDR must be at least P.
%
%    Input, integer P, the order of the matrix R.
%
%    Input, integer K, the first column to be permuted.
%
%    Input, integer L, the last column to be permuted.
%    L must be strictly greater than K.
%
%    Input, real Z(LDZ,NZ), an array of NZ P-vectors into
%    which the transformation U is multiplied.  Z is not referenced if NZ = 0.
%
%    Input, integer LDZ, the leading dimension of the array Z.
%    LDZ must be at least P.
%
%    Input, integer NZ, the number of columns of the matrix Z.
%
%    Input, integer JOB, determines the type of permutation.
%    1, right circular shift.
%    2, left circular shift.
%
%    Output, real R(LDR,P), the updated upper triangular factor.
%
%    Output real Z(LDZ,NZ), the updated array of vectors.
%
%    Output, real C(P), S(P), the cosines and sines of the
%    transforming rotations.
%

%
%  Initialize
%
  lmk = l - k;
  lm1 = l - 1;
%
%  Right circular shift.
%
  if ( job == 1 )
%
%  Reorder the columns.
%
    for i = 1 : l
      ii = l - i + 1;
      s(i) = r(ii,l);
    end

    for jj = k : lm1
      j = lm1 - jj + k;
      for i = 1 : j
        r(i,j+1) = r(i,j);
      end
      r(j+1,j+1) = 0.0;
    end

    for i = 1 : k-1
      ii = l - i + 1;
      r(i,k) = s(ii);
    end
%
%  Calculate the rotations.
%
    t = s(1);
    for i = 1 : lmk
      [ c(i), s(i), s(i+1), t ] = srotg ( s(i+1), t );
      t = s(i+1);
    end

    r(k,k) = t;

    for j = k+1 : p
      il = max ( 1, l-j+1 );
      for ii = il : lmk
        i = l - ii;
        t = c(ii) * r(i,j) + s(ii) * r(i+1,j);
        r(i+1,j) = c(ii) * r(i+1,j) - s(ii) * r(i,j);
        r(i,j) = t;
      end
    end
%
%  If required, apply the transformations to Z.
%
    for j = 1 : nz
      for ii = 1 : lmk
        i = l - ii;
        t = c(ii) * z(i,j) + s(ii) * z(i+1,j);
        z(i+1,j) = c(ii) * z(i+1,j) - s(ii) * z(i,j);
        z(i,j) = t;
      end
    end
%
%  Left circular shift.
%
  else
%
%  Reorder the columns.
%
    for i = 1 : k
      ii = lmk + i;
      s(ii) = r(i,k);
    end

    for j = k : lm1
      for i = 1 : j;
        r(i,j) = r(i,j+1);
      end
      jj = j - k + 1;
      s(jj) = r(j+1,j+1);
    end

    for i = 1 : k
      ii = lmk + i;
      r(i,l) = s(ii);
    end

    r(k+1:l,l) = 0.0;
%
%  Reduction loop.
%
    for j = k : p
%
%  Apply the rotations.
%
      if ( j ~= k )

        iu = min ( j-1, l-1 );

        for i = k : iu
          ii = i - k + 1;
          t = c(ii) * r(i,j) + s(ii) * r(i+1,j);
          r(i+1,j) = c(ii) * r(i+1,j) - s(ii) * r(i,j);
          r(i,j) = t;
        end

      end

      if ( j < l )
        jj = j - k + 1;
        t = s(jj);
        [ c(jj), s(jj), r(j,j), t ] = srotg ( r(j,j), t );
      end

    end
%
%  Apply the rotations to Z.
%
    for j = 1 : nz
      for i = k : lm1
        ii = i - k + 1;
        t = c(ii) * z(i,j) + s(ii) * z(i+1,j);
        z(i+1,j) = c(ii) * z(i+1,j) - s(ii) * z(i,j);
        z(i,j) = t;
      end
    end

  end

  return
end


function [rstat]=AddIMUErr(finam, fonam, fenam, nrow, act_row, errdefs, tem_mod, rstat)

%randomize
if (rstat(1)~=0)
    randn('state',rstat);
else
    rstat=randn('state');
end

if isempty(tem_mod) %if no tem_mod is specified, use constant temp=0.
    tem_mod.A=1;
    tem_mod.B=0;
    tem_mod.u=0;
end

%input data
fimu=fopen(finam,'rb');
imu_inp=fread(fimu,[nrow inf],'double');
fclose(fimu);
imu_inp=imu_inp(act_row,:);

ndat=size(imu_inp,2);

%output files
feimu=fopen(fonam,'wb');
ferr=fopen(fenam,'wb');

if (isempty(errdefs))   %do not add error
     out=[0:ndat-1;zeros(1,ndat);imu_inp];
     fwrite(feimu,out,'double');
else    %generate and add errors
    %Generate the system model for the TI imu errors
    [Ati, Bti, Cti, Dti, sPti]=imu_modTI_v000(errdefs);
    nst_ti=size(Ati,1);
    st_ti=sPti*randn(nst_ti,1);

    tem=0;
    tem_dif=0;
    [Atv, Btv, Ctv, sPtv]=imu_modTV_v000(errdefs, tem_dif, tem, 1);
    if (~isempty(Atv))
        nst_tv=size(Atv,1);
        st_tv=sPtv*randn(nst_tv,1);
    else
        nst_tv=0;
    end

    %add errors
    nsen=size(Cti,1);
    if (nst_tv==0) %only ti part
        for in=1:ndat
            imu_out=imu_inp(:,in)+Cti*st_ti+Dti*randn(nsen,1);
            fwrite(ferr,[in-1;st_ti],'double');
            fwrite(feimu,[in-1;tem;imu_out],'double');

            %new error values
            tem=tem_mod.A*tem+tem_mod.B*randn(1)+tem_mod.u;
            st_ti=Ati*st_ti+Bti*randn(nst_ti,1);
        end
    else    %both ti and tv parts are generated
        for in=1:ndat
            imu_out=imu_inp(:,in)+Cti*st_ti+Dti*randn(nsen,1)+Ctv*st_tv;
            fwrite(ferr,[in-1;st_ti;st_tv],'double');
            fwrite(feimu,[in-1;tem;imu_out],'double');

            %new error values
            sr_a=tem_mod.A*tem+tem_mod.B*randn(1)+tem_mod.u;
            tem_dif=sr_a-tem;
            tem=sr_a;
            [Atv, Btv, Ctv]=imu_modTV_v000(errdefs, tem_dif, tem, 0);
            st_tv=Atv*st_tv+Btv*randn(nst_tv,1);
            st_ti=Ati*st_ti+Bti*randn(nst_ti,1);
        end
    end
end
fclose(feimu);
fclose(ferr);


classdef MultiObjectiveEGO < ALGORITHM
% <multi> <real/integer> <constrained/none> <expensive>
% Multi-objective efficient global optimization
% alpha --- 0.7 --- portion of samples for Kriging construction
% num_k ---   5 --- number of infill points per iteration
% H     ---  21 --- number of reference directions

%------------------------------- Reference --------------------------------
% R. Hussein, K. Deb, A Generative Kriging Surrogate Model for Constrained
% and Unconstrained Multi-objective Optimization, in: Proc. Genet. Evol.
% Comput. Conf. 2016, Denver, 2016, 573-580.
%--------------------------------------------------------------------------

% This function is written by Youwei He (email: 1554748356@qq.com)

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            [alpha,num_k,H] = Algorithm.ParameterSet(0.7,5,21);
            % parameter for AASF in equation (10)
            rho = 1e-3;
            %% Generate the initial design points
            % number of design variables
            D = Problem.D;
            % number of initial design points 11*D-1
            N = Problem.N;
            %% step1-1: generate initial design points using Latin Hypercube sampling
            PopDec = repmat(Problem.upper-Problem.lower,N,1).*UniformPoint(N,D,'Latin') ...
                +repmat(Problem.lower,N,1);
            %% step1-2: evaluate initial design points
            Population = Problem.Evaluation(PopDec);

            %% step 2: Generate the reference direction set
            [R,N_R] = UniformPoint(H,Problem.M);
            R = R./sqrt(sum(R.^2,2));
            %% Optimization
            while Algorithm.NotTerminated(Population)
                %% step 2 diversity_preserver procedure: neighborhood approach
                i_direction = linspace(1,N_R,N_R);
                for i = i_direction
                    for j = 1 : num_k
                        Algorithm.NotTerminated(Population);
                        PopDecT = Population.decs;
                        PopObjT = Population.objs;
                        PopConT = Population.cons;% cons>=0: meet constraints
                        num_con = size(PopConT,2);
                        N_Pop=length(PopDecT(:,1));
                        % determine whether the problem is constrained or not
                        if all(all(PopConT==0)) && N_Pop==1
                            constrained=1;
                        else
                            constrained=0;
                        end
                        %% step 3: Points_Selector procedure
                        normW = sqrt(sum(R(i,:).^2,2));
                        normP = sqrt(sum(PopObjT.^2,2));
                        CosineP = sum((PopObjT).*R(i,:),2)./normP./normW;
                        % orthogonal distance of each point to the given reference direction
                        distB = normP.*sqrt(1-CosineP.^2);
                        [~,indx] = sort(distB);
                        N = ceil(alpha*N_Pop);
                        PopObj = PopObjT(indx(1:N),:);
                        PopDec = PopDecT(indx(1:N),:);
                        PopCon = PopConT(indx(1:N),:);
                        %% step 4-1
                        index = sum(PopCon >= 0, 2) == num_con;% feasible solutions
                        PopCon(index,:)=0;
                        PopCon(~index,:)=-PopCon(~index,:);
                        PopObjScaled = (PopObj-repmat(min(PopObj),N,1))./(repmat(max(PopObj),N,1)-repmat(min(PopObj),N,1));
                        if constrained
                            PopConScaled = (PopCon-repmat(min(PopCon),N,1))./(repmat(max(PopCon),N,1)-repmat(min(PopCon),N,1));
                        end
                        %% step 4-2
                        PopSmetric = zeros(N,1);
                        PopSmetricT = PopObjScaled(index,:)./repmat(R(i,:),sum(index),1);
                        PopSmetric(index,:)=max(PopSmetricT,[],2)+rho*sum(PopSmetricT,2);
                        if constrained
                            ASF_max = max(PopSmetric(index,1));
                            PopSmetric(~index,:) = ASF_max.*ones(sum(~index),1) + sum(PopConScaled(~index,:),2);
                        end
                        %% step 4-3
                        kriging_obj= dacefit(PopDec,PopSmetric,'regpoly0','corrgauss',1*ones(1,D),0.001*ones(1,D),1000*ones(1,D));
                        f_min = min(PopSmetric);
                        %% step 5: Optimization
                        infill_criterion = @(x)Infill_Standard_EI(x, kriging_obj, f_min);
                        best_x=rGA(infill_criterion,Problem);
                        % infill point too close (not used in the paper,
                        % but this happens sometimes especially for low dimensional problem)
                        if min(sqrt(sum((PopDecT-best_x).^2,2)))<1E-8
                            best_x = rGA(@(x)Infill_Maximal_Distance(x, PopDecT),Problem);
                        end
                        Population = [Population,Problem.Evaluation(best_x)];
                    end
                end
            end
        end
    end
end