function [mxc,mlat,OUT] = xc_stats(xc,xl,varargin)
% [mxc,mlat,OUT] = xc_stats(xc,xl,varargin)
% calculate t-statistic values for a k x k x n 3-D matrix of
% correlation (xc) and latency (xl) values across n subjects.
%
% tor wager

names = [];
if length(varargin) > 0, names = varargin{1};,end

 if isempty(xl), mlat = [];, end

% ------------------------------------------------------------
% stats on correlation values
% ------------------------------------------------------------

warning off, clear stelatency, clear tlat, clear stecorr, clear tcorr
% standard errors and t-values, element by element

% only do this on the upper triangle, to be faster

for j = 1:size(xc,2)-1
    for k = j + 1 : size(xc,2)

        if ~isempty(xl)
            tmp = squeeze(xl(j,k,:));
            z = tmp;
            [stelatency(j,k),tlat(j,k)] = ste(z);   % t-test on latencies; maybe should be poisson dist?
        end

        tmp = squeeze(xc(j,k,:));
        z = .5 * log( (1+tmp) ./ (1-tmp) );     % Fisher's r-to-z transform
        [stecorr(j,k),tcorr(j,k)] = ste(z);     % correl in rand fx analysis across ss
    end
end
warning on

% makes symmetric matrices from upper tri

if ~isempty(xl), stelatency(end+1,:) = 0; ,end
stecorr(end+1,:) = 0;
tcorr(end+1,:) = 0;
if ~isempty(xl),
    tlat(end+1,:) = 0;
    stelatency = stelatency + stelatency';
end

stecorr = stecorr + stecorr' + Inf * eye(size(stecorr,1));
tcorr = tcorr + tcorr';     % t-scores for significance/reliability of cross-correlations across subjects

if ~isempty(xl)
    tlat = tlat + tlat';        % same for time lags across subjects
end

% group means

if ~isempty(xl)
    mlat = mean(xl,3);          % mean of latency data (cross-lag, group average)
end
mxc = mean(xc,3);           % mean cross-correlations among k regions, group average

% ------------------------------------------------------------
% print and save output
% ------------------------------------------------------------

OUT.desc1 = 'xl is latency mtx for each subject, xc is individual correlation mtx';
if ~isempty(xl), OUT.xl = xl;, end
OUT.xc = xc;
OUT.desc2 = 'mlat and mxc: matrices of means across subjects for latency and correl'
OUT.desc3 = 'tlat/tcorr are t-values across z-transformed correlations';
OUT.mxc = mxc;

if ~isempty(xl)
    OUT.mlat = mlat;
    OUT.tlat = tlat;
    OUT.tcorr = tcorr;
end

% Output
if ~isempty(xl)
    fprintf(1,'Latency\n');
    str = correlation_to_text(mlat,Inf,names);
end

fprintf(1,'correlation T-values, p<.05 corrected, assuming normal distribution of latency diffs\n');
% Alpha correction - bonferroni.
numobs=(size(mxc,1)*(size(mxc,1)-1))/2;
corrp=1-(0.05/ (2 * (   numobs   )));       % 2-tailed corr p
crit_t = tinv_t(corrp,size(xc,1)-3);          % critical t-value for corrected significance
crit_tu = tinv_t(1-(.05/2),size(xc,1)-3);         % critical t-value for uncorrected significance

if ~isempty(xl)
    str = correlation_to_text(tlat,crit_t,names);
end

OUT.crit_t = crit_t;
OUT.crit_tu = crit_tu;
OUT.crit_tdesc = 'crit_t crit t value corrected, _tu uncorrected';
OUT.corrp = corrp;
OUT.corrpdesc = 'critical corrected p-value (bonferroni)';

fprintf(1,'Average correlation across subjects\n');
[str, sigu] = correlation_to_text(mxc,Inf,names);

fprintf(1,'Corr T-values, p<.05 uncorrected, 2-tailed, assuming independent samples over time\n');
[str, sigu] = correlation_to_text(tcorr,crit_tu,names);
OUT.sigmat_uncorrected = sigu;

fprintf(1,'Corr T-values, p<.05 corrected, 2-tailed, assuming independent samples over time\n');
[str, sigmat] = correlation_to_text(tcorr,crit_t,names);

OUT.sigmat = sigmat;

return


function grad = compute_NN_grad(W, X, resp, hout, nonlinearity, nonzero_grad)

% nonlinearity = 1 -- hyperbolic tangent
% nonlinearity = 2 -- logistic sigmoid (subtract 0.5 from last layer's outputs)
% nonlinearity = 3 -- logistic sigmoid + hyperbolic tangent (last layer)

ncases = size(resp{1},2);
if isa(X, 'gsingle')
  gputype = 'gsingle';
  onesncases = gones(ncases,1,gputype);
elseif isa(X, 'gdouble')
  gputype = 'gsingle';
  onesncases = gones(ncases,1,gputype);
else
  onesncases = ones(ncases,1);
end

nlayer = numel(W);

if (nonlinearity == 1)

  if (exist('nonzero_grad', 'var'))
    snonzero_grad = sum(nonzero_grad);
    if (snonzero_grad == 0)
      for i=nlayer:-1:1
	grad{i} = zeros(size(W{i}));
      end
      return;
    end

    for i=nlayer:-1:1
      if (i == nlayer)
	tmp = hout(:, nonzero_grad);
      else
	tmp = (W{i+1}' * backward{i+1});
      end

      if (size(tmp, 1) ~= size(resp{i}, 1))
	tmp = tmp(1:end-1, :);
      end

      backward{i} = tmp .* (1 - resp{i}(:,nonzero_grad).^2);

      if (i-1) >= 1
	if (size(W{i}, 2) == size(resp{i-1}, 1) + 1)
	  grad{i} = backward{i} * [resp{i-1}(:,nonzero_grad)' ones(snonzero_grad, 1)];
	else
	  grad{i} = backward{i} * resp{i-1}(:,nonzero_grad)';
	end
      else
	grad{i} = backward{i} * X(:, nonzero_grad)';
      end
    end
  else
    for i=nlayer:-1:1
      if (i == nlayer)
	tmp = hout;
      else
	tmp = (W{i+1}' * backward{i+1});
      end

      if (size(tmp, 1) ~= size(resp{i}, 1))
	tmp = tmp(1:end-1, :);
      end

      backward{i} = tmp .* (1 - resp{i}.^2);

      if (i-1) >= 1
	if (size(W{i}, 2) == size(resp{i-1}, 1) + 1)
	  grad{i} = backward{i} * [resp{i-1}' onesncases];
	else
	  grad{i} = backward{i} * resp{i-1}';
	end
      else
	grad{i} = backward{i} * X';
      end
    end
  end

elseif (nonlinearity == 2)

  if (exist('nonzero_grad', 'var'))
    snonzero_grad = sum(nonzero_grad);
    if (snonzero_grad == 0)
      for i=nlayer:-1:1
	grad{i} = zeros(size(W{i}));
      end
      return;
    end

    for i=nlayer:-1:1
      if (i == nlayer)
	tmp = hout(:, nonzero_grad);
      else
	tmp = (W{i+1}' * backward{i+1});
      end

      if (size(tmp, 1) ~= size(resp{i}, 1))
	tmp = tmp(1:end-1, :);
      end

      backward{i} = tmp .* (1 - resp{i}(:,nonzero_grad)) .* resp{i}(:,nonzero_grad);

      if (i-1) >= 1
	if (size(W{i}, 2) == size(resp{i-1}, 1) + 1)
	  grad{i} = backward{i} * [resp{i-1}(:,nonzero_grad)' gones(snonzero_grad,1,gputype)];
	else
	  grad{i} = backward{i} * resp{i-1}(:,nonzero_grad)';
	end
      else
	grad{i} = backward{i} * X(:, nonzero_grad)';
      end
    end
  else
    for i=nlayer:-1:1
      if (i == nlayer)
	tmp = hout;
      else
	tmp = (W{i+1}' * backward{i+1});
      end

      if (size(tmp, 1) ~= size(resp{i}, 1))
	tmp = tmp(1:end-1, :);
      end

      backward{i} = tmp .* (1 - resp{i}) .* resp{i};

      if (i-1) >= 1
	if (size(W{i}, 2) == size(resp{i-1}, 1) + 1)
	  grad{i} = backward{i} * [resp{i-1}' onesncases];
	else
	  grad{i} = backward{i} * resp{i-1}';
	end
      else
	grad{i} = backward{i} * X';
      end
    end
  end

elseif (nonlinearity == 3)

  if (exist('nonzero_grad', 'var'))
    snonzero_grad = sum(nonzero_grad);
    if (snonzero_grad == 0)
      for i=nlayer:-1:1
	grad{i} = zeros(size(W{i}));
      end
      return;
    end

    for i=nlayer:-1:1
      if (i == nlayer)
	tmp = hout(:, nonzero_grad);
      else
	tmp = (W{i+1}' * backward{i+1});
      end

      if (size(tmp, 1) ~= size(resp{i}, 1))
	tmp = tmp(1:end-1, :);
      end

      if (i == nlayer)
	backward{i} = tmp .* (1 - resp{i}(:,nonzero_grad)^2);
      else
	backward{i} = tmp .* (1 - resp{i}(:,nonzero_grad)) .* resp{i}(:,nonzero_grad);
      end

      if (i-1) >= 1
	if (size(W{i}, 2) == size(resp{i-1}, 1) + 1)
	  grad{i} = backward{i} * [resp{i-1}(:,nonzero_grad)' gones(snonzero_grad,1,gputype)];
	else
	  grad{i} = backward{i} * resp{i-1}(:,nonzero_grad)';
	end
      else
	grad{i} = backward{i} * X(:, nonzero_grad)';
      end
    end
  else
    for i=nlayer:-1:1
      if (i == nlayer)
	tmp = hout;
      else
	tmp = (W{i+1}' * backward{i+1});
      end

      if (size(tmp, 1) ~= size(resp{i}, 1))
	tmp = tmp(1:end-1, :);
      end

      if (i == nlayer)
	backward{i} = tmp .* (1 - resp{i}.^2);
      else
	backward{i} = tmp .* (1 - resp{i}) .* resp{i};
      end

      if (i-1) >= 1
	if (size(W{i}, 2) == size(resp{i-1}, 1) + 1)
	  grad{i} = backward{i} * [resp{i-1}' onesncases];
	else
	  grad{i} = backward{i} * resp{i-1}';
	end
      else
	grad{i} = backward{i} * X';
      end
    end
  end

end


% GP_COV_SUM - Covariance function as a sum of two covariance functions.
%
% Usage:
%
%   COVFUNC = GP_COV_SUM(COVFUNC1, COVFUNC2)
%
% The returned covariance function is called as
%
%   K = COVFUNC(THETA)
%   [K, DK] = COVFUNC(THETA)
%
% where THETA is a vector of parameters. The parameters of COVFUNC1 and
% COVFUNC2 are concatenated into a single parameter vector.
%
% If no arguments are given, dimensionality information is returned:
%
%   [N_THETA, N1, N2] = COVFUNC()
%
% N_THETA : Number of non-fixed parameters
% N1      : Number of rows in the covariance matrix
% N2      : Number of columns in the covariance matrix
%
% See also GP_COV, GP_COV_PRODUCT.

% Last modified 2010-01-25
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function func = gp_cov_sum(varargin)

func = @get_covariance;

if nargin < 2
  error('Must give at least two covariance functions')
end

covfuncs = varargin ;

% Number of covariance functions
n_funcs = nargin ;

% Number of parameters for each covariance function
n_theta = zeros(n_funcs,1) ;
% Dimensionalities of each covariance function
M = zeros(n_funcs,1) ;
N = zeros(n_funcs,1) ;
% Extract the values from the covariance functions
for i = 1:n_funcs
  [n_theta(i), M(i), N(i)] = varargin{i}();
end

% Indices of the hyperparameters for each covariance function
ind_theta = cell(n_funcs,1);
for i = 1:n_funcs
  ind_theta{i} = (1+sum(n_theta(1:(i-1)))):(sum(n_theta(1:i))) ;
end

%[n_theta1, M1, N1] = covfunc1();
%[n_theta2, M2, N2] = covfunc2();

if ~all(M == M(1)) || ~all(N == N(1))
  error('Can''t sum covariance matrices with different dimensionalities');
end
M = M(1) ;
N = N(1) ;

  function varargout = get_covariance(theta)

  if nargout == 0
    nout = 1 ;
  else
    nout = nargout ;
  end

  varargout = cell(nout,1);

  out = cell(nout,1);
  varargout = cell(nout,1);

  % Return only dimension information if requested
  if nargin == 0
    if nout >= 1
      varargout{1} = sum(n_theta); % number of parameters
      if nout >= 2
        varargout{2} = M; % dimensionalities
        if nout >= 3
          varargout{3} = N; % dimensionalities
        end
      end
    end
    return
  end

  varargout{1} = 0 ;
  varargout{2} = cell(sum(n_theta),1) ;

  for i = 1:n_funcs
    % Compute the covariance matrix
    [out{:}] = covfuncs{i}(theta(ind_theta{i}));
    varargout{1} = varargout{1} + out{1};

    % Compute the derivative
    if nout >= 2
      varargout{2}(ind_theta{i}) = out{2}(:);
    end
  end

  end

end


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


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


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


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


function [ev,ee,ebound,xyp] = q1p0grid(x,y,xy,mv,bound,mbound);
%q1p0grid   Q1-P0 element grid generator
%   [ev,ee,ebound,xyp] = q1p0grid(x,y,xy,mv,bound,mbound);
%   input
%          x          x coordinate vector
%          y          y coordinate vector
%          xy         nodal coordinate vector
%          mv         Q2 macroelement mapping matrix
%          bound      boundary vertex vector
%          mbound     macroelement boundary vertex vector
%   output
%          ev         element vertex matrix
%          ee         element edge connection matrix
%          ebound     element boundary edge matrix
%          xyp        vertex coordinate vector
%
%   IFISS function: DJS; 28 February 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
xx=xy(:,1); yy=xy(:,2); nvtx=length(xx);
adj=sparse(nvtx,nvtx);
mel=length(mv(:,1)); nel=4*mel;
ev=zeros(nel,4);
%
%% loop over macroelements
k=1:mel;
% first element
ke=4*k-3;
ev(ke,1)=mv(k,1);
ev(ke,2)=mv(k,5);
ev(ke,3)=mv(k,9);
ev(ke,4)=mv(k,8);
% second element
ke=4*k-2;
ev(ke,1)=mv(k,5);
ev(ke,2)=mv(k,2);
ev(ke,3)=mv(k,6);
ev(ke,4)=mv(k,9);
% third element
ke=4*k-1;
ev(ke,1)=mv(k,9);
ev(ke,2)=mv(k,6);
ev(ke,3)=mv(k,3);
ev(ke,4)=mv(k,7);
% fourth element
ke=4*k;
ev(ke,1)=mv(k,8);
ev(ke,2)=mv(k,9);
ev(ke,3)=mv(k,7);
ev(ke,4)=mv(k,4);
%
%% define element edges
ect=1;
% bottom boundary edges
k1=find(mbound(:,2)==1)';
for k=mbound(k1)
   ebound(ect,1)=4*k-3; ebound(ect+1,1)=4*k-2;
   ebound(ect,2)=1    ; ebound(ect+1,2)=1;
   ect=ect+2;
end
% right boundary edges
k2=find(mbound(:,2)==2)';
for k=mbound(k2)
   ebound(ect,1)=4*k-2; ebound(ect+1,1)=4*k-1;
   ebound(ect,2)=2    ; ebound(ect+1,2)=2;
   ect=ect+2;
end
% top boundary edges
k3=find(mbound(:,2)==3)';
for k=mbound(k3)
   ebound(ect,1)=4*k-1; ebound(ect+1,1)=4*k;
   ebound(ect,2)=3    ; ebound(ect+1,2)=3;
   ect=ect+2;
end
% left boundary edges
k4=find(mbound(:,2)==4)';
for k=mbound(k4)
   ebound(ect,1)=4*k; ebound(ect+1,1)=4*k-3;
   ebound(ect,2)=4    ; ebound(ect+1,2)=4;
   ect=ect+2;
end
%%
% centroid coordinates
for ielem=1:nel
xc(ielem)=mean(xx(ev(ielem,:))); yc(ielem)=mean(yy(ev(ielem,:)));
end
xyp=[xc',yc'];
%
%% compute edge to edge connection array ee
      np=nel;
% initialise global matrices
      adj = sparse(nvtx,nvtx);
      ee = zeros(nel,4);
%
% evaluate element number on each edge in turn
% and assemble into adjacency matrix
%% nx= 0, ny=-1
		 adj=adj + sparse(ev(:,1),ev(:,2),1:np,nvtx,nvtx);
%% nx= 1, ny= 0
		 adj=adj + sparse(ev(:,2),ev(:,3),1:np,nvtx,nvtx);
%% nx= 0, ny= 1
		 adj=adj + sparse(ev(:,3),ev(:,4),1:np,nvtx,nvtx);
%% nx=-1, ny= 0
		 adj=adj + sparse(ev(:,4),ev(:,1),1:np,nvtx,nvtx);
%
       for el=1:nel
		   [ii,jj]=find(adj==el);
           ee(el,:)=diag(adj(jj,ii))';
		   end
           ee=ee(:,[2,4,3,1]);
%
% plotting of the grid
%
%if mel <=64,
	adj=sparse(nvtx,nvtx);
    for i=1:nel
	adj(ev(i,1),ev(i,2)) =1;
	adj(ev(i,2),ev(i,3)) =1;
	adj(ev(i,3),ev(i,4)) =1;
	adj(ev(i,4),ev(i,1)) =1;
    end
    figure(30)
    gplot(adj,xy,'b')
    axis('square')
    hold on
    adj=sparse(nvtx,nvtx);
    k1=find(ebound(:,2)==1);
    for k=1:length(k1)
    kk=ebound(k1(k));
    adj(ev(kk,1),ev(kk,2))=1;
    end
    k2=find(ebound(:,2)==2);
    for k=1:length(k2)
    kk=ebound(k2(k));
    adj(ev(kk,2),ev(kk,3))=1;
    end
    k3=find(ebound(:,2)==3);
    for k=1:length(k3)
    kk=ebound(k3(k));
    adj(ev(kk,3),ev(kk,4))=1;
    end
    k4=find(ebound(:,2)==4);
    for k=1:length(k4)
    kk=ebound(k4(k));
    adj(ev(kk,4),ev(kk,1))=1;
    end
%   gplot(adj,xy,'r')
%   axis('off')
plot(xy(:,1),xy(:,2),'ro')
xybd=xy(bound,:);
plot(xybd(:,1),xybd(:,2),'ko')
plot(xyp(:,1),xyp(:,2),'k*')
hold off
drawnow
title('Q1-P0 finite element subdivision')
%end
return


function [label, model, L] = vbgm(X, init, prior)
% Perform variational Bayesian inference for Gaussian mixture.
%   X: d x n data matrix
%   init: k (1 x 1) or label (1 x n, 1<=label(i)<=k) or center (d x k)
% Reference: Pattern Recognition and Machine Learning by Christopher M. Bishop (P.474)
% Written by Michael Chen (sth4nth@gmail.com).

fprintf('Variational Bayesian Gaussian mixture: running ... \n');
[d,n] = size(X);
if nargin < 3
    prior.alpha = 1;
    prior.kappa = 1;
    prior.m = mean(X,2);
    prior.v = d+1;
    prior.M = eye(d);   % M = inv(W)
end
tol = 1e-20;
maxiter = 2000;
L = -inf(1,maxiter);
converged = false;
t = 1;

model.R = initialization(X,init);
while  ~converged && t < maxiter
    t = t+1;
    model = vmax(X, model, prior);
    model = vexp(X, model);
    L(t) = vbound(X,model,prior)/n;
    converged = abs(L(t)-L(t-1)) < tol*abs(L(t));
end
L = L(2:t);
label = zeros(1,n);
[~,label(:)] = max(model.R,[],2);
[~,~,label] = unique(label);
if converged
    fprintf('Converged in %d steps.\n',t-1);
else
    fprintf('Not converged in %d steps.\n',maxiter);
end

function R = initialization(X, init)
[d,n] = size(X);
if length(init) == 1  % random initialization
    k = init;
    idx = randsample(n,k);
    m = X(:,idx);
    [~,label] = max(bsxfun(@minus,m'*X,dot(m,m,1)'/2),[],1);
    [u,~,label] = unique(label);
    while k ~= length(u)
        idx = randsample(n,k);
        m = X(:,idx);
        [~,label] = max(bsxfun(@minus,m'*X,dot(m,m,1)'/2),[],1);
        [u,~,label] = unique(label);
    end
    R = full(sparse(1:n,label,1,n,k,n));
elseif size(init,1) == 1 && size(init,2) == n  % initialize with labels
    label = init;
    k = max(label);
    R = full(sparse(1:n,label,1,n,k,n));
elseif size(init,1) == d  %initialize with only centers
    k = size(init,2);
    m = init;
    [~,label] = max(bsxfun(@minus,m'*X,dot(m,m,1)'/2),[],1);
    R = full(sparse(1:n,label,1,n,k,n));
else
    error('ERROR: init is not valid.');
end
% Done
function model = vmax(X, model, prior)
alpha0 = prior.alpha;
kappa0 = prior.kappa;
m0 = prior.m;
v0 = prior.v;
M0 = prior.M;
R = model.R;

nk = sum(R,1); % 10.51
alpha = alpha0+nk; % 10.58
nxbar = X*R;
kappa = kappa0+nk; % 10.60
m = bsxfun(@times,bsxfun(@plus,kappa0*m0,nxbar),1./kappa); % 10.61
v = v0+nk; % 10.63

[d,k] = size(m);
M = zeros(d,d,k);
sqrtR = sqrt(R);

xbar = bsxfun(@times,nxbar,1./nk); % 10.52
xbarm0 = bsxfun(@minus,xbar,m0);
w = (kappa0*nk./(kappa0+nk));
for i = 1:k
    Xs = bsxfun(@times,bsxfun(@minus,X,xbar(:,i)),sqrtR(:,i)');
    xbarm0i = xbarm0(:,i);
    M(:,:,i) = M0+Xs*Xs'+w(i)*(xbarm0i*xbarm0i'); % 10.62
end

model.alpha = alpha;
model.kappa = kappa;
model.m = m;
model.v = v;
model.M = M; % Whishart: M = inv(W)
% Done
function model = vexp(X, model)
alpha = model.alpha; % Dirichlet
kappa = model.kappa;   % Gaussian
m = model.m;         % Gasusian
v = model.v;         % Whishart
M = model.M;         % Whishart: inv(W) = V'*V

n = size(X,2);
[d,k] = size(m);

logW = zeros(1,k);
EQ = zeros(n,k);
for i = 1:k
    U = chol(M(:,:,i));
    logW(i) = -2*sum(log(diag(U)));
    Q = (U'\bsxfun(@minus,X,m(:,i)));
    EQ(:,i) = d/kappa(i)+v(i)*dot(Q,Q,1);    % 10.64
end

ElogLambda = sum(psi(0,bsxfun(@minus,v+1,(1:d)')/2),1)+d*log(2)+logW; % 10.65
Elogpi = psi(0,alpha)-psi(0,sum(alpha)); % 10.66

logRho = (bsxfun(@minus,EQ,2*Elogpi+ElogLambda-d*log(2*pi)))/(-2); % 10.46
logR = bsxfun(@minus,logRho,logsumexp(logRho,2)); % 10.49
R = exp(logR);

model.logR = logR;
model.R = R;
% Done
function L = vbound(X, model, prior)
alpha0 = prior.alpha;
kappa0 = prior.kappa;
m0 = prior.m;
v0 = prior.v;
M0 = prior.M;

alpha = model.alpha; % Dirichlet
kappa = model.kappa;   % Gaussian
m = model.m;         % Gasusian
v = model.v;         % Whishart
M = model.M;         % Whishart: inv(W) = V'*V
R = model.R;
logR = model.logR;

[d,k] = size(m);
nk = sum(R,1); % 10.51

Elogpi = psi(0,alpha)-psi(0,sum(alpha));

Epz = dot(nk,Elogpi);
Eqz = dot(R(:),logR(:));
logCalpha0 = gammaln(k*alpha0)-k*gammaln(alpha0);
Eppi = logCalpha0+(alpha0-1)*sum(Elogpi);
logCalpha = gammaln(sum(alpha))-sum(gammaln(alpha));
Eqpi = dot(alpha-1,Elogpi)+logCalpha;
L = Epz-Eqz+Eppi-Eqpi;

U0 = chol(M0);
sqrtR = sqrt(R);
xbar = bsxfun(@times,X*R,1./nk); % 10.52

logW = zeros(1,k);
trSW = zeros(1,k);
trM0W = zeros(1,k);
xbarmWxbarm = zeros(1,k);
mm0Wmm0 = zeros(1,k);
for i = 1:k
    U = chol(M(:,:,i));
    logW(i) = -2*sum(log(diag(U)));

    Xs = bsxfun(@times,bsxfun(@minus,X,xbar(:,i)),sqrtR(:,i)');
    V = chol(Xs*Xs'/nk(i));
    Q = V/U;
    trSW(i) = dot(Q(:),Q(:));  % equivalent to tr(SW)=trace(S/M)
    Q = U0/U;
    trM0W(i) = dot(Q(:),Q(:));

    q = U'\(xbar(:,i)-m(:,i));
    xbarmWxbarm(i) = dot(q,q);
    q = U'\(m(:,i)-m0);
    mm0Wmm0(i) = dot(q,q);
end

ElogLambda = sum(psi(0,bsxfun(@minus,v+1,(1:d)')/2),1)+d*log(2)+logW; % 10.65
Epmu = sum(d*log(kappa0/(2*pi))+ElogLambda-d*kappa0./kappa-kappa0*(v.*mm0Wmm0))/2;
logB0 = v0*sum(log(diag(U0)))-0.5*v0*d*log(2)-logmvgamma(0.5*v0,d);
EpLambda = k*logB0+0.5*(v0-d-1)*sum(ElogLambda)-0.5*dot(v,trM0W);

Eqmu = 0.5*sum(ElogLambda+d*log(kappa/(2*pi)))-0.5*d*k;
logB =  -v.*(logW+d*log(2))/2-logmvgamma(0.5*v,d);
EqLambda = 0.5*sum((v-d-1).*ElogLambda-v*d)+sum(logB);

EpX = 0.5*dot(nk,ElogLambda-d./kappa-v.*trSW-v.*xbarmWxbarm-d*log(2*pi));

L = L+Epmu-Eqmu+EpLambda-EqLambda+EpX;


function [f,g] = hs71F(x)

f = x(1)*x(4)*sum(x(1:3)) + x(3);

if(nargout > 1)
    g = [ x(1)*x(4) + x(4)*sum(x(1:3))
                    x(1)*x(4)
                    x(1)*x(4) + 1
                    x(1)*sum(x(1:3)) ];
end


 function [kb, alpha, kb_m] = kaiser_bessel(x, J, alpha, kb_m, K_N)
%function [kb, alpha, kb_m] = kaiser_bessel(x, J, alpha, kb_m)
%function [kb, alpha, kb_m] = kaiser_bessel(x, J, 'best', 0, K_N)
%
% generalized Kaiser-Bessel function for x in support [-J/2,J/2]
% shape parameter "alpha" (default 2.34 J)
% order parameter "kb_m" (default 0)
% see (A1) in lewitt:90:mdi, JOSA-A, Oct. 1990
% in
%	x	[M,1]	arguments
% out
%	kb	[M,1]	KB function values, if x is numbers
%			or string for kernel(k,J), if x is 'string'
%			or inline function, if x is 'inline'
%	alpha
%	kb_m
%
% Copyright 2001-3-30, Jeff Fessler, The University of Michigan

% Modification 2002-10-29 by Samuel Matej
% - for Negative & NonInteger kb_m the besseli() function has
%	singular behavior at the boundaries - KB values shooting-up/down
%	(worse for small alpha) leading to unacceptable interpolators
% - for real arguments and higher/reasonable values of alpha the
%	besseli() gives similar values for positive and negative kb_m
%	except close to boundaries - tested for kb_m=-2.35:0.05:2.35
%	(besseli() gives exactly same values for integer +- kb_m)
%	=> besseli(kb_m,...) approximated by besseli(abs(kb_m),...), which
%	behaves well at the boundaries
% WARNING: it is not clear how correct the FT formula (JOSA) is
%	for this approximation (for NonInteger Negative kb_m)
% NOTE: Even for the original KB formula, the JOSA FT formula
%	is derived only for m > -1 !

% if no arguments, make example plots
if nargin < 2
	help(mfilename)
	J = 8; alpha = 2.34 * J;
	x = linspace(-(J+1)/2, (J+1)/2, 1001)';
%	x = linspace(J/2-1/4, J/2+1/4, 1001)';

	mlist = [-4 0 2 7];
	leg = {};
	for ii=1:length(mlist)
		kb_m = mlist(ii);
		yy(:,ii) = kaiser_bessel(x, J, alpha, kb_m);
		func = kaiser_bessel('inline', 0, alpha, kb_m);
		yf = func(x, J);
		if any(yf ~= yy(:,ii)),
		[yf yy(:,ii)]
		error 'bug', end
		leg{ii} = sprintf('m=%d', kb_m);
	end
	yb = kaiser_bessel(x, J, 'best', [], 2);
	plot(	x, yy(:,1), 'c-', x, yy(:,2), 'y-', ...
		x, yy(:,3), 'm-', x, yy(:,4), 'g-', x, yb, 'r--')
	leg{end+1} = 'best';
	axis tight, legend(leg)
%	axisy(0, 0.01)	% to see endpoints
	xlabel \kappa, ylabel F(\kappa)
	title(sprintf('KB functions, J=%g \\alpha=%g', J, alpha) )
return
end

if ~isvar('J'), J = 6; end
if ~isvar('alpha') | isempty('alpha'), alpha = 2.34 * J; end
if ~isvar('kb_m') | isempty('kb_m'), kb_m = 0; end

if ischar(alpha)
	[alpha kb_m] = kaiser_bessel_params(alpha, J, K_N);
end

if ischar(x)
	if ischar(alpha)
		if ~isvar('K_N'), error 'K_N required', end
		kb = 'kaiser_bessel(k, J, ''%s'', [], %g)';
		kb = sprintf(kb, alpha, K_N);
	else
		kernel_string = 'kaiser_bessel(k, J, %g, %g)';
		kb = sprintf(kernel_string, alpha, kb_m);
	end
	if streq(x, 'inline')
		kb = inline(kb, 'k', 'J');
	elseif ~streq(x, 'string')
		error '1st argument must be "inline" or "string"'
	end
return
end

%
% Warn about use of modified formula for negative kb_m
%
if (kb_m < 0) & ((abs(round(kb_m)-kb_m)) > eps)
	persistent warned
	if isempty(warned)	% only print this reminder the first time
		printf('\nWarning: Negative NonInt kb_m=%g in kaiser_bessel()', kb_m)
		printf('	- using modified definition of KB function\n')
		warned = 1;
	end
end

kb_m_bi = abs(kb_m);		% modified "kb_m" as described above
ii = abs(x) < J/2;
f = sqrt(1 - (x(ii)/(J/2)).^2);
denom = besseli(kb_m_bi,alpha);
if ~denom
	printf('m=%g alpha=%g', kb_m, alpha)
end
kb = zeros(size(x));
kb(ii) = f.^kb_m .* besseli(kb_m_bi, alpha*f) / denom;
kb = reale(kb);

%
% optimized shape and order parameters
%
function [alpha, kb_m] = kaiser_bessel_params(alpha, J, K_N)
if streq(alpha, 'best')
	if K_N ~= 2
		warning 'kaiser_bessel optimized only for K/N=2'
		printf('using good defaults: m=0 and alpha = 2.34*J')
		kb_m = 0;
		alpha = 2.34 * J;
	else
		kb_m = 0;	% hardwired, because it was nearly the best!
		try
			s = 'private/kaiser,m=0';
			s = load(s);
			ii = find(J == s.Jlist);
			if isempty(ii)
				ii = imin(abs(J - s.Jlist));
				warning(sprintf('J=%d not found, using %d', ...
					J, s.Jlist(ii)))
			end
			alpha = J * s.abest.zn(ii);
		catch
			warning(['could not open file "' s '" so using default alpha = 2.34 J which should be fine.'])
			alpha = 2.34 * J;
		end
	end
else
	error 'unknown alpha mode'
end


function [x, mx, sx] = standardise(x, dim, lim)

% STANDARDISE computes the zscore of a matrix along dimension dim
% has similar functionality as the stats-toolbox's zscore function
%
% Use as
%   x = standardise(x, dim)
%
% See also ZSCORE

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

if nargin == 1,
  dim = find(size(x)>1,1,'first');
end

if nargin == 3,
  ft_error('third input argument is not used');
end

switch dim
case 1
  n  = size(x,dim);
  mx = mean(x,dim);
  x  = x-mx(ones(1,n),:,:,:,:,:,:,:);
  sx = sqrt(sum(abs(x).^2,dim)./n);
  x  = x./sx(ones(1,n),:,:,:,:,:,:,:);
case 2
  n  = size(x,dim);
  mx = mean(x,dim);
  x  = x-mx(:,ones(1,n),:,:,:,:,:,:);
  sx = sqrt(sum(abs(x).^2,dim)./n);
  x  = x./sx(:,ones(1,n),:,:,:,:,:,:);
case 3
  n  = size(x,dim);
  mx = mean(x,dim);
  x  = x-mx(:,:,ones(1,n),:,:,:,:,:);
  sx = sqrt(sum(abs(x).^2,dim)./n);
  x  = x./sx(:,:,ones(1,n),:,:,:,:,:);
case 4
  n  = size(x,dim);
  mx = mean(x,dim);
  x  = x-mx(:,:,:,ones(1,n),:,:,:,:);
  sx = sqrt(sum(abs(x).^2,dim)./n);
  x  = x./sx(:,:,:,ones(1,n),:,:,:,:);
case 5
  n  = size(x,dim);
  mx = mean(x,dim);
  x  = x-mx(:,:,:,:,ones(1,n),:,:,:);
  sx = sqrt(sum(abs(x).^2,dim)./n);
  x  = x./sx(:,:,:,:,ones(1,n),:,:,:);
case 6
  n  = size(x,dim);
  mx = mean(x,dim);
  x  = x-mx(:,:,:,:,:,ones(1,n),:,:);
  sx = sqrt(sum(abs(x).^2,dim)./n);
  x  = x./sx(:,:,:,:,:,ones(1,n),:,:);
case 7
  n  = size(x,dim);
  mx = mean(x,dim);
  x  = x-mx(:,:,:,:,:,:,ones(1,n),:);
  sx = sqrt(sum(abs(x).^2,dim)./n);
  x  = x./sx(:,:,:,:,:,:,ones(1,n),:);
case 8
  n  = size(x,dim);
  mx = mean(x,dim);
  x  = x-mx(:,:,:,:,:,:,:,ones(1,n));
  sx = sqrt(sum(abs(x).^2,dim)./n);
  x  = x./sx(:,:,:,:,:,:,:,ones(1,n));
otherwise
  ft_error('dim too large, standardise currently supports dimensionality up to 8');
end


 function [err, sn, T1] = nufft1_err_mm(om, N1, J1, K1, type, alpha, beta)
%function [err, sn, T1] = nufft1_err_mm(om, N1, J1, K1, type, alpha, beta)
% Compute worst-case error for each input frequency for min-max 1D NUFFT.
% in:
%	om	[M,1]	digital frequency omega in radians
%	N1		signal length
%	J1		# of neighbors used per frequency location
%	K1		FFT size (should be > N1)
%	type		'sinc' 'diric' 'qr'
%	alpha	[L,1]	Fourier series coefficients of scaling factors
%			trick: or, "sn" if length N1
%	beta		scale gamma=2pi/K by this in Fourier series
%			typically is K/N (me) or 0.5 (Liu)
% out:
%	err	[M,1]	worst-case error over unit-norm signals
%	sn	[N,1]	scaling factors corresponding to alpha,beta
%	T1	[J,J]	T matrix
%
% Copyright 2001-12-7, Jeff Fessler, The University of Michigan

% if no arguments, give an example
if nargin < 4
	help(mfilename)
	N = 100; K = 2*N; gam = 2*pi/K;
	J = 14;
	om = gam * linspace(0,1,101);
	[alpha, beta, ok] = nufft_best_alpha(J, 2, K/N);
	if ~ok, alpha = []; beta = 0.5; end
	errd = nufft1_err_mm(om, N, J, K, 'diric');
	errs = nufft1_err_mm(om, N, J, K, 'sinc');
	errq = nufft1_err_mm(om, N, J, K, 'qr');
	semilogy(om/gam, errs, 'g-x', om/gam, errd, 'y-+', om/gam, errq, 'c-o')
	xlabel '\omega / \gamma', ylabel 'E_{max}(\omega)'
	legend('Tr sinc', 'Tr diric', 'QR approach')
return
end

if ~isvar('type') | isempty(type),	type = 'sinc'; end
if ~isvar('alpha') | isempty(alpha)
	alpha = [1];	% default Fourier series coefficients of scaling factors
end
if ~isvar('beta') | isempty(beta)
	beta = 0.5;	% default is Liu version for now
end

use_qr = logical(0);
if streq(type, 'sinc')
	use_true_diric = logical(0);
elseif streq(type, 'diric')
	use_true_diric = logical(1);
elseif streq(type, 'qr')
	use_qr = logical(1);
else
	error 'unknown type'
end

%
% see if 'best' alpha is desired
%
if ischar(alpha)
	if streq(alpha, 'uniform')
		alpha = [1];
		beta = 0.5;
	else
		if streq(alpha, 'best')
			L = 0;
		elseif streq(alpha, 'best,L=1')
			L = 1;
		elseif streq(alpha, 'best,L=2')
			L = 2;
		else
			error 'unknown alpha argument'
		end
		[alpha, beta, ok] = nufft_best_alpha(J1, L, K1/N1);
		if ~ok
			tmp = 'optimal alpha unknown for J=%d, K/N=%g, L=%d';
			warning(sprintf(tmp, J1, K1/N1, L))
			sn = ones(N1,1);
			err = nan;
			return
		end
	end
end

%
% if requested, return corresponding scaling factors too
%
if length(alpha) == N1	% trick: special way to give explicit sn's
	sn = alpha;
	if ~use_qr, error 'give sn only allowed for QR version', end
elseif nargout > 1 | use_qr
	sn = nufft_scale(N1, K1, alpha, beta);
end

%
% QR approach to error
%
if use_qr
	n = [0:N1-1]' - (N1-1)/2;
	[nn, jj] = ndgrid(n, 1:J1);
	gam1 = 2*pi/K1;
	C = exp(i * gam1 * nn .* jj) / sqrt(N1);
	S = spdiag(sn);
	A = S' * C;
	[Q,R] = qr(S' * C, 0);	% [N,J] compact QR decomposition

	do = col(om - gam1*nufft_offset(om, J1, K1));
	Db = exp(i * n * do') / sqrt(N1);	% [N,M]
	err = Db - Q * (Q' * Db);
	err = sqrt(sum(abs(err).^2,1))';	% [M]
return
end

tol = 0;
T1 = nufft_T(N1, J1, K1, tol, alpha, beta, use_true_diric);	% [J,J]
r1 = nufft_r(om, N1, J1, K1, alpha, beta, use_true_diric);	% [J,M]

%
% worst-case error at each frequency
%
Tr1 = T1 * r1;			% [J,M]
err = sum(conj(r1) .* Tr1).';	% [M,1]
err = min(real(err), 1);
err = sqrt(1 - err);		% caution: this "1 -" may cause numerical error


function [out,Wc,Wo] = hankelsv(SYS)
%HANKELSV Compute Hankel singular values and grammians.
%
% [OUT,Wc,Wo] = HANKELSV(SYS) computes controllability and observability
%    grammians Wc, Wo, and the Hankel singular values OUT of an LTI
%    model SYS (created with either TF, ZPK, SS, or FRD). The model
%    SYS can be either continuous-time or discrete-time. However, only
%    in continuous-time case that SYS is allowed to be unstable.
%    The computed Hankel singular values are sorted in ascending order.
%
%    For unstable continuous-time system, state-space stable/anti-stable
%    decomposition is used instead, and OUT=[OUT_stable;OUT_anti-stable].
%    In addition, Wc={Wc_stable,Wc_anti-stable} and Wo is either.

%    The former version of HKSV employs an obsolete fashion in using the
%    Matlab function GRAM, i.e., instead of using GRAM(SYS,'c') and
%    GRAM(SYS,'o'), it uses GRAM(A,B) and GRAM(A',C'), respectively.
%    This restricts the computation of gramians to continuous-time case,
%    only, since GRAM(A,B) and GRAM(A',C') solve LYAP, not DLYAP.
%
%    This improved version also correct some other bugs in the former
%    version, for example, HKSV does not return gramians when SYS is
%    unstable, but not anti-stable.

%    Developer: Wathanyoo Khaisongkram
%    Date Developed: Oct 20, 2004
%    Email: sunboom15@yahoo.com
%    Improved version of HKSV by R. Y. Chiang & M. G. Safonov March, 1986
% -----------------------------------------------------------------------

SYS=ss(SYS);                            % convert to state-space model

% Discrete-time LTI model
if get(SYS,'Ts')~=0                     % discrete-time LTI model
   if max(abs(pole(SYS))) > 1           % unstable discrete system
      error('For discrete LTI model, system must be stable')
   end
   Wc = gram(SYS,'c');                  % controllability matrix
   Wo = gram(SYS,'o');                  % observability matrix
   out = sqrt(eig(Wc*Wo));              % Hankel singular values
   [no_use,index] = sort(out);          % index for sorting
   out = out(index);                    % sorted singular values
   return                               % end function
end

% Continuous-time LTI model
[A,B,C,D]=ssdata(SYS);                  % extract the system matrices
   mode = eig(A);                       % the eigen values of 'a'
   nrow = size(A,1);                    % the number of rows in 'a'
   nsta = length(find(real(mode) < 0)); % the number of unstable modes
if isequal(nsta,nrow),                  % completely stable
   Wc = gram(SYS,'c');                  % controllability matrix
   Wo = gram(SYS,'o');                  % observability matrix
   out = sqrt(eig(Wc*Wo));              % Hankel singular values
   [no_use,index] = sort(out);          % index for sorting
   out = out(index);                    % sorted singular values
elseif isequal(nsta,0),                 % completely unstable
   SYS=ss(-A,-B,C,D,SYS);               % change the dynamic and the input matrices to '-a' and '-b',
                                        % respectively with all LTI properties inherited from
                                        % original SYS, e.g., discrete or continuous domain.
   Wc = gram(SYS,'c');                  % controllability matrix
   Wo = gram(SYS,'o');                  % observability matrix
   out = sqrt(eig(Wc*Wo));              % Hankel singular values
   [no_use,index] = sort(out);          % index for sorting
   out = out(index);                    % sorted singular values
else, % 0 < nsta < nrow
   [SYSs,SYSu] = stabproj(SYS);         % decompose stable/anti-stable parts
   % Stable part
   Wcs = gram(SYSs,'c');                % controllability matrix
   Wos = gram(SYSs,'o');                % observability matrix
   outl = sqrt(eig(Wcs*Wos));           % Hankel singular values1
   [no_use,index] = sort(outl);         % index for sorting
   outl = outl(index);                  % sorted singular values
   % Anti-stable part
   [Au,Bu,Cu,Du]=ssdata(SYSu);          % obtain the system matrices of SYSu
   SYSu=ss(-Au,-Bu,Cu,Du,SYSu);         % change the dynamic and the input matrices to '-ar' and '-br'.
   Wcu = gram(SYSu,'c');                % controllability matrix
   Wou = gram(SYSu,'o');                % observability matrix
   outr = sqrt(eig(Wcu*Wou));           % Hankel singular values
   [no_use,index] = sort(outr);         % index for sorting
   outr = outr(index);                  % sorted singular values
   out = [outl;outr];                   % gather two cases
   Wc={Wcs,Wcu};                        % controllability gramian
   Wo={Wos,Wou};                        % observability gramian
end

% end of HKSV_MOD


classdef ZXH_CF14 < PROBLEM
% <multi/many> <real> <large/none> <constrained>
% Constrained benchmark MOP proposed by Zhou, Xiang, and He

%------------------------------- Reference --------------------------------
% Y. Zhou, Y. Xiang, and X. He, Constrained multiobjective optimization:
% Test problem construction and performance evaluations, IEEE Transactions
% on Evolutionary Computation, 2021, 25(1): 172-186.
%------------------------------- 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".
%--------------------------------------------------------------------------

    properties
        k;  % Number of constrained variables
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            if isempty(obj.M); obj.M = 2; end
            if isempty(obj.D); obj.D = obj.M+10;  end
            obj.lower    = zeros(1,obj.D) + 1e-10;
            obj.upper    = ones(1,obj.D)  - 1e-10;
            obj.encoding = ones(1,obj.D);
            if obj.M <= 3
                obj.k = obj.M - 1;
            elseif obj.M > 3 && obj.M <= 8
                obj.k = floor(obj.M/2);
            else
                obj.k = 3;
            end
        end
        %% Evaluate multiple solutions
        function Population = Evaluation(obj,varargin)
            PopDec = varargin{1};
            OptX   = 0.2;
            [N,D]  = size(PopDec);
            M      = obj.M;
            % Step 1: Compute cumsum
            Sx = cumsum(PopDec(:,1:M).^2,2,'reverse');
            % Step 2: Compute theta
            THETA = 2/pi*atan(sqrt(Sx(:,2:end))./PopDec(:,1:M-1));
            % Step 3: Calculate Ackley function
            h = 20 - 20 * exp(-0.2 * sqrt(sum((PopDec(:,M+1:end)-OptX).^2,2)/(D-M))) + exp (1) - exp(sum(cos(2 * pi .*(PopDec(:,M+1:end)-OptX)),2)/(D-M));
            % Step 4: Compute T_
            T = (1 - Sx(:,1)).^2 + h;
            % Step 5: Objectives (linear)
            G      = [ones(N,1) cumprod(THETA,2)] .* [1-THETA ones(N,1)];
            PopObj = G .* repmat((1+T),1,M);
            % Step 6: Constraints
            PopCon(:,1) = Sx(:,1) + h - 1;
            PopCon(:,2) = -(Sx(:,1) + h - 1/4);
            for i = 1 : obj.k
                PopCon(:,i+2) = min(min(THETA(:,i)-1/10,4/5-THETA(:,i)),max(2/5-THETA(:,i),THETA(:,i)-7/10));
            end
            Population = SOLUTION(varargin{1},PopObj,PopCon,varargin{2:end});
            obj.FE     = obj.FE + length(Population);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R = UniformPoint(N,obj.M);
            T = zeros(size(R));
            for i = obj.M-1 : -1 : 1
                T(:,i) = R(:,i+1)./R(:,i)./(1-T(:,i+1)+R(:,i+1)./R(:,i));
            end
            THETA = T(:,1:obj.k);
            Valid = all(THETA<=1/10|THETA>=2/5&THETA<=7/10|THETA>=4/5,2);
            R     = R(Valid,:);
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            if obj.M == 2
                R = obj.GetOptimum(100);
            elseif obj.M == 3
                a = linspace(0,1,30)';
                R = {a*a',a*(1-a'),(1-a)*ones(size(a'))};
                T2 = R{3}./R{2}./(1+R{3}./R{2});
                T1 = R{2}./(1-T2);
                THETA = cat(3,T1,T2);
                Valid = all(THETA<=1/10|THETA>=2/5&THETA<=7/10|THETA>=4/5,3);
                R{1}(~Valid) = nan;
            else
                R = [];
            end
        end
    end
end