function [samples, energies, diagn] = metrop(f, x, options, gradf, varargin)
%METROP	Markov Chain Monte Carlo sampling with Metropolis algorithm.
%
%	Description
%	 SAMPLES = METROP(F, X, OPTIONS) uses the Metropolis algorithm to
%	sample from the distribution P ~ EXP(-F), where F is the first
%	argument to METROP.   The Markov chain starts at the point X and each
%	candidate state is picked from a Gaussian proposal distribution and
%	accepted or rejected according to the Metropolis criterion.
%
%	SAMPLES = METROP(F, X, OPTIONS, [], P1, P2, ...) allows additional
%	arguments to be passed to F().  The fourth argument is ignored, but
%	is included for compatibility with HMC and the optimisers.
%
%	[SAMPLES, ENERGIES, DIAGN] = METROP(F, X, OPTIONS) also returns a log
%	of the energy values (i.e. negative log probabilities) for the
%	samples in ENERGIES and DIAGN, a structure containing diagnostic
%	information (position and acceptance threshold) for each step of the
%	chain in DIAGN.POS and DIAGN.ACC respectively.  All candidate states
%	(including rejected ones) are stored in DIAGN.POS.
%
%	S = METROP('STATE') returns a state structure that contains the state
%	of the two random number generators RAND and RANDN. These are
%	contained in fields randstate,  randnstate.
%
%	METROP('STATE', S) resets the state to S.  If S is an integer, then
%	it is passed to RAND and RANDN. If S is a structure returned by
%	METROP('STATE') then it resets the generator to exactly the same
%	state.
%
%	The optional parameters in the OPTIONS vector have the following
%	interpretations.
%
%	OPTIONS(1) is set to 1 to display the energy values and rejection
%	threshold at each step of the Markov chain. If the value is 2, then
%	the position vectors at each step are also displayed.
%
%	OPTIONS(14) is the number of samples retained from the Markov chain;
%	default 100.
%
%	OPTIONS(15) is the number of samples omitted from the start of the
%	chain; default 0.
%
%	OPTIONS(18) is the variance of the proposal distribution; default 1.
%
%	See also
%	HMC
%

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

if nargin <= 2
  if ~strcmp(f, 'state')
    error('Unknown argument to metrop');
  end
  switch nargin
    case 1
      % Return state of sampler
      samples = get_state(f);	% Function defined in this module
      return;
    case 2
      % Set the state of the sampler
      set_state(f, x);		% Function defined in this module
      return;
  end
end

if 0
seed = 42;
randn('state', seed);
rand('state', seed)
end

display = options(1);
if options(14) > 0
  nsamples = options(14);
else
  nsamples = 100;
end
if options(15) >= 0
  nomit = options(15);
else
  nomit = 0;
end
if options(18) > 0.0
  std_dev = sqrt(options(18));
else
  std_dev = 1.0;   % default
end
nparams = length(x);

% Set up string for evaluating potential function.
f = fcnchk(f, length(varargin));

samples = zeros(nsamples, nparams);		% Matrix of returned samples.
if nargout >= 2
  en_save = 1;
  energies = zeros(nsamples, 1);
else
  en_save = 0;
end
if nargout >= 3
  diagnostics = 1;
  diagn_pos = zeros(nsamples, nparams);
  diagn_acc = zeros(nsamples, 1);
else
  diagnostics = 0;
end

% Main loop.
n = - nomit + 1;
Eold = feval(f, x, varargin{:});	% Evaluate starting energy.
nreject = 0;				% Initialise count of rejected states.
while n <= nsamples

  xold = x;
  % Sample a new point from the proposal distribution
  x = xold + randn(1, nparams)*std_dev;
  %fprintf('netlab propose: xold = %5.3f,%5.3f, xnew = %5.3f,%5.3f\n',...
  %	xold(1), xold(2), x(1), x(2));

  % Now apply Metropolis algorithm.
  Enew = feval(f, x, varargin{:});	% Evaluate new energy.
  a = exp(Eold - Enew);			% Acceptance threshold.
  if (diagnostics & n > 0)
    diagn_pos(n,:) = x;
    diagn_acc(n,:) = a;
  end
  if (display > 1)
    fprintf(1, 'New position is\n');
    disp(x);
  end

  r = rand(1);
  %fprintf('netlab: n=%d, a=%f/%f=%5.3f (%5.3f), r=%5.3f\n',...
  %	  n, exp(-Enew), exp(-Eold), a, exp(-Enew)/exp(-Eold), r);
  if a > r	% Accept the new state.
    Eold = Enew;
    if (display > 0)
      fprintf(1, 'Finished step %4d  Threshold: %g\n', n, a);
    end
  else			% Reject the new state
    if n > 0
      nreject = nreject + 1;
    end
    x = xold;	% Reset position
    if (display > 0)
      fprintf(1, '  Sample rejected %4d.  Threshold: %g\n', n, a);
    end
  end
  if n > 0
    samples(n,:) = x;			% Store sample.
    if en_save
      energies(n) = Eold;		% Store energy.
    end
  end
  n = n + 1;
end

if (display > 0)
  fprintf(1, '\nFraction of samples rejected:  %g\n', ...
          nreject/(nsamples));
end

if diagnostics
  diagn.pos = diagn_pos;
  diagn.acc = diagn_acc;
end

% Return complete state of the sampler.
function state = get_state(f)

state.randstate = rand('state');
state.randnstate = randn('state');
return

% Set state of sampler, either from full state, or with an integer
function set_state(f, x)

if isnumeric(x)
  rand('state', x);
  randn('state', x);
else
  if ~isstruct(x)
    error('Second argument to metrop must be number or state structure');
  end
  if (~isfield(x, 'randstate') | ~isfield(x, 'randnstate'))
    error('Second argument to metrop must contain correct fields')
  end
  rand('state', x.randstate);
  randn('state', x.randnstate);
end
return


function fmridat = rescale(fmridat, meth, varargin)
% Rescales data in an fmri_data object
% Data is observations x images, so operating on the columns operates on
% images, and operating on the rows operates on voxels (or variables more
% generally) across images.
%
% :Usage:
% ::
%
%    fmridat = rescale(fmridat, meth)
%
% :Inputs:
%
%   **Methods:**
%     Rescaling of voxels
%     - centervoxels        subtract voxel means
%     - zscorevoxels        subtract voxel means, divide by voxel std. dev
%     - rankvoxels          replace values in each voxel with rank across images
%
%                           *Note: these methods must exclude invalid (0 or
%                           NaN) voxels image-wise. Some images (but not others) in an
%                           object may be missing some voxels.
%
%     Rescaling of images
%     - centerimages        subtract image means
%     - zscoreimages        subtract image means, divide by image std. dev
%
%                           *Note: these methods must exclude invalid (0 or
%                           NaN) voxels image-wise. Some images (but not others) in an
%                           object may be missing some voxels.
%
%     - l2norm_images       divide each image by its l2 norm, multiply by sqrt(n valid voxels)
%     - divide_by_csf_l2norm  divide each image by CSF l2 norm. requires MNI space images for ok results!
%     - rankimages          rank voxels within image;
%     - csf_mean_var        Subtract mean CSF signal from image and divide by CSF variance. (Useful for PE
%                               maps where CSF mean and var should be 0).
%
%     Other procedures
%     - windsorizevoxels    Winsorize extreme values to 3 SD for each image (separately) using trimts
%     - percentchange       Scale each voxel (column) to percent signal change with a mean of 100
%                           based on smoothed mean values across space (cols), using iimg_smooth_3d
%     - tanh                Rescale variables by hyperbolic tangent function
%                           this will shrink the tails (and outliers) towards the mean,
%                           making subsequent algorithms more robust to
%                           outliers. Affects normality of distribution.
%
% Appropriate for multi-session (time series) only:
%     - session_global_percent_change
%     - session_global_z
%     - session_multiplicative
%
% See also fmri_data.preprocess

switch meth

    case 'centervoxels'

        % Consider missing voxels, and exclude case-wise (image-wise)
        ismissing = fmridat.dat == 0 | isnan(fmridat.dat);
        fmridat.dat(ismissing) = NaN;

        m = nanmean(fmridat.dat');
        fmridat.dat = (fmridat.dat' - m)';  % Note: Sizes are wrong without repmat, but Matlab 2020b at least figures this out.

        % Old - this does not handle different missing voxels in different images
        fmridat.dat = scale(fmridat.dat', 1)';

        fmridat.dat(ismissing) = 0;  % Replace with 0 for compatibility with image format

        fmridat.history{end+1} = 'Centered voxels (rows) across images';

    case 'zscorevoxels'

        % Consider missing voxels, and exclude case-wise (image-wise)
        ismissing = fmridat.dat == 0 | isnan(fmridat.dat);
        fmridat.dat(ismissing) = NaN;

        m = nanmean(fmridat.dat');
        s = nanstd(fmridat.dat');
        fmridat.dat = ((fmridat.dat' - m) ./ s)';  % Note: Sizes are wrong without repmat, but Matlab 2020b at least figures this out.

        fmridat.dat(ismissing) = 0;  % Replace with 0 for compatibility with image format

        % Old - this does not handle different missing voxels in different images
        %         fmridat.dat = scale(fmridat.dat')';

        fmridat.history{end+1} = 'Z-scored voxels (rows) across images';

    case 'rankvoxels'
        for i = 1:size(fmridat.dat, 1) % for each voxel

            d = fmridat.dat(i, :)';

            % Consider missing voxels, and exclude case-wise (image-wise)
            ismissing = d == 0 | isnan(d);
            d(ismissing) = NaN;

            if ~all(d == 0)
                fmridat.dat(i, ~ismissing) = rankdata(d(~ismissing))';
            end

        end

        ismissing = fmridat.dat == 0 | isnan(fmridat.dat);
        fmridat.dat(ismissing) = 0; % Replace with 0 for compatibility with image format

        fmridat.history{end+1} = 'Ranked voxels (rows) across images';

    case 'rankimages'
        dat = zeros(size(fmridat.dat));

        parfor i = 1:size(fmridat.dat, 2)

            d = fmridat.dat(:,i);

            % Consider missing voxels, and exclude case-wise (image-wise)
            ismissing = d == 0 | isnan(d);
            d(ismissing) = NaN;

            if ~all(d == 0)
                d(~ismissing, i) = rankdata(d(~ismissing));
            end

            dat(:, i) = d;

        end

        dat(isnan(dat)) = 0; % Replace with 0 for compatibility with image format

        fmridat.dat = dat;

        fmridat.history{end+1} = 'Ranked images (columns) across voxels';

    case 'centerimages'

        % center images (observations)

        % Consider missing voxels, and exclude case-wise (image-wise)
        ismissing = fmridat.dat == 0 | isnan(fmridat.dat);
        fmridat.dat(ismissing) = NaN;

        m = nanmean(fmridat.dat);
        fmridat.dat = (fmridat.dat - m);  % Note: Sizes are wrong without repmat, but Matlab 2020b at least figures this out.

        fmridat.dat(ismissing) = 0;  % Replace with 0 for compatibility with image format

        % Old - this does not handle different missing voxels in different images
        % fmridat.dat = scale(fmridat.dat, 1);

        fmridat.history{end+1} = 'Centered images (columns) across voxels';

    case 'zscoreimages'

        % Consider missing voxels, and exclude case-wise (image-wise)
        ismissing = fmridat.dat == 0 | isnan(fmridat.dat);
        fmridat.dat(ismissing) = NaN;

        m = nanmean(fmridat.dat);
        s = nanstd(fmridat.dat);
        fmridat.dat = (fmridat.dat - m) ./ s;  % Note: Sizes are wrong without repmat, but Matlab 2020b at least figures this out.

        fmridat.dat(ismissing) = 0;  % Replace with 0 for compatibility with image format

        % Old - this does not handle different missing voxels in different images
        %         fmridat.dat = scale(fmridat.dat);

        fmridat.history{end+1} = 'Z-scored each image (columns) across voxels, excluding missing values (0 or NaN) for the image';

    case 'doublecenter'

        % Consider missing voxels, and exclude case-wise (image-wise)
        ismissing = fmridat.dat == 0 | isnan(fmridat.dat);
        fmridat.dat(ismissing) = NaN;

        imagemeans = nanmean(fmridat.dat);
        [v, n] = size(fmridat.dat);
        imagemeanmatrix = repmat(imagemeans, v, 1);
        dat_doublecent = fmridat.dat - imagemeanmatrix;

        voxelmeans = nanmean(dat_doublecent, 2);
        voxelmeanmatrix = repmat(voxelmeans, 1, n);
        dat_doublecent = dat_doublecent - voxelmeanmatrix;
        fmridat.dat = dat_doublecent;

        fmridat.dat(ismissing) = 0;  % Replace with 0 for compatibility with image format

        fmridat.history{end+1} = 'Double-centered data matrix across images and voxels';

    case 'l1norm_images'

        % Vector L1 norm of vector
        % divide by this value to normalize image

%         wh = x ~= 0 & ~isnan(x);  % valid values
%         nvalid = sum(wh);
%         l1norm = double(sum(abs(x(wh))))
%         normalized_x = x .* nvalid ./ l1norm;  % divides each voxel x in image by the mean valid voxel value.

        normfun = @(x) sum(abs(x));

        x = fmridat.dat;

        % Consider missing voxels, and exclude case-wise (image-wise)
        ismissing = fmridat.dat == 0 | isnan(fmridat.dat);
        x(ismissing) = NaN;

        for i = 1:size(x, 2)
            % remove nans, 0s
            xx = x(~ismissing(:, i), i);
            % divides each voxel xx in image by the mean valid voxel value.
            n(i) = normfun(xx);
            xx = xx.* length(xx) ./ n(i);
            x(~ismissing(:, i), i) = xx;
        end

        x(ismissing) = 0;  % Replace with 0 for compatibility with image format

        fmridat.dat = x;

    case 'l2norm_images'

        % Vector L2 norm / sqrt(length) of vector
        % divide by this value to normalize image

        normfun = @(x) sum(x .^ 2) .^ .5;

        x = fmridat.dat;

        % Consider missing voxels, and exclude case-wise (image-wise)
        ismissing = fmridat.dat == 0 | isnan(fmridat.dat);
        x(ismissing) = NaN;

        for i = 1:size(x, 2)

            % remove nans, 0s
            xx = x(~ismissing(:, i), i);
            %             isbad = xx == 0 | isnan(xx);
            %             xx(isbad) = [];

            % divide by sqrt(length) so number of elements will not change scaling
            n(i) = normfun(xx) ./ sqrt(length(xx));

            xx = xx ./ n(i);

            %             x(:, i) = zeroinsert(isbad, xx);

            x(~ismissing(:, i), i) = xx;
        end

        x(ismissing) = 0;  % Replace with 0 for compatibility with image format

        fmridat.dat = x;

    case 'divide_by_csf_l2norm'

        [~, ~, ~, l2norms] = extract_gray_white_csf(fmridat);

        % divide each column image by its respective ventricle l2norm
        fmridat.dat = bsxfun(@rdivide, fmridat.dat, l2norms(:, 3)') ;

    case 'session_global_percent_change'

        nscan = fmridat.images_per_session;  % num images per session
        I = intercept_model(nscan);
        for i = 1:size(I, 2)

            wh = find(I(:, i));
            y = fmridat.dat(:, wh)';   % y is images x voxels
            gm = mean(y); % mean at each voxel, 1 x voxels

            % subtract mean at each vox, divide by global session mean
            y = (y - repmat(gm, size(y, 1), 1)) ./ std(y(:));
            fmridat.dat(:, wh) = y;
        end

    case 'session_spm_style'
        % SPM's default method of global mean scaling
        % useful for replicating SPM analyses or comparing SPM's scaling to
        % other methods.
        % not implemented yet because tor decided to use spm_global on images for comparison; this could be done though...
        % see help spm_global
        %         nscan = fmridat.images_per_session;  % num images per session
        %         I = intercept_model(nscan);
        %         for i = 1:size(I, 2)
        %
        %             wh = find(I(:, i));
        %             y = fmridat.dat(:, wh)';   % y is images x voxels
        %             gm = mean(y); % mean at each voxel, 1 x voxels
        %
        %             % subtract mean at each vox, divide by global session mean
        %             y = (y - repmat(gm, size(y, 1), 1)) ./ std(y(:));
        %             fmridat.dat(:, wh) = y;
        %         end

    case 'session_global_z'

        % scale each session so that global brain mean and global brain std
        % across time are the same for each session

        % underlying model:
        % session-specific shifts in mean signal and scaling exist and are
        % independent

        % minus global (whole-brain) mean / global (whole-brain) std.
        % across time

        nscan = fmridat.images_per_session;  % num images per session
        I = intercept_model(nscan);
        for i = 1:size(I, 2)

            wh = find(I(:, i));
            y = fmridat.dat(:, wh);
            gm = mean(y); % mean at each time point

            % subtract mean at each vox, divide by session global std
            y = (y - repmat(gm, size(y, 1), 1)) ./ std(y(:));
            fmridat.dat(:, wh) = y;
        end

    case 'session_multiplicative'

        % scale - multiplicative
        % underlying model:
        % a is a process constant across time, but different for each voxel
        % (T2 contrast as a function of voxel properties)
        % b is a process constant across voxels, but different at each time
        % point within a session
        % (overall scaling, which varies across time)
        % these interact multiplicatively to create variation in both
        % global mean and std deviation jointly, which is why global mean
        % and global std are intercorrelated correlated.
        %
        % image std scales with b, and so does image mean.
        % model assumes a linear relationship between mean and std with
        % slope = 1

        gm = mean(fmridat.dat, 1); % mean across brain, obs series
        gs = std(fmridat.dat, 1);

        create_figure('global mean vs std', 1, 2);
        plot(gm, gs, 'k.')
        xlabel('Image mean');
        ylabel('Image std');
        title('Mean vs. std, before scaling, each dot is 1 image')
        drawnow

        %         if length(varargin) == 0 || isempty(varargin{1})
        %             error('Must enter number of images in each session as input argument');
        %         end

        if isempty(fmridat.images_per_session)
            fmridat.images_per_session = size(fmridat.dat, 2);
        end

        nscan = fmridat.images_per_session;  % num images per session
        I = intercept_model(nscan);

        for i = 1:size(I, 2)

            wh = find(I(:, i));
            y = fmridat.dat(:, wh);
            y(y < 0) = 0;  % images should be all positive-valued

            a = mean(y')';  % mean across time, for each voxel
            b = mean(y);    % mean across voxels, for each time point

            % ystar is reconstruction based on marginal means
            ystar = a*b ./ (mean(a));

            fmridat.dat(:, wh) = y ./ (ystar + .05*mean(ystar(:)));  % like m-estimator; avoid dividing by zero by adding a constant

            % fmridat.dat(:, wh) = y ./ repmat(b, size(y, 1), 1);  % intensity normalization
        end

        gm = mean(fmridat.dat, 1); % mean across brain, obs series
        gs = std(fmridat.dat, 1);

        subplot(1, 2, 2)
        plot(gm, gs, 'k.')
        xlabel('Image mean');
        ylabel('Image std');
        title('After scaling')
        drawnow

    case 'windsorizevoxels'

        whbad = all(fmridat.dat == 0, 2);
        nok = sum(~whbad);

        fprintf('windsorizing %3.0f voxels to 3 std: %05d', 0);
        for i = 1:nok
            if mod(i, 100) == 0, fprintf('\b\b\b\b\b%05d', i); end
            fmridat.dat(i, :) = trimts(fmridat.dat(i, :)', 3, [])';
        end

        fmridat.history{end+1} = 'Windsorized each voxel data series to 3 sd';

    case 'tanh'
        % rescale variables by hyperbolic tangent function
        % this will shrink the tails (and outliers) towards the mean,
        % making subsequent algorithms more robust to outliers.
        % However, it also truncates and flattens the distribution
        % (non-normal)

        fmridat.dat = tanh(zscore(fmridat.dat'))';

    case 'percentchange'
        % scale each voxel (column) to percent signal change with a mean of 100
        % based on smoothed mean values across space (cols), using iimg_smooth_3d

        m = mean(fmridat.dat',1)'; % mean at each voxel, voxels x 1

        sfwhm = 16;
        ms = iimg_smooth_3d(m, fmridat.volInfo, sfwhm, fmridat.removed_voxels);

        % subtract mean at each vox, divide by global session mean
        fmridat.dat = 100 + 100 .* (fmridat.dat - repmat(m, 1, size(fmridat.dat, 2))) ./ repmat(ms, 1, size(fmridat.dat, 2));

        fmridat.history{end+1} = 'Rescaled to mean 100, voxelwise % signal change with 16 mm fwhm smoothing of divisor mean';

    case 'correly'

        % correl with y
        % provides implicit feature selection when using algorithms that
        % are scale-dependent (e.g., SVM, PCA)

    case 'csf_mean_var'

        [~,~,tissues] = extract_gray_white_csf(fmridat);
        csfStd = nanstd(tissues{3}.dat);
        csfMean = nanmean(tissues{3}.dat);

        fmridat = fmridat.remove_empty;
        dat = fmridat.dat;

        dat = bsxfun(@minus,dat,csfMean);
        dat = bsxfun(@rdivide,dat,csfStd);

        fmridat.dat = dat;
        fmridat = fmridat.replace_empty;

    otherwise
        error('Unknown scaling method.')

end

end


function [fMedianMc, fMeanMc, fMc_org, v1Sigma, fStd_Mc, vMc] = calc_BstMc(mCatalog, fBinning, nSample, nMethod)
% function [fMedianMc, fMeanMc, fMc_org, v1Sigma, fStd_Mc, vMc] = calc_BstMc(mCatalog, fBinning, nSample, nMethod)
%---------------------------------------------------------------------------------------------------------
% Bootstrap EQ catalog and determine Mc choosing a specific method
%
% Incoming variables:
% mCatalog   : EQ catalog
% fBinning   : Magnitude binning interval
% nSample    : Number of bootstrap samples
% nMethod    : Method to determine the magnitude of completeness
%               1: Maximum curvature
%               2: Fixed Mc = minimum magnitude (Mmin)
%               3: Mc90 (90% probability)
%               4: Mc95 (95% probability)
%               5: Best combination (Mc95 - Mc90 - maximum curvature)
%               6: Mc using EMR-method
%               7: Mc due b using Shi & Bolt uncertainty
%               8: Mc due b using bootstrap uncertainty
%               9: Mc due b Cao-criterion
%
% Outgoing variables:
% fMedianMc  : Median (50 percentile) of vMc using nSample bootstraps
% fMeanMc    : Mean of Mc using nSample bootstraps
% fMc_org    : Mc estimate from original dataset
% v1Sigma    : [16 84] percentiles, according to standard deviation for normal distribution
% fStd_Mc    : Second moment of empirical Mc distribution (assuming normal distribution)
% vMc        : Mc estimates from bootstrap samples (empirical distribution)
%
% J. Woessner: woessner@seismo.ifg.ethz.ch
% last update: 28.05.03

% Check input
if nargin == 0, error('No catalog input'); end
if nargin == 1, fBinning = 0.1, nSample = 200, disp('Default Bin size 0.1, Default 200 bootstrap samples');end
if nargin == 2, nSample = 200, disp('Default 200 bootstrap samples'); end
if nargin == 3, nMethod = 6, disp('Default method: EMR-method'); end
if nargin > 4, error('Too many arguments!'); end

% Initialize
vMc = [];

% Get magnitudes
vMags = mCatalog(:,6);

% Create bootstrap samples using bootstrap matlab toolbox
mMag_bstsamp = bootrsp(vMags,nSample);
% Calculate Mc of original catalog
fMc_org = calc_Mc(mCatalog, nMethod);
% Determine Mc uncertainty
for nSamp=1:nSample
    mCatalog(:,6) = mMag_bstsamp(:,nSamp);
    fMc = calc_Mc(mCatalog, nMethod);
    vMc =  [vMc; fMc];
end

% Check for Nan and create output
vSel = isnan(vMc);
vNoNanMc = vMc(~vSel,:);
if ~isempty(vNoNanMc)
    fStd_Mc = calc_StdDev(vNoNanMc);
    fMeanMc = mean(vNoNanMc);
    fMedianMc = median(vNoNanMc);
else
    fStd_Mc = NaN;
    fMeanMc = NaN;
    fMedianMc = NaN;
end

if (~isempty(vNoNanMc) & length(vNoNanMc) > 1)
    v1Sigma = prctile(vNoNanMc,[16 84]);
elseif (~isempty(vNoNanMc)  &&  length(vNoNanMc) == 1)
    v1Sigma = prctile(vNoNanMc,[16 84]);
    v1Sigma = v1Sigma';
else
    v1Sigma = [NaN NaN];
end


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nick.battista@unc.edu
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "HIll+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting %%	lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us %to add a specific muscle model, please let Nick (nickabattista@gmail.com) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: Models the force interaction that is user-defined.
%
%           Note:
%                   1. User has complete control for artibtrary input file
%                      format and number of parameters
%                   2. Those parameters get passed to the matrix called
%                      "general_force"
%                   3. User gets options of including other things into
%                      force function outside of inputted parameters, e.g.,
%                      current_time, dt, previous location of Lag. Pts, etc.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [fx_genForce, fy_genForce] = give_Me_General_User_Defined_Force_Densities(ds,Nb,xLag,yLag,xLag_P,yLag_P,dt,current_time,general_force)

    %
    % INPUTS:
    %         ds: Lagragian spacing (defined by ds = 0.5*dx)
    %         Nb: # of Lagrangian Pts.
    %         xLag: current x-Lagrangian coordinate positions
    %         yLag: current y-Lagrangian coordinate positions
    %         xLag_P: previous x-Lagrangian coordinate positions
    %         yLag_P: previous y-Lagrangian coordinate positions
    %         dt: time-step value
    %         current_time: current time in simulation
    %         general_force: matrix containing all data from input file
    %

    %
    % NOTE: THIS EXAMPLE CREATES A USER-DEFINED FORCE THAT IS JUST AN
    % ORDINARY LINEAR SPRING
    %

    Nsprings = length(general_force(:,1));  % # of Springs
    sp_1 = general_force(:,1);              % Initialize storage for MASTER NODE Spring Connection
    sp_2 = general_force(:,2);              % Initialize storage for SLAVE NODE Spring Connection
    K_Vec = general_force(:,3);             % Stores spring stiffness associated with each spring
    RL_Vec = general_force(:,4);            % Stores spring resting length associated with each spring

    fx = zeros(Nb,1);                 % Initialize storage for x-forces
    fy = fx;                          % Initialize storage for y-forces

    %
    % Loops over all the master-nodes of the springs to compute forces
    %
    for i=1:Nsprings

        id_Master = sp_1(i);          % Master Node index
        id_Slave = sp_2(i);           % Slave Node index
        k_Spring = K_Vec(i);          % Spring stiffness of i-th spring
        L_r = RL_Vec(i);              % Resting length of i-th spring

        dx = xLag(id_Slave) - xLag(id_Master); % x-Distance btwn slave and master node
        dy = yLag(id_Slave) - yLag(id_Master); % y-Distance btwn slave and master node

        sF_x =  k_Spring * ( sqrt( dx^2 + dy^2 ) - L_r ) * ( dx / sqrt(dx^2+dy^2) ); % Compute x-Force
        sF_y =  k_Spring * ( sqrt( dx^2 + dy^2 ) - L_r ) * ( dy / sqrt(dx^2+dy^2) ); % Compute y-Force

        fx(id_Master,1) = fx(id_Master,1) + sF_x;  % Sum total forces for node, i in x-direction (this is MASTER node for this spring)
        fy(id_Master,1) = fy(id_Master,1) + sF_y;  % Sum total forces for node, i in y-direction (this is MASTER node for this spring)

        fx(id_Slave,1) = fx(id_Slave,1) - sF_x;    % Sum total forces for node, i in x-direction (this is SLAVE node for this spring)
        fy(id_Slave,1) = fy(id_Slave,1) - sF_y;    % Sum total forces for node, i in y-direction (this is SLAVE node for this spring)

    end

    % Store forces from artibrary force function onto desired Lagrangian Pts.
    fx_genForce = fx;
    fy_genForce = fy;


function [val,vec,info] = syev(jobz,uplo,a)
%SYEV   Computing eigen information.
%   [VAL,VEC,INFO] = SYEV(JOBZ,UPLO,A) computes eigenvalues and,
%	optionally, eigenvectors of a symmetric matrix A.
%
%	JOBZ allows the user to choose if eigenvectors are to be computed.
%	JOBZ
%          = 'N':  Compute eigenvalues only;
%          = 'V':  Compute eigenvalues and eigenvectors.
%
%	UPLO allows the user to choose which part of the matrix will be referenced.
%	UPLO
%          = 'U':  Upper triangle of A is stored;
%          = 'L':  Lower triangle of A is stored.
%
%   If only eigenvalues are desired, the QR algorithm is used.
%
%	No input parameter are optional. If you want an easy to use interface,
%	see SYEV_DRIVER.

    vec = [];
	val = [];
	info = [];

	if (nargin~=3),
		disp('Wrong number of input parameters.');
		return;
	end;

	    %Do all the boring checking
	if (~isnumeric(a)),
		disp('The matrix is not composed of numeric values.');
		return;
	elseif (isinteger(a)),
		a=double(a);
	end;

    if (size(a,1)~=size(a,2)),
        disp('The matrix must be square.');
        return;
	end;
	n=int32(size(a,1));
	lda=int32(n);

	if (isComplex(a)),
		if (isDouble(a))

			w=zeros(double(n),1);
			work=complex(zeros(1));
			lrwork=int32(max(1,3*n-2));
			rwork=zeros(double(lrwork),1);
			lwork=int32(-1);
			info=int32(-1);
			[jobz, uplo, n, vec, lda, val, work, lwork, rwork,info]=lapack_zheev(jobz, uplo, n, a, lda, w, work, lwork, rwork,info);
			lwork=int32(work(1));
			work=complex(zeros(double(lwork),1));
			[jobz, uplo, n, vec, lda, val, work, lwork, rwork,info]=lapack_zheev(jobz, uplo, n, a, lda, w, work, lwork, rwork,info);
		else
			w=single(zeros(double(n),1));
			work=single(complex(zeros(1,1)));
			work(1)=sqrt(-1)+1;
			lrwork=int32(single(max(1,3*n-2)));
			rwork=single(zeros(double(lrwork),1));
			lwork=int32(single(-1));
			info=int32(single(-1));
			[jobz, uplo, n, vec, lda, val, work, lwork, rwork,info]=lapack_cheev(jobz, uplo, n, a, lda, w, work, lwork, rwork,info);
			lwork=int32(work(1));
			work=single(complex(zeros(double(lwork),1)));
			[jobz, uplo, n, vec, lda, val, work, lwork, rwork,info]=lapack_cheev(jobz, uplo, n, a, lda, w, work, lwork, rwork,info);

		end;
	else
		if (isDouble(a)),
			w=zeros(double(n),1);
			work=zeros(1);
			lwork=int32(-1);
			info=int32(-1);
			[jobz, uplo, n, vec, lda, val, work, lwork, info]=lapack_dsyev(jobz, uplo, n, a, lda, w, work, lwork, info);
			lwork=int32(work(1));
			work=zeros(double(lwork),1);
			[jobz, uplo, n, vec, lda, val, work, lwork, info]=lapack_dsyev(jobz, uplo, n, a, lda, w, work, lwork, info);
		else
			w=single(zeros(double(n),1));
			work=single(zeros(1));
			lwork=int32(-1);
			info=int32(-1);
			[jobz, uplo, n, vec, lda, val, work, lwork, info]=lapack_ssyev(jobz, uplo, n, a, lda, w, work, lwork, info);
			lwork=int32(work(1));
			work=single(zeros(double(lwork),1));
			[jobz, uplo, n, vec, lda, val, work, lwork, info]=lapack_ssyev(jobz, uplo, n, a, lda, w, work, lwork, info);
		end;
	end;


clear all, close all, s = 25; randn('seed',s), rand('seed',s)
dev = @(x,z) norm(x(:)-z(:))/max([1,norm(x(:)),norm(z(:))]);

n = 194;
x = 5*rand(n,1); y = sin(x); xs = linspace(0,20,1e4)';

mean = {@meanConst}; hyp.mean = 0.2;
sn = 0.2; hyp.lik = log(sn);
ell = 0.2; sf = 2.3; hyp.cov = [log(ell); log(sf)];
v = 0; cov = {@covPPiso,v};

tic
[nlZ,dnlZ,post] = gp(hyp,[],mean,cov,[],x,y);
t = toc;

tic
  K = sparse(feval(cov{:},hyp.cov,x));
  m = feval(mean{:},hyp.mean,x);
  sn2 = sn*sn;
  L = chol(K/sn2+speye(n))';                                % fast column access
  alpha = L'\(L\((y-m)/sn2));
  nlZs = (y-m)'*alpha/2 + sum(log(diag(L))) + n*log(2*pi*sn2)/2;
  dnlZs = hyp;

  nz = 2*nnz(L)-n; r = zeros(nz,1); c = zeros(nz,1); v = zeros(nz,1);    % alloc
  k = 0; Z = zeros(n,n);
  for i=n:-1:1
    li = L(:,i); dii = li(i); jj = find(li); li = li(jj); nj = numel(jj);
    z = zeros(nj,1);
    for ii=nj:-1:1
      j = jj(ii);
      if nj>1, zij = li(2:nj)'*Z(jj(2:nj),j); else zij = 0; end
      zij = (i==j)/dii^2 - zij/dii;
      Z(i,j) = zij; Z(j,i) = zij;
      z(ii) = zij;
    end
    r(k+1  ) = i; c(k+1  ) = i;        v(k+1  ) = z(1    ); k = k+1;
    idx = 1:nj-1;
    r(k+idx) = i; c(k+idx) = jj(1+idx); v(k+idx) = z(1+idx); k = k+nj-1;
    c(k+idx) = i; r(k+idx) = jj(1+idx); v(k+idx) = z(1+idx); k = k+nj-1;
  end
  Q = sparse(r,c,v/sn^2,n,n);

%   Z = solve_chol(L',eye(n)); Z = Z.*(K>0);
%   [r,c,v] = find(K); for k=1:numel(r), v(k) = Z(r(k),c(k)); end
%   Q = sparse(r,c,v/sn^2,n,n);

  for i = 1:numel(hyp.cov)
    dK = sparse(feval(cov{:},hyp.cov,x,[],i));
    dnlZs.cov(i) = sum(sum(Q.*dK))/2 - (alpha'*dK*alpha)/2;
  end
  dnlZs.lik = sn2*(trace(Q)-alpha'*alpha);
  for i = 1:numel(hyp.mean)
    dnlZs.mean(i) = -feval(mean{:},hyp.mean,x,i)'*alpha;
  end
ts = toc;

dev(post.alpha,alpha)+dev(nlZ,nlZs) + ...
dev(dnlZ.cov,dnlZs.cov)+dev(dnlZ.lik,dnlZs.lik)+dev(dnlZ.mean,dnlZs.mean)
fprintf('times %1.2f, %1.2f\n',t,ts)


classdef matRad_SquaredDeviation < DoseObjectives.matRad_DoseObjective
% matRad_SquaredDeviation Implements a penalized least squares objective
%   See matRad_DoseObjective for interface description
%
% References
%     -
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright 2015 the matRad development team.
%
% This file is part of the matRad project. It is subject to the license
% terms in the LICENSE file found in the top-level directory of this
% distribution and at https://github.com/e0404/matRad/LICENSES.txt. No part
% of the matRad project, including this file, may be copied, modified,
% propagated, or distributed except according to the terms contained in the
% LICENSE file.
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    properties (Constant)
        name = 'Squared Deviation';
        parameterNames = {'d^{ref}'};
        parameterTypes = {'dose'};
    end

    properties
        parameters = {60};
        penalty = 1;
    end

    methods
        function obj = matRad_SquaredDeviation(penalty,dRef)

            %If we have a struct in first argument
            if nargin == 1 && isstruct(penalty)
                inputStruct = penalty;
                initFromStruct = true;
            else
                initFromStruct = false;
                inputStruct = [];
            end

            %Call Superclass Constructor (for struct initialization)
            obj@DoseObjectives.matRad_DoseObjective(inputStruct);

            %now handle initialization from other parameters
            if ~initFromStruct
                if nargin == 2 && isscalar(dRef)
                    obj.parameters{1} = dRef;
                end

                if nargin >= 1 && isscalar(penalty)
                    obj.penalty = penalty;
                end
            end
        end

        %% Calculates the Objective Function value
        function fDose = computeDoseObjectiveFunction(obj,dose)
            % deviation : dose minus prefered dose
            deviation = dose - obj.parameters{1};
            % claculate objective function
            fDose = obj.penalty/numel(dose) * (deviation'*deviation);
        end

        %% Calculates the Objective Function gradient
        function fDoseGrad   = computeDoseObjectiveGradient(obj,dose)
            % deviation : Dose minus prefered dose
            deviation = dose - obj.parameters{1};

            % calculate delta
            fDoseGrad = 2 * obj.penalty/numel(dose) * deviation;
        end
    end

end


function a = propa_no_random ( prob, k, n, key )

%*****************************************************************************80
%
%% PROPA_NO_RANDOM returns a random matrix that does not have property A.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real PROB, the probability that a link between
%    two eligible nodes will be made.
%
%    Input, integer K, the number of illegal links between nodes
%    to make.  The routine will TRY to make this many illegal links.  However,
%    it is obviously possible to make K too big.
%
%    Input, integer N, the order of A.
%
%    Input, integer KEY, a positive value that selects the data.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );
%
%  Assign each index randomly to one of two sets.
%  SET(I) is 0 if I is in set 0, and 1 if it is in set 1.
%
  seed = key;
  [ set, seed ] = sub_random ( n, seed );

  for i = 1 : n
    for j = 1 : n
      if ( set(i) ~= set(j) )
        [ chance, seed ] = r8_uniform_01 ( seed );
        if ( chance <= prob )
          a(i,j) = 1.0;
        end
      end
    end
  end
%
%  Now repeatedly pick a pair of indices, and consider setting the
%  corresponding entry of A to 1.
%
  bad = 0;
  tries = 0;

  while ( 1 )

    tries = tries + 1;

    if ( 1000 < tries )
      break
    end

    if ( k <= bad )
      break
    end

    [ i, seed ] = i4_uniform_ab ( 1, n, seed );
    [ j, seed ] = i4_uniform_ab ( 1, n, seed );

    if ( i == j )
      continue
    end

    if ( set(i) ~= set(j) )
      continue
    end

    if ( a(i,j) ~= 0.0 & a(j,i) ~= 0.0 )
      continue
    end

    if ( a(i,j) == 0.0 )
      a(i,j) = 1.0;
    else
      a(j,i) = 1.0;
    end

    bad = bad + 1;

  end

  return
end


function [response, x, s, f, v, q] = tapas_huge_bold( A, B, C, D, tau, ...
    kappa, epsilon, R, u, L, E_0, r_0, V_0, vartheta_0, alpha, gamma, TR, ...
    TE, dt)
% Integrates the DCM forward equations to generate the predicted fMRI bold
% time series.
%
% INPUTS:
%   A, B, C, D - DCM connectivity matrices.
%   tau        - Venous transit time.
%   kappa      - Decay of vasodilatory signal.
%   epsilon    - Ratio of intra- and extravascular signal.
%   R          - Number of regions.
%   u          - Experimental stimuli.
%   L          - Number of experimental stimuli.
%   E_0        - Resting oxygen extraction fraction.
%   r_0        - Slope of intravascular relaxation rate.
%   V_0        - Resting venous volume.
%   vartheta_0 - Frequency offset at the outer surface of magnetized
%                vessels (Hz).
%   alpha      - Grubb's exponent.
%   gamma      - rate constant of feedback regulation.
%   TR         - Repetition time.
%   TE         - Echo time.
%   dt         - Sampling interval of inputs.
%
% OUTPUTS:
%   response - matrix of predicted response for each region
%                  (column-wise)
%   x        - time series of neuronal states
%   s        - time series of vasodilatory signal
%   f1       - time series of flow
%   v1       - time series of blood volume
%   q1       - time series of deoxyhemoglobin content.
%

%
% REFERENCE:
%   Klaas Enno Stephan, Nikolaus Weiskopf, Peter M. Drysdale, Peter A.
%   Robinson, Karl J. Friston (2007). Comparing hemodynamic models with
%   DCM. NeuroImage, 38: 387-401
%
% https://doi.org/10.1016/j.neuroimage.2007.07.040
%

% Author: Yu Yao (yao@biomed.ee.ethz.ch), Sudhir Shankar Raman
% Copyright (C) 2019 Translational Neuromodeling Unit
%                    Institute for Biomedical Engineering,
%                    University of Zurich and ETH Zurich.
%
% This file is part of TAPAS, which is released under the terms of the GNU
% General Public Licence (GPL), version 3. For further details, see
% <https://www.gnu.org/licenses/>.
%
% This 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 non-infringement.
%
% This software is intended for research only. Do not use for clinical
% purpose. Please note that this toolbox is under active development.
% Considerable changes may occur in future releases. For support please
% refer to:
% https://github.com/translationalneuromodeling/tapas/issues
%

nt = size(u, 1);
rSmp = TR/dt;
C = C'/rSmp;
if isempty(D)
    D = zeros(R, R, R);
end

tau     = 2.*exp(tau);
kappa   = .64.*exp(kappa);
epsilon = exp(epsilon);

% resting oxygen extraction fraction
E_0 = repmat(E_0, 1, R);

k1  = 4.3*vartheta_0*TE*E_0;
k2  = epsilon.*(r_0*E_0*TE);
k3  = 1 - epsilon;

% Integrate the dynamical system
[x,s,f,v,q]  = tapas_huge_int_euler(...
    A',...
    full(u*C),...
    full(u),...
    permute(B,[2 1 3]),...
    permute(D,[2 1 3]),...
    E_0,...
    1/alpha,...
    tau,...
    gamma,...
    kappa,...
    [dt,nt,R,L,0,any(B(:)),any(D(:))]);

% generate the BOLD response
response = V_0*( ...
    bsxfun(@times,k1,...
        (1 - (q(rSmp:rSmp:end,:)))) +...
    bsxfun(@times,k2,...
        (1 - (q(rSmp:rSmp:end,:)./...
        v(rSmp:rSmp:end,:)))) +...
    bsxfun(@times,k3,...
        (1-v(rSmp:rSmp:end,:))));

% demean response
response = bsxfun(@minus, response, mean(response, 1));

end


function [Itheta,SigmaTheta,deltaMuTheta,suffStat] = VBA_Itheta(theta,y,posterior,suffStat,dim,u,options)
% Gauss-Newton update of the evolution parameters

if options.DisplayWin % Display progress
    set(options.display.hm(1),'string',...
        'VB Gauss-Newton on evolution parameters... ');
    set(options.display.hm(2),'string','0%');
    drawnow
end

%  Look-up which evolution parameter to update
indIn = options.params2update.theta;
indInx = options.params2update.x;

% Get precision parameters
alphaHat = posterior.a_alpha./posterior.b_alpha;

% Preallocate intermediate variables
iQx = options.priors.iQx;
Q = options.priors.SigmaTheta(indIn,indIn);
iQ = VBA_inv(Q);
muTheta0 = options.priors.muTheta;
Theta = muTheta0;
Theta(indIn) = theta;
dtheta0 = muTheta0-Theta;
dx = zeros(dim.n,dim.n_t);
div = 0;

%--- Initial condition ---%

% evaluate evolution function at current mode
[fx,dF_dX,dF_dTheta] = VBA_evalFun('f',posterior.muX0,Theta,u(:,1),options,dim,1);

% check infinite precision transition pdf
iQ2 = VBA_inv(iQx{1},indInx{1},'replace');

% posterior covariance matrix terms
d2fdx2 = dF_dTheta*iQ2*dF_dTheta';

% error terms
dx(:,1) = (posterior.muX(:,1) - fx);
dx2 = dx(:,1)'*iQ2*dx(:,1);
ddxdtheta = dF_dTheta*iQ2*dx(:,1);

%--- Loop over time series ---%
for t=1:dim.n_t-1

    % check infinite precision transition pdf
    iQ2 = VBA_inv(iQx{t+1},indInx{t+1},'replace');

    % evaluate evolution function at current mode
    [fx,dF_dX,dF_dTheta] = VBA_evalFun('f',posterior.muX(:,t),Theta,u(:,t+1),options,dim,t+1);

    % posterior covariance matrix terms
    d2fdx2 = d2fdx2 + dF_dTheta*iQ2*dF_dTheta';

    % error terms
    dx(:,t+1) = (posterior.muX(:,t+1) - fx);
    dx2 = dx2 + dx(:,t+1)'*iQ2*dx(:,t+1);
    ddxdtheta = ddxdtheta + dF_dTheta*iQ2*dx(:,t+1);

    % Display progress
    if options.DisplayWin && mod(t,dim.n_t./10) < 1
        set(options.display.hm(2),'string',[num2str(floor(100*t/dim.n_t)),'%']);
        drawnow
    end

    % Accelerate divergent update
    if VBA_isWeird ({dx2, dF_dX, dF_dTheta})
        div = 1;
        break
    end

end

if options.DisplayWin % Display progress
    set(options.display.hm(2),'string','OK');
    drawnow
end

% posterior covariance matrix
iSigmaTheta = iQ + alphaHat.*d2fdx2(indIn,indIn);
SigmaTheta = VBA_inv(iSigmaTheta);

% mode
tmp = iQ*dtheta0(indIn) + alphaHat.*ddxdtheta(indIn);
deltaMuTheta = SigmaTheta*tmp;

% variational energy
Itheta = -0.5.*dtheta0(indIn)'*iQ*dtheta0(indIn) -0.5*alphaHat.*dx2;
if VBA_isWeird ({Itheta, SigmaTheta}) || div
    Itheta = -Inf;
end

% update sufficient statistics
suffStat.Itheta = Itheta;
suffStat.dx = dx;
suffStat.dx2 = dx2;
suffStat.dtheta = dtheta0;
suffStat.div = div;


function [h,p,ci,stats, p_square, t_square, q_square, OUT] = ttest(bs, wh_group, varargin)
%TTEST Conducts a one- or two-sampled ttest between subjects at every node
%
% FDR correction: positive false discovery rate (pFDR) from
%   the p-values P of multiple-hypothesis testing using the procedure
%   described by Storey (2002).
%
%   wh_group: a logical array, n_subjects x 1, that defines the two groups.
%             Results show wh_group == T > wh_group == F
%             If empty, performs one-sample t-test
%
%   n_subjects must be equal to size(bs.connectivity.regions.r, 3). For
%   now, this function defaults to testing the matrices in
%   bs.connectivity.regions.r, but could be expanded later.
%
%   Input:
%       'doplot': show a plot
%       'labels': followed by a 2-element cell array, containing labels for
%                 the top two plots
%
%   Output:
%       [h,p,ci,stats] from matlab's ttest2
%       p_square, t_square: p and t values, in the square format
%       q_square: fdr corrected p-values, from mafdr()
%       OUT: structure of summary stats compatible with plot_correlation_matrix.m
%
% examples:
%
%  [h,p,ci,stats, p_square, t_square, q_square, OUT] = ttest(obj, []);
%  plot_correlation_matrix(OUT, 'nofigure', varargin{:});
%  title('Mean connectivity, q < 0.05 pFDR thresholded');
%
%   Yoni Ashar, March 2020

% Programmers' notes:
% Tor Wager updated Dec 2020 to add one-sample t-test

% vectorize. mat is then subjects x nodes in lower triangle

if isempty(which('mafdr')), error('Sorry, you need the Matlab bioinformatics toolbox on your Matlab path to use the Storey 2002 mafdr function called here.'); end

if nargin < 2
    twosample = false;
else
    twosample = ~isempty(wh_group);
end

if twosample && ~islogical(wh_group), error('wh_group must be a logical array'), end

doplot = 0;
labels = {'Mean r, Group == 1', 'Mean r, Group==0'};

for i=1:length(varargin)

    if ischar(varargin{i})
        switch varargin{i}
            case 'doplot'
                doplot = 1;
            case 'labels'
                labels = varargin{i+1};

        end
    end
end

mat = bs.flatten_conn_matrices();

% test at each node

if twosample
    % -- ttest2 tests columnwise
    [h,p,ci,stats] = ttest2(mat(wh_group, :), mat(~wh_group, :));
else
    [h,p,ci,stats] = ttest(mat);
end

% revert back to square form
t_square = squareform(stats.tstat, 'tomatrix');

p_square = squareform (p, 'tomatrix');
p_square(logical(eye(size(p_square)))) = 1; % set the diagonal p values to be 1

[fdr] = mafdr(p');%, 'BHFDR', true);
q_square = squareform (fdr, 'tomatrix');
q_square(logical(eye(size(q_square)))) = 1; % set the diagonal p values to be 1

if twosample

    OUT = struct('descrip', 'Two-sample t-test; r is connectivity diff group 1 - 2; p field is pFDR q-values', ...
        'r', mean(bs.connectivity.regions.r(:,:,wh_group), 3) - mean(bs.connectivity.regions.r(:,:, ~wh_group), 3), ...
        'p', q_square, 'sig', q_square < 0.05, 'wh_group', wh_group);

else

    OUT = struct('descrip', 'One-sample t-test; r is mean correlation, p field is pFDR q-values', 'r', mean(bs.connectivity.regions.r, 3), 'p', q_square, 'sig', q_square < 0.05);

end

if doplot

    if twosample

        create_figure('brainpathways multi two-sample t-test', 2, 2)
        mat = bs.connectivity.regions.r;
        imagesc(mean(mat(:,:,wh_group), 3)); colorbar, title(labels{1})
        subplot(2,2,2)
        imagesc(mean(mat(:,:,~wh_group), 3)); colorbar, title(labels{2})
        subplot(2,2,3)
        imagesc(t_square); colorbar, title('T-statistic: group difference')
        subplot(2,2,4)
        imagesc(q_square); colorbar, title('FDR corrected p values: group difference')

    else

        plot_correlation_matrix(OUT, 'nofigure', varargin{:});

        title('Mean connectivity, q < 0.05 pFDR thresholded');

%         create_figure('brainpathways multi one-sample t-test', 1, 3)
%         mat = bs.connectivity.regions.r;
%         m = mean(mat, 3);
%
%         imagesc(m);
%         colorbar, title('Mean correlation')
%         subplot(1, 3, 2)
%
%         imagesc(t_square); colorbar, title('T-statistic: group')
%
%         subplot(1, 3, 3)
%         mt = m;
%         mt(q_square >= 0.05) = 0;
%         imagesc(mt); colorbar, title('FDR corrected p values: group difference')

        drawnow
    end
end

end


function [x, cost, info, options] = neldermead(problem, x, options)
% Nelder Mead optimization algorithm for derivative-free minimization.
%
% function [x, cost, info, options] = neldermead(problem)
% function [x, cost, info, options] = neldermead(problem, x0)
% function [x, cost, info, options] = neldermead(problem, x0, options)
% function [x, cost, info, options] = neldermead(problem, [], options)
%
% Apply a Nelder-Mead minimization algorithm to the problem defined in
% the problem structure, starting with the population x0 if it is provided
% (otherwise, a random population on the manifold is generated). A
% population is a cell containing points on the manifold. The number of
% elements in the cell must be dim+1, where dim is the dimension of the
% manifold: problem.M.dim().
%
% To specify options whilst not specifying an initial guess, give x0 as []
% (the empty matrix).
%
% This algorithm is a plain adaptation of the Euclidean Nelder-Mead method
% to the Riemannian setting. It comes with no convergence guarantees and
% there is room for improvement. In particular, we compute centroids as
% Karcher means, which seems overly expensive: cheaper forms of
% average-like quantities might work better.
% This solver is useful nonetheless for problems for which no derivatives
% are available, and it may constitute a starting point for the development
% of other Riemannian derivative-free methods.
%
% None of the options are mandatory. See in code for details.
%
% Requires problem.M.pairmean(x, y) to be defined (computes the average
% between two points, x and y).
%
% If options.statsfun is defined, it will receive a cell of points x (the
% current simplex being considered at that iteration), and, if required,
% one store structure corresponding to the best point, x{1}. The points are
% ordered by increasing cost: f(x{1}) <= f(x{2}) <= ... <= f(x{dim+1}),
% where dim = problem.M.dim().
%
% Based on http://www.optimization-online.org/DB_FILE/2007/08/1742.pdf.
%
% See also: manopt/solvers/pso/pso

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:
%
%   Apr.  4, 2015 (NB):
%       Working with the new StoreDB class system.
%       Clarified interactions with statsfun and store.
%
%   Nov. 11, 2016 (NB):
%       If options.verbosity is < 2, prints minimal output.
%
%   Sep.  6, 2018 (NB):
%       Using retraction instead of exponential.

    % Verify that the problem description is sufficient for the solver.
    if ~canGetCost(problem)
        warning('manopt:getCost', ...
                'No cost provided. The algorithm will likely abort.');
    end

    % Dimension of the manifold
    dim = problem.M.dim();

    % Set local defaults here
    localdefaults.storedepth = 0;                     % no need for caching
    localdefaults.maxiter = max(2000, 4*dim);

    localdefaults.reflection = 1;
    localdefaults.expansion = 2;
    localdefaults.contraction = .5;
    % forced to .5 to enable using pairmean functions in manifolds.
    % localdefaults.shrinkage = .5;

    % Merge global and local defaults, then merge w/ user options, if any.
    localdefaults = mergeOptions(getGlobalDefaults(), localdefaults);
    if ~exist('options', 'var') || isempty(options)
        options = struct();
    end
    options = mergeOptions(localdefaults, options);

    % Start timing for initialization.
    timetic = tic();

    % If no initial simplex x is given by the user, generate one at random.
    if ~exist('x', 'var') || isempty(x)
        x = cell(dim+1, 1);
        for i = 1 : dim+1
            x{i} = problem.M.rand();
        end
    end

    % Create a store database and a key for each point.
    storedb = StoreDB(options.storedepth);
    key = cell(size(x));
    for i = 1 : dim+1;
        key{i} = storedb.getNewKey();
    end

    % Compute objective-related quantities for x, and setup a
    % function evaluations counter.
    costs = zeros(dim+1, 1);
    for i = 1 : dim+1
        costs(i) = getCost(problem, x{i}, storedb, key{i});
    end
    costevals = dim+1;

    % Sort simplex points by cost.
    [costs, order] = sort(costs);
    x = x(order);
    key = key(order);

    % Iteration counter.
    % At any point, iter is the number of fully executed iterations so far.
    iter = 0;

    % Save stats in a struct array info, and preallocate.
    % savestats will be called twice for the initial iterate (number 0),
    % which is unfortunate, but not problematic.
    stats = savestats();
    info(1) = stats;
    info(min(10000, options.maxiter+1)).iter = [];

    % Start iterating until stopping criterion triggers.
    while true

        % Make sure we don't use to much memory for the store database.
        storedb.purge();

        stats = savestats();
        info(iter+1) = stats; %#ok<AGROW>
        iter = iter + 1;

        % Start timing this iteration.
        timetic = tic();

        % Sort simplex points by cost.
        [costs, order] = sort(costs);
        x = x(order);
        key = key(order);

        % Log / display iteration information here.
        if options.verbosity >= 2
            fprintf('Cost evals: %7d\tBest cost: %+.4e\t', ...
                    costevals, costs(1));
        end

        % Run standard stopping criterion checks.
        [stop, reason] = stoppingcriterion(problem, x, options, info, iter);

        if stop
            if options.verbosity >= 1
                fprintf([reason '\n']);
            end
            break;
        end

        % Compute a centroid for the dim best points.
        xbar = centroid(problem.M, x(1:end-1));

        % Compute the direction for moving along the axis xbar - worst x.
        vec = problem.M.log(xbar, x{end});

        % Reflection step
        xr = problem.M.retr(xbar, vec, -options.reflection);
        keyr = storedb.getNewKey();
        costr = getCost(problem, xr, storedb, keyr);
        costevals = costevals + 1;

        % If the reflected point is honorable, drop the worst point,
        % replace it by the reflected point and start new iteration.
        if costr >= costs(1) && costr < costs(end-1)
            if options.verbosity >= 2
                fprintf('Reflection\n');
            end
            costs(end) = costr;
            x{end} = xr;
            key{end} = keyr;
            continue;
        end

        % If the reflected point is better than the best point, expand.
        if costr < costs(1)
            xe = problem.M.retr(xbar, vec, -options.expansion);
            keye = storedb.getNewKey();
            coste = getCost(problem, xe, storedb, keye);
            costevals = costevals + 1;
            if coste < costr
                if options.verbosity >= 2
                    fprintf('Expansion\n');
                end
                costs(end) = coste;
                x{end} = xe;
                key{end} = keye;
                continue;
            else
                if options.verbosity >= 2
                    fprintf('Reflection (failed expansion)\n');
                end
                costs(end) = costr;
                x{end} = xr;
                key{end} = keyr;
                continue;
            end
        end

        % If the reflected point is worse than the second to worst point,
        % contract.
        if costr >= costs(end-1)
            if costr < costs(end)
                % do an outside contraction
                xoc = problem.M.retr(xbar, vec, -options.contraction);
                keyoc = storedb.getNewKey();
                costoc = getCost(problem, xoc, storedb, keyoc);
                costevals = costevals + 1;
                if costoc <= costr
                    if options.verbosity >= 2
                        fprintf('Outside contraction\n');
                    end
                    costs(end) = costoc;
                    x{end} = xoc;
                    key{end} = keyoc;
                    continue;
                end
            else
                % do an inside contraction
                xic = problem.M.retr(xbar, vec, options.contraction);
                keyic = storedb.getNewKey();
                costic = getCost(problem, xic, storedb, keyic);
                costevals = costevals + 1;
                if costic <= costs(end)
                    if options.verbosity >= 2
                        fprintf('Inside contraction\n');
                    end
                    costs(end) = costic;
                    x{end} = xic;
                    key{end} = keyic;
                    continue;
                end
            end
        end

        % If we get here, shrink the simplex around x{1}.
        if options.verbosity >= 2
            fprintf('Shrinkage\n');
        end
        for i = 2 : dim+1
            x{i} = problem.M.pairmean(x{1}, x{i});
            key{i} = storedb.getNewKey();
            costs(i) = getCost(problem, x{i}, storedb, key{i});
        end
        costevals = costevals + dim;

    end

    info = info(1:iter);

    % Iteration done: return only the best point found.
    cost = costs(1);
    x = x{1};
    key = key{1};

    % Routine in charge of collecting the current iteration stats.
    function stats = savestats()
        stats.iter = iter;
        stats.cost = costs(1);
        stats.costevals = costevals;
        if iter == 0
            stats.time = toc(timetic);
        else
            stats.time = info(iter).time + toc(timetic);
        end
        % The statsfun can only possibly receive one store structure. We
        % pass the key to the best point, so that the best point's store
        % will be passed. But the whole cell x of points is passed through.
        stats = applyStatsfun(problem, x, storedb, key{1}, options, stats);
    end

end


% Test ICP scan matching algorithm

load corridor.mat;
rob = Robot;
rob.R = [-2; -1; 0];
rob.r = rob.R;
rob.u = [2; pi/1.5]; % Set robot control to move in straight line
sen = Sensor;
sen.range = 30;
sen.fov = 3*pi/2;
axis_size = 5.5;

% obs is an array of objects of type obstacle
scan_data_1 = [];
for i = 1:length(obs)
    scan_data_1_tmp = obs(i).getMeasured(sen, rob);
    scan_data_1 = [scan_data_1 scan_data_1_tmp];
end
scan_data_1 = removeDuplicateLasers(scan_data_1);
scan_data_1 = scan_data_1(2:3, :);
v = repmat(sen.noise, 1, size(scan_data_1, 2)) .* randn(2,size(scan_data_1, 2));
scan_data_1 = scan_data_1 + v;
scan_data_1 = getInvMeasurement(scan_data_1);

p1 = rob.computeTriangle('true');
p1_guess = rob.computeTriangle;

rob.R = rob.move(rob.q .* randn(2,1), 'true');
p2 = rob.computeTriangle('true');

scan_data_2 = [];
for i = 1:length(obs)
    scan_data_2_tmp = obs(i).getMeasured(sen, rob);
    scan_data_2 = [scan_data_2 scan_data_2_tmp];
end
scan_data_2 = removeDuplicateLasers(scan_data_2);
scan_data_2 = scan_data_2(2:3, :);
v = repmat(sen.noise, 1, size(scan_data_2, 2)) .* randn(2,size(scan_data_2, 2));
scan_data_2 = scan_data_2 + v;
scan_data_2 = getInvMeasurement(scan_data_2);

[R, T, corr, icp_var] = doICP(scan_data_1, scan_data_2, 1, rob.u);

da = getPiToPi(asin(R(2,1)));
rob.r(1:2) = transToGlobal(rob.r, T);
rob.r(3) = rob.r(3) + da;
p2_guess = rob.computeTriangle;

% Plot results
figure('color', 'white')
s1 = subplot(1,2,1);
h1 = plot(p1(1,:), p1(2,:), 'r-', p2(1,:), p2(2,:), 'b-', ...
    p2_guess(1,:), p2_guess(2,:), 'm--');
hold on
for i = 1:length(obs)
    obs(i).plot(s1);
end
axis square
axis([-axis_size axis_size -axis_size axis_size])
title(['Before and after wheelchair poses' char(10) 'relative to obstacles'])
legend('pose before', 'pose after', 'estimated pose after', 'obstacles')

% Rot = [cos(pi/2) -sin(pi/2); sin(pi/2) cos(pi/2)];
Rot = 1;
scan_data_2_rot = Rot*scan_data_2;
s2 = subplot(1,2,2);
h2 = plot(scan_data_1(1,:), scan_data_1(2,:), 'r+', ...
    scan_data_2_rot(1,:), scan_data_2_rot(2,:), 'b+');
axis square
axis([-axis_size axis_size -axis_size axis_size])
title(['Before and after scans' char(10) 'in local (Cartesian) frame'])
legend('scan before', 'scan after')

scan_data_2 = transToGlobal(rob.r, scan_data_2);
C = getICPCovariance(icp_var, scan_data_2(:, corr(:, 2)'));
S = 1/trace(C);
disp(['Saliency score: ' num2str(S)])


function Cij = triplet_conditionalgranger(H3,Z3,cmbindx3,H2,Z2,cmbindx2,cmbindx)

% TRIPLET_CONDITIONALGRANGER
%
% Inputs:
%   H3,Z3: transfer matrix, noise covariance for
%     triplets, 3x3(xtriplet)xnfreq
%   H2,Z2: transfer matrix, noise covariance for
%     duplets,  2x2(xnduplet)xnfreq
%   cmbindx: Nx3 indices determining the output, abc = b->a/c

% Copyright (C) 2012, Donders Centre for Cognitive Neuroimaging, Nijmegen, NL
%
% 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$

% the ordering for the triplet indices w.r.t. the duplet indices is crucial
% for the interpretation:
%
% abc - ab gives b->a, conditioned on c. If you want a->b, conditioned on
% c, one should also do a bac - ba computation
%
% Here, the cmbindx are taken, and the appropriately ordered corresponding
% cmbindx2 are generated, if necessary, by swapping the order

if nargin<7 || isempty(cmbindx)
    cmbindx = cmbindx3;
end

% FIXME ensure that the number of frequency bins matches

nfreq = size(H3,4);
Cij = zeros(size(cmbindx,1), nfreq);
for k = 1:size(cmbindx,1)
  % triplet
  sel3     = find(all(ismember(cmbindx3, cmbindx(k,:)),2));
  tmp      = cmbindx3(sel3,:);
  reorder3 = [find(tmp==cmbindx(k,1)) find(tmp==cmbindx(k,2)) find(tmp==cmbindx(k,3))];

  h123 = reshape(H3(reorder3, reorder3, sel3, :), [3 3 nfreq]);
  z123 = Z3(reorder3, reorder3, sel3);

  % normalization matrix 2->1/3
  tmp1 = -z123(2,1)/z123(1,1);
  tmp2 = -z123(3,1)/z123(1,1);
  tmp3 = -(z123(3,2)+tmp2*z123(1,2))/(z123(2,2)+tmp1*z123(1,2));

  p1  = [1    0 0;
         tmp1 1 0;
         tmp2 0 1];
  p2  = [1 0    0;
         0 1    0;
         0 tmp3 1];

  P    = p2*p1;
  invP = inv(P);

  %bivariate system
  sel2     = find(all(ismember(cmbindx2, cmbindx(k,[1 3])),2));
  tmp      = cmbindx2(sel2,:);
  reorder2 = [find(tmp==cmbindx(k,1)) find(tmp==cmbindx(k,3))];

  h2 = reshape(H2(reorder2, reorder2, sel2, :), [2 2 nfreq]);
  z2 = Z2(reorder2, reorder2, sel2);

  Q  = [1                 0;
        -z2(1,2)/z2(1,1) 1];
  numer = z2(1,1);

  invh2   = inv2x2(h2);

  HH   = mtimes3x3(h123, invP(:,:,ones(size(h123,3),1)));
  B    = mtimes2x2(Q(:,:,ones(size(invh2,3),1)), invh2);
  FF   = shiftdim(B(1,1,:).*HH(1,1,:)+B(1,2,:).*HH(3,1,:),1);
  denom = abs(FF.*conj(FF)).*z123(1,1);
  Cij(k,:) = log(numer./denom);

%   % it seems only FF(1,1) is needed (as per the commented for-loop
%   for kk = 1:size(h2,3)
%     HH = h123(:,:,kk)/P;
%     B  = Q/h2(:,:,kk);
%     FF = B(1,:)*HH([1 3],1);
%     denom = abs(FF(1,1)*z123(1,1)*conj(FF(1,1)));
%
%     Cij(k,kk) = log(numer./denom);
%
%   end
%

%   for kk = 1:size(h2,3)
%     HH = h123(:,:,kk)/P;
%     Q  = [1                 0;
%           -z2(1,2)/z2(1,1) 1];
%     B  = Q/h2(:,:,kk);
%     BB = [B(1,1) 0 B(1,2);
%           0      1 0;
%           B(2,1) 0 B(2,2)];
%     FF = BB*HH;
%     numer = z2(1,1);
%     denom = abs(FF(1,1)*z123(1,1)*conj(FF(1,1)));
%
%     Cij(k,kk) = log(numer./denom);
%   end
end


  function [img sino] = fbp_helix_gh(cg, ig, proj, varargin)
%|function [img sino] = fbp_helix_gh(cg, ig, proj, varargin)
%|
%| A single slice rebinning method for cone-beam tomography data
%| collected with a helical source trajectory
%|
%| in
%|	cg			ct_geom()
%|	ig			image_geom()
%|	proj	[ns nt na]	cone-beam projection views (line integrals)
%|
%| option
%|	'short'	1|0		1 for short-scan fan beam (default); 0 for 360
%|	'chat'	1|0		verbosity
%|
%| out
%|	img	[nx ny nz]	reconstructed image
%|	sino	[ns na_rebin nz] fan-beam sinogram
%|
%| See helix_example, for information on how to call
%|
%| Equations used are taken from
%| Noo F, Defrise M, Clackdoyle R and Kudo H; Phys. Med. Biol. 44:561-70, 1999
%| "Single-slice rebinning for helical cone-beam CT"
%| @u doi 10.1088/0031-9155/44/2/019
%|
%| Copyright 2010-07-21, Gregory Handy and Jeff Fessler, University of Michigan

if nargin == 1 && streq(cg, 'test')
	run_mfile_local helix_example
return
end

if nargin < 3, ir_usage, end

warn 'this file is obsolete; use rebin_helix and fbp_helix_stat'

arg.short = true;
arg.chat = false;
arg = vararg_pair(arg, varargin);

[img sino] = fbp_helix_do(proj, ig.mask_or, ...
		ig.z, ig.dz, ig.dx, ig.nx, ig.ny, ig.nz, ...
		cg.na, cg.ns, cg.ds, cg.s, cg.t, cg.nt, cg.dt, ...
		cg.dsd, cg.dso, cg.dod, cg.dfs, cg.offset_s, cg.wt, ...
		cg.orbit, cg.orbit_start, cg.pitch, cg.rmax, ...
		cg.source_zs, arg.short, arg.chat);

end % fbp_helix_gh()

%
% fbp_helix_do()
%
function [img sino] = fbp_helix_do(proj, mask2, ...
		zslice, dz, dx, nx, ny, nz, ...
		na, ns, ds, spoints, tpoints, nt, dt, ...
		dsd, dso, dod, dfs, offset_s, wt, ...
		orbit, orbit_start, pitch, rmax, source_zs, short, chat)

% step 1: z-sampling
na1 = na/orbit*360; % # of views in one turn - should be integer!
if abs(round(na1) - na1) > 1e-4
	keyboard
	fail 'bad na/orbit'
end
betas = mod(deg2rad(orbit_start + 360 * [0:(na1-1)]'/na1), 2*pi);

% check to see if the data is used "fully"
if dz > (dso/dsd * dt) * 1.1
	warn('Full use of CB data is not achieved')
end

myPitch = pitch * nt * dso / dsd * dt;
delta = asin(rmax/dso); % fan angle (one sided)

% dist = d in the paper, and allows for either a short scan,
% or for a full 360 scan for each z-slice
if short == 1
	dist = 0.5 * myPitch * (pi + 2 * delta)/(2*pi);
else
	dist = 0.5 * myPitch;

	max_t = (max(abs(spoints)) + dsd^2) / (dso*dsd) * dist;
	if max_t >= max(abs(tpoints))
		error('CT geometry does not allow for 360 degree rebinning')
	end
end

% step 2: rebin the cone-beam data into fan-beam projections
fan_beam_proj = zeros(ns, na1, nz);

% loop over the different view angles
if chat
	printm('Rebinning step beginning')
end
for ia=0:na-1
	ticker(mfilename, ia, na)
	currentZ = source_zs(ia+1);

	upper_limit = currentZ + dist;
	lower_limit = currentZ - dist;

	% acceptable range of z-slices
	iz_list = find((lower_limit <= zslice) & (zslice <= upper_limit));
	if isempty(iz_list), continue, end

	% loop over the acceptable z-slices for the current view angle
	for iz=iz_list'
		% Calculate the values of t to be used for each s
		deltaZ = zslice(iz) - currentZ;
		tpoints = (((spoints).^2 + dsd^2) / (dso*dsd)) * deltaZ; % [ns]

		t_index = tpoints / dt + wt; % [ns]
		it0 = floor(t_index); % [ns]
		it1 = 1 + it0;
		alpha = t_index - it0; % [ns] for linear interpolation
		it0 = it0 + 1; % matlab indexing
		it1 = it1 + 1; % matlab indexing

		if any(it0 < 1) || any(it0 > nt), fail 'bug', end
		if any(it1 < 1) || any(it1 > nt), fail 'bug', end

		% scaling factor due to different ray lengths
		scale = sqrt(spoints.^2 + dsd^2) ...
			./ sqrt(spoints.^2 + tpoints.^2 + dsd^2); % [ns]

		tmp = sub2ind([ns nt], 1:ns, it0') + ia * ns * nt;
		y0 = proj(tmp');
		tmp = sub2ind([ns nt], 1:ns, it1') + ia * ns * nt;
		y1 = proj(tmp');
		fan_beam_proj(:, mod(ia, na1) + 1, iz) = ...
			scale .* (alpha.*y1 + (1-alpha).*y0);
	end
end

if short
	if chat, printm('Preparing sinogram for FBP2'), end

	fb_orbit_start = zeros(nz,1);
	% beginning orbit for each z-slice
	new_orbit = deg2rad(orbit_start);
	zloc = source_zs(1);
	change_betas = betas(2)-betas(1);
	for iz = 1:nz
		orbitFound = 0;
		while orbitFound == 0
			if zslice(iz) < zloc + dist
				fb_orbit_start(iz) = new_orbit;
				orbitFound = 1;
			else
				new_orbit = new_orbit + change_betas;
				zloc = zloc + (source_zs(2) - source_zs(1));
			end
		end
	end

	% # of angles for each z-slice sinogram
%	newNA = floor(2 * dist / myPitch * na1); % GH
	newNA = ceil(2 * dist / myPitch * na1); % JF
	new_fan_beam_proj = zeros(ns, newNA, nz);

	% delete the empty rows of the sinograms
	for iz = 1:nz
		angle = round((fb_orbit_start(iz)-deg2rad(orbit_start))...
				/ change_betas);
		for ia = 0:newNA-1
			nextAngle = angle + ia;
			new_fan_beam_proj(:, ia+1, iz) = ...
			fan_beam_proj(:, mod(nextAngle, na1)+1, iz);
		end
	end

	% same image geometry as before, except for the z direction
	ig = image_geom('nx', nx, 'ny', ny, 'dx', dx, 'mask', mask2);

	img = nans(nx, ny, nz);

	if chat, printm('Performing the filter back projection.'), end
	% filter back project for each z-slice
	for iz = 1:nz
		sg = sino_geom('fan', 'ns', ns, 'na', newNA, ...
			'ds', ds, 'offset_s', offset_s, ...
			'dsd', dsd, 'dod', dod, 'dfs', dfs, ...
			'orbit', 'short', ...
			'orbit_start', rad2deg(fb_orbit_start(iz)));

		if iz == 1 % trick: Parker weights do not depend on orbit_start
			[parker_wt scale180] = fbp_fan_short_wt(sg);
		end

		% apply Parker weighting and scaling
		tmp = scale180 * parker_wt .* new_fan_beam_proj(:,:,iz);
		geomsino = fbp2(sg, ig);
		img(:,:,iz) = fbp2(tmp, geomsino);
	end

	if nargout >= 2
		sino = new_fan_beam_proj;
	end

else % 360
	ig = image_geom('nx', nx, 'ny', ny, 'dx', dx);
	mask2 = true([nx ny]);
	mask2(end) = 0; % trick: test it
	ig.mask = repmat(mask2, [1 1]);
	clear mask2

	img = zeros(nx,ny,nz);

	if chat, printm('Performing the filter back projection.'), end
	% filter back project for each z-slice
	for iz = 1:nz
		sg = sino_geom('fan', 'ns', ns, 'na', na1, ...
				'ds', ds, 'orbit', 360, ...
				'orbit_start', orbit_start, ...
				'offset_s', offset_s, ...
				'dsd', dsd, 'dod', dod, 'dfs', dfs);
		geomsino = fbp2(sg, ig);
		img(:,:,iz) = fbp2(fan_beam_proj(:,:,iz), geomsino);
	end

	if nargout >= 2
		sino = fan_beam_proj;
	end
end

end % fbp_helix_do()


function [x, info] = analyticCenter(f, x, opts)
% x = analyticCenter(f, opts, x)
% compute the analytic center for the domain {Ax=b} intersect the domain of f
%
% Input:
%    f - a ConvexProgram
%    x - a feasible initial point (optional)
%    opts - a structure for options with the following properties (optional)
%       MaxIter - maximum number of iterations
%       Output - maximum number of iterations
%       CentralityTol, FeasibilityTol - stop the following are satisfied
%           ||(A' * lambda - grad f(x)) / sqrt(hess)||_inf < CentralityTol
%           ||A x - b||_inf < FeasibilityTol
%       CollapseDistanceTol -
%           if x_i is CollapseDistanceTol close to some boundary,
%           we assume the block i is tight.
%       SolverIter - the number of iteration in solving linear systems
%       VectorType - the class we use for x (@double, @ddouble, @qdouble)
%
% Output:
%  x - It outputs the analytic center of f
%  info - a structure containing centrality, feasibility and iter.
%  f - the problem f will be modified as we discover collapsed subspace

defaultOpts = struct('MaxIter', 1000, 'Output', @disp, 'CentralityTol', 1e-8, 'FeasibilityTol', 1e-12, ...
            'CollapseDistanceTol', 1e-8, 'SolverIter', 3, 'VectorType', @ddouble);
if nargin >= 3
   opts = setField(defaultOpts, opts);
else
   opts = defaultOpts;
end

if f.feasible == false
   x = []; info = [];
   return;
end

%% prepare the printout
output = TableDisplay('Iter', '5i', 'PredStep', '13.2e', 'CorrStep', '13.2e', 'Centrality', '13.2e', 'Feasibility', '13.2e', 'mu', '13.2e');
output.output = opts.Output;
output.header();

%% initial parameters
f.removeRedundantRows();
if nargin <= 1 || isempty(x) || any(f.distance(x) <= 0)
   % a heuristic initial point
   f.solver.factorize(ones(size(f.A,2),1));
   v = f.A' * f.solver.solve(f.b);
   x = f.barrier.center;
   t = stepSize(x, v-x, 0.95);
   x = x + t * (v-x);
end

A = f.A; At = A'; b = f.b; rx = [];
x = opts.VectorType(x);
y = 0*b;
feasibilityMode = true;
mu = 0;
lastProgress = 0; % record the last iteration making progress
bestCentrality = 1e32; bestFeasibility = 1e32;

%% find the central path
for iter = 1:opts.MaxIter
   % Compute the residual
   if (isempty(rx))
      [rs, rx, h] = updateResidual(x, y, mu);
   end

   % Compute the cholesky decomposition
   cholErr = f.solver.factorize(1./h);

   % Compute the direction
   if (cholErr < f.solver.cholTol)
      v = f.solver.solve([A*(rs./h) rx], [], opts.SolverIter);
      Atv = At * v;
      y = y - v(:,1);

      if (feasibilityMode)
         % corr_dx = (rs - At * (R\(R'\(A*(rs./hess)))))./hess;
         % pred_dx = (At * (R\(R'\rx)))./hess;

         corr_dx = (rs - Atv(:,1))./h;
         pred_dx = -Atv(:,2)./h;

         pred_t = stepSize(x, pred_dx, 1.0);
         dx = corr_dx + pred_t * pred_dx;
         t = stepSize(x, dx, 0.95);
         x = x + t * dx;
      else
         % y = y - (R\(R'\(A*(rs./hess))))
         % dx = (rs + At * (R\(R'\(rx - A*(rs./hess)))))./hess;

         pred_t = 1.0;
         dx = (rs - Atv(:,1))./h - Atv(:,2)./h;
         t = stepSize(x, dx, 0.95);
         x = x + t * dx;
      end

      [rs, rx, h] = updateResidual(x, y, mu);

      centrality = max(abs(rs./sqrt(h)));
      feasibility = max(abs(rx));
      if isempty(feasibility), feasibility = 0; end % Fix the case rx is []

      % Output the error
      o = struct('Iter', iter, 'PredStep', t * pred_t, 'CorrStep', t, 'Centrality', centrality, 'Feasibility', feasibility, 'mu', mu);
      output.row(o);

      if (centrality < 0.9 * bestCentrality)
         bestCentrality = centrality;
         if ~feasibilityMode, lastProgress = iter; end
      end

      if (feasibility < 0.9 * bestFeasibility)
         bestFeasibility = feasibility;
         if feasibilityMode, lastProgress = iter; end
      end

      % Check stop criteria
      if centrality < opts.CentralityTol && feasibility < opts.FeasibilityTol
         if feasibilityMode && (~isempty(f.c) || ~isempty(f.df))
            feasibilityMode = false;
            mu = 1;
            [rs, rx, h] = updateResidual(x, y, mu);
            opts.Output('Switch to optimize mode.');

            bestCentrality = 1e32; bestFeasibility = 1e32;
         else
            break;
         end
      end
   end

   % Perform the collpase if needed
   dist = f.distance(x);
   if any(dist < opts.CollapseDistanceTol / 10)
      blocks = find(dist < opts.CollapseDistanceTol);

      % Update barrier
      block_idx = f.collapse(x, blocks);

      % check feasibility after collapsing
      if f.feasible == false
         x = []; info = [];
         opts.Output('The problem is infeasible.');
         return;
      end

      % Update other variables
      A = f.A; At = A'; b = f.b;
      x(block_idx) = [];
      y = 0*b;
      rx = [];

      lastProgress = iter;
      bestCentrality = 1e32; bestFeasibility = 1e32;
      opts.Output(sprintf('Collapsed %i blocks.', length(blocks)));
      continue;
   end

   if (cholErr >= f.solver.cholTol)
      opts.Output('Failed due to numerical issue.');
      f.feasible = false;
      x = []; info = [];
      return;
   end

   if (iter > lastProgress + 20)
      opts.Output('Stopped due to no progress.');
      break;
   end
end
x = double(x);
info = struct('centrality', centrality, 'feasibility', feasibility, 'iter', iter, 'hess', h);

function t = stepSize(x, dx, factor)
   t = min(double(factor*min(f.barrier.distance(x, dx))),1); % min(Nan,1) = 1 for double
end

function [rs, rx, h] = updateResidual(x, y, mu)
   [gb, hb] = f.barrier.derivatives(x);

   gc = zeros(size(gb), class(gb));
   hc = zeros(size(hb), class(hb));

   if ~isempty(f.c)
      gc = gc + f.c;
   end

   if ~isempty(f.df)
      gc_ = f.df(f.export(x));
      gc = gc + f.scale .* gc_(f.idx);

      hc_ = f.ddf(f.export(x));
      hc = hc + f.scale2 .* hc_(f.idx);
   end

   g = gb + mu * gc; h = hb + mu * hc;

   rs = At * y - g;
   rx = A * x - b;
end

end


function [gx,dgdx,dgdp] = g_classif(x,P,u,in)

gx = sss([in.X',ones(size(in.X,2),1)]*P);
dgdx = [];
dgdp = diag(gx.*(1-gx))*[in.X',ones(size(in.X,2),1)];
dgdp = dgdp';

function sx = sss(x)
sx = 1./(1+exp(-x));
sx(sx < 1e-8) = 1e-8;
sx(sx > 1-1e-8) = 1-1e-8;


%FINDESSENTIALMAT  Calculates an essential matrix from the corresponding points in two images
%
%     E = cv.findEssentialMat(points1, points2)
%     [E, mask] = cv.findEssentialMat(...)
%     [...] = cv.findEssentialMat(..., 'OptionName', optionValue, ...)
%
% ## Input
% * __points1__ Cell array of N (N>=5) 2D points from the first image, or numeric array
%   Nx2/Nx1x2/1xNx2. The point coordinates should be floating-point (single or
%   double precision).
% * __points2__ Cell array or numeric array of the second image points of the
%   same size and format as `points1`.
%
% ## Output
% * __E__ Essential matrix, 3x3.
% * __mask__ Output vector of N elements, every element of which is set to 0
%   for outliers and to 1 for the other points (inliers). The array is
%   computed only in the RANSAC and LMedS robust methods.
%
% ## Options
% * __CameraMatrix__ Camera matrix `K = [fx 0 cx; 0 fy cy; 0 0 1]`. Note that
%   this function assumes that `points1` and `points2` are feature points from
%   cameras with the same camera matrix. default `eye(3)`.
% * __Method__ Method for computing an essential matrix. One of:
%   * __Ransac__ for the RANSAC algorithm. (default)
%   * __LMedS__ for the LMedS algorithm.
% * __Confidence__ Parameter used for the RANSAC or LMedS methods only. It
%   specifies a desirable level of confidence (probability) that the estimated
%   matrix is correct. In the range 0..1 exclusive. default 0.999
% * __Threshold__ Parameter used for RANSAC. It is the maximum distance from a
%   point to an epipolar line in pixels, beyond which the point is considered
%   an outlier and is not used for computing the final essential matrix. It
%   can be set to something like 1-3, depending on the accuracy of the point
%   localization, image resolution, and the image noise. default 1.0
%
% This function estimates essential matrix based on the five-point algorithm
% solver in [Nister03]. [SteweniusCFS] is also a related. The epipolar
% geometry is described by the following equation:
%
%     [p2;1]' * inv(K)' * E * inv(K) * [p1;1] = 0
%
% where `E` is an essential matrix, `p1` and `p2` are corresponding points in
% the first and the second images, respectively. The result of this function
% may be passed further to cv.decomposeEssentialMat or cv.recoverPose to
% recover the relative pose between cameras.
%
% `K` is the camera matrix with focal length `fx` and `fy` and principal point
% `[cx,cy]`:
%
%     K = [fx  0 cx;
%          0  fy cy;
%          0   0  1]
%
% ## Example
% Estimation of essential matrix using the RANSAC algorithm:
%
%     % initialize the points here
%     points1 = {[1,1],[3,1],[5,1],...}
%     points2 = {[2,3],[4,3],[6,3],...}
%     % estimate essential matrix
%     [E, mask] = cv.findEssentialMat(points1, points2, 'Method','Ransac');
%
% ## References
% [Nister03]:
% > David Nister. "An efficient solution to the five-point relative pose
% > problem". Pattern Analysis and Machine Intelligence, IEEE Transactions on,
% > 26(6):756-770, 2004.
%
% [SteweniusCFS]:
% > Henrik Stewenius. "Calibrated fivepoint solver".
%
% See also: cv.findFundamentalMat, cv.recoverPose, estimateEssentialMatrix
%


function p_eff = gp_peff(gp, x, y, varargin);
%GP_PEFF  The effective number of parameters in GP model with focus
%         on latent variables
%
%  Description
%    P_EFF = EP_PEFF(GP, X, Y) Takes the Gaussian process structure
%    GP, training inputs X and training outputs and returns the
%    effective number of parameters as defined by Spiegelhalter et
%    al. (2002).
%
%    NOTE! The effective number of parameters is evaluated with
%    focus on latent variable f. This means that the parameters th
%    (parameters of covariance function and likelihood) are
%    considered fixed. (See Spiegelhalter et al (2002) for
%    discussion on the parameters in focus in Bayesian model).
%    Thus, the returned p_eff tells the effective number of latent
%    variables. This statistics is important for example when
%    assessing the goodness of Laplace or EP approximation in case
%    of non-Gaussian likelihood (See Vanhatalo et al for
%    discussion).
%
%    If you want to evaluate the effective number of parameters
%    with focus on parameters, see GP_DIC.
%
%    The effective number of parameters is approximated as follows:
%        p_eff = n - trace( K\C ),
%
%    where K is the prior covariance matrix and C the posterior
%    covariance matrix. This approximation is introduced by
%    Spiegelhalter et al. (2002) in equation (16). If the
%    likelihood is non-Gaussian and gp.latent_method is either
%    Laplace or EP, then C is the Laplace or EP approximation for
%    the posterior covariance.
%
%  References:
%    Spiegelhalter, Best, Carlin and van der Linde (2002).
%    Bayesian measures of model complexity and fit. J. R.
%    Statist. Soc. B, 64(4):583-639.
%
%    Vanhatalo, J., Pietil�inen V. and Vehtari, A. (2010).
%    Approximate inference for disease mapping with sparse
%    Gaussian processes. Statistics in Medicine, 29(15):1580-1607.
%
%  See also
%    GP_DIC, DEMO_MODELASSESMENT1
%
%
% Copyright (c) 2009-2010 Jarno Vanhatalo

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'GP_PEFF';
  ip.addRequired('gp',@isstruct);
  ip.addRequired('x', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
  ip.addRequired('y', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
  ip.addParamValue('z', [], @(x) isreal(x) && all(isfinite(x(:))))
  ip.parse(gp, x, y, varargin{:});
  z=ip.Results.z;

  tn = size(x,1);

    if isfield(gp.lik.fh,'trcov')
      % a Gaussian likelihood

        switch gp.type
          case 'FULL'

            [K, C] = gp_trcov(gp, x);
            L = chol(C);
            p_eff = trace( L\(L'\K) );

          case 'FIC'
            u = gp.X_u;
            m = size(u,1);
            % Turn the inducing vector on right direction
            if size(u,2) ~= size(x,2)
                u=u';
            end
            % Calculate some help matrices
            [Kv_ff, Cv_ff] = gp_trvar(gp, x);  % 1 x f  vector
            K_fu = gp_cov(gp, x, u);   % f x u
            K_uu = gp_trcov(gp, u);     % u x u, noiseles covariance K_uu
            Luu = chol(K_uu)';

            % Evaluate Lambda (La) for specific model
            % Q_ff = K_fu*inv(K_uu)*K_fu'
            % Here we need only the diag(Q_ff), which is evaluated below
            B=Luu\(K_fu');
            Qv_ff=sum(B.^2)';
            Lav = Kv_ff-Qv_ff;
            Lav2 = Cv_ff-Qv_ff;

            iLaKfu = zeros(size(K_fu));  % f x u,
            n = size(x,1);
            for i=1:n
                iLaKfu(i,:) = K_fu(i,:)./Lav2(i);  % f x u
            end
            A = K_uu+K_fu'*iLaKfu;
            A = (A+A')./2;

            L = iLaKfu/chol(A);

            p_eff = sum(Lav./Lav2) + sum(sum( repmat(Lav2,1,m).\B'.*B' - L.*(L'*B'*B)' - L.*(L'.*repmat(Lav',m,1))', 2));

% $$$             % Check the result using full matrices
% $$$             C = B'*B + diag(Lav2);
% $$$             K = B'*B + diag(Lav);
% $$$             L = chol(C);
% $$$             p_eff = trace( L\(L'\K) );

          case {'PIC' 'PIC_BLOCK'}
            u = gp.X_u;
            ind = gp.tr_index;
            if size(u,2) ~= size(x,2)
                u=u';
            end

            % Calculate some help matrices
            [Kv_ff, Cv_ff] = gp_trvar(gp, x);  % 1 x f  vector
            K_fu = gp_cov(gp, x, u);         % f x u
            K_uu = gp_trcov(gp, u);    % u x u, noiseles covariance K_uu
            Luu = chol(K_uu)';

            % Evaluate the Lambda (La) for specific model
            % Q_ff = K_fu*inv(K_uu)*K_fu'
            % Here we need only the diag(Q_ff), which is evaluated below
            B=Luu\K_fu';
            iLaKfu = zeros(size(K_fu));  % f x u
            for i=1:length(ind)
                Qbl_ff = B(:,ind{i})'*B(:,ind{i});
                [Kbl_ff, Cbl_ff] = gp_trcov(gp, x(ind{i},:));
                La{i} = Kbl_ff - Qbl_ff;
                La2{i} = Cbl_ff - Qbl_ff;
                iLaKfu(ind{i},:) = La2{i}\K_fu(ind{i},:);
            end
            A = K_uu+K_fu'*iLaKfu;
            A = (A+A')./2;            % Ensure symmetry
            L = iLaKfu/chol(A);

            p_eff = sum(sum(- L.*(L'*B'*B)',2));
            for i=1:length(ind)
                LLa2 = chol(La2{i});
                p_eff = p_eff + trace(LLa2\(LLa2'\La{i})) + trace( LLa2\(LLa2'\B(:,ind{i})'*B(:,ind{i})) - L(ind{i},:)*L(ind{i},:)'*La{i} );
            end

          case 'CS+FIC'
            u = gp.X_u;
            m = size(u,1);
            % Turn the inducing vector on right direction
            if size(u,2) ~= size(x,2)
                u=u';
            end

            % Indexes to all non-compact support and compact support covariances.
            cf1 = [];
            cf2 = [];

            ncf = length(gp.cf);
            % Loop through all covariance functions
            for i = 1:ncf
                % Non-CS covariances
                if ~isfield(gp.cf{i},'cs')
                    cf1 = [cf1 i];
                    % CS-covariances
                else
                    cf2 = [cf2 i];
                end
            end

            [Kv_ff, Cv_ff] = gp_trvar(gp, x, cf1);  % f x 1  vector
            K_fu = gp_cov(gp,x,u,cf1);         % f x u
            K_uu = gp_trcov(gp,u,cf1);    % u x u, noiseles covariance K_uu
            K_uu = (K_uu+K_uu')./2;     % ensure the symmetry of K_uu

            Kcs = gp_trcov(gp, x, cf2);
            Luu = chol(K_uu)';
            B=Luu\(K_fu');       % u x f
            Qv_ff=sum(B.^2)';
            Lav = Cv_ff-Qv_ff;   % f x 1, Vector of diagonal elements
            Lav2 = Kv_ff-Qv_ff;   % f x 1, Vector of diagonal elements

            La = sparse(1:tn,1:tn,Lav,tn,tn) + Kcs;
            La2 = sparse(1:tn,1:tn,Lav2,tn,tn) + Kcs;

            iLaKfu = La\K_fu;
            A = K_uu+K_fu'*iLaKfu;
            A = (A+A')./2;     % Ensure symmetry
            L = iLaKfu/chol(A);

            LLa2 = L'*La2;
            LaB = La\B';
            LBB = L'*B'*B;

            VD = ldlchol(La);
            spiLa = spinv(VD,1);

            p_eff = sum( sum(LaB.*B',2) + sum(spiLa.*La2,2) - sum(L.*LBB',2) - sum(L.*LLa2',2) );

% $$$
% $$$             C = B'*B + Kcs + diag(Lav);
% $$$             K = B'*B + Kcs + diag(Lav2);
% $$$
% $$$             L = chol(C);
% $$$             p_eff = trace( L\(L'\K) );
        end

    else
      % ============================
      % A non Gaussian likelihood
      % ============================

        switch gp.type
          case 'FULL'
            switch gp.latent_method
              case 'EP'
                %[e, edata, eprior, tautilde, nutilde, L] = gpep_e(gp_pak(gp), gp, x, y, 'z', z);
                [e, edata, eprior, p] = gpep_e(gp_pak(gp), gp, x, y, 'z', z);
                [tautilde, L] = deal(p.tautilde, p.L);

                % The prior variance
                K=gp_trcov(gp,x);

                if all(tautilde > 0) && ~isequal(gp.latent_opt.optim_method, 'robust-EP')
                    sqrttautilde = sqrt(tautilde);
                    Stildesqroot = sparse(1:tn, 1:tn, sqrttautilde, tn, tn);

                    if issparse(L)
                        p_eff = trace(Stildesqroot*ldlsolve(L, Stildesqroot*K));
                    else
                        p_eff = trace(Stildesqroot*(L'\(L\Stildesqroot)*K));
                    end
                else
                    C = L*L';
                    L = chol(K);
                    Varf = tn - trace(L\(L'\C));
                end

              case 'Laplace'
                %[e, edata, eprior, f, L] = gpla_e(gp_pak(gp), gp, x, y, 'z', z);
                [e, edata, eprior, p] = gpla_e(gp_pak(gp), gp, x, y, 'z', z);
                [f, L] = deal(p.f, p.L);

                W = -gp.lik.fh.llg2(gp.lik, y, f, 'latent', z);

                % Evaluate the prior variance
                K = gp_trcov(gp,x);

                if W >= 0
                    if issparse(K) && issparse(L)

                        sqrtW = sparse(1:tn, 1:tn, sqrt(W), tn, tn);
                        sqrtWK = sqrtW*K;
                        p_eff = trace(sqrtW*ldlsolve(L,sqrtWK));
                    else
                        W = diag(W);
                        p_eff = trace(sqrt(W)*(L'\(L\(sqrt(W)*K))));
                    end
                else
                    C = L*L';
                    L = chol(K);
                    p_eff = tn - trace(L\(L'\C));
                end
            end

          case 'FIC'
            u = gp.X_u;
            m = size(u,1);
            % Calculate some help matrices
            [Kv_ff, Cv_ff] = gp_trvar(gp, x);  % 1 x f  vector
            K_fu = gp_cov(gp, x, u);   % f x u
            K_uu = gp_trcov(gp, u);     % u x u, noiseles covariance K_uu
            Luu = chol(K_uu)';
            B=Luu\(K_fu');
            Qv_ff=sum(B.^2)';
            Lav = Kv_ff-Qv_ff;

            switch gp.latent_method
              case 'EP'
                %[e, edata, eprior, tautilde, nutilde, L, La, b] = gpep_e(gp_pak(gp), gp, x, y, 'z', z);
                %[e, edata, eprior, p] = gpep_e(gp_pak(gp), gp, x, y, 'z', z);
                [L, La] = deal(p.L, p.La2);

                k = gp_trvar(gp,x);

                p_eff = sum( sum( (repmat(1./La,1,m).*B').*B',2) ) - sum(sum(L.*((L'*B')*B)',2));
                Lav = k - sum(B.^2)';
                p_eff = p_eff + sum(Lav./La) - sum(sum(L.*L,2).*Lav);

% $$$                 C = diag(1./La) - L*L';
% $$$                 K = B'*B + diag(k - sum(B.^2)');
% $$$
% $$$                 p_eff = trace(C*K);

              case 'Laplace'
                %[e, edata, eprior, f, L, a, La2] = gpla_e(gp_pak(gp), gp, x, y, 'z', z);
                [e, edata, eprior, p] = gpla_e(gp_pak(gp), gp, x, y, 'z', z);
                [f, La2] = deal(p.f, p.La2);

                W = -gp.lik.fh.llg2(gp.lik, y, f, 'latent', z);
                La = W.*Lav;
                Lahat = 1 + La;
                sqrtW = sqrt(W);
                B = (repmat(sqrtW,1,m).*K_fu);

                % Components for (I + W^(1/2)*(Qff + La2)*W^(1/2))^(-1) = Lahat^(-1) - L2*L2'
                B2 = repmat(Lahat,1,m).\B;
                A2 = K_uu + B'*B2; A2=(A2+A2)/2;
                L2 = B2/chol(A2);

                BB=Luu\(K_fu');
                BB2=B/Luu';

                p_eff = sum(W./Lahat.*Lav);
                p_eff = p_eff + sum(sqrtW .* (sum((repmat(Lahat,1,m).\BB2).*BB',2)...
                                             - sum(L2.*(L2'.*repmat(sqrtW'.*La2',m,1))',2)...
                                             - sum(L2.*(L2'*BB2*BB)',2)) );

% $$$                 K = BB'*BB + diag(Lav);
% $$$                 sW = diag(sqrt(W));
% $$$                 B = eye(size(K)) + sW*K*sW;
% $$$                 L = chol(B)';
% $$$
% $$$                 W = diag(W);
% $$$                 p_eff = trace(sqrt(W)*(L'\(L\(sqrt(W)*K))));
            end

          case {'PIC' 'PIC_BLOCK'}
            u = gp.X_u;
            K_fu = gp_cov(gp, x, u);
            K_uu = gp_trcov(gp, u);
            K_uu = (K_uu+K_uu')./2;

            m = size(u,1);
            ind = gp.tr_index;
            Luu = chol(K_uu)';
            B=Luu\(K_fu');

            switch gp.latent_method
              case 'EP'

                %[e, edata, eprior, tautilde, nutilde, L, La, b] = gpep_e(gp_pak(gp), gp, x, y, 'z', z);
                [e, edata, eprior, p] = gpep_e(gp_pak(gp), gp, x, y, 'z', z);
                [L, La] = deal(p.L, p.La2);

                p_eff = - sum(sum(L.*((L'*B')*B)',2));

                for i=1:length(ind)
                    La1 = gp_trcov(gp, x(ind{i},:)) - B(:,ind{i})'*B(:,ind{i});
                    p_eff = p_eff + trace(La{i}\B(:,ind{i})'*B(:,ind{i}));
                    p_eff = p_eff + trace(La{i}\La1);
                    p_eff = p_eff - trace(L(ind{i},:)*L(ind{i},:)'*La1);
                end

              case 'Laplace'
                %[e, edata, eprior, f, L, a, La2] = gpla_e(gp_pak(gp), gp, x, y, 'z', z);
                [e, edata, eprior, p] = gpla_e(gp_pak(gp), gp, x, y, 'z', z);
                [f, La2] = deal(p.f, p.La2);

                iKuuKuf = K_uu\K_fu';

                % Evaluate the variance

                W = -gp.lik.fh.llg2(gp.lik, y, f, 'latent', z);
                sqrtW = sqrt(W);

                % Components for (I + W^(1/2)*(Qff + La2)*W^(1/2))^(-1) = Lahat^(-1) - L2*L2'
                for i=1:length(ind)
                    La{i} = diag(sqrtW(ind{i}))*La2{i}*diag(sqrtW(ind{i}));
                    Lahat{i} = eye(size(La{i})) + La{i};
                    LLahat{i} = chol(Lahat{i});
                end
                sKfu = (repmat(sqrt(W),1,m).*K_fu);
                for i=1:length(ind)
                    iLasKfu(ind{i},:) = Lahat{i}\sKfu(ind{i},:);
                end
                A2 = K_uu + sKfu'*iLasKfu; A2=(A2+A2)/2;
                L2 = iLasKfu/chol(A2);

                p_eff = -sum(sqrtW.*sum(L2.*(L2'*(repmat(sqrtW,1,m).*B')*B)',2));
                for i=1:length(ind)
                    dsqrtW = diag(sqrtW(ind{i}));
                   p_eff = p_eff + trace( dsqrtW*(LLahat{i}\(LLahat{i}'\(dsqrtW*La2{i}))));
                   p_eff = p_eff + trace( dsqrtW*(LLahat{i}\(LLahat{i}'\(dsqrtW*B(:,ind{i})'*B(:,ind{i})))));
                   p_eff = p_eff - trace(dsqrtW*L2(ind{i},:)*L2(ind{i},:)'*dsqrtW*La2{i});
                end

% $$$                 K = B'*B;
% $$$                 C = -L2*L2';
% $$$                 for i=1:length(ind)
% $$$                    K(ind{i},ind{i}) =  K(ind{i},ind{i}) + La2{i};
% $$$                    C(ind{i},ind{i}) =  C(ind{i},ind{i}) + inv(Lahat{i});
% $$$                 end
% $$$
% $$$                 p_eff = trace (diag(sqrtW)*C*diag(sqrtW)*K) ;

            end

          case 'CS+FIC'
            u = gp.X_u;
            [n,nin]=size(x);
            % Indexes to all non-compact support and compact support covariances.
            cf1 = [];
            cf2 = [];

            ncf = length(gp.cf);
            % Loop through all covariance functions
            for i = 1:ncf
                % Non-CS covariances
                if ~isfield(gp.cf{i},'cs')
                    cf1 = [cf1 i];
                    % CS-covariances
                else
                    cf2 = [cf2 i];
                end
            end

            K_fu = gp_cov(gp,x,u,cf1);         % f x u
            K_uu = gp_trcov(gp,u,cf1);    % u x u, noiseles covariance K_uu
            K_uu = (K_uu+K_uu')./2;     % ensure the symmetry of K_uu

            Kcs = gp_trcov(gp, x, cf2);
            Luu = chol(K_uu)';
            B=Luu\(K_fu');

            switch gp.latent_method
              case 'EP'

                %[e, edata, eprior, tautilde, nutilde, L, La, b] = gpep_e(gp_pak(gp), gp, x, y, 'z', z);
                [e, edata, eprior, p] = gpep_e(gp_pak(gp), gp, x, y, 'z', z);
                [L, La] = deal(p.L, p.La2);

                k = gp_trvar(gp,x,cf1);
                Lav = k - sum(B.^2)';
                La1 = Kcs + sparse(1:n, 1:n, Lav, n, n);

                issparse(La)
                VD = ldlchol(La);
                siLa = spinv(VD,1);
                p_eff = sum(sum(ldlsolve(VD,B').*B')) + sum(sum(siLa.*La1)) - sum(sum(L.*((L'*B')*B)',2));
                p_eff = p_eff - sum(sum(L.*(L'*La1)',2));

% $$$                 C = inv(La) - L*L';
% $$$                 K = B'*B + diag(k - sum(B.^2)') + Kcs;
% $$$
% $$$                 p_eff = trace(C*K);

              case 'Laplace'
                %[e, edata, eprior, f, L, a, La2] = gpla_e(gp_pak(gp), gp, x, y, 'z', z);
                [e, edata, eprior, p] = gpla_e(gp_pak(gp), gp, x, y, 'z', z);
                [f, La2] = deal(p.f, p.La2);

                W = -gp.lik.fh.llg2(gp.lik, y, f, 'latent', z);
                sqrtW = sparse(1:tn,1:tn,sqrt(W),tn,tn);
                Lahat = sparse(1:tn,1:tn,1,tn,tn) + sqrtW*La2*sqrtW;
                B = sqrtW*K_fu;

                % Components for (I + W^(1/2)*(Qff + La2)*W^(1/2))^(-1) = Lahat^(-1) - L2*L2'
                B2 = Lahat\B;
                A2 = K_uu + B'*B2; A2=(A2+A2)/2;
                L2 = B2/chol(A2);

                BB = Luu\(K_fu');
                BB2 = B/Luu';

                VD = ldlchol(Lahat);
                spiLahat = spinv(VD,1);

                p_eff = sum(sum(sqrtW*spiLahat*sqrtW.*La2,2)) + sum(sqrtW * (sum(ldlsolve(VD, BB2).*BB',2) - sum(L2.*(L2'*sqrtW*La2)',2) - sum(L2.*(L2'*BB2*BB)',2)) );

            end

        end
    end

end