function [beta, t, pvals, convals, con_t, con_pvals, sigma, Phi, df, stebeta, conste, F] = fit_gls(y, X, c, p, varargin)
% Fit a linear model using generalized least squares and an AR(p) model
%
% :Usage:
% ::
%
%     [beta, t, pvals, convals, con_t, con_pvals, sigma, Phi, df, stebeta, conste, F] = fit_gls(y,X,c,p,[PX, equal to pinv(X), for speed], [Weights])
%
% This program uses the Cochrane-Orcutt algorithm to iteratively find the
% GLS solution and estimate the noise parameters.
%
% Step 1: Find the OLS solution.
%
% Step 2: Use residuals from the previous fit to estimate the parameters in
% the AR(p) noise model.
%
% Step 3: Find the GLS solution using the covariance matrix corresponding
% to an AR(p) model with parameters estimated in Step 2 inserted.
%
% Step 4: Repeat steps 2-3 until convergence.
%
% :Inputs:
%
%   **y:**
%        fMRI time course (T x 1 vector)
%
%   **X:**
%        Design matrix (T x param matrix)
%
%   **c:**
%        contrast vector(s) (param x # contrasts matrix)
%
%   **p:**
%        order of AR model.
%
%   **PX:**
%        pinv(X), for speeded, repeated calculations with different y vectors
%
%        Note: if using weights, px = inv(X' * W * X) * X' * W;
%              where W = diag(Weights);
%
%   **Weights:**
%        Optional, vector of weights for each observation
%
%        Empty or missing: Unweighted analysis.
%
% Note that setting p=0 implies a white noise model.
%
% :Output:
%
%   **t:**
%        t-value for the contrast c'beta
%
%   **df:**
%        degrees of freedom using Satterthwaite approximation
%
%   **beta:**
%        beta vector
%
%   **Phi:**
%        vector of coefficients in AR(p) model
%
%   **sigma:**
%        standard deviation
%
%   **stebeta:**
%        standard error of betas
%
% ..
%    by Martin Lindquist
%
%    Last updated: 3/29/08, Tor Wager, added weighted least squares
%                        verified that beta and t-values are identical to
%                        glmfit.m in matlab7.5 with ar p = 0
%                        ***AR(p) with weighted least squares needs to be
%                        checked.  behaving reasonably.
%               4/1/08,  Tor : output stats for boht betas and contrasts
%                        Reorganized order of outputs
%               5/15/08  Tor : Weird things happening with single inputs; particulaly with aryule; force
%                         double
% ..

y = double(y);
X = double(X);

T = length(y);      % Length of time course

k = size(X, 2);     % predictors

if length(varargin) > 1
    w = double(varargin{2});        % weights for weighted least squares
else
    w = ones(T, 1);
end
W = diag(w);
sqrtW = sqrt(W);     % for weighted residuals

if ~isempty(varargin) && ~isempty(varargin{1})
    px = double(varargin{1});
else
    invxvx = inv(X' * W * X);   % we can re-use this later if p == 0
    px = invxvx * X' * W;
end

% Step 1: Find the OLS solution
beta = px*y;                                      % Beta values; weighted, if weights are used
resid = sqrtW * y - sqrtW * X * beta;             % Residuals (Weighted, if weights are used)
sigma = sqrt((1 / (T - k)) * resid' * resid);     %sum(resid.^2)));  % Estimate of Sigma

% Weighted residuals: Three equivalent ways
% We pick one that is compatible with AR estimation
% 1)
% beta = px*y;                       % Beta values
% resid = y - X*beta;                     % Residuals
% sigma = sqrt((1 / (T - k)) * resid' * W * resid); %sum(resid.^2)));  % Estimate of Sigma

% 2)
%r2 = sqrt(W) * resid; sigma2 = sqrt((1 / (T - k)) * r2' *  r2)

% 3)
% r2 = sqrt(W) * y - sqrt(W) * X * beta;
% sigma2 = sqrt((1 / (T - k)) * r2' *  r2)

% Stuff needed for future iterations
iV = W;  % for ar p = 0 case
A = W;   % for ar p = 0 case
Phi = 0;
betaold = 0;

% Steps 2-4: Find the GLS solution by iteration
% If p=0, skip this step. Appropriate solution already calculated above.
% Continue iteration until convergence or for at most 10 loops.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Note to Jack and Tor: Keith Worsley uses a similar algorithm when fitting
% a GLM with an AR(p) noise model. However, he skips the iterative step
% and only goes through the loop one time. He claims that this is enough. I
% am not entirely convinced, therefore it is probably better to go through
% a few times if needed.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

i=1;                % Set counter

while i < 10 && p > 0 && (i == 1 || sum((beta - betaold).^2) > 0.001)
    % Do up to 10 iterations, if arp > 0 and either first iteration or
    % there's a difference from last iteration

    % resid = y - X*beta;             % Calculate residuals of current model fit

    resid = sqrtW * y - sqrtW * X * beta;             % Residuals (Weighted, if weights are used)

    % Estimate AR parameters using residuals
    [a,e] = aryule(resid, p);
    Phi =zeros(length(a)-1,1);
    Phi(1:p) = -a(2:(p+1));

    %sigma = sqrt(e); % swrtW*e??  % Moved later, because never used until
    %after iteration loop

    % Find the inverse of the covariance matrix
    A = sqrtW; % ***should be sqrt(W)?

    for j=1:p
        %A = A + diag(-Phi(j)*ones((T-j),1),-j);

        A = A + sqrtW * diag(-Phi(j)*ones((T-j),1),-j);
    end;

    %create_figure('A'); imagesc(A, [-.2 .2]); colorbar, drawnow, input(' ')

    iV = A*A';                      %  New weights, with AR estimates, The inverse of the covariance matrix

    betaold = beta;                 % Set old solution to be betaold
    beta = inv(X'*iV*X)*X'*iV*y;    % Calculate new solution

    i = i+1;                        % Add one to counter

    %create_figure('COV'); imagesc(iV, [-.02 .02]); colorbar, drawnow, input(' ')
end

if p > 0
    % re-calc sigma
    sigma = sqrt(e);

    invxvx = inv(X'*iV*X);

    % Should we use the kind of thing below? Seems like sqrt(e) is unweighted,
    % though it seems reasonable...
    % sqrtiv = sqrt(iV);
    % resid = sqrtiv * y - sqrtiv * X * beta;             % Residuals (Weighted, if weights are used)
    % sigma = sqrt((1 / (T - k)) * resid' * resid)
end

R = (eye(T) - X * invxvx * X' * iV);        % Residual inducing matrix

Wd = R * A * inv(iV) * A';                  % inv(iV) = Covariance matrix
df = (trace(Wd).^2)./trace(Wd*Wd);       % Satterthwaite approximation for degrees of freedom

% Should check on below: this creates difference in t-values from
% glmfit...but why wouldn't we need to re-calculate a (larger) sigma if we have reduced df?
% if df ~= (T - k)                    % Tor added 3/29, have to re-calculate sigma
%     sigma = sqrt((1 / df) * resid' * iV * resid); %sum(resid.^2)));  % Estimate of Sigma
% end

varbeta = sigma^2.* invxvx;            % Var(beta)
stebeta = diag(varbeta).^.5;
t = beta ./ stebeta;

convals = [];
conste = [];
con_t = [];
con_pvals = [];
F = [];
if ~isempty(c)
    % Contrast(s)
    convals = (c' * beta);
    conste = diag(c' * varbeta * c).^.5;                   % tor added as output
    con_t = convals ./ conste;                            %sqrt(c'*varbeta*c);             % t-value
end

% get rid of nuisance regressors that aren't in any contrast
% wh = stebeta > 0;
% c = c(wh,wh);
% beta = beta(wh);
% stebeta = stebeta(wh);
%
% if ~isempty(c)
%     beta = (c' * beta);
%     %beta = beta(wh);
%     t = beta ./ stebeta;   %sqrt(c'*varbeta*c);            % t-value
% else
%     t = beta(wh) ./ stebeta;
% end

%%%%%% Test H0: beta_1 = beta_2 = .... = beta_param = 0
if nargout > 6
    SSE = y'*y - beta'*X'*y;                 % Error sum of squares
    mSSE = SSE/df;
    J = ones(T);
    SST=y'*y - (1/T).*y'*J*y;           % Total sum of squares
    SSM=SST-SSE;                        % Model sum of squares

    dfSSM=length(c) - 1;                % degrees of freedom for model (param - 1)
    mSSM=SSM/dfSSM;

    F=mSSM/mSSE;         % F-statistic - compare with F-distruibution with (param-1, df) degrees of freedom
end

% % get contrast values if we need those
% if ~isempty(c)
%     beta = (beta' * c)';
% end

if nargout > 2
    pvals = 2 .* (1 - tcdf(abs(t), df)); % two-tailed

    % make sure p-values for valid results are not zero...
    pvals(pvals == 0 & beta ~= 0 & ~isnan(beta)) = 1000*eps;

    if ~isempty(c) && nargout > 5
        con_pvals = 2 .* (1 - tcdf(abs(con_t), df)); % two-tailed

        con_pvals(con_pvals == 0 & convals ~= 0 & ~isnan(convals)) = 1000*eps;
    end

end

return


function [ quasi, seed_new ] = sobol ( dim_num, seed )

%% SOBOL generates a new quasirandom Sobol vector with each call.
%
%  Discussion:
%
%    The routine adapts the ideas of Antonov and Saleev.
%
%  Modified:
%
%    30 March 2003
%
%  Reference:
%
%    Antonov and Saleev,
%    USSR Computational Mathematics and Mathematical Physics,
%    Volume 19, 1980, pages 252 - 256.
%
%    Paul Bratley and Bennett Fox,
%    Algorithm 659:
%    Implementing Sobol's Quasirandom Sequence Generator,
%    ACM Transactions on Mathematical Software,
%    Volume 14, Number 1, pages 88-100, 1988.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, pages 362-376, 1986.
%
%    I Sobol,
%    USSR Computational Mathematics and Mathematical Physics,
%    Volume 16, pages 236-242, 1977.
%
%    I Sobol and Levitan,
%    The Production of Points Uniformly Distributed in a Multidimensional
%    Cube (in Russian),
%    Preprint IPM Akad. Nauk SSSR,
%    Number 40, Moscow 1976.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the number of spatial dimensions.
%    DIM_NUM must satisfy 2 <= DIM_NUM <= 40.
%
%    Input/output, integer SEED, the "seed" for the sequence.
%    This is essentially the index in the sequence of the quasirandom
%    value to be generated.  On output, SEED has been set to the
%    appropriate next value, usually simply SEED+1.
%    If SEED is less than 0 on input, it is treated as though it were 0.
%    An input value of 0 requests the first (0-th) element of the sequence.
%
%    Output, real QUASI(DIM_NUM), the next quasirandom vector.
%
  global SOBOL_lastq;
  global SOBOL_seed;

  dim_max = 40;
%
%  Initialize (part of) V.
%
    v(1:40,1:30) = zeros(40,30);

    v(1:40,1) = [ ...
      1, 1, 1, 1, 1, 1, 1, 1, 1, 1, ...
      1, 1, 1, 1, 1, 1, 1, 1, 1, 1, ...
      1, 1, 1, 1, 1, 1, 1, 1, 1, 1, ...
      1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ]';

    v(3:40,2) = [ ...
            1, 3, 1, 3, 1, 3, 3, 1, ...
      3, 1, 3, 1, 3, 1, 1, 3, 1, 3, ...
      1, 3, 1, 3, 3, 1, 3, 1, 3, 1, ...
      3, 1, 1, 3, 1, 3, 1, 3, 1, 3 ]';

    v(4:40,3) = [ ...
               7, 5, 1, 3, 3, 7, 5, ...
      5, 7, 7, 1, 3, 3, 7, 5, 1, 1, ...
      5, 3, 3, 1, 7, 5, 1, 3, 3, 7, ...
      5, 1, 1, 5, 7, 7, 5, 1, 3, 3 ]';

    v(6:40,4) = [ ...
                     1, 7, 9,13,11, ...
      1, 3, 7, 9, 5,13,13,11, 3,15, ...
      5, 3,15, 7, 9,13, 9, 1,11, 7, ...
      5,15, 1,15,11, 5, 3, 1, 7, 9 ]';

    v(8:40,5) = [ ...
                           9, 3,27, ...
     15,29,21,23,19,11,25, 7,13,17, ...
      1,25,29, 3,31,11, 5,23,27,19, ...
     21, 5, 1,17,13, 7,15, 9,31, 9 ]';

    v(14:40,6) = [ ...
              37,33, 7, 5,11,39,63, ...
     27,17,15,23,29, 3,21,13,31,25, ...
      9,49,33,19,29,11,19,27,15,25 ]';

    v(20:40,7) = [ ...
                                         13, ...
     33,115, 41, 79, 17, 29,119, 75, 73,105, ...
      7, 59, 65, 21,  3,113, 61, 89, 45,107 ]';

    v(38:40,8) = [ ...
                                  7, 23, 39 ]';
%
%  Set POLY.
%
    poly(1:40)= [ ...
        1,   3,   7,  11,  13,  19,  25,  37,  59,  47, ...
       61,  55,  41,  67,  97,  91, 109, 103, 115, 131, ...
      193, 137, 145, 143, 241, 157, 185, 167, 229, 171, ...
      213, 191, 253, 203, 211, 239, 247, 285, 369, 299 ];
%
%  Check parameters.
%
    if ( dim_num < 2 | dim_max < dim_num )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SOBOL - Fatal error!\n' );
      fprintf ( 1, '  The spatial dimension DIM_NUM should satisfy:\n' );
      fprintf ( 1, '    2 <= DIM_NUM <= %d\n', dim_max );
      fprintf ( 1, '  But this input value is DIM_NUM = %d\n', dim_num );
      return
    end

    atmost = 2^30 - 1;
%
%  Find the number of bits in ATMOST.
%
    maxcol = bit_hi1_base_2 ( atmost );
%
%  Initialize row 1 of V.
%
    v(1,1:maxcol) = 1;
%
%  Initialize the remaining rows of V.
%
    for ( i = 2 : dim_num )
%
%  The bit pattern of the integer POLY(I) gives the form
%  of polynomial I.
%
%  Find the degree of polynomial I from binary encoding.
%
      j = poly(i);
      m = 0;

      while ( 1 )

        j = floor ( j / 2 );

        if ( j <= 0 )
          break;
        end

        m = m + 1;

      end
%
%  We expand this bit pattern to separate components of the logical array INCLUD.
%
      j = poly(i);
      for ( k = m : -1 : 1 )
        j2 = floor ( j / 2 );
        includ(k) = ( j ~= 2 * j2 );
        j = j2;
      end
%
%  Calculate the remaining elements of row I as explained
%  in Bratley and Fox, section 2.
%
      for ( j = m + 1 : maxcol )

        newv = v(i,j-m);
        l = 1;

        for ( k = 1 : m )

          l = 2 * l;

          if ( includ(k) )
            newv = exor ( newv, l * v(i,j-k) );
          end

        end

        v(i,j) = newv;

      end

    end
%
%  Multiply columns of V by appropriate power of 2.
%
    l = 1;
    for ( j = maxcol-1 : -1 : 1 )
      l = 2 * l;
      v(1:dim_num,j) = v(1:dim_num,j) * l;
    end
%
%  RECIPD is 1/(common denominator of the elements in V).
%
    recipd = 1.0E+00 / ( 2 * l );

  seed = floor ( seed );

  if ( seed < 0 )
    seed = 0;
  end

  if ( seed == 0 )

    l = 1;
    SOBOL_lastq(1:dim_num) = 0;

  elseif ( seed == SOBOL_seed + 1 )
%
%  Find the position of the right-hand zero in SEED.
%
    l = bit_lo0_base_2 ( seed );

  elseif ( seed <= SOBOL_seed )

    SOBOL_seed = 0;
    l = 1;
    SOBOL_lastq(1:dim_num) = 0;

    for ( seed_temp = SOBOL_seed : seed-1 )

      l = bit_lo0_base_2 ( seed_temp );

      for ( i = 1 : dim_num )
        SOBOL_lastq(i) = exor ( SOBOL_lastq(i), v(i,l) );
      end

    end

    l = bit_lo0_base_2 ( seed );

  elseif ( SOBOL_seed+1 < seed )

    for ( seed_temp = SOBOL_seed+1 : seed-1 )

      l = bit_lo0_base_2 ( seed_temp );

      for ( i = 1 : dim_num )
        SOBOL_lastq(i) = exor ( SOBOL_lastq(i), v(i,l) );
      end

    end

    l = bit_lo0_base_2 ( seed );

  end
%
%  Check that the user is not calling too many times!
%
  if ( maxcol < l )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SOBOL - Fatal error!\n' );
    fprintf ( 1, '  Too many calls!\n' );
    fprintf ( 1, '  MAXCOL = %d\n', maxcol );
    fprintf ( 1, '  L =      %d\n', l );
    return
  end
%
%  Calculate the new components of QUASI.
%
  for ( i = 1 : dim_num )

    quasi(i) = SOBOL_lastq(i) * recipd;

    SOBOL_lastq(i) = exor ( SOBOL_lastq(i), v(i,l) );

  end

  SOBOL_seed = seed;
  seed_new = seed + 1;


function tests = test_ft_preproc_bandpassfilter

% MEM 1gb
% WALLTIME 00:10:00
% DEPENDENCY ft_preproc_bandpassfilter

if nargout
  % assume that this is called by RUNTESTS
  tests = functiontests(localfunctions);
else
  % assume that this is called from the command line
  fn = localfunctions;
  for i=1:numel(fn)
    feval(fn{i});
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function testOptions(testCase)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nchan   = 8;
nsample = 1000;

dat = randn(nchan, nsample) + 1;
Fs  = 1000;
Fbp = [1 35];

instabilityfix  = [];
df              = [];
wintype         = [];
dev             = [];
plotfiltresp    = [];
usefftfilt      = [];

filttypes = {'brickwall', 'firws' 'fir' 'firls' 'but'};
filtdirs  = {'onepass' 'onepass-reverse' 'twopass' 'twopass-reverse' 'twopass-average' 'onepass-zerophase' 'onepass-reverse-zerophase' 'onepass-minphase'};
filtorders = {[] 1 2 4 8};

result = {};
opts   = {};
for i1 = 1:numel(filttypes)
  if isequal(filttypes{i1}, 'brickwall')
    opts{end+1}   =[filttypes{i1}];
    result{end+1} = ft_preproc_bandpassfilter(dat, Fs, Fbp, [], filttypes{i1}, [], instabilityfix, df, wintype, dev, plotfiltresp, usefftfilt);
    continue;
  elseif isequal(filttypes{i1}, 'but')
    sel2 = 1:5;
    sel3 = 2:4; % order 8 may become unstable, and thus requires an instabilityfix
  elseif isequal(filttypes{i1}, 'firws')
    sel2 = find(~contains(filtdirs, 'twopass')); % this leads to a filter that is not stable
    sel3 = 1; % the low number orders do not make sense at all for the finite impulse filters.
  else
    sel2 = 1:numel(filtdirs);
    sel3 = 1;
  end
  for i2 = sel2
    for i3 = sel3
      tmp = filtorders{i3};
      if isempty(tmp)
        tmp = 'defaultorder';
      elseif isnumeric(tmp)
        tmp = sprintf('order %d', tmp);
      end
      opts{end+1}   = sprintf('%s_%s_%s', filttypes{i1}, filtdirs{i2}, tmp);
      result{end+1} = ft_preproc_bandpassfilter(dat, Fs, Fbp, filtorders{i3}, filttypes{i1}, filtdirs{i2}, instabilityfix, df, wintype, dev, plotfiltresp, usefftfilt);
    end
  end
end

% result{end+1} = ft_preproc_bandpassfilter(dat, Fs, Fbp, 8, 'but'      , 'onepass'                  , 'split', [], [], [], [], []); % this one is instable
% result{end+1} = ft_preproc_bandpassfilter(dat, Fs, Fbp, 8, 'but'      , 'onepass-reverse'          , 'split', [], [], [], [], []);
% result{end+1} = ft_preproc_bandpassfilter(dat, Fs, Fbp, 8, 'but'      , 'twopass'                  , 'split', [], [], [], [], []);
% result{end+1} = ft_preproc_bandpassfilter(dat, Fs, Fbp, 8, 'but'      , 'twopass-reverse'          , 'split', [], [], [], [], []);
% result{end+1} = ft_preproc_bandpassfilter(dat, Fs, Fbp, 8, 'but'      , 'twopass-average'          , 'split', [], [], [], [], []);

% all iterations were done with (slightly) different options, hence the results should not be equal
for i=1:numel(result)
  for j=(i+1):numel(result)
    assert(~isequal(result{i}, result{j}), 'the results %d and %d should not be equal', i, j);
  end
end

for i=1:size(result{1},1)
  for k=1:numel(result)
    for m=(k+1):numel(result)
      b(k,m) = result{k}(i,:)/result{m}(i,:);
      b(m,k) = result{m}(i,:)/result{k}(i,:);
    end
    B(:,:,i) = b;
  end
end
n = numel(result);
figure;imagesc((mean(B,3)+mean(B,3)')/2);
set(gca, 'xtick', 1:n, 'ytick', 1:n, 'xticklabel', opts', 'yticklabel', opts', 'ticklabelinterpreter', 'none');
set(gcf, 'position', [230 47 993 750]);
colorbar
axis equal;axis tight


function [h,t,dat,d,m1,m2,sterr] = tor_fill_steplot(dat,color,varargin)
% :Usage:
% ::
%
%    [h,t] = tor_fill_steplot(dat,color,[robust flag],[p-thresh],[x vector],[covs no interest])
%
% Plots a mean vector (mean of each column of dat)
% surrounded by a fill with standard err bars
%
% If dat has 3 dimensions, then
% the diff between dat(:,:,1) and dat(:,:,2) is
% used as the difference for computing standard err
% (as in repeated measures)
%
% if behavior is entered as optional argument, removes it before plotting
% lines.  Also returns adjusted output in d, dat
%
% Optional: robust flag (1/0), robust IRLS
%
% :Examples:
% ::
%
%    tor_fig;
%    tor_fill_steplot(dat,{'b' 'r'},0,.05,secs);

dorobust = 0; pthresh = 0; x = 1:size(dat,2);
t = []; m1 = []; m2 = []; X = [];

if length(varargin) > 0, dorobust = varargin{1};,end
if length(varargin) > 1, pthresh = varargin{2};,end
if length(varargin) > 2, x = varargin{3};,end   % small x, time
if length(varargin) > 3, X = varargin{4};,end   % big X, model matrix of covs

if isempty(x), x = 1:size(dat,2);, end

if length(size(dat)) > 2

    % remove covariates of no interest, if any
    % center covs and fit without intercept to preserve mean in data
    if ~isempty(X)
        disp('Adjusting for covariates.');
        X = scale(X,1); % center
        W = X * pinv(X);
        y = squeeze(dat(:,:,1));
        dat(:,:,1) = y - W * y;

        y = squeeze(dat(:,:,2));
        dat(:,:,2) = y - W * y;
    end

    d = dat(:,:,1) - dat(:,:,2);

    if dorobust
        [m1] = robust_mean(dat(:,:,1));
        [m2] = robust_mean(dat(:,:,2));
        [dummy,t,p,sterr] = robust_mean(d);
    else

        m1 = nanmean(dat(:,:,1));
        m2 = nanmean(dat(:,:,2));
        md = nanmean(d);
        sterr = ste(d);
        t = md ./ sterr;
        [h,p,ci,stat] = ttest(d);
    end

    if ~iscell(color), error('For two groups, color should be cell, e.g.,  {''r'' ''b''})');,end

    hold on;
    h(1) = plot(x,m1,color{1},'LineWidth',2);
    h(2) = plot(x,m2,color{2},'LineWidth',2);

    drawnow

    fill_around_line(m1,sterr,color{1},x);
    fill_around_line(m2,sterr,color{2},x);

    drawnow

    if pthresh
        % significance markers at top of plot.
        yval = max([m1 m2]) + .04 * max([m1 m2]);
        k = 1;
        df = size(d,1) - k;
        %tthr = tinv(1 - pthresh,df);
        tsig = (p < pthresh) .* t;

        wh = yval * (tsig > 0);%double((t > tthr)); wh(wh==0) = NaN; wh(wh>0) = yval;
        %plot(x,wh,color{1},'LineWidth',3);
        for i = 1:length(wh)
            if wh(i)
                text(x(i),wh(i),'*','Color',color{1}(1),'FontSize',24);
            end
        end

        %wh = yval(find(tsig < 0)); % double((t < -tthr)); wh(wh==0) = NaN; wh(wh>0) = yval;
        %plot(x,wh,color{2},'LineWidth',3);

        wh = yval * (tsig < 0);%double((t > tthr)); wh(wh==0) = NaN; wh(wh>0) = yval;
        %plot(x,wh,color{1},'LineWidth',3);
        for i = 1:length(wh)
            if wh(i)
                text(x(i),wh(i),'*','Color',color{2}(1),'FontSize',24);
            end
        end

        text(x(5),yval+.04 * max([m1 m2]),'Significant','FontSize',16);
    end

    drawnow

else

    if dorobust
        [m1,t,p,sterr] = robust_mean(dat(:,:,1));
    else
        m1 = nanmean(dat(:,:,1), 1);
        sterr = ste(dat);
    end

    if ~iscell(color), tmp = color; color = []; color{1} = tmp; end

    hold on;
    if ischar(color{1})
        h = plot(x,m1,color{1},'LineWidth',2);
    else
        h = plot(x,m1,'o-', 'Color', color{1},'LineWidth',2);
    end

    fill_around_line(m1,sterr,color{1},x);

end

return


function w = dwt3D(x, J, af)

% 3-D Discrete Wavelet Transform
%
% USAGE:
%   w = dwt3D(x, stages, af)
% INPUT:
%   x - N1 by N2 by N3 matrix
%       1) Ni all even
%       2) min(Ni) >= 2^(J-1)*length(af)
%   J - number of stages
%   af  - analysis filters
% OUTPUT:
%   w - cell array of wavelet coefficients
% EXAMPLE:
%   [af, sf] = farras;
%   x = rand(128,64,64);
%   w = dwt3D(x,3,af);
%   y = idwt3D(w,3,sf);
%   err = x-y;
%   max(max(max(abs(err))))
%
% WAVELET SOFTWARE AT POLYTECHNIC UNIVERSITY, BROOKLYN, NY
% http://taco.poly.edu/WaveletSoftware/

for k = 1:J
    [x w{k}] = afb3D(x, af, af, af);
end
w{J+1} = x;


function [nuisance_ratio, rsquare_design, rsquare_nuis] = scn_component_rsquare(V, nuisanceX, designX)
% Print a table of r-square values (variance explained) for each of V data
% vectors by nuisance (mvmt, physio) and task-related predictors
%
% Designed to work with components
%
% :Examples:
% ::
%
%     % Typical operation
%     scn_component_rsquare(compscore, movement_params(1:157, :), X(1:157, :));
%
%     % No design
%     scn_component_rsquare(compscore, movement_params(1:157, :));
%
%     % Neither design nor nuisance, uses linear drift
%     scn_component_rsquare(compscore, []);
%
% ..
%    Tor Wager, Feb 2008
% ..

[t, m] = size(V);

if nargin < 3, designX = []; end

if isempty(nuisanceX)
    disp('Looking for nuisance covariates, but none found. Using linear drift.');
    nuisanceX = (1:t)';
end

disp('Variance in each component explained:')

% Nuisance
rsquare_nuis = get_rsquare(V, nuisanceX)';

if ~isempty(designX)
rsquare_design = get_rsquare(V, designX)';
else
    rsquare_design = .01 * ones(m, 1);
end

nuisance_ratio = rsquare_nuis ./ rsquare_design;

nuis_related = find(nuisance_ratio > 2);
task_related = find(nuisance_ratio < 1);

%rank_badness = sort(nuisance_ratio, 1, 'descend');
disp('All components');
print_matrix([(1:m)' rsquare_design rsquare_nuis nuisance_ratio], {'Comp.' 'R^2 Task' 'R^2 Nuisance' 'Ratio'});
fprintf('\n');

disp('Most task-related');
if isempty(designX)
    disp('NO DESIGN INFORMATION.');
else
print_matrix([task_related rsquare_design(task_related) rsquare_nuis(task_related) nuisance_ratio(task_related)], {'Comp.' 'R^2 Task' 'R^2 Nuisance' 'Ratio'});
end
fprintf('\n');

disp('Most nuisance-related');
print_matrix([nuis_related rsquare_design(nuis_related) rsquare_nuis(nuis_related) nuisance_ratio(nuis_related)], {'Comp.' 'R^2 Task' 'R^2 Nuisance' 'Ratio'});
fprintf('\n');

%fprintf('\tComp %3.0f : %3.0f%%\n', j, rsquare(j)*100);
end

function rsquare = get_rsquare(V, X)

    [t, m] = size(V);

    for j = 1:m
    % Center component to avoid counting intercept in r-square?
    % Don't have to, doesn't matter because var operator is 2nd moment
    y = V(:, j);
    b =  X \ y;

    fits = X * b;

    rsquare(j) = var(fits) / var(y);

    end

end


% plot_lognorm_scat2_1d: Plots the log normalized 2nd order scattering
% coefficients for 1d signals.
% Usage
%    plot_lognorm_scat2_1d(S)
% Input
%    S: The log normalized scattering coefficients.
% Output
%    N/A
% Description
%   Plots the second order log normalized scattering coefficients as a
%   function of j2-j1 (where 2^(-j1) is the first frequency, 2^(-j2) is the
%   second frequency) for various values of j1.

function plot_lognorm_scat2_1d(S)

% Settings

% The lower limit of j2-j1
lowlimit = -2;

% Stop plotting j2-j1 at the max-earlystop
earlystop = 2;

% The values of j1 go up to max-earlyend
earlyend = 1;

%% Initialize

% Adjust settings as needed
earlyend = max(earlyend,earlystop);

% Second order coefficients
S2 = S{3};

% Number of frequencies minus one
maxj1 = max(S2.meta.j(1,:));

% Colormap
C = jet(maxj1-earlyend+1);

% Legend
leg = cell(maxj1-earlyend+1,1);

%% Loop through values of j1 and plot in terms of j2-j1

figure,hold on;
for j1=0:(maxj1-earlyend)
    indj1 = find(S2.meta.j(1,:)==j1);
    j2 = S2.meta.j(2,indj1);

    S2j1j2 = S2.signal(indj1);
    S2j1j2 = cat(2,S2j1j2{:});
    S2j1j2 = S2j1j2(1,:);

    [j2,indj2] = sort(j2,'ascend');
    S2j1j2 = S2j1j2(indj2);

    j2minj1 = j2-j1;
    indj2j1 = j2minj1 >= lowlimit;
    indj2j1((end-earlystop+1):end) = false;

    plot(j2minj1(indj2j1),S2j1j2(indj2j1),'Color',C(j1+1,:));
    leg{j1+1} = sprintf('%d',j1);
end
legend(leg);
hold off;

end


%% load some data
load leq
%% plot it
plot(yy);, grid on
title('1-D signal')
%% embed it into a 3-D space with a delay of 10
N=10;
A=[yy(1:end-(N-1)),yy(N/2:end-N/2),yy(N:end)];
%% plot it again
figure
plot3(A(:,1),A(:,2),A(:,3))
grid on, axis tight
title('3-D signal')


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_SCARA(robot, T)
%   Solves the inverse kinematic problem for the SCARA example robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC_SCARA returns 2 possible solutions, thus,
%   Q is a 4x4 matrix where each column stores 4 feasible joint values.
%
%
%   Example code:
%
%   robot=load_robot('example', 'scara');
%   q = [0 0 0 0];
%   T = directkinematic(robot, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(robot, T);
%   %check that all of them are feasible solutions!
%   %and every Ti equals T
%   for i=1:2,
%        Ti = directkinematic(robot, qinv(:,i))
%   end
%
%	See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function q = inversekinematic_scara(robot, T)

fprintf('\nComputing inverse kinematics for the %s robot', robot.name);

%initialize q
q=zeros(4,2);

%Evaluate the DH table to obtain geometric parameters
d = eval(robot.DH.d);
a = eval(robot.DH.a);

%Store geometric parameters
L1=abs(d(1));
L2=abs(a(1));
L3=abs(a(2));

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Distance of the point to the origin of S0
R = sqrt(Px^2+Py^2);

%Compute angles
gamma = real(acos((L2^2+R^2-L3^2)/(2*R*L2)));
beta = atan2(Py,Px);
delta = real(acos((L2^2+L3^2-R^2)/(2*L2*L3)));

%find the last rotation for the two possible configurations
q4_1= find_last_rotation(robot,[beta+gamma delta-pi L1-Pz 0], T);
q4_2= find_last_rotation(robot,[beta-gamma pi-delta L1-Pz 0], T);

%Arrange all possible solutions
q=[beta+gamma beta-gamma;
    delta-pi pi-delta;
    L1-Pz    L1-Pz;
    q4_1 q4_2];

% Compute the last rotation
function q4 = find_last_rotation(robot, q, T)

U = T(1:3,1);

%Recompute the DH table according to q1, q2 and q3
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%now compute the position/orientation of the system S3
H=eye(4);
for i=1:3,
    H=H*dh(theta(i), d(i), a(i), alpha(i));
end

X3=H(1:3,1);
Y3=H(1:3,2);

coseno=X3'*U;
seno=U'*Y3;
%compute the last rotation
q4=atan2(seno,coseno);


function Gamma = simgamma_ltd(T,P,Pi,rate,L,refractory_period,grouping)
%
% Simulate state time courses with longer-than-order-1-markov time dependencies
%
% INPUTS:
%
% T                     Number of time points for each time series
% P                     Transition probability matrix (K by K)
% Pi                    Initial probabilities (K by 1)
% rate                  The weights that model the contribution of the
%                       latest L points are modelled by a Gamma
%                       distribution, whose shape is 1 - and rate is
%                       specified here
% L                     The length of the history (in number of time points)
%                       that influences the state at time t
% refractory_period     to prevent bursts of quick changes, refractory_period
%                       can be set so that, after a change, you cannot change again
%                       after 'refractory_period' number of iterations
%
% OUTPUTS
%
% Gamma         simulated  p(state | data)
%
% Author: Diego Vidaurre, OHBA, University of Oxford

N = length(T); K = length(Pi);

if nargin<4, rate = 2; end
if nargin<5, L = 10; end
if nargin<6, refractory_period = 2; end
if nargin<7, grouping = []; end

Gamma = zeros(sum(T),K);
weights = gampdf(0:L-1,1,rate)';
weights = weights / sum(weights);
weights = weights(end:-1:1);

for n = 1:N
    if ~isempty(grouping)
        i = grouping(n);
        Pn = P(:,:,i); Pin = Pi(:,i)';
    else
        Pn = P; Pin = Pi;
    end
    Gammai = zeros(T(n),K);
    if any(Pin==1)
        Gammai(1,Pin==1) = 1;
    else
        Gammai(1,:) = mnrnd(1,Pin);
    end
    last_ch = Inf;
    for t = 2:L
        if last_ch < refractory_period
            Gammai(t,:) = Gammai(t-1,:);
            last_ch = last_ch + 1;
        else
            if t==2
                g = repmat(weights(end-t+2:end,1),1,K) .* Gammai(1:t-1,:);
            else
                g = sum(repmat(weights(end-t+2:end,1),1,K) .* Gammai(1:t-1,:));
            end
            g = g / sum(g);
            Gammai(t,:) = mnrnd(1,g*Pn);
            if any(Gammai(t,:)~=Gammai(t-1,:)), last_ch = 1; end
            Gammai(t,:) = Gammai(t,:) / sum(Gammai(t,:));
        end
    end
    for t=L+1:T(n)
        if last_ch < refractory_period
            Gammai(t,:) = Gammai(t-1,:);
            last_ch = last_ch + 1;
        else
            g = sum(repmat(weights,1,K) .* Gammai(t-L:t-1,:));
            Gammai(t,:) = mnrnd(1,g*Pn);
            if any(Gammai(t,:)~=Gammai(t-1,:)), last_ch = 1; end
        end
    end
    t = (1:T(n)) + sum(T(1:n-1));
    Gamma(t,:) = Gammai;
end

end


function [Population,Fitness] = EnvironmentalSelection(Population,N)
% The environmental selection of SPEA2+SDE

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    %% Calculate the fitness of each solution
    Fitness = CalFitness(Population.objs);

    %% Environmental selection
    Next = Fitness < 1;
    if sum(Next) < N
        [~,Rank] = sort(Fitness);
        Next(Rank(1:N)) = true;
    elseif sum(Next) > N
        Del  = Truncation(Population(Next).objs,sum(Next)-N);
        Temp = find(Next);
        Next(Temp(Del)) = false;
    end
    % Population for next generation
    Population = Population(Next);
    Fitness    = Fitness(Next);
end

function Del = Truncation(PopObj,K)
% Select part of the solutions by truncation

    N = size(PopObj,1);

    %% Calculate the shifted distance between each two solutions
    Distance = inf(N);
    for i = 1 : N
        SPopObj = max(PopObj,repmat(PopObj(i,:),N,1));
        for j = [1:i-1,i+1:N]
            Distance(i,j) = norm(PopObj(i,:)-SPopObj(j,:));
        end
    end

    %% Truncation
    Del = false(1,N);
    while sum(Del) < K
        Remain   = find(~Del);
        Temp     = sort(Distance(Remain,Remain),2);
        [~,Rank] = sortrows(Temp);
        Del(Remain(Rank(1))) = true;
    end
end


function energy = computeEnergyBinaryPairwise( unaryTerms, pairwiseTerms, labels )
%computeEnergyBinaryPairwise computes the value of the energy with unary and pairwise potentials

%% check input
numLabels = 2;

if ~isnumeric(unaryTerms) || ~ismatrix(unaryTerms) || size(unaryTerms, 2) ~= 2
    error('Incorrect format for unaryTerms, has to be numNodes x 2')
end
numNodes = size(unaryTerms, 1);

if ~isnumeric(pairwiseTerms) || ~ismatrix(pairwiseTerms) || size(pairwiseTerms, 2) ~= 6
    error('Incorrect format for pairwiseTerms, has to be numEdges x 6')
end
numEdges = size(pairwiseTerms, 1);

if ~isnumeric(labels) || ~isvector(labels) || length(labels) ~= numNodes
    error('Incorrect format for labels, has to be numNodes x 1')
end
labels = labels(:);
if any(labels > numLabels) || any(labels < 1)
    error('Incorrect values for labels, has to be an integer from 1 to numLabels')
end

%% computation
energy = sum( unaryTerms((1 : numNodes)' + numNodes * (labels - 1) ) );

label1 = labels(pairwiseTerms(:, 1));
label2 = labels(pairwiseTerms(:, 2));
labelMap = [ 3, 4; 5, 6];
jointLabelMap = labelMap( label1 + 2 * (label2 - 1) );

energy = energy + sum( pairwiseTerms( (1 : numEdges)' + numEdges * (jointLabelMap - 1) ) );

end


function rchy = realized_hayashi_yoshida(priceA,timeA,priceB,timeB,timeType,samplingType,samplingInterval,overlap)
% Computed the Hayashi-Yoshida estimator of quadratic covariation, and allows for
% the empirical-performance motivated K-lead-and-lag version similar to Drost and Nijman (1997).
%
% USAGE:
%   [RCHY] = realized_hayashi_yoshida_(PRICEA,TIMEA,PRICEB,TIMEB,TIMETYPE,SAMPLINGTYPE,SAMPLINGINTERVAL,OVERLAP)
%
% INPUTS:
%   PRICEA            - mA by 1 vector of high frequency prices
%   TIMEA             - mA by 1 vector of times where TIMEB(i) corresponds to PRICEA(i)
%   PRICEA            - mB by 1 vector of high frequency prices
%   TIMEA             - mB by 1 vector of times where TIMEB(i) corresponds to PRICEB(i)
%   TIMETYPE          - String describing the way times are measured
%                        'wall'    24-hour clock of the form HHMMSS, e.g. 101543 or 153217
%                        'seconds' Time measures in seconds past midnight.
%                          TIME must satisfy 0<=TIME<86400
%                        'unit'  Unit normalized date format, e.g. .1, .234, .9
%                          Unit normalized times are more general than the
%                          other types and can be applied to data from more
%                          than one calendar day
%   SAMPLINGTYPE      - String describing the type of sampling to use when
%                         filtering PRICE
%                         'CalendarTime' - Sample in calendar time using
%                           observations separated by SAMPLINGINTERVAL
%                           seconds. If TIMETYPE is 'unit',
%                           SAMPLINGINTERVAL must be between 0 and 1 and
%                           represents the fraction of the sample to skip
%                           when sampling.
%                         'CalendarUniform' - Sample in calendar time using
%                           SAMPLINGINTERVAL observations spread uniformly
%                           between TIME(1) and TIME(m)
%                         'BusinessTime' - Sample in business (tick) time
%                           using observation separated by SAMPLINGINTERVAL
%                           ticks
%                         'BusinessUniform' - Sample in business (tick)
%                           time using observations uniformly spaced in
%                           business time.
%                         'Fixed' - Sample at specific points in time. When
%                           using fixed, SAMPLINGINTERVAL must be a n by 1 vector
%                           of times with the same TIMETYPE as TIME (i.e.
%                           seconds if TIME is in seconds)
%   SAMPLINGINTERVAL   - Scalar integer or n by 1 vector whose meaning depends on the
%                          selected SAMPLINGTYPE
%   OVERLAP            - Number of ticks to overlap when computing the HY estimator.  The original HY
%                          estimator uses 0, which corresponds to the maximum likelihood estimator for
%                          price processes whose observation times are driven by independent Poisson
%                          processes.  Empirically this estimator performs poorly because prices are
%                          not a vector semi-martingale and using a larger number of lags can alleviate
%                          this problem.
%
% OUTPUTS:
%   RCHY             - The K-lead-and-lag Hayashi-Yoshida covariance estimator
%
% COMMENTS:
%   Filtering the price in calendar time allow the creation of a Hayashi-Yoshida corrected
%   calendar-time sampled realized covariance.  The value in SAMPLINGTYPE is applied to price A and
%   then the realized HY estimator is computed using the filtered price of A and the filtered times
%   of A, and all observations of B.  Price filtering is done using realized_price_filter
%
% EXAMPLES:
%   % Standard use with all prices with wall time prices
%   RCHY = realized_hayashi_yoshida(PRICEA,TIMEA,PRICEB,TIMEB,'wall','BusinessTime',1,0)
%
%   % 10 lead and lag RCHY with all prices with wall time prices
%   RCHY = realized_hayashi_yoshida(PRICEA,TIMEA,PRICEB,TIMEB,'wall','BusinessTime',1,10)
%
%   % 1-minute realized covariance with a HY correction
%   RCHY = realized_hayashi_yoshida(PRICEA,TIMEA,PRICEB,TIMEB,'wall','CalendarTime',60,0)
%
%  See also REALIZED_MULTIVARIATE_KERNEL, REALIZED_COVARIANCE, REALIZED_KERNEL, REALIZED_VARIANCE,
%  REALIZED_RANGE, REALIZED_QUANTILE_VARIANCE

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 5/1/2008

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin<7 || nargin>8
    error('Seven or eight inputs required.')
end
if size(priceA,2)>size(priceA,1)
    priceA=priceA';
end
if size(priceA,2)>1
    error('PRICEA must be a m by 1 vector.')
end
if size(timeA,2)>size(timeA,1)
    timeA=timeA';
end
if any(diff(timeA)<0)
    error('TIMEA must be sorted and increasing')
elseif any(diff(timeA)==0)
    warning('oxfordRealized:realizedPriceFilter','TIMEA contains multiple entries with the same value. This creates an ambiguity and FILTEREDPRICE will contain the last price if TIMEA does not only unique elements.')
end
if size(timeA,2)>1 || length(timeA)~=length(priceA)
    error('TIMEA must be a m by 1 vector.')
end
% Inserted to protect against inputing integer times
timeA = double(timeA);
if size(priceB,2)>size(priceB,1)
    priceB=priceB';
end
if size(priceB,2)>1
    error('PRICEB must be a m by 1 vector.')
end
if size(timeB,2)>size(timeB,1)
    timeB=timeB';
end
if any(diff(timeB)<0)
    error('TIMEB must be sorted and increasing')
elseif any(diff(timeB)==0)
    warning('oxfordRealized:realizedPriceFilter','TIMEB contains multiple entries with the same value. This creates an ambiguity and FILTEREDPRICE will contain the last price if TIMEB does not only unique elements.')
end
if size(timeB,2)>1 || length(timeB)~=length(priceB)
    error('TIMEB must be a m by 1 vector.')
end
% Inserted to protect against inputing integer times
timeB = double(timeB);

% make sure the intersection is non-empty
if ~(min(timeB)<max(timeA) || min(timeA)<max(timeB))
    warning('oxfordRealized:realizedHayashiYoshida','The intersection of TIMESA and TIMESB is empty.  The RCHY will necessarily be 0.');
    intersectionIsEmpty = true;
else
    intersectionIsEmpty = false;
end

timeType=lower(timeType);
if ~ismember(timeType,{'wall','seconds','unit'})
    error('TIMETYPE must be one of ''wall'', ''seconds'' or ''unit''.');
end

samplingType=lower(samplingType);
if ~ismember(samplingType,{'calendartime','calendaruniform','businesstime','businessuniform','fixed'})
    error('SAMPLINGTYPE must be one of ''CalendarTime'', ''CalendarUniform'', ''BusinessTime'', ''BusinessUniform'' or ''Fixed''.');
end

m=size(priceA,1);
t0Original=timeA(1);
tTOriginal=timeA(m);
if ismember(samplingType,{'calendartime','calendaruniform','businesstime','businessuniform'})
    % Must be a scalar integer, unless using unit times
    if (~isscalar(samplingInterval) || floor(samplingInterval)~=samplingInterval || samplingInterval<1) && ~strcmp(timeType,'unit')
        error('SAMPLINGINTERVAL must be a positive integer for the SAMPLINGTYPE selected.')
    end
else
    if size(samplingInterval,2)>size(samplingInterval,1)
        samplingInterval=samplingInterval';
    end
    if ~(any(samplingInterval>=t0Original) && any(samplingInterval<=tTOriginal))
        error('At least one sampling interval must be between min(TIME) and max(TIME) when using ''Fixed'' as SAMPLINGTYPE.')
    end
    if any(diff(samplingInterval)<=0)
        error('When using ''Fixed'' as SAMPLINGTYPE the vector of sampling times in SAMPLINGINTERVAL must be sorted and strictly increasing.')
    end
end

if strcmp(timeType,'unit') && strcmp(samplingType,'calendartime')
    % samplingInterval must be between 0 and 1
    if samplingInterval>1
        error('When TIMETYPE is ''unit'' and SAMPLINGTYPE is ''CalendarTime'', SAMPLINGINTERVAL must also be in ''unit'' terms, and so must be between 0 and 1')
    end
end

if nargin==7
    overlap = 0;
elseif overlap<0 || floor(overlap)~=overlap || max(size(overlap))>1
    error('OVERLAP must be a non-negative integer.')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Price A is the base price, price B is the other price.  First sample priceA according to
% samplingType and samplingInterval, then use the actual times of these to estimate the HY
% respecting the value in overlap

if ~intersectionIsEmpty
    % First filter the price
    [filteredPriceA,filteredTimeA,actualTimeA] = realized_price_filter(priceA,timeA,timeType,samplingType,samplingInterval);

    % Then call the core routine
    rchy = realized_hayashi_yoshida_core(filteredPriceA,actualTimeA,priceB,timeB,overlap);
else
    rchy = 0;
end

function [rchy,times] = realized_hayashi_yoshida_core(priceA,timeA,priceB,timeB,overlap)
% Core routine for computing the Hayashi-Yoshida estimator of quadratic covariation, and allows for
% the empirical-performance motivated K-lead-and-lag version similar to Drost and Nijman (1997).
%
% USAGE:
%   [RCHY,TIMES] = realized_hayashi_yoshida_core(PRICEA,TIMEA,PRICEB,TIMEB,OVERLAP)
%
% INPUTS:
%   PRICEA           - mA by 1 vector of high frequency prices
%   TIMEA            - mA by 1 vector of times where TIMEB(i) corresponds to PRICEA(i)
%   PRICEA           - mB by 1 vector of high frequency prices
%   TIMEA            - mB by 1 vector of times where TIMEB(i) corresponds to PRICEB(i)
%   OVERLAP          - [OPTIONA] Number of ticks to overlap when computing the HY estimator.  The
%                        original HY estimator uses 0, which corresponds to the maximum likelihood
%                        estimator for price processes whose observation times are driven by
%                        independent Poisson processes.  Empirically this estimator performs poorly
%                        because prices are not a vector semi-martingale and using a larger number
%                        of lags can alleviate this problem. If omitted OVERLAP = 0.
%
% OUTPUTS:
%   RCHY             - The K-lead-and-lag Hayashi-Yoshida covariance estimator
%   TIMES            - A mA by 1 matrix of time stamps contains the times where the prices were
%                        sampled for computing the returns in the HY estimator.  This is mostly for
%                        diagnostic purposes.
%
% COMMENTS:
%   This is a helper function of realized_hayashi_yoshida and does no input checking.  In general
%   it should not be directly called.
%
%  See also REALIZED_MULTIVARIATE_KERNEL, REALIZED_COVARIANCE, REALIZED_KERNEL, REALIZED_VARIANCE,
%  REALIZED_RANGE, REALIZED_QUANTILE_VARIANCE

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 5/1/2008

priceA = log(priceA);
priceB = log(priceB);
timeA = double(timeA);
timeB = double(timeB);
% Price A is the base price, price B is the one which will move

% First find the first time that B is available before timeA(1).  If it is empty then find the first
% timeA(1) which is weakly after the timeB(1)
nA = size(priceA,1);
nB = size(priceB,1);

if timeA(1)<timeB(1)
    % A starts before B, so the easy solution is to project backward the price of the first B to the
    % time of the first A.  This will generate a 0 return but makes the algorithm easier.
    timeB=[timeA(1);timeB];
    priceB=[priceB(1);priceB];
    nB = size(priceB,1);
end

if timeA(nA)>timeB(nB)
    % A ends after B, so the easy solution is to project forward the last price B to the
    % time of the last A.  This will generate a 0 return but makes the algorithm easier.
    timeB=[timeB;timeA(nA)];
    priceB=[priceB;priceB(nB)];
    nB = size(priceB,1);
end

% Initialize the indices for A and B
indexA = 1;
indexB = find(timeB<=timeA(1), 1,'last');
if overlap>0
    indexB = max(indexB-overlap,1); % Make sure that indexB is >= 1
end

% Initialize rchy and the times
rchy = 0;
times = zeros(nA-1,4);
while indexA<nA
    % Get the two prices at indexA and indexA+1
    pA1 = priceA(indexA);
    pA2 = priceA(indexA+1);
    % get the times
    times(indexA-1,1) = timeA(indexA);
    times(indexA-1,2) = timeA(indexA+1);
    % Increment the index to A
    indexA = indexA+1;

    % Compute the index, including overlap if needed, make sure it doesn't end up before the first
    % index
    indexB_minus_overlap = max(indexB - overlap,1);
    % Get the first price of B
    pB1 = priceB(indexB_minus_overlap);
    % Get the time
    times(indexA-1,3) = timeB(indexB_minus_overlap);
    while timeB(indexB)<timeA(indexA)
        % The time stamp of B is strictly less than the time stamp of the second price in the return
        % to asset A so increment the counter.  Since we do the pre- and post- pend trick above,
        % there is no need to worry about uneven ending.
        indexB = indexB + 1;
    end
    % Compute the index plus the overlap, make sure it doesn't go outsie the number of obs
    indexB_plus_overlap = min(indexB + overlap,nB);
    % Get the second price
    pB2 = priceB(indexB_plus_overlap);
    % Get the final time
    times(indexA-1,4) = timeB(indexB_plus_overlap);
    % Add to rchy
    rchy = rchy + (pA2-pA1)*(pB2-pB1);

    % Rewind indexB by 1 since it is now after A
    if timeB(indexB)>timeA(indexA)
        indexB = indexB - 1;
    end
end


function [idx, frame] = registration_target(frames, useGPU)

%% Parameters
[ly, lx, nFrames] = size(frames);
data = single(frames);

if nargin < 2
  useGPU = false;
end

%% Prepare common arrays
% Taper mask
[ys, xs] = ndgrid(1:ly, 1:lx);
ys = abs(ys - mean(ys(:)));
xs = abs(xs - mean(xs(:)));
mY      = max(ys(:)) - 4;
mX      = max(xs(:)) - 4;
slope   = 1.2; % was 2

maskMul = single(1./(1 + exp((ys - mY)/slope)) ./(1 + exp((xs - mX)/slope)));
maskOffset = mean(data(:,:,1))*(1 - maskMul);
% Smoothing filter in frequency domain
sigma = 0.76; %.76 with mask
hgx = exp(-(((0:lx-1) - fix(lx/2))/sigma).^2);
hgy = exp(-(((0:ly-1) - fix(ly/2))/sigma).^2);
hg = hgy'*hgx;
fhg = real(fftn(ifftshift(single(hg/sum(hg(:))))));

eps0 = single(1e-20);
if useGPU
  data = gpuArray(data);
%   peakCorrMatrix = zeros(nFrames, nFrames, 'single', 'gpuArray');
% else
%   peakCorrMatrix = zeros(nFrames, nFrames, 'single');
end

corrRanges = zeros(nFrames, 3, 'single');

[a, b] = ndgrid(1:nFrames, 1:nFrames);
data = fft2(bsxfun(@plus, maskOffset, bsxfun(@times, maskMul, data)));
data = data./(eps0 + abs(data));
cdata = conj(data);
for fi = 1:nFrames
  % compute correlation map of frame fi with every other frame
  cmap = bsxfun(@times, data(:,:,a(:,fi)).*cdata(:,:,b(:,fi)), fhg);
  cmap = real(ifft2(cmap));
  % find peak of each correlation map
  peaks = gather(max(reshape(cmap, ly*lx, nFrames), [], 1));
  % compute first quartile of peak correlations, ignoring result
  % from correlation of this frame with itself.
  corrRanges(fi,:) = prctile(peaks((1:nFrames) ~= fi), [25 50 75]);
end
% find the frame with the best correlations with other frames
[bestCorr, idx] = max(corrRanges(:,1));
% plot(corrRanges)
frame = frames(:,:,idx);
% peakCorrMatrix = gather(peakCorrMatrix);
% [mxi, idx] = max(prctile(peakCorrMatrix, 25, 1))
% [mnx, mni] = min(prctile(peakCorrMatrix, 25, 2));
% figure, imagesc(peakCorrMatrix);
% figure, plot(1:nFrames, peakCorrMatrix(mxi,:), 1:nFrames, peakCorrMatrix(mni,:))
end


% Set system limits
lims = mr.opts('MaxGrad',32,'GradUnit','mT/m',...
    'MaxSlew',130,'SlewUnit','T/m/s',...
    'rfRingdownTime', 30e-6, 'rfDeadtime', 100e-6);

seq=mr.Sequence(lims);          % Create a new sequence object
fov=220e-3; Nx=256; Ny=256;     % Define FOV and resolution

foe=200e-3;             % Field of excitation
targetWidth=22.5e-3;    % Diameter of target excitation pattern
n=8;                    % Number of spiral turns
T=8e-3;                 % Pulse duration

% Define spiral k-space trajectory
kMax=(2*n)/foe/2;       % Units of 1/m (not rad/m)
tk=0:seq.gradRasterTime:T-seq.gradRasterTime;
kx=kMax*(1-tk/T).*cos(2*pi*n*tk/T);
ky=kMax*(1-tk/T).*sin(2*pi*n*tk/T);

% Define RF pulse
tr=0:seq.rfRasterTime:T-seq.rfRasterTime;
kxRf=interp1(tk,kx,tr,'linear','extrap');
kyRf=interp1(tk,ky,tr,'linear','extrap');
beta=2*pi*kMax*targetWidth/2/sqrt(2);  % Gaussian width in k-space
signal0 = exp(-beta.^2.*(1-tr/T).^2).*sqrt((2*pi*n*(1-tr/T)).^2+1);
signal = signal0.*(1 + exp(-1j.*2*pi*5e-2*(kxRf + kyRf)));

% Add gradient ramps
[kx,ky,signal]=mr.addRamps({kx,ky},'rf',signal);

rf = mr.makeArbitraryRf(signal,20*pi/180,'system',lims);
gxRf = mr.makeArbitraryGrad('x',mr.traj2grad(kx));
gyRf = mr.makeArbitraryGrad('y',mr.traj2grad(ky));

% Define other gradients and ADC events
deltak=1/fov;
gx = mr.makeTrapezoid('x','FlatArea',Nx*deltak,'FlatTime',6.4e-3);
adc = mr.makeAdc(Nx,'Duration',gx.flatTime,'Delay',gx.riseTime);
gxPre = mr.makeTrapezoid('x','Area',-gx.area/2,'Duration',2e-3);
phaseAreas = ((0:Ny-1)-Ny/2)*deltak;

% Refocusing pulse and spoiling gradients
%[rf180, gz] = mr.makeBlockPulse(pi,'Duration',1e-3,'SliceThickness',5e-3);
[rf180, gz] = mr.makeSincPulse(pi,'system',lims,'Duration',3e-3,...
    'SliceThickness',5e-3,'apodization',0.5,'timeBwProduct',4);

gzSpoil = mr.makeTrapezoid('z','Area',gx.area,'Duration',2e-3);

%%% Calculate timing (TE=20ms, TR=500ms)
delayTE1=ceil((20e-3/2 - mr.calcDuration(gzSpoil) - mr.calcDuration(rf180)/2)/seq.gradRasterTime)*seq.gradRasterTime;
delayTE2=delayTE1 - mr.calcDuration(gxPre) - mr.calcDuration(gx)/2;
delayTR=500e-3 - 20e-3 - mr.calcDuration(rf) - mr.calcDuration(gx)/2;

%% Loop over phase encodes and define sequence blocks
for i=1:Ny
    seq.addBlock(rf,gxRf,gyRf);
    seq.addBlock(mr.makeDelay(delayTE1));
    seq.addBlock(gzSpoil);
    seq.addBlock(rf180,gz);
    seq.addBlock(gzSpoil);
    seq.addBlock(mr.makeDelay(delayTE2));
    gyPre = mr.makeTrapezoid('y','Area',phaseAreas(i),'Duration',2e-3);
    seq.addBlock(gxPre,gyPre);
    seq.addBlock(gx,adc);
    seq.addBlock(mr.makeDelay(delayTR));
end

%% check whether the timing of the sequence is correct
[ok, error_report]=seq.checkTiming;

if (ok)
    fprintf('Timing check passed successfully\n');
else
    fprintf('Timing check failed! Error listing follows:\n');
    fprintf([error_report{:}]);
    fprintf('\n');
end

%%
seq.setDefinition('Name', 'se_selRF');

seq.write('selectiveRf.seq');   % Write to pulseq file
seq.plot();

return

%% Write to file
% The sequence is written to file in compressed form according to the file
% format specification using the |write| method.
%seq.setDefinition('Scan_ID',2068);
%seq.setDefinition('Recon_Mode',1);
%seq.write('external.seq')

%seq.plot


function mags = gtmmag(net, latent_data)
%GTMMAG	Magnification factors for a GTM
%
%	Description
%	 MAGS = GTMMAG(NET, LATENTDATA) takes a GTM structure NET, and
%	computes the magnification factors for each point the latent space
%	contained in LATENTDATA.
%
%	See also
%	GTM, GTMPOST, GTMLMEAN
%

%	Copyright (c) Ian T Nabney (1996-2001)

errstring = consist(net, 'gtm');
if ~isempty(errstring)
  error(errstring);
end

Jacs = rbfjacob(net.rbfnet, latent_data);
nlatent = size(latent_data, 1);
mags = zeros(nlatent, 1);
temp = zeros(net.rbfnet.nin, net.rbfnet.nout);
for m = 1:nlatent
  temp = squeeze(Jacs(m, :, :));  % Turn into a 2d matrix
  mags(m) = sqrt(det(temp*temp'));
end


function [ S, f, Serr ]= mtspectrumc_unequal_length_trials( data, movingwin, params, sMarkers )

% This routine computes the multi-taper spectrum for a given set of unequal length segments. It is
% based on modifications to the Chronux routines. The segments are continuously structured in the
% data matrix, with the segment boundaries given by markers. Below,
% movingwin is used in a non-overlaping way to partition each segment into
% various windows. Th spectrum is evaluated for each window, and then the
% window spectrum estimates averaged. Further averaging is conducted by
% repeating the process for each segment.
%
% Inputs:
%
%   data = data( samples, channels )- here segments must be stacked
%   as explained in the email
%   movingwin = [window winstep] i.e length of moving
%              window and step size. Note that units here have
%              to be consistent with units of Fs. If Fs=1 (ie normalized)
%              then [window winstep]should be in samples, or else if Fs is
%              unnormalized then they should be in time (secs).
%   sMarkers = N x 2 array of segment start & stop marks. sMarkers(n, 1) = start
%           sample index; sMarkers(n,2) = stop sample index for the nth segment
%   params = see Chronux help on mtspecgramc
%
% Output:
%
%       S       frequency x channels
%       f       frequencies x 1
%       Serr    (error bars) only for err(1)>=1
%
%

iwAvg = 1; % 0=no weighted average, 1=weighted average
debug = 0; % will display intermediate calcs.

if nargin < 2; error('Unequal length trials:: Need data and window parameters'); end;
if nargin < 3; params=[]; end;
if isempty( sMarkers ), error( 'Unequal length trials:: Need Markers...' ); end
[ tapers, pad, Fs, fpass, err, trialave, params ] = getparams( params );
if nargout > 2 && err(1)==0;
%   Cannot compute error bars with err(1)=0. change params and run again.
    error('Unequal length trials:: When Serr is desired, err(1) has to be non-zero.');
end;

% Set moving window parameters to no-overlapping
if abs(movingwin(2) - movingwin(1)) >= 1e-6, disp( 'avgSpectrum:: Warming: Window parameters for averaging should be non-overlapping. Set movingwin(2) = movingwin(1).' ); end

wLength = round( Fs * movingwin(1) ); % number of samples in window
wStep = round( movingwin(2) * Fs ); % number of samples to step through

% Check whether window lengths satify segment length > NW/2
if ( wLength < 2*tapers(1) ), error( 'avgSpectrum:: movingwin(1) > 2*tapers(1)' ); end

% Left align segment markers for easier coding
sM = ones( size( sMarkers, 1 ), 2 );
sM( :, 2 ) = sMarkers( :, 2 ) - sMarkers( :, 1 ) + 1;

% min-max segments
Nmax = max( sM(:,2) ); Nmin = min( sM(:,2) );
if ( Nmin < 2*tapers(1) ), error( 'avgSpectrum:: Smallest segment length > 2*tapers(1). Change taper settings' ); end

% max time-sample length will be the window length.
nfft = 2^( nextpow2( wLength ) + pad );
[ f, findx ] = getfgrid( Fs, nfft, fpass);

% Precompute all the tapers
sTapers = tapers;
sTapers = dpsschk( sTapers, wLength, Fs ); % compute tapers for window length

nChannels = size( data, 2 );
nSegments = size( sMarkers, 1 );

if debug
    disp( ['Window Length = ' num2str(wLength)] );
    disp( ['Window Step = ' num2str(wStep)] );
    disp( ' ' );
end

s = zeros( length(f), nChannels );
serr = zeros( 2, length(f), nChannels );
S = zeros( length(f), nChannels );
Serr = zeros( 2, length(f), nChannels );
nWins = 0;
for sg = 1 : nSegments
    % Window lengths & steps fixed above
    % For the given segment, compute the positions & number of windows
    N = sM(sg,2);
    wStartPos = 1 : wStep : ( N - wLength + 1 );
    nWindows = length( wStartPos );
    if nWindows
        nWins = nWins + nWindows; % for averaging purposes

        w=zeros(nWindows,2);
        for n = 1 : nWindows
            w(n,:) = [ wStartPos(n), (wStartPos(n) + wLength - 1) ]; % nWindows x 2. just like segment end points
        end

        % Shift window limits back to original sample-stamps
        w(:, 1) = w(:,1) + (sMarkers( sg, 1 ) - 1);
        w(:, 2) = w(:,2) + (sMarkers( sg, 1 ) - 1);

        if debug
            disp( ['Segment Start/Stop = ' num2str( w(1,1) ) ' ' num2str( w(end,2) ) ] );
            disp( ['Min / Max Window Positions = ' num2str( min(w(:,1)) ) ' ' num2str( max(w(:,1)) ) ] );
            disp( ['Total Number of Windows = ' num2str(nWindows) ]);
            disp( ' ' );
        end

        % Pile up window segments similar to segment pileup
        wData = zeros( wLength, nChannels, nWindows ); %initialize to avoid fragmentation
        for n = 1:nWindows
            %wData( :, :, n ) = detrend( data( w(n,1):w(n,2), : ), 'constant' );
            wData( :, :, n ) = detrend( data( w(n,1):w(n,2), : ) );
        end

        % J1 = frequency x taper x nWindows
        % J2 = frequency x taper x nWindows x nChannels
        J2 = zeros( length(f), tapers(2), nWindows, nChannels ); J2 = complex( J2, J2 );
        for c = 1 : nChannels
            J1 = mtfftc( squeeze(wData( :, c, : )), sTapers, nfft, Fs ); % FFT for the tapered data
            J2( :, :, :, c ) = J1(findx,:,:);
        end
        % J2 = frequency x taper x nWindows x nChannels
        % Inner mean = Average over tapers => frequency x nWindows x nChannels
        % Outer mean = Average over windows => frequency x nChannels
        dim1 = [length(f), nWindows, nChannels];
        dim2 = [length(f), nChannels];
        % s = frequency x nChannels
        s = reshape( squeeze( mean( reshape( squeeze( mean( conj(J2).*J2, 2 ) ), dim1), 2 ) ), dim2 );

        % Now treat the various "windowed data" as "trials"
        % serr = 2 x frequency x channels. Output from specerr = 2 x frequency x 1
        for c = 1 : nChannels
            serr( :, :, c ) = specerr( squeeze( s(:, c ) ), squeeze( J2(:,:,:, c ) ), err, 1 );
        end

        if iwAvg
            % Segment Weighted error estimates.
            S = S + nWindows*s;
            Serr = Serr + nWindows*serr;
        else
            S = S + s;
            Serr = Serr + serr;
        end

    else
        if debug, disp(['avgSpectrum:: Zero windows for segment: ' num2str(sg) ]); end
    end
end

% Segment Weighted error estimates.
% Only over those that had non-zero windows
if nWins && iwAvg
    S=S/nWins; Serr=Serr/nWins;
end
if ~nWins
    if debug, disp(['avgCoherence:: No segment long enough with movingwin parameters found. Reduce movingwin.' ]); end
end


%IM_BDILATION Fixed mapping for binary dilation (DIP_Image)
%
%	B = IM_BDILATION(A,N,CONNECTIVITY,EDGE_CONDITION)
%	B = A*IM_BDILATION([],N,CONNECTIVITY,EDGE_CONDITION)
%	B = A*IM_BDILATION(N,CONNECTIVITY,EDGE_CONDITION)
%
% INPUT
%   A        Dataset with binary object images dataset (possibly multi-band)
%   N        Number of iterations (default 1)
%   CONNECTIVITY    See BDILATION
%   EDGE_CONDITION  Value of edge, default 0
%
% OUTPUT
%   B        Dataset with dilated images
%
% SEE ALSO (PRTools Guide)
% DATASETS, DATAFILES, DIP_IMAGE, BDILATION

% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Physics, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

function b = im_bdilation(varargin)

	argin = shiftargin(varargin,'scalar');
  argin = setdefaults(argin,[],1,-2,0);
  if mapping_task(argin,'definition')
    b = define_mapping(argin,'fixed');
    b = setname(b,'Image dilation');
  else
    [a,n,connect,edgecon] = deal(argin{:});
    if isa(a,'prdataset') % allows datafiles too
      isobjim(a);
      b = filtim(a,mfilename,{n,connect,edgecon});
    elseif isa(a,'double') || isa(a,'dip_image') % here we have a single image
      if checktoolbox('dipimage')
        a = dip_image(a,'bin');
        b = bdilation(a,n,connect,edgecon);
      else
        diplibwarn
        b = bwmorph(a,'dilate',n);
      end
    else
      error('Illegal input')
    end
	end

return


classdef LIRCMOP1 < PROBLEM
% <multi> <real> <large/none> <constrained>
% Constrained benchmark MOP with large infeasible regions

%------------------------------- Reference --------------------------------
% Z. Fan, W. Li, X. Cai, H. Huang, Y. Fang, Y. You, J. Mo, C. Wei, and E.
% Goodman, An improved epsilon constraint-handling method in MOEA/D for
% CMOPs with large infeasible regions, Soft Computing, 2019, 23:
% 12491-12510.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Wenji Li

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 30; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values and constraint violations
        function Population = Evaluation(obj,varargin)
            X = varargin{1};
            X = max(min(X,repmat(obj.upper,size(X,1),1)),repmat(obj.lower,size(X,1),1));
            x_odd       = X(:,3:2:end);
            x_even      = X(:,2:2:end);
            g_1         = sum((x_odd - sin(0.5 * pi * X(:,1))).^2,2);
            g_2         = sum((x_even - cos(0.5 * pi * X(:,1))).^2,2);
            PopObj(:,1) = X(:,1) + g_1;
            PopObj(:,2) = 1 - X(:,1) .^ 2 + g_2;
            PopCon(:,1) = (0.5 - g_1).*(0.51 - g_1);
            PopCon(:,2) = (0.5 - g_2).*(0.51 - g_2);
            Population  = SOLUTION(X,PopObj,PopCon,varargin{2:end});
            obj.FE      = obj.FE + length(Population);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = 1 - R(:,1).^2;
            R      = R + 0.5;
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
    end
end


function segment_length = p07_boundary_segment_length ( segment_index, h )

%*****************************************************************************80
%
%% P07_BOUNDARY_SEGMENT_LENGTH returns boundary segment lengths in problem 07.
%
%  Discussion:
%
%    No attempt has been made here to accurately compute a value of N
%    which would guarantee that the boundary would be divided into pieces
%    of length no more than H.  The curve is a little too complicated
%    to make this easy to do.
%
%    Moreover, the points that will be generated will only be equally
%    spaced in their X argument, not in their arc length.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, integer SEGMENT_INDEX, the index of one of the boundary segments.
%
%    Input, real H, the suggested spacing between points.
%
%    Output, integer SEGMENT_LENGTH, the number of points in the segment.
%
  if ( h <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'P07_BOUNDARY_SEGMENT_LENGTH - Fatal error!' );
    fprintf ( 1, '  Nonpositive H = %f\n', h );
    error ( 'P07_BOUNDARY_SEGMENT_LENGTH - Fatal error!' );
  end

  if ( segment_index == 1 )

    n = round ( 10.0 * pi / h );
    n = max ( n, 13 );
    segment_length = n;

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'P07_BOUNDARY_SEGMENT_LENGTH - Fatal error!' );
    fprintf ( 1, '  Illegal SEGMENT_INDEX = %d\n', segment_index );
    error ( 'P07_BOUNDARY_SEGMENT_LENGTH - Fatal error!' );

  end

  return
end


close all; clear all; clc;
rng('default');

A = [3 2; 2 6];
X = [ 2 1 0 4;
     -2 2 0 4];
B = A * X;

solver = SPX_SteepestDescent(A, B);
x = solver.solve();
solver.printResults();


function [oa, pa, K, CM] = USFE_LPP(HSI, Tr, Te, dim, Trees)
[nx,ny,nz]=size(HSI);
data=reshape(HSI,nx*ny,nz);
[mappedX, mapping] = lpp(data, dim);
FE_lpp=reshape(mappedX,nx,ny,dim);
[acc_Mean,acc_std,CM]=RF_ntimes_overal(FE_lpp,Tr,Te,Trees);
pa=acc_Mean(1:dim,1);
oa=acc_Mean(dim+2,1);
K=acc_Mean(dim+3,1);


function [rc,fval,it] = ARjones(ti,xi,rcinit,p)

nobs = length(xi);

opties = optimset('Display','off','TolX',.001/sqrt(nobs),'TolFun',.0001);

[rc_tan,fval,exitflag,output]= fminunc('Jonesfit',tan(.5*pi*rcinit),opties,ti,xi,p);
rc = 2/pi*atan(rc_tan);
it = output.iterations;


% simple demo to convert a polygon mesh into a voxel representation

% code is 99.99% based on
% [1] http://www.mathworks.com/matlabcentral/fileexchange/24086-polygon2voxel
% [2] http://www.mathworks.com/matlabcentral/fileexchange/21044-3d-voxelizer

%   % Compile the c-coded function
%   mex polygon2voxel_double.c -v

load model;

vertices = vertices - repmat(mean(vertices,1),size(vertices,1),1);

FV.faces = faces;
FV.vertices = vertices;

Volume=polygon2voxel(FV,[20 20 20],'auto');

%% visualization 1
figure
[X,Y,Z]=ind2sub(size(Volume),find(Volume(:)));
plot3(X,Y,Z,'.');
axis equal;
xlabel('x');
ylabel('y');
zlabel('z');

%% visualization 2
% 3d pirnter style visualization to add layer by layer

careMask =  imdilate((Volume),ones(2,2,2));
figure,plot3D(careMask,1,'timed', 0.1)
hold on;%plot3D(Volume,1,'b','*')
 saveas(gcf,sprintf('/Users/shurans/Dropbox/ModelNet/ECCV2014_sub/MaskVideo/%05d.png',21 ));
viewA =33:5:176;
for i =1:length(viewA)
    view(viewA(i),28);
    saveas(gcf,sprintf('/Users/shurans/Dropbox/ModelNet/ECCV2014_sub/MaskVideo/%05d.png',21+i ));
end
%%
%{
%% visualization 3
figure
for i=1:size(Volume,1)
    imagesc(squeeze(Volume(i,:,:)));
    axis equal;
    axis tight;
    axis off
    title(i);
    pause(0.1);
end

for i=1:size(Volume,2)
    imagesc(squeeze(Volume(:,i,:)));
    axis equal;
    axis tight;
    axis off
    title(i);
    pause(0.1);
end

for i=1:size(Volume,3)
    imagesc(squeeze(Volume(:,:,i)));
    axis equal;
    axis tight;
    axis off
    title(i);
    pause(0.1);
end
%}


here=pwd;
cd /home/bernard/Data/ABC/apascal
attrs=dlmread('all-perimage.attributes');
classes=dlmread('all.classid');
cd (here);
classes=classes+1;

nClasses=length(unique(classes));
[nInstances, nAttrs]=size(attrs);

classesAttrs=zeros(nClasses*3,nAttrs);
classesCells=cell(1,nClasses);

for i=1:nInstances
    class=classes(i);
    signature=attrs(i,:);
    classesCells{class}=[classesCells{class}; signature];
end
counter=1;
for class=1:nClasses
    classesAttrs(counter,:)=mean(classesCells{class});
    classesAttrs(counter+1,:)=mean(classesCells{class})+0.5*std(classesCells{class});
    classesAttrs(counter+2,:)=mean(classesCells{class})-0.5*std(classesCells{class});
    counter=counter+3;
end


%% Author: epokh
%% Website: www.epokh.org/drupy
%% This software is under GPL

clf
clear
%%Cartesian manipulator with an RPY wrist
px=2.3;
py=1.7;
pz=7.4;

fia=32;
fio=178;
fin=4;
Tcartesian=Tras(px,py,pz);
Torient=RPY(fia,fio,fin);

%%The trasformation matrix is:
Tend=Tcartesian*Torient;
figure(1);
plot3(0,0,0,'r');
plotT(Tend);
title('Cartesian manipulator example');
%%Cylindrical manipulator with an Euler wrist
Tcyl=Tcyl(62,8.2,5.2)*Euler(32,15,17);
figure(2);
plot3(0,0,0,'r');
plotT(Tcyl);
title('Cylindrical manipulator example');
%%Spherical maninupaltor with an Euler an RPY wrist
Tsfer=Tsfer(40,50,10)*RPY(10,0,10);
figure(3);
plot3(0,0,0,'r');
plotT(Tsfer);
title('Spherical manipulator example');

%%Invert the Euler transformation
euwrist=Euler(32,15,17);
[fi1,fio,fi2]=invEuler(euwrist);
fprintf('Invers euler angles %d %d %d \n',fi1,fio,fi2);
rpywrist=RPY(5,10,15);
[fia,fio,fin]=invRPY(rpywrist);
fprintf('Invers rpy angles %d %d %d \n',fia,fio,fin);


% The original source code is from https://github.com/zhengliu6699/imageFusionMetrics/blob/master/metricChenBlum.m
% The interface is modified by the authors of VIFB to integrate it into VIFB.

function res=metricsQcb(img1,img2,fused)

    fused = double(fused);
    img1 = double(img1);
    img2 = double(img2);
    % Get the size of img
    [m,n,b] = size(fused);
    [m1,n1,b1] = size(img2);

    if b == 1
        g = Qcb(img1,img2,fused);
        res = g;
    elseif b1 == 1
        for k = 1 : b
            g(k) = Qcb(img1(:,:,k),img2,fused(:,:,k));
        end
        res = mean(g);
    else
        for k = 1 : b
            g(k) = Qcb(img1(:,:,k),img2(:,:,k),fused(:,:,k));
        end
        res = mean(g);
    end

end

function output = Qcb(im1, im2, fused)

    % function res=metricChenBlum(im1,im2,fused)
    %
    % This function implements Yin Chen's algorithm for fusion metric.
    % im1, im2 -- input images;
    % fused      -- fused image;
    %
    % IMPORTANT: The size of the images need to be 2X.
    % See also: evalu_fusion.m
    %
    % Z. Liu [July 2009]    %

    % Ref: A new automated quality assessment algorithm for image fusion, Image and Vision Computing, 27 (2009) 1421-1432
    % By Yin Chen et al.
    %

    im1 = im2double(im1);
    im2 = im2double(im2);
    fused = im2double(fused);

    im1=normalize1(im1);
    im2=normalize1(im2);
    fused=normalize1(fused);

    %% set up some constant values for experiment

    f0=15.3870;
    f1=1.3456;
    a=0.7622;

    % parameters for local constrast computation
    k=1;
    h=1;
    p=3; %2.4;
    %p=2.4;
    q=2;
    Z=0.0001;
    sigma=2;
    %% caculate the quality Q

    [hang,lie]=size(im1);

    %DoG filter
    %DoG1
    %HH=hang/2; LL=lie/2;
    HH=hang/30; LL=lie/30;

    %DoG2
    %HH=hang/4; LL=lie/4;

    %DoG3
    %HH=hang/8; LL=lie/8;

    [u,v]=freqspace([hang,lie],'meshgrid');
    u=LL*u; v=HH*v;
    r=sqrt(u.^2+v.^2);

    Sd=exp(-(r/f0).^2)-a*exp(-(r/f1).^2);

    % constrast sensitivity filtering
    fused1=ifft2(ifftshift(fftshift(fft2(im1)).*Sd));
    fused2=ifft2(ifftshift(fftshift(fft2(im2)).*Sd));
    ffused=ifft2(ifftshift(fftshift(fft2(fused)).*Sd));

    %--------------------
    %fused1=normalize1(fused1);
    %fused2=normalize1(fused2);
    %ffused=normalize1(ffused);

    % local contrast computation
    % one level of contrast
    G1=gaussian2d(hang,lie,2);
    G2=gaussian2d(hang,lie,4);

    % filtering in frequency domain
    C1=contrast(G1,G2,fused1);
    C1=abs(C1); % I add this. (see your notes)
    C1P=(k*(C1.^p))./(h*(C1.^q)+Z);

    C2=contrast(G1,G2,fused2);
    C2=abs(C2); % I add this.
    C2P=(k*(C2.^p))./(h*(C2.^q)+Z);

    Cf=contrast(G1,G2,ffused);
    Cf=abs(Cf); % I add this.
    CfP=(k*(Cf.^p))./(h*(Cf.^q)+Z);

    % contrast preservation calculation
    mask=(C1P<CfP);
    mask=double(mask);
    Q1F=(C1P./CfP).*mask+(CfP./C1P).*(1-mask);

    mask=(C2P<CfP);
    mask=double(mask);
    Q2F=(C2P./CfP).*mask+(CfP./C2P).*(1-mask);

    % Saliency map generation
    ramda1=(C1P.*C1P)./(C1P.*C1P+C2P.*C2P);
    ramda2=(C2P.*C2P)./(C1P.*C1P+C2P.*C2P);

    % global quality map

    Q=ramda1.*Q1F+ramda2.*Q2F;

    output=mean2(Q);
end

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% sub-functions
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function res=gaussian2d(n1,n2,sigma)

% creat a 2D Gaussian filter in spatial domain
%

% hang (H)-> y; lie (L) -> x

H=floor((n1-1)/2);
L=floor((n2-1)/2);

[x,y]=meshgrid(-15:15,-15:15);
G=exp(-(x.*x+y.*y)/(2*sigma*sigma))/(2*pi*sigma*sigma);

%This is to normalize
%G=G/sum(G(:));
res=G;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function res=contrast(G1,G2,im)

%[hang,lie]=size(im);

%FG1=fft2(G1,hang,lie);
%FG2=fft2(G2,hang,lie);
%fused=fft2(im);

%buff=real(ifft2(FG1.*fused));
%buff1=real(ifft2(FG2.*fused));

buff=filter2(G1,im,'same');
buff1=filter2(G2,im,'same');

res=buff./buff1-1;
end

function RES=normalize1(data)

    % function RES=normalize1(data)
    %
    % This function is to NORMALIZE the data.
    % The data will be in the interval 0-255 (gray level) and pixel value has
    % been rounded to an integer.
    %
    % See also: normalize.m
    %
    % Z. Liu @NRCC (Aug 24, 2009)

    data=double(data);
    da=max(data(:));
    xiao=min(data(:));
    if (da==0 & xiao==0)
        RES=data;
    else
        newdata=(data-xiao)/(da-xiao);
        RES=round(newdata*255);
    end
end


function sys = probability(c)
% PROBABILITY Create basis for chance constraint
%
% EXAMPLE:
% The following example computes the largest value t such that the
% probability that a zero mean unit variance of a Gaussian variable is
% larger than t, is larger than 0.9
%
%  w = sdpvar(1,1);
%  t = sdpvar(1);
%  Model = [probability(a >= t) >= 0.9,uncertain(a,'normal',0,eye(1))];
%  solvesdp(derandomize(Model),-t)

if isa(c,'constraint') | isa(c,'lmi')
    if ~is(c,'elementwise')
        error('Probability constraints only applicable to single elementwise constraints');
    else
        sys.Constraint = c;
        sys = class(sys,'probability');
    end
else
    error('Probability constraints only applicable to single elementwise constraints');
end


function [ftps2] = mGal2ftps2(mGal)
% Convert acceleration from milligals to feet per second-squared
% Chad A. Greene 2012
ftps2 = mGal*3.28083333e-5;
end


function equalizedData = dfe_frac(data, trainingSymbols, nFTaps, nBTaps, SamplesPerSymbol, Constellation, channelDelay)

inputBuffer = zeros(nFTaps,1);
decisionBuffer = zeros(nBTaps,1);

forwardTaps = [1; zeros(nFTaps-1,1)];
backwardTaps = [zeros(nBTaps,1)];

numTrainingSymbols = length(trainingSymbols);

ind = 0;
equalizedData = complex(zeros(length(data),1));
mu = 0.001;

for s=1:SamplesPerSymbol:length(data)
    ind = ind + 1;

    % Add data to buffer
    inputBuffer = [data(s:s+SamplesPerSymbol-1); inputBuffer(1:end-SamplesPerSymbol)];

    % Fill equalizer
    if s<=channelDelay
        continue
    end

    % Apply equalizer
    eqOut = forwardTaps'*inputBuffer - backwardTaps'*decisionBuffer;

    % Make decision
    [~,i] = min(abs(Constellation - eqOut));
    d = Constellation(i);

    % Determine error
    if s<=numTrainingSymbols
        e = trainingSymbols(ind-channelDelay/SamplesPerSymbol) - eqOut;
    else
        e = d - eqOut;
    end
    e = conj(e);

    % Update taps
    forwardTaps  = forwardTaps  + mu*e*inputBuffer;
    %backwardTaps = backwardTaps - mu*e*decisionBuffer;

    % Update decision buffer
    decisionBuffer = [d; decisionBuffer(1:end-1)];

    % Output
    equalizedData(ind) = eqOut;
end
%s = channelDelay/SamplesPerSymbol+1;
%[equalizedData(s:s+100-1), trainingSymbols(1:100), data(s:s+100-1)]

end


function g = gammaFun(j, k)
%GAMMAFUN   Get a function handle to a gamma function.
%   G = GAMMAFUN(J, K) returns a function handle to the gamma function (J, K).
%
% See also EXPINTEG/GAMMAEVAL.

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

% Trivial case:
if ( j == 0 )
    g = @(z) (exp(k*z) - 1)./z;

% Compute them recursively using the recurrence formula:
else
    g = @(z) 0*z;
    for m = 1:j
        gm = expinteg.gammaFun(j-m, k);
        g = @(z) g(z) + (-1)^(m-1)/m*gm(z);
    end
    if ( j <= k )
        g = @(z) (g(z) - nchoosek(k, j))./z;
    else
        g = @(z) g(z)./z;
    end
end

end


function plot_Maps4(sFilename)
%
%
%
load(sFilename)

%% plot results mean z values
cmin=-4;cmax=4;
figure_w_normalized_uicontrolunits('Name','Z-Value','Position',[100 25 400 400]);
mData=nan(size(median(params.mZ,1, 'omitnan')));
mData=median(params.mZ,1, 'omitnan');
% mData=params.mZ;
pcolor(params.vX,params.vY,...
    reshape(mData,...
    size(params.vY,1),size(params.vX,1)));
h=colorbar;
% set(h,'XAxisLocation','Top','XTick',0.5,'XTickLabel', 'median(z)')
colormap(jet);
xlabel('longitude');
ylabel('latitude');
shading interp;
hold on;plot(params.mCatalog(:,1),params.mCatalog(:,2),'k.','MarkerSize',0.5);
plot_WiemerWyss1994
xlim([-117.1 -115.6]);
ylim([33 35.2]);

% figure_w_normalized_uicontrolunits('Name','Std Z-Value');
% mData=nan(size(mean(params.mZ,'omitnan')));
% mData=std(params.mZ, 'omitnan');
% pcolor(params.vX,params.vY,...
%     reshape(mData,...
%     size(params.vY,1),size(params.vX,1)));
% h=colorbar;
% % set(h,'XAxisLocation','Top','XTick',0.5,'XTickLabel', 'std(z)')
% colormap(jet);
% xlabel('longitude');
% ylabel('latitude');
% shading interp;
% hold on;plot(params.mCatalog(:,1),params.mCatalog(:,2),'k.','MarkerSize',0.5);

%
% figure_w_normalized_uicontrolunits('Name','Z-probability by cross-comparison');
% pcolor(params.vX,params.vY,...
%     reshape(calc_ProbColorbar2Value(1-params.vPcrZ),...
%     size(params.vY,1),size(params.vX,1)));
% plot_ProbColorbar2(cmin, cmax);
% % set(gca,'XAxisLocation','Top','XTick',0.5,'XTickLabel', 'P(z)')
%
% xlabel('longitude');
% ylabel('latitude');
% shading interp;
% hold on;plot(params.mCatalog(:,1),params.mCatalog(:,2),'k.','MarkerSize',0.5);
% % colorbar

figure_w_normalized_uicontrolunits('Name','Z-probability by overlap','Position',[100 25 400 400]);
pcolor(params.vX,params.vY,...
    reshape(calc_ProbColorbar2Value(1-params.vPolZ),...
    size(params.vY,1),size(params.vX,1)));
xlabel('longitude');
ylabel('latitude');
plot_ProbColorbar2(cmin, cmax);
% set(gca,'XAxisLocation','Top','XTick',0.5,'XTickLabel', 'P(z)')
shading interp;
hold on;plot(params.mCatalog(:,1),params.mCatalog(:,2),'k.','MarkerSize',0.5);
plot_WiemerWyss1994
xlim([-117.1 -115.6]);
ylim([33 35.2]);

%% plot results mean B-values
cmin=-4;cmax=4;
% figure_w_normalized_uicontrolunits('Name','Median Beta-Value');
% mData=nan(size(median(-params.mB, 'omitnan')));
% mData=median(params.mB, 'omitnan');
% pcolor(params.vX,params.vY,...
%     reshape(mData,...
%     size(params.vY,1),size(params.vX,1)));
% h=colorbar;
% % set(h,'XAxisLocation','Top','XTick',0.5,'XTickLabel', 'median(1-B)')
% colormap(jet);
% xlabel('longitude');
% ylabel('latitude');
% shading interp;
% hold on;plot(params.mCatalog(:,1),params.mCatalog(:,2),'k.','MarkerSize',0.5);
%
% figure_w_normalized_uicontrolunits('Name','Std Beta-Value');
% mData=nan(size(std(params.mB, 'omitnan')));
% mData=std(params.mB, 'omitnan');
% pcolor(params.vX,params.vY,...
%     reshape(mData,...
%     size(params.vY,1),size(params.vX,1)));
% h=colorbar;
% % set(h,'XAxisLocation','Top','XTick',0.5,'XTickLabel', 'std(1-B)')
% colormap(jet);
% xlabel('longitude');
% ylabel('latitude');
% shading interp;
% hold on;plot(params.mCatalog(:,1),params.mCatalog(:,2),'k.','MarkerSize',0.5);

%
% figure_w_normalized_uicontrolunits('Name','Beta-probability by cross-comparison');
% pcolor(params.vX,params.vY,...
%     reshape(calc_ProbColorbar2Value(params.vPcrB),...
%     size(params.vY,1),size(params.vX,1)));
% plot_ProbColorbar2(cmin, cmax);
% % set(gca,'XAxisLocation','Top','XTick',0.5,'XTickLabel', 'P(B)')
% xlabel('longitude');
% ylabel('latitude');
% shading interp;
% hold on;plot(params.mCatalog(:,1),params.mCatalog(:,2),'k.','MarkerSize',0.5);
% % colorbar

figure_w_normalized_uicontrolunits('Name','Beta-probability by overlap','Position',[100 25 400 400]);
pcolor(params.vX,params.vY,...
    reshape(calc_ProbColorbar2Value(params.vPolB),...
    size(params.vY,1),size(params.vX,1)));
plot_ProbColorbar2(cmin, cmax);
% set(gca,'XAxisLocation','Top','XTick',0.5,'XTickLabel', 'P(B)')
xlabel('longitude');
ylabel('latitude');
shading interp;
hold on;plot(params.mCatalog(:,1),params.mCatalog(:,2),'k.','MarkerSize',0.5);
plot_WiemerWyss1994
xlim([-117.1 -115.6]);
ylim([33 35.2]);

figure_w_normalized_uicontrolunits('Name','Resolution','Position',[100 25 400 400]);
pcolor(params.vX,params.vY,...
    reshape(params.mSamples_,...
    size(params.vY,1),size(params.vX,1)));
h=colorbar;
% set(h,'XAxisLocation','Top','XTick',0.5,'XTickLabel', 'N')
xlabel('longitude');
ylabel('latitude');
shading interp;
hold on;plot(params.mCatalog(:,1),params.mCatalog(:,2),'k.','MarkerSize',0.5);
plot_WiemerWyss1994
xlim([-117.1 -115.6]);
ylim([33 35.2]);

end

function plot_WiemerWyss1994
mLatLon=[[-117.1 33];
    [-115.6 33];
    [-115.6 35.2];
    [-117.1 35.2];
    [-117.1 33]];
hold on;plot(mLatLon(:,1),mLatLon(:,2),'k--');
mLanders=[-116.4 34.3];
hold on;plot(mLanders(1),mLanders(2),'ko','MarkerSize',20)
end


function judge = TestIfSO3(mat)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes mat: A 3x3 matrix.
% Check if mat is close to or on the manifold SO(3).
% Example Inputs:
%
% clear; clc;
% mat = [1.0, 0.0,   0.0;
%        0.0, 0.1, -0.95;
%        0.0, 1.0,   0.1];
% judge = TestIfSO3(mat)
%
% Output:
% dudge =
%     0

judge = norm(DistanceToSO3(mat)) < 1e-3;
end


function [lat, lon, gam, k] = utm_inv(zone, northp, x, y)
%UTM_INV  Forward transverse Mercator projection
%
%   [LAT, LON] = UTM_INV(ZONE, NORTHP, X, Y)
%   [LAT, LON, GAM, K] = UTM_INV(ZONE, NORTHP, X, Y)
%
%   performs the inverse universal transverse Mercator projection of points
%   (X,Y) to (LAT,LON) using ZONE and NORTHP.  X and Y can be scalars or
%   arrays of equal size.  ZONE should be an integer in [1,60] and NORTHP
%   is a logical indicating whether the transformation should use the false
%   northing for the northern (NORTHP = true) or southern (NORTHP = false)
%   hemisphere.  The forward projection is given by UTM_FWD.
%
%   GAM and K give metric properties of the projection at (LAT,LON); GAM is
%   the meridian convergence at the point and K is the scale.
%
%   LAT, LON, GAM are in degrees.  The projected coordinates X, Y are in
%   meters.  K is dimensionless.
%
%   This implementation for the UTM projection is based on the series
%   method described in
%
%     C. F. F. Karney, Transverse Mercator with an accuracy of a few
%     nanometers, J. Geodesy 85(8), 475-485 (Aug. 2011);
%     Addenda: http://geographiclib.sf.net/tm-addenda.html
%
%   This extends the series given by Krueger (1912) to sixth order in the
%   flattening.  This is a substantially better series than that used by
%   the MATLAB mapping toolbox.  In particular the errors in the projection
%   are less than 5 nanometers withing 3900 km of the central meridian (and
%   less than 1 mm within 7600 km of the central meridian).  The mapping
%   can be continued accurately over the poles to the opposite meridian.
%
%   This routine depends on the MATLAB File Exchange package "Geodesics on
%   an ellipsoid of revolution":
%
%     http://www.mathworks.com/matlabcentral/fileexchange/39108
%
%   See also GEODPROJ, UTM_FWD, TRANMERC_INV.

% Copyright (c) Charles Karney (2012) <charles@karney.com>.
%
% This file was distributed with GeographicLib 1.29.

  if nargin < 4, error('Too few input arguments'), end
  lon0 = -183 + 6 * zone; lat0 = 0;
  fe = 500e3; fn = cvmgt(0, 10000e3, logical(northp)); k0 = 0.9996;
  x = (x - fe) / k0; y = (y - fn) / k0;
  [lat, lon, gam, k] = tranmerc_inv(lat0, lon0, x, y);
  k = k * k0;
end


function D = driving_function_mono_nfchoa_ps(x0,xs,f,N,conf)
%DRIVING_FUNCTION_MONO_NFCHOA_PS driving signal for a point source in NFC-HOA
%
%   Usage: D = driving_function_mono_nfchoa_ps(x0,xs,f,N,conf)
%
%   Input parameters:
%       x0          - position of the secondary sources / m [nx3]
%       xs          - position of virtual point source / m [nx3]
%       f           - frequency of the monochromatic source / Hz
%       N           - maximum order of spherical harmonics
%       conf        - configuration struct (see SFS_config)
%
%   Output parameters:
%       D           - driving function signal [nx1]
%
%   See also: driving_function_mono_nfchoa, driving_function_imp_nfchoa_ps

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Checking of input  parameters ==================================
nargmin = 5;
nargmax = 5;
narginchk(nargmin,nargmax);
isargmatrix(x0,xs);
isargpositivescalar(f,N);
isargstruct(conf);

%% ===== Configuration ==================================================
xref = conf.xref;
c = conf.c;
dimension = conf.dimension;
driving_functions = conf.driving_functions;
X0 = conf.secondary_sources.center;

%% ===== Computation ====================================================

% Secondary source positions
x00 = bsxfun(@minus,x0,X0);
[phi0,theta0,r0] = cart2sph(x00(:,1),x00(:,2),x00(:,3));

% Point source
xs0 = bsxfun(@minus,xs,X0);
[phi,theta,r] = cart2sph(xs0(:,1),xs0(:,2),xs0(:,3));

% Wavenumber
omega = 2*pi*f;

% modal window
win = modal_weighting(N,conf);

% Initialize empty driving signal
D = zeros(size(x0,1),1);

if strcmp('2D',dimension)

    % === 2-Dimensional ==================================================

    switch driving_functions
    case 'default'
        % --- SFS Toolbox ------------------------------------------------
        to_be_implemented;
    otherwise
        error(['%s: %s, this type of driving function is not implemented ', ...
            'for a 2D point source.'],upper(mfilename),driving_functions);
    end

elseif strcmp('2.5D',dimension)

    % === 2.5-Dimensional ================================================

    % Reference point
    xref = repmat(xref,[size(x0,1) 1]);

    switch driving_functions
    case 'default'
        % --- SFS Toolbox ------------------------------------------------
        % 2.5D point source
        %
        %                     _N_    (2)
        %               1     \     h|m|(w/c r)
        % D(phi0,w) = ------  /__  ------------- e^(i m (phi0-phi))
        %             2pi r0  m=-N   (2)
        %                           h|m|(w/c r0)
        %
        % https://sfs.rtfd.io/en/3.2/d_nfchoa/#equation-fd-nfchoa-point-25d
        for m=-N:N
            D = D + 1 ./ (2.*pi.*r0) ...
                .* win(abs(m)+1) ...
                .* sphbesselh(abs(m),2,omega./c.*r) ...
                ./ sphbesselh(abs(m),2,omega./c.*r0) ...
                .* exp(1i.*m.*(phi0-phi));
        end
    otherwise
        error(['%s: %s, this type of driving function is not implemented ', ...
            'for a 2.5D point source.'],upper(mfilename),driving_functions);
    end

elseif strcmp('3D',dimension)

    % === 3-Dimensional ==================================================

    switch driving_functions
    case 'default'
        % --- SFS Toolbox ------------------------------------------------
        % 3D point source
        %
        %                              _N_  _n_   (2)
        %                       1      \    \    hn(w/c r)   -m
        % D(theta0,phi0,w) = -------   /__  /__ ----------- Yn(theta,phi) ...
        %                    2pi r0^2  n=0 m=-n   (2)
        %                                        hn(w/c r0)
        %                       m
        %                    x Yn(theta0,phi0)
        %
        % https://sfs.rtfd.io/en/3.2/d_nfchoa/#equation-fd-nfchoa-point-3d
        %
        for n=0:N
            for m=-n:n
                D = D + 1 ./ (2.*pi.*r0.^2) ...
                    .* win(n+1) ...
                    .* sphbesselh(n,2,omega./c.*r) ...
                    ./ sphbesselh(n,2,omega./c.*r0) ...
                    .* sphharmonics(n,-m,theta,phi) ...
                    .* sphharmonics(n,m,theta0,phi0);
            end
        end
    otherwise
        error(['%s: %s, this type of driving function is not implemented ', ...
            'for a 3D point source.'],upper(mfilename),driving_functions);
    end

else
    error('%s: the dimension %s is unknown.',upper(mfilename),dimension);
end


%
% A variational approach to SPCP (Aravkin et al. 2014)
%
% RPCA | SPCP-sum-SPG | Stable PCP-sum solved by Spectral Projected Gradient (Aravkin et al. 2014)
% process_video('RPCA', 'flip-SPCP-sum-SPG', 'dataset/demo.avi', 'output/demo_flip-SPCP-sum-SPG.avi');

alg_path_aux = fullfile(lrs_conf.rpca_path,'SPGL1');
addpath(genpath(alg_path_aux));

nFrames     = size(M,2);
lambda      = 1/sqrt(max(size(M,1),size(M,2)));
L0          = repmat(median(M,2), 1, nFrames);
S0          = M - L0;
epsilon     = 5e-3*norm(M,'fro'); % tolerance for fidelity to data

% Flip-Flop version of SPCP-sum solved by Spectral Projected Gradient
opts = struct('sum',true,'L0',L0,'S0',S0,'max',false,...
  'tau0',3e5,'SPGL1_tol',1e-1,'tol',1e-3);
[L,S] = solver_RPCA_SPGL1(M,lambda,epsilon,[],opts);

rmpath(genpath(alg_path_aux));


classdef CommunityDetection < PROBLEM
% <single> <label> <large/none> <expensive/none>
% The community detection problem with label based encoding
% dataNo --- 1 --- Number of dataset

%------------------------------- Reference --------------------------------
% Y. Tian, S. Yang, and X. Zhang, An evolutionary multiobjective
% optimization based fuzzy method for overlapping community detection, IEEE
% Transactions on Fuzzy Systems, 2020, 28(11): 2841-2855.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB Platform
% for Evolutionary Multi-Objective Optimization [Educational Forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% The datasets are taken from
% http://www-personal.umich.edu/~mejn/netdata/
% No.   Name        Nodes   Edges
% 1     Karate      34      78
% 2     Dolphin     62      159
% 3     Polbook     105     441
% 4     Football    115     613

    properties(Access = private)
        Adj;    % Adjacency matrix of the network
        G;      % The graph object
    end
    methods
    	%% Default settings of the problem
        function Setting(obj)
            % Load data
            dataNo    = obj.ParameterSet(1);
            CallStack = dbstack('-completenames');
            load(fullfile(fileparts(CallStack(1).file),'Dataset_CD.mat'),'Dataset');
            str     = {'Karate','Dolphin','Polbook','Football'};
            obj.Adj = Dataset.(str{dataNo});
            obj.G   = graph(obj.Adj);
            % Parameter setting
            obj.M = 1;
            obj.D = size(obj.Adj,2);
            obj.lower    = 1     + zeros(1,obj.D);
            obj.upper    = obj.D + zeros(1,obj.D);
            obj.encoding = 3     + zeros(1,obj.D);
        end
        %% Repair invalid solutions
        function PopDec = CalDec(obj,PopDec)
            for i = 1 : size(PopDec,1)
                P = zeros(1,obj.D);
                while ~all(P)
                    x = find(~P,1);
                    P(PopDec(i,:)==PopDec(i,x)) = max(P) + 1;
                end
                PopDec(i,:) = P;
            end
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            PopObj = zeros(size(PopDec,1),1);
            for i = 1 : size(PopObj,1)
                PopObj(i) = 1 - Modularity(obj.Adj,Decoding(PopDec(i,:)));
            end
        end
        %% Display a population in the decision space
        function DrawDec(obj,Population)
            [~,best] = min(Population.objs);
            C = Decoding(Population(best).dec);
            h = plot(Draw([]),obj.G,'MarkerSize',6,'EdgeColor',[.3 .3 .3]);
            tempStream = RandStream('mlfg6331_64','Seed',2);
            for i = 1 : length(C)
                highlight(h,C{i},'NodeColor',rand(tempStream,1,3));
            end
        end
    end
end

function Community = Decoding(Dec)
    Community = {};
    while any(Dec)
        current      = find(Dec==Dec(find(Dec,1)));
        Community    = [Community,current];
        Dec(current) = 0;
    end
end

function Q = Modularity(Adj,C)
    Q = 0;
    M = sum(sum(Adj))/2;
    for i = 1 : length(C)
        Q = Q + sum(sum(Adj(C{i},C{i})))/2/M - (sum(sum(Adj(C{i},:)))/2/M)^2;
    end
end


function [Population,Dec,Mask,FrontNo,CrowdDis] = SPEA2_EnvironmentalSelection(Population,Dec,Mask,N)
% The environmental selection of MSKEA

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Lei Chen

    %% Delete duplicated solutions
    [~,uni] = unique(Population.objs,'rows');
    Population = Population(uni);
    Dec        = Dec(uni,:);
    Mask       = Mask(uni,:);
    N          = min(N,length(Population));
    %% Calculate the fitness of each solution

    [FrontNo,MaxFNo] = NDSort(Population.objs,N);
    Next = false(1,length(FrontNo));
    Next(FrontNo<MaxFNo) = true;

    PopObj = Population.objs;
    fmax   = max(PopObj(FrontNo==1,:),[],1);
    fmin   = min(PopObj(FrontNo==1,:),[],1);
    PopObj = (PopObj-repmat(fmin,size(PopObj,1),1))./repmat(fmax-fmin,size(PopObj,1),1);

    %% Environmental selection
    Last = find(FrontNo==MaxFNo);
    del  = Truncation(PopObj(Last,:),length(Last)-N+sum(Next));
    Next(Last(~del)) = true;
    % Population for next generation
    Population = Population(Next);
    Dec        = Dec(Next,:);
    Mask       = Mask(Next,:);
    FrontNo    = FrontNo(Next);
    CrowdDis = CrowdingDistance(Population.objs,FrontNo);
end

function Del = Truncation(PopObj,K)
% Select part of the solutions by truncation

    %% Truncation
    Distance = pdist2(PopObj,PopObj);
    Distance(logical(eye(length(Distance)))) = inf;
    Del = false(1,size(PopObj,1));
    while sum(Del) < K
        Remain   = find(~Del);
        Temp     = sort(Distance(Remain,Remain),2);
        [~,Rank] = sortrows(Temp);
        Del(Remain(Rank(1))) = true;
    end
end