function [qx,qP,qD,xhist] = spm_pf(M,y,U)
% Particle Filtering for dynamic models
% FORMAT [qx,qP,qD,xhist] = spm_pf(M,y)
% M - model specification structure
% y - output or data (N x T)
% U - exogenous input
%
% M(1).x                            % initial states
% M(1).f  = inline(f,'x','v','P')   % state equation
% M(1).g  = inline(g,'x','v','P')   % observer equation
% M(1).pE                           % parameters
% M(1).V                            % observation noise precision
%
% M(2).v                            % initial process noise
% M(2).V                            % process noise precision
%
% qx - conditional expectation of states
% qP - {1 x T} conditional covariance of states
% qD - full sample
%__________________________________________________________________________
% See notes at the end of this script for details and a demo.  This routine
% is based on:
%
% var der Merwe R, Doucet A, de Freitas N and Wan E (2000). The
% unscented particle filter.  Technical Report CUED/F-INFENG/TR 380
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_pf.m 1143 2008-02-07 19:33:33Z spm $

% check model specification
%--------------------------------------------------------------------------
M  = spm_DEM_M_set(M);
dt = M(1).E.dt;
if length(M) ~=2
    errordlg('spm_pf requires a two-level model')
    return
end

% INITIALISATION:
%==========================================================================
T    = length(y);                          % number of time points
n    = M(2).l;                             % number of innovations
N    = 200;                                % number of particles.

% precision of measurement noise
%--------------------------------------------------------------------------
R    = M(1).V;
for i = 1:length(M(1).Q)
    R = R + M(1).Q{i}*exp(M(1).h(i));
end
P  = M(1).pE;                              % parameters
Q  = M(2).V.^-.5;                          % root covariance of innovations
v  = kron(ones(1,N),M(2).v);               % innovations
x  = kron(ones(1,N),M(1).x);               % hidden states
v  = v + 128*Q*randn(size(v));

% inputs
%--------------------------------------------------------------------------
if nargin < 3
    U = sparse(n,T);
end

for t = 1:T

    % PREDICTION STEP: with the (8x) transition prior as proposal
    %----------------------------------------------------------------------
    for i = 1:N
        v(:,i)     = 8*Q*randn(n,1) + U(:,t);
        f          = M(1).f(x(:,i),v(:,i),P);
        dfdx       = spm_diff(M(1).f,x(:,i),v(:,i),P,1);
        xPred(:,i) = x(:,i) + spm_dx(dfdx,f,dt);
    end

    % EVALUATE IMPORTANCE WEIGHTS: and normalise
    %----------------------------------------------------------------------
    for i = 1:N
        yPred  = M(1).g(xPred(:,i),v(:,i),P);
        ePred  = yPred - y(:,t);
        w(i)   = ePred'*R*ePred;
    end
    w   = w - min(w);
    w   = exp(-w/2);
    w   = w/sum(w);

    % SELECTION STEP: multinomial resampling.
    %----------------------------------------------------------------------
    x   = xPred(:,multinomial(1:N,w));

    % report and record moments
    %----------------------------------------------------------------------
    qx(:,t)  = mean(x,2);
    qP{t}    = cov(x');
    qX(:,t)  = x(:);
    fprintf('PF: time-step = %i : %i\n',t,T);
end

% sample density
%==========================================================================
if nargout > 3
    xhist = linspace(min(qX(:)),max(qX(:)),32);
    for i = 1:T
        q = hist(qX(:,i),xhist);
        qD(:,i) = q(:);
    end
end

return

function I = multinomial(inIndex,q);
%==========================================================================
% PURPOSE : Performs the resampling stage of the SIR
%           in order(number of samples) steps.
% INPUTS  : - inIndex = Input particle indices.
%           - q       = Normalised importance ratios.
% OUTPUTS : - I = Resampled indices.
% AUTHORS : Arnaud Doucet and Nando de Freitas

% MULTINOMIAL SAMPLING:
% generate S ordered random variables uniformly distributed in [0,1]
% high speed Niclas Bergman Procedure
%--------------------------------------------------------------------------
q        = q(:);
S        = length(q);  % S = Number of particles.
N_babies = zeros(1,S);
cumDist  = cumsum(q');

u = fliplr(cumprod(rand(1,S).^(1./(S:-1:1))));
j = 1;
for i = 1:S
    while (u(1,i) > cumDist(1,j))
        j = j + 1;
    end
    N_babies(1,j) = N_babies(1,j) + 1;
end;

% COPY RESAMPLED TRAJECTORIES:
%--------------------------------------------------------------------------
index = 1;
for i = 1:S
    if (N_babies(1,i)>0)
        for j=index:index+N_babies(1,i)-1
            I(j) = inIndex(i);
        end;
    end;
    index = index + N_babies(1,i);
end

return
%==========================================================================

% notes and demo:
%==========================================================================
% The code below generates a nonlinear, non-Gaussian problem (S) comprising
% a model S.M and data S.Y (c.f. van der Merwe et al 2000))
%
% The model is   f(x) = dxdt
%                     = 1 + sin(0.04*pi*t) - log(2)*x + n
%                y    = g(x)
%                     = (x.^2)/5  : if t < 30
%                       -2 + x/2  : otherwise
% i.e. the output nonlinearity becomes linear after 30 time steps.  In this
% implementation time is modelled as an auxiliary state variable.  n is
% the process noise, which is modelled as a log-normal variate.  e is
% Gaussian observation noise.

% model specification
%--------------------------------------------------------------------------
f       = '[1; (1 + sin(P(2)*pi*x(1)) - P(1)*x(2) + exp(v))]';
g       = '(x(1) > 30)*(-2 + x(2)/2) + ~(x(1) > 30)*(x(2).^2)/5';
M(1).x  = [1; 1];                  % initial states
M(1).f  = inline(f,'x','v','P');   % state equation
M(1).g  = inline(g,'x','v','P');   % observer equation
M(1).pE = [log(2) 0.04];           % parameters
M(1).V  = exp(4);                  % observation noise precision

M(2).v  = 0;                       % initial process log(noise)
M(2).V  = 2.4;                     % process log(noise) precision

% generate data (output)
%--------------------------------------------------------------------------
T       = 60;                      % number of time points
S       = spm_DEM_generate(M,T);

% Particle filtering
%--------------------------------------------------------------------------
pf_x    = spm_pf(M,S.Y);

% plot results
%--------------------------------------------------------------------------
x       = S.pU.x{1};
plot([1:T],x(2,:),[1:T],pf_x(2,:))
legend({'true','PF'})


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 [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


% 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 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 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 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]


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 [means,stats] = weighted_reg(Y,varargin)
% Calculate weighted average using weighted linear least squares
% See examples below for usage
%
% :Model:
%
%   Y_i = 1*Ypop + noise
%
% :Inputs:
%
%   **Y:**
%        data matrix (nsub x T)
%
%   **w:**
%        weights
%
%   **varY:**
%        variance of data at each time point (nsub x T) + var between
%
% :Outputs:
%
%   **Ymean:**
%        weighted mean of each column of Y
%
%   **dfe:**
%        error degrees of freedom, adjusted for inequality of variance
%        (Sattherwaite) and pooled across data columns
%
% Extended output in stats structure:
%
%   **stats.t:**
%        t-values for weighted t-test
%
%   **stats.p:**
%        2-tailed p-values for weighted t-test
%
%
%   **r:**
%        weighted correlation coeff across columns of Y
%
%   **xy:**
%        weighted covariance matrix
%
%   **v:**
%        weighted variance estimates for each column of Y
%      - sqrt(v) is the standard error of the mean (or grp difference)
%
%
%   **stats.fits:**
%        fits  for each group (Ymean by group), low contrast weight group then high
%        Fastest if no stats are asked for.
%
% Computation time:
% For FULL stats report
%  - Triples from 500 -> 1000 columns of Y, continues to increase
%
% For mean/dfe only, fast for full dataset (many columns of Y)
%
% :Examples:
% ::
%
%    % Basic multivariate stats for 1000 columns of dat, no weighting
%    % Multivariate covariances are meaningful if cols of Y are organized, e.g., timeseries
%    [means,stats] = weighted_reg(dat(:,1:1000));
%
%    % The same, but return univariate stats only (good for large Y)
%    [means,stats] = weighted_reg(dat,'uni');
%
% ..
%    NOTE: TOR CHANGED INPUT TO ASSUME THAT WE SHOULD ENTER VARWI + VARBETWEEN
% ..

% ..
%    Set up arguments
% ..

if nargin == 0, error('Must at least enter data as 1st argument.'); end

domultivariate = 1;     % multivariate covariance est for Y
dobtwn = 0;             % between-subjects contrast

bcon = []; Ydiff = []; w = []; varY = [];

for i = 1:length(varargin)
    arg = varargin{i};
    if ischar(arg)
        switch lower(arg)
            case 'w', w = varargin{i+1};
            case 'btwn', bcon = contrast_code(varargin{i+1});
            case 'vary', varY = varargin{i+1};
            case 'uni', domultivariate = 0;
        end
    end
end

[m,n] = size(Y);

% fill in missing inputs with default values

if ~is_entered(w), w = ones(m,1);  end
if ~is_entered(varY), varY = ones(m,1); end
if is_entered(bcon), dobtwn = 1; end

% --------------------------------------
% * Weights and computational steps
% --------------------------------------

W = diag(w);                    % Weight matrix

X = repmat(1,m,1);              % Design matrix - 1 column of all ones to calculate average
% and, separately, use bcon if that's entered

invxwx = inv(X'*W*X);
hat = invxwx * X'* W;         % hat matrix

% Between-observations, if entered
% ----------------------------------------
if dobtwn
    invxwx_diff = inv(bcon'*W*bcon);
    hatdiff = invxwx_diff*bcon'*W;
else
    hatdiff = [];
end

% --------------------------------------
% * Means and contrast
% --------------------------------------

Ymean = hat*Y;

% Output: weighted population mean
means.Ymean = Ymean;

% Between-observation contrast, if entered
% ----------------------------------------
if dobtwn

    Ydiff = hatdiff*Y;

    % for output; fits for each group; low then high
    grpfits = repmat(Ymean,2,1) + repmat(sort(unique(bcon)),1,n) .* repmat(Ydiff,2,1);

    means.Ydiff = Ydiff;
    means.grpmeans = grpfits;
end

if nargout == 1, return, end

% --------------------------------------
% * Residuals
% --------------------------------------

e = Y - repmat(Ymean,m,1);         % residuals

if dobtwn

    % fitted values depending on group
    fits = repmat(Ymean,m,1) + repmat(bcon,1,n) .* repmat(Ydiff,m,1);
    ediff = Y - fits;
end

% --------------------------------------
% * Degrees of freedom
% --------------------------------------

[dfe,dfediff] = get_dfe(m,n,X,hat,varY,dobtwn,hatdiff,bcon);

if ~domultivariate
    % ======================================
    %
    %
    % Univariate stats: MSE, t, and p-values
    %
    %
    % ======================================

    % --------------------------------------
    % * Mean squared error
    % --------------------------------------
    % Loop version of MSE: avoids out of memory errors for large voxel sets
    MSE = zeros(1,n); for i=1:n, MSE(i) = e(:,i)'*W*e(:,i); end, MSE = MSE/dfe;
    v = invxwx * MSE;       % variances for mean

    if dobtwn
        MSEdiff = zeros(1,n);
        for i=1:n, MSEdiff(i) = ediff(:,i)'*W*ediff(:,i); end, MSEdiff = MSEdiff/dfediff;
        vdiff = invxwx_diff * MSEdiff;       % variances for mean
    end

    % output
    stats.descrip1 = 'Univariate stats for test against zero:';
    stats.v = v;
    stats.v_descrip = 'V = ste^2; variance of mean estimate';
    stats.t = Ymean ./ sqrt(v);
    stats.p = 2 * ( 1 - tcdf(abs(stats.t),dfe) );
    stats.dfe = dfe;

    if dobtwn
        stats.descrip2 = 'Univariate stats for between-case contrast:';
        stats.bcon = bcon;
        stats.vdiff = vdiff;
        stats.tdiff = fits ./ sqrt(v);
        stats.pdiff = 2 * ( 1 - tcdf(abs(stats.tdiff),dfediff) );
        stats.dfediff = dfediff;
    end

else
    % ======================================
    %
    %
    % Multivariate stats: MSE, cov(Y), r(Y)
    % Useful for simulating t-values under dependence
    %
    % ======================================

    % --------------------------------------
    % * Mean squared error
    % --------------------------------------

    % additional output: covariance matrix for Ymean and zdiff across time
    % (columns)
    % and correlation matrix for Ymean and zdiff
    % used in Monte Carlo simulations for controlling false positives
    % across columns

    MSE = (e'*W*e)/dfe;             % Mean square error

    if dobtwn
        MSEdiff = (ediff'*W*ediff)/dfediff;
    end

    % --------------------------------------
    % * Estimated covariance and correlation
    %   Estimated between-subjects variance (v)
    % --------------------------------------

    xy = invxwx * MSE;           % Covariance matrix for Ymean;

    xy = 0.5*(xy+xy');              % Remove rounding error

    if dobtwn
        xydiff = inv(bcon'*W*bcon)*MSEdiff;           % Covariance matrix for Ymean;
        xydiff = 0.5*(xydiff+xydiff');
    end

    v = diag(xy);                   % Variance for Ymean

    if dobtwn
        vdiff = diag(xydiff);
    end

    r = xy./sqrt(v*v');             % Correlation matrix for Ymean

    if dobtwn
        rdiff = xydiff./sqrt(vdiff*vdiff');         % Correlation matrix for Ymean
    end

    stats.descrip1 = 'Multivariate stats for test against zero:';
    stats.r = r;
    stats.v = v;
    stats.xy = xy;

    stats.dfe = dfe;
    stats.t = Ymean ./ sqrt(v');
    stats.p = 2 * ( 1 - tcdf(abs(stats.t),dfe) );

    if dobtwn
        stats.descrip2 = 'Univariate stats for between-case contrast:';
        stats.rdiff = rdiff;
        stats.vdiff = vdiff;
        stats.xydiff = xydiff;

        stats.dfediff = dfediff;

        stats.tdiff = Ydiff ./ sqrt(vdiff');
        stats.p = 2 * ( 1 - tcdf(abs(stats.tdiff),dfediff) );
    end
end

return

function [dfe,dfediff] = get_dfe(m,n,X,hat,varY,dobtwn,hatdiff,bcon)

dfediff = [];

% Set up residual-forming matrix
% --------------------------------------
dfe_v = zeros(n,1);
R = eye(m) - X*hat;    % residual inducing matrix

% contrast, if entered
if dobtwn
    dfe_vdiff = zeros(n,1);
    Rdiff = eye(m) - bcon * hatdiff;
end

% Calculate effective degrees of freedom
% --------------------------------------

have_unique_vars = size(varY,2) == n;

if ~have_unique_vars

    % Only one (pooled?) vector of variance estimates
    % --------------------------------------
    V = diag(varY(:,1));
    dfe = (trace(R*V)^2)/trace(R*V*R*V);       % Satherwaite approximation
    if dobtwn, dfediff = (trace(Rdiff*V)^2)/trace(Rdiff*V*Rdiff*V); end
else
    % Variance estimates for each data vector
    % --------------------------------------
    for i=1:n,

        % make diagonal matrix of variances
        V = diag(varY(:,i));

        dfe_v(i) = (trace(R*V)^2)/trace(R*V*R*V);       % Satherwaite approximation

        if dobtwn
            dfe_vdiff(i) = (trace(Rdiff*V)^2)/trace(Rdiff*V*Rdiff*V);
        end

    end

    dfe = mean(dfe_v);               % Calculate average df over all columns (pool over data vectors)
    if dobtwn
        dfediff = mean(dfe_vdiff);
    end

end

return

function bool = is_entered(x)

bool = exist('x','var') && ~isempty(x);

return


function denu=tide_oload(tut,odisp)

D2R=pi/180;
args=[1.40519E-4, 2.0,-2.0, 0.0, 0.00;  % M2
      1.45444E-4, 0.0, 0.0, 0.0, 0.00;  % S2
      1.37880E-4, 2.0,-3.0, 1.0, 0.00;  % N2
      1.45842E-4, 2.0, 0.0, 0.0, 0.00;  % K2
      0.72921E-4, 1.0, 0.0, 0.0, 0.25;  % K1
      0.67598E-4, 1.0,-2.0, 0.0,-0.25;  % O1
      0.72523E-4,-1.0, 0.0, 0.0,-0.25;  % P1
      0.64959E-4, 1.0,-3.0, 1.0,-0.25;  % Q1
      0.53234E-5, 0.0, 2.0, 0.0, 0.00;  % Mf
      0.26392E-5, 0.0, 1.0,-1.0, 0.00;  % Mm
      0.03982E-5, 2.0, 0.0, 0.0, 0.00];  % Ssa
ep1975=[1975,1,1,0,0,0];
dp=zeros(3,1);

%angular argument
ep=time2epoch(tut);
fday=ep(4)*3600.0+ep(5)*60.0+ep(6);
ep(4)=0;ep(5)=0;ep(6)=0.0;
days=timediff(epoch2time(ep),epoch2time(ep1975))/86400.0+1.0;
t=(27392.500528+1.000000035*days)/36525.0;
t2=t*t; t3=t2*t;
a(1)=fday;
a(2)=(279.69668+36000.768930485*t+3.03E-4*t2)*D2R;% H0
a(3)=(270.434358+481267.88314137*t-0.001133*t2+1.9E-6*t3)*D2R;% S0
a(4)=(334.329653+4069.0340329577*t-0.010325*t2-1.2E-5*t3)*D2R;% P0
a(5)=2.0*pi;

%dispalcements by 11 constituents
for i=1:11
    ang=0;
    for j=1:5
        ang=ang+a(j)*args(i,j);
    end
    for j=1:3
        dp(j)=dp(j)+odisp(i,j)*cos(ang-odisp(i,j+3)*D2R);
    end
end

denu(1)=-dp(2);
denu(2)=-dp(3);
denu(3)=dp(1);

return


function Y = invariantMultiply( psfMatData, X, padsize );
%
%           Y = invariantMultiply( psfMatData, X, padsize );
%
%  This function computes the multiplication of a spatially invariant
%  point spread function (PSF) times an image (i.e., convolution):
%                y = A*x
%
%  Here we assume A is made up of a single PSF whose extent may be
%  (much) smaller than the image.
%
%  Input:
%   psfMatData  -  complex array containing the matrix data, usually
%                  computed from onePsfMatrix.m
%            X  -  array containing the image to which the psfMatrix
%                  is to be multiplied.
%
%  Output:
%            Y  -  contains the result after PSF multiplication.
%

%  J. Nagy  1/7/02

imsize = size( X ) - 2*padsize;

%
%  In order for this to be consistent for 2-D and 3-D images, we need to make
%  sure there is a third dimension ...
%
if length(imsize) == 1
  imsize = [imsize, 1, 1];
  padsize = [padsize, 0, 0];
elseif length(imsize) == 2
  imsize = [imsize, 1];
  padsize = [padsize, 0];
end

%
%  partition_info computes number of subregions, and their sizes
%
[nregions, rsize] = partition_info(imsize, padsize);

%
%  Coding the rest of this will be easier if all of the image subregions
%  have the same dimensions.  If it's not, we pad with a few zeros to make
%  it so ...
%
padsize1 = rsize .* nregions - imsize;
if any( padsize1 < 0 )
  error('Something is wrong here ...')
end
X = padarray(X, padsize1, 'post');

Y = zeros( size(X) );

%
%  Now we get information about beginning and ending indices of subregions
%  so we can "put" and "get" subregions correctly ...
%
[RIidx, RJidx, RKidx] = region_indices( nregions, rsize );
[EIidx, EJidx, EKidx] = eregion_indices( RIidx, RJidx, RKidx, 2*padsize );
Tidx = [padsize+1; padsize+rsize];

%
%  Now loop over all the subregions ...
%
for k = 1:nregions(3)
  for j = 1:nregions(2)
    for i = 1:nregions(1)
      Xt = X(EIidx(i,1):EIidx(i,2), EJidx(j,1):EJidx(j,2), EKidx(k,1):EKidx(k,2));

      Yt = multiplyOneRegion( psfMatData, Xt );

      Y(RIidx(i,1):RIidx(i,2), RJidx(j,1):RJidx(j,2), RKidx(k,1):RKidx(k,2)) = ...
           Yt(Tidx(1,1):Tidx(2,1), Tidx(1,2):Tidx(2,2), Tidx(1,3):Tidx(2,3));
    end
  end
end

Y = Y(1:imsize(1), 1:imsize(2), 1:imsize(3));


function [err,XYZdic] = AFNI_Index2XYZcontinuous (Indx, Info, CoordCode)
%
%   [err,XYZdic] = AFNI_Index2XYZcontinuous (Indx, Info, [CoordCode])
%
%Purpose:
%   Change from voxel XYZindex (called Voxel Coords in AFNI) to XYZ in mm
%   The mm and voxel coordinates refer to the values displayed
%   on the top left corner of AFNI controller.
%   CoordCode is the one you'd set from the Coord Order plugin
%
%
%Input Parameters:
%   Indx an Mx3 matrix or an  Mx1 vector containing the voxel indices to be
%        transformed to voxel coordinates.  (indices start at 0)
%   Info is the output of BrikInfo
%   CoordCode is an optional parameter used to specify the coordinates system of the output
%      if empty or not specified, the default is 'RAI'. The code can be either a string or a vector
%      of numbers (see AFNI_CoordChange for more on that)
%
%Output Parameters:
%   err : 0 No Problem
%       : 1 Mucho Problems
%   XYZdic : The continuous coordinates corresponding to Indx
%       The coordnate system output is in RAI (DICOM)
%       unless otherwise specified by CoordCode
%
%
%Key Terms:
%
%More Info :
%   BrikInfo
%   Test_AFNI_Index2XYZcontinuous
%   AFNI_XYZcontinuous2Index
%   Test_AFNI_XYZcontinuous2Index
%
% You can also go from index to XYZ using the header field IJK_TO_DICOM_REAL
%  For instance, say you have voxel indices 12, 2, 4 (matlab indexing 13, 3, 5)
%  to go from AFNI index to AFNI DICOM RAI you can do:
%     M = [reshape(Info.IJK_TO_DICOM_REAL, 4, 3)' ; 0 0 0 1];
%     I = [12 2 4 1]';
%     X = M*I;
%     To go from AFNI DICOM RAI to AFNI indices:
%     I = inv(M)*X;
%
%     Author : Ziad Saad
%     Date : Tue Sep 5 21:48:06 PDT 2000           Latest Modification: Feb 18 04
%     LBC/NIMH/ National Institutes of Health, Bethesda Maryland

%Define the function name for easy referencing
FuncName = 'AFNI_Index2XYZcontinuous';

%Debug Flag
DBG = 1;

ChangeCoord = 0;
if (nargin == 3)
	if (~isempty(CoordCode)),
		ChangeCoord = 1;
	end
end

%initailize return variables
err = 1;
XYZmm = [];

%make sure Indx is the right size
switch size(Indx,2),
	case 1, %change 1D index to XYZ index
		[err, Indx] = AfniIndex2AfniXYZ (Indx, Info.DATASET_DIMENSIONS(1), Info.DATASET_DIMENSIONS(2))
	case 3, %OK
	otherwise,
		err = ErrEval(FuncName,'Err_Bad dimension for Indx');
		return
end

XYZmm = Indx;

	%The equations that would change the indices to coordinate system result in a coordinate system that
	% may be any permutation of RAI (like IRA or AIR or IAR or RIA or ARI) so one only needs to find the
	%dimension permutation needed to bring the final result to RAI.

	%determine the ordering map to go from any permutation of RAI to RAI
		%[maploc(1),jnk] = find(Info.Orientation == 'R');
		%[maploc(2),jnk] = find(Info.Orientation == 'A');
		%[maploc(3),jnk] = find(Info.Orientation == 'I');

	%pre - Wed May 23 18:20:56 PDT 2001 - WRONG !
		%XYZmm(:, maploc(1)) = Info.ORIGIN(1) + Indx(:,1) .* Info.DELTA(1);
		%XYZmm(:, maploc(2)) = Info.ORIGIN(2) + Indx(:,2) .* Info.DELTA(2);
		%XYZmm(:, maploc(3)) = Info.ORIGIN(3) + Indx(:,3) .* Info.DELTA(3);

	%post - Wed May 23 18:20:56 PDT 2001 - WRONG!
		%XYZmm(:, 1) = Info.ORIGIN(maploc(1)) + Indx(:,maploc(1)) .* Info.DELTA(maploc(1));
		%XYZmm(:, 2) = Info.ORIGIN(maploc(2)) + Indx(:,maploc(2)) .* Info.DELTA(maploc(2));
		%XYZmm(:, 3) = Info.ORIGIN(maploc(3)) + Indx(:,maploc(3)) .* Info.DELTA(maploc(3));

   %Feb 18 04, back to the original
      XYZmm(:, 1) = Info.ORIGIN(1) + Indx(:,1) .* Info.DELTA(1);
		XYZmm(:, 2) = Info.ORIGIN(2) + Indx(:,2) .* Info.DELTA(2);
		XYZmm(:, 3) = Info.ORIGIN(3) + Indx(:,3) .* Info.DELTA(3);
      %Now this is in the axis orientation which is Info.Orientation(:,1)' called 3dmm in thd_coords.c
      [err,XYZdic, map] = THD_3dmm_to_dicomm (Info, XYZmm);

if (ChangeCoord),
	[err, maplocation, mapsign, XYZdic] = AFNI_CoordChange ('RAI', CoordCode, XYZdic);
end

err = 0;
return;


function f = perform_wavortho_transf(f,Jmin,dir,options)

% perform_wavortho_transf - compute orthogonal wavelet transform
%
%   fw = perform_wavortho_transf(f,Jmin,dir,options);
%
%   You can give the filter in options.h.
%
%   Works in arbitrary dimension.
%
%   Copyright (c) 2009 Gabriel Peyre

options.null = 0;
h = getoptions(options,'h', compute_wavelet_filter('Daubechies',4) );
g = [0 h(length(h):-1:2)] .* (-1).^(1:length(h));

n = size(f,1);
Jmax = log2(n)-1;

if dir==1
    %%% FORWARD %%%
    for j=Jmax:-1:Jmin
        sel = 1:2^(j+1);
        a = subselect(f,sel);
        for d=1:nb_dims(f)
            a = cat(d, subsampling(cconv(a,h,d),d), subsampling(cconv(a,g,d),d) );
        end
        f = subassign(f,sel,a);
    end
else
    %%% FORWARD %%%
    for j=Jmin:Jmax
        sel = 1:2^(j+1);
        a = subselect(f,sel);
        for d=1:nb_dims(f)
            w = subselectdim(a,2^j+1:2^(j+1),d);
            a = subselectdim(a,1:2^j,d);
            a = cconv(upsampling(a,d),reverse(h),d) + cconv(upsampling(w,d),reverse(g),d);
        end
        f = subassign(f,sel,a);
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function f = subselect(f,sel)
switch nb_dims(f)
    case 1
        f = f(sel);
    case 2
        f = f(sel,sel);
    case 3
        f = f(sel,sel,sel);
    case 4
        f = f(sel,sel,sel,sel);
    case 5
        f = f(sel,sel,sel,sel,sel);
    case 6
        f = f(sel,sel,sel,sel,sel,sel);
    case 7
        f = f(sel,sel,sel,sel,sel,sel,sel);
    case 8
        f = f(sel,sel,sel,sel,sel,sel,sel,sel);
    otherwise
        error('Not implemented');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function f = subselectdim(f,sel,d)
switch d
    case 1
        f = f(sel,:,:,:,:,:,:,:);
    case 2
        f = f(:,sel,:,:,:,:,:,:);
    case 3
        f = f(:,:,sel,:,:,:,:,:);
    case 4
        f = f(:,:,:,sel,:,:,:,:);
    case 5
        f = f(:,:,:,:,sel,:,:,:);
    case 6
        f = f(:,:,:,:,:,sel,:,:);
    case 7
        f = f(:,:,:,:,:,:,sel,:);
    case 8
        f = f(:,:,:,:,:,:,:,sel);
    otherwise
        error('Not implemented');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function f = subassign(f,sel,g)
switch nb_dims(f)
    case 1
        f(sel) = g;
    case 2
        f(sel,sel) = g;
    case 3
        f(sel,sel,sel) = g;
    case 4
        f(sel,sel,sel,sel) = g;
    case 5
        f(sel,sel,sel,sel,sel) = g;
    case 6
        f(sel,sel,sel,sel,sel,sel) = g;
    case 7
        f(sel,sel,sel,sel,sel,sel,sel) = g;
    case 8
        f(sel,sel,sel,sel,sel,sel,sel,sel) = g;
    otherwise
        error('Not implemented');
end


function [rar,yclean] = spm_rar (Z,p,m,verbose)
% Bayesian autoregressive modelling with zero-mean Gaussian mixture noise
% function [rar,yclean] = spm_rar (Z,p,m,verbose)
%
% Z          [N x 1] vector of data points
% p          Number of AR coefficients
% m          Number of mixture components (default=2)
% verbose    0/1 to printout inner workings (default=0)
%
% rar        Returned model
% yclean     'Clean' data (ie. with outlier errors removed)
%
% -------------------------------------------------------
% The fields in rar are:
%
% p                The number of AR coefficients
% m                The number of components
% fm               The negative free energy
%
%                  In the field priors:
% lambda_0         Dirichlet parameters for mixing coeffs
% b_0,c_0          Gamma parameters for precisions
%
%                  In the field posts:
% lambda           Dirichlet parameters  for mixing coeffs
% b,c              Gamma parameters for precisions
% a_mean           AR parameters (posterior mean)
% a_cov            AR parameters (posterior cov)
% b_alpha,c_alpha  Gamma parameters for weight precisions
%
%                  Mean posterior values:
% pi               mixing coefficients (lambda/sum(lambda))
% variances        variances (1./(b.*c))
%
% gamma            the responsibilities of each noise component
%
% For details of algorithm see:
%
% S.J. Roberts and W.D. Penny. Variational Bayes for Generalised Autoregressive
% models. IEEE Transactions on Signal Processing, 50(9):2245-2257, 2002
%___________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_rar.m 1276 2008-03-28 18:29:19Z guillaume $

if nargin < 3 | isempty(m)
    m=2;
end

if nargin < 4 | isempty(verbose)
  verbose=0;
end

if (m==1)
  rar=spm_ar(Z,p);
  return;
end

N=length(Z);

% Initialise AR coefficients to maximum likelihood solution
Z=Z(:);
y=Z(p+1:end);
for i=1:p,
    x(:,i)=Z(p-i+1:end-i);
end
x=-x;
y2=y.^2;
xt=x';
N=size(x,1);
wun=ones(N,1);

a_mean = pinv(x)*y;
y_pred = x*a_mean;
err=y-y_pred;
v=mean((y-y_pred).^2);
a_cov = v*inv(x'*x);

% Set mixing priors
lambda_0=5;

% Setting to these values gives updates for mean_alpha
% v. close to evidence framework
bishop_prior=1;
if bishop_prior
  b_alpha_prior=1000;
  c_alpha_prior=0.001;
  b_0=1000;
  c_0=0.001;
end
mean_alpha_prior=b_alpha_prior*c_alpha_prior;

% Cluster on absolute difference from mean
zmix=spm_kmeans1(abs(err-mean(err)),m);
% Posterior for mixers
lambda=100*zmix.pi;
zmean=[zmix.m].^2;
% Posterior for precisions
var_precision=1/std(zmean)^2;
for s=1:m,
      % Set so that b*c=precision and  b^2*c=var_precision
      precision=1/zmean(s);
      b(s)=var_precision/precision;
      c(s)=(precision^2)/var_precision;
end

% Initialise weight precision posterior
E_w=a_mean'*a_mean;
b_alpha=0.5*E_w+0.5*trace(a_cov)+(1/b_alpha_prior);
b_alpha=1/b_alpha;
c_alpha=0.5*p+c_alpha_prior;
mean_alpha=b_alpha*c_alpha;

if verbose
  disp('Init');
  disp('AR Coefficients');
  a_mean(:)'
  for s=1:m,
    disp(sprintf('State %d mix=%1.2f var=%1.2f',s,lambda(s)/sum(lambda),1/(b(s)*c(s))));
  end
end

lik=[];
tol=0.0001;
max_loops=32;
WLOOPS=5;
for loops=1:max_loops,

    % E-step
    lambda_tot=sum(lambda);
    ypred=x*a_mean;
    ypred2=ypred.^2;
    y_err=sum(xt.*(a_cov*xt));
    tv=y2-2*ypred.*y+y_err'+ypred2;
    tv=tv';
    for s=1:m,
        log_tilde_pi(s)=psi(lambda(s))-psi(lambda_tot);
        log_tilde_beta(s)=psi(c(s))+log(b(s));
        tilde_pi(s)=exp(log_tilde_pi(s));
        tilde_beta(s)=exp(log_tilde_beta(s));
        mean_beta(s)=c(s)*b(s);
        tilde_var(s,:)=tv;
        gamma(s,:)=tilde_pi(s)*(tilde_beta(s)^0.5)*exp(-0.5*mean_beta(s)*tv);
    end
    gamma_n=sum(gamma);
    for s=1:m,
        if mean(gamma_n) > eps
            % If component still exists
            gamma(s,:)=gamma(s,:)./gamma_n;
        end
    end

    % M-step
    % Part I
    for s=1:m,
        pi_bar(s)=mean(gamma(s,:));
        N_bar(s)=N*pi_bar(s);
        mean_bar(s)=mean(gamma(s,:)'.*y);
        dg=diag(gamma(s,:));
        x_bar(s,:)=mean(dg*x);
        var_bar_mu(s)=mean(gamma(s,:).*tilde_var(s,:));
    end

    % CALCULATE THE FREE ENERGY
    avg_likelihood=sum(N_bar.*(log_tilde_pi+0.5*log_tilde_beta));
    fit=-0.5*N*sum(mean_beta.*var_bar_mu);
    % Ensure that 0 log 0 = 0
    ent_s=sum(sum(-gamma.*log(gamma+eps)));
    avg_likelihood=avg_likelihood+fit+ent_s;
    lambda_p=lambda_0*ones(1,m);
    kl_dir=spm_kl_dirichlet(lambda,lambda_p,log_tilde_pi);
    kl_gamm=0;
    for s=1:m,
        kl_gamm=kl_gamm+spm_kl_gamma(b(s),c(s),b_0,c_0);
    end
    kl_weights=spm_kl_normal(a_mean,a_cov,zeros(1,p),(1/mean_alpha)*eye(p));
    kl_alpha=spm_kl_gamma(b_alpha,c_alpha,b_alpha_prior,c_alpha_prior);
    fm= avg_likelihood - kl_dir - kl_gamm - kl_weights - kl_alpha;

    % Convergence criterion
    oldlik=lik;
    lik=fm;

    if (mod(loops-1,WLOOPS)==0)
        if (loops>1)
            if abs((lik-oldlik)/lik) < tol
                break;
            end
        end
    end

    % M-Step: Part II
    for s=1:m,
        % Mixers
        lambda(s)=N_bar(s)+lambda_0;
        % Precisions
        b(s)=1/(0.5*N*var_bar_mu(s)+1/b_0);
        c(s)=0.5*N_bar(s)+c_0;
        mean_beta(s)=c(s)*b(s);
    end

    if mod(loops,WLOOPS)==0
        % Weight precisions
        E_w=0.5*a_mean'*a_mean;
        b_alpha=E_w+0.5*trace(a_cov)+(1/b_alpha_prior);
        b_alpha=1/b_alpha;
        c_alpha=0.5*p+c_alpha_prior;
        mean_alpha=b_alpha*c_alpha;

        % AR coefficients
        cc=zeros(p,p);
        cw=zeros(p,1);
        for s=1:m,
            dg=diag(gamma(s,:));
            cc=cc+mean_beta(s)*x'*dg*x;
            cw=cw+mean_beta(s)*x'*dg*y;
        end
        cc=cc+mean_alpha*eye(p);
        a_cov=inv(cc);
        a_mean=a_cov*cw;
    end

    if verbose
        disp(sprintf('It=%d, L_AV =%1.2f, KL Mix=%1.2f, KL Prec=%1.2f, KL-AR=%1.2f, KL-alpha=%1.2f, Fm=%1.2f',loops,avg_likelihood,kl_dir,kl_gamm,kl_weights,kl_alpha,fm));
    end

end

% Put variables into data structure
rar.posts.a_mean=a_mean;
rar.posts.a_cov=a_cov;
rar.m=m;
rar.fm=fm;
rar.priors.lambda_0=lambda_0;
rar.priors.c_0=c_0;
rar.priors.b_0=b_0;

for k=1:m,
  rar.posts.lambda(k)=lambda(k);
end

for k=1:m,
  rar.posts.c(k)=c(k);
  rar.posts.b(k)=b(k);
end

for k=1:m,
  rar.pi(k)=lambda(k)/sum(lambda);
end
rar.variances=1./(b.*c);

% Pre-pad gamma with zeros to get original length time series
rar.gamma=[zeros(m,p),gamma];

% Get 'clean' data
if m > 1
    [tmp,outlier_class]=min(rar.pi);
    e=y-ypred;
    outlier_error=gamma(outlier_class,:)'.*e;
    yclean=ypred+e-outlier_error;
    yclean=[Z(1:p);yclean];  % Pre-padding
else
    yclean=Z;
end


function H = cross(F, G)
%CROSS   Vector cross product.
%   CROSS(F, G) returns a SPHEREFUNV representing the 3D cross product of
%   the SPHEREFUNV objects F and G.
%
%   See also SPHEREFUNV/DOT.

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

% [TODO]: Implement the following option
%   CROSS(F, G, 'n') returns a SPHEREFUN representing the normal component
%   of the cross product of the SPHEREFUNV objects F and G.
%   For two vector fields tangent to the sphere, the cross product is a
%   a vector that points in the normal direction.  CROSS returns a
%   SPHEREFUN object representing the component in the normal (radial)
%   direction.  Mathematically, this is N dot (F x G), where N is the
%   normal to the sphere.

% Empty check:
if ( isempty(F) || isempty(G) )
    H = spherefunv;
    return
end

% Get the components:
Fc = F.components;
Gc = G.components;

% Do cross:
H = [ Fc{2} .* Gc{3} - Fc{3} .* Gc{2} ; ...
      Fc{3} .* Gc{1} - Fc{1} .* Gc{3} ; ...
      Fc{1} .* Gc{2} - Fc{2} .* Gc{1} ];

end


function [beta, bo] = svm_multi_predK(X,Y,C,K)
% SVM_MULTI_PREDK
%
% Support Vector Multi Classification
%
% USAGE: [beta, bo] = svm_multi_pred(X,Y,C,K)
%
% PARAMETERS:  X      - (m,d) matrix of m Training inputs in R^d
%
%              Y      - m vector of m Training targets in {1,..,Q}
%
%              C      - Trade-off between regularization and empirical error
%
%              K      - (m,m) Gram matrix
%
%              beta   - (m,Q) Coefficients matrix of the expansion of
%                       the vectors w_i over the x_p's
%                       w_i = \sum_p beta(p,i)x_p
%
%              bo     - (Q,1) bias terms
%
% SUBROUTINES: svm_multi_init.m -> initialization of the optimization problem
%             squadsolve.m          -> optimization
%             compute_kernel.m -> computation of the gram matrix
%
% DESCRIPTION:
%
%              This procedure is the implementation of a multiclass SVM corresponding to
%              the following problem
%
%              \min_{w_i,w_j} \sum_{i\neq j} \|w_i - w_j\|^2 + C\sum_{p,j} \xi_{pj}
%
%              subject to :
%               for all p=1,..,m   ( w_{c(p)} - w_j ).x_p + b_{c(p)} - b_j \geq 1 - \xi_{pj}
%                                  \xi_{pj} \geq 0
%
%      where:  m is the number of training points
%              Q is the number of classes
%              i and j are indices between 1 and Q
%              p is an index between 1 and m
%              w.x is the dot product between w and x
%
%              To solve this problem, a direct approach is used. All the gram matrix K (K_{i,j} =x_i.x_j)
%              is computed before beginning the optimization. The memory cost is thus proportionnal to Q^2*m^2.
%              The solution is not approximated but is provided by a quadratic programming method implemented in
%              squadsolve.m. Instead of minimizing the primal objective function, we maximize the dual. Its
%              formulation is not given here.
%
%
% ERRORS AND BUGS:
%              It is recommended not to use this code for large problems: unless your computer has a huge memory,
%              the procedure will stop with the message: 'memory exhausted'. Use the code msvm_fw.m or chunk_msvm.m
%              instead. Sometimes, the bias may not be computable because the term 'bias_eps' (see below) is too large.
%              In that case, decrease 'bias_eps' and it should work. In all cases, the program will output the bias and
%              try to give the best solution.
%
% EXAMPLE OF USE:
%
%      > [beta,bo] = svm_multi_pred(inputs,targets,10,K);
%
%       -> solve the multiclass optimization problem for a learning set S = \cup_i {(inputs(i,:),targets(i))}
%          with C=10, and with the gram matrix K.
%
% Description of variables:
%
%      kernel = 0,1,2 (integer)    -> type of kernel
%      m              (integer)    -> number of input points
%      Q              (integer)    -> number of classes
%      bias_eps       (real)       -> precision for the computation of the bias
%      reg_eps        (real)       -> to avoid ill conditionning, a diagonal matrix
%                                     is added to the hessian of the minimization problem,
%                                     reg_eps scales this diagonal matrix. (usual < 10^(-6))
%      A              ((Q,Q*m) matrix) -> equality constraint matrix
%      H            ((Q*m,Q*m) matrix) -> hessian of the objective function
%      c               (Q*m vector)    -> linear part of the objectif function
%      alpha           (Q*m vector)    -> variables of the optimization problem
%      constraints     (Q*m vector)    -> store 1-(w_{c(p)} - w_j).x_p
%      beta            ((m,Q) matrix)  -> vector w_i is computed as w_i = sum_p beta_{pi} x_p
%      b               ((Q,Q) matrix)  -> store the differences between bias: b(i,j) = b_i - b_j
% Avoid warning messages
warning off;
disp('Optimizing...');
% Test if nargin is correct
if (nargin < 2) | (nargin > 6),
help svm_multi_pred;
else

% Init
 empty_list_elements_ok = 1;
 m = size(X,1);
 Q = max(Y);
 if (nargin<3),
     C=Inf;
 end;
 if (C==Inf)
    tol = 1e-5;
  else
    tol = C*1e-6;
 end;
 bias_eps=10^(-8)/C;
 reg_eps = 10^(-6);
 sv_eps=10^(-6);
% Message
 % disp(sprintf('\nMulti Support Vector Classification\n'));
 % disp(sprintf('-----------------------------------\n'));

% Set up the parameters for the Optimisation problem
%disp(sprintf('Initialization...\n'));
[H,A,c]=svm_multi_init(Y,K);
% Add small amount of zero order regularization to
% avoid problems when Hessian is badly conditioned.
H = 1/(2*Q^2)*H+reg_eps*eye(size(H));% the problem is to minimize (1/4Q^2)x'Hx - c.x
c = -c;
% Some variables are irrelevant. Consider only relevant variables
mul = Q*(0:m-1);
indx = mul' + Y;
s1 = (1:Q*m);
s2 = (indx);
indx = setdiff(s1,s2)';
clear s1;clear s2;clear mul;
% Solve the optimization problem
% the solution is stored in alpha
%disp(sprintf('Begin the optimization ...\n'));
bo = zeros(size(A,1),1);
[x1,y] = quadsolve(H(indx,indx),c(indx),A(:,indx),bo,C);
x1=x1(1:length(indx));
obj = 0.5*x1'*(H(indx,indx)-reg_eps*eye(length(indx)))*x1 + c(indx)'*x1;
clear H;clear c;
alpha = zeros(Q*m,1);
alpha(indx)=x1;
clear x1;
% Output the objective value
%disp(sprintf('Final objective function: %f \n',-obj));
% if the alpha's are all greater than C then, C can be considered as infinite
% (for the computation of the bias)
if (max(abs(alpha)) < C*0.95)
    C==Inf;
end;
% Compute the number of support vector
% Can be removed if not desired
M = max(alpha);
nsv=0;
for i=1:m,
  if ~(isempty(find(alpha(Q*(i-1)+1:Q*i)>100*eps)))
    nsv = nsv+1;
  end;
end;
%disp(sprintf('Support vectors : %d (%3.1f)\n',nsv,100*nsv/(m)));
% Compute the coefficients beta_ip of the vector w_i = sum_p beta_ip x_p
% clean alpha
beta=zeros(m,Q);
notvoid = find(alpha>=100*eps);
alpha_tmp=zeros(Q*m,1);
alpha_tmp(notvoid) = alpha(notvoid);
alpha=alpha_tmp;
clear notvoid;clear alpha_tmp;
% compute beta
for i =1:Q,
 for p=1:m,
  tmp=0;
  if (i==Y(p))
   tmp = tmp + sum(alpha((p-1)*Q + 1: (p-1)*Q + Q))/Q;
  end;
  beta(p,i) = tmp - alpha((p-1)*Q + i)/Q;
 end;
end;
% Computation of the bias
%disp(sprintf('\n Computation of the bias........\n'));
% Computation of the outputs 1-(w_{c(i)} - w_j).x_i
errorcache = zeros(Q*m,1);
for p=1:m,
  for i=1:Q,
    temp = ((beta(:,Y(p))-beta(:,i))'*K(:,p));
    errorcache((p-1)*Q+i)=temp;
  end;
end;

  % Computation of the output 1-(w_Y(p)-w_i).x_p
  constraints=1-errorcache;
   if C==Inf,
     b=-Inf*ones(Q,Q);
     for i=1:m,
         for k=1:Q,
            b(Y(i),k) = max([b(Y(i),k),(constraints(Q*(i-1)+k) - 0.05)]);
         end;
     end;
     for k=1:Q,
         b(k,k)=0;
         b(k,find(b(k,:)==-Inf))=0;
     end;
   end; %if C>max(alpha),
%%% For finite C
if C<Inf,
% computation of alpha_bias
  % here: computing of the bias that minimize the \sum_{ip} \xi_{pi}
  % subject to linear constraints:
  % (w_{c(p)} - w_i).x_p + b_{c(p)} - b_i >= 1 - \xi_{pi}
  % \xi_{pi}
  % This is a linear program whose dual is easy to compute. It can be
  % solved with the slinearsolve method.
  alpha_bias=zeros(Q*m,1);
  c_un = ones(Q*m,1);
  [alpha_bias(indx)] = slinearsolve(errorcache(indx)-c_un(indx),A(1:(Q-1),indx),zeros(Q-1,1),1);

  % clean alpha_bias
  tmp = 0:m-1;
  tmp = tmp*Q;
  tmp = tmp + Y';
  alpha_bias(tmp)=0;

  % if problem, consider the alpha instead
  if (flag~=1)
    alpha_bias=alpha;
  end;

  b=zeros(Q,Q); % matrix of the difference b_i-b_j
  % Computation of the output 1-(w_Y(p)-w_i).x_p
  constraints=1-errorcache;

  % consider the alpha_ip s.t. 0 < alpha_bias_ip < 1
  % here, i consider alpha_new, because i compute the bias
  % that minimize the l_1 error of the current solution
  % and not of the optimal.
  if C == Inf,
    Cm =1;
  else
    Cm=C;
  end;
  if (flag~=1)
   indice_fix = find(alpha_bias < C*(1-bias_eps) & alpha_bias > Cm*bias_eps);
  else
   indice_fix = find(alpha_bias < (1-bias_eps) & alpha_bias > bias_eps);
  end;

  % if there are no such alpha -> problem
  if isempty(indice_fix)
    disp('WARNING : Unable to compute the bias, use another method\n');
  end;
  % the bias are computed by averaging
  % the matrix 'compteur' counts when  b_i-b_j is computed
  compteur=zeros(Q,Q);
  for ind=indice_fix',
    % find the index Q and p corresponding to ind
    indQ = rem(ind,Q);
    if (indQ==0), indQ=Q;end;
    indp = (ind-indQ)/Q;
    compteur(Y(indp+1),indQ)=compteur(Y(indp+1),indQ)+1;
    b(Y(indp+1),indQ) = b(Y(indp+1),indQ) + constraints(ind);
  end;
  % average by dividing by compteur
  for i=1:Q,
    b(i,i) = 0;
    for j=1:Q,
      if (compteur(i,j))
       b(i,j)=b(i,j)/compteur(i,j);
      end;
    end;
  end;
  end; %if C...,
  clear indice_fix;clear constraints;

  % transform b in order to be symetric
  for i=1:Q,
    for j=i+1:Q,
      if (b(i,j)==0)
        b(i,j)=-b(j,i);
      end;
      if (b(j,i)==0)
        b(j,i)=-b(i,j);
      end;
    end;
  end;

  % average between both symetric elements
  if C==Inf,
   for i=1:Q,
    for j=i+1:Q,
        b(i,j)=max(b(i,j),-b(j,i));
        b(j,i)=-b(i,j);
    end;
  end;
  else,
  for i=1:Q,
    for j=i+1:Q,
        b(i,j)=(b(i,j)-b(j,i))/2;
        b(j,i)=-b(i,j);
    end;
  end;
  end;

  % compute the values of b_i s.t. sum_i b_i = 0
  ok=0;% ok > 0, if computation is possible otherwise ok=0
  b_guess = zeros(Q,1); % if no computation is possible still give a guess
  bo = zeros(Q,1); % the bias vector
  for i=1:Q,
    z = find(b(i,:)~=0);
    if (length(z)==Q-1)
      ok=ok+1;
      temp = sum(b(i,:))/Q;
      for j=1:Q,
        bo(j) =bo(j)+temp-b(i,j);
      end;
    else
      %disp(sprintf('Warning : bias %d -> problem... %d\n',i,length(z)));
      if length(z),
        b_guess(i) = sum(b(i,:))/length(z);
      end;
      for j=1:length(z),
        b_guess(z(j)) =b_guess(i) -b(i,z(j));
      end;
    end;
  end;

  if ~ok
    % then one should compute the bias with another method
    disp('WARNING : Problem with the bias, use another method');
    bo=b_guess;
  else
    bo =bo/ok;
  end;

end; %if nargin...


%BZreaction animation
%Belousov-Zhabotinsky Reaction animation
%This MATLAB code is converted from Processing code available in this link
%http://www.aac.bartlett.ucl.ac.uk/processing/samples/bzr.pdf

%version 2. Corrected the drift of pixels as suggested
%           by Jonh.

xres=75; %x resolution
yres=75; %y resolution

a=rand(xres,yres,2);
b=rand(xres,yres,2);
c=rand(xres,yres,2);
c_a = zeros(xres,yres);
c_b = zeros(xres,yres);
c_c = zeros(xres,yres);
p = 1;
q = 2;
img=zeros(xres,yres,3);

for k=1:100
         c_a = 0*c_a;
         c_b = 0*c_b;
         c_c = 0*c_c;
         for m=1:xres
            for n=1:yres
               for mm = m:m+2
                  for nn = n:n+2
                    c_a(m,n) =c_a(m,n)+ a( mod(mm+xres,xres)+1 ,mod(nn+yres,yres)+1 ,p);
                    c_b(m,n) =c_b(m,n)+ b( mod(mm+xres,xres)+1 ,mod(nn+yres,yres)+1 ,p);
                    c_c(m,n) =c_c(m,n)+ c( mod(mm+xres,xres)+1 ,mod(nn+yres,yres)+1 ,p);
                  end
               end
            end
         end

        %correction of pixel drift
        c_a = circshift(c_a,[2 2]);
        c_b = circshift(c_b,[2 2]);
        c_c = circshift(c_c,[2 2]);

        c_a =c_a/ 9.0;
        c_b =c_b/ 9.0;
        c_c =c_c/ 9.0;

        a(:,:,q) = double(uint8(255*(c_a + c_a .* (c_b - c_c))))/255;
        b(:,:,q) = double(uint8(255*(c_b + c_b .* (c_c - c_a))))/255;
        c(:,:,q) = double(uint8(255*(c_c + c_c .* (c_a - c_b))))/255;

        img(:,:,1)=c(:,:,q);
        img(:,:,2)=b(:,:,q);
        img(:,:,3)=a(:,:,q);

        if  p == 1
          p = 2; q = 1;
        else
          p = 1; q = 2;
        end

        image(uint8(255*hsv2rgb(img)))
        axis equal off
        drawnow
end