% NMFLAB for Signal Processing written by A. Cichocki and R. Zdunek
% in cooperation with other members of Laboratory for Advanced Brain Signal
% Processing, BSI, RIKEN, Saitama, JAPAN

function [A,X,Distance_output]=nmf_smart_alg(Y,r,Index_norm,A_true,S, mc_on, NoAlts,restart_mc_on,AL,Y_true,type_alg_A,type_alg_X, max_restart,no_iter, alpha, alpha_BE, omegaX, omegaA, alphaS)
%
%
% Non-negative Matrix Factorization (NMF) with Extended SMART algorithms
%
% [A,X]=nmf_smart_alg(Y, r, Index_norm, A_true, S, mc_on, NoAlts, restart_mc_on, AL, Y_true, type_alg_A, type_alg_X, max_restart, no_iter,
%       alpha, alpha_BE, omegaX, omegaA, alphaS)
%       returns mixing matrix A of dimension [m by r],
%       and source matrix X of dimension [r by T], for the linear mixing model: AX = Y,
%       where Y is an observation matrix [m by T].
% Note: > m: number of sensors,
%       > r: number of sources,
%       > T: number of samples,
%
% INPUTS:
%       > Index_norm:    vector of 13 binary entries indicating which number
%                        divergence measures are turned on in the View Options,
%
%       > A_true:        true mixing matrix (only for synthetic data),
%       > S:             true source matrix (only for synthetic data),
%       > mc_on:         1 - Monte Carlo analysis enabled, 0 - otherwise,
%       > NoAlts:        number of alternating steps (only for Monte Carlo
%                        analysis and the option "Fixed Alternatings" is selected)
%       > restart_mc_on: 1 - restarts in Monte Carlo analysis are enabled,
%                        0 - otherwise,
%       > AL:            mixing matrix estimaed from the preceeding layer,
%       > Y_true:        the first layer mixed signals (mixtures),
%       > type_alg_A:    indicates the selected algorithm for computation
%                        of the mixing matrix,
%       > type_alg_X:    indicates the selected algorithm for computation of the sources,
%       > no_iter:       number of inner iterations,
%       > alpha:         parameter "alpha" in the Amari alpha-divergence
%       > alpha_BE:      parameter "alpha" in the Bose-Einstein divergence
%       > omegaX:        relaxation parameter (omega) in computation of the sources,
%       > omegaA:        relaxation parameter (omega) in computation of
%                        the mixing matrix,
%       > alphaS:        parameter of non-linear projection in computation
%                        of the sources,
%
% OUTPUTS:
%       > A:               estimated mixing matrix,
%       > X:               estimated source matrix,
%       > Distance_output: structures of different divergences measured between "Y" and estimated "AX" versus iterations,
%
% #########################################################################
A = [];
X = [];
if (nargin < 19) | isempty(alphaS) | max(size(alphaS) > 1)
   disp('Incorrect parameter alphaS');
   return
end
if (nargin < 18) | isempty(omegaA) | max(size(omegaA) > 1)
   disp('Incorrect parameter alpha');
   return
end
if (nargin < 17) | isempty(omegaX) | max(size(omegaX) > 1)
   disp('Incorrect parameter omegaX');
   return
end
if (nargin < 16) | isempty(alpha_BE) | max(size(alpha_BE) > 1)
   disp('Incorrect parameter alpha in the Bose_Einstein divergence');
   return
end
if (nargin < 15) | isempty(alpha) | max(size(alpha) > 1)
   disp('Incorrect parameter alpha');
   return
end
if (nargin < 14) | isempty(no_iter) | (no_iter < 1) | max(size(no_iter) > 1)
   disp('Incorrect number of inner iterations');
   return
end
if (nargin < 13) | isempty(max_restart)  | (max_restart < 0) | max(size(max_restart) > 1)
   disp('Number of restarts must be given correctly');
   return
end
if (nargin < 12) | isempty(type_alg_X) | (type_alg_X < 1) | max(size(type_alg_X) > 1)
   disp('Incorrect algorithm for X');
   return
end
if (nargin < 11) | isempty(type_alg_A) | (type_alg_A < 1) | max(size(type_alg_A) > 1)
   disp('Incorrect algorithm for A');
   return
end
if (nargin < 10) | isempty(Y_true)
   disp('The first layer mixed signals are unknown');
   Y_true = zeros(size(Y_true));
end
if (nargin < 9) | isempty(AL)
   disp('Mixing matrix from the preceeding layer unknown');
   AL = eye(size(Y,1));
end
if (nargin < 8) | isempty(restart_mc_on) | max(size(restart_mc_on) > 1)
   disp('Index od restarts in MC analysis unknown');
   restart_mc_on = 0;
end
if (nargin < 7) | isempty(NoAlts) | max(size(NoAlts) > 1)
   disp('Adjustable number of alternatings');
   NoAlts = [];
end
if (nargin < 6) | isempty(mc_on) | max(size(mc_on) > 1)
   disp('No Monte Carlo Analysis');
   mc_on = 0;
end
if (nargin < 5) | isempty(S)
   disp('X_true not given');
end
if (nargin < 4) | isempty(A_true)
   disp('A_true not given');
   index_fixed_A = 1;
else
   index_fixed_A = 0;
end
if (nargin < 3) | isempty(Index_norm)
   '"Index_norm" must be specified'
   return
end
if (nargin < 2) | isempty(r)
   'Rank of factorization must be given'
   return
end
if isempty(Y) | isnan(Y)
   error('No data');
   return
end
% test for negative values in Y
if min(min(Y)) < 0
    disp('Some matrix entries are changed from negative to small positive');
    Y(Y< 0) = eps;
end
if min(sum(Y,2)) == 0
    disp('Not all entries in a row can be zero');
    return
end

if (alpha == 0)
    if (type_alg_X == 1)
        type_alg_X = 2;
    end
    if (type_alg_A == 1)
        type_alg_A = 2;
    end
end

if (type_alg_A == 12) & (size(A_true,1) ~= size(Y,1))
   disp('Multilayer technique cannot be used with A fixed');
   Distance_output = [];
   return
end

Y = Y + eps;

[m,T]=size(Y);
niter_selected = 2000;     % maximum number of iterations for the selected sample (can be adjusted)
niter_sample = 30; % maximum number of iterations for each random sample
epsil_normA = 1E-4; % tolerance for alternating

% Monte Carlo and alternatings adjustment
if mc_on & ~restart_mc_on
    max_restart = 0;
end
if ~isempty(NoAlts)
    niter_selected = NoAlts;
end

% Declaration for A and X
A=zeros(m,r);
Ap = A;
X=zeros(r,T);
Ainit = A;
Xinit = X;
nr_best = -1;
Z = zeros(m,T);
KL_outer_temp = 0;
nr = 0; restart_on = 0; norm_A = 10;
m_sx = 1:m; r_sx = 1:r; T_sx = 1:T; s_dist = 0;

while (nr <= max_restart)

     % Initialize random A and X
       if ~nr & (~mc_on | restart_mc_on)
          Ainit(m_sx',r_sx) = abs(repmat(.1*sin(2*pi*.1*m_sx'),1,r) + repmat(.1*cos(2*pi*.1*r_sx),m,1) + repmat(cos(2*pi*.471*m_sx'),1,r) + repmat(sin(2*pi*.471*r_sx),m,1));
          Ainit = Ainit/max(max(Ainit));

          Xinit(r_sx',T_sx) = abs(repmat(.1*sin(2*pi*.1*r_sx'),1,T) + repmat(.1*cos(2*pi*.1*T_sx),r,1) + repmat(cos(2*pi*.471*r_sx'),1,T) + repmat(sin(2*pi*.471*T_sx),r,1));
          Xinit = Xinit/max(max(Xinit));
       else
          Ainit=rand(m,r);
          Xinit=rand(r,T);
       end

        % Normalization of initial guess
        Ainit = Ainit*diag(1./sum(Ainit,1));

        if (nr == max_restart)&(max_restart > 0)
           A = A_best;
           X = X_best;
        else
           A = Ainit;
           X = Xinit;
        end % initial guess assignment

    Yx = zeros(m,T);
    n = 0; k = 0;

while ((k <= niter_sample)&(nr < max_restart)) | ((k <= niter_selected)&(nr == max_restart)&(norm_A > epsil_normA)& isempty(NoAlts)) | ((k <= niter_selected)&(nr == max_restart)& (NoAlts > 0))

k = k + 1;

if no_iter == 1

    switch type_alg_X

        case 1 % Asymmetric alpha-divergence

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = (1/alpha)*((Y./Z).^alpha - 1);
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1 + alphaS);

        case 2 % EG-DKL

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = log(Y./Z + eps);
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1 + alphaS);

        case 3 % EG-RAG

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = log(2*Y./(Z + Y) + eps);
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1 + alphaS);

        case 4 % EG-SAG

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = log(2*sqrt(Y.*Z)./(Z + Y) + eps) + (Y - Z)./Z ;
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1 + alphaS);

        case 5 % EG-DJ

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = .5*log(Y./Z + eps) + .5*(Y - Z)./Z ;
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1 + alphaS);

        case 6 % EG-RJS

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = (Y - Z)./(Y + Z) ;
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1 + alphaS);

        case 7 % EG-DRJS

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = 2*log(.5*(Y + Z)./Z) + (Z - Y)./(Y + Z);
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1 + alphaS);

        case 8 % EG-SJS

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = log(.5*(Y + Z)./Z + eps) ;
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1 + alphaS);

        case 9 % EG-Tri

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = (2*Y./(Y + Z)).^2 - 1;
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1 + alphaS);

        case 10 % EG-BE

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = alpha_BE*log((Y + alpha_BE*Z)./((1 + alpha_BE)*Z) + eps);
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1 + alphaS);

        case 11 % Pinv

            X = max(1E6*eps,pinv(A)*Y);

        case 12 % Fixed X

            X = S + eps;
            niter_selected = 1000;     % maximum number of iterations for the selected sample (can be adjusted)
            X = diag(sum(X,2))*X;

    end % switch for X

 switch type_alg_A

        case 1 % Asymmetric alpha-divergence

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = (1/alpha)*((Y./Z).^alpha - 1);
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

        case 2 % EG-DKL

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = log(Y./Z + eps);
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

        case 3 % EG-RAG

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = log(2*Y./(Z + Y) + eps);
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

         case 4 % EG-SAG

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = log(2*sqrt(Y.*Z)./(Z + Y) + eps) + (Y - Z)./Z ;
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

         case 5 % EG-DJ

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = .5*log(Y./Z + eps) + .5*(Y - Z)./Z ;
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

         case 6 % EG-RJS

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = (Y - Z)./(Y + Z) ;
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

         case 7 % EG-DRJS

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = 2*log(.5*(Y + Z)./Z) + (Z - Y)./(Y + Z);
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

         case 8 % EG-SJS

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = log(.5*(Y + Z)./Z + eps) ;
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

          case 9 % EG-Tri

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = (2*Y./(Y + Z)).^2 - 1;
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

          case 10 % EG-BE

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = alpha_BE*log((Y + alpha_BE*Z)./((1 + alpha_BE)*Z) + eps);
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

          case 11 % Pinv

            Ap = A;
            A = max(1E6*eps,Y*pinv(X')');
            A = A*diag(1./sum(A,1));

          case 12 % Fixed A

            niter_selected = 1000;     % maximum number of iterations for the selected sample (can be adjusted)
            A = A_true + eps;
            A = A*diag(1./(sum(A,1) + eps));

    end % switch for A

else % no_iter

   for t = 1:no_iter

        switch type_alg_X

        case 1 % Asymmetric alpha-divergence

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = (1/alpha)*((Y./Z).^alpha - 1);
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1+alphaS);

        case 2 % EG-DKL

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = log(Y./Z + eps);
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1+alphaS);

        case 3 % EG-RAG

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = log(2*Y./(Z + Y) + eps);
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1+alphaS);

        case 4 % EG-SAG

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = log(2*sqrt(Y.*Z)./(Z + Y) + eps) + (Y - Z)./Z ;
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1+alphaS);

        case 5 % EG-DJ

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = .5*log(Y./Z + eps) + .5*(Y - Z)./Z ;
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1+alphaS);

        case 6 % EG-RJS

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = (Y - Z)./(Y + Z) ;
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1+alphaS);

        case 7 % EG-DRJS

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = 2*log(.5*(Y + Z)./Z) + (Z - Y)./(Y + Z);
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1+alphaS);

        case 8 % EG-SJS

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = log(.5*(Y + Z)./Z + eps) ;
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1+alphaS);

        case 9 % EG-Tri

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = (2*Y./(Y + Z)).^2 - 1;
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1+alphaS);

        case 10 % EG-BE

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = alpha_BE*log((Y + alpha_BE*Z)./((1 + alpha_BE)*Z) + eps);
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1+alphaS);

        case 11 % Pinv

            X = max(1E6*eps,pinv(A)*Y);

        case 12 % Fixed X

            X = S + eps;
            niter_selected = 1000;     % maximum number of iterations for the selected sample (can be adjusted)
            X = diag(sum(X,2))*X;

    end % switch for X

   end % for t in X

   for t = 1:no_iter

       switch type_alg_A

        case 1 % Asymmetric alpha-divergence

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = (1/alpha)*((Y./Z).^alpha - 1);
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

        case 2 % EG-DKL

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = log(Y./Z + eps);
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

        case 3 % EG-RAG

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = log(2*Y./(Z + Y) + eps);
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

         case 4 % EG-SAG

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = log(2*sqrt(Y.*Z)./(Z + Y) + eps) + (Y - Z)./Z ;
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

         case 5 % EG-DJ

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = .5*log(Y./Z + eps) + .5*(Y - Z)./Z ;
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

         case 6 % EG-RJS

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = (Y - Z)./(Y + Z) ;
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

         case 7 % EG-DRJS

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = 2*log(.5*(Y + Z)./Z) + (Z - Y)./(Y + Z);
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

         case 8 % EG-SJS

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = log(.5*(Y + Z)./Z + eps) ;
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

          case 9 % EG-Tri

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = (2*Y./(Y + Z)).^2 - 1;
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

          case 10 % EG-BE

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = alpha_BE*log((Y + alpha_BE*Z)./((1 + alpha_BE)*Z) + eps);
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

          case 11 % Pinv

            Ap = A;
            A = max(1E6*eps,Y*pinv(X')');
            A = A*diag(1./sum(A,1));

          case 12 % Fixed A

            niter_selected = 1000;     % maximum number of iterations for the selected sample (can be adjusted)
            A = A_true + eps;
            A = A*diag(1./(sum(A,1) + eps));

        end % switch for A

   end % for t in A

end % if no_iter

                if (nr == max_restart)&(mod(k,50)==0)& (~mc_on | restart_mc_on)
                    norm_A = norm(abs(A - Ap),'fro');
                    fprintf(1, 'Restart %d,  %d-th alternating step\n',nr_best+1,k);
                end

                if sum(Index_norm)
                   if (nr == max_restart) & (((k < 50) & (mod(k,5)==0)) | ((k>49) & ((mod(k,50)==0))))

                       s_dist = s_dist + 1;
                       k_select(s_dist) = k;
                       Z = A*X + eps;
                       Z = diag(1./sqrt(var(Z')))*Z;

                        dist_Fro(s_dist) = norm(Y - Z,'fro');
                        dist_KL(s_dist) = sum(sum(Y.*log(Y./Z + eps) - Y + Z));
                        dist_KL2(s_dist) = sum(sum(Z.*log(Z./Y + eps) + Y - Z));
                        dist_Pearson(s_dist) = sum(sum( ((Y - Z).^2)./Z ));
                        dist_Hellinger(s_dist) = sum(sum( (sqrt(Z) - sqrt(Y)).^2 ));
                        dist_JS_rel(s_dist) = sum(sum(2*Y.*log(2*Y./(Y + Z) + eps) + Z - Y));
                        dist_JS_rel2(s_dist) = sum(sum(2*Z.*log(2*Z./(Y + Z) + eps) - Z + Y));
                        Zy = Y + Z;
                        dist_JS(s_dist) = sum(sum(Y.*log(2*Y./Zy + eps) + Z.*log(2*Z./Zy + eps) ));
                        dist_AG_rel(s_dist) = sum(sum(Zy.*log(.5*Zy./Y + eps) + Y - Z));
                        dist_AG(s_dist) = sum(sum(.5*Zy.*log(.5*Zy./sqrt(Y.*Z) + eps)));
                        dist_J(s_dist) = sum(sum( .5*(Y - Z).*log(Y./Z + eps) ));
                        dist_Chi(s_dist) = sum(sum( ((Y + Z).*(Y - Z).^2)./(Y.*Z) ));
                        dist_Tria(s_dist) = sum(sum( ((Y - Z).^2)./(Y + Z) ));
                    end % if multiple
                end % if sum

end % while (k)

% Outer KL divergence
Z = AL*A*X;
Z_outer = norm(Z,'fro') + eps;
KL_outer = sum(sum(Y_true.*log((Y_true + eps)./(Z + eps)) - Y_true + Z))/Z_outer;

           if (nr == 0) | (KL_outer < KL_outer_temp)
              A_best = A; X_best = X; KL_outer_temp = KL_outer; nr_best = nr;
           end % multi-conditions

   nr = nr + 1;

   if nr <=max_restart
      fprintf(1, 'Restart %d, Kullback-Leibler divergence = %e\n',	nr, KL_outer);
   end

end % while (restarts)

X(X <= 0) = eps;

Distance_output = cell(length(s_dist),1);
Distance_output(1) = {[]};
Distance_output(2) = {[]};
Distance_output(3) = {[]};
Distance_output(4) = {[]};
Distance_output(5) = {[]};
Distance_output(6) = {[]};
Distance_output(7) = {[]};
Distance_output(8) = {[]};
Distance_output(9) = {[]};
Distance_output(10) = {[]};
Distance_output(11) = {[]};
Distance_output(12) = {[]};
Distance_output(13) = {[]};
Distance_output(14) = {[]};

if sum(Index_norm)
   Distance_output(1) = {k_select};
   Distance_output(2) = {dist_Fro};
   Distance_output(3) = {dist_KL};
   Distance_output(4) = {dist_KL2};
   Distance_output(5) = {dist_Pearson};
   Distance_output(6) = {dist_Hellinger};
   Distance_output(7) = {dist_JS_rel};
   Distance_output(8) = {dist_JS_rel2};
   Distance_output(9) = {dist_JS};
   Distance_output(10) = {dist_AG_rel};
   Distance_output(11) = {dist_AG};
   Distance_output(12) = {dist_J};
   Distance_output(13) = {dist_Chi};
   Distance_output(14) = {dist_Tria};
end


% -------------------------------------------------------------------------
% Author: Jai Juneja
% Date: 12/02/2013
% -------------------------------------------------------------------------
function World = ekfSLAM(handles, AxisDim, Lmks, Wpts, Obstacles)

    %%%%%%%%%%%%%%%%%%%%%%%%% INITIALISATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    cla; % Clear axes
    rob = Robot; % Create new robot object
    sen = Sensor;
    sen_rf = Sensor;
    sen_rf.noise = [0.1; 3 * pi/180];
    sen_rf.range = 10;
    sen.range = 30;
    sen.fov = 270 * pi/180;
    rob.q = [0.1;2*pi/180];

    [World, rob] = configuration(rob, sen_rf, Lmks, Wpts, AxisDim);

    Graphics = initGraphics(World, Obstacles, handles);

    % Determine which obstacles are moving
    numObstacles = length(Obstacles);
    numSegments = 0;
    movingObstacles = zeros(1, numObstacles);
    for i = 1:numObstacles
        if ~isequal([0; 0], Obstacles(i).velocity)
            movingObstacles(i) = 1;
        end
        numSegments = numSegments + size(Obstacles(i).vertices, 2) - 1;
    end
    movingObstacles = find(movingObstacles);

    %%%%%%%%%%%%%%%%%%%%%%%% TEMPORAL LOOP %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    % Initialise some loop variables
    pose_this_scan = [];
    this_scan_good = 0;
    big_turn = 0;
    r = World.r;    % Location of robot pose in mapspace

    for t = 1:World.tend

        World.t = t;

        %%%%%%%%%%%%%%%%%%%%%%%%% SIMULATE WORLD %%%%%%%%%%%%%%%%%%%%%%%%%%
        R_old = rob.R;
        if ~isempty(World.Wpts)
            rob.steer(World.Wpts);
        end
        for i = movingObstacles
            Obstacles(i).move(AxisDim);
        end
        % n = rob.q .* randn(2,1);    % Control noise in real world
        rob.R = rob.move(zeros(2,1), 'true');        % Move with noise
        for lid = 1:size(World.W,2)
            v = sen_rf.noise .* randn(2,1);
            World.y(:,lid) = scanPoint(rob.R, World.W(:,lid)) + v;
        end
        lmks_all = World.y;
        % Determine landmarks that are within the sensor range
        [World.y, lmks_visible] = sen_rf.constrainMeasurement(World.y);

        %%%%%%%%%%%%%%%%%%%%%%%%%%% START EKF %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%%%%%%%%% EKF PREDICTION STEP %%%%%%%%%%%%%%%%%%%%%%%

        % 1. POSE PREDICTION USING SCAN MATCHING %%%%%%%%%%%%%%%%%%%%%%%%%%
        % TODO: if scan_matching, then do this step. Scan match only if
        % ~isempty(Obstacles)
        enough_correlations = 0;

        % Current stored scan is now the last scan
        last_scan_good = this_scan_good;
        pose_last_scan = pose_this_scan;
        this_scan_good = 0; % Initialise this_scan_good for this iteration

        % Put the previous scan's data in scan_ref
        scan_ref = World.scan_data;
        % Initialise scan data vector at maximum possible size it can take
        World.scan_data = -ones(3, numSegments * (round(sen.fov/sen.angular_res) + 1));
        scan_ndx = 1;
        for i = 1:length(Obstacles)
            scan_data = Obstacles(i).getMeasured(sen, rob);
            % Build scan_data vector
            if ~isempty(scan_data)
                World.scan_data(:, scan_ndx:scan_ndx+size(scan_data, 2)-1) = scan_data;
                scan_ndx = scan_ndx + size(scan_data, 2);
            end
        end
        % Reduce scan to only those appended in above for loop
        World.scan_data = World.scan_data(:, World.scan_data(2, :) ~= -1);

        % Reduce scan so that each laser angle only has one associated
        % measurement
        World.scan_data = removeDuplicateLasers(World.scan_data);
        World.scan_data = World.scan_data(2:3, :); % Remove index row

        % First add Gaussian white noise to scan
        v = repmat(sen.noise, 1, size(World.scan_data, 2)) .* randn(2,size(World.scan_data, 2));
        World.scan_data = World.scan_data + v;

        if ~isempty(World.scan_data)
            obstaclesDetected = 1;

            % Convert to Cartesian co-ordinates for scan matching
            World.scan_data = getInvMeasurement(World.scan_data);

            % If the number of point scanned exceeds the specified
            % tolerance, it is suitable for scan matching
            if size(World.scan_data, 2) > World.scan_corr_tolerance
                this_scan_good = 1;
            end

            % Convert to global co-ordinates for graphical display
            % World.scan_global = transToGlobal(rob.R, World.scan_data);
        else
            obstaclesDetected = 0;
            World.scan_global = [];
        end
        % If both last and current scans are good for scan matching,
        % proceed to match scans:
        if last_scan_good && this_scan_good
            n = rob.q .* randn(2,1); % Noise in odometry measurement

            % Do ICP scan matching using odometry input as initial guess
            [R, T, correl, icp_var] = doICP(scan_ref, World.scan_data, 1, rob.u + n);
            % If there are enough points correlated between the two
            % scans:
            if length(correl) > World.scan_corr_tolerance
                enough_correlations = 1;
                % Determine estimated pose from scan match
                r_scan = zeros(3, 1);
                da = getPiToPi(asin(R(2,1)));
                r_scan(1:2) = transToGlobal(pose_last_scan, T);
                r_scan(3) = pose_last_scan(3) + da;

                % Transform scan data to global co-ordinates
                scan_data_corr = World.scan_data;
                scan_data_corr = transToGlobal(r_scan, scan_data_corr);
                % Grab data points that were successfully corellated
                scan_data_corr = scan_data_corr(:, correl(:, 2)');

                [r_odo, R_r, R_n] = rob.move(n);

                % Compute covariance matrix of scan match
                C = getICPCovariance(icp_var, scan_data_corr);
                J_u = [R zeros(2, 1); 0 0 1];
                cov_scan = J_u * C * J_u';
                cov_scan_norm = trace(cov_scan);
            end
        end

        % 2. POSE PREDICTION USING ODOMETRY MEASUREMENTS %%%%%%%%%%%%%%%%%%

        % If there isn't a useful scan, just use odometry data
        if ~enough_correlations
            n = rob.q .* randn(2,1);
            [r_odo, R_r, R_n] = rob.move(n);
            cov_scan = zeros(3, 3);
            dr_scan = zeros(3, 1);
        else
            dr_scan = r_scan - rob.r;
            dr_scan(3) = getPiToPi(dr_scan(3));
        end

        % 3. WEIGHTAGE OF ODOMETRY AND SCAN MATCH PREDICTIONS %%%%%%%%%%%%%

        cov_odo = R_n * World.Q * R_n';
        cov_odo_norm = trace(cov_odo);

        dr_odo = r_odo - rob.r;
        dr_odo(3) = getPiToPi(dr_odo(3));

        dr_true = rob.R - R_old;

        % Check if the robot is making a large turn
        if abs(dr_scan(2)) > pi/6 || abs(dr_odo(2)) > pi/6
            big_turn = 1;
        elseif abs(dr_odo(2)) < pi/18
            big_turn = 0;
        end

        % Determine weights:
        % If not enough correlations, or turning is large, use odometry
        if ~enough_correlations % || big_turn
            weight_odo = 1;
            weight_scan = 0;
        else
            weight_odo = cov_scan_norm / (cov_scan_norm + cov_odo_norm);
            weight_scan = 1 - weight_odo;
            weight_scan = 1;
            weight_odo = 0;
        end

        % Determine robot pose using weights:
        rob.r = weight_odo * dr_odo + weight_scan * dr_scan + rob.r;
        World.x(r) = rob.r;

        % Covariance update
        % Caution: this method of update is suboptimal for CPU processing.
        % Alternative is to have a single equation that updates both
        % the pose and landmark covariances in a single step, but this
        % equation is rather complicated.
        P_rr = World.P(r,r);
        World.P(r,:) = R_r * World.P(r,:);
        World.P(:,r) = World.P(r,:)';
        World.P(r,r) = R_r * P_rr * R_r' + ...
            weight_scan * cov_scan + ...
            weight_odo * cov_odo;

        %%%%%%%%%%%%%%%%%%%%%% EKF UPDATE STEP %%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % 1. CORRECT KNOWN VISIBLE LANDMARKS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        % Find the visible landmarks. The below code circumvents the data
        % association problem:
        r_old = rob.r;

        lmks_visible_known = find(World.l(1,lmks_visible));
        lids = lmks_visible(lmks_visible_known);

        for lid = lids
            % Measurement prediction
            [e, E_r, E_l] = scanPoint(rob.r, World.x(World.l(:,lid)));

            E_rl = [E_r E_l];
            rl   = [r World.l(:,lid)'];
            E    = E_rl * World.P(rl,rl) * E_rl';

            % Actual Measurement
            yi = World.y(:,lid);

            % Innovation
            z = yi - e;
            z(2) = getPiToPi(z(2));
            Z = World.M + E;

            % Kalman gain
            K = World.P(:, rl) * E_rl' * Z^-1;

            % Update state and covariance
            World.x = World.x + K * z;
            World.P = World.P - K * Z * K'; % The complexity of this line is
                                            % very high (subtraction of two
                                            % large matrices)
            rob.r = World.x(r);
        end

        % 2. INITIALISE NEW LANDMARKS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        % Check landmark availabiliy.
        % Again data association is avoided. A new landmark is initialised
        % in the storage space reserved specifically for that landmark:
        lmks_visible_unknown = find(World.l(1,lmks_visible)==0);
        lids = lmks_visible(lmks_visible_unknown);

        if ~isempty(lids)
            for lid = lids
                s = find(World.mapspace, 2);
                if ~isempty(s)
                    World.mapspace(s) = 0;
                    World.l(:,lid) = s';
                    % Measurement
                    yi = World.y(:,lid);

                    [World.x(World.l(:,lid)), L_r, L_y] = invScanPoint(rob.r, yi);
                    World.P(s,:) = L_r * World.P(r,:);
                    World.P(:,s) = World.P(s,:)'; % Cross variance of lmk with all other lmks
                    World.P(s,s) = L_r * World.P(r,r) * L_r' + L_y * World.M * L_y';
                end
            end
        end
        pose_this_scan = rob.r; % The corrected pose at which the scan was taken

        %%%%%%%%%%%%%%%%%%%%%%%%%% MAPPING %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Determine pose correction during update step
        update = rob.r - r_old;

        if enough_correlations && ~isequal(rob.r, r_old)
            % Adjust latest correlated scan points to account for pose correction
            World.scan_data = transformPoints(World.scan_data, update);

            World = doMapping(World, sen, rob.r, World.scan_data, handles);
        elseif ~obstaclesDetected && isequal(mod(World.t, 5), 0)
            % If no obstacles have been detected, only compute map every 5
            % iterations
            World = doMapping(World, sen, rob.r, World.scan_data, handles);
        end

        if ~isempty(World.scan_data)
            World.scan_global = transToGlobal(rob.r, World.scan_data);
        end

        %%%%%%%%%%%%%%%%%%%%%%% HISTORICAL DATA COLECTION %%%%%%%%%%%%%%%%%
        World.R_hist(:, t) = rob.R(1:2);	% True position history
        World.r_hist(:, t) = rob.r(1:2);   % Estimated position history
        pose_error2 = (rob.R(1:2) - rob.r(1:2)).^2;
        World.error_hist(t) = sqrt(pose_error2(1) + pose_error2(2));
        World.Pr_hist(:, t) = [World.P(r(1),r(1)); World.P(r(2),r(2))];

        if enough_correlations
            scan_error = abs(dr_scan-dr_true); scan_error(3) = abs(getPiToPi(scan_error(3)));
            scan_error(1) = pdist([0 0; scan_error(1) scan_error(2)]); scan_error(2) = [];
        else
            scan_error = [0; 0];
        end

        odo_error = abs(dr_odo-dr_true); odo_error(3) = abs(getPiToPi(odo_error(3)));
        odo_error(1) = pdist([0 0; odo_error(1) odo_error(2)]); odo_error(2) = [];

        World.scan_error_hist(:, t) = scan_error;
        World.odo_error_hist(:, t) = odo_error;
        World.turning_hist(t) = rob.u(2);
        World.weight_scan_hist(t) = weight_scan;
        World.weight_odo_hist(t) = weight_odo;

        %%%%%%%%%%%%%%%%%%%%%%%%%%%% GRAPHICS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        Graphics.lmks_all = lmks_all; Graphics.lmks_visible = lmks_visible;
        doGraphics(rob, World, Graphics, Obstacles(movingObstacles), AxisDim);

    end

    %%%%%%%%%%%%%%%%%%%%%%%% PLOT HISTORICAL DATA %%%%%%%%%%%%%%%%%%%%%%%%%
    % Plot trajectories;
    set(Graphics.R_hist, 'xdata', World.R_hist(1,:), 'ydata', World.R_hist(2, :))
    set(Graphics.r_hist, 'xdata', World.r_hist(1,:), 'ydata', World.r_hist(2, :))

    % Plot position uncertainties
    figure('color', 'white')
    subplot(2, 1, 1)
    plot(1:World.tend, World.error_hist)
    title('EKF SLAM: position error over time')
    xlabel('Time (s)'), ylabel('Position Error (m)')
    subplot(2, 1, 2)
    plot(1:World.tend, sqrt(World.Pr_hist(1, :)), 'r', ...
        1:World.tend, sqrt(World.Pr_hist(2, :)), 'b')
    title('EKF SLAM: position uncertainty over time')
    xlabel('Time (s)'), ylabel('Standard Deviation (m)')
    legend('St. Dev. in x', 'St. Dev. in y')

    figure('color', 'white')
    subplot(4, 1, 1)
    AX = plotyy(1:World.tend, World.scan_error_hist(1,:), ...
        1:World.tend, World.scan_error_hist(2,:), 'plot');
    set(get(AX(1),'Ylabel'),'String',['Position' char(10) 'Error (m)'])
    set(get(AX(2),'Ylabel'),'String',['Angular' char(10) 'Error (rad)'])
    title('Scan errors over time')
    xlabel('Time (s)')

    subplot(4, 1, 2)
    AX2 = plotyy(1:World.tend, World.odo_error_hist(1,:), ...
        1:World.tend, World.odo_error_hist(2,:), 'plot');
    set(get(AX2(1),'Ylabel'),'String',['Position' char(10) 'Error (m)'])
    set(get(AX2(2),'Ylabel'),'String',['Angular' char(10) 'Error (rad)'])
    title('Odometry error over time')

    subplot(4, 1, 3)
    plot(1:World.tend, World.weight_scan_hist, 'r', ...
        1:World.tend, World.weight_odo_hist, 'b')
    title('Scan and odometry weights over time')
    xlabel('Time (s)'), ylabel('Weight')
    legend('Scan', 'Odometry')
    axis tight

    subplot(4, 1, 4)
    plot(1:World.tend, World.turning_hist)
    title('Turning control over time')
    xlabel('Time (s)'), ylabel('Angle (rad)')
    axis tight
end


%% This script simulates the LODCO-based epsilon-Greedy algorithm.
%  author: Hailiang Zhao
clc, clear
opt = optimset('Display', 'none');

%% basic parameter settings (had better not change those paras)
k = 1e-28;                % effective switched capacitance (a constant decided by the chip architecture)
tau = 0.002;              % the length of time slot (in second)
phi = 0.002;              % the cost of task dropping (in second)
omega = 1e6;              % the bandwidth of MEC server (in Hz)
sigma = 1e-13;            % the noise power of the receiver (in W)
p_tx_max = 1;             % the maximum transmit power of mobile device (in W)
f_max = 1.5e9;            % the maximum CPU-cycle frequency of mobile device (in Hz)
E_max = 0.002;            % the maximum amout of battery output energy (in J)
L = 1000;                 % the input size of the computation task (in bit)
X = 737.5;                % the number of CPU cycles needed on processing one bit of task
W = L * X;                % the number of CPU cycles needed on processing one task
E_H_max = 48e-6;          % the upper bound of the energy arrive at the mobile device (in J)
p_H = E_H_max / (2*tau);  % the average Energy Harvesting (EH) power (in W)
g0 = power(10, -4);       % the path-loss constant
d0 = 1;                   % the relative distance between each mobile device and each MEC server

%% parameter control
N = 10;                   % the number of mobile devices
M = 8;                    % the number of MEC servers
T = 2000;                 % the number of time slot (a.k.a. the size of the time horizon)
tau_d = 0.002;            % execution deadline (in second)
d = 50;                   % the distance between the mobile device and the MEC server (in meter)
E_min = 0.02e-3;          % the minimum amout of battery output energy (in J)
V = 1e-5;                 % the weight of penalty (the control parameter introduced by Lyapunov Optimization)
rho = 0.6;                % the probability that the computation task is requested
max_connects = 4;         % the maximum number of processible mobile devices for each MEC server ($ \frac{f_s^{max} \tau}{L X} $)
min_distance = 10;        % the minimum distance from mobile device to MEC server
max_distance = 50;        % the maximum distance from mobile device to MEC server
eps = 0.8;                % if rand() <= eps, for the best i-j pair, we always choose MEC server execution mode (except no MEC server to choose)

% the lower bound of perturbation parameter
E_max_hat = min(max(k * W * (f_max)^2, p_tx_max * tau), E_max);
theta = E_max_hat + V * phi / E_min;

%% allocate storage for valuable results
B = zeros(T, N);                   % the battery energy level (in J)
B_hat = zeros(T, N);               % the virtual battery energy level ($B_hat = B - theta$)
e = zeros(T, N);                   % the amout of the harvested and stored energy (in J)
chosen_mode = zeros(T, N);         % {1: local, 2: remote, 3: drop, 4: no task request}
chosen_server = zeros(T, N);       % record the index of chosen server for each mobile device if its choice is MEC server execution
f = zeros(T, N);                   % the CPU-cycle frequency of local execution (in Hz)
p = zeros(T, N);                   % the transmit power of computation offloading (in W)

mobile_exe_cost = zeros(T, N);     % the mobile execution cost (delay) (in second)
server_exe_cost = zeros(T, N);     % the MEC server execution cost (delay) (in second)
final_chosen_cost = zeros(T, N);   % the final execution delay under currently chosen modes (in second)

mobile_exe_E = zeros(T, N);        % the energy consumption for mobile execution (in J)
server_exe_E = zeros(T, N);        % the energy consumption for MEC server execution (in J)
final_chosen_E = zeros(T, N);      % the energy consumption of the final chosen modes (in J)

%% simulation begin
t = 1;
while t <= T
    disp(['===> Time slot #', num2str(t), ' <==='])

    %% allocate storage for mode-chosen
    device_server_pairs = [];      % each column represents i, j, J_s^{\star}(i, j), respectively
    remained_connects = max_connects * ones(M, 1);    % the available connections of MEC servers
    J_m = zeros(N, 1); J_s = zeros(N, M);             % the matrices for J_m and J_s values
    p_mat = zeros(N, M);                              % the matrix for transmit power
    server_cost_mat = zeros(N, M);                    % the matrix for MEC server execution cost
    server_E_mat = zeros(N, M);                       % the matrix for energy consumption of MEC server

    %% initialization
    % generate the virtual battery energy level
    B_hat(t, :) = B(t, :) - theta;
    % generate the channel power gain (from each mobile device to each MEC sever)
    distances = unifrnd(min_distance, max_distance, N, M);
    gamma = exprnd(1, N, M);
    h_mat = g0 * gamma .* power(d0 ./ distances, 4);

    %% step 1: for each mobile device, choose the initial mode
    for i = 1: N
        disp(['Mobile device #', num2str(i)])

        %% step 1.1: get the optimal energy harvesting no matter whether task is requested
        E_H_t = unifrnd(0, E_H_max);
        if B_hat(t, i) <= 0
            e(t, i) = E_H_t;
        end

        %% step 1.2: get the (initial) optimal computation offloading strategy (I_m(i), I_s(i, :), I_d(i), f(t, i), p(t, i))
        % generate the task request
        zeta = binornd(1, rho);
        if zeta == 0
            % chosen mode has to be 4
            disp('no task request generated!')
            chosen_mode(t, i) = 4;
        else
            % chosen_mode is chosen from {1, 2, 3}
            disp('task request generated!')

            %% step 1.2.1: solve the optimization problem $\mathcal{P}_{ME}$ (f(t, i) > 0)
            % calculate f_L and f_U
            f_L = max(sqrt(E_min / (k * W)), W / tau_d);
            f_U = min(sqrt(E_max / (k * W)), f_max);
            if f_L <= f_U
                % the sub-problem is feasible
                disp('mobile execution ($\mathcal{P}_{ME}$) is feasible!')

                if B_hat(t, i) < 0
                    f_0 = (V / (-2 * B_hat(t, i) * k))^(1/3);
                else
                    % complex number may exist, which may lead to error
                    f_0 = -(V / (2 * B_hat(t, i) * k))^(1/3);
                end

                if (f_0 > f_U && B_hat(t, i) < 0) || (B_hat(t, i) >= 0)
                    f(t, i) = f_U;
                elseif f_0 >= f_L && f_0 <= f_U && B_hat(t, i) < 0
                    f(t, i) = f_0;
                elseif f_0 < f_L && B_hat(t, i) < 0
                    f(t, i) = f_L;
                end
                % check whether f(t, i) is zero
                if f(t, i) == 0
                    disp('Something wrong! f is 0!')
                end

                % calculate the delay of mobile execution
                mobile_exe_cost(t, i) = W / f(t, i);
                % calculate the energy consumption of mobile execution
                mobile_exe_E(t, i) = k * W * (f(t, i)^2);
                % calculate the value of optimization goal
                J_m(i) = -B_hat(t, i) * k * W * (f(t, i)^2 + V * W / f(t, i));
            else
                % the sub-problem is not fasible because (i) the limited
                % computation capacity or (ii) time cosumed out of deadline
                % or (iii) the energy consumed out of battery energy level
                % If it is not feasible, it just means that we cannot choose
                % 'I_m(i)=1'. It dosen't mean that the task has to be dropped.
                disp('mobile execution ($\mathcal{P}_{ME}$) is not feasible!')
                f(t, i) = 0;
                mobile_exe_cost(t, i) = 0;
                mobile_exe_E(t, i) = 0;
                % 'I_m(i)=1' can never be chosen if mobile execution goal is inf
                J_m(i) = inf;
            end

            %% step 1.2.2: solve the optimization problem $\mathcal{P}_{SE}$ (p(t, i) > 0)
            % calculate J_s(i, j) from mobile device i to each MEC server j
            for j = 1: M
                disp(['MEC server #', num2str(j)])
                h = h_mat(i, j);

                E_tmp = sigma * L * log(2) / (omega * h);
                p_L_taud = (power(2, L / (omega * tau_d)) - 1) * sigma / h;
                % calculate p_L
                if E_tmp >= E_min
                    p_L = p_L_taud;
                else
                    % calculate p_E_min (use inline function and fsolve)
                    y = @(x) x * L - omega * log2(1 + h*x/sigma) * E_min;
                    % accroding to the function figure, p_L_taud is a positive
                    % number around 0.2
                    p_E_min = fsolve(y, 0.2, opt);
                    p_L = max(p_L_taud, p_E_min);
                end
                % calculate p_U
                if E_tmp >= E_max
                    p_U = 0;
                else
                    % caculate p_E_max (use inline function and fsolve)
                    y = @(x) x * L - omega * log2(1 + h*x/sigma) * E_max;
                    % accroding to the function figure, p_E_max is a large positive
                    % number around 20
                    p_E_max = fsolve(y, 100, opt);
                    p_U = min(p_tx_max, p_E_max);
                end

                if p_L <= p_U
                    % the sub-problem is feasible
                    disp('MEC server execution ($\mathcal{P}_{SE}$) is feasible!')
                    % calculate p_0
                    virtual_battery = B_hat(t, i);
                    y = @(x) virtual_battery * log2(1 + h*x/sigma) + ...
                        h * (V - virtual_battery*x) / log(2) / (sigma + h*x);
                    p_0 = fsolve(y, 0.5, opt);

                    if (p_U < p_0 && B_hat(t, i) < 0) || B_hat(t, i) >= 0
                        p_mat(i, j) = p_U;
                    elseif p_0 < p_L && B_hat(t) < 0
                        p_mat(i, j) = p_L;
                    elseif p_0 >= p_L && p_0 <= p_U && B_hat(t, i) < 0
                        p_mat(i, j) = p_0;
                    end
                    % check whether p_mat(i, j) is zero
                    if p_mat(i, j) == 0
                        disp('Something wrong! p is 0!')
                    end

                    % calculate the delay of MEC server execution
                    server_cost_mat(i, j) = L / (omega * log2(1 + h*p_mat(i, j)/sigma));
                    % calculate the energy consumption of MEC server execution
                    server_E_mat(i, j) = p_mat(i, j) * server_cost_mat(i, j);
                    % calculate the value of optimization goal
                    J_s(i, j) = (-B_hat(t, i) * p_mat(i, j) + V) * server_cost_mat(i, j);

                    % (we can not set server_exe_cost(t, i) and server_exe_E(t, i) for now)
                else
                    % the sub-problem is not feasible because (i) the limited transmit
                    % power or (ii) time cosumed out of deadline or (iii) the energy
                    % consumed out of battery energy level
                    % If it is not feasible, it just means that we cannot choose
                    % 'I_s(i,j)=1'. It dosen't mean that the task has to be dropped.
                    disp('MEC server execution ($\mathcal{P}_{SE}$) is not feasible!')
                    p_mat(i, j) = 0;
                    server_cost_mat(i, j) = 0;
                    server_E_mat(i, j) = 0;
                    % 'I_s(i,j)=1' can never be chosen if MEC server execution goal is inf
                    J_s(i, j) = inf;

                    % (we can not set server_exe_cost(t, i) and server_exe_E(t, i) for now)
                % Similarly, we do not check whether the energy cunsumed is larger than
                % battery energy level because the problem $\mathcal{J}_{CO}$ does
                % not have constraint (8).
                end
            end

            %% step 1.2.3: choose the (initial) optimal execution mode
            J_d = V * phi;
            disp(['J_m(i):', num2str(J_m(i))])
            disp(['J_s(i,:):', num2str(J_s(i, :))])
            [~, mode] = min([J_m(i), J_s(i, :), J_d]);
            if mode == 1
                % mobile execution
                chosen_mode(t, i) = 1;
                final_chosen_cost(t, i) = mobile_exe_cost(t, i);
                final_chosen_E(t, i) = mobile_exe_E(t, i);
            elseif mode == (M+2)
                % drop
                chosen_mode(t, i) = 3;
                final_chosen_cost(t, i) = phi;
                final_chosen_E(t, i) = 0;
            else
                % MEC servre execution
                chosen_mode(t, i) = 2;
                % add i, the chosen j, and their J_s(i, j) value into the pairs
                device_server_pairs = [device_server_pairs; [i, mode-1, J_s(i, mode-1)]];
            end
        end
    end

    %% step 2: allocate connection right for each mobile device who chooses MEC server execution
    while ~isempty(device_server_pairs)
        if rand() <= eps
            % find the {i, j, J_s(i, j)} with the minimum J_s(i, j) in the device_server_pairs, choose MEC server execution mode,
            % except no MEC server to choose
            [min_Js, idx] = min(device_server_pairs(:, 3));
            i = device_server_pairs(idx, 1); j = device_server_pairs(idx, 2);    % find the i and its j
            if remained_connects(j) >= 1
                % the chosen i can be allocated to the j-th MEC server
                % set its final modes as 2 and record the final cost and energy consumption for it
                chosen_mode(t, i) = 2;          % this is not necessary
                p(t, i) = p_mat(i, j);
                server_exe_cost(t, i) = server_cost_mat(i, j);
                server_exe_E(t, i) = server_E_mat(i, j);
                chosen_server(t, i) = j;
                final_chosen_cost(t, i) = server_exe_cost(t, i);
                final_chosen_E(t, i) = server_exe_E(t, i);

                % update remained_connects for j
                remained_connects(j) = remained_connects(j) - 1;
                % finally, remove it from global pairs, it is done
                device_server_pairs(device_server_pairs(:, 1) == i, :) = [];
            else
                % the chosen i can not be allocated to the j-th MEC server
                % set the mobile device's J_s(i, j) as inf
                % (which means no matter what happens, the MEC server j can never be chosen)
                J_s(i, j) = inf;
                % remove it from global pairs
                device_server_pairs(device_server_pairs(:, 1) == i, :) = [];
                % if there still exists j' where J_s(i, j') is not inf, we choose the best j',
                % otherwise we can only choose from mobile execution and drop
                if min(J_s(i, :)) ~= inf
                    [second_min_Js, second_min_j] = min(J_s(i, :));
                    % add the new pair into global pairs
                    device_server_pairs = [device_server_pairs; [i, second_min_j, second_min_Js]];
                else
                    % choose from mobile execution and drop for i
                    [~, mode] = min([J_m(i), inf, J_d]);
                    chosen_mode(t, i) = mode;
                    if mode == 1
                        final_chosen_cost(t, i) = mobile_exe_cost(t, i);
                        final_chosen_E(t, i) = mobile_exe_E(t, i);
                    else
                        final_chosen_cost(t, i) = phi;
                        final_chosen_E(t, i) = 0;
                    end
                end
            end
        else
            % do as LODCO-based Greedy algorithm do
            for j = 1: M
                % find every pair who chooses the MEC server j
                device_j_pairs = device_server_pairs(device_server_pairs(:, 2) == j, :);
                % find those devices is
                is = device_j_pairs(:, 1);
                if isempty(is)
                    disp(['For current MEC server #', num2str(j), ', no mobile device choose it!'])
                    % go to handle next MEC server
                    continue;
                end
                if remained_connects(j) >= length(is)
                    % every mobile device who chooses j can be connected,
                    % set their final modes as 2 and record the final cost and energy consumption for them
                    chosen_mode(t, is) = 2;     % this is not necessary
                    p(t, is) = transp(p_mat(is, j));
                    server_exe_cost(t, is) = transp(server_cost_mat(is, j));
                    server_exe_E(t, is) = transp(server_E_mat(is, j));
                    chosen_server(t, is) = repmat(j, 1, length(is));
                    final_chosen_cost(t, is) = server_exe_cost(t, is);
                    final_chosen_E(t, is) = server_exe_E(t, is);

                    % update remained_connects for j
                    remained_connects(j) = remained_connects(j) - length(is);
                    % finally, remove them from global pairs, they are done
                    device_server_pairs(device_server_pairs(:, 2) == j, :) = [];
                else
                    if remained_connects(j) == 0
                        % no mobile device can be connected, remove all mobile devices who chooses j from global pairs
                        device_server_pairs(device_server_pairs(:, 2) == j, :) = [];
                        % set those mobile devices' J_s(i, j) as inf
                        % (which means no matter what happens, the MEC server j can never be chosen)
                        J_s(is, j) = inf;
                        % choose mode again for those i and insert new potential pairs into global pairs
                        for idx = 1: numel(is)
                            i = is(idx);
                            [~, mode] = min([J_m(i), J_s(i, :), J_d]);
                            if mode == 1
                                chosen_mode(t, i) = 1;
                                final_chosen_cost(t, i) = mobile_exe_cost(t, i);
                                final_chosen_E(t, i) = mobile_exe_E(t, i);
                            elseif mode == (M+2)
                                chosen_mode(t, i) = 3;
                                final_chosen_cost(t, i) = phi;
                                final_chosen_E(t, i) = 0;
                            else
                                chosen_mode(t, i) = 2;
                                device_server_pairs = [device_server_pairs; [i, mode-1, J_s(i, mode-1)]];
                            end
                        end
                    else
                        % some mobile devices can be connected, set their final modes as 2 and remove them from global pairs
                        % besides, record the final cost and energy consumption for them
                        % for the left i', remove them from global pairs and set J_s(i', j) as inf,
                        % choose mode again for those i' and insert new potential pairs into global pairs

                        % sort the J_s(is, j) and return those lucky idxs
                        [~, idxs] = sort(device_j_pairs(:, 3));
                        for idx = 1: remained_connects(j)
                            i = idxs(idx);
                            chosen_mode(t, i) = 2;
                            p(t, i) = p_mat(i, j);
                            server_exe_cost(t, i) = server_cost_mat(i, j);
                            server_exe_E(t, i) = server_E_mat(i, j);
                            chosen_server(t, i) = j;
                            final_chosen_cost(t, i) = server_exe_cost(t, i);
                            final_chosen_E(t, i) = server_exe_E(t, i);

                            % remove i from global pairs
                            device_server_pairs(device_server_pairs(:, 1) == i, :) = [];
                        end

                        % update remained_connects for j
                        remained_connects(j) = 0;

                        % for those unlucky mobile devices i', set J_s(i', j) as inf (i' are in currently device_server_pairs(: , j) now)
                        residual_is = device_server_pairs(device_server_pairs(:, 2) == j, 1);
                        J_s(residual_is, j) = inf;
                        for idx = 1: numel(residual_is)
                            residual_i = residual_is(idx);
                            [~, mode] = min([J_m(residual_i), J_s(residual_i, :), J_d]);
                            if mode == 1
                                chosen_mode(t, residual_i) = 1;
                                final_chosen_cost(t, residual_i) = mobile_exe_cost(t, residual_i);
                                final_chosen_E(t, residual_i) = mobile_exe_E(t, residual_i);
                            elseif mode == (M+2)
                                chosen_mode(t, residual_i) = 3;
                                final_chosen_cost(t, residual_i) = phi;
                                final_chosen_E(t, residual_i) = 0;
                            else
                                chosen_mode(t, residual_i) = 2;
                                device_server_pairs = [device_server_pairs; [residual_i, mode-1, J_s(residual_i, mode-1)]];
                            end
                        end
                    end
                end
            end
        end
    end

    %% step 3: update the battery energy level and go to the next time slot
    B(t + 1, :) = B(t, :) - final_chosen_E(t, :) + e(t, :);
    t = t + 1;

end

%% step 4: evaluate the simulation results
% 1. the battery energy level vs. time slot
figure
plot(1:T, B(1:T, :))
hold on
plot(1:T, repmat(theta + E_H_max, [T, 1]), '-')
title('Envolution of battery energy level')
xlabel('time slot')
ylabel('battery energy level $B_t$ of each mobile device', 'Interpreter','latex')

% 2. the average execution cost vs. time slot
accumulated = 0;
average_cost = zeros(T, N);
figure
for i = 1: N
    % draw for each mobile device
    request_num = 0;
    for t = 1: T
        accumulated = accumulated + final_chosen_cost(t, i);
        if chosen_mode(t, i) ~= 4
            % there exists task request
            request_num = request_num + 1;
        end
        average_cost(t, i) = accumulated / request_num;
    end
    plot(1:T, average_cost(:, i));
    hold on
end
title('Envolution of average execution cost')
xlabel('time slot')
ylabel('average execution cost $\frac{1}{T} \sum_{t=0}^{T-1} cost^t$ of each mobile device', 'Interpreter','latex')

% 3. the average ratio of each chosen mode of the ith mobile device vs. time slot
average_ratio = zeros(T, 3);
mobile_exe = 0; server_exe = 0; drop = 0;
request_num = 0;
i = 1;          % we simply choose the first mobile device
for t = 1: T
    if final_chosen_cost(t, i) == 0
        continue
    else
        request_num = request_num + 1;
        if chosen_mode(t, i) == 1
            mobile_exe = mobile_exe + 1;
        elseif chosen_mode(t, i) == 2
            server_exe = server_exe + 1;
        else
            drop = drop + 1;
        end
    end
    average_ratio(t, :) = [mobile_exe, server_exe, drop] / request_num;
end
figure
plot(1:T, average_ratio(:, 1));
hold on
plot(1:T, average_ratio(:, 2));
hold on
plot(1:T, average_ratio(:, 3));
legend('mobile execution', 'MEC server execution', 'drop')
title('Envolution of average ratio of chosen modes')
xlabel('time slot')
ylabel('average  ratio of chosen modes $\frac{1}{T} \sum_{t=0}^{T-1} \{I_m^t, I_s^t, I_d^t\}$ of the i-th mobile device', 'Interpreter','latex')


%%***********************************************************************
%%  Read in a problem in SDPA sparse format.
%%
%%  [blk,At,C,b] = read_sdpa(fname)
%%
%%  Input: fname = name of the file containing SDP data in
%%                 SDPA foramt.
%%  Important: the data is assumed to contain only
%%             semidefinite and linear blocks.
%%
%%***********************************************************************
%% SDPNAL+
%% Copyright (c) 2014 by
%% Liuqin Yang, Defeng Sun, and Kim-Chuan Toh
%%***********************************************************************
   function [blk,At,C,b] = read_sdpa(fname)

%%
%%  Open the file for input
%%
   compressed = 0;
   if exist(fname)
      fid = fopen(fname,'r');
   elseif exist([fname,'.Z']);
      compressed = 1;
      unix(['uncompress ',fname,'.Z']);
      fid = fopen(fname,'r');
   elseif exist([fname,'.gz']);
      compressed = 2;
      unix(['gunzip ',fname,'.gz']);
      fid = fopen(fname,'r');
   else
      fprintf(['*** Problem "',fname,'" not found, please specify the correct path or problem name. \n']);
      blk = []; At = []; C = []; b = [];
      return;
   end
%%
%%  Clean up special characters and comments from the file
%%
   [datavec,count] = fscanf(fid,'%c');
   linefeeds = findstr(datavec,char(10));
   comment_chars = '*"=';
   cumidx = [];
   for i=1:length(comment_chars)
      idx = findstr(datavec,comment_chars(i));
      cumidx = [cumidx,idx];
   end
   for j=length(cumidx):-1:1
      if (cumidx(j)==1) || (strcmp(datavec(cumidx(j)-1),char(10)))
         datavec(cumidx(j):linefeeds(min(find(cumidx(j)<linefeeds))))='';
      else
         datavec(cumidx(j):linefeeds(min(find(cumidx(j)<linefeeds)))-1)='';
      end
   end
   special_chars = ',{}()';
   cumidx=[];
   for i=1:length(special_chars)
      idx = findstr(datavec,special_chars(i));
      cumidx = [cumidx,idx];
   end
   datavec(cumidx) = blanks(length(cumidx));
   clear linefeeds;
%%
%% Close the file
%%
   fclose('all');
   if compressed==1; unix(['compress ',fname]); end;
   if compressed==2; unix(['gzip ',fname]); end;
%%
%%  Next, read in basic problem size parameters.
%%
   datavec = sscanf(datavec,'%f');
   if size(datavec,1) < size(datavec,2); datavec = datavec'; end;
   m = datavec(1);
   numblk  = datavec(2);
   blksize = datavec(2+[1:numblk]);
   if size(blksize,1) > size(blksize,2); blksize = blksize'; end
%%
%% Get input  b.
%%
   idxstrt = 2+numblk;
   b = datavec(idxstrt+[1:m]);
   idxstrt = idxstrt+m;
   b = -b;
%%
%% Construct blk
%%
   deblksize = 80;
   spblkidxtmp = find( (blksize>1) & (blksize < deblksize) );
   spblkidxtmp = sort(spblkidxtmp);
   deblkidx = find( (blksize<=1) | (blksize >= deblksize) );
   denumblk = length(deblkidx);
   linblkidx = zeros(1,denumblk);
   for p = 1:denumblk
      n = blksize(deblkidx(p));
      if (n > 1);
         blk{p,1} = 's'; blk{p,2} = n;
         n2 = n*(n+1)/2;
         At{p,1} = sparse(n2,m);
         C{p,1} = sparse(n,n);
      else
         linblkidx(p) = p;
         blk{p,1} = 'l'; blk{p,2} = abs(n);
         At{p,1} = sparse(abs(n),m);
         C{p,1} = sparse(abs(n),1);
      end
   end
   if ~isempty(spblkidxtmp)
      maxnumblk = 200;
      spnumblk = ceil(length(spblkidxtmp)/maxnumblk);
      for q = 1:spnumblk
         if (q < spnumblk)
            spblkidxall{q} = spblkidxtmp([(q-1)*maxnumblk+1: q*maxnumblk]);
         else
            spblkidxall{q} = spblkidxtmp([(q-1)*maxnumblk+1: length(spblkidxtmp)]);
         end
         tmp = blksize(spblkidxall{q});
         blk{denumblk+q,1} = 's';
         blk{denumblk+q,2} = tmp;
         n2 = sum(tmp.*(tmp+1))/2;
         At{denumblk+q,1} = sparse(n2,m);
         C{denumblk+q,1} = sparse(sum(tmp),sum(tmp));
      end
   else
      spnumblk = 0;
   end
   linblkidx(denumblk+[1:spnumblk]) = zeros(1,spnumblk);
%%
%% Construct single blocks of A,C
%%
   len = length(datavec);
   Y = reshape(datavec(idxstrt+1:len),5,(len-idxstrt)/5)';
   clear datavec;
   Y = sortrows(Y,[1 2]);
   matidx = [0; find(diff(Y(:,1)) ~= 0); size(Y,1)];
%%
   for k = 1:length(matidx)-1
      idx = [matidx(k)+1 : matidx(k+1)];
      Ytmp  = Y(idx,1:5);
      matno = Ytmp(1,1);
      Ytmp2 = Ytmp(:,2);
      for p = 1:denumblk
         n  = blksize(deblkidx(p));
         idx = find(Ytmp2 == deblkidx(p));
         ii = Ytmp(idx,3); jj = Ytmp(idx,4); vv =Ytmp(idx,5);
         len = length(idx);
         if (n > 1)
            idxtmp = find(ii > jj);
            if ~isempty(idxtmp);
               tmp = jj(idxtmp);
               jj(idxtmp) = ii(idxtmp); ii(idxtmp) = tmp;
            end
            tmp = -sparse(ii,jj,vv,n,n);
            tmp = tmp + triu(tmp,1)';
         else
            tmp = -sparse(ii,ones(len,1),vv,abs(n),1);
         end
         if (matno == 0)
            C{p,1} = tmp;
         else
            if (n > 1)
               At{p,1}(:,matno) = svec(blk(p,:),tmp,1);
            else
               At{p,1}(:,matno) = tmp;
            end
         end
      end
   end
%%
%% Construct big sparse block of A,C
%%
if (spnumblk > 0)
   Y1 = Y(:,1);
   diffY1 = find(diff([-1; Y1; inf]));
   for kk = 1:length(diffY1)-1
      idx = [diffY1(kk) : diffY1(kk+1)-1];
      matno = Y1(diffY1(kk));
      Ytmp  = Y(idx,1:5);
      Ytmp2 = Ytmp(:,2);
      maxYtmp2  = Ytmp2(length(Ytmp2));
      minYtmp2  = Ytmp2(1);
      diffYtmp2 = Ytmp2(find(diff([-1; Ytmp2])));
      for q = 1:spnumblk
         spblkidx    = spblkidxall{q};
         maxspblkidx = spblkidx(length(spblkidx));
         minspblkidx = spblkidx(1);
         count = 0;
         if (minYtmp2 <= maxspblkidx) & (maxYtmp2 >= minspblkidx)
            tmpblksize = blksize(spblkidx);
            n = sum(tmpblksize);
            cumspblksize = [0 cumsum(tmpblksize)];
            n2 = sum(tmpblksize.*(tmpblksize+1))/2;
            idx = zeros(n2,1); offset = zeros(n2,1);
            for t = [1:length(diffYtmp2)]
  	       p = find(spblkidx == diffYtmp2(t));
               if ~isempty(p)
	          idxtmp = find(Ytmp2 == spblkidx(p));
                  len = length(idxtmp);
     	          idx(count+[1:len]) = idxtmp;
                  offset(count+[1:len]) = cumspblksize(p);
                  count = count + len;
               end
            end
            idx = idx(1:count); offset = offset(1:count);
            ii = Ytmp(idx,3)+offset; jj = Ytmp(idx,4)+offset; vv = Ytmp(idx,5);
            idxtmp = find(ii > jj);
            if ~isempty(idxtmp);
               tmp = jj(idxtmp);
               jj(idxtmp) = ii(idxtmp); ii(idxtmp) = tmp;
            end
	    idxeq = find(ii==jj);
            tmp = spconvert([ii jj -vv; jj ii -vv; n n 0]) ...
                  + spconvert([ii(idxeq) jj(idxeq) vv(idxeq); n n 0]);
            if (matno == 0)
               C{denumblk+q,1} = tmp;
            else
               At{denumblk+q,1}(:,matno) = svec(blk(denumblk+q,:),tmp,1);
            end
         end
      end
   end
end
%%
%% put all linear blocks together as a single linear block
%%
   idx = find(linblkidx);
   if (length(idx) > 1)
      sdpidx = find(linblkidx==0);
      blktmp = 0; Atmp = []; Ctmp = [];
      for k = 1:length(idx)
          tmp = linblkidx(idx(k));
          blktmp = blktmp+blk{tmp,2};
          Atmp = [Atmp; At{tmp}];
          Ctmp = [Ctmp; C{tmp}];
      end
      At = At(sdpidx); C = C(sdpidx); blk = blk(sdpidx,:);
      len = length(sdpidx);
      blk(2:len+1,:) = blk;
      blk{1,1} = 'l'; blk{1,2} = blktmp;
      At(2:len+1,1) = At; C(2:len+1,1) = C;
      At{1,1} = Atmp; C{1,1} = Ctmp;
   end
%%******************************************************************


% DeJong_f3.m
% De Jong's f3 function, ND, also called STEP
% from: http://www.cs.rpi.edu/~hornda/pres/node4.html
%
% f(x) = sum( floor(x) )
%
% x = N element row vector containing [ x0, x1,..., xN ]
%   each row is processed independently,
%   you can feed in matrices of timeXN no prob
%
% example: cost = DeJong_f3([1,2;3,4;5,6])
% note minimum @ x= -Inf or whatever lower bound you've chosen

% Brian Birge
% Rev 1.0
% 9/12/04

function [out]=DeJong_f3(in)
 out = sum( floor(in) , 2);


function [stat, cfg] = ft_statistics_montecarlo(cfg, dat, design, varargin)

% FT_STATISTICS_MONTECARLO performs a nonparametric statistical test by calculating
% Monte-Carlo estimates of the significance probabilities and/or critical values from
% the permutation distribution. This function should not be called directly, instead
% you should call the function that is associated with the type of data on which you
% want to perform the test.
%
% Use as
%   stat = ft_timelockstatistics(cfg, data1, data2, data3, ...)
%   stat = ft_freqstatistics    (cfg, data1, data2, data3, ...)
%   stat = ft_sourcestatistics  (cfg, data1, data2, data3, ...)
%
% where the data is obtained from FT_TIMELOCKANALYSIS, FT_FREQANALYSIS or
% FT_SOURCEANALYSIS respectively, or from FT_TIMELOCKGRANDAVERAGE,
% FT_FREQGRANDAVERAGE or FT_SOURCEGRANDAVERAGE respectively
% and with cfg.method = 'montecarlo'
%
% The configuration options that can be specified are:
%   cfg.numrandomization = number of randomizations, can be 'all'
%   cfg.correctm         = string, apply multiple-comparison correction, 'no', 'max', cluster', 'tfce', 'bonferroni', 'holm', 'hochberg', 'fdr' (default = 'no')
%   cfg.alpha            = number, critical value for rejecting the null-hypothesis per tail (default = 0.05)
%   cfg.tail             = number, -1, 1 or 0 (default = 0)
%   cfg.correcttail      = string, correct p-values or alpha-values when doing a two-sided test, 'alpha','prob' or 'no' (default = 'no')
%   cfg.ivar             = number or list with indices, independent variable(s)
%   cfg.uvar             = number or list with indices, unit variable(s)
%   cfg.wvar             = number or list with indices, within-cell variable(s)
%   cfg.cvar             = number or list with indices, control variable(s)
%   cfg.feedback         = string, 'gui', 'text', 'textbar' or 'no' (default = 'text')
%   cfg.randomseed       = string, 'yes', 'no' or a number (default = 'yes')
%
% If you use a cluster-based statistic, you can specify the following options that
% determine how the single-sample or single-voxel statistics will be thresholded and
% combined into one statistical value per cluster.
%   cfg.clusterstatistic = how to combine the single samples that belong to a cluster, 'maxsum', 'maxsize', 'wcm' (default = 'maxsum')
%                          the option 'wcm' refers to 'weighted cluster mass', a statistic that combines cluster size and intensity;
%                          see Hayasaka & Nichols (2004) NeuroImage for details
%   cfg.clusterthreshold = method for single-sample threshold, 'parametric', 'nonparametric_individual', 'nonparametric_common' (default = 'parametric')
%   cfg.clusteralpha     = for either parametric or nonparametric thresholding per tail (default = 0.05)
%   cfg.clustercritval   = for parametric thresholding (default is determined by the statfun)
%   cfg.clustertail      = -1, 1 or 0 (default = 0)
%
% To include the channel dimension for clustering of channel level data, you should specify
%   cfg.neighbours       = neighbourhood structure, see FT_PREPARE_NEIGHBOURS
% If you specify an empty neighbourhood structure, clustering will only be done
% over frequency and/or time and not over neighbouring channels.
%
% The statistic that is computed for each sample in each random reshuffling
% of the data is specified as
%   cfg.statistic       = 'indepsamplesT'           independent samples T-statistic,
%                         'indepsamplesF'           independent samples F-statistic,
%                         'indepsamplesregrT'       independent samples regression coefficient T-statistic,
%                         'indepsamplesZcoh'        independent samples Z-statistic for coherence,
%                         'depsamplesT'             dependent samples T-statistic,
%                         'depsamplesFmultivariate' dependent samples F-statistic MANOVA,
%                         'depsamplesregrT'         dependent samples regression coefficient T-statistic,
%                         'actvsblT'                activation versus baseline T-statistic.
% or you can specify your own low-level statistical function.
%
% You can also use a custom statistic of your choice that is sensitive to the
% expected effect in the data. You can implement the statistic in a "statfun" that
% will be called for each randomization. The requirements on a custom statistical
% function is that the function is called ft_statfun_xxx, and that the function returns
% a structure with a "stat" field containing the single sample statistical values.
% Have a look at the functions in the fieldtrip/statfun directory (e.g.
% FT_STATFUN_INDEPSAMPLEST) for the correct format of the input and output.
%
% See also FT_TIMELOCKSTATISTICS, FT_FREQSTATISTICS, FT_SOURCESTATISTICS,
% FT_STATISTICS_ANALYTIC, FT_STATISTICS_STATS, FT_STATISTICS_MVPA,
% FT_STATISTICS_CROSSVALIDATE

% Undocumented local options:
%   cfg.resampling       permutation, bootstrap
%   cfg.computecritval   yes|no, for the statfun
%   cfg.computestat      yes|no, for the statfun
%   cfg.computeprob      yes|no, for the statfun
%   cfg.voxelstatistic   deprecated
%   cfg.voxelthreshold   deprecated
%   cfg.precondition     before|after|[], for the statfun

% Copyright (C) 2005-2015, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% do a sanity check on the input data
assert(isnumeric(dat),    'this function requires numeric data as input, you probably want to use FT_TIMELOCKSTATISTICS, FT_FREQSTATISTICS or FT_SOURCESTATISTICS instead');
assert(isnumeric(design), 'this function requires numeric data as input, you probably want to use FT_TIMELOCKSTATISTICS, FT_FREQSTATISTICS or FT_SOURCESTATISTICS instead');

% check if the input cfg is valid for this function
cfg = ft_checkconfig(cfg, 'renamed',     {'factor',           'ivar'});
cfg = ft_checkconfig(cfg, 'renamed',     {'unitfactor',       'uvar'});
cfg = ft_checkconfig(cfg, 'renamed',     {'repeatedmeasures', 'uvar'});
cfg = ft_checkconfig(cfg, 'renamedval',  {'clusterthreshold', 'nonparametric', 'nonparametric_individual'});
cfg = ft_checkconfig(cfg, 'renamedval',  {'correctm', 'yes', 'max'});
cfg = ft_checkconfig(cfg, 'renamedval',  {'correctm', 'none', 'no'});
cfg = ft_checkconfig(cfg, 'renamedval',  {'correctm', 'bonferoni', 'bonferroni'});
cfg = ft_checkconfig(cfg, 'renamedval',  {'correctm', 'holms', 'holm'});
cfg = ft_checkconfig(cfg, 'required',    {'statistic'});
cfg = ft_checkconfig(cfg, 'forbidden',   {'ztransform', 'removemarginalmeans', 'randomfactor', 'voxelthreshold', 'voxelstatistic'});
cfg = ft_checkconfig(cfg, 'renamedval',  {'statfun', 'depsamplesF', 'ft_statfun_depsamplesFmultivariate'});
cfg = ft_checkconfig(cfg, 'renamedval',  {'statfun', 'ft_statfun_depsamplesF', 'ft_statfun_depsamplesFmultivariate'});

% set the defaults for the main function
cfg.alpha        = ft_getopt(cfg, 'alpha',      0.05);
cfg.tail         = ft_getopt(cfg, 'tail',       0);
cfg.correctm     = ft_getopt(cfg, 'correctm',   'no');
cfg.resampling   = ft_getopt(cfg, 'resampling', 'permutation');
cfg.feedback     = ft_getopt(cfg, 'feedback',   'text');
cfg.ivar         = ft_getopt(cfg, 'ivar',       'all');
cfg.uvar         = ft_getopt(cfg, 'uvar',       []);
cfg.cvar         = ft_getopt(cfg, 'cvar',       []);
cfg.wvar         = ft_getopt(cfg, 'wvar',       []);
cfg.correcttail  = ft_getopt(cfg, 'correcttail',  'no');
cfg.precondition = ft_getopt(cfg, 'precondition', []);

% explicit check for option 'yes' in cfg.correctail.
if strcmp(cfg.correcttail, 'yes')
  ft_error('cfg.correcttail = ''yes'' is not allowed, use either ''prob'', ''alpha'' or ''no''')
end

if strcmp(cfg.correctm, 'tfce')
  % TODO this could require some better defaults
  cfg.connectivity = ft_getopt(cfg, 'connectivity', []);
  cfg.tfce_h0      = ft_getopt(cfg, 'tfce_h0', 0);
  cfg.tfce_H       = ft_getopt(cfg, 'tfce_H',  2);
  cfg.tfce_E       = ft_getopt(cfg, 'tfce_E',  0.5);
  cfg.tfce_nsteps  = ft_getopt(cfg, 'tfce_nsteps', 100);
else
  % these options only apply to tfce, to ensure appropriate configs they are forbidden when _not_ clustering
  cfg = ft_checkconfig(cfg, 'unused', {'tfce_h0', 'tfce_H', 'tfce_E', 'tfce_nsteps'});
end

if strcmp(cfg.correctm, 'cluster')
  % set the defaults for clustering
  cfg.clusterstatistic = ft_getopt(cfg, 'clusterstatistic', 'maxsum');
  cfg.clusterthreshold = ft_getopt(cfg, 'clusterthreshold', 'parametric');
  cfg.clusteralpha     = ft_getopt(cfg, 'clusteralpha',     0.05);
  cfg.clustercritval   = ft_getopt(cfg, 'clustercritval',   []);
  cfg.clustertail      = ft_getopt(cfg, 'clustertail',      cfg.tail);
  cfg.connectivity     = ft_getopt(cfg, 'connectivity',     []); % the default is dealt with below
else
  % these options only apply to clustering, to ensure appropriate configs they are forbidden when _not_ clustering
  cfg = ft_checkconfig(cfg, 'unused', {'clusterstatistic', 'clusteralpha', 'clustercritval', 'clusterthreshold', 'clustertail'});
end

if any(strcmp(cfg.correctm, {'cluster' 'tfce'}))
  % these options might require a spatial neighbourhood matrix

  % deal with the neighbourhood of the channels/triangulation/voxels
  if isempty(cfg.connectivity)
    if isfield(cfg, 'dim') && ~isfield(cfg, 'channel') && ~isfield(cfg, 'tri')
      % input data can be reshaped into a 3D volume, use bwlabeln/spm_bwlabel rather than clusterstat
      ft_info('using connectivity of voxels in 3-D volume\n');
      cfg.connectivity = nan;
    elseif isfield(cfg, 'tri')
      % input data describes a surface along which neighbours can be defined
      ft_info('using connectivity of vertices along triangulated surface\n');
      cfg.connectivity = triangle2connectivity(cfg.tri);
      if isfield(cfg, 'insideorig')
        cfg.connectivity = cfg.connectivity(cfg.insideorig, cfg.insideorig);
      end
    elseif isfield(cfg, 'avgoverchan') && istrue(cfg.avgoverchan)
      % channel dimension has been averaged across, no sense in clustering across space
      cfg.connectivity = true(1);
    elseif isfield(cfg, 'channel')
      cfg.neighbours   = ft_getopt(cfg, 'neighbours', []);
      cfg.connectivity = channelconnectivity(cfg);
    else
      % there is no connectivity in the spatial dimension
      cfg.connectivity = false(size(dat,1));
    end
  else
    % use the specified connectivity: this is not fully robust because
    % there is no guarantee that the order of the spatial elements in the
    % data is the same as the order of the spatial elements in the
    % adjacency matrix
  end
end

% for backward compatibility and other warnings relating correcttail
if isfield(cfg,'correctp') && strcmp(cfg.correctp,'yes')
  ft_warning('cfg.correctp has been renamed to cfg.correcttail and the options have been changed')
  disp('setting cfg.correcttail to ''prob''')
  cfg.correcttail = 'prob';
  cfg = rmfield(cfg,'correctp');
elseif isfield(cfg,'correctp') && strcmp(cfg.correctp,'no')
  cfg = ft_checkconfig(cfg, 'renamed', {'correctp', 'correcttail'});
end
if strcmp(cfg.correcttail,'no') && cfg.tail==0 && cfg.alpha==0.05
  ft_warning('Doing a two-sided test without correcting p-values or alpha-level, p-values and alpha-level will reflect one-sided tests per tail. See http://bit.ly/2YQ1Hm8')
end

% for backward compatibility
if size(design,2)~=size(dat,2)
  design = transpose(design);
end

if ischar(cfg.ivar) && strcmp(cfg.ivar, 'all')
  cfg.ivar = 1:size(design,1);
end

% fetch function handle to the low-level statistics function
statfun = ft_getuserfun(cfg.statistic, 'statfun');
if isempty(statfun)
  ft_error('could not locate the appropriate statistics function');
else
  ft_info('using "%s" for the single-sample statistics\n', func2str(statfun));
end

% construct the resampled design matrix or data-shuffling matrix
ft_info('constructing randomized design\n');
resample = resampledesign(cfg, design);
Nrand = size(resample,1);

% most of the statfuns result in this warning, which is not interesting
ws = ft_warning('off', 'MATLAB:warn_r14_stucture_assignment');

if strcmp(cfg.correctm, 'cluster')
  % determine the critical value for cluster thresholding
  if strcmp(cfg.clusterthreshold, 'nonparametric_individual') || strcmp(cfg.clusterthreshold, 'nonparametric_common')
    ft_info('using a nonparametric threshold for clustering\n');
    cfg.clustercritval = [];  % this will be determined later
  elseif strcmp(cfg.clusterthreshold, 'parametric') && isempty(cfg.clustercritval)
    ft_info('computing a parametric threshold for clustering\n');
    tmpcfg = cfg; % the next line does not pass on non-standard options that a statfun might use
    % tmpcfg = keepfields(cfg, {'dim' 'dimord' 'clusteralpha' 'clustertail' 'ivar' 'uvar' 'cvar' 'wvar' 'contrastcoefs'});
    tmpcfg.computecritval = 'yes';  % explicitly request the computation of the crtitical value
    tmpcfg.computestat    = 'no';   % skip the computation of the statistic
    tmpcfg.alpha          = cfg.clusteralpha; % the statfun uses cfg.alpha most likely
    try
      cfg.clustercritval    = getfield(statfun(tmpcfg, dat, design), 'critval');
    catch
      disp(lasterr);
      ft_error('could not determine the parametric critical value for clustering');
    end
  elseif strcmp(cfg.clusterthreshold, 'parametric') && ~isempty(cfg.clustercritval)
    ft_info('using the specified parametric threshold for clustering\n');
    cfg.clusteralpha = [];
  end
end

% compute the statistic for the observed data
ft_progress('init', cfg.feedback, 'computing statistic');
% get an estimate of the time required per evaluation of the statfun
time_pre = cputime;

try
  % the nargout function in MATLAB 6.5 and older does not work on function handles
  num = nargout(statfun);
catch
  num = 1;
end

if num==1
  % only the statistic is returned
  [statobs] = statfun(cfg, dat, design);
elseif num==2
  % both the statistic and the (updated) configuration are returned
  [statobs, cfg] = statfun(cfg, dat, design);
elseif num==3
  % both the statistic and the (updated) configuration and the (updated) data are returned
  tmpcfg = cfg;
  if strcmp(cfg. precondition, 'before'), tmpcfg.preconditionflag = 1; end
  [statobs, tmpcfg, dat]  = statfun(tmpcfg, dat, design);
  tmpcfg.preconditionflag = 0;
  cfg = tmpcfg;
end

if isstruct(statobs)
  % remember all details for later reference, continue to work with the statistic
  statfull = statobs;
  statobs  = statobs.stat;
end

% remember the statistic for later reference, continue to work with the statistic
statfull.stat = statobs;

time_eval = cputime - time_pre;
ft_info('estimated time per randomization is %.2f seconds\n', time_eval);

% pre-allocate some memory
if strcmp(cfg.correctm, 'cluster')
  statrand = zeros(size(statobs,1), size(resample,1), class(dat)); % this reduces the memory footprint, requires the user to use ft_struct2single on the input data
else
  prb_pos   = zeros(size(statobs));
  prb_neg   = zeros(size(statobs));
end

if strcmp(cfg.precondition, 'after')
  tmpcfg = cfg;
  tmpcfg.preconditionflag = 1;
  [tmpstat, tmpcfg, dat] = statfun(tmpcfg, dat, design);
end

if any(strcmp(cfg.correctm, {'tfce' 'max'}))
  % pre-allocate the memory to hold the distribution of most extreme positive (right) and negative (left) statistical values
  posdistribution = nan(1,Nrand);
  negdistribution = nan(1,Nrand);
end

% compute the statistic for the randomized data and count the outliers
for i = 1:Nrand
  ft_progress(i/Nrand, 'computing statistic %d from %d\n', i, Nrand);
  if strcmp(cfg.resampling, 'permutation')
    tmpdesign = design(:,resample(i,:));     % the columns in the design matrix are reshufled by means of permutation
    tmpdat    = dat;                        % the data itself is not shuffled
    if size(tmpdesign,1)==size(tmpdat,2)
      tmpdesign = transpose(tmpdesign);
    end
  elseif strcmp(cfg.resampling, 'bootstrap')
    tmpdesign = design;                     % the design matrix is not shuffled
    tmpdat    = dat(:,resample(i,:));        % the columns of the data are resampled by means of bootstrapping
  end
  if any(strcmp(cfg.correctm, {'cluster' 'tfce'}))
    % keep each randomization in memory for cluster postprocessing
    dum = statfun(cfg, tmpdat, tmpdesign);
    if isstruct(dum)
      statrand(:,i) = dum.stat;
    else
      statrand(:,i) = dum;
    end
  else
    % do not keep each randomization in memory, but process them on the fly
    statrand = statfun(cfg, tmpdat, tmpdesign);
    if isstruct(statrand)
      statrand = statrand.stat;
    end

    % the following line is for debugging
    % stat.statkeep(:,i) = statrand;
    if strcmp(cfg.correctm, 'max')
      % compare each data element with the maximum statistic
      prb_pos = prb_pos + (statobs<max(statrand(:)));
      prb_neg = prb_neg + (statobs>min(statrand(:)));
      posdistribution(i) = max(statrand(:));
      negdistribution(i) = min(statrand(:));
    else
      % compare each data element with its own statistic
      prb_pos = prb_pos + (statobs<statrand);
      prb_neg = prb_neg + (statobs>statrand);
    end
  end
end
ft_progress('close');

if strcmp(cfg.correctm, 'cluster')
  % do the cluster postprocessing
  [stat, cfg] = clusterstat(cfg, statrand, statobs);
elseif strcmp(cfg.correctm, 'tfce')
  [stat, cfg] = tfcestat(cfg, statrand, statobs);
else
  if ~isequal(cfg.numrandomization, 'all')
    % in case of random permutations (i.e., montecarlo sample, and NOT full
    % permutation), the minimum p-value should not be 0, but 1/N
    prb_pos = prb_pos + 1;
    prb_neg = prb_neg + 1;
    Nrand = Nrand + 1;
  end
  switch cfg.tail
    case 1
      clear prb_neg  % not needed any more, free some memory
      stat.prob = prb_pos./Nrand;
    case -1
      clear prb_pos  % not needed any more, free some memory
      stat.prob = prb_neg./Nrand;
    case 0
      % for each observation select the tail that corresponds with the lowest probability
      prb_neg = prb_neg./Nrand;
      prb_pos = prb_pos./Nrand;
      stat.prob = min(prb_neg, prb_pos); % this is the probability for the most unlikely tail
  end
end

% In case of a two tailed test, the type-I error rate (alpha) refers to
% both tails of the distribution, whereas the stat.prob value computed sofar
% corresponds with one tail, i.e. the probability, under the assumption of
% no effect or no difference (the null hypothesis), of obtaining a result
% equal to or more extreme than what was actually observed. The decision
% rule whether the null-hopothesis should be rejected given the observed
% probability therefore should consider alpha divided by two, to correspond
% with the probability in one of the tails (the most extreme tail). This
% is conceptually equivalent to performing a Bonferroni correction for the
% two tails.
%
% An alternative solution to distribute the alpha level over both tails is
% achieved by multiplying the probability with a factor of two, prior to
% thresholding it wich cfg.alpha.  The advantage of this solution is that
% it results in a p-value that corresponds with a parametric probability.
% Below both options are realized
if strcmp(cfg.correcttail, 'prob') && cfg.tail==0
  stat.prob = stat.prob .* 2;
  stat.prob(stat.prob>1) = 1; % clip at p=1
  % also correct the probabilities in the pos/negcluster fields
  if isfield(stat, 'posclusters')
    for i=1:length(stat.posclusters)
      stat.posclusters(i).prob = stat.posclusters(i).prob*2;
      if stat.posclusters(i).prob>1; stat.posclusters(i).prob = 1; end
    end
  end
  if isfield(stat, 'negclusters')
    for i=1:length(stat.negclusters)
      stat.negclusters(i).prob = stat.negclusters(i).prob*2;
      if stat.negclusters(i).prob>1; stat.negclusters(i).prob = 1; end
    end
  end
elseif strcmp(cfg.correcttail, 'alpha') && cfg.tail==0
  cfg.alpha = cfg.alpha / 2;
end

% compute range of confidence interval p ? 1.96(sqrt(var(p))), with var(p) = var(x/n) = p*(1-p)/N
stddev = sqrt(stat.prob.*(1-stat.prob)/Nrand);
stat.cirange = 1.96*stddev;

if isfield(stat, 'posclusters')
  for i=1:length(stat.posclusters)
    stat.posclusters(i).stddev  = sqrt(stat.posclusters(i).prob.*(1-stat.posclusters(i).prob)/Nrand);
    stat.posclusters(i).cirange =  1.96*stat.posclusters(i).stddev;
    if i==1 && stat.posclusters(i).prob<cfg.alpha && stat.posclusters(i).prob+stat.posclusters(i).cirange>=cfg.alpha
      ft_warning('FieldTrip:posCluster_exceeds_alpha', sprintf('The p-value confidence interval of positive cluster #%i includes %.3f - consider increasing the number of permutations!', i, cfg.alpha));
    end
  end
end
if isfield(stat, 'negclusters')
  for i=1:length(stat.negclusters)
    stat.negclusters(i).stddev  = sqrt(stat.negclusters(i).prob.*(1-stat.negclusters(i).prob)/Nrand);
    stat.negclusters(i).cirange =  1.96*stat.negclusters(i).stddev;
    if i==1 && stat.negclusters(i).prob<cfg.alpha && stat.negclusters(i).prob+stat.negclusters(i).cirange>=cfg.alpha
      ft_warning('FieldTrip:negCluster_exceeds_alpha', sprintf('The p-value confidence interval of negative cluster #%i includes %.3f - consider increasing the number of permutations!', i, cfg.alpha));
    end
  end
end

if ~isfield(stat, 'prob')
  ft_warning('probability was not computed');
else
  switch lower(cfg.correctm)
    case 'max'
      % the correction is implicit in the method
      ft_notice('using a maximum-statistic based method for multiple comparison correction\n');
      ft_notice('the returned probabilities and the thresholded mask are corrected for multiple comparisons\n');
      stat.mask = stat.prob<=cfg.alpha;
      stat.posdistribution = posdistribution;
      stat.negdistribution = negdistribution;
    case 'tfce'
      ft_notice('using a threshold free cluster enhancement based method for multiple comparison correction\n');
      ft_notice('the returned probabilities and the thresholded mask are corrected for multiple comparisons\n');
      stat.mask = stat.prob<=cfg.alpha;
    case 'cluster'
      % the correction is implicit in the method
      ft_notice('using a cluster-based method for multiple comparison correction\n');
      ft_notice('the returned probabilities and the thresholded mask are corrected for multiple comparisons\n');
      stat.mask = stat.prob<=cfg.alpha;
    case 'bonferroni'
      ft_notice('performing Bonferroni correction for multiple comparisons\n');
      ft_notice('the returned probabilities are uncorrected, the thresholded mask is corrected\n');
      stat.mask = stat.prob<=(cfg.alpha ./ numel(stat.prob));
    case 'holm'
      % test the most significatt significance probability against alpha/N, the second largest against alpha/(N-1), etc.
      ft_notice('performing Holm-Bonferroni correction for multiple comparisons\n');
      ft_notice('the returned probabilities are uncorrected, the thresholded mask is corrected\n');
      [pvals, indx] = sort(stat.prob(:));                                   % this sorts the significance probabilities from smallest to largest
      k = find(pvals > (cfg.alpha ./ ((length(pvals):-1:1)')), 1, 'first'); % compare each significance probability against its individual threshold
      mask = (1:length(pvals))'<k;
      stat.mask = zeros(size(stat.prob));
      stat.mask(indx) = mask;
    case 'hochberg'
      % test the most significant significance probability against alpha/N, the second largest against alpha/(N-1), etc.
      ft_notice('performing Hochberg''s correction for multiple comparisons (this is *not* the Benjamini-Hochberg FDR procedure!)\n');
      ft_notice('the returned probabilities are uncorrected, the thresholded mask is corrected\n');
      [pvals, indx] = sort(stat.prob(:));                     % this sorts the significance probabilities from smallest to largest
      k = find(pvals <= (cfg.alpha ./ ((length(pvals):-1:1)')), 1, 'last'); % compare each significance probability against its individual threshold
      mask = (1:length(pvals))'<=k;
      stat.mask = zeros(size(stat.prob));
      stat.mask(indx) = mask;
    case 'fdr'
      ft_notice('performing FDR correction for multiple comparisons\n');
      ft_notice('the returned probabilities are uncorrected, the thresholded mask is corrected\n');
      stat.mask = fdr(stat.prob, cfg.alpha);
    otherwise
      ft_notice('not performing a correction for multiple comparisons\n');
      stat.mask = stat.prob<=cfg.alpha;
  end
end

% return the observed statistic
if ~isfield(stat, 'stat')
  stat.stat = statobs;
end

if exist('statrand', 'var')
  stat.ref = mean(statrand,2);
end

% return optional other details that were returned by the statfun
stat = copyfields(statfull, stat, fieldnames(statfull));

ft_warning(ws); % revert to original state