function [Results, OPTIONS] = bst_inverse_linear_2016(HeadModel,OPTIONS)
% BST_INVERSE_LINEAR_2016: Compute an inverse solution (minimum norm, dipole fitting or beamformer)
% USAGE:  [Results,OPTIONS] = bst_inverse_linear_2016(HeadModel, OPTIONS) : Compute mininum operator
%                   OPTIONS = bst_inverse_linear_2016()                   : Return default options
%
% DESCRIPTION:
%     This program computes the whitened and weighted minimum-norm operator,
%     (the wMNE imaging kernel), which is used to compute whitened
%     and weighted minimum-norm estimates (MNE).
%         (e.g., J=wMNEoperator*B; where B is the unwhitened data).
%     It can also compute the whitened and noise-normalized dynamic
%     statistical parametric mapping (dSPM) inverse operator, and/or the
%     whitened standardized low resolution brain electromagnetic tomography
%     (sLORETA) inverse operator, which are used to compute whitened source
%     activity dSPM and sLORETA maps.
%         (e.g., S_dSPM=dSPMoperator*B; where B is the unwhitened data).
%         (e.g., S_sLORETA=sLORETAoperator*B; where B is the unwhitened data).
%
%     The function was originally written with the goal of providing some of the same
%     functionality of the MNE software written by Matti Hamalainen, but no
%     guarantees are made that it performs all computations in the same
%     exact way. It also provides some functionalities not available in the
%     MNE software.
%
%     In March 2015, this function was completely overhauled by John
%     Mosher to create dipole fitting, beamforming, and min norm images all
%     in the same imaging kernel framework. After the imaging kernel is
%     generated, subsequent Process Operations can be applied to further
%     interpret the results, such as finding the optimal dipole in each
%     time slice.
%
% INPUTS:
%    - HeadModel: Array of Brainstorm head model structures
%         |- Gain       : Forward field matrix for all the channels (unconstrained source orientations)
%         |- GridOrient : Dipole orientation matrix
%    - OPTIONS: structure
%         |- NoiseCovMat        : Noise covariance structure
%         |   |- NoiseCov       : Noise covariance matrix
%         |   |- FourthMoment   : Fourth moment (F^2 * F^2'/n)
%         |   |- nSamples       : Number of times samples used to compute those measures
%         |- DataCovMat         : Data covariance structure
%         |   |- NoiseCov       : Data covariance matrix  (F*F'/n)
%         |   |- FourthMoment   : Fourth moment (F^2 * F^2'/n)
%         |   |- nSamples       : Number of times samples used to compute those measures
%         |- ChannelTypes   : Type of each channel (for each row of the Leadfield and the NoiseCov matrix)
%         |- InverseMethod  : {'minnorm', 'gls', 'lcmv'}
%         |- InverseMeasure :    | minnorm: {'amplitude',  'dspm', 'sloreta'}
%         |                      |     gls: {'performance'}
%         |                      |    lcmv: {'performance'}
%         |- SourceOrient   : String or a cell array of strings specifying the type of orientation constraints for each HeadModel (default: 'fixed')
%         |- Loose          :    | Value that weights the source variances of the dipole components defining the tangent space of the cortical surfaces (default: 0.2).
%         |- UseDepth       : Flag to do depth weighting (default: 1).
%         |- WeightExp      :    | Order of the depth weighting. {0=no, 1=full normalization, default=0.8}
%         |- WeightLimit    :    | Maximal amount depth weighting (default: 10).
%         |- NoiseMethod    : {'shrink', 'median', 'reg', 'diag', 'none'}
%         |- NoiseReg       :    | NoiseMethod='reg' : Amount of regularization
%         |- SnrMethod      : {'rms', 'fixed'}
%         |- SnrRms         :    | SnrMethod='rms'   : RMS source amplitude, in nAm  (Default=1000)
%         |- SnrFixed       :    | SnrMethod='fixed' : Fixed SNR value (Default=3)
%
% OUTPUTS:
%    - Results : Source structure
%       NEW in 2015: if GridOrient is returned empty, the orientation is assumed to be
%       the original grid orientation (usually normal to the surface, or unconstrained in a volume). If
%       it is returned here, then we are returning an optimized orientation
%       that overrides the original orientation.
%    - OPTIONS : Return the modified options

% NOTES (mostly not updated for 2015, see notes below):
%     - More leadfield matrices can be used: the solution will combine all
%       leadfield matrices appropriately.
%
%     - This leadfield structure allows to combine surface and volume
%       source spaces with and without dipole orientation constraints,and
%       with and without area or volumetric current density computations.
%       If using a single sphere headmodel, the silent radial
%       component could be eliminated using the SVD (e.g., use
%       bst_remove_silent.m).
%       NOTE by JCM 2015: Under what considerations would we mix different
%       source spaces? I don't understand combining, for example,
%       cortically constrained surface dipoles with unconstrained volume
%       dipoles. In my 2015 version, I anticipate the user has picked a
%       consistent source modeling space, e.g. unconstrained volume or
%       fixed orientation surface, as dictated by the head model.
%
%
%     - NoisCov: This should be computed from the pre-stimulus period for
%       averaged ERF data (e.g., using MNE), or from an empty room recording
%       for unaveraged spontaneous or induced data.
%
%     - Orientation constrains for dipoles (.SourceOrient field)
%         - "fixed"     : Dipoles constrained to point normal to cortical surfaces
%         - "free"      : No constraints, dipoles point in x,y,z directions
%         - "loose"     : Source variances of dipole components pointing
%                         tangentially to the cortical surfaces are
%                         multipled by OPTIONS.Loose
%         - "optimal"   : NEW IN 2015: Optimal orientation is returned
%                         in GridOrients for each dipole
%       => OBSOLETE IN 2015: For dealing with multiple source spaces with different types of orientation constraints use for example,
%          OPTION.SourceOrient{1}='fixed';
%          OPTION.SourceOrient{2}='free';
%          This has to correspond with the HeadModel Structure
%
%     - sLORETA: TODO: August 2016, Output values are multiplied by 1e12 for display in
%                Brainstorm (time series and cortical maps). There is a
%                discrepancy in the Pascual-Marqui publications regarding
%                resolution kernels vs data covariance that needs
%                addressing by a sLORETA expert. For now, we leave it as
%                its legacy version since 2011.

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors:  Rey Rene Ramirez, Ph.D, 2010-2012
%           Francois Tadel, 2010-2016
%           John Mosher, 2014-2016

%% ===== DEFINE DEFAULT OPTIONS =====
Def_OPTIONS.NoiseCovMat    = [];
Def_OPTIONS.DataCovMat     = [];
Def_OPTIONS.ChannelTypes   = {};
Def_OPTIONS.InverseMethod  = 'minnorm';
Def_OPTIONS.InverseMeasure = 'amplitude';
Def_OPTIONS.SourceOrient   = {'fixed'};
Def_OPTIONS.Loose          = 0.2;
Def_OPTIONS.UseDepth       = 1;
Def_OPTIONS.WeightExp      = 0.5;
Def_OPTIONS.WeightLimit    = 10;
Def_OPTIONS.NoiseMethod    = 'reg';
Def_OPTIONS.NoiseReg       = .1;
Def_OPTIONS.SnrMethod      = 'fixed';
Def_OPTIONS.SnrRms         = 1000;
Def_OPTIONS.SnrFixed       = 3;
Def_OPTIONS.FunctionName   = [];
% Return the default options
if (nargin == 0)
    Results = Def_OPTIONS;
    return
end

% Make the default for all the leadfields
% JCM: Why are there multiple headmodels being passed to this function?
numL = size(HeadModel,2);

%TODO: Understand this:
if numL > 1,
    error('BST_INVERSE > Mosher does not understand why we are passing multiple headmodels')
    % This is for supporting mixed head models:
    % https://neuroimage.usc.edu/brainstorm/News#Mixed_head_models
    % @JCM: You need to add this support back in your function (see bst_wmne.m for an example).
end

Def_OPTIONS.SourceOrient = repmat(Def_OPTIONS.SourceOrient, [1 numL]);

% Copy default options to OPTIONS structure (do not replace defined values)
OPTIONS = struct_copy_fields(OPTIONS, Def_OPTIONS, 0);

%% ===== CHECK FOR INCONSISTENT VALUES =====
if (OPTIONS.UseDepth && strcmpi(OPTIONS.InverseMeasure, 'sloreta'))
    disp('BST_INVERSE > Depth weighting is not necessary when using sLORETA normalization, ignoring option UseDepth=1');
    OPTIONS.UseDepth = 0;
elseif (OPTIONS.UseDepth && ~strcmpi(OPTIONS.InverseMethod, 'minnorm'))
    disp('BST_INVERSE > Depth weighting is only available for minnorm, ignoring option UseDepth=1');
    OPTIONS.UseDepth = 0;
end
if (numL ~= length(OPTIONS.SourceOrient))
    error('BST_INVERSE > The number of elements in the HeadModel structure should equal the length of the cell array OPTIONS.SourceOrient.')
end
if ~isempty(OPTIONS.Loose) && (OPTIONS.Loose>=1 || OPTIONS.Loose<=0)
    error('BST_INVERSE > Loose value should be smaller than 1 and bigger than 0, or empty for no loose orientations.')
end
if (OPTIONS.WeightExp > 1) || (OPTIONS.WeightExp < 0)
    error('BST_INVERSE > WeightExp should be a scalar between 0 and 1')
end

if (OPTIONS.NoiseReg > 1) || (OPTIONS.NoiseReg < 0)
    error('BST_INVERSE > NoiseReg should be a scalar between 0 and 1')
end

%% ===== NOISE COVARIANCE REGULARIZATION =====

% Convenient notation
C_noise = OPTIONS.NoiseCovMat.NoiseCov; % all of the channels requested
Var_noise = diag(C_noise); % Diagonal vector of the noise variances per channel
nChannels = length(Var_noise);

% JCM: March 2015, probably don't need this, but will retain
% Detect if the input noise covariance matrix is or should be diagonal
if (norm(C_noise,'fro') - norm(Var_noise,'fro')) < eps(single(norm(Var_noise,'fro'))),
    % no difference between the full matrix and the diagonal matrix
    disp(['BST_INVERSE > Detected diagonal noise covariance, ignoring option NoiseMethod="' OPTIONS.NoiseMethod '".']);
    OPTIONS.NoiseMethod = 'diag';
end

% Commentary April 2015: There is a tendency to apply generic scale values to the
% channels to balance them with regards to units, such as Volts and Tesla.
% But we also have a problem with gradiometer channels vs magnetometers. So
% the "natural" way is to use the channel variances themselves to balance
% out the differences between modalities. But we don't want to do each
% channel, since a dead channel has (near) zero variance. So instead we
% calculate a common variance for each modality, to get us in the ball
% park. So we initially treat each modality as Independent and Identically
% Distributed (IID) and pre-whiten by this to bring the modalities into
% closer alignment.

% Because the units can be different, we need to first balance the
% different types of arrays. How many unique array types do we have?

% Updated Commentary August 2016, by John Mosher: The issue of balancing is
% primarily a problem of multi-modal statistics, i.e., across multiple
% channel types. Ideally, we should allow for possible cross-covariances
% between the modalities to assist in the overal spatial correlation of the
% array. The problem is that we simultaneously need to "regularize" the
% covariance matrices to catch bad channels and experimental dependencies
% that creep into measurements, while also calculating cross-dependencies
% between modalities of disparate units.
%
% We have tried numerous "black-box" methods to get, for example, Neuromag
% magnetometers and gradiometers combined in the same covariance matrix.
% Unfortunately, each situation can be unique, such as in a clinical
% setting, where the magnetometers can span an enormous dynamic range,
% while gradiometers span a much tighter range, and these ranges overlap.
%
% In consultation with co-investigator Matti Hamalainen and MNE, we opted
% for now to keep the use of the multimodal calculation simpler by
% eliminating the cross modality terms, then regularizing within each
% modality. This has been the default approach to multimodal noise
% covariance calculations in Brainstorm since 2011, and we retain that
% here.

% So how many channel types do we have:
Unique_ChannelTypes = unique(OPTIONS.ChannelTypes);

% Initialize the modality whitening matrices:
% iPrior_Matrix = eye(nChannels);

% Retain the below code in comments if only to discuss what does and does
% not work in trying to capture cross-modalities. August 2016

%     % calculate the average variance per channel as a prior
%     Prior_IID = zeros(length(Unique_ChannelTypes),1);
%     Prior_Vector = zeros(nChannels,1); % initialize as a vector
%
%     for i = 1:length(Unique_ChannelTypes),
%         ndx = find(strcmpi(Unique_ChannelTypes(i),OPTIONS.ChannelTypes)); % which ones
%         Prior_IID(i) = mean(Var_noise(ndx)); % mean of this type
%         % let's get a bit more sophisticated on calculating this IID value
%         % We wouldn't want a few extreme values distorting our mean
%         % How many channels are there:
%         len_ndx = length(ndx);
%         % let's toss out the upper and lower values
%         ndx_clip = round(len_ndx/10); % 10%
%         Variances_This_Modality = sort(Var_noise(ndx));
%         % trim to central values
%         Variances_This_Modality = Variances_This_Modality((ndx_clip+1):(end-ndx_clip));
%         Prior_IID(i) = median(Variances_This_Modality); % mean of the middle values
%         Prior_Vector(ndx) = sqrt(Prior_IID(i)); % map to this part of the array
%     end
%
%     %TODO: Test for bizarre case of Prior_IID too small
%
%     % build whitener to balance out the different types of channels
%     iPrior_Matrix = spdiags(1./Prior_Vector,0,nChannels,nChannels);
%
%     % Now we can use this iPrior_Matrix to "pre-whiten" the noise covariance
%
%     % Block Whitened noise covariance
%     Cw_noise = iPrior_Matrix * C_noise * iPrior_Matrix;
%
%     % Now the units imbalance between different subarrays is theoretically
%     % balanced.

% Now remove the off_diagonal components
Cw_noise = zeros(size(C_noise)); % initialize

for i = 1:length(Unique_ChannelTypes),
    % for each unique modality
    ndx = find(strcmpi(Unique_ChannelTypes(i),OPTIONS.ChannelTypes)); % which ones
    Cw_noise(ndx,ndx) = C_noise(ndx,ndx);
end
% the off diagonal components corresponding to cross-modality
% covariances have been removed

% By continuing to call it "Cw_noise", the below codes can continue as
% before.

% Before any additional regularizations that may interfere

% Make sure the noise covariance matrix is strictly symmetric
% (the previous operations may cause rounding errors that make the matrix not exactly symmetrical)
Cw_noise = (Cw_noise + Cw_noise')/2;

% So the block whitened noise covariance matrix is Cw_noise.

% If the modalities were truly balanced, we could proceed with
% regularization in one fell swoop; however, the reality is that
% regularization should be applied modality by modality, with the cross
% covariance of the modalities zeroed out.

% initialize inverse whitener for noise covariance matrix
iWw_noise = zeros(size(Cw_noise));

FourthMoment = []; % initialize
nSamples = [];
if strcmpi(OPTIONS.NoiseMethod, 'shrink')
   % Has the user calculated the Fourth Order moments?
   if ~isfield(OPTIONS.NoiseCovMat,'FourthMoment'),
      error('BST_INVERSE > For Method ''shrink'' please recalculate Noise Covariance, to include Fourth Order Moments');
   else
      FourthMoment = OPTIONS.NoiseCovMat.FourthMoment;
   end

   % How many samples used to calculate covariance
   if isempty(OPTIONS.NoiseCovMat.nSamples),
      error('BST_INVERSE > No noise samples found. For Method ''shrink'' please recalculate Noise Covariance from actual data samples.');
   else
      nSamples = OPTIONS.NoiseCovMat.nSamples(1);
      % FIX: What if different lengths are used, but only an issue at this
      % point for "shrink" method
   end
end

for i = 1:length(Unique_ChannelTypes),
   % each modality
    ndx = find(strcmpi(Unique_ChannelTypes(i),OPTIONS.ChannelTypes)); % which ones

    % regularize and form whitener for each modality, using local
    % subfunction
    [Cw_noise(ndx,ndx),iWw_noise(ndx,ndx)] = ...
        truncate_and_regularize_covariance(Cw_noise(ndx,ndx),...
        OPTIONS.NoiseMethod,Unique_ChannelTypes{i},...
        OPTIONS.NoiseReg,FourthMoment,nSamples);
end

% So now we have calculated the noise covariance matrix across all
% modalities, but have eliminated the cross covariances between modalities.
% Within each modality, we have checked for rank deficiencies and have
% added possible regularization within the modality.

% so Ww * Ww' = Cw, and iWw*iWw' = pinv(Cw)

% so the entire "whitening" process is to first apply the modality
% whitener, then apply the unitless whitener.
% iWw_noise * iPrior_Matrix * d;

% For convenience in the later modeling, let's combine into one whitener

iW_noise = iWw_noise; % * iPrior_Matrix; % August 2016, iPrior is simpy I

%% ======== Data Covariance Manipulation, if there is one =============

% August 2016, we treat Data Covariances in exactly the same manner as
% Noise Covariance matrices. To be done is to explore whitening methods of
% data covariance matrices, such as would be needed in MUSIC, but not there
% yet, part of future work.

% We only calculate the data covariance matrix for LCMV

if isempty(OPTIONS.DataCovMat) || ~strcmpi(OPTIONS.InverseMethod, 'lcmv')  % user may not have calculated

    iW_data = [];

else
    % Regularize and invert the data covariance matrix, same as the noise
    % covariance matrix.

    % Convenient notation
    C_data = OPTIONS.DataCovMat.NoiseCov; % all of the channels requested
    Var_data = diag(C_data); % Diagonal vector of the noise variances per channel

    % quick error check
    if nChannels ~= length(Var_data),
        error('BST_INVERSE > Data covariance is not the same size as noise covariance, something is wrong')
    end

    % Zero out any cross covariance between modalities.
    Cw_data =zeros(size(C_data));

    for i = 1:length(Unique_ChannelTypes),
        % for each unique modality
        ndx = find(strcmpi(Unique_ChannelTypes(i),OPTIONS.ChannelTypes)); % which ones
        Cw_data(ndx,ndx) = C_data(ndx,ndx);
    end

    % ensure symmetry

    Cw_data = (Cw_data + Cw_data')/2;

    % So the block whitened noise covariance matrix is Cw_noise.

    % If the modalities were truly balanced, we could proceed with
    % regularization in one fell swoop; however, the reality is that
    % regularization should be applied modality by modality, with the cross
    % covariance of the modalities zeroed out.

    % initialize inverse whitener for noise covariance matrix
    iWw_data = zeros(size(Cw_data));

    FourthMoment = []; % initialize
    nSamples = [];
    if strcmpi(OPTIONS.NoiseMethod, 'shrink')
       % Has the user calculated the Fourth Order moments?
       if ~isfield(OPTIONS.DataCovMat,'FourthMoment'),
          error('BST_INVERSE > For Method ''shrink'' please recalculate Data Covariance, to include Fourth Order Moments');
       else
          FourthMoment = OPTIONS.DataCovMat.FourthMoment;
       end

       % How many samples used to calculate covariance
       if isempty(OPTIONS.DataCovMat.nSamples),
          error('BST_INVERSE > No data samples found. For Method ''shrink'' please recalculate Data Covariance from actual data samples.');
       else
          nSamples = OPTIONS.DataCovMat.nSamples(1);
          % FIX: What if different lengths are used, but only an issue at this
          % point for "shrink" method
       end
    end % for method "shrink"

    for i = 1:length(Unique_ChannelTypes),
       % each modality
       ndx = find(strcmpi(Unique_ChannelTypes(i),OPTIONS.ChannelTypes)); % which ones

        % regularize and form whitener for each modality, using local
        % subfunction. Note DataCov still used NoiseMethod and NoiseReg to
        % pass parameters.

        [Cw_data(ndx,ndx),iWw_data(ndx,ndx)] = ...
            truncate_and_regularize_covariance(Cw_data(ndx,ndx),...
            OPTIONS.NoiseMethod,Unique_ChannelTypes{i},...
            OPTIONS.NoiseReg,FourthMoment,nSamples);
    end

    iW_data = iWw_data; % * iPrior_Matrix; % August 2016, iPrior is simply I

    % =============  MAJOR PROGRAMMING SIMPLICITY HERE ==============
    iW_noise_true = iW_noise; % save the true noise whitener
    iW_noise = iW_data; % we replace the noise whitener with the data whitener in the below equations.

end % if there is a data covariance matrix

%% ===== Source Model Assumptions ===============
%
% Calculated separately from the data and noise covariance considerations
% number of sources:
NumDipoles = size(HeadModel.GridLoc,1); % number of dipoles (source points)
% (insensitive to the number of components per dipole)

% orientation of each source:
% Each current dipole has an explicit or implied covariance matrix for its
% moment, C_q. Let W_q be the matrix square root, C_q = W_q * W_q'.

% If the HeadModelType is "surface" then there is an orientation available,
% if it is "volume" then an orientation is not pre-defined in
% HeadModel.GridOrient.

Wq = cell(1,NumDipoles); % source unit orientations

% Optional Depth Weighting Scalar (used particularly in Min Norms)
% Calculate Depth Weighting Scalar
Alpha = ones(1,NumDipoles); % initialize to unity

if OPTIONS.UseDepth
    % See eq. 6.2.10 of MNE Manual version 2.7 (Mar 2010).
    % Original code had some backflips to check for instabilities.
    % Here we take a simpler direct approach.

    % We are assuming unconstrained (three source directions) per source
    % point. We form the unconstrained norm of each point
    ColNorms2 = sum(HeadModel.Gain .* HeadModel.Gain); % square of each column
    SourceNorms2 = sum(reshape(ColNorms2,3,[]),1); % Frobenius norm squared of each source

    % Now Calculate the *non-inverted* value
    Alpha2 = SourceNorms2 .^ OPTIONS.WeightExp; % note not negative weight (yet)
    AlphaMax2 = max(Alpha2); % largest squared source norm
    % The limit is meant to keep the smallest from getting to small
    Alpha2 = max(Alpha2, AlphaMax2 ./ (OPTIONS.WeightLimit^2)); % sets lower bound on source strengths

    % Now invert it
    Alpha2 = AlphaMax2 ./ Alpha2; % goes from WeightLimit^2 to 1, depending on the inverse strength of the source

    Alpha = sqrt(Alpha2);
    % Thus deep weak sources can be amplified up to WeightLimit times greater
    % strength, relative to the stronger shallower sources.
end

% So, at this point, we have an orientation matrix defined for every source
% point, and we have a possible weighting scalar to apply to it. Each
% source has (as yet unknown) source variance of sigma2. The total source
% covariance we are modeling is
% Cq = Alpha2 * sigma2 * Wq * Wq'; % where sigma2 is unknown and to be
% estimated from the data.

% So we now define Wq to include the optional alpha weighting, as
% calculated above

% So if the headmodel is "volume", user should not have been able to select
% "fixed" (normal to cortex), nor "loose". Francois has controlled this
% through the GUI interface. Check anyway

if strcmpi('volume', HeadModel.HeadModelType)
    switch OPTIONS.SourceOrient{1}
        case 'free'
            % do nothing, okay
        otherwise
            error('BST SOURCE ERROR: HeadModel is a volume with no defined orientation. Change Orientation from %s to ''unconstrained''.\n',OPTIONS.SourceOrient{1})
    end
end

% Initialize each source orientation

fprintf('BST_INVERSE > Using ''%s'' surface orientations\n',OPTIONS.SourceOrient{1})

for i = 1:NumDipoles

    switch OPTIONS.SourceOrient{1}
        case 'fixed'
            % fprintf('BST_INVERSE > Using constrained surface orientations\n');
            NumDipoleComponents = 1;
            tmp = HeadModel.GridOrient(i,:)'; % 3 x 1
            Wq{i} = tmp/norm(tmp); % ensure unity

        case 'loose'
            % fprintf('BST_INVERSE > Using loose surface orientations\n');
            NumDipoleComponents = 3;
            tmp = HeadModel.GridOrient(i,:)'; % preferred direction
            tmp = tmp/norm(tmp); % ensure unity
            tmp_perp = null(tmp'); % orientations perpedicular to preferred
            Wq{i} = [tmp tmp_perp*OPTIONS.Loose]; % weaken the other directions

        case 'free'
            % fprintf('BST_INVERSE > Using unconstrained orientations\n');
            NumDipoleComponents = 3;
            Wq{i} = eye(NumDipoleComponents);

        otherwise
            error('BST_INVERSE > Unknown Source Orientation')
    end

    % L2norm of Wq in everycase above is 1, (even for loose, since L2 norm
    % of matrix is largest singular value of the matrix).

    Wq{i} = Alpha(i)*Wq{i}; % now weight by desired amplifier

    % So L2 norm of Wq is equal to the desired amplifier (if any).
end

%% ===== PROCESSING LEAD FIELD MATRICES, WEIGHTS & ORIENTATIONS =====

% put all covariance priors into one big sparese matrix
WQ = blkdiag(sparse(Wq{1}),Wq{2:end});
% (by making first element sparse, we force Matlab to use efficient sparse
% mex function)

% With the above defined, then the whitened lead field matrix is simply

L = iW_noise * (HeadModel.Gain * WQ);  % if LCMV, this is data whitened.

% we note that the number of columns of the ith source is found as the number of
% corresponding columns in WQ{i}. In this version (March 2016), we still
% assume equal numbers of columns (1 or 3). The Mixed Head Model may
% contain mixed quantities.
%

% The model at this point is d = {A}{x} + n, where d and n are whitened,
% and x has a covariance prior of unity times unknown lambda (= sigma q 2)

% Every NumDipoleComponents columns of L is a source, which we call A

% (We could optimize this to only do when needed, but it's not too
% expensive in any event, taking less than two seconds for 15000 dipoles)

% Decompose for each source
tic
clear A
A(1:NumDipoles) = deal(struct('Ua',[],'Sa',[],'Va',[]));

for i = 1:NumDipoles,
    % index to next source in the lead field matrix
    ndx = ((1-NumDipoleComponents):0) + i*NumDipoleComponents;
    [Ua,Sa,Va] = svd(L(:,ndx),'econ');
    Sad = diag(Sa); % strip to vector

    tol = length(Sad) * eps(single(Sad(1))); % single precision tolerance
    Rank_Dipole = sum(Sad > tol);

    % Trim or not to the rank of the model.
    switch 'notrim'
        case 'trim' % trim each source model to just the non-zero
            A(i).Ua = Ua(:,1:Rank_Dipole);
            A(i).Sa = Sad(1:Rank_Dipole);
            A(i).Va = Va(:,1:Rank_Dipole);
        case 'notrim'
            % don't trim each source model, but do force small singular values to perfectly zero
            Sad((Rank_Dipole+1):end) = 0; % force to perfect zeros
            A(i).Ua = Ua;
            A(i).Sa = Sad;
            A(i).Va = Va;
    end % switch

end
fprintf('BST_INVERSE > Time to decompose is %.1f seconds\n',toc);
% So L = [A.Ua]*blkdiag(A.Sa)*blkdiag(A.Va)'; % good for any subset too.

% Now do a global decomposition for setting SNR and doing min norms
% Won't need the expensive VL of the SVD, and UL is relatively small.

[UL,SL2] = svd(L*L');
SL2 = diag(SL2);
SL = sqrt(SL2); % the singular values of the lead field matrix
tol = length(SL)*eps(single(SL(1))); % single precision tolerance
Rank_Leadfield = sum(SL > tol);
fprintf('BST_INVERSE > Rank of leadfield matrix is %.0f out of %.0f components\n',Rank_Leadfield,length(SL));
% but don't trim to rank, we use full matrix below

%% =========== SNR METHODS ================

% Recall our source covariance prior is Cx = Lamda * I, where we have already
% factored out the dipolar covariance, Cq = alpha2 * Lambda * Wq * Wq';
%
% We use the SNR settings to establish a prior on the source variance,
% which is in turn used as essentially a regularizer in the inverse
% methods.
%
% This may seem backwards, i.e. the signal variance with respect
% to the noise variance establishes the SNR, but in inverse processing, we
% may do this backwards, setting an SNR in order to regularize, depending
% on the selection below.

% Calculate Lambda multiplier to achieve desired SNR

switch (OPTIONS.SnrMethod)
    case 'rms'
        % user had directly specified the variance
        Lambda =  OPTIONS.SnrRms^2;
        SNR = Lambda * SL2(1); % the assumed SNR for the entire leadfield
    case 'fixed'
        % user had given a desired SNR, set lambda of the Grammian to achieve it
        SNR = OPTIONS.SnrFixed^2;

        % several options here. Hamalainen prefers the average eigenvalue
        % of the Grammian. Mosher would prefer the maximum (norm) for other
        % consistency measures, however user's have become accustomed to
        % the Hamalainen measure.

        % Maximum (matrix norm):
        % Lambda = SNR/(SL(1)^2); % thus SL2(1)*Lambda = desired SNR.

        % Hamalainen definition of SNR in the min norm:
        Lambda = SNR/mean(SL.^2); % should be equalivent to Matti's average eigenvalue definition

    otherwise
        error(['BST_INVERSE > Not supported yet: NoiseMethod="' OPTIONS.SnrMethod '"']);
end

fprintf('BST_INVERSE > Confirm units\n')
if exist('engunits', 'file')
    [LambdaY,tmp,LambdaU] = engunits(sqrt(Lambda));
    fprintf('BST_INVERSE > Assumed RMS of the sources is %.3g %sA-m\n',LambdaY,LambdaU);
else
    fprintf('BST_INVERSE > Assumed RMS of the sources is %g A-m\n', sqrt(Lambda));
end
fprintf('BST_INVERSE > Assumed SNR is %.1f (%.1f dB)\n',SNR,10*log10(SNR));

%% ===== INVERSE METHODS =====
% Generate first the inversions (current dipole time series)
switch lower(OPTIONS.InverseMethod) % {minnorm, lcmv, gls}
    case 'minnorm'

        % We set a single lambda to achieve desired source variance. In min
        % norm, all dipoles have the same lambda. We already added an
        % optional amplifier weighting into the covariance prior above.

        % So the data covariance model in the MNE is now
        % Cd = Lambda * L * L' + I
        % = Lambda *UL * SL * UL' + I = UL * (LAMBDA*SL + I) * UL'
        % so invert Cd and use for kernel
        % xhat = Lambda * L' * inv(Cd)
        % we reapply all of the covariances to put data back in original
        % space in the last step.

        % as distinct from GLS, all dipoles have a common data covariance,
        % but each has a unique noise covariance.

        switch OPTIONS.InverseMeasure % {'amplitude',  'dspm', 'sloreta'}
            case 'amplitude'
                OPTIONS.FunctionName = 'mn';
                % xhat = Lambda * L' * inv(Cd)
                % Kernel = Lambda * L' * inv(Lambda * L * L' + eye(size(L,1))) * iW_noise;

                Kernel = Lambda * L' * (UL * diag(1./(Lambda * SL2 + 1)) * UL')*iW_noise;

            case 'dspm'
                OPTIONS.FunctionName = 'dspm';
                % ===== dSPM OPERATOR =====
                % =========== NEEDS REWRITING by JCM ======

                % xhat = Lambda * L' * inv(Cd)
                % Kernel = Lambda * L' * inv(Lambda * L * L' + eye(size(L,1))) * iW_noise;

                Kernel = Lambda * L' * (UL * diag(1./(Lambda * SL2 + 1)) * UL');

                dspmdiag = sum(Kernel .^2, 2);
                if (NumDipoleComponents == 1)
                    dspmdiag = sqrt(dspmdiag);
                elseif (NumDipoleComponents==3 || NumDipoleComponents==2)
                    dspmdiag = reshape(dspmdiag, NumDipoleComponents,[]);
                    dspmdiag = sqrt(sum(dspmdiag,1)); % Taking trace and sqrt.
                    dspmdiag = repmat(dspmdiag, [NumDipoleComponents, 1]);
                    dspmdiag = dspmdiag(:);
                end
                Kernel = bst_bsxfun(@rdivide, Kernel, dspmdiag);

                Kernel = Kernel * iW_noise; % overall whitener

            case 'sloreta'
                OPTIONS.FunctionName = 'sloreta';

                % calculate the standard min norm solution
                Kernel = Lambda * L' * (UL * diag(1./(Lambda * SL2 + 1)) * UL');

                % until I fix multiple head models

                %                 if (NumDipoleComponents(k) == 1)
                %                     sloretadiag = sqrt(sum(Kernel(start:endd,:) .* L(:,start:endd)', 2));
                %                     Kernel(start:endd,:) = bst_bsxfun(@rdivide, Kernel(start:endd,:), sloretadiag);
                %                 elseif (NumDipoleComponents(k)==3 || NumDipoleComponents(k)==2)
                %                     for spoint = start:NumDipoleComponents(k):endd
                %                         R = Kernel(spoint:spoint+NumDipoleComponents(k)-1,:) * L(:,spoint:spoint+NumDipoleComponents(k)-1);
                %                         SIR = sqrtm(pinv(R));
                %                         Kernel(spoint:spoint+NumDipoleComponents(k)-1,:) = SIR * Kernel(spoint:spoint+NumDipoleComponents(k)-1,:);
                %                     end
                %                 end

                if (NumDipoleComponents == 1)
                    sloretadiag = sqrt(sum(Kernel .* L', 2));
                    Kernel = bst_bsxfun(@rdivide, Kernel, sloretadiag);
                elseif (NumDipoleComponents==3 || NumDipoleComponents==2)
                    for spoint = 1:NumDipoleComponents:size(Kernel,1),
                        R = Kernel(spoint:spoint+NumDipoleComponents-1,:) * L(:,spoint:spoint+NumDipoleComponents-1);
                        % SIR = sqrtm(pinv(R)); % Aug 2016 can lead to errors if
                        % singular Use this more explicit form instead
                        [Ur,Sr,Vr] = svd(R); Sr = diag(Sr);
                        RNK = sum(Sr > (length(Sr) * eps(single(Sr(1))))); % single precision Rank
                        SIR = Vr(:,1:RNK) * diag(1./sqrt(Sr(1:RNK))) * Ur(:,1:RNK)'; % square root of inverse

                        Kernel(spoint:spoint+NumDipoleComponents-1,:) = SIR * Kernel(spoint:spoint+NumDipoleComponents-1,:);
                    end
                end

                Kernel = Kernel * iW_noise; % overall whitener

            otherwise
                error('Unknown Option Inverse Measure: %s',OPTIONS.InverseMeasure)

        end

    case {'gls','lcmv'}

        % In generalized least-squares, each dipolar source may have a
        % unique lambda, to achieve the desired SNR. So unlike min norm, we
        % may need to set lambda for each and every dipole.

        % However, as a prior, this seems counter-intuitive, since it would
        % mean adjusting the variance of deep sources to be much greater
        % (and therefore less reqularized) than shallower sources.

        % So in this version, JCM opted to set one global source variance, as in min norm.

        % Note that each Wq may already contain a desired amplifier for the
        % gain matrix of that source. That's okay.

        % As distinct from minimum norm, every dipole has the exact same
        % noise covariance assumption.

        % Neyman-Pearson Performance
        if strcmpi(OPTIONS.InverseMethod, 'gls')
            OPTIONS.FunctionName = 'glsp';
        else
            OPTIONS.FunctionName = 'lcmvp';
        end

        Kernel = zeros(size(L')); % preallocate

        for i = 1:NumDipoles,
            ndx = ((1-NumDipoleComponents):0) + i*NumDipoleComponents;
            %Kernel(ndx,:) =  A(i).Va*(Lambda*A(i).Sa)*inv(Lambda*A(i).Sa + I)*A(i).Ua';
            Kernel(ndx,:) =  A(i).Ua';
        end
        Kernel = Kernel * iW_noise; % final noise whitening

    otherwise
        error('BST> Unknown inverse method: %s',OPTIONS.InverseMethod)

end

%% ===== ASSIGN IMAGING KERNEL =====
% Multiply inverse operator and whitening matrix, so no need to whiten data.
% premultiply by covariance priors to put back into original domain

% Now the Kernel has been designed for the number of dipole components.
% Ordinarily, to convert back to three d, we would use
% Kernel = WQ * Kernel, which puts all one-d answers back in three-d. But
% that's not optimal for storage. We do need, however, to account for the
% possible Alpha increase in the storage.

% Key assumption is that the source prior is norm 1, as designed in the
% beginning of this program.
if strcmpi(OPTIONS.InverseMeasure, 'amplitude')
    % we need to put orientation and weighting back into solution
    if NumDipoleComponents == 3, % we are in three-d,
        Kernel = WQ * Kernel; % put the source prior back into the solution
    elseif NumDipoleComponents == 1,
        Kernel = diag(Alpha)*Kernel;  % put possible alpha weighting back in
    end
end

% now orient the dipoles, if requested
% Orientation Optimization is now done in the "dipole scanning" process
% applied to the performance measure.
GridOrient = [];

% Return results structure
Results.ImagingKernel = Kernel;
Results.GridOrient    = GridOrient; % either empty or optimized
Results.ImageGridAmp  = [];
Results.Whitener      = iW_noise; % full noise covariance whitener
if strcmpi(OPTIONS.InverseMethod, 'lcmv')
    % in the lcmv case, we used the data covariance as the whitener for
    % programming simplicity
    Results.Whitener = iW_noise_true; % the true noise whitener, unused in the case of LCMV, but may be useful in post processing
end
Results.DataWhitener  = iW_data;  % full data covariance whitener, null unless lcmv
Results.nComponents   = NumDipoleComponents;

% clear SourceDecomp % decomposition products of the sources
% % don't need Ua, it was already included in the ImagingKernel
% % Need the other components for reconstructing the source orientation and
% % amplitude
% [SourceDecomp(1:length(A))] = deal(struct('Sa',[],'Va',[]));
%
% for i =1:length(A),
%     SourceDecomp(i).Sa = diag(A(i).Sa); % let's be efficient and store as vector
%     SourceDecomp(i).Va = A(i).Va;
% end
%
% Results.SourceDecomp = SourceDecomp;

% More storage efficient:
Results.SourceDecompVa = [A.Va];
Results.SourceDecompSa = [A.Sa];

end

%% ==============================================================================
%  ===== HELPER FUNCTIONS =======================================================
%  ==============================================================================

%% =========== Covariance Truncation and Regularization
function [Cov,iW] = truncate_and_regularize_covariance(Cov,Method,Type,NoiseReg,FourthMoment,nSamples)
% Cov is the covariance matrix, to be regularized using Method
% Type is the sensor type for display purposes
% NoiseReg is the regularization fraction, if Method "reg" selected
% FourthMoment and nSamples are used if Method "shrinkage" selected

VERBOSE = true; % be talkative about what's happening

% Ensure symmetry
Cov = (Cov + Cov')/2;

% Note,impossible to be complex by above symmetry check
% Decompose just this covariance.
[Un,Sn2] = svd(Cov,'econ');
Sn = sqrt(diag(Sn2)); % singular values
tol = length(Sn) * eps(single(Sn(1))); % single precision tolerance
Rank_Noise = sum(Sn > tol);

if VERBOSE,
    fprintf('BST_INVERSE > Rank of the %s channels, keeping %.0f noise eigenvalues out of %.0f original set\n',...
        Type,Rank_Noise,length(Sn));
end

Un = Un(:,1:Rank_Noise);
Sn = Sn(1:Rank_Noise);

% now rebuild the noise covariance matrix with just the non-zero
% components
Cov = Un*diag(Sn.^2)*Un'; % possibly deficient matrix now

% With this modality truncated, see if we need any additional
% regularizations, and build the inverse whitener

if VERBOSE
    fprintf('BST_INVERSE > Using the ''%s'' method of covariance regularization.\n',Method);
end

switch(Method) % {'shrink', 'reg', 'diag', 'none', 'median'}

    case 'none'
        %  "none" in Regularization means no
        % regularization was applied to the computed Noise Covariance
        % Matrix
        % Do Nothing to Cw_noise
        iW = Un*diag(1./Sn)*Un'; % inverse whitener
        if VERBOSE,
            fprintf('BST_INVERSE > No regularization applied to covariance matrix.\n');
        end

    case 'median'
        if VERBOSE,
            fprintf('BST_INVERSE > Covariance regularized by flattening tail of eigenvalues spectrum to the median value of %.1e\n',median(Sn));
        end
        Sn = max(Sn,median(Sn)); % removes deficient small values
        Cov = Un*diag(Sn.^2)*Un'; % rebuild again.
        iW = Un*diag(1./Sn)*Un'; % inverse whitener

    case 'diag'
        Cov = diag(diag(Cov)); % strip to diagonal
        iW = diag(1./sqrt(diag(Cov))); % inverse of diagonal
        if VERBOSE,
            fprintf('BST_INVERSE > Covariance matrix reduced to diagonal.\n');
        end

    case 'reg'
        % The unit of "Regularize Noise Covariance" is as a percentage of
        % the mean variance of the modality.

        % Ridge Regression:
        RidgeFactor = Sn2(1) * NoiseReg ; % percentage of max

        Cov = Cov + RidgeFactor * eye(size(Cov,1));
        iW = Un*diag(1./(Sn + sqrt(RidgeFactor)))*Un'; % inverse whitener

        if VERBOSE,
            fprintf('BST_INVERSE > Diagonal of %.1f%% of largest eigenvalue added to covariance matrix.\n',NoiseReg * 100);
        end

    case 'shrink'
        % Method of Ledoit, recommended by Alexandre Gramfort

        % Need to scale the Fourth Moment for the modalities

        % use modified version of cov1para attached to this function
        % TODO, can we adjust this routine to handle different numbers of
        % samples in the generation of the fourth order moments
        % calculation? As of August 2016, still relying on a single scalar
        % number.
        [Cov,shrinkage]=cov1para_local(Cov,FourthMoment,nSamples);
        if VERBOSE,
            fprintf('\nShrinkage factor is %f\n\n',shrinkage)
        end
        % we now have the "shrunk" whitened noise covariance
        % Recalculate
        [Un,Sn2] = svd(Cov,'econ');
        Sn = sqrt(diag(Sn2)); % singular values
        tol = length(Sn) * eps(single(Sn(1))); % single precision tolerance
        Rank_Noise = sum(Sn > tol);

        if VERBOSE,
            fprintf('BST_INVERSE > Ledoit covariance regularization, after shrinkage, rank of the %s channels, keeping %.0f noise eigenvalues out of %.0f original set\n',...
                Type,Rank_Noise,length(Sn));
        end

        Un = Un(:,1:Rank_Noise);
        Sn = Sn(1:Rank_Noise);

        % now rebuild the noise covariance matrix with just the non-zero
        % components
        Cov = Un*diag(Sn.^2)*Un'; % possibly deficient matrix now

        iW = Un*diag(1./Sn)*Un'; % inverse whitener

    otherwise
        error(['BST_INVERSE > Unknown covariance regularization method: NoiseMethod="' Method '"']);

end % method of regularization

% Note the design of full rotating whiteners. We don't expect dramatic reductions in
% rank here, and it's convenient to rotate back to the original space.
% Note that these whitener matrices may not be of full rank.

end
%% ======== Ledoit Shrinkage
% Modified to use the precalculated stats

function [sNoiseCov,shrinkage]=cov1para_local(NoiseCov,FourthMoment,nSamples)
% Based on Ledoit's "cov1para" with some modifications
%   x is t x n, returns
%   sigma n x n
%
% shrinkage is the final computed shrinkage factor, used to weight the
%  i.i.d. prior vs the sample estimate. If shrinkage is specified, it is
%  used on input; else, it's computed.

% Original code from
% http://www.ledoit.net/cov1para.m
% Original Ledoit comments:
% function sigma=cov1para(x)
% x (t*n): t iid observations on n random variables
% sigma (n*n): invertible covariance matrix estimator
%
% Shrinks towards one-parameter matrix:
%    all variances are the same
%    all covariances are zero
% if shrink is specified, then this const. is used for shrinkage

% Based on
% http://www.ledoit.net/ole1_abstract.htm
% http://www.ledoit.net/ole1a.pdf (PDF of paper)
%
% A Well-Conditioned Estimator for Large-Dimensional Covariance Matrices
% Olivier Ledoit and Michael Wolf
% Journal of Multivariate Analysis, Volume 88, Issue 2, February 2004, pages 365-411
%
% Abstract
% Many economic problems require a covariance matrix estimator that is not
% only invertible, but also well-conditioned (that is, inverting it does
% not amplify estimation error). For large-dimensional covariance matrices,
% the usual estimator - the sample covariance matrix - is typically not
% well-conditioned and may not even be invertible. This paper introduces an
% estimator that is both well-conditioned and more accurate than the sample
% covariance matrix asymptotically. This estimator is distribution-free and
% has a simple explicit formula that is easy to compute and interpret. It
% is the asymptotically optimal convex combination of the sample covariance
% matrix with the identity matrix. Optimality is meant with respect to a
% quadratic loss function, asymptotically as the number of observations and
% the number of variables go to infinity together. Extensive Monte-Carlo
% confirm that the asymptotic results tend to hold well in finite sample.

% Original Code Header, updated to be now from (2014)
% http://www.econ.uzh.ch/faculty/wolf/publications/cov1para.m.zip
%
% x (t*n): t iid observations on n random variables
% sigma (n*n): invertible covariance matrix estimator
%
% Shrinks towards constant correlation matrix
% if shrink is specified, then this constant is used for shrinkage
%
% The notation follows Ledoit and Wolf (2003, 2004)
% This version 04/2014
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file is released under the BSD 2-clause license.
%
% Copyright (c) 2014, Olivier Ledoit and Michael Wolf
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright
% notice, this list of conditions and the following disclaimer in the
% documentation and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
% IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
% THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
% PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
% CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
% EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
% PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
% PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
% NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
% SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Wolf's site now,
% http://www.econ.uzh.ch/faculty/wolf/publications/cov1para.m.zip
% some differences from original Ledoit that confirm Mosher's
% original re-coding.

% % de-mean returns
% [t,n]=size(x);
% meanx=mean(x);
% x=x-meanx(ones(t,1),:);

% compute sample covariance matrix
% Provided
% NoiseCov=(1/t).*(x'*x);

% compute prior
n=size(NoiseCov,1); % number of channels
meanvar=mean(diag(NoiseCov));
prior=meanvar*eye(n); % Note, should be near identity by our pre-whitening

% what we call p
%y=x.^2;
%phiMat=y'*y/t - NoiseCov.^2;
phiMat = FourthMoment - NoiseCov.^2;
phi=sum(sum(phiMat));

% what we call r is not needed for this shrinkage target

% what we call c
gamma=norm(NoiseCov-prior,'fro')^2;

% compute shrinkage constant
kappa=phi/gamma;
% ensure bounded between zero and one
shrinkage=max(0,min(1,kappa/nSamples));

% compute shrinkage estimator
sNoiseCov=shrinkage*prior+(1-shrinkage)*NoiseCov;

end


function cuboidhyp = omapobj2cuboidhyp(rpo, reglabel, regori, vp, OMAP_FACTOR)

vps{1} = OMAP_FACTOR * vp{1};
vps{2} = OMAP_FACTOR * vp{2};
vps{3} = OMAP_FACTOR * vp{3};

regoriobj = regori;
statsobj = regionprops(reglabel,'PixelIdxList','PixelList');

%%
rm = regionmask(vps, size(reglabel));
sr = segregion(rm, statsobj);

convexpair = find_convex_pair(rpo, sr, regoriobj);

%% get all bounding quadrilaterals of convex pairs
[r c v] = find(convexpair);

rl = unique([r; c]);
for i = 1:length(rl)
    boundingquad(rl(i)) = fit_bounding_quad(statsobj(rl(i)).PixelList, regori(rl(i)), vps);
end

% % display quadrilaterals
% figure;
% img = zeros(size(reglabel));
% for i = 1:length(rl)
%     img(statsobj(rl(i)).PixelIdxList) = 1;
%     imshow(img,[]);
%     hold on;
%     for j = 1:4
%     plot(boundingquad(rl(i)).pt(j,1), boundingquad(rl(i)).pt(j,2), ...
%         'x', 'Markersize', 10, 'Color', [1 0 0]);
%     text(boundingquad(rl(i)).pt(j,1), boundingquad(rl(i)).pt(j,2), ...
%         num2str(j), 'FontSize', 20, 'Color', [1 0 0]);
%     end
%     pause;
%     hold off
%     img(statsobj(rl(i)).PixelIdxList) = 0;
% end

if ~exist('boundingquad', 'var')
    cuboidhyp = [];
    return;
end

%%

partcubhyp = convexpair2partialcuboidhyp(convexpair, boundingquad);

% upscale
for i = 1:length(partcubhyp)
    for j = 1:8
        if ~isempty(partcubhyp(i).junc3(j).pt)
            partcubhyp(i).junc3(j).pt = partcubhyp(i).junc3(j).pt / OMAP_FACTOR;
        end
    end
end

%% get cuboidhyp from partial cuboids
for i = 1:length(partcubhyp)
    prm = cuboid_pts2prm_incomp(partcubhyp(i), vp);
    pts = cuboid_prm2pts(prm, vp);
    cuboidhyp(i) = cuboid_pts2hyp(pts, vp);
end

%% display...
% global omapmore;
% % figure;
% % img = zeros(size(reglabel));
% [row col] = find(convexpair);
% for i = 1:length(row)
%
%     rimg = disp_region(omapmore, reglabel, [row(i) col(i)], OMAP_FACTOR);
%
%     for j = 1:8
%         figure; imshow(rimg); hold on;
%
%         pt = cat(1, boundingquad(row(i)).pt);
%         for k=1:4, text(pt(k,1),pt(k,2),num2str(k), 'Color',[0 1 1]); end
%         pt = pt([1 2 4 3 1], :) / OMAP_FACTOR;
%         plot(pt(:,1), pt(:,2), 'm', 'LineWidth',2);
%
%         pt = cat(1, boundingquad(col(i)).pt);
%         for k=1:4, text(pt(k,1),pt(k,2),num2str(k), 'Color',[0 1 1]); end
%         pt = pt([1 2 4 3 1], :) / OMAP_FACTOR;
%         plot(pt(:,1), pt(:,2), 'm', 'LineWidth',2);
%
%
%         disp_cubes(cuboidhyp((i-1)*8+j), []);
%         pause;
%         close;
%     end
% end

%% discard invalid cuboids
valid = check_validity(cuboidhyp, vp);
cuboidhyp = cuboidhyp(valid);

%% drop thin cuboids
cuboidhyp = drop_thin_cuboids(cuboidhyp, vp);

%% this does not work yet.
% ch = convexpair2cuboidhyp(convexpair, statsobj, vp);

%%
function convexpair = find_convex_pair(rel, sr, regoriobj)
% rel{1}(i,j) = 1 if i is up, j is down
% rel{2}(i,j) = 1 if i is left, j is right
% rel{3}(i,j) = 1 if i is front, j is back
% sr[n x 4]: sr(i,j)=1 if segment i is in region j
% regoriobj(i): orientation of region
% convexpair = 1,2,3,4, or 5 depending on configuration of convex pair
%    = 1 if top & front & region3or4
%    = 2 if top & left & region4
%    = 3 if top & right & region3
%    = 4 if left & front & region4
%    = 5 if right & front & region3

convexpair = zeros(size(rel{1}));

[r c] = find(rel{1} | rel{2} | rel{3});
row = [r; c];
col = [c; r];

for i = 1:length(row)
    si = row(i);
    sj = col(i);
    orii = regoriobj(si);
    orij = regoriobj(sj);
    regioni = sr(si,:);
    regionj = sr(sj,:);
    if orii==1 && orij==3 && ... % top & front & region3or4
       rel{1}(si,sj)==1 && rel{3}(sj,si)==1 && ...
       (regioni(3)==1 || regioni(4)==1) && ...
       (regionj(3)==1 || regionj(4)==1) && ...
       (regioni(1)==0 && regioni(2)==0)
           convexpair(si,sj) = 1;
    elseif orii==1 && orij==2 && ... % top & left & region4
        rel{1}(si,sj)==1 && rel{2}(sj,si)==1 && ...
        regioni(4)==1 && regionj(4)==1 && ...
        regioni(1)==0 && regioni(2)==0
            convexpair(si,sj) = 2;
    elseif orii==1 && orij==2 && ... % top & right & region3
        rel{1}(si,sj)==1 && rel{2}(si,sj)==1 && ...
        regioni(3)==1 && regionj(3)==1 && ...
        regioni(1)==0 && regioni(2)==0
            convexpair(si,sj) = 3;
    elseif orii==2 && orij==3 && ... % left & front & region4
        rel{2}(si,sj)==1 && rel{3}(sj,si)==1 && ...
        regioni(4)==1 && regionj(4)==1
            convexpair(si,sj) = 4;
    elseif orii==2 && orij==3 && ... & right & front & region3
        rel{2}(sj,si)==1 && rel{3}(sj,si)==1 && ...
        regioni(3)==1 && regionj(3)==1
            convexpair(si,sj) = 5;
    end
end

%%
function sr = segregion(rm, stats)
% sr[n x 4]: sr(i,j)=1 if segment i is in region j

sr = false(length(stats), 4);
for i = 1:length(stats)
    sr(i,1) = any( rm{1}(stats(i).PixelIdxList) );
    sr(i,2) = any( rm{2}(stats(i).PixelIdxList) );
    sr(i,3) = any( rm{3}(stats(i).PixelIdxList) );
    sr(i,4) = any( rm{4}(stats(i).PixelIdxList) );
end

%%
function rel = convert_rpo_to_udlrfb(rpo, vp)
% rpo{1,2,3}(j,k) = 1 if region j is closer to camera than k (k is closer to vp than j)
% rel{1}(i,j) = 1 if i is up, j is down
% rel{2}(i,j) = 1 if i is left, j is right
% rel{3}(i,j) = 1 if i is front, j is back

rel = rpo;
if vp{1}(2) < 0
    rel{1} = rel{1}';
end
if vp{2}(1) < 0
    rel{2} = rel{2}';
end

%%
function rm = regionmask(vp, imgsize)
% %    1 | 2
% %   ---+---
% %    3 | 4
% ***** TODO: fix this. need to check various conditions of vp *****

if vp{3}(2) < 1
    upmask = false(imgsize(1),imgsize(2));
elseif vp{3}(2) > imgsize(1)
    upmask = true(imgsize(1),imgsize(2));
else
    [p1ext p2ext] = extline(vp{2}, vp{3}, imgsize(2), imgsize(1));

    if p1ext(1) < p2ext(1)
        x = [p1ext(1) p2ext(1) imgsize(2)+100 -100];
    else
        x = [p1ext(1) p2ext(1) -100 imgsize(2)+100];
    end
    y = [p1ext(2) p2ext(2) -100 -100];

    upmask = poly2mask(x,y,imgsize(1),imgsize(2));
end
if vp{3}(1) < 1
    leftmask = false(imgsize(1),imgsize(2));
elseif vp{3}(1) > imgsize(2)
    leftmask = true(imgsize(1),imgsize(2));
else
    [p1ext p2ext] = extline(vp{1}, vp{3}, imgsize(2), imgsize(1));
    if p1ext(2) < p2ext(2)
        y = [p1ext(2) p2ext(2) imgsize(1)+100 -100];
    else
        y = [p1ext(2) p2ext(2) -100 imgsize(1)+100];
    end
    x = [p1ext(1) p2ext(1) -100 -100];

    leftmask = poly2mask(x,y,imgsize(1),imgsize(2));
end

rm{1} =  upmask &  leftmask;
rm{2} =  upmask & ~leftmask;
rm{3} = ~upmask &  leftmask;
rm{4} = ~upmask & ~leftmask;


function parfor_sparseGV( qname, dbname, params )

[~, dbbasename, ~] = fileparts(dbname);
this_sparsegv_matname = fullfile(params.output.gv_sparse.dir, qname, [dbbasename, params.output.gv_sparse.matformat]);

if exist(this_sparsegv_matname, 'file') ~= 2
    %load features
    qfmatname = fullfile(params.input.feature.dir, params.data.q.dir, [qname, params.input.feature.q_sps_matformat]);
    if exist(qfmatname, 'file') ~= 2
        Iqname = fullfile(params.data.dir, params.data.q.dir, qname);
        [ f, d ] = Features_WUSTL( im2single(rgb2gray(imread(Iqname))) );
        d = relja_rootsift(d);
        [qfdir, ~, ~] = fileparts(qfmatname);
        if exist(qfdir, 'dir') ~= 7
            mkdir(qfdir);
        end
        save('-v6', qfmatname, 'f', 'd');
    end
    features_q = load(qfmatname);

    dbfmatname = fullfile(params.input.feature.dir, params.data.db.cutout.dir, [dbname, params.input.feature.db_sps_matformat]);
    if exist(dbfmatname, 'file') ~= 2
        Idbname = fullfile(params.data.dir, params.data.db.cutout.dir, dbname);
        [ f, d ] = Features_WUSTL( im2single(rgb2gray(imread(Idbname))) );
        d = relja_rootsift(d);
        [dbfdir, ~, ~] = fileparts(dbfmatname);
        if exist(dbfdir, 'dir') ~= 7
            mkdir(dbfdir);
        end
        save('-v6', dbfmatname, 'f', 'd');
    end
    features_db = load(dbfmatname);

    %geometric verification
    if size(features_db.d, 2) < 6
        H = nan(3, 3);
        inls_qidx = [];
        inls_dbidx = [];
        inliernum = 0;
        matches = [];
        inliers = [];
    else

        %geometric verification (homography lo-ransac)
        [matches, inliers, H, ~] = at_sparseransac(features_q.f,features_q.d,features_db.f,features_db.d,3,10);
        inliernum = length(inliers);
        inls_qidx = inliers(1, :); inls_dbidx = inliers(2, :);
    end

    %save
    if exist(fullfile(params.output.gv_sparse.dir, qname), 'dir') ~= 7
        mkdir(fullfile(params.output.gv_sparse.dir, qname));
    end
    save('-v6', this_sparsegv_matname, 'H', 'inliernum', 'inls_qidx', 'inls_dbidx', 'matches', 'inliers');

%     %debug
%     Iq = imread(fullfile(params.data.dir, params.data.q.dir, qname));
%     Idb = imread(fullfile(params.data.dir, params.data.db.cutout.dir, dbname));
%     figure();
%     ultimateSubplot ( 2, 1, 1, 1, 0.01, 0.05 );
%     imshow(rgb2gray(Iq));hold on;
%     plot(features_q.f(1, inls_qidx), features_q.f(2, inls_qidx),'g.');
%     ultimateSubplot ( 2, 1, 2, 1, 0.01, 0.05 );
%     imshow(rgb2gray(Idb));hold on;
%     plot(features_db.f(1, inls_dbidx), features_db.f(2, inls_dbidx),'g.');
%
%     keyboard;

end

end


%     NeuroSLAM System Copyright (C) 2018-2019
%     NeuroSLAM: A Brain inspired SLAM System for 3D Environments
%
%     Fangwen Yu (www.yufangwen.com), Jianga Shang, Youjian Hu, Michael Milford(www.michaelmilford.com)
%
%     The NeuroSLAM V1.0 (MATLAB) was developed based on the OpenRatSLAM (David et al. 2013).
%     The RatSLAM V0.3 (MATLAB) developed by David Ball, Michael Milford and Gordon Wyeth in 2008.
%
%     Reference:
%     Ball, David, Scott Heath, Janet Wiles, Gordon Wyeth, Peter Corke, and Michael Milford.
%     "OpenRatSLAM: an open source brain-based SLAM system." Autonomous Robots 34, no. 3 (2013): 149-176.
%
%     This program 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.
%
%     This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.

% Jan 2, 2019
groundTruthFile = '';
expMapFile = 'C:\NeuroSLAM_Datasets\03_NeuroSLAM_Experiments_Results\QUTCarparData\01_exp_map_ml.txt';
% plot_3d_multilayer_experience_map(groundTruthFile, expMapFile, xExpMapScaling, yExpMapScaling, zExpMapScaling, xExpMapTrans, yExpMapTrans, zExpMapTrans, xGtScaling, yGtScaling, zGtScaling)
plot_3d_multilayer_experience_map_qut(groundTruthFile, expMapFile, 0.8, 0.8, 0.1, 0, 0.5, 0, 20,20, 20);


function t1 = joinTrees(t1, t2, alpha)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% t12 = joinTrees(t1, t2, alpha)
%   create a new KD-tree with t1 and t2 as the children of the root
%   The t1 subtree recieves weight alpha; the t2 subtree has wt. 1-alpha
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2003 Alexander Ihler; distributable under GPL -- see README.txt

if(t1.D ~= t2.D)
  error('Input trees have different dimensionality!');
end

if(isfield(t1, 'type') ~= isfield(t2, 'type'))
  error('Cant merge BallTree with BallTreeDensity!');
end

if(t1.type ~= t2.type)
  error('Trees must have the same type of kernel!');
end

if(nargin < 2 || nargin > 3)
  error('wrong number of arguments');
end
if(nargin == 2)
  alpha = 0.5;
end

% leave off one of the zeros between leaves and nodes
if(t1.N==1) t1nodes = []; t1leaves=[1];
  else t1nodes = [1:t1.N-1]; t1leaves = [t1.N+1:2*t1.N]; end;
if(t2.N==1) t2nodes = []; t2leaves = [1];
  else t2nodes = [1:t2.N-1]; t2leaves = [t2.N+1:2*t2.N]; end;

if(t1.N~=1) t1root=2;
else t1root=3+length(t2nodes);
end
if(t2.N~=1) t2root=2+length(t1nodes);
else t2root=3+length(t1nodes)+length(t1leaves);
end
t1N = t1.N;
t1.N = t1.N + t2.N;

Os = zeros(t1.D, 1);

t1.centers = [Os t1.centers(:,t1nodes) t2.centers(:,t2nodes) Os ...
              t1.centers(:,t1leaves) t2.centers(:,t2leaves)];
t1.ranges = [Os t1.ranges(:,t1nodes) t2.ranges(:,t2nodes) Os t1.ranges(:,t1leaves) ...
             t2.ranges(:,t2leaves)];
t1.weights = [0 alpha*t1.weights(t1nodes) (1-alpha)*t2.weights(t2nodes) 0 ...
              alpha*t1.weights(t1leaves) (1-alpha)*t2.weights(t2leaves)];

t1.means = [Os t1.means(:,t1nodes) t2.means(:,t2nodes) Os ...
            t1.means(:,t1leaves) t2.means(:,t2leaves)];

% take care of variable BWs
t1varBWs = size(t1.bandwidth, 2) > 2*t1N;
t2varBWs = size(t2.bandwidth, 2) > 2*t2.N;
varBWs = t1varBWs || t2varBWs || ...
         sum(t1.bandwidth(:,1) ~= t2.bandwidth(:,1)) > 0;
if(varBWs)
  t1.bandwidth = [Os t1.bandwidth(:,t1nodes) t2.bandwidth(:,t2nodes) ...
                  Os t1.bandwidth(:,t1leaves) t2.bandwidth(:,t2leaves) ...
                  Os t1.bandwidth(:,t1nodes+2*t1N*t1varBWs) ...
                  t2.bandwidth(:,t2nodes+2*t2.N*t2varBWs) Os ...
                  t1.bandwidth(:,t1leaves+2*t1N*t1varBWs) ...
                  t2.bandwidth(:,t2leaves+2*t2.N*t2varBWs) Os ...
                  t1.bandwidth(:,t1nodes+4*t1N*t1varBWs) ...
                  t2.bandwidth(:,t2nodes+4*t2.N*t2varBWs) Os ...
                  t1.bandwidth(:,t1leaves+4*t1N*t1varBWs) ...
                  t2.bandwidth(:,t2leaves+4*t2.N*t2varBWs) ];
else
  t1.bandwidth = [Os t1.bandwidth(:,t1nodes) t2.bandwidth(:,t2nodes) ...
                  Os t1.bandwidth(:,t1leaves) t2.bandwidth(:,t2leaves)];
end

%%%% Do stuff from calcStats because calcStats is protected.  Don't
% change calc stats or this won't work.

ax = max(t1.centers(:,t1root)+t1.ranges(:,t1root), ...
         t1.centers(:,t2root)+t1.ranges(:,t2root));
in = min(t1.centers(:,t1root)-t1.ranges(:,t1root), ...
         t1.centers(:,t2root)-t1.ranges(:,t2root));
t1.centers(:,1) = (ax+in)/2;
t1.ranges(:,1) = (ax-in)/2;

%calcuate weight
t1.weights(1) = t1.weights(t1root) + t1.weights(t2root);
W = sum(t1.weights(t1.N+1:2*t1.N));
t1.weights = t1.weights / W;  % normalize
t1w = t1.weights(t1root) / t1.weights(1);
t2w = t1.weights(t2root) / t1.weights(1);

%calculate mean
t1.means(:,1) = t1w*t1.means(:,t1root) + t2w*t1.means(:,t2root);

%calculate bandwidth
type = getType(t1);
if(strcmp(type, 'Gaussian'))
  t1.bandwidth(:,1) = t1w * (t1.bandwidth(:,t1root) + t1.means(:,t1root).^2) ...
      + t2w * (t1.bandwidth(:,t2root) + t1.means(:,t2root).^2) - ...
      t1.means(:,1).^2;
elseif(strcmp(type, 'Epanetchnikov'))
  t1.bandwidth(:,1) = sqrt(.5 * (t1w * (2*t1.bandwidth(:,t1root).^2 ...
                                                 + t1.means(:,t1root).^2) ...
                                + t2w * (2*t1.bandwidth(:,t2root).^2 ...
                                                   + t1.means(:,t2root).^2) ...
                                - t1.means(:,1).^2));
elseif(strcmp(type, 'Laplacian'))
  t1.bandwidth(:,1) = sqrt(5 * (t1w * (.2*t1.bandwidth(:,t1root).^2 ...
                                                + t1.means(:,t1root).^2) ...
                               + t2w * (.2*t1.bandwidth(:,t2root).^2 ...
                                                  + t1.means(:,t2root).^2) ...
                               - t1.means(:,1).^2));
else
  error(['unknown kernel type: ' type])
end

% take care of max and min BWs for variable bandwidths
if(varBWs)
  t1.bandwidth(:,1+2*t1.N) = max(t1.bandwidth(:,t1root+2*t1.N), t1.bandwidth(:,t2root+2*t1.N));
  t1.bandwidth(:,1+4*t1.N) = min(t1.bandwidth(:,t1root+4*t1.N), t1.bandwidth(:,t2root+4*t1.N));
end

t1n = 1;
t2n = 1 + length(t1nodes);
t1l = 1 + length(t2nodes);
t2l = 1 + length(t1nodes) + length(t1leaves);

% arrays are zero indexed
t1.lower = [t1.N addUints(t1.lower(t1nodes),t1l) ...
           addUints(t2.lower(t2nodes),t2l) 0 ...
           addUints(t1.lower(t1leaves),t1l) ...
           addUints(t2.lower(t2leaves),t2l)];
t1.upper = [2*t1.N-1 addUints(t1.upper(t1nodes),t1l) ...
           addUints(t2.upper(t2nodes),t2l) 0 ...
           addUints(t1.upper(t1leaves),t1l) ...
           addUints(t2.upper(t2leaves),t2l)];

t1leftch = addUints(t1.leftch, (t1.leftch < t1N) * t1n + ...
                     (t1.leftch >= t1N) * t1l);
t2leftch = addUints(t2.leftch, (t2.leftch < t2.N) * t2n + ...
                     (t2.leftch >= t2.N) * t2l);
t1rightch = addUints(t1.rightch, (t1.rightch < t1N) * t1n + ...
                     (t1.rightch >= t1N) .* (t1.rightch < 4e9) * t1l);
t2rightch = addUints(t2.rightch, (t2.rightch < t2.N) * t2n + ...
                     (t2.rightch >= t2.N) .* (t2.rightch < 4e9) * t2l);

t1.leftch = [d2uint(t1root) t1leftch(t1nodes) t2leftch(t2nodes) ...
             0 t1leftch(t1leaves) t2leftch(t2leaves)];
t1.rightch = [d2uint(t2root) t1rightch(t1nodes), t2rightch(t2nodes) ...
              0 t1rightch(t1leaves) t2rightch(t2leaves)];
t1.perm = [0 t1.perm(t1nodes) t2.perm(t2nodes) 0 t1.perm(t1leaves) ...
          addUints(t2.perm(t2leaves),length(t1leaves))];

function c = addUints(a, b)
c = uint32(double(a) + double(b));

function u = d2uint(d)
u = uint32(d - 1);


function [res,I_patch,I]=getCNNFeature(obj,theNet,theConf,IsFlip,meanData)
theNet.layers{end}.class=theNet.layers{end}.class(:,:,:,1);
theNet.layers{end}.class(:)=1;
[I_patch,I]=getI(obj,theConf,theNet.meta.normalization.imageSize(1:2),IsFlip);
im_=I_patch-meanData;
if(isa(theNet.layers{1}.weights{1},'gpuArray'))
    res=our_vl_simplenn(theNet,gpuArray(im_));
    for i=1:length(res)
        res(i).x=gather(res(i).x);
    end
else
    res=our_vl_simplenn(theNet,im_);
end
I_patch=uint8(I_patch);
end


% this is an example code for training a PixelNet model using caffe
% here we consider a 224x224 image
% uniform sampling of pixels in an image, used for segmentation
function trainSeg(gpuid, options)

	method = 'seg';
	cachepath = [options.cachepath, method, '/'];
	if(~isdir(cachepath))
		mkdir(cachepath);
	end

	% --
	caffe.reset_all;
	caffe.set_device(gpuid);
	caffe.set_mode_gpu;

	% train the network --
	trainNet(options, cachepath);
	caffe.reset_all;
end

function trainNet(options,  cachepath)

	% check if it has already been trained
	trainfolder = [cachepath, 'TRAIN/'];
	if(~isdir(trainfolder))
		mkdir(trainfolder);
	end

	% check if the dataset is ready
	% this would be available in the download_data.sh script
	datasetfile = [options.datapath,...
			'/train/seg-voc2012-aug_trainval.mat'];
	if ~(exist(datasetfile, 'file'))
    		error('Dataset is not prepared!');
	end
	load(datasetfile, 'imagelist', 'seglist');

	imagelist = strcat(options.datapath, '/',imagelist);
	seglist = strcat(options.datapath, '/',seglist);

	% load the network --
	solverpath = [options.datapath, '/train/solver.prototxt'];
	initmodelpath = [options.datapath,'/train/VGG16_fconv.caffemodel'];
	trainModel(options,trainfolder,imagelist,seglist,solverpath,initmodelpath);
end

function trainModel(options,trainfolder,imagelist,seglist,solverpath,initmodelpath)

	% Though this is not required --
	imagelist = repmat(imagelist, 10,1);
	seglist = repmat(seglist,10,1);
	rand_ids = randperm(length(imagelist));
	imagelist = imagelist(rand_ids);
	seglist = seglist(rand_ids);

	%
	li = length(imagelist);
	caffe.reset_all;

	% initialize network
	solver = caffe.get_solver(solverpath);
	% load the model
	solver.net.copy_from(initmodelpath);

	% --
	maxsize = options.cnn_input_size + 200;
	input_data = zeros(maxsize, maxsize, 3, options.segimbatch, 'single');
	input_index = zeros(3,options.segbatchsize, 'single');
	input_label = zeros(1,options.segbatchsize, 'single');
	solver.net.blobs('data').reshape([maxsize, maxsize, 3, options.segimbatch]);
	solver.net.blobs('pixels').reshape([3,options.segbatchsize]);
	solver.net.blobs('labels').reshape([1,options.segbatchsize]);

	% set up memory
	solver.net.forward_prefilled();
	% then start training
	oldrng = rng;
	rng(options.seed, 'twister');

	for epoch = 1:options.segepoch
   	 index = randperm(li);
   	 for i = 1:options.segimbatch:li
        	j = i+options.segimbatch-1;
        	if j > li
            	continue;
        	end

	        st = 1;
       		ed = options.segsamplesize;
        	im = 0;
        	for k=i:j
            		ik = index(k);

            		if options.trainFlip
		                flip = rand(1) > 0.5;
            		end

	    		im_data = seg_image_provider(options, imagelist{ik}, flip);
	    		[sampled, label] = seg_label_provider(...
				options, seglist{ik}, options.segsamplesize, flip);

            		% notice the zero-index vs. the one-index
            		sampled(1,:) = im;
            		sampled(2,:) = sampled(2,:) + 100;
            		sampled(3,:) = sampled(3,:) + 100;

            		im = im + 1;
            		input_data(101:options.cnn_input_size+100,....
		      	 101:options.cnn_input_size+100, :, im) = im_data;
            		input_index(:, st:ed) = sampled;
            		input_label(st:ed) = label;
           		st = st + options.segsamplesize;
            		ed = ed + options.segsamplesize;
        	end

	        solver.net.blobs('data').set_data(input_data);
        	solver.net.blobs('pixels').set_data(input_index);
        	solver.net.blobs('labels').set_data(input_label);
        	solver.step(1);
        	% clean up everything
        	input_data(:) = 0;
    	end
    	% add another condition
    	if mod(epoch, options.saveEpoch) == 0 && epoch < options.segepoch
        	epochfile = [trainfolder, sprintf('(%02d).caffemodel',epoch)];
        	solver.net.save(epochfile);
    	end

	% --
      end

	targetfile = [trainfolder,'final_model.caffemodel'];
	solver.net.save(targetfile);
	caffe.reset_all;
	rng(oldrng);

end


%% NISwGSP-06_SienaCathedralLibrary
% %{
imfolder = 'images\NISwGSP-06_SienaCathedralLibrary';
im_n = 14;
imfile = cell(im_n,1);
for ii = 1:im_n
    imfile{ii} = sprintf('%s\\image%02d.jpg', imfolder, ii);
end

im = cell(im_n,1);
for ii = 1:im_n
    im{ii} = imread(imfile{ii});
end

imsize = zeros(im_n,3);

for ii = 1:im_n
    imsize(ii,:) = size(im{ii});
    if imsize(ii,1) > 720
        scale = 720/size(im{ii}, 1);
        im{ii} = imresize(im{ii}, scale);

        imsize(ii,:) = size(im{ii});
    end
end

mosaic = REW_mosaic( im, [], 0, 'equi', 0.02, imfolder );
%}


function SupervisedClassification_Callback(hObject, eventdata, handles)

% This function uses a convolutional neural network, trained in
% "TrainSupervisedClassifier_Callback.m", to classify USVs.

[FileName,PathName] = uigetfile(fullfile(handles.data.squeakfolder,'Clustering Models','*.mat'),'Select Network');
net = load([PathName FileName],'ClassifyNet','wind','noverlap','nfft','imageSize');

if exist('net','var') ~= 1
    errordlg('Network not be found. Is this file a trained CNN?')
    return
end

if exist(handles.data.settings.detectionfolder,'dir')==0
    errordlg('Please Select Detection Folder')
    uiwait
    load_detectionFolder_Callback(hObject, eventdata, handles)
    handles = guidata(hObject);  % Get newest version of handles
end

options.imageSize = [128, 128, 1];
[ClusteringData, Class, options.freqRange, options.maxDuration, options.spectrogram] = CreateClusteringData(handles, 'scale_duration', true, 'fixed_frequency', true);

% Resize the images to match the input image size
images = zeros([options.imageSize, size(ClusteringData, 1)]);
for i = 1:size(ClusteringData, 1)
    images(:,:,:,i) = imresize(ClusteringData.Spectrogram{i}, options.imageSize(1:2));
end
% wind=options.spectrogram.windowsize;
% noverlap=options.spectrogram.overlap;
% nfft=options.spectrogram.nfft;
imageSize=options.imageSize;

h = waitbar(0,'Initializing');
for j=1:height(ClusteringData);
    waitbar(j/height(ClusteringData), h, ['Classifying Image ' num2str(j) ' of ' num2str(height(ClusteringData))]);
    X = images(:,:,:,j) ./ 256;
    [Cl, sc] = classify(net.ClassifyNet, X);
    clustAssign(j,1)=Cl;
end
close(h);
clusterName=unique(clustAssign);
saveChoice =  questdlg('Update files with new classifications?','Save Classification','Yes','No','Yes');
switch saveChoice
    case 'Yes'
        UpdateCluster(ClusteringData, clustAssign, clusterName, zeros(1,height(ClusteringData)));
        update_folders(hObject, eventdata, handles);
        if isfield(handles,'current_detection_file')
            loadcalls_Callback(hObject, eventdata, handles, true)
        end
    case 'No'
        return
end
end


function [hdr,otherendian] = spm_read_hdr(fname)
% Read (SPM customised) Analyze header
% FORMAT [hdr,otherendian] = spm_read_hdr(fname)
% fname       - .hdr filename
% hdr         - structure containing Analyze header
% otherendian - byte swapping necessary flag
%_______________________________________________________________________
% @(#)spm_read_hdr.m	2.2 John Ashburner 03/07/17

fid         = fopen(fname,'r','native');
otherendian = 0;
if (fid > 0)
	dime = read_dime(fid);
	if dime.dim(1)<0 | dime.dim(1)>15, % Appears to be other-endian
		% Re-open other-endian
		fclose(fid);
		if spm_platform('bigend'), fid = fopen(fname,'r','ieee-le');
		else,                      fid = fopen(fname,'r','ieee-be'); end;
		otherendian = 1;
		dime = read_dime(fid);
	end;
	hk       = read_hk(fid);
	hist     = read_hist(fid);
	hdr.hk   = hk;
	hdr.dime = dime;
	hdr.hist = hist;

	% SPM specific bit - unused
	%if hdr.hk.sizeof_hdr > 348,
	%	spmf = read_spmf(fid,dime.dim(5));
	%	if ~isempty(spmf),
	%		hdr.spmf = spmf;
	%	end;
	%end;

	fclose(fid);
else,
	hdr = [];
	otherendian = NaN;
	%error(['Problem opening header file (' fopen(fid) ').']);
end;
return;
%_______________________________________________________________________
%_______________________________________________________________________
function hk = read_hk(fid)
% read (struct) header_key
%-----------------------------------------------------------------------
fseek(fid,0,'bof');
hk.sizeof_hdr 		= fread(fid,1,'int32');
hk.data_type  		= mysetstr(fread(fid,10,'uchar'))';
hk.db_name    		= mysetstr(fread(fid,18,'uchar'))';
hk.extents    		= fread(fid,1,'int32');
hk.session_error	= fread(fid,1,'int16');
hk.regular			= mysetstr(fread(fid,1,'uchar'))';
hk.hkey_un0			= mysetstr(fread(fid,1,'uchar'))';
if isempty(hk.hkey_un0), error(['Problem reading "hk" of header file (' fopen(fid) ').']); end;
return;
%_______________________________________________________________________
%_______________________________________________________________________
function dime = read_dime(fid)
% read (struct) image_dimension
%-----------------------------------------------------------------------
fseek(fid,40,'bof');
dime.dim		= fread(fid,8,'int16')';
dime.vox_units	= mysetstr(fread(fid,4,'uchar'))';
dime.cal_units	= mysetstr(fread(fid,8,'uchar'))';
dime.unused1	= fread(fid,1,'int16');
dime.datatype	= fread(fid,1,'int16');
dime.bitpix		= fread(fid,1,'int16');
dime.dim_un0	= fread(fid,1,'int16');
dime.pixdim		= fread(fid,8,'float')';
dime.vox_offset	= fread(fid,1,'float');
dime.funused1	= fread(fid,1,'float');
dime.funused2	= fread(fid,1,'float');
dime.funused3	= fread(fid,1,'float');
dime.cal_max	= fread(fid,1,'float');
dime.cal_min	= fread(fid,1,'float');
dime.compressed	= fread(fid,1,'int32');
dime.verified	= fread(fid,1,'int32');
dime.glmax		= fread(fid,1,'int32');
dime.glmin		= fread(fid,1,'int32');
if isempty(dime.glmin), error(['Problem reading "dime" of header file (' fopen(fid) ').']); end;
return;
%_______________________________________________________________________
%_______________________________________________________________________
function hist = read_hist(fid)
% read (struct) data_history
%-----------------------------------------------------------------------
fseek(fid,148,'bof');
hist.descrip	= mysetstr(fread(fid,80,'uchar'))';
hist.aux_file	= mysetstr(fread(fid,24,'uchar'))';
hist.orient		= fread(fid,1,'uchar');
hist.origin		= fread(fid,5,'int16')';
hist.generated	= mysetstr(fread(fid,10,'uchar'))';
hist.scannum	= mysetstr(fread(fid,10,'uchar'))';
hist.patient_id	= mysetstr(fread(fid,10,'uchar'))';
hist.exp_date	= mysetstr(fread(fid,10,'uchar'))';
hist.exp_time	= mysetstr(fread(fid,10,'uchar'))';
hist.hist_un0	= mysetstr(fread(fid,3,'uchar'))';
hist.views		= fread(fid,1,'int32');
hist.vols_added	= fread(fid,1,'int32');
hist.start_field= fread(fid,1,'int32');
hist.field_skip	= fread(fid,1,'int32');
hist.omax		= fread(fid,1,'int32');
hist.omin		= fread(fid,1,'int32');
hist.smax		= fread(fid,1,'int32');
hist.smin		= fread(fid,1,'int32');
if isempty(hist.smin), error(['Problem reading "hist" of header file (' fopen(fid) ').']); end;
return;
%_______________________________________________________________________
%_______________________________________________________________________
function spmf = read_spmf(fid,n)
% Read SPM specific fields
% This bit may be used in the future for extending the Analyze header.

fseek(fid,348,'bof');
mgc = fread(fid,1,'int32');    % Magic number
if mgc ~= 20020417, spmf = []; return; end;

for j=1:n,
	spmf(j).mat    = fread(fid,16,'double'); % Orientation information
	spmf(j).unused = fread(fid,384,'uchar'); % Extra unused stuff
	if length(spmf(j).unused)<384,
		error(['Problem reading "spmf" of header file (' fopen(fid) ').']);
	end;
 	spmf(j).mat = reshape(spmf(j).mat,[4 4]);
end;
return;
%_______________________________________________________________________
%_______________________________________________________________________
function out = mysetstr(in)
tmp = find(in == 0);
tmp = min([min(tmp) length(in)]);
out = setstr([in(1:tmp)' zeros(1,length(in)-(tmp))])';
return;
%_______________________________________________________________________
%_______________________________________________________________________


function [dat,stats] = hewma_extract_voxel(EXPT,coords)
% Extracts data for one voxel and runs hewma2 on it.
%
% :Usage:
% ::
%
%     function [dat,stats] = hewma_extract_voxel(EXPT,coords)
%
% Uses plot option in hewma2 to create plots.

dat = [];
stats = [];

persistent timeseriesfile
persistent lamfile
persistent files
persistent vfiles

zsl = num2str(coords(3));

mm_coords = spm_orthviews('Pos')';
fprintf(1, 'mm coordinates are (x, y, z) = (%3.0f, %3.0f, %3.0f)\t\t Voxel coords: (%3.0f, %3.0f, %3.0f)\n', mm_coords, coords);

if isempty(files) || isempty(vfiles)
    mydir = spm_get(-1,'*','Select Directory above individual subject directories',pwd);
end

if isempty(files)

    % EXPT = getfunctnames2(EXPT,['z_slice' zsl '.mat'],'tmp');
    EXPT = getfunctnames2(EXPT,['z_slice' zsl '.mat'], 'tmp', [], mydir);

    files = str2mat(EXPT.tmp{:});
end

if isempty(vfiles)
    %EXPT = getfunctnames2(EXPT,['var_slice' zsl '.mat'],'tmp');
    EXPT = getfunctnames2(EXPT,['var_slice' zsl '.mat'], 'tmp', [], mydir);

    vfiles = str2mat(EXPT.tmp{:});
end

if isempty(files) || all(files(:) == ' ')
    disp('Cannot find z_slice files for subjects.  Check that path names are correct in EXPT structure.');
    return
end

if isempty(vfiles) || all(vfiles(:) == ' ')
    disp('Cannot find var_slice files for subjects.  Check that path names are correct in EXPT structure.');
    return
end

if isempty(timeseriesfile)

    timeseriesfile = spm_get(1,'*mat','Select hewma_timeseries.mat containing img dims',pwd);

end

  load(timeseriesfile, 'xdim', 'ydim');
% % try
% %     load hewma0001/hewma_timeseries xdim ydim
% % catch
% %     if ~(exist('hewma_timeseries') == 2)
% %             file = spm_get(1,'*mat','Select hewma_timeseries.mat',pwd);
% %             [dd,ff,ee] = fileparts(file);
% %             cd(dd)
% %             load hewma_timeseries xdim ydim
% %     end
% % end

if isempty(lamfile)
    lamfile = spm_get(1,'*mat','Select lam.mat file containing lambda parameter for one subject',pwd);
end

load(lamfile, 'lam');

% % try
% %     load([EXPT.subjects{1} filesep 'lam'])    % load lambda param, assume same for all subjects
% % catch
% %     error('Cannot find lam.mat.  Start in dir above individual ewma results dirs.')
% % end

fprintf(1,'Loading data, subject ');

for i = 1:length(EXPT.subjects)
    fprintf(1,'%3.0f ',i);

    % load EWMA stat
    load(deblank(files(i,:)));
    zdat = full(zdat);
    T = size(zdat,2);
    zdat = reshape(zdat,xdim,ydim,T);

    try
        dat(i,:) = squeeze(zdat(coords(1),coords(2),:));
    catch
        disp('Warning!  data for this subject does not match others in size.  ')
        dat(i,:) = squeeze(zdat(coords(1),coords(2),1:size(dat,2)));
    end

    % load variance
    load(deblank(vfiles(i,:)));
    vardat = full(vardat);
    vardat = reshape(vardat,xdim,ydim,T);

    try
        vdat(i,:) = squeeze(vardat(coords(1),coords(2),:));
    catch
        vdat(i,:) = squeeze(vardat(coords(1),coords(2),1:size(vdat,2)));
    end

end
fprintf(1,'\n');

if isfield(EXPT,'cov');
    mycov = EXPT.cov;
end
if ~isempty(mycov), mycov = mycov(:,1); end

[p,tm,Zcor,sbmean,Zpop,tvals,sb,stats] = hewma2(dat, vdat, lam,1,1,mycov);

%figure;plot(dat');
%hold on; plot(mean(dat),'k','LineWidth',2);
%title('EWMA statistics with mean');

return


function [volToEval,maskBoundingBox3M,gridS,paramS,diagS] = ...
    preProcessForRadiomics(scanNum,structNum, paramS, planC)
% preProcessForRadiomics.m
% Pre-process image for radiomics feature extraction. Includes
% perturbation, resampling, cropping, and intensity-thresholding.
%
% AI 3/28/19

%% Get scan array, mask and x,y,z grid for reslicing
if numel(scanNum) == 1

    if ~exist('planC','var')
        global planC
    end

    indexS = planC{end};

    % Get scan
    scanArray3M = getScanArray(planC{indexS.scan}(scanNum));
    scanArray3M = double(scanArray3M) - ...
        planC{indexS.scan}(scanNum).scanInfo(1).CTOffset;

    scanSiz = size(scanArray3M);

    if numel(structNum) == 1
        % Get structure mask
        [rasterSegments] = getRasterSegments(structNum,planC);
        [maskUniq3M, uniqueSlices] = rasterToMask(rasterSegments, scanNum, planC);
        mask3M = false(scanSiz);
        mask3M(:,:,uniqueSlices) = maskUniq3M;
    else
        mask3M = structNum;
    end

    % Get x,y,z grid for reslicing and calculating the shape features
    [xValsV, yValsV, zValsV] = getScanXYZVals(planC{indexS.scan}(scanNum));
    if yValsV(1) > yValsV(2)
        yValsV = fliplr(yValsV);
    end
    %zValsV = zValsV(uniqueSlices);

%     % Get image types with various parameters
%     fieldNamC = fieldnames(paramS.imageType);
%     for iImg = 1:length(fieldNamC)
%         for iFilt = 1:length(paramS.imageType.(fieldNamC{iImg}))
%             imageType = fieldNamC{iImg};
%             if strcmpi(imageType,'SUV')
%                 scanInfoS = planC{indexS.scan}(scanNum).scanInfo;
%                 paramS.imageType.(fieldNamC{iImg})(iFilt).scanInfoS = scanInfoS;
%             end
%         end
%     end

else

    scanArray3M = scanNum;
    mask3M = structNum;

    % Assume xValsV,yValsV,zValsV increase monotonically.
    xValsV = planC{1};
    yValsV = planC{2};
    zValsV = planC{3};

end

if isempty(scanArray3M)
    volToEval = [];
    maskBoundingBox3M = [];
    gridS = [];
    return;
end

% Pixelspacing (dx,dy,dz)
PixelSpacingX = abs(xValsV(1) - xValsV(2));
PixelSpacingY = abs(yValsV(1) - yValsV(2));
PixelSpacingZ = abs(zValsV(1) - zValsV(2));

diagS.numVoxelsOrig = sum(mask3M(:));

%% Apply global settings
whichFeatS = paramS.whichFeatS;

perturbX = 0;
perturbY = 0;
perturbZ = 0;

%--- 1. Perturbation ---
if whichFeatS.perturbation.flag

    [scanArray3M,mask3M] = perturbImageAndSeg(scanArray3M,mask3M,planC,...
        scanNum,whichFeatS.perturbation.sequence,...
        whichFeatS.perturbation.angle2DStdDeg,...
        whichFeatS.perturbation.volScaleStdFraction,...
        whichFeatS.perturbation.superPixVol);

    % Get grid perturbation deltas
    if ismember('T',whichFeatS.perturbation.sequence)
        perturbFractionV = [-0.75,-0.25,0.25,0.75];
        perturbFractionV(randsample(4,1));
        perturbX = PixelSpacingX*perturbFractionV(randsample(4,1));
        perturbY = PixelSpacingY*perturbFractionV(randsample(4,1));
        perturbZ = PixelSpacingZ*perturbFractionV(randsample(4,1));
    end

end

%--- 2. Crop scan around mask and pad as required ---
padScaleX = 1;
padScaleY = 1;
padScaleZ = 1;
if whichFeatS.resample.flag
    dx = median(diff(xValsV));
    dy = median(diff(yValsV));
    dz = median(diff(zValsV));
    padScaleX = ceil(whichFeatS.resample.resolutionXCm/dx);
    padScaleY = ceil(whichFeatS.resample.resolutionYCm/dy);
    padScaleZ = ceil(whichFeatS.resample.resolutionZCm/dz);
end

%---For IBSI2 ------
cropForResampling = 0;
padSizV = [];
padMethod = 'none';
%------------------
if cropForResampling
    %Default:Pad by 5 voxels (original image intensities) before resampling
    padMethod = 'expand';
    padSizV = [5,5,5];
    if whichFeatS.padding.flag
        if isfield(whichFeatS.padding,'size')
            filtPadSizV = whichFeatS.padding.size;
            filtPadSizV = reshape(filtPadSizV,1,[]);
            if length(filtPadSizV)==2
                filtPadSizV = [filtPadSizV,0];
            end
            filtPadSizV = filtPadSizV.*[padScaleX padScaleY padScaleZ];
            repIdxV = filtPadSizV > padSizV;
            padSizV(repIdxV) = filtPadSizV(repIdxV);
            padMethod = whichFeatS.padding.method;
        end
    end
end

% Crop to ROI and pad
scanArray3M = double(scanArray3M);
[padScanBoundsForResamp3M,padMaskBoundsForResamp3M,outLimitsV] = ...
    padScan(scanArray3M,mask3M,padMethod,padSizV,cropForResampling);
xValsV = xValsV(outLimitsV(3):outLimitsV(4));
yValsV = yValsV(outLimitsV(1):outLimitsV(2));
zValsV = zValsV(outLimitsV(5):outLimitsV(6));
slcIndV = outLimitsV(5):outLimitsV(6);

%--- 3. Resampling ---

%Get resampling method
if whichFeatS.resample.flag
    % Pixelspacing (dx,dy,dz) after resampling
    if ~isempty(whichFeatS.resample.resolutionXCm)
        PixelSpacingX = whichFeatS.resample.resolutionXCm;
    end
    if ~isempty(whichFeatS.resample.resolutionYCm)
        PixelSpacingY = whichFeatS.resample.resolutionYCm;
    end
    if ~isempty(whichFeatS.resample.resolutionZCm)
        PixelSpacingZ = whichFeatS.resample.resolutionZCm;
    end

    % Interpolate using the method defined in settings file
    roiInterpMethod = 'linear';
    scanInterpMethod = whichFeatS.resample.interpMethod;
    extrapVal = 0;
    gridResampleMethod = 'center';

    % Interpolate using the method defined in settings file
    origVolToEval = padScanBoundsForResamp3M;
    origMask = padMaskBoundsForResamp3M;
    outputResV = [PixelSpacingX,PixelSpacingY,PixelSpacingZ];
    originV = [xValsV(1),yValsV(1),zValsV(end)];

    %Get resampling grid
    [xResampleV,yResampleV,zResampleV] = getResampledGrid(outputResV,...
        xValsV,yValsV,zValsV,originV,gridResampleMethod,...
        [perturbX,perturbY,perturbZ]);

    %Resample scan
    resampScanBounds3M = imgResample3d(origVolToEval,xValsV,yValsV,zValsV,...
        xResampleV,yResampleV,zResampleV,scanInterpMethod,extrapVal);
    %Option to round
    if isfield(whichFeatS.resample,'intensityRounding') && ...
            strcmpi(whichFeatS.resample.intensityRounding,'on')
        resampScanBounds3M = round(resampScanBounds3M);
    end

    %Resample mask
    resampMaskBounds3M = imgResample3d(single(origMask),xValsV,yValsV,...
        zValsV,xResampleV,yResampleV,zResampleV,roiInterpMethod) >= 0.5;

    newSlcIndV = zeros(1,length(zResampleV));
    for iSlc = 1:length(zResampleV)
        newSlcIndV(iSlc) = findnearest(zValsV, zResampleV(iSlc));
    end

else
    resampScanBounds3M = padScanBoundsForResamp3M;
    resampMaskBounds3M = padMaskBoundsForResamp3M;
    xResampleV = xValsV;
    yResampleV = yValsV;
    zResampleV = zValsV;
    newSlcIndV = 1:length(slcIndV);
    dx = abs(median(diff(xResampleV)));
    dy = abs(median(diff(yResampleV)));
    dz = abs(median(diff(zResampleV)));
    outputResV = [dx dy dz];
end

%Apply padding as required for convolutional filtering
filtPadMethod = 'none';
filtPadSizeV = [0 0 0];
cropFlag = 1;  %Default
if ~cropForResampling
    if whichFeatS.padding.flag
        if isfield(whichFeatS.padding,'cropToMaskBounds')
            cropFlag = strcmpi(whichFeatS.padding.cropToMaskBounds,'yes');
        end
        if isfield(whichFeatS.padding,'method') && ...
                ~strcmpi(whichFeatS.padding.method,'none')
            filtPadMethod = whichFeatS.padding.method;
            filtPadSizeV = reshape(whichFeatS.padding.size,1,[]);
            if length(filtPadSizeV)==2
                filtPadSizeV = [filtPadSizeV,0];
            end
        end
    end
    [volToEval,maskBoundingBox3M,outLimitsV] = padScan(resampScanBounds3M,...
        resampMaskBounds3M,filtPadMethod,filtPadSizeV,cropFlag);

    %Extend resampling grid if padding original image (cropFlag=0)
    if outLimitsV(1)<1
        numPad = (1-outLimitsV(1));
        yExtendV = yResampleV(1)-(1:numPad)*outputResV(2);
        yResampleV = [yExtendV,yResampleV];
        outLimitsV(1) = outLimitsV(1)+numPad;
        outLimitsV(2) = outLimitsV(2)+numPad;
    end
    if outLimitsV(3)<1
        numPad = (1-outLimitsV(3));
        xExtendV = xResampleV(1)-(1:numPad)*outputResV(1);
        xResampleV = [xExtendV,xResampleV];
        outLimitsV(3) = outLimitsV(3)+numPad;
        outLimitsV(4) = outLimitsV(4)+numPad;
    end
    if outLimitsV(5)<1
        numPad = (1-outLimitsV(5));
        zExtendV = zResampleV(1)-(1:numPad)*outputResV(3);
        zResampleV = [zExtendV,zResampleV];
        outLimitsV(5) = outLimitsV(5)+numPad;
        outLimitsV(6) = outLimitsV(6)+numPad;
    end
    if outLimitsV(2)>length(yResampleV)
        numPad = (outLimitsV(2)-length(yResampleV));
        yExtendV = yResampleV(end)+(1:numPad)*outputResV(2);
        yResampleV = [yResampleV,yExtendV];
    end
    if outLimitsV(4)>length(xResampleV)
        numPad = (outLimitsV(4)-length(xResampleV));
        xExtendV = xResampleV(end)+(1:numPad)*outputResV(1);
        xResampleV = [xResampleV,xExtendV];
    end
    if outLimitsV(6)>length(zResampleV)
        numPad = (outLimitsV(6)-length(zResampleV));
        zExtendV = zResampleV(end)+(1:numPad)*outputResV(3);
        zResampleV = [zResampleV,zExtendV];
    end
    xResampleV = xResampleV(outLimitsV(3):outLimitsV(4));
    yResampleV = yResampleV(outLimitsV(1):outLimitsV(2));
    zResampleV = zResampleV(outLimitsV(5):outLimitsV(6));
else
    %     resampSizeV = size(resampScanBounds3M);
    %     volToEval = resampScanBounds3M(padSizV(1)+1:resampSizeV(1)-padSizV(1),...
    %         padSizV(2)+1:resampSizeV(2)-padSizV(2),...
    %         padSizV(3)+1:resampSizeV(3)-padSizV(3));
    %     maskBoundingBox3M = resampMaskBounds3M(padSizV(1)+1:resampSizeV(1)-padSizV(1),...
    %         padSizV(2)+1:resampSizeV(2)-padSizV(2),...
    %         padSizV(3)+1:resampSizeV(3)-padSizV(3));
    %    resampSizeV = size(resampScanBounds3M);
    volToEval = resampScanBounds3M;
    maskBoundingBox3M = resampMaskBounds3M;
end

%--- 4. Ignore voxels below and above cutoffs, if defined ----
minSegThreshold = [];
maxSegThreshold = [];
if isfield(paramS,'textureParamS')
    if isfield(paramS.textureParamS,'minSegThreshold')
        minSegThreshold = paramS.textureParamS.minSegThreshold;
    end
    if isfield(paramS.textureParamS,'maxSegThreshold')
        maxSegThreshold = paramS.textureParamS.maxSegThreshold;
    end
    if ~isempty(minSegThreshold)
        maskBoundingBox3M(volToEval < minSegThreshold) = 0;
    end
    if ~isempty(maxSegThreshold)
        maskBoundingBox3M(volToEval > maxSegThreshold) = 0;
    end
end
%volToEval(~maskBoundingBox3M) = NaN;
volV = volToEval(maskBoundingBox3M);
diagS.numVoxelsInterpReseg = sum(maskBoundingBox3M(:));
diagS.MeanIntensityInterpReseg = mean(volV);
diagS.MaxIntensityInterpReseg = max(volV);
diagS.MinIntensityInterpReseg = min(volV);

% Return grid and pixel-spacing
gridS.xValsV = xResampleV;
gridS.yValsV = yResampleV;
gridS.zValsV = zResampleV;
gridS.PixelSpacingV = [PixelSpacingX,PixelSpacingY,PixelSpacingZ];

% Pass scanInfo as an additional parameter for imageType = SUV
if numel(scanNum) == 1
    % Get image types with various parameters
    fieldNamC = fieldnames(paramS.imageType);
    for iImg = 1:length(fieldNamC)
        for iFilt = 1:length(paramS.imageType.(fieldNamC{iImg}))
            imageType = fieldNamC{iImg};
            if strcmpi(imageType,'SUV')
                scanInfoS = planC{indexS.scan}(scanNum).scanInfo;
                scanInfoS = scanInfoS(slcIndV); % slices for the cropped volume
                scanInfoS = scanInfoS(newSlcIndV); % resampled slices
                paramS.imageType.(fieldNamC{iImg})(iFilt).scanInfoS = scanInfoS;
            end
        end
    end
end

end


function varargout = eig_yalmip_internal(varargin)

switch class(varargin{1})

    case 'double'
        X = varargin{1}(:);
        n = sqrt(length(X));
        X = reshape(X,n,n);
        varargout{1} = eig((X+X')/2);

    case 'sdpvar' % Overloaded operator for SDPVAR objects. Pass on args and save them.

        x = varargin{1};
        y = yalmip('definemulti',mfilename,x,[size(x,1) 1]);
        varargout{1} = y;

    case 'char' % YALMIP send 'graph' when it wants the epigraph or hypograph
        %   disp('The EIG operator is not supported in optimization problems.')
        %   error

        operator = CreateBasicOperator('callback');

        varargout{1} = [];
        varargout{2} = operator;
        varargout{3} = varargin{3};

        % Inofficial way to model several nonlinear variables in
        % one call
        % varargout{2}.models = getvariables(t):getvariables(t)+length(X)-1;

    otherwise
        error([upper(mfilename) ' called with weird argument']);
end